diff --git a/CHANGELOG.md b/CHANGELOG.md index c468291..334832f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,33 +1,80 @@ # Change Log This change log lists all modifications for each sbgECom library release. -sbgECom C library change log issued on: 2024-01-30 -Copyright (C) 2024, SBG Systems SAS. All rights reserved. +sbgECom C library change log issued on: 2024-12-04 +Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. ## Release Summary -The sbgECom 4.0.1987-stable is a major release that supports the latest High Performance INS firmware 5.1 and ELLIPSE firmware 2.5. +The sbgECom 5.1.708-stable is a major release that supports the following products: + - **High Performance INS**: Firmware 5.x and above + - **ELLIPSE v3**: Firmware 3.x and above + - **ELLIPSE v1/v2**: Firmware 2.6 (legacy support) -This update focuses on advanced GNSS monitoring indicators such as spoofing, jamming, etc. -Several new outputs have been added such as SBG_ECOM_LOG_EKF_ROT_ACCEL_BODY or SBG_ECOM_LOG_EKF_VELOCITY_BODY. +This release introduces support for the **ELLIPSE v3** major firmware update. +The ELLIPSE v3 firmware primarily relies on the [sbgRestApi](https://developer.sbg-systems.com/sbgInsRestApi/) for product configuration. -The sbgBasicLogger console application has been completely rewritten from scratch to offer an powerful but yet easy to use tool. -This tool is designed to easily view and convert to CSV like files data from SBG Systems INS. -The C++ 14 source code is also available to you can adapt it to your custom needs. +All new features and capabilities introduced by the ELLIPSE firmware v3 are fully accessible through this [sbgECom](https://developer.sbg-systems.com/sbgECom/) release, alongside the latest version of the [sbgRestApi](https://developer.sbg-systems.com/sbgInsRestApi/). -A lot of work has been done to cleanup several parts of the code base and offer a better experience for developers. -The backward compatibility has been kept as much as possible but please read the Migration.md file for further information. +A significant effort has been dedicated to improving the sbgECom logs and protocol documentation. +The previously used Firmware Reference Manual PDF has been replaced with an integrated, always up-to-date, Markdown/Doxygen-based documentation system. + +Access this new and improved documentation here: [sbgECom Documentation](https://developer.sbg-systems.com/sbgECom/). + +Backward compatibility has been maintained as much as possible; however, please review the **Migration.md** file for further details on transitioning to this version. ## Import Highlights for this release -Please find below, the main improvements and modifications: - - Support for latest firmware - - Added EFK based rotation and accelerations outputs - - Added body velocity output from the INS filter - - Added GNSS spoofing/jamming/OSNMA indicators - - Added several GNSS related monitoring data - - Completely rewritten sbgBasicLogger with a lot of new features - - General code cleanup and improvements +Below are the key improvements and modifications in this release: + - Support for the new ELLIPSE firmware v3 + - Support for output rates of up to 1 kHz + - Added device info/settings streaming + - Completely rewritten sbgECom protocol documentation + - General code cleanup and improvements for better maintainability + +## Release - 5.1.708-stable + +### New Features + - SBGECOM-433 - Added SBG_ECOM_FEATURE_IM / OSNMA to report GNSS capabilities + - SBGECOM-432 - Doc: Add instruction for the magnetic factory calibration + - SBGECOM-426 - Doc: Explain standard/high range scale factor for SBG_ECOM_LOG_IMU_SHORT output + - SBGECOM-425 - Add SBG_ECOM_LOG_PTP_STATUS message to report PTP time offset and status + - SBGECOM-423 - Returns the CPU usage in the SBG_ECOM_LOG_STATUS message + - SBGECOM-422 - Logger: updated EKF euler output to use only degrees instead of radians + - SBGECOM-420 - Output: Add magnetic heading/declination/inclination in EKF Euler/Quat logs + - SBGECOM-419 - Add proper support for more than 200Hz output logs for ELLIPSE firmware v3 + - SBGECOM-414 - Add support for up to 1 kHz Sync Out events for ELLIPSE firmware v3 + - SBGECOM-413 - Add support for new straight GNSS NMEA outputs logs (GGA/RMC/VTG/HDT) + - SBGECOM-411 - Add new motion profiles to better support new ELLIPSE firmware v3 + - SBGECOM-408 - Add a new sbgECom output log with advanced internal INS filter debug data + - SBGECOM-407 - Add support for Nortek DVL + - SBGECOM-406 - Add SBG_ECOM_SOL_ZARU_USED used in solution bitmask + - SBGECOM-402 - Add a new SBG_ECOM_CLASS_LOG_ALL option to disabled/enable all outputs on an interface + - SBGECOM-394 - Add SBG_ECOM_LOG_SESSION_INFO message to stream settings & device info + - SBGECOM-393 - Added in SBG_ECOM_CMD_SET_MAG_CALIB a field to indicate if the magnetic calibration is 2D or 3D + - SBGECOM-381 - Add a high range scale variant for IMU Short output logs + +### Improvements + - SBGECOM-448 - Improve public CMake build process and documentation + - SBGECOM-431 - New sbgEComMinimal example that supports all Serial and Ethernet devices + - SBGECOM-429 - Doc: Improved Ship Motion description about output monitoring points + - SBGECOM-428 - Doc: Completly reworked documentation with built-in firmware reference manual + - SBGECOM-427 - Improved overall doxygen code base documentation + - SBGECOM-424 - Updated unix serial interface to report error with SBG_LOG_XXXXX macros + - SBGECOM-404 - Doc: Add sbgECom verion to the online URL documentation + - SBGECOM-399 - Add new error models for external sbgECom protocol magnetometers support + - SBGECOM-305 - Improved documentation for extended sbgECom frame definition + +### Bug Fixes + - SBGECOM-439 - Fix typo for SGB_CALIB_MAG_NOT_ENOUGH_POINTS + - SBGECOM-409 - sbgBasicLogger: Fixed erroneous output times for utcIso8601 mode + - SBGECOM-405 - CAN: Fix depth and air data airspeed logs types from integers to float values + - SBGECOM-398 - Removed unused SBG_ECOM_SYNC_OUT_MODE_EVENT_IN_# definitions + - SBGECOM-391 - sbgBasicLogger: Fixed invalid GNSS#_HDT output for numSvTracked, numSvUsed fields + +### Removed Features + - SBGECOM-430 - Magnetometer: Remove robust magnetic model (not relevant anymore) + - SBGECOM-410 - Doc: Indicate all 'preciseInstallation' fields to be deprecated ## Release - 4.0.1987-stable @@ -314,7 +361,7 @@ Please find below, the main improvements and modifications: - SBGECOM-20 - Better error checking for sbgStreamBuffer with new method sbgStreamBufferGetLastError - SBGECOM-22 - Added UTC & Clock status to the binary log SbgLogUtcData - SBGECOM-23 - Added Solution status to the binary log SbgLogEkfEuler, SbgLogEkfQuat, SbgLogEkfNav - - SBGECOM-24 - Added time stamp to the log SBG_ECOM_LOG_MAG_CALIB + - SBGECOM-24 - Added timestamp to the log SBG_ECOM_LOG_MAG_CALIB ## Release - 1.2 diff --git a/CMakeLists.txt b/CMakeLists.txt index 7a6dc67..42d1404 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,35 +1,40 @@ cmake_minimum_required(VERSION 3.16 FATAL_ERROR) -set(CMAKE_BUILD_TYPE Release CACHE STRING "build type") - -project(sbgECom) +# Set project metadata +project(sbgECom VERSION 5.1.708 LANGUAGES C;CXX) # # Compiler configuration # set(CMAKE_C_STANDARD 99) -set(CMAKE_C_STANDARD_REQUIRED ON) +set(CMAKE_C_STANDARD_REQUIRED YES) set(CMAKE_C_EXTENSIONS ON) set(CMAKE_POSITION_INDEPENDENT_CODE ON) set(CMAKE_CXX_STANDARD 14) -set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_STANDARD_REQUIRED YES) # -# Project configuration +# Project configuration options # option(BUILD_EXAMPLES "Build examples" OFF) option(BUILD_TOOLS "Build tools" OFF) option(USE_DEPRECATED_MACROS "Enable deprecated preprocessor defines and macros" ON) -if (USE_DEPRECATED_MACROS) - message(NOTICE "deprecated definitions, macros and enum values enabled") - add_compile_definitions(SBG_ECOM_USE_DEPRECATED_MACROS) -endif() +# Display chosen options +message(STATUS "C Standard: ${CMAKE_C_STANDARD}") +message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}") +message(STATUS "Build Examples: ${BUILD_EXAMPLES}") +message(STATUS "Build Tools: ${BUILD_TOOLS}") +message(STATUS "Use Deprecated Macros: ${USE_DEPRECATED_MACROS}") # # sbgECom library # +if (USE_DEPRECATED_MACROS) + add_compile_definitions(SBG_ECOM_USE_DEPRECATED_MACROS) +endif() + add_library(${PROJECT_NAME} STATIC) file(GLOB_RECURSE COMMON_SRC ${PROJECT_SOURCE_DIR}/common/*.c) @@ -37,135 +42,155 @@ file(GLOB_RECURSE ECOM_SRC ${PROJECT_SOURCE_DIR}/src/*.c) # Exclude platform specific files if (NOT MSVC) - list(REMOVE_ITEM COMMON_SRC ${PROJECT_SOURCE_DIR}/common/interfaces/sbgInterfaceSerialWin.c) + 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) + list(REMOVE_ITEM COMMON_SRC ${PROJECT_SOURCE_DIR}/common/interfaces/sbgInterfaceSerialUnix.c) endif() target_sources(${PROJECT_NAME} PRIVATE ${COMMON_SRC} ${ECOM_SRC}) target_include_directories(${PROJECT_NAME} - PRIVATE - ${PROJECT_SOURCE_DIR}/src - ${PROJECT_SOURCE_DIR}/common - INTERFACE - $ - $ - $) + PRIVATE + ${PROJECT_SOURCE_DIR}/src + ${PROJECT_SOURCE_DIR}/common + INTERFACE + $ + $ + $) target_compile_definitions(${PROJECT_NAME} PUBLIC SBG_COMMON_STATIC_USE) if (MSVC) - target_compile_definitions(${PROJECT_NAME} PUBLIC _CRT_SECURE_NO_WARNINGS) - target_link_libraries(${PROJECT_NAME} PUBLIC Ws2_32) + target_compile_definitions(${PROJECT_NAME} PUBLIC _CRT_SECURE_NO_WARNINGS) + target_link_libraries(${PROJECT_NAME} PUBLIC Ws2_32) +endif() + +# +# Fetch and make available dependencies for both Tools and Examples code +# +if (BUILD_EXAMPLES OR BUILD_TOOLS) + include(FetchContent) + + # Set options for argtable3 before fetching it + set(BUILD_SHARED_LIBS OFF CACHE BOOL "Build shared library") + set(ARGTABLE3_ENABLE_CONAN OFF CACHE BOOL "Enable Conan dependency manager") + set(ARGTABLE3_ENABLE_TESTS OFF CACHE BOOL "Enable unit tests") + set(ARGTABLE3_ENABLE_EXAMPLES OFF CACHE BOOL "Enable examples") + + FetchContent_Declare( + argtable3 + GIT_REPOSITORY https://github.com/argtable/argtable3.git + GIT_TAG v3.2.2.f25c624 + ) + + FetchContent_MakeAvailable(argtable3) endif() # # Examples # if (BUILD_EXAMPLES) - add_executable(airDataInput ${PROJECT_SOURCE_DIR}/examples/airDataInput/src/airDataInput.c) - target_link_libraries(airDataInput ${PROJECT_NAME}) - install(TARGETS airDataInput DESTINATION bin/examples COMPONENT executables) - - add_executable(ellipseMinimal ${PROJECT_SOURCE_DIR}/examples/ellipseMinimal/src/ellipseMinimal.c) - target_link_libraries(ellipseMinimal ${PROJECT_NAME}) - install(TARGETS ellipseMinimal DESTINATION bin/examples COMPONENT executables) - - add_executable(ellipseOnboardMagCalib ${PROJECT_SOURCE_DIR}/examples/ellipseOnboardMagCalib/src/ellipseOnboardMagCalib.c) - target_link_libraries(ellipseOnboardMagCalib ${PROJECT_NAME}) - install(TARGETS ellipseOnboardMagCalib DESTINATION bin/examples COMPONENT executables) - - add_executable(hpInsMinimal ${PROJECT_SOURCE_DIR}/examples/hpInsMinimal/src/hpInsMinimal.c) - target_link_libraries(hpInsMinimal ${PROJECT_NAME}) - install(TARGETS hpInsMinimal DESTINATION bin/examples COMPONENT executables) - - add_executable(pulseMinimal ${PROJECT_SOURCE_DIR}/examples/pulseMinimal/src/pulseMinimal.c) - target_link_libraries(pulseMinimal ${PROJECT_NAME}) - install(TARGETS pulseMinimal DESTINATION bin/examples COMPONENT executables) -endif(BUILD_EXAMPLES) + # Build airDataInput example + add_executable(airDataInput ${PROJECT_SOURCE_DIR}/examples/airDataInput/src/main.c) + target_link_libraries(airDataInput PRIVATE ${PROJECT_NAME}) + install(TARGETS airDataInput DESTINATION bin/examples COMPONENT executables) + + # Build ellipseLegacy example + add_executable(ellipseLegacy ${PROJECT_SOURCE_DIR}/examples/ellipseLegacy/src/main.c) + target_link_libraries(ellipseLegacy PRIVATE ${PROJECT_NAME}) + install(TARGETS ellipseLegacy DESTINATION bin/examples COMPONENT executables) + + # Build ellipseOnboardMagCalib example + add_executable(ellipseOnboardMagCalib ${PROJECT_SOURCE_DIR}/examples/ellipseOnboardMagCalib/src/main.c) + target_link_libraries(ellipseOnboardMagCalib PRIVATE ${PROJECT_NAME}) + install(TARGETS ellipseOnboardMagCalib DESTINATION bin/examples COMPONENT executables) + + # Build sbgEComExample + add_executable(sbgEComExample + ${PROJECT_SOURCE_DIR}/examples/sbgEComExample/src/cJSON.c + ${PROJECT_SOURCE_DIR}/examples/sbgEComExample/src/main.c + ${PROJECT_SOURCE_DIR}/examples/sbgEComExample/src/restApiHelper.c) + + target_include_directories(sbgEComExample PRIVATE ${argtable3_SOURCE_DIR}/src) + target_link_libraries(sbgEComExample PRIVATE ${PROJECT_NAME} argtable3) + install(TARGETS sbgEComExample DESTINATION bin/examples COMPONENT executables) +endif() # # Tools # if (BUILD_TOOLS) - include(FetchContent) - - FetchContent_Declare(argtable3 - GIT_REPOSITORY https://github.com/argtable/argtable3.git - GIT_TAG v3.1.5.1c1bb23 - ) - - FetchContent_GetProperties(argtable3) - - if (NOT argtable3_POPULATED) - FetchContent_Populate(argtable3) - add_subdirectory(${argtable3_SOURCE_DIR} ${argtable3_BINARY_DIR} EXCLUDE_FROM_ALL) - endif() - - # - # sbgBasicLogger tool - log sbgECom logs to CSV like files - # - add_executable(sbgBasicLogger - ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/loggerEntry/helpers/imuDataMean.h - ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/loggerEntry/loggerEntryAidings.h - ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/loggerEntry/loggerEntryDvl.h - ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/loggerEntry/loggerEntryEkf.h - ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/loggerEntry/loggerEntryEkfRotAccel.h - ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/loggerEntry/loggerEntryEvent.h - ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/loggerEntry/loggerEntryGeneral.h - ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/loggerEntry/loggerEntryGnss.h - ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/loggerEntry/loggerEntryImu.h - ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/loggerEntry/loggerEntryMag.h - ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/loggerEntry/loggerEntryShipMotion.h - ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/loggerManager/loggerContext.h - ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/loggerManager/loggerEntry.h - ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/loggerManager/loggerManager.h - ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/loggerManager/loggerSettings.h - ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/loggerApp.h - ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/loggerEntry/helpers/imuDataMean.cpp - ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/loggerEntry/loggerEntryAidings.cpp - ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/loggerEntry/loggerEntryDvl.cpp - ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/loggerEntry/loggerEntryEkf.cpp - ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/loggerEntry/loggerEntryEkfRotAccel.cpp - ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/loggerEntry/loggerEntryEvent.cpp - ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/loggerEntry/loggerEntryGeneral.cpp - ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/loggerEntry/loggerEntryGnss.cpp - ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/loggerEntry/loggerEntryImu.cpp - ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/loggerEntry/loggerEntryMag.cpp - ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/loggerEntry/loggerEntryShipMotion.cpp - ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/loggerManager/loggerContext.cpp - ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/loggerManager/loggerEntry.cpp - ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/loggerManager/loggerManager.cpp - ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/loggerManager/loggerSettings.cpp - ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/loggerApp.cpp - ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/main.cpp - ) - target_include_directories(sbgBasicLogger - PRIVATE ${argtable3_SOURCE_DIR}/src - PRIVATE ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src - ) - target_link_libraries(sbgBasicLogger ${PROJECT_NAME} argtable3_static) - install(TARGETS sbgBasicLogger DESTINATION bin/tools/sbgBasicLogger COMPONENT executables) - install(FILES tools/sbgBasicLogger/README.md DESTINATION bin/tools/sbgBasicLogger COMPONENT executables) - - # - # sbgEComApi tool - use sbgInsRestApi GET/POS method over a serial port - # - add_executable(sbgEComApi ${PROJECT_SOURCE_DIR}/tools/sbgEComApi/src/main.c) - target_include_directories(sbgEComApi PRIVATE ${argtable3_SOURCE_DIR}/src) - target_link_libraries(sbgEComApi ${PROJECT_NAME} argtable3_static) - install(TARGETS sbgEComApi DESTINATION bin/tools/sbgEComApi COMPONENT executables) - install(FILES tools/sbgEComApi/README.md DESTINATION bin/tools/sbgEComApi COMPONENT executables) -endif(BUILD_TOOLS) + # List all source/header files for sbgBasicLogger tool + file(GLOB_RECURSE SBGBASICLOGGER_SRC + ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/*.cpp + ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/*.h + ) + + # Build sbgBasicLogger tool + add_executable(sbgBasicLogger ${SBGBASICLOGGER_SRC}) + target_include_directories(sbgBasicLogger + PRIVATE ${argtable3_SOURCE_DIR}/src + PRIVATE ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src + ) + target_link_libraries(sbgBasicLogger PRIVATE ${PROJECT_NAME} argtable3) + + install(TARGETS sbgBasicLogger DESTINATION bin/tools/sbgBasicLogger COMPONENT executables) + install(FILES tools/sbgBasicLogger/README.md DESTINATION bin/tools/sbgBasicLogger COMPONENT executables) + + # Build sbgEComApi tool + add_executable(sbgEComApi ${PROJECT_SOURCE_DIR}/tools/sbgEComApi/src/main.c) + target_include_directories(sbgEComApi PRIVATE ${argtable3_SOURCE_DIR}/src) + target_link_libraries(sbgEComApi PRIVATE ${PROJECT_NAME} argtable3) + + install(TARGETS sbgEComApi DESTINATION bin/tools/sbgEComApi COMPONENT executables) + install(FILES tools/sbgEComApi/README.md DESTINATION bin/tools/sbgEComApi COMPONENT executables) +endif() # -# Install +# Install the main library target # -install(TARGETS ${PROJECT_NAME} EXPORT sbgEComTargets) + +# Export the target so it can be used by other projects +install(TARGETS ${PROJECT_NAME} + EXPORT sbgEComTargets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include) + +# Export CMake targets for use in other projects install(EXPORT sbgEComTargets FILE sbgEComTargets.cmake - NAMESPACE sbg:: - DESTINATION lib/cmake/sbg) -install(DIRECTORY common/ TYPE INCLUDE FILES_MATCHING REGEX ".*\\.h") -install(DIRECTORY src/ TYPE INCLUDE FILES_MATCHING REGEX ".*\\.h") + NAMESPACE sbgECom:: + DESTINATION lib/cmake/sbgECom) + +# Create a version file for compatibility checks +include(CMakePackageConfigHelpers) +write_basic_package_version_file( + "${CMAKE_CURRENT_BINARY_DIR}/sbgEComConfigVersion.cmake" + VERSION 5.1.708 + COMPATIBILITY SameMajorVersion +) + +# Create a config file to describe the package +configure_package_config_file( + "${CMAKE_CURRENT_SOURCE_DIR}/cmake/sbgEComConfig.in.cmake" + "${CMAKE_CURRENT_BINARY_DIR}/sbgEComConfig.cmake" + INSTALL_DESTINATION lib/cmake/sbgECom +) + +# Install cmake config and version files +install(FILES + "${CMAKE_CURRENT_BINARY_DIR}/sbgEComConfig.cmake" + "${CMAKE_CURRENT_BINARY_DIR}/sbgEComConfigVersion.cmake" + DESTINATION lib/cmake/sbgECom +) + +# Install header files from common and src directories +install(DIRECTORY common/ + DESTINATION include + FILES_MATCHING PATTERN "*.h") + +install(DIRECTORY src/ + DESTINATION include + FILES_MATCHING PATTERN "*.h") diff --git a/LICENSE.md b/LICENSE.md index 0117237..258ecf1 100644 --- a/LICENSE.md +++ b/LICENSE.md @@ -1,7 +1,7 @@ -# License +# License {#license} The MIT License (MIT) -Copyright (C) 2022, SBG Systems SAS. All rights reserved. +Copyright (C) 2007-2024, 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 diff --git a/README.md b/README.md index 310e38d..1fdb99c 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,7 @@ -# sbgECom Library +# sbgECom Library 5.1.708-stable sbgECom is a C library provided under the MIT License and used to interface easily [SBG Systems](https://www.sbg-systems.com/) IMU, AHRS and INS to your application. +## Introduction This library has been designed to be platform and OS independent and can safely be used on small micro-controller or larger multi-core CPUs. This package contains the following items: - sbgECom library full C source code @@ -13,119 +14,28 @@ The pre-compiled sbgECom examples and tools are 64 bits binaries available on Wi The library is written and maintained by SBG Systems SAS. You can contact the support@sbg-systems.com for if you need assistance. -# Documentation +## Documentation -You can access the full online sbgECom Doxygen documentation [here](https://developer.sbg-systems.com/sbgECom/4.0). +You can access the full online sbgECom Doxygen documentation [here](https://developer.sbg-systems.com/sbgECom/5.1). You should also read the SBG Systems [Support Center](https://support.sbg-systems.com) to quickly start using and integrating your products. Please also have a look at the [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi/) documentation that is used to configure High Performance INS products. -# Platform support -The library has been designed to be easily ported to any platform by just providing/implementing a few low level methods and some configuration: - - The platform configuration is set in `common/sbgConfig.h` - - In the file `common/platform/sbgPlatform.c` please provide _sbgGetTime_, _sbgSleep_ and _sbgPlatformDebugLogMsg_ - - In the directory `common/interfaces/` you can provide custom open/read/write implementations for serial communications +## Maintenance and Contribution -# Building sbgECom Library -The sbgECom library and code examples are very easy to compile on any platform using CMake. -The sbgECom library and code samples have no third party library dependencies. +This documentation and the sbgECom C Library are developed and maintained by SBG Systems SAS. +For assistance, feedback, or to report issues, please contact us at [support@sbg-systems.com](mailto:support@sbg-systems.com). -However, if you would like to build sbgECom CLI tools such as the `sbgBasicLogger` or `sbgInsRestApi` there is a dependency on Argtable3. +The sbgECom C Library and code sample is available on GitHub as a public repository. +While you can open issues to report bugs or suggest improvements, please do not submit pull requests as we do not use GitHub for development purposes and cannot merge them. -SBG Systems doesn't provide the sbgECom as a pre-compiled library for obvious and good reasons. +## Support -## Dependencies -SBG Systems has validated the following tool-chain and libraries: -- \>= CMake 3.0 -- \>= GNU GCC 8 (any platform) -- \>= AppleClang 13 (Mac OS X) -- \>= Visual Studio 2015 or MSBuild equivalent (Windows) -- \>= Argtable3 (to build sbgECom tools) -- \>= Git (to fetch Argtable3) +If you need assistance with the sbgECom C Library or have any questions, please refer to the following resources: -## Building sbgECom -To build the sbgECom static library, the C example and the command line tools go to the sbgECom library folder and type the following commands: +1. **Documentation**: Detailed documentation is available within the repository and covers the usage, installation, and API of the sbgECom library. +2. **Issues**: If you encounter any bugs or have feature requests, please open an issue on the [GitHub issues page](https://github.com/SBG-Systems/sbgECom/issues). Our team will review and address it as soon as possible. +3. **Email Support**: For more direct support, including technical questions or troubleshooting, contact us at [support@sbg-systems.com](mailto:support@sbg-systems.com). Please include as much detail as possible in your request to help us assist you effectively. +4. **Online Support:** You can also visit our [Support Page](https://www.sbg-systems.com/support/) for additional resources, including product manuals, FAQs, and contact information for our technical support team. -```sh -cmake -Bbuild -DBUILD_EXAMPLES=ON -DBUILD_TOOLS=ON -cmake --build build -``` - -You should find the sbgECom static library, examples and tools binaries in the `build/Debug` folder. - -> **Disable Deprecated Macros** -> Make sure to add `-DUSE_DEPRECATED_MACROS=OFF` to disable support of deprecated defines, macros and enum values. - -# Code Examples -SBG Systems provides several and simple C code examples to quickly use the sbgECom library. -You can find both the source code as well as a binary for each example. - -All examples source code are located in the `examples` directory. You can find pre-compiled 64 bits binaries in the `bin/examples` folder. - -## Ellipse Minimal -Simple C example to illustrate how to connect and read data from an ELLIPSE using the sbgECom library. - -You can test this example using the command below: - -```sh -ellipseMinimal COM4 115200 -``` - -## High Performance INS Minimal -Simple C example to illustrate how to read data from an High Performance INS over an UDP connection and using the sbgECom library. - -You can test this example using the command below. The INS ip address is *192.168.1.1* and send logs on the UDP port *1234*: - -```sh -hpInsMinimal COM4 192.168.1.1 5678 1234 -``` - -## Pulse Minimal -Simple C example to illustrate how to connect and read data from a PULSE IMU using the sbgECom library. - -You can test this example using the command below: - -```sh -pulseMinimal COM4 921600 -``` - -## Ellipse On Board Magnetic Calibration -Simple C example to illustrate how to use the ELLIPSE on board magnetic calibration algorithms. - -You can test this example using the command below: - -```sh -ellipseOnBoardMagCalib COM4 115200 -``` - -## Air Date Input -Simple C example to illustrate how to send air date aiding measurements to an ELLIPSE using the sbgECom library. - -You can test this example using the command below: - -```sh -airDataInput COM4 115200 -``` - -# Command Line Tools -SBG Systems offers two very useful tools to ease evaluation and integration. You can find the C source code for each tool in the `tools` directory. -The `bin/tools` directory contains pre-compiled 64 bits binaries for Windows, Linux and Mac OS X platforms. - -> Please read the dedicated README.md files provided with each tool. - -## sbgBasicLogger -Simply parse sbgECom logs from a serial or ethernet interface and write log content to CSV like files. -This tool can also read sbgECom logs from a binary file making it very interesting to convert ELLIPSE binary streams to easy to use text files. -It can also extract RAW GNSS data stream as well as real time differential correction (RTCM) stream to binary files. - -## sbgEComApi -Easily access sbgInsRest API configuration over a serial or UDP interface. You can execute GET and POST queries using simple to use command lines arguments. -This tool is perfect if you would like to setup a High Performance INS product over a serial or ethernet interface and using only bash scripts for example. - -# CAN messages -SBG Systems provides a DBC and BusMaster CAN messages database definition to quickly interface your product with a CAN logger and application. -You can find these CAN database in the `can` directory - - - -Read Next: [Migrations](doc/migrations.md) - +Please note that while we appreciate contributions, we do not accept pull requests for this project on GitHub. +You are welcome to suggest improvements or report issues via the methods above. diff --git a/cmake/sbgEComConfig.in.cmake b/cmake/sbgEComConfig.in.cmake new file mode 100644 index 0000000..2d6031d --- /dev/null +++ b/cmake/sbgEComConfig.in.cmake @@ -0,0 +1,5 @@ +# cmake/sbgEComConfig.cmake.in + +@PACKAGE_INIT@ + +include("${CMAKE_CURRENT_LIST_DIR}/sbgEComTargets.cmake") \ No newline at end of file diff --git a/common/crc/sbgCrc.c b/common/crc/sbgCrc.c index 3b5c5f6..dffd747 100644 --- a/common/crc/sbgCrc.c +++ b/common/crc/sbgCrc.c @@ -1,3 +1,7 @@ +// sbgCommonLib headers +#include + +// Local headers #include "sbgCrc.h" //----------------------------------------------------------------------// @@ -6,22 +10,22 @@ /*!< 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}; + 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] = @@ -81,191 +85,134 @@ static const uint32_t crc32EthernetTable[256] = }; //----------------------------------------------------------------------// -//- 32 bits Ethernet CRC -// +//- 32 bits CRC methods -// //----------------------------------------------------------------------// -/*! - * 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); + assert(pInstance); - *pInstance = 0xFFFFFFFF; + *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]; - } + const uint8_t *pBuffer = (const uint8_t*)pData; + uint32_t byte; + size_t dataSizeCorrected; + size_t numBytesLeft; + size_t index; + size_t i; + + 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); + // + // 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]; + } +} - // - // Get the current byte value - // - byte = pBuffer[index]; +SBG_COMMON_LIB_API uint32_t sbgCrc32Get(const SbgCrc32 *pInstance) +{ + assert(pInstance); - // - // Update the CRC value - // - *pInstance = (*pInstance << 8) ^ crc32EthernetTable[((*pInstance >> 24) ^ byte) & 0xFF]; - } + 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) { - SbgCrc32 crcInst; - - // - // Initialize the CRC system - // - sbgCrc32Initialize(&crcInst); + SbgCrc32 crcInst; - // - // Compute the CRC - // - sbgCrc32Update(&crcInst, pData, dataSize); + sbgCrc32Initialize(&crcInst); + sbgCrc32Update(&crcInst, pData, dataSize); - // - // Return it - // - return sbgCrc32Get(&crcInst); + return sbgCrc32Get(&crcInst); } //----------------------------------------------------------------------// -//- CRC-16 operations -// +//- 16 bits CRC methods -// //----------------------------------------------------------------------// -/*! - * 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); + assert(pInstance); - *pInstance = 0; + *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; + const uint8_t *pBuffer = (const uint8_t*)pData; + uint8_t index; + size_t i; - // - // Test input arguments - // - assert(pInstance); - assert(pData); + 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) +SBG_COMMON_LIB_API uint16_t sbgCrc16Get(const SbgCrc16 *pInstance) { - SbgCrc16 crcInst; + assert(pInstance); + + return *pInstance; +} - // - // Initialize the CRC system - // - sbgCrc16Initialize(&crcInst); +SBG_COMMON_LIB_API uint16_t sbgCrc16Compute(const void *pData, size_t dataSize) +{ + SbgCrc16 crcInst; - // - // Compute the CRC - // - sbgCrc16Update(&crcInst, pData, dataSize); + sbgCrc16Initialize(&crcInst); + sbgCrc16Update(&crcInst, pData, dataSize); - // - // Return it - // - return sbgCrc16Get(&crcInst); + return sbgCrc16Get(&crcInst); } diff --git a/common/crc/sbgCrc.h b/common/crc/sbgCrc.h index 9d945de..e9de7e2 100644 --- a/common/crc/sbgCrc.h +++ b/common/crc/sbgCrc.h @@ -1,13 +1,13 @@ /*! - * \file sbgCrc.h - * \ingroup common - * \author SBG Systems - * \date 15 January 2013 + * \file sbgCrc.h + * \ingroup common + * \author SBG Systems + * \date 15 January 2013 * - * \brief This file provides CRC-32 and CRC-16 methods. + * \brief This file provides CRC-32 and CRC-16 methods. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -52,76 +52,84 @@ typedef uint32_t SbgCrc32; typedef uint16_t SbgCrc16; //----------------------------------------------------------------------// -//- 32 bits Ethernet CRC -// +//- 32 bits CRC methods -// //----------------------------------------------------------------------// /*! - * Initialize the 32 bit CRC computation system. - * \param[in] pInstance Pointer on an allocated but non initialized Crc32 object. + * 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. + * 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. + * 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; -} +SBG_COMMON_LIB_API uint32_t sbgCrc32Get(const SbgCrc32 *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. + * Compute a 32 bit CRC using an Ethernet polynome and return it. + * + * This helper methods calls sbgCrc16Initialize, sbgCrc16Update and sbgCrc16Get at once. + * + * \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 -// +//- 16 bits CRC methods -// //----------------------------------------------------------------------// /*! - * Initialize the 16 bit CRC computation system. - * \param[in] pInstance Pointer on an allocated but non initialized Crc16 object. + * 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. + * 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. + * Returns the computed 16 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; -} +SBG_COMMON_LIB_API uint16_t sbgCrc16Get(const SbgCrc16 *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. + * Compute a 16 bit CRC using an the polynome 0x8408 and return it. + * + * This helper methods calls sbgCrc16Initialize, sbgCrc16Update and sbgCrc16Get at once. + * + * \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); @@ -132,4 +140,4 @@ SBG_COMMON_LIB_API uint16_t sbgCrc16Compute(const void *pData, size_t dataSize); } #endif -#endif /* SBG_CRC_H */ +#endif // SBG_CRC_H diff --git a/common/debug/sbgDebug.c b/common/debug/sbgDebug.c index fdf22cb..0b69b02 100644 --- a/common/debug/sbgDebug.c +++ b/common/debug/sbgDebug.c @@ -1,22 +1,3 @@ -/*! - * \file sbgDebug.c - * \author SBG Systems - * - * \brief Error logging for the SBG Systems common C library. - * - * \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. - */ - // Standard headers #include @@ -33,12 +14,12 @@ /*! * Number of bytes translated per line. */ -#define SBG_DEBUG_NR_BYTES_PER_LINE (16) +#define SBG_DEBUG_NR_BYTES_PER_LINE (16) /*! * Size of the buffer used to generate lines, in bytes. */ -#define SBG_DEBUG_LINE_BUFFER_SIZE (256) +#define SBG_DEBUG_LINE_BUFFER_SIZE (256) //----------------------------------------------------------------------// //- Private functions -// @@ -47,63 +28,63 @@ /*! * Dump the given buffer on a single text line. * - * \param[out] pLine Output line buffer. - * \param[in] lineSize Size of the ouput line buffer. - * \param[in] pBuffer Buffer. - * \param[in] size Buffer size. + * \param[out] pLine Output line buffer. + * \param[in] lineSize Size of the ouput line buffer. + * \param[in] pBuffer Buffer. + * \param[in] size Buffer size. */ static void sbgDebugHexDumpGenerateLine(char *pLine, size_t lineSize, const uint8_t *pBuffer, size_t size) { - size_t length; - - assert(pLine); - assert(lineSize != 0); - assert(pBuffer); - assert(size <= SBG_DEBUG_NR_BYTES_PER_LINE); - - for (size_t i = 0; i < size; i++) - { - length = snprintf(pLine, lineSize, "%02x ", pBuffer[i]); - assert(length < lineSize); - - pLine = &pLine[length]; - lineSize -= length; - } - - for (size_t i = size; i < SBG_DEBUG_NR_BYTES_PER_LINE; i++) - { - length = snprintf(pLine, lineSize, " "); - assert(length < lineSize); - - pLine = &pLine[length]; - lineSize -= length; - } - - length = snprintf(pLine, lineSize, " | "); - assert(length < lineSize); - - pLine = &pLine[length]; - lineSize -= length; - - for (size_t i = 0; i < size; i++) - { - char c; - - if (isprint(pBuffer[i])) - { - c = pBuffer[i]; - } - else - { - c = '.'; - } - - length = snprintf(pLine, lineSize, "%c", c); - assert(length < lineSize); - - pLine = &pLine[length]; - lineSize -= length; - } + size_t length; + + assert(pLine); + assert(lineSize != 0); + assert(pBuffer); + assert(size <= SBG_DEBUG_NR_BYTES_PER_LINE); + + for (size_t i = 0; i < size; i++) + { + length = snprintf(pLine, lineSize, "%02x ", pBuffer[i]); + assert(length < lineSize); + + pLine = &pLine[length]; + lineSize -= length; + } + + for (size_t i = size; i < SBG_DEBUG_NR_BYTES_PER_LINE; i++) + { + length = snprintf(pLine, lineSize, " "); + assert(length < lineSize); + + pLine = &pLine[length]; + lineSize -= length; + } + + length = snprintf(pLine, lineSize, " | "); + assert(length < lineSize); + + pLine = &pLine[length]; + lineSize -= length; + + for (size_t i = 0; i < size; i++) + { + char c; + + if (isprint(pBuffer[i])) + { + c = pBuffer[i]; + } + else + { + c = '.'; + } + + length = snprintf(pLine, lineSize, "%c", c); + assert(length < lineSize); + + pLine = &pLine[length]; + lineSize -= length; + } } //----------------------------------------------------------------------// @@ -112,42 +93,42 @@ static void sbgDebugHexDumpGenerateLine(char *pLine, size_t lineSize, const uint void sbgDebugHexDump(const char *pPrefix, const void *pBuffer, size_t size) { - char line[SBG_DEBUG_LINE_BUFFER_SIZE]; - size_t index = 0; - bool multiLine = false; - - assert(pPrefix); - assert(pBuffer || (size == 0)); - - if (size > SBG_DEBUG_NR_BYTES_PER_LINE) - { - SBG_LOG_DEBUG("%s: multi-line dump start (%zu bytes)", pPrefix, size); - multiLine = true; - } - - while (size != 0) - { - const uint8_t *pByteBuffer = pBuffer; - size_t rangeSize; - - if (size < SBG_DEBUG_NR_BYTES_PER_LINE) - { - rangeSize = size; - } - else - { - rangeSize = SBG_DEBUG_NR_BYTES_PER_LINE; - } - - sbgDebugHexDumpGenerateLine(line, sizeof(line), &pByteBuffer[index], rangeSize); - SBG_LOG_DEBUG("%s: %s", pPrefix, line); - - size -= rangeSize; - index += rangeSize; - } - - if (multiLine) - { - SBG_LOG_DEBUG("%s: multi-line dump end", pPrefix); - } + char line[SBG_DEBUG_LINE_BUFFER_SIZE]; + size_t index = 0; + bool multiLine = false; + + assert(pPrefix); + assert(pBuffer || (size == 0)); + + if (size > SBG_DEBUG_NR_BYTES_PER_LINE) + { + SBG_LOG_DEBUG("%s: multi-line dump start (%zu bytes)", pPrefix, size); + multiLine = true; + } + + while (size != 0) + { + const uint8_t *pByteBuffer = pBuffer; + size_t rangeSize; + + if (size < SBG_DEBUG_NR_BYTES_PER_LINE) + { + rangeSize = size; + } + else + { + rangeSize = SBG_DEBUG_NR_BYTES_PER_LINE; + } + + sbgDebugHexDumpGenerateLine(line, sizeof(line), &pByteBuffer[index], rangeSize); + SBG_LOG_DEBUG("%s: %s", pPrefix, line); + + size -= rangeSize; + index += rangeSize; + } + + if (multiLine) + { + SBG_LOG_DEBUG("%s: multi-line dump end", pPrefix); + } } diff --git a/common/debug/sbgDebug.h b/common/debug/sbgDebug.h index 35a6e71..923b718 100644 --- a/common/debug/sbgDebug.h +++ b/common/debug/sbgDebug.h @@ -1,16 +1,16 @@ /*! - * \file sbgDebug.h - * \ingroup common - * \author SBG Systems - * \date 17 March 2015 + * \file sbgDebug.h + * \ingroup common + * \author SBG Systems + * \date 17 March 2015 * - * \brief Define and handle error logging for the SBG Systems common C library. + * \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. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -39,7 +39,7 @@ #include #ifndef SBG_DEBUG_LOG_CATEGORY - #define SBG_DEBUG_LOG_CATEGORY ("None") + #define SBG_DEBUG_LOG_CATEGORY ("None") #endif //----------------------------------------------------------------------// @@ -47,14 +47,14 @@ //----------------------------------------------------------------------// /*! - * Enum that identify the type of error / warning that has been thrown. + * 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. */ + 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; //----------------------------------------------------------------------// @@ -62,53 +62,69 @@ typedef enum _SbgDebugLogType //----------------------------------------------------------------------// /*! - * 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). + * \brief Log an error with its associated message. + * + * This macro logs an error message along with the specified error code. The message can include formatted text similar to printf. + * + * \param[in] errorCode The error code that triggered this error. + * \param[in] format The string literal for the associated error message, supporting printf-like formatting. + * \param[in] ... Additional arguments for the format string. */ -#define SBG_LOG_ERROR_CALL(errorCode, format, ...) sbgPlatformDebugLogMsg((const char*)__BASE_FILE__, (const char*)__func__, __LINE__, SBG_DEBUG_LOG_CATEGORY, SBG_DEBUG_LOG_TYPE_ERROR, errorCode, format, ##__VA_ARGS__) +#define SBG_LOG_ERROR_CALL(errorCode, format, ...) sbgPlatformDebugLogMsg((const char*)__BASE_FILE__, (const char*)__func__, __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 + #define SBG_LOG_ERROR SBG_LOG_ERROR_CALL #else - #define SBG_LOG_ERROR(format, ...) ((void)sizeof(SBG_LOG_ERROR_CALL(format, ## __VA_ARGS__), 0)) + #define SBG_LOG_ERROR(format, ...) ((void)sizeof(SBG_LOG_ERROR_CALL(format, ## __VA_ARGS__), 0)) #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). + * \brief Log a warning with its associated message. + * + * This macro logs a warning message along with the specified error code. The message can include formatted text similar to printf. + * + * \param[in] errorCode The error code that triggered this warning. + * \param[in] format The string literal for the associated error message, supporting printf-like formatting. + * \param[in] ... Additional arguments for the format string. */ -#define SBG_LOG_WARNING_CALL(errorCode, format, ...) sbgPlatformDebugLogMsg((const char*)__BASE_FILE__, (const char*)__func__, __LINE__, SBG_DEBUG_LOG_CATEGORY, SBG_DEBUG_LOG_TYPE_WARNING, errorCode, format, ##__VA_ARGS__) +#define SBG_LOG_WARNING_CALL(errorCode, format, ...) sbgPlatformDebugLogMsg((const char*)__BASE_FILE__, (const char*)__func__, __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 + #define SBG_LOG_WARNING SBG_LOG_WARNING_CALL #else - #define SBG_LOG_WARNING(format, ...) ((void)sizeof(SBG_LOG_WARNING_CALL(format, ## __VA_ARGS__), 0)) + #define SBG_LOG_WARNING(format, ...) ((void)sizeof(SBG_LOG_WARNING_CALL(format, ## __VA_ARGS__), 0)) #endif /*! - * Log an information message. - * \param[in] format String litteral for the information message (you can use printf like string formating). + * \brief Log an information message. + * + * This macro logs an information message. The message can include formatted text similar to printf. + * + * \param[in] format The string literal for the associated information message, supporting printf-like formatting. + * \param[in] ... Additional arguments for the format string. */ -#define SBG_LOG_INFO_CALL(format, ...) sbgPlatformDebugLogMsg((const char*)__BASE_FILE__, (const char*)__func__, __LINE__, SBG_DEBUG_LOG_CATEGORY, SBG_DEBUG_LOG_TYPE_INFO, SBG_NO_ERROR, format, ##__VA_ARGS__) +#define SBG_LOG_INFO_CALL(format, ...) sbgPlatformDebugLogMsg((const char*)__BASE_FILE__, (const char*)__func__, __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 + #define SBG_LOG_INFO SBG_LOG_INFO_CALL #else - #define SBG_LOG_INFO(format, ...) ((void)sizeof(SBG_LOG_INFO_CALL(format, ## __VA_ARGS__), 0)) + #define SBG_LOG_INFO(format, ...) ((void)sizeof(SBG_LOG_INFO_CALL(format, ## __VA_ARGS__), 0)) #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). + * \brief Log a a debug message. + * + * This macro logs a debug message, which is only output when compiled in debug mode. The message can include formatted text similar to printf. + * + * \param[in] format The string literal for the associated debug message, supporting printf-like formatting. + * \param[in] ... Additional arguments for the format string. */ -#define SBG_LOG_DEBUG_CALL(format, ...) sbgPlatformDebugLogMsg((const char*)__BASE_FILE__, (const char*)__func__, __LINE__, SBG_DEBUG_LOG_CATEGORY, SBG_DEBUG_LOG_TYPE_DEBUG, SBG_NO_ERROR, format, ##__VA_ARGS__) +#define SBG_LOG_DEBUG_CALL(format, ...) sbgPlatformDebugLogMsg((const char*)__BASE_FILE__, (const char*)__func__, __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 + #define SBG_LOG_DEBUG SBG_LOG_DEBUG_CALL #else - #define SBG_LOG_DEBUG(format, ...) ((void)sizeof(SBG_LOG_DEBUG_CALL(format, ## __VA_ARGS__), 0)) + #define SBG_LOG_DEBUG(format, ...) ((void)sizeof(SBG_LOG_DEBUG_CALL(format, ## __VA_ARGS__), 0)) #endif //----------------------------------------------------------------------// @@ -118,33 +134,33 @@ typedef enum _SbgDebugLogType /*! * Convert a log type into a string representation. * - * \param[in] logType Log type. - * \return String representation for the given log type. + * \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; + 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; } //----------------------------------------------------------------------// @@ -154,10 +170,10 @@ SBG_INLINE const char *sbgDebugLogTypeToStr(SbgDebugLogType logType) /*! * Produce a text dump of a buffer. * - * \param[in] pPrefix Prefix string before each line. - * \param[in] pBuffer Data buffer, may be NULL. - * \param[in] size Data size, in bytes. + * \param[in] pPrefix Prefix string before each line. + * \param[in] pBuffer Data buffer, may be NULL. + * \param[in] size Data size, in bytes. */ void sbgDebugHexDump(const char *pPrefix, const void *pBuffer, size_t size); -#endif /* SBG_DEBUG_H */ +#endif // SBG_DEBUG_H diff --git a/common/interfaces/sbgInterface.c b/common/interfaces/sbgInterface.c index b3b5167..6b2cfe9 100644 --- a/common/interfaces/sbgInterface.c +++ b/common/interfaces/sbgInterface.c @@ -10,11 +10,11 @@ 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" + [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" }; //----------------------------------------------------------------------// @@ -23,58 +23,58 @@ static const char *gInterfaceType[] = void sbgInterfaceZeroInit(SbgInterface *pInterface) { - assert(pInterface); + assert(pInterface); - memset(pInterface, 0x00, sizeof(*pInterface)); + memset(pInterface, 0x00, sizeof(*pInterface)); } SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceDestroy(SbgInterface *pInterface) { - SbgErrorCode errorCode = SBG_NO_ERROR; + SbgErrorCode errorCode = SBG_NO_ERROR; - assert(pInterface); + assert(pInterface); - if (pInterface->pDestroyFunc) - { - errorCode = pInterface->pDestroyFunc(pInterface); - } - - return errorCode; + if (pInterface->pDestroyFunc) + { + errorCode = pInterface->pDestroyFunc(pInterface); + } + + return errorCode; } const char *sbgInterfaceTypeGetAsString(const SbgInterface *pInterface) { - assert(pInterface); + assert(pInterface); - if (pInterface->type < SBG_ARRAY_SIZE(gInterfaceType)) - { - return gInterfaceType[pInterface->type]; - } - else - { - SBG_LOG_ERROR(SBG_INVALID_PARAMETER, "Unknown interface type: %" PRIu32, pInterface->type); - return gInterfaceType[SBG_IF_TYPE_UNKNOW]; - } + if (pInterface->type < SBG_ARRAY_SIZE(gInterfaceType)) + { + return gInterfaceType[pInterface->type]; + } + else + { + SBG_LOG_ERROR(SBG_INVALID_PARAMETER, "Unknown interface type: %" PRIu32, pInterface->type); + return gInterfaceType[SBG_IF_TYPE_UNKNOW]; + } } void sbgInterfaceNameSet(SbgInterface *pInterface, const char *pName) { - size_t nameLen; + size_t nameLen; - assert(pInterface); - assert(pName); + assert(pInterface); + assert(pName); - // - // Only keep the end of the name that can fit in the interface name buffer - // - nameLen = strlen(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))); - } + 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/common/interfaces/sbgInterface.h b/common/interfaces/sbgInterface.h index 6ebf057..3259229 100644 --- a/common/interfaces/sbgInterface.h +++ b/common/interfaces/sbgInterface.h @@ -1,16 +1,16 @@ /*! - * \file sbgInterface.h - * \ingroup common - * \author SBG Systems - * \date 10 December 2012 + * \file sbgInterface.h + * \ingroup common + * \author SBG Systems + * \date 10 December 2012 * - * \brief This file implements the base interface for all Serial and Ethernet ports. + * \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. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -50,24 +50,24 @@ extern "C" { //- Constant definitions -// //----------------------------------------------------------------------// -#define SBG_IF_NAME_MAX_SIZE (48) /*!< Maximum size in bytes for the interface name string */ +#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. */ +#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. */ // // Flags for the flush operation. // -#define SBG_IF_FLUSH_INPUT ((uint32_t)1 << 0) /*!< Flush input data flag. */ -#define SBG_IF_FLUSH_OUTPUT ((uint32_t)1 << 1) /*!< Flush output data flag. */ -#define SBG_IF_FLUSH_ALL (SBG_IF_FLUSH_INPUT | SBG_IF_FLUSH_OUTPUT) /*!< Flag combination to flush both input and output data. */ +#define SBG_IF_FLUSH_INPUT ((uint32_t)1 << 0) /*!< Flush input data flag. */ +#define SBG_IF_FLUSH_OUTPUT ((uint32_t)1 << 1) /*!< Flush output data flag. */ +#define SBG_IF_FLUSH_ALL (SBG_IF_FLUSH_INPUT | SBG_IF_FLUSH_OUTPUT) /*!< Flag combination to flush both input and output data. */ //----------------------------------------------------------------------// //- Predefinitions -// @@ -90,8 +90,8 @@ typedef void* SbgInterfaceHandle; /*! * Method to implement that close and destroy an interface. * - * \param[in] pInterface Interface instance. - * \return SBG_NO_ERROR if the interface has been closed successfully. + * \param[in] pInterface Interface instance. + * \return SBG_NO_ERROR if the interface has been closed successfully. */ typedef SbgErrorCode (*SbgInterfaceDestroyFunc)(SbgInterface *pInterface); @@ -101,10 +101,10 @@ typedef SbgErrorCode (*SbgInterfaceDestroyFunc)(SbgInterface *pInterface); * 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] pInterface Interface instance. - * \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. + * \param[in] pInterface Interface instance. + * \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 *pInterface, const void *pBuffer, size_t bytesToWrite); @@ -115,11 +115,11 @@ typedef SbgErrorCode (*SbgInterfaceWriteFunc)(SbgInterface *pInterface, const vo * 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] pInterface Interface instance. - * \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. + * \param[in] pInterface Interface instance. + * \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 *pInterface, void *pBuffer, size_t *pReadBytes, size_t bytesToRead); @@ -131,9 +131,9 @@ typedef SbgErrorCode (*SbgInterfaceReadFunc)(SbgInterface *pInterface, void *pBu * * WARNING: The method has no action if not applicable for a type of interface * - * \param[in] pInterface Interface instance. - * \param[in] flags Combination of the SBG_IF_FLUSH_INPUT and SBG_IF_FLUSH_OUTPUT flags. - * \return SBG_NO_ERROR if successful. + * \param[in] pInterface Interface instance. + * \param[in] flags Combination of the SBG_IF_FLUSH_INPUT and SBG_IF_FLUSH_OUTPUT flags. + * \return SBG_NO_ERROR if successful. */ typedef SbgErrorCode (*SbgInterfaceFlushFunc)(SbgInterface *pInterface, uint32_t flags); @@ -148,9 +148,9 @@ typedef SbgErrorCode (*SbgInterfaceFlushFunc)(SbgInterface *pInterface, uint32_t * * WARNING: The method has no action if not applicable for a type of interface * - * \param[in] pInterface Interface instance. - * \param[in] speed The new interface speed to set in bps. - * \return SBG_NO_ERROR if successful. + * \param[in] pInterface Interface instance. + * \param[in] speed The new interface speed to set in bps. + * \return SBG_NO_ERROR if successful. */ typedef SbgErrorCode (*SbgInterfaceSetSpeed)(SbgInterface *pInterface, uint32_t speed); @@ -159,8 +159,8 @@ typedef SbgErrorCode (*SbgInterfaceSetSpeed)(SbgInterface *pInterface, uint32_t * * WARNING: The method will returns zero if not applicable for a type of interface * - * \param[in] pInterface Interface instance. - * \return The current interface baud rate in bps or zero if not applicable. + * \param[in] pInterface Interface instance. + * \return The current interface baud rate in bps or zero if not applicable. */ typedef uint32_t (*SbgInterfaceGetSpeed)(const SbgInterface *pInterface); @@ -169,9 +169,9 @@ typedef uint32_t (*SbgInterfaceGetSpeed)(const SbgInterface *pInterface); * * WARNING: The method will returns zero if not applicable for a type of interface. * - * \param[in] pInterface Interface instance. - * \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. + * \param[in] pInterface Interface instance. + * \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)(const SbgInterface *pInterface, size_t numBytes); @@ -191,17 +191,17 @@ typedef uint32_t (*SbgInterfaceGetDelayFunc)(const SbgInterface *pInterface, siz */ 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 */ - - SbgInterfaceDestroyFunc pDestroyFunc; /*!< Optional method used to destroy an interface. */ - SbgInterfaceWriteFunc pWriteFunc; /*!< Optional method used to write some data to this interface. */ - SbgInterfaceReadFunc pReadFunc; /*!< Optional method used to read some data to this interface. */ - SbgInterfaceFlushFunc pFlushFunc; /*!< Optional method used to make this interface flush all pending data. */ - SbgInterfaceSetSpeed pSetSpeedFunc; /*!< Optional method used to set the interface speed in bps. */ - SbgInterfaceGetSpeed pGetSpeedFunc; /*!< Optional method used to retrieve the interface speed in bps. */ - SbgInterfaceGetDelayFunc pDelayFunc; /*!< Optional method used to compute an expected delay to transmit/receive X bytes */ + 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 */ + + SbgInterfaceDestroyFunc pDestroyFunc; /*!< Optional method used to destroy an interface. */ + SbgInterfaceWriteFunc pWriteFunc; /*!< Optional method used to write some data to this interface. */ + SbgInterfaceReadFunc pReadFunc; /*!< Optional method used to read some data to this interface. */ + SbgInterfaceFlushFunc pFlushFunc; /*!< Optional method used to make this interface flush all pending data. */ + SbgInterfaceSetSpeed pSetSpeedFunc; /*!< Optional method used to set the interface speed in bps. */ + SbgInterfaceGetSpeed pGetSpeedFunc; /*!< Optional method used to retrieve the interface speed in bps. */ + SbgInterfaceGetDelayFunc pDelayFunc; /*!< Optional method used to compute an expected delay to transmit/receive X bytes */ }; //----------------------------------------------------------------------// @@ -211,7 +211,7 @@ struct _SbgInterface /*! * Initialize an interface instance to zero. * - * \param[in] pInterface The interface instance. + * \param[in] pInterface The interface instance. */ SBG_COMMON_LIB_API void sbgInterfaceZeroInit(SbgInterface *pInterface); @@ -220,43 +220,43 @@ SBG_COMMON_LIB_API void sbgInterfaceZeroInit(SbgInterface *pInterface); * * This method will call the specialized interface destructor if any. * - * \param[in] pInterface The interface instance. - * \return SBG_NO_ERROR if the interface has been destroyed successfully. + * \param[in] pInterface The interface instance. + * \return SBG_NO_ERROR if the interface has been destroyed successfully. */ SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceDestroy(SbgInterface *pInterface); /*! * Returns the interface type. * - * \param[in] pInterface Interface instance - * \return The interface type. + * \param[in] pInterface Interface instance + * \return The interface type. */ SBG_INLINE uint32_t sbgInterfaceTypeGet(const SbgInterface *pInterface) { - assert(pInterface); + assert(pInterface); - return pInterface->type; + return pInterface->type; } /*! * Returns the interface as string. * - * \param[in] pInterface Interface instance - * \return The interface type. + * \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. + * \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); + assert(pInterface); - return pInterface->name; + return pInterface->name; } /*! @@ -268,8 +268,8 @@ SBG_INLINE const char *sbgInterfaceNameGet(const SbgInterface *pInterface) * 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 + * \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); @@ -279,29 +279,29 @@ SBG_COMMON_LIB_API void sbgInterfaceNameSet(SbgInterface *pInterface, const char * 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] pInterface Interface instance. - * \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_INVALID_PARAMETER if the interface doesn't support write operations. + * \param[in] pInterface Interface instance. + * \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_INVALID_PARAMETER if the interface doesn't support write operations. */ SBG_INLINE SbgErrorCode sbgInterfaceWrite(SbgInterface *pInterface, const void *pBuffer, size_t bytesToWrite) { - SbgErrorCode errorCode; + SbgErrorCode errorCode; - assert(pInterface); - assert(pBuffer); + assert(pInterface); + assert(pBuffer); - if (pInterface->pWriteFunc) - { - errorCode = pInterface->pWriteFunc(pInterface, pBuffer, bytesToWrite); - } - else - { - errorCode = SBG_INVALID_PARAMETER; - } + if (pInterface->pWriteFunc) + { + errorCode = pInterface->pWriteFunc(pInterface, pBuffer, bytesToWrite); + } + else + { + errorCode = SBG_INVALID_PARAMETER; + } - return errorCode; + return errorCode; } /*! @@ -311,32 +311,32 @@ SBG_INLINE SbgErrorCode sbgInterfaceWrite(SbgInterface *pInterface, const void * * 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] pInterface Interface instance. - * \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_INVALID_PARAMETER if the interface doesn't support read operations. + * \param[in] pInterface Interface instance. + * \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_INVALID_PARAMETER if the interface doesn't support read operations. */ SBG_INLINE SbgErrorCode sbgInterfaceRead(SbgInterface *pInterface, void *pBuffer, size_t *pReadBytes, size_t bytesToRead) { - SbgErrorCode errorCode; - - assert(pInterface); - assert(pBuffer); - assert(pReadBytes); - - if (pInterface->pReadFunc) - { - errorCode = pInterface->pReadFunc(pInterface, pBuffer, pReadBytes, bytesToRead); - } - else - { - *pReadBytes = 0; - errorCode = SBG_INVALID_PARAMETER; - } - - return errorCode; + SbgErrorCode errorCode; + + assert(pInterface); + assert(pBuffer); + assert(pReadBytes); + + if (pInterface->pReadFunc) + { + errorCode = pInterface->pReadFunc(pInterface, pBuffer, pReadBytes, bytesToRead); + } + else + { + *pReadBytes = 0; + errorCode = SBG_INVALID_PARAMETER; + } + + return errorCode; } /*! @@ -347,27 +347,27 @@ SBG_INLINE SbgErrorCode sbgInterfaceRead(SbgInterface *pInterface, void *pBuffer * * WARNING: The method has no action if not applicable for a type of interface * - * \param[in] pInterface Interface instance. - * \param[in] flags Combination of the SBG_IF_FLUSH_INPUT and SBG_IF_FLUSH_OUTPUT flags. - * \return SBG_NO_ERROR if successful. + * \param[in] pInterface Interface instance. + * \param[in] flags Combination of the SBG_IF_FLUSH_INPUT and SBG_IF_FLUSH_OUTPUT flags. + * \return SBG_NO_ERROR if successful. */ SBG_INLINE SbgErrorCode sbgInterfaceFlush(SbgInterface *pInterface, uint32_t flags) { - SbgErrorCode errorCode; + SbgErrorCode errorCode; - assert(pInterface); - assert((flags & ~SBG_IF_FLUSH_ALL) == 0); + assert(pInterface); + assert((flags & ~SBG_IF_FLUSH_ALL) == 0); - if (pInterface->pFlushFunc) - { - errorCode = pInterface->pFlushFunc(pInterface, flags); - } - else - { - errorCode = SBG_NO_ERROR; - } + if (pInterface->pFlushFunc) + { + errorCode = pInterface->pFlushFunc(pInterface, flags); + } + else + { + errorCode = SBG_NO_ERROR; + } - return errorCode; + return errorCode; } /*! @@ -381,23 +381,23 @@ SBG_INLINE SbgErrorCode sbgInterfaceFlush(SbgInterface *pInterface, uint32_t fla * * WARNING: The method has no action if not applicable for a type of interface * - * \param[in] pInterface Interface instance. - * \param[in] speed The new interface speed to set in bps. - * \return SBG_NO_ERROR if successful. + * \param[in] pInterface Interface instance. + * \param[in] speed The new interface speed to set in bps. + * \return SBG_NO_ERROR if successful. */ SBG_INLINE SbgErrorCode sbgInterfaceSetSpeed(SbgInterface *pInterface, uint32_t speed) { - assert(pInterface); - assert(speed > 0); - - if (pInterface->pSetSpeedFunc) - { - return pInterface->pSetSpeedFunc(pInterface, speed); - } - else - { - return SBG_NO_ERROR; - } + assert(pInterface); + assert(speed > 0); + + if (pInterface->pSetSpeedFunc) + { + return pInterface->pSetSpeedFunc(pInterface, speed); + } + else + { + return SBG_NO_ERROR; + } } /*! @@ -405,21 +405,21 @@ SBG_INLINE SbgErrorCode sbgInterfaceSetSpeed(SbgInterface *pInterface, uint32_t * * WARNING: The method will returns zero if not applicable for a type of interface * - * \param[in] pInterface Interface instance. - * \return The current interface baud rate in bps or zero if not applicable. + * \param[in] pInterface Interface instance. + * \return The current interface baud rate in bps or zero if not applicable. */ SBG_INLINE uint32_t sbgInterfaceGetSpeed(const SbgInterface *pInterface) { - assert(pInterface); - - if (pInterface->pGetSpeedFunc) - { - return pInterface->pGetSpeedFunc(pInterface); - } - else - { - return 0; - } + assert(pInterface); + + if (pInterface->pGetSpeedFunc) + { + return pInterface->pGetSpeedFunc(pInterface); + } + else + { + return 0; + } } /*! @@ -427,22 +427,22 @@ SBG_INLINE uint32_t sbgInterfaceGetSpeed(const SbgInterface *pInterface) * * WARNING: The method will returns zero if not applicable for a type of interface. * - * \param[in] pInterface Interface instance. - * \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. + * \param[in] pInterface Interface instance. + * \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(const SbgInterface *pInterface, size_t numBytes) { - assert(pInterface); - - if (pInterface->pDelayFunc) - { - return pInterface->pDelayFunc(pInterface, numBytes); - } - else - { - return 0; - } + assert(pInterface); + + if (pInterface->pDelayFunc) + { + return pInterface->pDelayFunc(pInterface, numBytes); + } + else + { + return 0; + } } //----------------------------------------------------------------------// @@ -452,4 +452,4 @@ SBG_INLINE uint32_t sbgInterfaceGetDelay(const SbgInterface *pInterface, size_t } #endif -#endif /* SBG_INTERFACE_H */ +#endif // SBG_INTERFACE_H diff --git a/common/interfaces/sbgInterfaceFile.c b/common/interfaces/sbgInterfaceFile.c index 7478b9e..f3aa6ee 100644 --- a/common/interfaces/sbgInterfaceFile.c +++ b/common/interfaces/sbgInterfaceFile.c @@ -12,113 +12,113 @@ /*! * Returns the interface FILE descriptor. * - * \param[in] pInterface Interface instance. - * \return The associated FILE descriptor. + * \param[in] pInterface Interface instance. + * \return The associated FILE descriptor. */ static FILE *sbgInterfaceFileGetDesc(SbgInterface *pInterface) { - assert(pInterface); - assert(pInterface->type == SBG_IF_TYPE_FILE); - assert(pInterface->handle); + assert(pInterface); + assert(pInterface->type == SBG_IF_TYPE_FILE); + assert(pInterface->handle); - return (FILE*)pInterface->handle; + return (FILE*)pInterface->handle; } /*! * Destroy the interface by closing the file descriptor. * - * \param[in] pInterface Interface instance. - * \return SBG_NO_ERROR if the interface has been closed successfully. + * \param[in] pInterface Interface instance. + * \return SBG_NO_ERROR if the interface has been closed successfully. */ static SbgErrorCode sbgInterfaceFileDestroy(SbgInterface *pInterface) { - FILE *pInputFile; + FILE *pInputFile; - assert(pInterface); - assert(pInterface->type == SBG_IF_TYPE_FILE); + assert(pInterface); + assert(pInterface->type == SBG_IF_TYPE_FILE); - pInputFile = sbgInterfaceFileGetDesc(pInterface); + pInputFile = sbgInterfaceFileGetDesc(pInterface); - fclose(pInputFile); - sbgInterfaceZeroInit(pInterface); + fclose(pInputFile); + sbgInterfaceZeroInit(pInterface); - return SBG_NO_ERROR; + return SBG_NO_ERROR; } /*! * Write some data the the file * - * \param[in] pInterface 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. + * \param[in] pInterface 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. */ static SbgErrorCode sbgInterfaceFileWrite(SbgInterface *pInterface, const void *pBuffer, size_t bytesToWrite) { - FILE *pOutputFile; - size_t bytesWritten; + FILE *pOutputFile; + size_t bytesWritten; - assert(pInterface); - assert(pInterface->type == SBG_IF_TYPE_FILE); - assert(pBuffer); + assert(pInterface); + assert(pInterface->type == SBG_IF_TYPE_FILE); + assert(pBuffer); - pOutputFile = sbgInterfaceFileGetDesc(pInterface); + pOutputFile = sbgInterfaceFileGetDesc(pInterface); - // - // Write the data and check if all bytes have been written - // - bytesWritten = fwrite(pBuffer, sizeof(uint8_t), bytesToWrite, pOutputFile); + // + // Write the data and check if all bytes have been written + // + bytesWritten = fwrite(pBuffer, sizeof(uint8_t), bytesToWrite, pOutputFile); - if (bytesWritten == bytesToWrite) - { + if (bytesWritten == bytesToWrite) + { - return SBG_NO_ERROR; - } - else - { - return SBG_WRITE_ERROR; - } + return SBG_NO_ERROR; + } + else + { + return SBG_WRITE_ERROR; + } } /*! * Try to read some data from an interface. * - * \param[in] pInterface 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. + * \param[in] pInterface 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. */ static SbgErrorCode sbgInterfaceFileRead(SbgInterface *pInterface, void *pBuffer, size_t *pReadBytes, size_t bytesToRead) { - SbgErrorCode errorCode = SBG_NO_ERROR; - FILE *pInputFile; - - assert(pInterface); - assert(pInterface->type == SBG_IF_TYPE_FILE); - assert(pBuffer); - assert(pReadBytes); - - pInputFile = sbgInterfaceFileGetDesc(pInterface); - - // - // Read some bytes from the file and check if an error has occurred - // - *pReadBytes = fread(pBuffer, sizeof(uint8_t), bytesToRead, pInputFile); - - if (*pReadBytes < bytesToRead) - { - // - // Don't report an error if we have reached the end of the file to comply with sbgInterface specification - // - if (ferror(pInputFile) != 0) - { - errorCode = SBG_READ_ERROR; - SBG_LOG_ERROR(errorCode, "File read error %u", ferror(pInputFile)); - } - } - - return errorCode; + SbgErrorCode errorCode = SBG_NO_ERROR; + FILE *pInputFile; + + assert(pInterface); + assert(pInterface->type == SBG_IF_TYPE_FILE); + assert(pBuffer); + assert(pReadBytes); + + pInputFile = sbgInterfaceFileGetDesc(pInterface); + + // + // Read some bytes from the file and check if an error has occurred + // + *pReadBytes = fread(pBuffer, sizeof(uint8_t), bytesToRead, pInputFile); + + if (*pReadBytes < bytesToRead) + { + // + // Don't report an error if we have reached the end of the file to comply with sbgInterface specification + // + if (ferror(pInputFile) != 0) + { + errorCode = SBG_READ_ERROR; + SBG_LOG_ERROR(errorCode, "File read error %u", ferror(pInputFile)); + } + } + + return errorCode; } /*! @@ -127,42 +127,42 @@ static SbgErrorCode sbgInterfaceFileRead(SbgInterface *pInterface, void *pBuffer * If flags include SBG_IF_FLUSH_INPUT, all pending input data is discarded. * If flags include SBG_IF_FLUSH_OUTPUT, the function blocks until all output data has been written out. * - * \param[in] pInterface Valid handle on an initialized interface. - * \param[in] flags Combination of the SBG_IF_FLUSH_INPUT and SBG_IF_FLUSH_OUTPUT flags. - * \return SBG_NO_ERROR if successful. + * \param[in] pInterface Valid handle on an initialized interface. + * \param[in] flags Combination of the SBG_IF_FLUSH_INPUT and SBG_IF_FLUSH_OUTPUT flags. + * \return SBG_NO_ERROR if successful. */ static SbgErrorCode sbgInterfaceFileFlush(SbgInterface *pInterface, uint32_t flags) { - SbgErrorCode errorCode; - FILE *pInputFile; - - assert(pInterface); - assert(pInterface->type == SBG_IF_TYPE_FILE); - - pInputFile = sbgInterfaceFileGetDesc(pInterface); - - if ((pInterface->pReadFunc && (flags & SBG_IF_FLUSH_INPUT)) || - (pInterface->pWriteFunc && (flags & SBG_IF_FLUSH_OUTPUT))) - { - int ret; - - ret = fflush(pInputFile); - - if (ret == 0) - { - errorCode = SBG_NO_ERROR; - } - else - { - errorCode = SBG_ERROR; - } - } - else - { - errorCode = SBG_NO_ERROR; - } - - return errorCode; + SbgErrorCode errorCode; + FILE *pInputFile; + + assert(pInterface); + assert(pInterface->type == SBG_IF_TYPE_FILE); + + pInputFile = sbgInterfaceFileGetDesc(pInterface); + + if ((pInterface->pReadFunc && (flags & SBG_IF_FLUSH_INPUT)) || + (pInterface->pWriteFunc && (flags & SBG_IF_FLUSH_OUTPUT))) + { + int ret; + + ret = fflush(pInputFile); + + if (ret == 0) + { + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_ERROR; + } + } + else + { + errorCode = SBG_NO_ERROR; + } + + return errorCode; } //----------------------------------------------------------------------// @@ -171,135 +171,135 @@ static SbgErrorCode sbgInterfaceFileFlush(SbgInterface *pInterface, uint32_t fla SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceFileOpen(SbgInterface *pInterface, const char *filePath) { - SbgErrorCode errorCode = SBG_NO_ERROR; - FILE *pInputFile; - - assert(pInterface); - assert(filePath); - - // - // Always call the underlying zero init method to make sure we can correctly handle SbgInterface evolutions - // - sbgInterfaceZeroInit(pInterface); - - // - // Try to open the file - // - pInputFile = fopen(filePath, "rb"); - - // - // Test if the input file has been opened - // - if (pInputFile) - { - // - // Define base interface members - // - pInterface->handle = pInputFile; - pInterface->type = SBG_IF_TYPE_FILE; - - // - // Define the interface name - // - sbgInterfaceNameSet(pInterface, filePath); - - // - // Define all specialized members - // - pInterface->pDestroyFunc = sbgInterfaceFileDestroy; - pInterface->pReadFunc = sbgInterfaceFileRead; - pInterface->pWriteFunc = NULL; - pInterface->pFlushFunc = sbgInterfaceFileFlush; - } - else - { - // - // Unable to open the input file - // - errorCode = SBG_INVALID_PARAMETER; - } - - return errorCode; + SbgErrorCode errorCode = SBG_NO_ERROR; + FILE *pInputFile; + + assert(pInterface); + assert(filePath); + + // + // Always call the underlying zero init method to make sure we can correctly handle SbgInterface evolutions + // + sbgInterfaceZeroInit(pInterface); + + // + // Try to open the file + // + pInputFile = fopen(filePath, "rb"); + + // + // Test if the input file has been opened + // + if (pInputFile) + { + // + // Define base interface members + // + pInterface->handle = pInputFile; + pInterface->type = SBG_IF_TYPE_FILE; + + // + // Define the interface name + // + sbgInterfaceNameSet(pInterface, filePath); + + // + // Define all specialized members + // + pInterface->pDestroyFunc = sbgInterfaceFileDestroy; + pInterface->pReadFunc = sbgInterfaceFileRead; + pInterface->pWriteFunc = NULL; + pInterface->pFlushFunc = sbgInterfaceFileFlush; + } + else + { + // + // Unable to open the input file + // + errorCode = SBG_INVALID_PARAMETER; + } + + return errorCode; } SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceFileWriteOpen(SbgInterface *pInterface, const char *filePath) { - SbgErrorCode errorCode = SBG_NO_ERROR; - FILE *pInputFile; - - assert(pInterface); - assert(filePath); - - // - // Try to open the file - // - pInputFile = fopen(filePath, "wb"); - - // - // Test if the input file has been opened - // - if (pInputFile) - { - // - // Define base interface members - // - pInterface->handle = pInputFile; - pInterface->type = SBG_IF_TYPE_FILE; - - // - // Define the interface name - // - sbgInterfaceNameSet(pInterface, filePath); - - // - // Define all specialized members - // - pInterface->pDestroyFunc = sbgInterfaceFileDestroy; - pInterface->pReadFunc = NULL; - pInterface->pWriteFunc = sbgInterfaceFileWrite; - pInterface->pFlushFunc = sbgInterfaceFileFlush; - } - else - { - // - // Unable to open the input file - // - errorCode = SBG_INVALID_PARAMETER; - } - - return errorCode; + SbgErrorCode errorCode = SBG_NO_ERROR; + FILE *pInputFile; + + assert(pInterface); + assert(filePath); + + // + // Try to open the file + // + pInputFile = fopen(filePath, "wb"); + + // + // Test if the input file has been opened + // + if (pInputFile) + { + // + // Define base interface members + // + pInterface->handle = pInputFile; + pInterface->type = SBG_IF_TYPE_FILE; + + // + // Define the interface name + // + sbgInterfaceNameSet(pInterface, filePath); + + // + // Define all specialized members + // + pInterface->pDestroyFunc = sbgInterfaceFileDestroy; + pInterface->pReadFunc = NULL; + pInterface->pWriteFunc = sbgInterfaceFileWrite; + pInterface->pFlushFunc = sbgInterfaceFileFlush; + } + else + { + // + // Unable to open the input file + // + errorCode = SBG_INVALID_PARAMETER; + } + + return errorCode; } SBG_COMMON_LIB_API size_t sbgInterfaceFileGetSize(SbgInterface *pInterface) { - FILE *pInputFile; - long cursorPos; - long fileSize; - - assert(pInterface); - assert(pInterface->type == SBG_IF_TYPE_FILE); - - pInputFile = sbgInterfaceFileGetDesc(pInterface); - - // - // Compute the file size - // - cursorPos = ftell(pInputFile); - fseek(pInputFile, 0, SEEK_END); - fileSize = ftell(pInputFile); - fseek(pInputFile, cursorPos, SEEK_SET); - - return (size_t)fileSize; + FILE *pInputFile; + long cursorPos; + long fileSize; + + assert(pInterface); + assert(pInterface->type == SBG_IF_TYPE_FILE); + + pInputFile = sbgInterfaceFileGetDesc(pInterface); + + // + // Compute the file size + // + cursorPos = ftell(pInputFile); + fseek(pInputFile, 0, SEEK_END); + fileSize = ftell(pInputFile); + fseek(pInputFile, cursorPos, SEEK_SET); + + return (size_t)fileSize; } SBG_COMMON_LIB_API size_t sbgInterfaceFileGetCursor(const SbgInterface *pInterface) { - FILE *pInputFile; + FILE *pInputFile; - assert(pInterface); - assert(pInterface->type == SBG_IF_TYPE_FILE); + assert(pInterface); + assert(pInterface->type == SBG_IF_TYPE_FILE); - pInputFile = sbgInterfaceFileGetDesc((SbgInterface*)pInterface); + pInputFile = sbgInterfaceFileGetDesc((SbgInterface*)pInterface); - return (size_t)ftell(pInputFile); + return (size_t)ftell(pInputFile); } diff --git a/common/interfaces/sbgInterfaceFile.h b/common/interfaces/sbgInterfaceFile.h index ea3b9b0..e996406 100644 --- a/common/interfaces/sbgInterfaceFile.h +++ b/common/interfaces/sbgInterfaceFile.h @@ -1,13 +1,13 @@ /*! - * \file sbgInterfaceFile.h - * \ingroup common - * \author SBG Systems (Raphael Siryani) - * \date 01 April 2013 + * \file sbgInterfaceFile.h + * \ingroup common + * \author SBG Systems + * \date 01 April 2013 * - * \brief This file implements a file interface for read only operations. + * \brief This file implements a file interface for read only operations. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -50,36 +50,36 @@ extern "C" { //----------------------------------------------------------------------// /*! - * Open a file as an interface for read only operations. + * Open a file as an interface for read only operations. * - * \param[in] pInterface 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. + * \param[in] pInterface 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 *pInterface, const char *filePath); /*! - * Open a file as an interface for write only operations. + * Open a file as an interface for write only operations. * - * \param[in] pInterface 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. + * \param[in] pInterface 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 *pInterface, const char *filePath); /*! - * Returns the file size in bytes. + * Returns the file size in bytes. * - * \param[in] pInterface Valid handle on an initialized interface. - * \return 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 *pInterface); /*! - * Returns the current cursor position in the file in bytes. + * 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. + * \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 *pInterface); @@ -90,4 +90,4 @@ SBG_COMMON_LIB_API size_t sbgInterfaceFileGetCursor(const SbgInterface *pInterfa } #endif -#endif /* SBG_INTERFACE_FILE_H */ +#endif // SBG_INTERFACE_FILE_H diff --git a/common/interfaces/sbgInterfaceSerial.h b/common/interfaces/sbgInterfaceSerial.h index 70cfee0..6adb5d7 100644 --- a/common/interfaces/sbgInterfaceSerial.h +++ b/common/interfaces/sbgInterfaceSerial.h @@ -1,13 +1,13 @@ /*! - * \file sbgInterfaceSerial.h - * \ingroup common - * \author SBG Systems - * \date 06 February 2013 + * \file sbgInterfaceSerial.h + * \ingroup common + * \author SBG Systems + * \date 06 February 2013 * - * \brief This file implements a serial interface. + * \brief This file implements a serial interface. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -49,12 +49,12 @@ extern "C" { //----------------------------------------------------------------------// /*! - * Initialize a serial interface for read and write operations. + * Initialize a serial interface for read and write operations. * - * \param[in] pInterface 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. + * \param[in] pInterface 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 *pInterface, const char *deviceName, uint32_t baudRate); diff --git a/common/interfaces/sbgInterfaceSerialUnix.c b/common/interfaces/sbgInterfaceSerialUnix.c index e4010f3..eb7796c 100644 --- a/common/interfaces/sbgInterfaceSerialUnix.c +++ b/common/interfaces/sbgInterfaceSerialUnix.c @@ -14,148 +14,147 @@ //----------------------------------------------------------------------// //- 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. */ +#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. */ //----------------------------------------------------------------------// //- Private methods declarations -// //----------------------------------------------------------------------// - /*! - * Returns the right Unix baud rate const according to a baud rate value. + * 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. + * \param[in] baudRate The baud rate value (ie 115200). + * \return The Unix baud rate constant. */ static uint32_t sbgInterfaceSerialGetBaudRateConst(uint32_t baudRate) { - uint32_t baudRateConst; - - // - // Create the right baud rate value for Unix platforms - // - switch (baudRate) - { - case 9600: - baudRateConst = B9600; - break; - case 19200: - baudRateConst = B19200; - break; + uint32_t 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; + case 38400: + baudRateConst = B38400; + break; #endif #ifdef B57600 - case 57600: - baudRateConst = B57600; - break; + case 57600: + baudRateConst = B57600; + break; #endif #ifdef B115200 - case 115200: - baudRateConst = B115200; - break; + case 115200: + baudRateConst = B115200; + break; #endif #ifdef B230400 - case 230400: - baudRateConst = B230400; - break; + case 230400: + baudRateConst = B230400; + break; #endif #ifdef B460800 - case 460800: - baudRateConst = B460800; - break; + case 460800: + baudRateConst = B460800; + break; #endif #ifdef B921600 - case 921600: - baudRateConst = B921600; - break; + case 921600: + baudRateConst = B921600; + break; #endif #ifdef B1000000 - case 1000000: - baudRateConst = B1000000; - break; + case 1000000: + baudRateConst = B1000000; + break; #endif #ifdef B1152000 - case 1152000: - baudRateConst = B1152000; - break; + case 1152000: + baudRateConst = B1152000; + break; #endif #ifdef B1500000 - case 1500000: - baudRateConst = B1500000; - break; + case 1500000: + baudRateConst = B1500000; + break; #endif #ifdef B2000000 - case 2000000: - baudRateConst = B2000000; - break; + case 2000000: + baudRateConst = B2000000; + break; #endif #ifdef B2500000 - case 2500000: - baudRateConst = B2500000; - break; + case 2500000: + baudRateConst = B2500000; + break; #endif #ifdef B3000000 - case 3000000: - baudRateConst = B3000000; - break; + case 3000000: + baudRateConst = B3000000; + break; #endif #ifdef B3500000 - case 3500000: - baudRateConst = B3500000; - break; + case 3500000: + baudRateConst = B3500000; + break; #endif #ifdef B4000000 - case 4000000: - baudRateConst = B4000000; - break; + case 4000000: + baudRateConst = B4000000; + break; #endif - default: - baudRateConst = baudRate; - } + default: + baudRateConst = baudRate; + } - return baudRateConst; + return baudRateConst; } /*! - * Destroy an interface initialized using sbgInterfaceSerialCreate. + * 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. + * \param[in] pInterface Valid handle on an initialized interface. + * \return SBG_NO_ERROR if the interface has been closed and released. */ static SbgErrorCode sbgInterfaceSerialDestroy(SbgInterface *pInterface) { - int *pSerialHandle; - - assert(pInterface); - assert(pInterface->type == SBG_IF_TYPE_SERIAL); - - // - // Test that we have a valid interface - // - if (pInterface) - { - // - // Get the internal serial handle - // - pSerialHandle = (int *)pInterface->handle; - - // - // Close the port com - // - close((*pSerialHandle)); - SBG_FREE(pSerialHandle); - sbgInterfaceZeroInit(pInterface); - - return SBG_NO_ERROR; - } - else - { - return SBG_NULL_POINTER; - } + int *pSerialHandle; + + assert(pInterface); + assert(pInterface->type == SBG_IF_TYPE_SERIAL); + + // + // Test that we have a valid interface + // + if (pInterface) + { + // + // Get the internal serial handle + // + pSerialHandle = (int *)pInterface->handle; + + // + // Close the port com + // + close((*pSerialHandle)); + SBG_FREE(pSerialHandle); + sbgInterfaceZeroInit(pInterface); + + return SBG_NO_ERROR; + } + else + { + return SBG_NULL_POINTER; + } } /*! @@ -164,111 +163,111 @@ static SbgErrorCode sbgInterfaceSerialDestroy(SbgInterface *pInterface) * If flags include SBG_IF_FLUSH_INPUT, all pending input data is discarded. * If flags include SBG_IF_FLUSH_OUTPUT, the function blocks until all output data has been written out. * - * \param[in] pInterface Interface instance. - * \param[in] flags Combination of the SBG_IF_FLUSH_INPUT and SBG_IF_FLUSH_OUTPUT flags. - * \return SBG_NO_ERROR if successful. + * \param[in] pInterface Interface instance. + * \param[in] flags Combination of the SBG_IF_FLUSH_INPUT and SBG_IF_FLUSH_OUTPUT flags. + * \return SBG_NO_ERROR if successful. */ static SbgErrorCode sbgInterfaceSerialFlush(SbgInterface *pInterface, uint32_t flags) { - SbgErrorCode errorCode; - int fd; - int result = 0; - - assert(pInterface); - assert(pInterface->type == SBG_IF_TYPE_SERIAL); - - fd = *((int *)pInterface->handle); - - if ((result == 0) && (flags & SBG_IF_FLUSH_INPUT)) - { - result = tcflush(fd, TCIFLUSH); - - if (result != 0) - { - SBG_LOG_ERROR(SBG_READ_ERROR, "unable to flush input, error:%s", strerror(errno)); - } - } - - if ((result == 0) && (flags & SBG_IF_FLUSH_OUTPUT)) - { - result = tcdrain(fd); - - if (result != 0) - { - SBG_LOG_ERROR(SBG_WRITE_ERROR, "unable to flush output, error:%s", strerror(errno)); - } - } - - if (result == 0) - { - errorCode = SBG_NO_ERROR; - } - else - { - errorCode = SBG_ERROR; - } - - return errorCode; + SbgErrorCode errorCode; + int fd; + int result = 0; + + assert(pInterface); + assert(pInterface->type == SBG_IF_TYPE_SERIAL); + + fd = *((int *)pInterface->handle); + + if ((result == 0) && (flags & SBG_IF_FLUSH_INPUT)) + { + result = tcflush(fd, TCIFLUSH); + + if (result != 0) + { + SBG_LOG_ERROR(SBG_READ_ERROR, "unable to flush input, error: %s", strerror(errno)); + } + } + + if ((result == 0) && (flags & SBG_IF_FLUSH_OUTPUT)) + { + result = tcdrain(fd); + + if (result != 0) + { + SBG_LOG_ERROR(SBG_WRITE_ERROR, "unable to flush output, error: %s", strerror(errno)); + } + } + + if (result == 0) + { + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_ERROR; + } + + return errorCode; } /*! * Change the serial interface baud rate immediately. * - * \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 + * \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 */ static SbgErrorCode sbgInterfaceSerialChangeBaudrate(SbgInterface *pInterface, uint32_t baudRate) { - int hSerialHandle; - struct termios options; - uint32_t baudRateConst; - - assert(pInterface); - assert(pInterface->type == SBG_IF_TYPE_SERIAL); - - // - // Get the internal serial handle - // - hSerialHandle = *((int*)pInterface->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; - } + int hSerialHandle; + struct termios options; + uint32_t baudRateConst; + + assert(pInterface); + assert(pInterface->type == SBG_IF_TYPE_SERIAL); + + // + // Get the internal serial handle + // + hSerialHandle = *((int*)pInterface->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) ) + { + SBG_LOG_ERROR(SBG_ERROR, "unable to change interface speed"); + return SBG_ERROR; + } + + // + // Define options + // + if (tcsetattr(hSerialHandle, TCSADRAIN, &options) != -1) + { + return SBG_NO_ERROR; + } + else + { + SBG_LOG_ERROR(SBG_ERROR, "tcsetattr has failed"); + return SBG_ERROR; + } + } + else + { + SBG_LOG_ERROR(SBG_ERROR, "tcgetattr has failed"); + return SBG_ERROR; + } } //----------------------------------------------------------------------// @@ -278,125 +277,122 @@ static SbgErrorCode sbgInterfaceSerialChangeBaudrate(SbgInterface *pInterface, u /*! * Try to write some data to an interface. * - * \param[in] pInterface 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. + * \param[in] pInterface 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. */ static SbgErrorCode sbgInterfaceSerialWrite(SbgInterface *pInterface, const void *pBuffer, size_t bytesToWrite) { - size_t numBytesLeftToWrite = bytesToWrite; - uint8_t *pCurrentBuffer = (uint8_t*)pBuffer; - ssize_t numBytesWritten; - int hSerialHandle; - - assert(pInterface); - assert(pInterface->type == SBG_IF_TYPE_SERIAL); - assert(pBuffer); - - // - // Get the internal serial handle - // - hSerialHandle = *((int*)pInterface->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) - { - if (errno == EAGAIN) - { - sbgSleep(1); - } - else - { - // - // An error has occurred during the write - // - fprintf(stderr, "sbgDeviceWrite: Unable to write to our device: %s\n", strerror(errno)); - return SBG_WRITE_ERROR; - } - } - else - { - // - // Update the buffer pointer and the number of bytes to write - // - numBytesLeftToWrite -= (size_t)numBytesWritten; - pCurrentBuffer += (size_t)numBytesWritten; - } - } - - return SBG_NO_ERROR; + size_t numBytesLeftToWrite = bytesToWrite; + uint8_t *pCurrentBuffer = (uint8_t*)pBuffer; + ssize_t numBytesWritten; + int hSerialHandle; + + assert(pInterface); + assert(pInterface->type == SBG_IF_TYPE_SERIAL); + assert(pBuffer); + + // + // Get the internal serial handle + // + hSerialHandle = *((int*)pInterface->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) + { + if (errno == EAGAIN) + { + sbgSleep(1); + } + else + { + SBG_LOG_ERROR(SBG_WRITE_ERROR, "unable to write to the device: %s", strerror(errno)); + return SBG_WRITE_ERROR; + } + } + else + { + // + // Update the buffer pointer and the number of bytes to write + // + numBytesLeftToWrite -= (size_t)numBytesWritten; + pCurrentBuffer += (size_t)numBytesWritten; + } + } + + return SBG_NO_ERROR; } /*! * Try to read some data from an interface. * - * \param[in] pInterface 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. + * \param[in] pInterface 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. */ static SbgErrorCode sbgInterfaceSerialRead(SbgInterface *pInterface, void *pBuffer, size_t *pReadBytes, size_t bytesToRead) { - SbgErrorCode errorCode; - int hSerialHandle; - ssize_t numBytesRead; - - assert(pInterface); - assert(pInterface->type == SBG_IF_TYPE_SERIAL); - assert(pBuffer); - assert(pReadBytes); - - // - // Get the internal serial handle - // - hSerialHandle = *((int*)pInterface->handle); - - // - // Read our buffer - // - numBytesRead = read(hSerialHandle, pBuffer, bytesToRead); - - // - // Check if the read operation was successful - // - if (numBytesRead >= 0) - { - errorCode = SBG_NO_ERROR; - } - else - { - if (errno == EAGAIN) - { - errorCode = SBG_NO_ERROR; - } - else - { - errorCode = SBG_READ_ERROR; - } - - numBytesRead = 0; - } - - // - // If we can, returns the number of read bytes - // - *pReadBytes = (size_t)numBytesRead; - - return errorCode; + SbgErrorCode errorCode; + int hSerialHandle; + ssize_t numBytesRead; + + assert(pInterface); + assert(pInterface->type == SBG_IF_TYPE_SERIAL); + assert(pBuffer); + assert(pReadBytes); + + // + // Get the internal serial handle + // + hSerialHandle = *((int*)pInterface->handle); + + // + // Read our buffer + // + numBytesRead = read(hSerialHandle, pBuffer, bytesToRead); + + // + // Check if the read operation was successful + // + if (numBytesRead >= 0) + { + errorCode = SBG_NO_ERROR; + } + else + { + if (errno == EAGAIN) + { + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_READ_ERROR; + } + + numBytesRead = 0; + } + + // + // If we can, returns the number of read bytes + // + *pReadBytes = (size_t)numBytesRead; + + return errorCode; } //----------------------------------------------------------------------// @@ -405,137 +401,137 @@ static SbgErrorCode sbgInterfaceSerialRead(SbgInterface *pInterface, void *pBuff SbgErrorCode sbgInterfaceSerialCreate(SbgInterface *pInterface, const char *deviceName, uint32_t baudRate) { - int *pSerialHandle; - struct termios options; - uint32_t baudRateConst; - - assert(pInterface); - assert(deviceName); - - // - // Always call the underlying zero init method to make sure we can correctly handle SbgInterface evolutions - // - sbgInterfaceZeroInit(pInterface); - - // - // 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) - { - // - // Retrieve 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 - // - pInterface->handle = (void*)pSerialHandle; - pInterface->type = SBG_IF_TYPE_SERIAL; - - // - // Define the interface name - // - sbgInterfaceNameSet(pInterface, deviceName); - - // - // Define all overloaded members - // - pInterface->pDestroyFunc = sbgInterfaceSerialDestroy; - pInterface->pReadFunc = sbgInterfaceSerialRead; - pInterface->pWriteFunc = sbgInterfaceSerialWrite; - pInterface->pFlushFunc = sbgInterfaceSerialFlush; - pInterface->pSetSpeedFunc = sbgInterfaceSerialChangeBaudrate; - - // - // Purge the communication - // - return sbgInterfaceSerialFlush(pInterface, SBG_IF_FLUSH_ALL); - } - 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; + int *pSerialHandle; + struct termios options; + uint32_t baudRateConst; + + assert(pInterface); + assert(deviceName); + + // + // Always call the underlying zero init method to make sure we can correctly handle SbgInterface evolutions + // + sbgInterfaceZeroInit(pInterface); + + // + // 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) + { + // + // Retrieve 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 + // + pInterface->handle = (void*)pSerialHandle; + pInterface->type = SBG_IF_TYPE_SERIAL; + + // + // Define the interface name + // + sbgInterfaceNameSet(pInterface, deviceName); + + // + // Define all overloaded members + // + pInterface->pDestroyFunc = sbgInterfaceSerialDestroy; + pInterface->pReadFunc = sbgInterfaceSerialRead; + pInterface->pWriteFunc = sbgInterfaceSerialWrite; + pInterface->pFlushFunc = sbgInterfaceSerialFlush; + pInterface->pSetSpeedFunc = sbgInterfaceSerialChangeBaudrate; + + // + // Purge the communication + // + return sbgInterfaceSerialFlush(pInterface, SBG_IF_FLUSH_ALL); + } + else + { + SBG_LOG_ERROR(SBG_ERROR, "tcsetattr has failed"); + } + } + else + { + SBG_LOG_ERROR(SBG_ERROR, "unable to change interface speed"); + } + } + else + { + SBG_LOG_ERROR(SBG_ERROR, "tcgetattr has failed"); + } + } + else + { + SBG_LOG_ERROR(SBG_ERROR, "fcntl has failed"); + } + } + else + { + SBG_LOG_ERROR(SBG_ERROR, "unable to open the interface: %s", deviceName); + } + + // + // Release the allocated serial handle + // + SBG_FREE(pSerialHandle); + + return SBG_ERROR; } diff --git a/common/interfaces/sbgInterfaceSerialWin.c b/common/interfaces/sbgInterfaceSerialWin.c index 1bea74a..627ddf4 100644 --- a/common/interfaces/sbgInterfaceSerialWin.c +++ b/common/interfaces/sbgInterfaceSerialWin.c @@ -8,8 +8,8 @@ //----------------------------------------------------------------------// //- 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. */ +#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. */ //----------------------------------------------------------------------// //- Internal methods declarations -// @@ -18,185 +18,185 @@ /*! * Returns the last error message for windows api calls. * - * \param[out] pErrorMsg Pointer on an allocated string that can stores the windows error message. - * \param[in] errorMsgSize Error message buffer size in bytes. - * \return The last error number. + * \param[out] pErrorMsg Pointer on an allocated string that can stores the windows error message. + * \param[in] errorMsgSize Error message buffer size in bytes. + * \return The last error number. */ static uint32_t sbgGetWindowsErrorMsg(char *pErrorMsg, size_t errorMsgSize) { - DWORD dw = GetLastError(); - DWORD numCharWritten; - LPVOID lpMsgBuf = NULL; - - assert(pErrorMsg); - - // - // Get the error message - // - numCharWritten = FormatMessageA(FORMAT_MESSAGE_ALLOCATE_BUFFER|FORMAT_MESSAGE_FROM_SYSTEM|FORMAT_MESSAGE_IGNORE_INSERTS, - NULL, dw, MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT), (LPSTR)&lpMsgBuf, 0, NULL); - - // - // Test if a message has been correctly written - // - if (numCharWritten > 0) - { - // - // Copy the error message - // - strcpy_s(pErrorMsg, errorMsgSize, lpMsgBuf); - } - else - { - pErrorMsg[0] = '\0'; - } - - // - // Release the buffer - // - LocalFree(lpMsgBuf); - - return dw; + DWORD dw = GetLastError(); + DWORD numCharWritten; + LPVOID lpMsgBuf = NULL; + + assert(pErrorMsg); + + // + // Get the error message + // + numCharWritten = FormatMessageA(FORMAT_MESSAGE_ALLOCATE_BUFFER|FORMAT_MESSAGE_FROM_SYSTEM|FORMAT_MESSAGE_IGNORE_INSERTS, + NULL, dw, MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT), (LPSTR)&lpMsgBuf, 0, NULL); + + // + // Test if a message has been correctly written + // + if (numCharWritten > 0) + { + // + // Copy the error message + // + strcpy_s(pErrorMsg, errorMsgSize, lpMsgBuf); + } + else + { + pErrorMsg[0] = '\0'; + } + + // + // Release the buffer + // + LocalFree(lpMsgBuf); + + return dw; } /*! * Returns the serial interface descriptor. * - * \param[in] pInterface Interface instance. - * \return The associated serial descriptor. + * \param[in] pInterface Interface instance. + * \return The associated serial descriptor. */ static HANDLE *sbgInterfaceFileGetDesc(SbgInterface *pInterface) { - assert(pInterface); - assert(pInterface->type == SBG_IF_TYPE_SERIAL); - assert(pInterface->handle); + assert(pInterface); + assert(pInterface->type == SBG_IF_TYPE_SERIAL); + assert(pInterface->handle); - return (HANDLE*)pInterface->handle; + return (HANDLE*)pInterface->handle; } /*! * Destroy and close the serial interface. * - * \param[in] pInterface Valid handle on an initialized interface. - * \return SBG_NO_ERROR if the interface has been closed and released. + * \param[in] pInterface Valid handle on an initialized interface. + * \return SBG_NO_ERROR if the interface has been closed and released. */ static SbgErrorCode sbgInterfaceSerialDestroy(SbgInterface *pInterface) { - HANDLE pSerialDevice; - - assert(pInterface); - assert(pInterface->type == SBG_IF_TYPE_SERIAL); - - pSerialDevice = sbgInterfaceFileGetDesc(pInterface); - - // - // Close the port com and clear the interface - // - CloseHandle(pSerialDevice); - sbgInterfaceZeroInit(pInterface); - - return SBG_NO_ERROR; + HANDLE pSerialDevice; + + assert(pInterface); + assert(pInterface->type == SBG_IF_TYPE_SERIAL); + + pSerialDevice = sbgInterfaceFileGetDesc(pInterface); + + // + // Close the port com and clear the interface + // + CloseHandle(pSerialDevice); + sbgInterfaceZeroInit(pInterface); + + return SBG_NO_ERROR; } /*! * Try to write some data to an interface. * - * \param[in] pInterface 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. + * \param[in] pInterface 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. */ static SbgErrorCode sbgInterfaceSerialWrite(SbgInterface *pInterface, const void *pBuffer, size_t bytesToWrite) { - DWORD numBytesLeftToWrite; - uint8_t *pCurrentBuffer; - DWORD numBytesWritten; - HANDLE pSerialDevice; - char errorMsg[256]; - - assert(pInterface); - assert(pInterface->type == SBG_IF_TYPE_SERIAL); - assert(pBuffer); - - pSerialDevice = sbgInterfaceFileGetDesc(pInterface); - - // - // Write the whole buffer chunk by chunk - // - numBytesLeftToWrite = (DWORD)bytesToWrite; - pCurrentBuffer = (uint8_t*)pBuffer; - - while (numBytesLeftToWrite > 0) - { - // - // Write these bytes to the serial interface - // - if (WriteFile(pSerialDevice, pCurrentBuffer, numBytesLeftToWrite, (LPDWORD)&numBytesWritten, NULL) == false) - { - // - // An error has occurred during the write - // - sbgGetWindowsErrorMsg(errorMsg, sizeof(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; + DWORD numBytesLeftToWrite; + uint8_t *pCurrentBuffer; + DWORD numBytesWritten; + HANDLE pSerialDevice; + char errorMsg[256]; + + assert(pInterface); + assert(pInterface->type == SBG_IF_TYPE_SERIAL); + assert(pBuffer); + + pSerialDevice = sbgInterfaceFileGetDesc(pInterface); + + // + // Write the whole buffer chunk by chunk + // + numBytesLeftToWrite = (DWORD)bytesToWrite; + pCurrentBuffer = (uint8_t*)pBuffer; + + while (numBytesLeftToWrite > 0) + { + // + // Write these bytes to the serial interface + // + if (WriteFile(pSerialDevice, pCurrentBuffer, numBytesLeftToWrite, (LPDWORD)&numBytesWritten, NULL) == false) + { + // + // An error has occurred during the write + // + sbgGetWindowsErrorMsg(errorMsg, sizeof(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] pInterface 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. + * \param[in] pInterface 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. */ static SbgErrorCode sbgInterfaceSerialRead(SbgInterface *pInterface, void *pBuffer, size_t *pReadBytes, size_t bytesToRead) { - HANDLE pSerialDevice; - char errorMsg[256]; - DWORD bytesRead; - - assert(pInterface); - assert(pInterface->type == SBG_IF_TYPE_SERIAL); - assert(pBuffer); - assert(pReadBytes); - - pSerialDevice = sbgInterfaceFileGetDesc(pInterface); - - // - // Read some bytes on the serial buffer - // - if (ReadFile(pSerialDevice, pBuffer, (DWORD)bytesToRead, (LPDWORD)&bytesRead, NULL) == true) - { - // - // 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, sizeof(errorMsg)); - SBG_LOG_ERROR(SBG_READ_ERROR, "Read failed: %s", errorMsg); - return SBG_READ_ERROR; - } + HANDLE pSerialDevice; + char errorMsg[256]; + DWORD bytesRead; + + assert(pInterface); + assert(pInterface->type == SBG_IF_TYPE_SERIAL); + assert(pBuffer); + assert(pReadBytes); + + pSerialDevice = sbgInterfaceFileGetDesc(pInterface); + + // + // Read some bytes on the serial buffer + // + if (ReadFile(pSerialDevice, pBuffer, (DWORD)bytesToRead, (LPDWORD)&bytesRead, NULL) == true) + { + // + // 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, sizeof(errorMsg)); + SBG_LOG_ERROR(SBG_READ_ERROR, "Read failed: %s", errorMsg); + return SBG_READ_ERROR; + } } /*! @@ -205,118 +205,118 @@ static SbgErrorCode sbgInterfaceSerialRead(SbgInterface *pInterface, void *pBuff * If flags include SBG_IF_FLUSH_INPUT, all pending input data is discarded. * If flags include SBG_IF_FLUSH_OUTPUT, the function blocks until all output data has been written out. * - * \param[in] pInterface Interface instance. - * \param[in] flags Combination of the SBG_IF_FLUSH_INPUT and SBG_IF_FLUSH_OUTPUT flags. - * \return SBG_NO_ERROR if successful. + * \param[in] pInterface Interface instance. + * \param[in] flags Combination of the SBG_IF_FLUSH_INPUT and SBG_IF_FLUSH_OUTPUT flags. + * \return SBG_NO_ERROR if successful. */ static SbgErrorCode sbgInterfaceSerialFlush(SbgInterface *pInterface, uint32_t flags) { - SbgErrorCode errorCode; - HANDLE pSerialDevice; - BOOL result = TRUE; - - assert(pInterface); - assert(pInterface->type == SBG_IF_TYPE_SERIAL); - - pSerialDevice = sbgInterfaceFileGetDesc(pInterface); - - if (result && (flags & SBG_IF_FLUSH_INPUT)) - { - uint8_t buffer[256]; - size_t nrBytes; - - // - // XXX The PurgeComm function is too unreliable. Instead, perform the flush by manually - // draining all input data. - // - do - { - errorCode = sbgInterfaceSerialRead(pInterface, buffer, &nrBytes, sizeof(buffer)); - } - while ((errorCode == SBG_NO_ERROR) && (nrBytes != 0)); - - if (errorCode != SBG_NO_ERROR) - { - result = FALSE; - } - } - - if (result && (flags & SBG_IF_FLUSH_OUTPUT)) - { - result = FlushFileBuffers(pSerialDevice); - - if (!result) - { - char errorMsg[256]; - - sbgGetWindowsErrorMsg(errorMsg, sizeof(errorMsg)); - SBG_LOG_ERROR(SBG_WRITE_ERROR, "unable to flush output, error:%s", errorMsg); - } - } - - if (result == TRUE) - { - errorCode = SBG_NO_ERROR; - } - else - { - errorCode = SBG_ERROR; - } - - return errorCode; + SbgErrorCode errorCode; + HANDLE pSerialDevice; + BOOL result = TRUE; + + assert(pInterface); + assert(pInterface->type == SBG_IF_TYPE_SERIAL); + + pSerialDevice = sbgInterfaceFileGetDesc(pInterface); + + if (result && (flags & SBG_IF_FLUSH_INPUT)) + { + uint8_t buffer[256]; + size_t nrBytes; + + // + // XXX The PurgeComm function is too unreliable. Instead, perform the flush by manually + // draining all input data. + // + do + { + errorCode = sbgInterfaceSerialRead(pInterface, buffer, &nrBytes, sizeof(buffer)); + } + while ((errorCode == SBG_NO_ERROR) && (nrBytes != 0)); + + if (errorCode != SBG_NO_ERROR) + { + result = FALSE; + } + } + + if (result && (flags & SBG_IF_FLUSH_OUTPUT)) + { + result = FlushFileBuffers(pSerialDevice); + + if (!result) + { + char errorMsg[256]; + + sbgGetWindowsErrorMsg(errorMsg, sizeof(errorMsg)); + SBG_LOG_ERROR(SBG_WRITE_ERROR, "unable to flush output, error:%s", errorMsg); + } + } + + if (result == TRUE) + { + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_ERROR; + } + + return errorCode; } /*! * Change the serial interface baudrate and flush the serial port to avoid having trash. * - * \param[in] pInterface Valid handle on an initialized interface. - * \param[in] baudRate The new baudrate to apply in bps. - * \return SBG_NO_ERROR if everything is OK + * \param[in] pInterface Valid handle on an initialized interface. + * \param[in] baudRate The new baudrate to apply in bps. + * \return SBG_NO_ERROR if everything is OK */ static SbgErrorCode sbgInterfaceSerialSetSpeed(SbgInterface *pInterface, uint32_t baudRate) { - SbgErrorCode errorCode = SBG_NO_ERROR; - HANDLE pSerialDevice; - DCB comState; - char errorMsg[256]; - - assert(pInterface); - assert(pInterface->type == SBG_IF_TYPE_SERIAL); - - pSerialDevice = sbgInterfaceFileGetDesc(pInterface); - - // - // Try to retreive current com state - // - if (GetCommState(pSerialDevice, &comState)) - { - comState.BaudRate = baudRate; - - // - // Send the new configuration - if (SetCommState(pSerialDevice, &comState)) - { - // - // Flush the serial interface - // - errorCode = sbgInterfaceFlush(pInterface, SBG_IF_FLUSH_ALL); - } - else - { - errorCode = SBG_ERROR; - sbgGetWindowsErrorMsg(errorMsg, sizeof(errorMsg)); - SBG_LOG_ERROR(errorCode, "Unable to set com state: %s", errorMsg); - - } - } - else - { - errorCode = SBG_ERROR; - sbgGetWindowsErrorMsg(errorMsg, sizeof(errorMsg)); - SBG_LOG_ERROR(errorCode, "Unable to retreive com state: %s", errorMsg); - } - - return errorCode; + SbgErrorCode errorCode = SBG_NO_ERROR; + HANDLE pSerialDevice; + DCB comState; + char errorMsg[256]; + + assert(pInterface); + assert(pInterface->type == SBG_IF_TYPE_SERIAL); + + pSerialDevice = sbgInterfaceFileGetDesc(pInterface); + + // + // Try to retreive current com state + // + if (GetCommState(pSerialDevice, &comState)) + { + comState.BaudRate = baudRate; + + // + // Send the new configuration + if (SetCommState(pSerialDevice, &comState)) + { + // + // Flush the serial interface + // + errorCode = sbgInterfaceFlush(pInterface, SBG_IF_FLUSH_ALL); + } + else + { + errorCode = SBG_ERROR; + sbgGetWindowsErrorMsg(errorMsg, sizeof(errorMsg)); + SBG_LOG_ERROR(errorCode, "Unable to set com state: %s", errorMsg); + + } + } + else + { + errorCode = SBG_ERROR; + sbgGetWindowsErrorMsg(errorMsg, sizeof(errorMsg)); + SBG_LOG_ERROR(errorCode, "Unable to retrieve com state: %s", errorMsg); + } + + return errorCode; } /*! @@ -324,33 +324,33 @@ static SbgErrorCode sbgInterfaceSerialSetSpeed(SbgInterface *pInterface, uint32_ * * WARNING: The method will returns zero if not applicable for a type of interface * - * \param[in] pInterface Interface instance. - * \return The current interface baud rate in bps or zero if not applicable. + * \param[in] pInterface Interface instance. + * \return The current interface baud rate in bps or zero if not applicable. */ static uint32_t sbgInterfaceSerialGetSpeed(const SbgInterface *pInterface) { - SbgErrorCode errorCode = SBG_NO_ERROR; - HANDLE pSerialDevice; - DCB comState; - char errorMsg[256]; - - assert(pInterface); - assert(pInterface->type == SBG_IF_TYPE_SERIAL); - - pSerialDevice = sbgInterfaceFileGetDesc((SbgInterface*)pInterface); - - if (GetCommState(pSerialDevice, &comState)) - { - return comState.BaudRate; - } - else - { - errorCode = SBG_ERROR; - sbgGetWindowsErrorMsg(errorMsg, sizeof(errorMsg)); - SBG_LOG_ERROR(errorCode, "Unable to retreive com state: %s", errorMsg); - } - - return errorCode; + SbgErrorCode errorCode = SBG_NO_ERROR; + HANDLE pSerialDevice; + DCB comState; + char errorMsg[256]; + + assert(pInterface); + assert(pInterface->type == SBG_IF_TYPE_SERIAL); + + pSerialDevice = sbgInterfaceFileGetDesc((SbgInterface*)pInterface); + + if (GetCommState(pSerialDevice, &comState)) + { + return comState.BaudRate; + } + else + { + errorCode = SBG_ERROR; + sbgGetWindowsErrorMsg(errorMsg, sizeof(errorMsg)); + SBG_LOG_ERROR(errorCode, "Unable to retrieve com state: %s", errorMsg); + } + + return errorCode; } //----------------------------------------------------------------------// @@ -359,152 +359,152 @@ static uint32_t sbgInterfaceSerialGetSpeed(const SbgInterface *pInterface) SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceSerialCreate(SbgInterface *pInterface, const char *deviceName, uint32_t baudRate) { - char errorMsg[256]; - char comPortPath[32]; - COMMTIMEOUTS comTimeOut; - DCB comState; - uint32_t deviceNum; - HANDLE hSerialDevice; - - assert(pInterface); - assert(deviceName); - - // - // Always call the underlying zero init method to make sure we can correctly handle SbgInterface evolutions - // - sbgInterfaceZeroInit(pInterface); - - // - // 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 = CreateFileA(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)) - { - // - // Retrieve 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)) ) - { - // - // Define the COM port buffer size - // - if (SetupComm(hSerialDevice, SBG_IF_SERIAL_RX_BUFFER_SIZE, SBG_IF_SERIAL_TX_BUFFER_SIZE)) - { - // - // Define base interface members - // - pInterface->handle = hSerialDevice; - pInterface->type = SBG_IF_TYPE_SERIAL; - - // - // Define the interface name - // - sbgInterfaceNameSet(pInterface, deviceName); - - // - // Define all overloaded members - // - pInterface->pDestroyFunc = sbgInterfaceSerialDestroy; - pInterface->pReadFunc = sbgInterfaceSerialRead; - pInterface->pWriteFunc = sbgInterfaceSerialWrite; - pInterface->pFlushFunc = sbgInterfaceSerialFlush; - pInterface->pSetSpeedFunc = sbgInterfaceSerialSetSpeed; - pInterface->pGetSpeedFunc = sbgInterfaceSerialGetSpeed; - - // - // Purge the communication - // - return sbgInterfaceSerialFlush(pInterface, SBG_IF_FLUSH_ALL); - } - else - { - sbgGetWindowsErrorMsg(errorMsg, sizeof(errorMsg)); - SBG_LOG_ERROR(SBG_ERROR, "Unable to define buffer size: %s", errorMsg); - } - } - else - { - sbgGetWindowsErrorMsg(errorMsg, sizeof(errorMsg)); - SBG_LOG_ERROR(SBG_ERROR, "Unable to set com state and/or timeout: %s", errorMsg); - } - } - else - { - sbgGetWindowsErrorMsg(errorMsg, sizeof(errorMsg)); - SBG_LOG_ERROR(SBG_ERROR, "Unable to retreive com state and/or timeout: %s", errorMsg); - } - } - else - { - sbgGetWindowsErrorMsg(errorMsg, sizeof(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; - } + char errorMsg[256]; + char comPortPath[32]; + COMMTIMEOUTS comTimeOut; + DCB comState; + uint32_t deviceNum; + HANDLE hSerialDevice; + + assert(pInterface); + assert(deviceName); + + // + // Always call the underlying zero init method to make sure we can correctly handle SbgInterface evolutions + // + sbgInterfaceZeroInit(pInterface); + + // + // 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 = CreateFileA(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)) + { + // + // Retrieve 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)) ) + { + // + // Define the COM port buffer size + // + if (SetupComm(hSerialDevice, SBG_IF_SERIAL_RX_BUFFER_SIZE, SBG_IF_SERIAL_TX_BUFFER_SIZE)) + { + // + // Define base interface members + // + pInterface->handle = hSerialDevice; + pInterface->type = SBG_IF_TYPE_SERIAL; + + // + // Define the interface name + // + sbgInterfaceNameSet(pInterface, deviceName); + + // + // Define all overloaded members + // + pInterface->pDestroyFunc = sbgInterfaceSerialDestroy; + pInterface->pReadFunc = sbgInterfaceSerialRead; + pInterface->pWriteFunc = sbgInterfaceSerialWrite; + pInterface->pFlushFunc = sbgInterfaceSerialFlush; + pInterface->pSetSpeedFunc = sbgInterfaceSerialSetSpeed; + pInterface->pGetSpeedFunc = sbgInterfaceSerialGetSpeed; + + // + // Purge the communication + // + return sbgInterfaceSerialFlush(pInterface, SBG_IF_FLUSH_ALL); + } + else + { + sbgGetWindowsErrorMsg(errorMsg, sizeof(errorMsg)); + SBG_LOG_ERROR(SBG_ERROR, "Unable to define buffer size: %s", errorMsg); + } + } + else + { + sbgGetWindowsErrorMsg(errorMsg, sizeof(errorMsg)); + SBG_LOG_ERROR(SBG_ERROR, "Unable to set com state and/or timeout: %s", errorMsg); + } + } + else + { + sbgGetWindowsErrorMsg(errorMsg, sizeof(errorMsg)); + SBG_LOG_ERROR(SBG_ERROR, "Unable to retrieve com state and/or timeout: %s", errorMsg); + } + } + else + { + sbgGetWindowsErrorMsg(errorMsg, sizeof(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; + } } diff --git a/common/interfaces/sbgInterfaceUdp.c b/common/interfaces/sbgInterfaceUdp.c index d75aa10..78035fb 100644 --- a/common/interfaces/sbgInterfaceUdp.c +++ b/common/interfaces/sbgInterfaceUdp.c @@ -9,7 +9,7 @@ #include #include -#define SOCKLEN int +#define SOCKLEN int #else // WIN32 #include #include @@ -19,35 +19,35 @@ #include #include -#define SOCKADDR_IN struct sockaddr_in -#define SOCKADDR struct sockaddr -#define SOCKET int -#define SOCKLEN socklen_t -#define INVALID_SOCKET (~((SOCKET)0)) -#define SOCKET_ERROR (-1) -#define NO_ERROR (0) -#define SD_BOTH (2) +#define SOCKADDR_IN struct sockaddr_in +#define SOCKADDR struct sockaddr +#define SOCKET int +#define SOCKLEN socklen_t +#define INVALID_SOCKET (~((SOCKET)0)) +#define SOCKET_ERROR (-1) +#define NO_ERROR (0) +#define SD_BOTH (2) -#define closesocket close +#define closesocket close #endif // WIN32 //----------------------------------------------------------------------// //- Private definitions -// //----------------------------------------------------------------------// -#define SBG_INTERFACE_UDP_PACKET_MAX_SIZE (1400) +#define SBG_INTERFACE_UDP_PACKET_MAX_SIZE (1400) /*! * Structure that stores all internal data used by the UDP interface. */ typedef struct _SbgInterfaceUdp { - SOCKET udpSocket; /*!< The socket used to send and / or receive some UDP data. */ - bool useConnected; /*!< Set to true to use connected mode or false for unconnected. */ + SOCKET udpSocket; /*!< The socket used to send and / or receive some UDP data. */ + bool useConnected; /*!< Set to true to use connected mode or false for unconnected. */ - 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. */ + 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; //----------------------------------------------------------------------// @@ -57,274 +57,274 @@ typedef struct _SbgInterfaceUdp /*! * Returns the UDP interface instance. * - * \param[in] pInterface Interface instance. - * \return The UDP interface instance. + * \param[in] pInterface Interface instance. + * \return The UDP interface instance. */ static SbgInterfaceUdp *sbgInterfaceUdpGet(SbgInterface *pInterface) { - assert(pInterface); - assert(pInterface->type == SBG_IF_TYPE_ETH_UDP); - assert(pInterface->handle); + assert(pInterface); + assert(pInterface->type == SBG_IF_TYPE_ETH_UDP); + assert(pInterface->handle); - return (SbgInterfaceUdp*)pInterface->handle; + return (SbgInterfaceUdp*)pInterface->handle; } /*! * Returns the UDP interface instance (const version) * - * \param[in] pInterface Interface instance. - * \return The UDP interface instance. + * \param[in] pInterface Interface instance. + * \return The UDP interface instance. */ static const SbgInterfaceUdp *sbgInterfaceUdpGetConst(const SbgInterface *pInterface) { - assert(pInterface); - assert(pInterface->type == SBG_IF_TYPE_ETH_UDP); - assert(pInterface->handle); + assert(pInterface); + assert(pInterface->type == SBG_IF_TYPE_ETH_UDP); + assert(pInterface->handle); - return (const SbgInterfaceUdp*)pInterface->handle; + return (const SbgInterfaceUdp*)pInterface->handle; } /*! * Initialize the socket API. * - * \return SBG_NO_ERROR if the socket API has been correctly initialized. + * \return SBG_NO_ERROR if the socket API has been correctly initialized. */ static SbgErrorCode sbgInterfaceUdpInitSockets(void) { #ifdef WIN32 - WSADATA wsaData; - - if (WSAStartup(MAKEWORD(2, 2), &wsaData) == NO_ERROR) - { - return SBG_NO_ERROR; - } - else - { - return SBG_ERROR; - } + WSADATA wsaData; + + if (WSAStartup(MAKEWORD(2, 2), &wsaData) == NO_ERROR) + { + return SBG_NO_ERROR; + } + else + { + return SBG_ERROR; + } #else - return SBG_NO_ERROR; + return SBG_NO_ERROR; #endif } /*! * Uninitialize the socket API. * - * \return SBG_NO_ERROR if the socket API has been uninitialized. + * \return SBG_NO_ERROR if the socket API has been uninitialized. */ static SbgErrorCode sbgInterfaceUpdateCloseSockets(void) { #ifdef WIN32 - if (WSACleanup() == NO_ERROR) - { - return SBG_NO_ERROR; - } - else - { - return SBG_ERROR; - } + if (WSACleanup() == NO_ERROR) + { + return SBG_NO_ERROR; + } + else + { + return SBG_ERROR; + } #else - return SBG_NO_ERROR; + return SBG_NO_ERROR; #endif } /*! * Define if a socket should block or not on receive and send calls. * - * \param[in] pInterfaceUdp The UDP interface to change socket mode. - * \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. + * \param[in] pInterfaceUdp The UDP interface to change socket mode. + * \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. */ static SbgErrorCode sbgInterfaceUdpSetSocketBlocking(SbgInterfaceUdp *pInterfaceUdp, bool blocking) { - assert(pInterfaceUdp); + assert(pInterfaceUdp); #ifdef WIN32 - u_long blockingMode; - - blockingMode = (blocking ? 0 : 1); - - if (ioctlsocket(pInterfaceUdp->udpSocket, FIONBIO, &blockingMode) == NO_ERROR) - { - return SBG_NO_ERROR; - } - else - { - return SBG_ERROR; - } + u_long blockingMode; + + blockingMode = (blocking ? 0 : 1); + + if (ioctlsocket(pInterfaceUdp->udpSocket, FIONBIO, &blockingMode) == NO_ERROR) + { + return SBG_NO_ERROR; + } + else + { + return SBG_ERROR; + } #else // WIN32 - int32_t flags; + int32_t flags; - flags = fcntl(pInterfaceUdp->udpSocket, F_GETFL, 0); + flags = fcntl(pInterfaceUdp->udpSocket, F_GETFL, 0); - if (flags >= 0) - { - flags = (blocking ? (flags & ~O_NONBLOCK) : (flags | O_NONBLOCK)); + if (flags >= 0) + { + flags = (blocking ? (flags & ~O_NONBLOCK) : (flags | O_NONBLOCK)); - if (fcntl(pInterfaceUdp->udpSocket, F_SETFL, flags) == 0) - { - return SBG_NO_ERROR; - } - } + if (fcntl(pInterfaceUdp->udpSocket, F_SETFL, flags) == 0) + { + return SBG_NO_ERROR; + } + } - return SBG_ERROR; + return SBG_ERROR; #endif // WIN32 } /*! * 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. + * \param[in] pInterface Pointer on a valid UDP interface created using sbgInterfaceUdpCreate. + * \return SBG_NO_ERROR if the interface has been closed and released. */ static SbgErrorCode sbgInterfaceUdpDestroy(SbgInterface *pInterface) { - SbgInterfaceUdp *pUdpHandle; + SbgInterfaceUdp *pUdpHandle; - assert(pInterface); - assert(pInterface->type == SBG_IF_TYPE_ETH_UDP); + assert(pInterface); + assert(pInterface->type == SBG_IF_TYPE_ETH_UDP); - pUdpHandle = sbgInterfaceUdpGet(pInterface); + pUdpHandle = sbgInterfaceUdpGet(pInterface); - // - // Close the socket - // - shutdown(pUdpHandle->udpSocket, SD_BOTH); - closesocket(pUdpHandle->udpSocket); + // + // Close the socket + // + shutdown(pUdpHandle->udpSocket, SD_BOTH); + closesocket(pUdpHandle->udpSocket); - // - // free the allocated sbgInterfaceUdp instance - // - free(pUdpHandle); + // + // free the allocated sbgInterfaceUdp instance + // + free(pUdpHandle); - sbgInterfaceZeroInit(pInterface); + sbgInterfaceZeroInit(pInterface); - return sbgInterfaceUpdateCloseSockets(); + return sbgInterfaceUpdateCloseSockets(); } /*! * 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. + * \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. */ static SbgErrorCode sbgInterfaceUdpWrite(SbgInterface *pInterface, const void *pBuffer, size_t bytesToWrite) { - SbgErrorCode errorCode; - SbgInterfaceUdp *pUdpHandle; - SOCKADDR_IN outAddr; - - assert(pInterface); - assert(pInterface->type == SBG_IF_TYPE_ETH_UDP); - assert(pBuffer); - - pUdpHandle = sbgInterfaceUdpGet(pInterface); - - outAddr.sin_family = AF_INET; - outAddr.sin_addr.s_addr = pUdpHandle->remoteAddr; - outAddr.sin_port = htons((uint16_t)pUdpHandle->remotePort); - - while (bytesToWrite != 0) - { - int partialWriteSize; - int nrBytesSent; - - partialWriteSize = (int)bytesToWrite; - - if (partialWriteSize > SBG_INTERFACE_UDP_PACKET_MAX_SIZE) - { - partialWriteSize = SBG_INTERFACE_UDP_PACKET_MAX_SIZE; - } - - nrBytesSent = sendto(pUdpHandle->udpSocket, pBuffer, partialWriteSize, 0, (SOCKADDR *)&outAddr, sizeof(outAddr)); - - if (nrBytesSent != partialWriteSize) - { - break; - } - - bytesToWrite -= (size_t)partialWriteSize; - pBuffer = (const uint8_t *)pBuffer + partialWriteSize; - } - - if (bytesToWrite == 0) - { - errorCode = SBG_NO_ERROR; - } - else - { - errorCode = SBG_WRITE_ERROR; - } - - return errorCode; + SbgErrorCode errorCode; + SbgInterfaceUdp *pUdpHandle; + SOCKADDR_IN outAddr; + + assert(pInterface); + assert(pInterface->type == SBG_IF_TYPE_ETH_UDP); + assert(pBuffer); + + pUdpHandle = sbgInterfaceUdpGet(pInterface); + + outAddr.sin_family = AF_INET; + outAddr.sin_addr.s_addr = pUdpHandle->remoteAddr; + outAddr.sin_port = htons((uint16_t)pUdpHandle->remotePort); + + while (bytesToWrite != 0) + { + int partialWriteSize; + int nrBytesSent; + + partialWriteSize = (int)bytesToWrite; + + if (partialWriteSize > SBG_INTERFACE_UDP_PACKET_MAX_SIZE) + { + partialWriteSize = SBG_INTERFACE_UDP_PACKET_MAX_SIZE; + } + + nrBytesSent = sendto(pUdpHandle->udpSocket, pBuffer, partialWriteSize, 0, (SOCKADDR *)&outAddr, sizeof(outAddr)); + + if (nrBytesSent != partialWriteSize) + { + break; + } + + bytesToWrite -= (size_t)partialWriteSize; + pBuffer = (const uint8_t *)pBuffer + partialWriteSize; + } + + if (bytesToWrite == 0) + { + errorCode = SBG_NO_ERROR; + } + else + { + 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_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. + * \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. */ static SbgErrorCode sbgInterfaceUdpRead(SbgInterface *pInterface, void *pBuffer, size_t *pReadBytes, size_t bytesToRead) { - SbgErrorCode errorCode; - SbgInterfaceUdp *pUdpHandle; - SOCKADDR_IN remoteAddr; - SOCKLEN remoteAddrLen; - int ret; - - assert(pInterface); - assert(pInterface->type == SBG_IF_TYPE_ETH_UDP); - assert(pBuffer); - assert(pReadBytes); - - pUdpHandle = sbgInterfaceUdpGet(pInterface); - - remoteAddrLen = sizeof(remoteAddr); - ret = recvfrom(pUdpHandle->udpSocket, pBuffer, (int)bytesToRead, 0, (SOCKADDR *)&remoteAddr, &remoteAddrLen); - - if (ret != -1) - { - if (pUdpHandle->useConnected) - { - if ( (pUdpHandle->remoteAddr != remoteAddr.sin_addr.s_addr) || (pUdpHandle->remotePort != ntohs(remoteAddr.sin_port)) ) - { - char remoteAddrString[16]; - - sbgNetworkIpToString(remoteAddr.sin_addr.s_addr, remoteAddrString, sizeof(remoteAddrString)); - SBG_LOG_DEBUG("received data from invalid remote host (%s:%u)", remoteAddrString, ntohs(remoteAddr.sin_port)); - ret = 0; - } - } - - errorCode = SBG_NO_ERROR; - } + SbgErrorCode errorCode; + SbgInterfaceUdp *pUdpHandle; + SOCKADDR_IN remoteAddr; + SOCKLEN remoteAddrLen; + int ret; + + assert(pInterface); + assert(pInterface->type == SBG_IF_TYPE_ETH_UDP); + assert(pBuffer); + assert(pReadBytes); + + pUdpHandle = sbgInterfaceUdpGet(pInterface); + + remoteAddrLen = sizeof(remoteAddr); + ret = recvfrom(pUdpHandle->udpSocket, pBuffer, (int)bytesToRead, 0, (SOCKADDR *)&remoteAddr, &remoteAddrLen); + + if (ret != -1) + { + if (pUdpHandle->useConnected) + { + if ( (pUdpHandle->remoteAddr != remoteAddr.sin_addr.s_addr) || (pUdpHandle->remotePort != ntohs(remoteAddr.sin_port)) ) + { + char remoteAddrString[16]; + + sbgNetworkIpToString(remoteAddr.sin_addr.s_addr, remoteAddrString, sizeof(remoteAddrString)); + SBG_LOG_DEBUG("received data from invalid remote host (%s:%u)", remoteAddrString, ntohs(remoteAddr.sin_port)); + ret = 0; + } + } + + errorCode = SBG_NO_ERROR; + } #ifdef WIN32 - else if (WSAGetLastError() == WSAEWOULDBLOCK) + else if (WSAGetLastError() == WSAEWOULDBLOCK) #else // WIN32 - else if ((errno == EAGAIN) || (errno == EWOULDBLOCK)) + else if ((errno == EAGAIN) || (errno == EWOULDBLOCK)) #endif // WIN32 - { - errorCode = SBG_NO_ERROR; - ret = 0; - } - else - { - errorCode = SBG_READ_ERROR; - SBG_LOG_ERROR(errorCode, "unable to receive data"); - } - - if (errorCode == SBG_NO_ERROR) - { - *pReadBytes = (size_t)ret; - } - - return errorCode; + { + errorCode = SBG_NO_ERROR; + ret = 0; + } + else + { + errorCode = SBG_READ_ERROR; + SBG_LOG_ERROR(errorCode, "unable to receive data"); + } + + if (errorCode == SBG_NO_ERROR) + { + *pReadBytes = (size_t)ret; + } + + return errorCode; } //----------------------------------------------------------------------// @@ -333,163 +333,163 @@ static SbgErrorCode sbgInterfaceUdpRead(SbgInterface *pInterface, void *pBuffer, SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceUdpCreate(SbgInterface *pInterface, sbgIpAddress remoteAddr, uint32_t remotePort, uint32_t localPort) { - SbgErrorCode errorCode; - - assert(pInterface); - - // - // Always call the underlying zero init method to make sure we can correctly handle SbgInterface evolutions - // - sbgInterfaceZeroInit(pInterface); - - errorCode = sbgInterfaceUdpInitSockets(); - - if (errorCode == SBG_NO_ERROR) - { - SbgInterfaceUdp *pNewUdpHandle; - - pNewUdpHandle = malloc(sizeof(*pNewUdpHandle)); - - if (pNewUdpHandle) - { - pNewUdpHandle->useConnected = false; - pNewUdpHandle->remoteAddr = remoteAddr; - pNewUdpHandle->remotePort = remotePort; - pNewUdpHandle->localPort = localPort; - - pNewUdpHandle->udpSocket = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); - - if (pNewUdpHandle->udpSocket != INVALID_SOCKET) - { - errorCode = sbgInterfaceUdpSetSocketBlocking(pNewUdpHandle, false); - - if (errorCode == SBG_NO_ERROR) - { - SOCKADDR_IN bindAddress; - int socketError; - - // - // Accept incoming data from any IP address but on localPort only - // - bindAddress.sin_family = AF_INET; - bindAddress.sin_addr.s_addr = INADDR_ANY; - bindAddress.sin_port = htons((uint16_t)localPort); - - socketError = bind(pNewUdpHandle->udpSocket, (SOCKADDR *)&bindAddress, sizeof(bindAddress)); - - if (socketError != SOCKET_ERROR) - { - char interfaceName[48]; - char ipStr[16]; - - // - // The serial port is ready so create a new serial interface - // - pInterface->handle = pNewUdpHandle; - pInterface->type = SBG_IF_TYPE_ETH_UDP; - - // - // Define the interface name - // - sbgNetworkIpToString(remoteAddr, ipStr, sizeof(ipStr)); - sprintf(interfaceName, "UDP: %s out: %u in: %u", ipStr, remotePort, localPort); - sbgInterfaceNameSet(pInterface, interfaceName); - - // - // Define all overloaded members - // - pInterface->pDestroyFunc = sbgInterfaceUdpDestroy; - pInterface->pReadFunc = sbgInterfaceUdpRead; - pInterface->pWriteFunc = sbgInterfaceUdpWrite; - - return SBG_NO_ERROR; - } - else - { - errorCode = SBG_ERROR; - SBG_LOG_ERROR(errorCode, "unable to bind socket"); - } - } - else - { - errorCode = SBG_ERROR; - SBG_LOG_ERROR(errorCode, "unable to set non-blocking mode"); - } - - shutdown(pNewUdpHandle->udpSocket, SD_BOTH); - closesocket(pNewUdpHandle->udpSocket); - } - else - { - errorCode = SBG_ERROR; - SBG_LOG_ERROR(errorCode, "unable to create socket"); - } - - SBG_FREE(pNewUdpHandle); - } - else - { - errorCode = SBG_MALLOC_FAILED; - SBG_LOG_ERROR(errorCode, "unable to allocate handle"); - } - - sbgInterfaceUpdateCloseSockets(); - } - - return errorCode; + SbgErrorCode errorCode; + + assert(pInterface); + + // + // Always call the underlying zero init method to make sure we can correctly handle SbgInterface evolutions + // + sbgInterfaceZeroInit(pInterface); + + errorCode = sbgInterfaceUdpInitSockets(); + + if (errorCode == SBG_NO_ERROR) + { + SbgInterfaceUdp *pNewUdpHandle; + + pNewUdpHandle = malloc(sizeof(*pNewUdpHandle)); + + if (pNewUdpHandle) + { + pNewUdpHandle->useConnected = false; + pNewUdpHandle->remoteAddr = remoteAddr; + pNewUdpHandle->remotePort = remotePort; + pNewUdpHandle->localPort = localPort; + + pNewUdpHandle->udpSocket = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); + + if (pNewUdpHandle->udpSocket != INVALID_SOCKET) + { + errorCode = sbgInterfaceUdpSetSocketBlocking(pNewUdpHandle, false); + + if (errorCode == SBG_NO_ERROR) + { + SOCKADDR_IN bindAddress; + int socketError; + + // + // Accept incoming data from any IP address but on localPort only + // + bindAddress.sin_family = AF_INET; + bindAddress.sin_addr.s_addr = INADDR_ANY; + bindAddress.sin_port = htons((uint16_t)localPort); + + socketError = bind(pNewUdpHandle->udpSocket, (SOCKADDR *)&bindAddress, sizeof(bindAddress)); + + if (socketError != SOCKET_ERROR) + { + char interfaceName[48]; + char ipStr[16]; + + // + // The serial port is ready so create a new serial interface + // + pInterface->handle = pNewUdpHandle; + pInterface->type = SBG_IF_TYPE_ETH_UDP; + + // + // Define the interface name + // + sbgNetworkIpToString(remoteAddr, ipStr, sizeof(ipStr)); + sprintf(interfaceName, "UDP: %s out: %u in: %u", ipStr, remotePort, localPort); + sbgInterfaceNameSet(pInterface, interfaceName); + + // + // Define all overloaded members + // + pInterface->pDestroyFunc = sbgInterfaceUdpDestroy; + pInterface->pReadFunc = sbgInterfaceUdpRead; + pInterface->pWriteFunc = sbgInterfaceUdpWrite; + + return SBG_NO_ERROR; + } + else + { + errorCode = SBG_ERROR; + SBG_LOG_ERROR(errorCode, "unable to bind socket"); + } + } + else + { + errorCode = SBG_ERROR; + SBG_LOG_ERROR(errorCode, "unable to set non-blocking mode"); + } + + shutdown(pNewUdpHandle->udpSocket, SD_BOTH); + closesocket(pNewUdpHandle->udpSocket); + } + else + { + errorCode = SBG_ERROR; + SBG_LOG_ERROR(errorCode, "unable to create socket"); + } + + SBG_FREE(pNewUdpHandle); + } + else + { + errorCode = SBG_MALLOC_FAILED; + SBG_LOG_ERROR(errorCode, "unable to allocate handle"); + } + + sbgInterfaceUpdateCloseSockets(); + } + + return errorCode; } SBG_COMMON_LIB_API void sbgInterfaceUdpSetConnectedMode(SbgInterface *pInterface, bool useConnected) { - SbgInterfaceUdp *pUdpHandle; + SbgInterfaceUdp *pUdpHandle; - assert(pInterface); - assert(pInterface->type == SBG_IF_TYPE_ETH_UDP); + assert(pInterface); + assert(pInterface->type == SBG_IF_TYPE_ETH_UDP); - pUdpHandle = sbgInterfaceUdpGet(pInterface); + pUdpHandle = sbgInterfaceUdpGet(pInterface); - pUdpHandle->useConnected = useConnected; + pUdpHandle->useConnected = useConnected; } SBG_COMMON_LIB_API bool sbgInterfaceUdpGetConnectedMode(const SbgInterface *pInterface) { - const SbgInterfaceUdp *pUdpHandle; + const SbgInterfaceUdp *pUdpHandle; - assert(pInterface); - assert(pInterface->type == SBG_IF_TYPE_ETH_UDP); + assert(pInterface); + assert(pInterface->type == SBG_IF_TYPE_ETH_UDP); - pUdpHandle = sbgInterfaceUdpGetConst(pInterface); + pUdpHandle = sbgInterfaceUdpGetConst(pInterface); - return pUdpHandle->useConnected; + return pUdpHandle->useConnected; } SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceUdpAllowBroadcast(SbgInterface *pInterface, bool allowBroadcast) { - SbgErrorCode errorCode; - SbgInterfaceUdp *pUdpHandle; - int socketError; - int optValue; - - assert(pInterface); - assert(pInterface->type == SBG_IF_TYPE_ETH_UDP); - - pUdpHandle = sbgInterfaceUdpGet(pInterface); - - // - // Change socket options to allow broadcast - // - optValue = allowBroadcast; - socketError = setsockopt(pUdpHandle->udpSocket, SOL_SOCKET, SO_BROADCAST, (const char *)&optValue, sizeof(optValue)); - - if (socketError == NO_ERROR) - { - errorCode = SBG_NO_ERROR; - } - else - { - errorCode = SBG_ERROR; - SBG_LOG_ERROR(errorCode, "unable to set socket options"); - } - - return errorCode; + SbgErrorCode errorCode; + SbgInterfaceUdp *pUdpHandle; + int socketError; + int optValue; + + assert(pInterface); + assert(pInterface->type == SBG_IF_TYPE_ETH_UDP); + + pUdpHandle = sbgInterfaceUdpGet(pInterface); + + // + // Change socket options to allow broadcast + // + optValue = allowBroadcast; + socketError = setsockopt(pUdpHandle->udpSocket, SOL_SOCKET, SO_BROADCAST, (const char *)&optValue, sizeof(optValue)); + + if (socketError == NO_ERROR) + { + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_ERROR; + SBG_LOG_ERROR(errorCode, "unable to set socket options"); + } + + return errorCode; } diff --git a/common/interfaces/sbgInterfaceUdp.h b/common/interfaces/sbgInterfaceUdp.h index 91fbbd4..2285653 100644 --- a/common/interfaces/sbgInterfaceUdp.h +++ b/common/interfaces/sbgInterfaceUdp.h @@ -1,13 +1,13 @@ /*! - * \file sbgInterfaceUdp.h - * \ingroup common - * \author SBG Systems - * \date 05 February 2013 + * \file sbgInterfaceUdp.h + * \ingroup common + * \author SBG Systems + * \date 05 February 2013 * - * \brief This file implements an UDP interface. + * \brief This file implements an UDP interface. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -61,11 +61,11 @@ extern "C" { * You can specify a SBG_IPV4_BROADCAST_ADDR address to send an UDP datagram to all hosts * However, you have to allow broadcasted packets with the `sbgInterfaceUdpAllowBroadcast` method. * - * \param[in] pInterface 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 Ethernet port on which the interface is listening. - * \return SBG_NO_ERROR if the interface has been created. + * \param[in] pInterface 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 Ethernet port on which the interface is listening. + * \return SBG_NO_ERROR if the interface has been created. */ SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceUdpCreate(SbgInterface *pInterface, sbgIpAddress remoteAddr, uint32_t remotePort, uint32_t localPort); @@ -74,25 +74,25 @@ SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceUdpCreate(SbgInterface *pInterface, * * If connected mode is enabled, the interface only accepts datagrams from remortAddr & remotePort. * - * \param[in] pInterface Pointer on a valid UDP interface created using sbgInterfaceUdpCreate. - * \param[in] useConnected Set to true to enable connected mode and false for unconnected. + * \param[in] pInterface Pointer on a valid UDP interface created using sbgInterfaceUdpCreate. + * \param[in] useConnected Set to true to enable connected mode and false for unconnected. */ SBG_COMMON_LIB_API void sbgInterfaceUdpSetConnectedMode(SbgInterface *pInterface, bool useConnected); /*! * Returns if the UDP socket is using or not connected mode. * - * \param[in] pInterface Pointer on a valid UDP interface created using sbgInterfaceUdpCreate. - * \return true if connected mode is enabled or false otherwise. + * \param[in] pInterface Pointer on a valid UDP interface created using sbgInterfaceUdpCreate. + * \return true if connected mode is enabled or false otherwise. */ SBG_COMMON_LIB_API bool sbgInterfaceUdpGetConnectedMode(const SbgInterface *pInterface); /*! * 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. + * \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 *pInterface, bool allowBroadcast); diff --git a/common/network/sbgNetwork.c b/common/network/sbgNetwork.c index 188d0ac..a2e0474 100644 --- a/common/network/sbgNetwork.c +++ b/common/network/sbgNetwork.c @@ -10,118 +10,96 @@ //- 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 - // - snprintf(pBuffer, maxSize, "%" PRIu8 ".%" PRIu8 ".%" PRIu8 ".%" PRIu8, sbgIpAddrGetA(ipAddr), sbgIpAddrGetB(ipAddr), sbgIpAddrGetC(ipAddr), sbgIpAddrGetD(ipAddr)); + assert(pBuffer); + assert(maxSize >= 16); + + SBG_UNUSED_PARAMETER(maxSize); + + snprintf(pBuffer, maxSize, "%" PRIu8 ".%" PRIu8 ".%" PRIu8 ".%" PRIu8, 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) { - int ipAddrA; - int ipAddrB; - int ipAddrC; - int ipAddrD; - int numReadParams; - sbgIpAddress ip; - sbgIpAddress ret; - char checkBuffer[SBG_NETWORK_IPV4_STRING_SIZE]; + int ipAddrA; + int ipAddrB; + int ipAddrC; + int ipAddrD; + int numReadParams; + sbgIpAddress ip; + sbgIpAddress ret; + char checkBuffer[SBG_NETWORK_IPV4_STRING_SIZE]; - assert(pBuffer); + assert(pBuffer); - ret = SBG_IPV4_UNSPECIFIED_ADDR; + ret = SBG_IPV4_UNSPECIFIED_ADDR; - numReadParams = sscanf(pBuffer, "%d.%d.%d.%d", &ipAddrA, &ipAddrB, &ipAddrC, &ipAddrD); + numReadParams = sscanf(pBuffer, "%d.%d.%d.%d", &ipAddrA, &ipAddrB, &ipAddrC, &ipAddrD); - if ((numReadParams == 4) && (ipAddrA >= 0) && (ipAddrA <= 255) && (ipAddrB >= 0) && (ipAddrB <= 255) && (ipAddrC >= 0) && (ipAddrC <= 255) && (ipAddrD >= 0) && (ipAddrD <= 255)) - { - ip = sbgIpAddr((uint8_t)ipAddrA, (uint8_t)ipAddrB, (uint8_t)ipAddrC, (uint8_t)ipAddrD); + if ((numReadParams == 4) && (ipAddrA >= 0) && (ipAddrA <= 255) && (ipAddrB >= 0) && (ipAddrB <= 255) && (ipAddrC >= 0) && (ipAddrC <= 255) && (ipAddrD >= 0) && (ipAddrD <= 255)) + { + ip = sbgIpAddr((uint8_t)ipAddrA, (uint8_t)ipAddrB, (uint8_t)ipAddrC, (uint8_t)ipAddrD); - sbgNetworkIpToString(ip, checkBuffer, SBG_ARRAY_SIZE(checkBuffer)); + sbgNetworkIpToString(ip, checkBuffer, SBG_ARRAY_SIZE(checkBuffer)); - if (strcmp(pBuffer, checkBuffer) == 0) - { - ret = ip; - } - } + if (strcmp(pBuffer, checkBuffer) == 0) + { + ret = ip; + } + } - return ret; + return ret; } //----------------------------------------------------------------------// //- 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 - // + 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); + 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; + // + // 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/common/network/sbgNetwork.h b/common/network/sbgNetwork.h index 3df7532..3adc11c 100644 --- a/common/network/sbgNetwork.h +++ b/common/network/sbgNetwork.h @@ -1,10 +1,10 @@ /*! - * \file sbgNetwork.h - * \ingroup common - * \author SBG Systems - * \date September 15, 2015 + * \file sbgNetwork.h + * \ingroup common + * \author SBG Systems + * \date September 15, 2015 * - * \brief Network related tools + * \brief Network related tools * * 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 @@ -20,8 +20,8 @@ * |MSB| | |LSB| * | A | B | C | D | * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -58,10 +58,10 @@ extern "C" { //----------------------------------------------------------------------// //- 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. */ +#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. */ -#define SBG_NETWORK_IPV4_STRING_SIZE (16) /*!< String size for representation of an IPV4 */ +#define SBG_NETWORK_IPV4_STRING_SIZE (16) /*!< String size for representation of an IPV4 */ //----------------------------------------------------------------------// //- IP setters / getters -// @@ -69,74 +69,79 @@ extern "C" { /*! * 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. + * + * \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; + return (a << 24) | (b << 16) | (c << 8) | d; #else - return a | (b << 8) | (c << 16) | (d << 24); + 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. + * + * \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); + return (uint8_t)((ipAddr & 0xFF000000) >> 24); #else - return (uint8_t)((ipAddr & 0x000000FF)); + 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. -*/ + * 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); + return (uint8_t)((ipAddr & 0x00FF0000) >> 16); #else - return (uint8_t)((ipAddr & 0x0000FF00) >> 8); + 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. -*/ + * 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); + return (uint8_t)((ipAddr & 0x0000FF00) >> 8); #else - return (uint8_t)((ipAddr & 0x00FF0000) >> 16); + 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. -*/ + * 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)); + return (uint8_t)((ipAddr & 0x000000FF)); #else - return (uint8_t)((ipAddr & 0xFF000000) >> 24); + return (uint8_t)((ipAddr & 0xFF000000) >> 24); #endif } @@ -146,16 +151,18 @@ SBG_INLINE uint8_t sbgIpAddrGetD(sbgIpAddress ipAddr) /*! * 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. + * + * \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. + * + * \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); @@ -165,24 +172,26 @@ SBG_COMMON_LIB_API sbgIpAddress sbgNetworkIpFromString(const char *pBuffer); /*! * 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. + * + * \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); + 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. + * + * \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); + return (ipAddress & ~netmask); } //----------------------------------------------------------------------// @@ -191,91 +200,90 @@ SBG_INLINE sbgIpAddress sbgIpGetHostAddr(sbgIpAddress ipAddress, sbgIpAddress ne /*! * 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. + * + * \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; - } + 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. + * + * \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; - } + // + // Check the if A part of the ip address is within 1 and 223 + // + if ((sbgIpAddrGetA(ipAddress) > 0) && (sbgIpAddrGetA(ipAddress) < 224)) + { + return true; + } + else + { + 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. + * + * \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; - } + // + // 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. + * + * \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; - } + 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. + * + * \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); diff --git a/common/platform/sbgPlatform.c b/common/platform/sbgPlatform.c index 73f366c..95a50ac 100644 --- a/common/platform/sbgPlatform.c +++ b/common/platform/sbgPlatform.c @@ -5,21 +5,21 @@ //- Include specific header for WIN32 and UNIX platforms -// //----------------------------------------------------------------------// #ifdef WIN32 - #include + #include #elif defined(__APPLE__) - #include + #include #else - #include + #include #endif //----------------------------------------------------------------------// -//- Global singleton for the log callback -// +//- Global singleton for the log callback -// //----------------------------------------------------------------------// /*! * Unique singleton used to log error messages. */ -SbgCommonLibOnLogFunc gLogCallback = NULL; +SbgCommonLibOnLogFunc gLogCallback = NULL; //----------------------------------------------------------------------// //- Public functions -// @@ -28,119 +28,119 @@ SbgCommonLibOnLogFunc gLogCallback = NULL; SBG_COMMON_LIB_API uint32_t sbgGetTime(void) { #ifdef WIN32 - // - // Return the current time in ms - // - return clock() / (CLOCKS_PER_SEC / 1000); + // + // 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); + 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; + // + // 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); + struct timespec now; + clock_gettime(CLOCK_REALTIME, &now); - // - // Return the current time in ms - // - return now.tv_sec * 1000 + now.tv_nsec / 1000000; + // + // Return the current time in ms + // + return now.tv_sec * 1000 + now.tv_nsec / 1000000; #endif } SBG_COMMON_LIB_API void sbgSleep(uint32_t ms) { #ifdef WIN32 - Sleep(ms); + Sleep(ms); #else - struct timespec req; - struct timespec rem; - int ret; - - req.tv_sec = ms / 1000; - req.tv_nsec = (ms % 1000) * 1000000L; - - for (;;) - { - ret = nanosleep(&req, &rem); - - if ((ret == 0) || (errno != EINTR)) - { - break; - } - else - { - req = rem; - } - } + struct timespec req; + struct timespec rem; + int ret; + + req.tv_sec = ms / 1000; + req.tv_nsec = (ms % 1000) * 1000000L; + + for (;;) + { + ret = nanosleep(&req, &rem); + + if ((ret == 0) || (errno != EINTR)) + { + break; + } + else + { + req = rem; + } + } #endif } SBG_COMMON_LIB_API void sbgCommonLibSetLogCallback(SbgCommonLibOnLogFunc logCallback) { - // - // TODO: should we implement lock / sync mechanisms ? - // - gLogCallback = logCallback; + // + // TODO: should we implement lock / sync mechanisms ? + // + gLogCallback = logCallback; } 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; - - assert(pFileName); - assert(pFunctionName); - assert(pCategory); - assert(pFormat); - - // - // Initialize the list of variable arguments on the latest function argument - // - va_start(args, pFormat); - - // - // Generate the error message string - // - vsnprintf(errorMsg, sizeof(errorMsg), pFormat, args); - - // - // Close the list of variable arguments - // - va_end(args); - - // - // Check if there is a valid logger callback if not use a default output - // - if (gLogCallback) - { - gLogCallback(pFileName, pFunctionName, line, pCategory, logType, errorCode, errorMsg); - } - else - { - // - // Log the correct message according to the log type - // - switch (logType) - { - case SBG_DEBUG_LOG_TYPE_ERROR: - fprintf(stderr, "*ERR * %s(%"PRIu32"): %s - %s\n\r", pFunctionName, line, sbgErrorCodeToString(errorCode), errorMsg); - break; - case SBG_DEBUG_LOG_TYPE_WARNING: - fprintf(stderr, "*WARN* %s(%"PRIu32"): %s - %s\n\r", pFunctionName, line, sbgErrorCodeToString(errorCode), errorMsg); - break; - case SBG_DEBUG_LOG_TYPE_INFO: - fprintf(stderr, "*INFO* %s(%"PRIu32"): %s\n\r", pFunctionName, line, errorMsg); - break; - case SBG_DEBUG_LOG_TYPE_DEBUG: - fprintf(stderr, "*DBG * %s(%"PRIu32"): %s\n\r", pFunctionName, line, errorMsg); - break; - default: - fprintf(stderr, "*UKNW* %s(%"PRIu32"): %s\n\r", pFunctionName, line, errorMsg); - break; - } - } + char errorMsg[SBG_CONFIG_LOG_MAX_SIZE]; + va_list args; + + assert(pFileName); + assert(pFunctionName); + assert(pCategory); + assert(pFormat); + + // + // Initialize the list of variable arguments on the latest function argument + // + va_start(args, pFormat); + + // + // Generate the error message string + // + vsnprintf(errorMsg, sizeof(errorMsg), pFormat, args); + + // + // Close the list of variable arguments + // + va_end(args); + + // + // Check if there is a valid logger callback if not use a default output + // + if (gLogCallback) + { + gLogCallback(pFileName, pFunctionName, line, pCategory, logType, errorCode, errorMsg); + } + else + { + // + // Log the correct message according to the log type + // + switch (logType) + { + case SBG_DEBUG_LOG_TYPE_ERROR: + fprintf(stderr, "*ERR * %s(%"PRIu32"): %s - %s\n\r", pFunctionName, line, sbgErrorCodeToString(errorCode), errorMsg); + break; + case SBG_DEBUG_LOG_TYPE_WARNING: + fprintf(stderr, "*WARN* %s(%"PRIu32"): %s - %s\n\r", pFunctionName, line, sbgErrorCodeToString(errorCode), errorMsg); + break; + case SBG_DEBUG_LOG_TYPE_INFO: + fprintf(stderr, "*INFO* %s(%"PRIu32"): %s\n\r", pFunctionName, line, errorMsg); + break; + case SBG_DEBUG_LOG_TYPE_DEBUG: + fprintf(stderr, "*DBG * %s(%"PRIu32"): %s\n\r", pFunctionName, line, errorMsg); + break; + default: + fprintf(stderr, "*UKNW* %s(%"PRIu32"): %s\n\r", pFunctionName, line, errorMsg); + break; + } + } } diff --git a/common/platform/sbgPlatform.h b/common/platform/sbgPlatform.h index 6ad5863..70740ab 100644 --- a/common/platform/sbgPlatform.h +++ b/common/platform/sbgPlatform.h @@ -1,18 +1,18 @@ /*! - * \file sbgPlatform.h - * \ingroup common - * \author SBG Systems - * \date March 17, 2015 + * \file sbgPlatform.h + * \ingroup common + * \author SBG Systems + * \date March 17, 2015 * - * \brief Platform-specific functions. + * \brief Platform-specific functions. * * 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. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -57,13 +57,13 @@ /*! * Type for logging functions. * - * \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] pMessage The message to log. + * \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] pMessage The message to log. */ typedef void (*SbgCommonLibOnLogFunc)(const char *pFileName, const char *pFunctionName, uint32_t line, const char *pCategory, SbgDebugLogType logType, SbgErrorCode errorCode, const char *pMessage); @@ -74,14 +74,14 @@ typedef void (*SbgCommonLibOnLogFunc)(const char *pFileName, const char *pFuncti /*! * Get the current time. * - * \return The current time, in ms. + * \return The current time, in ms. */ SBG_COMMON_LIB_API uint32_t sbgGetTime(void); /*! * Sleep. * - * \param[in] ms Time to wait, in ms. + * \param[in] ms Time to wait, in ms. */ SBG_COMMON_LIB_API void sbgSleep(uint32_t ms); @@ -91,20 +91,20 @@ SBG_COMMON_LIB_API void sbgSleep(uint32_t ms); * Some platforms may not provide the ability to set a user-provided log function, in which * case this function does nothing. * - * \param[in] logCallback Log function. + * \param[in] logCallback Log function. */ SBG_COMMON_LIB_API void sbgCommonLibSetLogCallback(SbgCommonLibOnLogFunc logCallback); /*! * Log a message. * - * \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. + * \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, ...) SBG_CHECK_FORMAT(printf, 7, 8); diff --git a/common/sbgCommon.h b/common/sbgCommon.h index b22adc2..4bef011 100644 --- a/common/sbgCommon.h +++ b/common/sbgCommon.h @@ -1,13 +1,13 @@ /*! - * \file sbgCommon.h - * \ingroup common - * \author SBG Systems - * \date March 17, 2015 + * \file sbgCommon.h + * \ingroup common + * \author SBG Systems + * \date March 17, 2015 * - * \brief Main header for the SBG Systems common C library. + * \brief Main header for the SBG Systems common C library. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -31,8 +31,8 @@ */ /*! - * \defgroup common Common - * \brief SBG Systems foundation framework that is common to all C/C++ projects. + * \defgroup common Common + * \brief SBG Systems foundation framework that is common to all C/C++ projects. */ #ifndef SBG_COMMON_H @@ -54,7 +54,7 @@ extern "C" { * Default: Support only Aligned access - Disabled */ #ifndef SBG_CONFIG_UNALIGNED_ACCESS_AUTH -#define SBG_CONFIG_UNALIGNED_ACCESS_AUTH (0) +#define SBG_CONFIG_UNALIGNED_ACCESS_AUTH (0) #endif /*! @@ -63,7 +63,7 @@ extern "C" { * Default: Little Endian - Disabled */ #ifndef SBG_CONFIG_BIG_ENDIAN -#define SBG_CONFIG_BIG_ENDIAN (0) +#define SBG_CONFIG_BIG_ENDIAN (0) #endif /*! @@ -71,7 +71,7 @@ extern "C" { * Default: Enabled */ #ifndef SBG_CONFIG_ENABLE_LOG_ERROR -#define SBG_CONFIG_ENABLE_LOG_ERROR (1) +#define SBG_CONFIG_ENABLE_LOG_ERROR (1) #endif /*! @@ -79,7 +79,7 @@ extern "C" { * Default: Enabled */ #ifndef SBG_CONFIG_ENABLE_LOG_WARNING -#define SBG_CONFIG_ENABLE_LOG_WARNING (1) +#define SBG_CONFIG_ENABLE_LOG_WARNING (1) #endif /*! @@ -87,7 +87,7 @@ extern "C" { * Default: Enabled */ #ifndef SBG_CONFIG_ENABLE_LOG_INFO -#define SBG_CONFIG_ENABLE_LOG_INFO (1) +#define SBG_CONFIG_ENABLE_LOG_INFO (1) #endif /*! @@ -95,7 +95,7 @@ extern "C" { * Default: Enabled */ #ifndef SBG_CONFIG_ENABLE_LOG_DEBUG -#define SBG_CONFIG_ENABLE_LOG_DEBUG (1) +#define SBG_CONFIG_ENABLE_LOG_DEBUG (1) #endif /*! @@ -103,7 +103,7 @@ extern "C" { * Default: 1024 */ #ifndef SBG_CONFIG_LOG_MAX_SIZE -#define SBG_CONFIG_LOG_MAX_SIZE ((size_t)(1024)) +#define SBG_CONFIG_LOG_MAX_SIZE ((size_t)(1024)) #endif /*! @@ -111,7 +111,7 @@ extern "C" { * Default: 256 */ #ifndef SBG_CONFIG_PATH_MAX_SIZE -#define SBG_CONFIG_PATH_MAX_SIZE ((size_t)(256)) +#define SBG_CONFIG_PATH_MAX_SIZE ((size_t)(256)) #endif //----------------------------------------------------------------------// diff --git a/common/sbgConfig.h b/common/sbgConfig.h index b6763b5..450f66f 100644 --- a/common/sbgConfig.h +++ b/common/sbgConfig.h @@ -1,14 +1,14 @@ /*! - * \file sbgConfig.h - * \author SBG Systems (Raphael Siryani) - * \date 17 March 2015 + * \file sbgConfig.h + * \author SBG Systems + * \date 17 March 2015 * - * \brief Header file used to configure the framework. + * \brief Header file used to configure the framework. * * You can configure for example the logging system. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -41,12 +41,12 @@ /*! * Windows x86 & x64 support both aligned and unaligned access */ -#define SBG_CONFIG_UNALIGNED_ACCESS_AUTH (0) +#define SBG_CONFIG_UNALIGNED_ACCESS_AUTH (0) /*! * Windows is using little endianess */ -#define SBG_CONFIG_BIG_ENDIAN (0) +#define SBG_CONFIG_BIG_ENDIAN (0) //----------------------------------------------------------------------// //- Logging configuration -// @@ -56,9 +56,22 @@ * Enable all error logs * Default: Enabled */ -#define SBG_CONFIG_ENABLE_LOG_ERROR (1) -#define SBG_CONFIG_ENABLE_LOG_WARNING (1) -#define SBG_CONFIG_ENABLE_LOG_INFO (1) -#define SBG_CONFIG_ENABLE_LOG_DEBUG (1) +#define SBG_CONFIG_ENABLE_LOG_ERROR (1) +#define SBG_CONFIG_ENABLE_LOG_WARNING (1) +#define SBG_CONFIG_ENABLE_LOG_INFO (1) +#define SBG_CONFIG_ENABLE_LOG_DEBUG (1) -#endif // SBG_CONFIG_H +//----------------------------------------------------------------------// +//- JSON configuration -// +//----------------------------------------------------------------------// + +/*! + * Reallocation increment size when printing a JSON object to a string, in bytes. + * + * If undefined or set to 0, the standard quadratic strategy is used. Otherwise, + * string buffers are reallocated linearly, adding this value to the buffer size + * on each reallocation. + */ +#define SBG_CONFIG_JSON_PRINT_REALLOC_INC_SIZE (0) + +#endif // SBG_CONFIG_H diff --git a/common/sbgDefines.h b/common/sbgDefines.h index 498acc4..d504909 100644 --- a/common/sbgDefines.h +++ b/common/sbgDefines.h @@ -1,13 +1,13 @@ /*! - * \file sbgDefines.h - * \ingroup common - * \author SBG Systems - * \date 17 March 2015 + * \file sbgDefines.h + * \ingroup common + * \author SBG Systems + * \date 17 March 2015 * - * \brief Header file that contains all common definitions. + * \brief Header file that contains all common definitions. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -66,28 +66,28 @@ * Macro used to handle export and import methods of the sbgCommon library */ #ifdef _MSC_VER - #ifdef SBG_COMMON_STATIC_USE - #define SBG_COMMON_LIB_API - #else - #ifdef SBG_COMMON_LIB_API_EXPORT - #define SBG_COMMON_LIB_API __declspec(dllexport) - #else - #define SBG_COMMON_LIB_API __declspec(dllimport) - #endif - #endif + #ifdef SBG_COMMON_STATIC_USE + #define SBG_COMMON_LIB_API + #else + #ifdef SBG_COMMON_LIB_API_EXPORT + #define SBG_COMMON_LIB_API __declspec(dllexport) + #else + #define SBG_COMMON_LIB_API __declspec(dllimport) + #endif + #endif #else - #define SBG_COMMON_LIB_API + #define SBG_COMMON_LIB_API #endif //----------------------------------------------------------------------// //- Global definitions -// //----------------------------------------------------------------------// #ifndef SBG_DISABLE - #define SBG_DISABLE (0) + #define SBG_DISABLE (0) #endif #ifndef SBG_ENABLE - #define SBG_ENABLE (1) + #define SBG_ENABLE (1) #endif #ifndef FALSE @@ -99,67 +99,67 @@ #endif #ifndef NULL - #define NULL (0) + #define NULL (0) #endif #ifndef SBG_INVALID_HANDLE - #define SBG_INVALID_HANDLE (NULL) + #define SBG_INVALID_HANDLE (NULL) #endif #ifndef SBG_ARRAY_SIZE - #define SBG_ARRAY_SIZE(a) (sizeof(a) / sizeof((a)[0])) + #define SBG_ARRAY_SIZE(a) (sizeof(a) / sizeof((a)[0])) #endif #ifndef SBG_STRLEN - #define SBG_STRLEN(s) (sizeof(s) - 1) + #define SBG_STRLEN(s) (sizeof(s) - 1) #endif #ifndef SBG_QUOTE_NX - #define SBG_QUOTE_NX(x) #x + #define SBG_QUOTE_NX(x) #x #endif #ifndef SBG_QUOTE - #define SBG_QUOTE(x) SBG_QUOTE_NX(x) + #define SBG_QUOTE(x) SBG_QUOTE_NX(x) #endif #ifndef SBG_CONCAT_NX - #define SBG_CONCAT_NX(a, b) a ## b + #define SBG_CONCAT_NX(a, b) a ## b #endif #ifndef SBG_CONCAT - #define SBG_CONCAT(a, b) SBG_CONCAT_NX(a, b) + #define SBG_CONCAT(a, b) SBG_CONCAT_NX(a, b) #endif #ifndef SBG_UNPACK - #define SBG_UNPACK(...) __VA_ARGS__ + #define SBG_UNPACK(...) __VA_ARGS__ #endif #ifndef SBG_CONTAINER_OF - #define SBG_CONTAINER_OF(ptr, type, member) ((type *)((char *)(ptr) - offsetof(type, member))) + #define SBG_CONTAINER_OF(ptr, type, member) ((type *)((char *)(ptr) - offsetof(type, member))) #endif #ifndef SBG_LIKELY - #if defined(__GNUC__) || defined(__clang__) - #define SBG_LIKELY(expr) __builtin_expect((bool)(expr), true) - #else - #define SBG_LIKELY(expr) (expr) - #endif + #if defined(__GNUC__) || defined(__clang__) + #define SBG_LIKELY(expr) __builtin_expect((bool)(expr), true) + #else + #define SBG_LIKELY(expr) (expr) + #endif #endif #ifndef SBG_UNLIKELY - #if defined(__GNUC__) || defined(__clang__) - #define SBG_UNLIKELY(expr) __builtin_expect((bool)(expr), false) - #else - #define SBG_UNLIKELY(expr) (expr) - #endif + #if defined(__GNUC__) || defined(__clang__) + #define SBG_UNLIKELY(expr) __builtin_expect((bool)(expr), false) + #else + #define SBG_UNLIKELY(expr) (expr) + #endif #endif #ifndef SBG_CHECK_FORMAT - #if defined(__GNUC__) || defined(__clang__) - #define SBG_CHECK_FORMAT(style, format_index, va_args) __attribute__((format(style, format_index, va_args))) - #else - #define SBG_CHECK_FORMAT(style, format_index, va_args) - #endif + #if defined(__GNUC__) || defined(__clang__) + #define SBG_CHECK_FORMAT(style, format_index, va_args) __attribute__((format(style, format_index, va_args))) + #else + #define SBG_CHECK_FORMAT(style, format_index, va_args) + #endif #endif /*! @@ -169,27 +169,27 @@ * 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__) || defined(__clang__) || defined(__TI_COMPILER_VERSION__) - #define __SBG_TYPEOF(x) typeof(x) - #endif + #ifdef __cplusplus + #define __SBG_TYPEOF(x) decltype(x) + #elif defined(__GNUC__) || defined(__clang__) || 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 + #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 + #ifdef __SBG_TYPEOF + #define SBG_CONST_CAST_PP(x) ((const __SBG_TYPEOF(**(x))**)(x)) + #else + #define SBG_CONST_CAST_PP(x) x + #endif #endif /*! @@ -197,20 +197,20 @@ */ #if !defined(__GNUC__) && !defined(__clang__) #ifndef __BASE_FILE__ - #define __BASE_FILE__ __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;} + #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;} + #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 //----------------------------------------------------------------------// @@ -221,18 +221,18 @@ * 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 + #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) + #define SBG_UNUSED_PARAMETER(x) (void)(x) #endif /*! @@ -242,17 +242,17 @@ * when an intentional break is missing */ #ifndef SBG_FALLTHROUGH - #if __cplusplus >= 201703L - #define SBG_FALLTHROUGH [[fallthrough]] /* introduced in C++ 17 */ - #elif defined(__GNUC__) || defined(__clang__) - #if (__GNUC__ >= 7) || defined(__clang__) - #define SBG_FALLTHROUGH __attribute__((fallthrough)) - #else - #define SBG_FALLTHROUGH ((void)0) - #endif - #else - #define SBG_FALLTHROUGH - #endif + #if __cplusplus >= 201703L + #define SBG_FALLTHROUGH [[fallthrough]] /* introduced in C++ 17 */ + #elif defined(__GNUC__) || defined(__clang__) + #if (__GNUC__ >= 7) || defined(__clang__) + #define SBG_FALLTHROUGH __attribute__((fallthrough)) + #else + #define SBG_FALLTHROUGH ((void)0) + #endif + #else + #define SBG_FALLTHROUGH + #endif #endif //----------------------------------------------------------------------// @@ -266,11 +266,11 @@ * All structures defined after this macro will be packed. */ #if defined(__GNUC__) || defined(__clang__) || defined(__TI_COMPILER_VERSION__) - #define SBG_BEGIN_PACKED() + #define SBG_BEGIN_PACKED() #elif defined(_MSC_VER) - #define SBG_BEGIN_PACKED() __pragma(pack(push, 1)) + #define SBG_BEGIN_PACKED() __pragma(pack(push, 1)) #else - #error you must byte-align these structures with the appropriate compiler directives + #error you must byte-align these structures with the appropriate compiler directives #endif /*! @@ -279,11 +279,11 @@ * This macro is used to specify that a structure is packed. */ #if defined(__GNUC__) || defined(__clang__) || defined(__TI_COMPILER_VERSION__) - #define SBG_PACKED __attribute__((packed)) + #define SBG_PACKED __attribute__((packed)) #elif defined(_MSC_VER) - #define SBG_PACKED + #define SBG_PACKED #else - #error you must byte-align these structures with the appropriate compiler directives + #error you must byte-align these structures with the appropriate compiler directives #endif /*! @@ -292,39 +292,39 @@ * This macro is used to close the section of packed structures and return to the default packing. */ #if defined(__GNUC__) || defined(__clang__) || defined(__TI_COMPILER_VERSION__) - #define SBG_END_PACKED() + #define SBG_END_PACKED() #elif defined(_MSC_VER) - #define SBG_END_PACKED() __pragma(pack(pop)) + #define SBG_END_PACKED() __pragma(pack(pop)) #else - #error you must byte-align these structures with the appropriate compiler directives + #error you must byte-align these structures with the appropriate compiler directives #endif //----------------------------------------------------------------------// -//- Deprecation definitions -// +//- Deprecation definitions -// //----------------------------------------------------------------------// /*! * Macro used to indicate that a function is deprecated. */ #if defined(__GNUC__) || defined(__clang__) - #define SBG_DEPRECATED(func) __attribute__((deprecated)) func + #define SBG_DEPRECATED(func) __attribute__((deprecated)) func #elif defined(__TI_COMPILER_VERSION__) - #define SBG_DEPRECATED(func) func __attribute__((deprecated)) + #define SBG_DEPRECATED(func) func __attribute__((deprecated)) #elif defined(_MSC_VER) - #define SBG_DEPRECATED(func) __declspec(deprecated) func + #define SBG_DEPRECATED(func) __declspec(deprecated) func #else - #define SBG_DEPRECATED(func) func + #define SBG_DEPRECATED(func) func #endif /*! * Macro used to indicate that a macro is deprecated. */ #if defined(__GNUC__) || defined(__clang__) - #define SBG_DEPRECATED_MACRO(func) __pragma(deprecated(func)) + #define SBG_DEPRECATED_MACRO(func) __pragma(deprecated(func)) #elif defined(_MSC_VER) #define SBG_DEPRECATED_MACRO(func) __pragma(deprecated(func)) #else - #define SBG_DEPRECATED_MACRO(func) func + #define SBG_DEPRECATED_MACRO(func) func #endif /*! @@ -341,185 +341,185 @@ * may currently be disabled by defining SBG_CONFIG_WARN_ABOUT_DEPRECATED_TYPES to 0. */ #if SBG_CONFIG_WARN_ABOUT_DEPRECATED_TYPES != 0 - #if defined(__GNUC__) || defined(__clang__) - #define SBG_DEPRECATED_TYPEDEF(decl) decl __attribute__((deprecated)) - #elif defined(_MSC_VER) - #define SBG_DEPRECATED_TYPEDEF(decl) __declspec(deprecated) decl - #else - #define SBG_DEPRECATED_TYPEDEF(decl) decl - #endif + #if defined(__GNUC__) || defined(__clang__) + #define SBG_DEPRECATED_TYPEDEF(decl) decl __attribute__((deprecated)) + #elif defined(_MSC_VER) + #define SBG_DEPRECATED_TYPEDEF(decl) __declspec(deprecated) decl + #else + #define SBG_DEPRECATED_TYPEDEF(decl) decl + #endif #else - #define SBG_DEPRECATED_TYPEDEF(decl) decl + #define SBG_DEPRECATED_TYPEDEF(decl) decl #endif //----------------------------------------------------------------------// //- Basic maths definitions -// //----------------------------------------------------------------------// #ifndef SBG_PI - #define SBG_PI 3.14159265358979323846 + #define SBG_PI 3.14159265358979323846 #endif #ifndef SBG_PI_F - #define SBG_PI_F 3.14159265358979323846f + #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. + * \param[in] x Signed integer value. + * \return The absolute value of x. */ #ifndef sbgAbs - #define sbgAbs(x) (((x) < 0) ? -(x) : (x)) + #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. + * \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)) + #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. + * \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)) + #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. + * \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)) + #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. - * \return Rounded division + * \param[in] n Dividend. + * \param[in] d Divisor. + * \return Rounded division */ #ifndef sbgDivCeil - #define sbgDivCeil(n, d) (((n) + (d) - 1) / (d)) + #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. + * \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; + 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. + * \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; + 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. + * \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; + 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. + * \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; + return angle * SBG_PI_F / 180.0f; } /*! * Test if two floating single-point numbers are equals or not. * - * \param[in] leftValue The first operand to test for equality. - * \param[in] rightValue The second operand to test for equality. - * \return true if both left and right operands are almost equal. + * \param[in] leftValue The first operand to test for equality. + * \param[in] rightValue The second operand to test for equality. + * \return true if both left and right operands are almost equal. */ SBG_INLINE bool sbgAlmostEqualsFloat(float leftValue, float rightValue) { - // - // The IEEE standard says that any comparison operation involving a NAN must return false. - // - if (isnan(leftValue) || isnan(rightValue)) - { - return false; - } - - // - // This method is not good enough and should be updated using DistanceBetweenSignAndMagnitudeNumbers methods - // - if (fabsf(leftValue - rightValue) < FLT_EPSILON) - { - return true; - } - else - { - return false; - } + // + // The IEEE standard says that any comparison operation involving a NAN must return false. + // + if (isnan(leftValue) || isnan(rightValue)) + { + return false; + } + + // + // This method is not good enough and should be updated using DistanceBetweenSignAndMagnitudeNumbers methods + // + if (fabsf(leftValue - rightValue) < FLT_EPSILON) + { + return true; + } + else + { + return false; + } } /*! * Test if two floating double-point numbers are equals or not using the epsilon technique * - * \param[in] leftValue The first operand to test for equality. - * \param[in] rightValue The second operand to test for equality. - * \return true if both left and right operands are almost equal. + * \param[in] leftValue The first operand to test for equality. + * \param[in] rightValue The second operand to test for equality. + * \return true if both left and right operands are almost equal. */ SBG_INLINE bool sbgAlmostEqualsDouble(double leftValue, double rightValue) { - // - // The IEEE standard says that any comparison operation involving a NAN must return false. - // - if (isnan(leftValue) || isnan(rightValue)) - { - return false; - } - - // - // This method is not good enough and should be updated using DistanceBetweenSignAndMagnitudeNumbers methods - // - if (fabs(leftValue - rightValue) < DBL_EPSILON) - { - return true; - } - else - { - return false; - } + // + // The IEEE standard says that any comparison operation involving a NAN must return false. + // + if (isnan(leftValue) || isnan(rightValue)) + { + return false; + } + + // + // This method is not good enough and should be updated using DistanceBetweenSignAndMagnitudeNumbers methods + // + if (fabs(leftValue - rightValue) < DBL_EPSILON) + { + return true; + } + else + { + return false; + } } -#endif // SBG_DEFINES_H +#endif // SBG_DEFINES_H diff --git a/common/sbgErrorCodes.c b/common/sbgErrorCodes.c new file mode 100644 index 0000000..8160fa2 --- /dev/null +++ b/common/sbgErrorCodes.c @@ -0,0 +1,39 @@ +// Local headers +#include "sbgErrorCodes.h" + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +SBG_COMMON_LIB_API const char *sbgErrorCodeToString(SbgErrorCode errorCode) +{ + static const char *errorCodeToStr[] = + { + [SBG_NO_ERROR] = "SBG_NO_ERROR", + [SBG_ERROR] = "SBG_ERROR", + [SBG_NULL_POINTER] = "SBG_NULL_POINTER", + [SBG_INVALID_CRC] = "SBG_INVALID_CRC", + [SBG_INVALID_FRAME] = "SBG_INVALID_FRAME", + [SBG_TIME_OUT] = "SBG_TIME_OUT", + [SBG_WRITE_ERROR] = "SBG_WRITE_ERROR", + [SBG_READ_ERROR] = "SBG_READ_ERROR", + [SBG_BUFFER_OVERFLOW] = "SBG_BUFFER_OVERFLOW", + [SBG_INVALID_PARAMETER] = "SBG_INVALID_PARAMETER", + [SBG_NOT_READY] = "SBG_NOT_READY", + [SBG_MALLOC_FAILED] = "SBG_MALLOC_FAILED", + [SBG_CALIB_MAG_NOT_ENOUGH_POINTS] = "SBG_CALIB_MAG_NOT_ENOUGH_POINTS", + [SBG_CALIB_MAG_INVALID_TAKE] = "SBG_CALIB_MAG_INVALID_TAKE", + [SBG_CALIB_MAG_SATURATION] = "SBG_CALIB_MAG_SATURATION", + [SBG_CALIB_MAG_POINTS_NOT_IN_A_PLANE] = "SBG_CALIB_MAG_POINTS_NOT_IN_A_PLANE", + [SBG_DEVICE_NOT_FOUND] = "SBG_DEVICE_NOT_FOUND", + [SBG_OPERATION_CANCELLED] = "SBG_OPERATION_CANCELLED", + [SBG_NOT_CONTINUOUS_FRAME] = "SBG_NOT_CONTINUOUS_FRAME", + [SBG_INCOMPATIBLE_HARDWARE] = "SBG_INCOMPATIBLE_HARDWARE", + [SBG_INVALID_VERSION] = "SBG_INVALID_VERSION" + }; + + assert(errorCode >= 0); + assert(errorCode < SBG_ARRAY_SIZE(errorCodeToStr)); + + return errorCodeToStr[errorCode]; +} diff --git a/common/sbgErrorCodes.h b/common/sbgErrorCodes.h index b31d7c9..4c9e20b 100644 --- a/common/sbgErrorCodes.h +++ b/common/sbgErrorCodes.h @@ -1,13 +1,13 @@ /*! - * \file sbgErrorCodes.h - * \ingroup common - * \author SBG Systems - * \date 17 March 2015 + * \file sbgErrorCodes.h + * \ingroup common + * \author SBG Systems + * \date 17 March 2015 * - * \brief Header file that defines all error codes for SBG Systems libraries. + * \brief Header file that defines all error codes for SBG Systems libraries. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -33,6 +33,10 @@ #ifndef SBG_ERROR_CODES_H #define SBG_ERROR_CODES_H +#ifdef __cplusplus +extern "C" { +#endif + // Local headers #include "sbgDefines.h" @@ -41,79 +45,52 @@ //----------------------------------------------------------------------// /*! - * Generic errors definitions for SBG Systems projects. + * List of errors and status common to all 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_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. */ + SBG_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_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 */ + 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 -// +//- Public methods -// //----------------------------------------------------------------------// /*! - * 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. + * Returns a human readable string from an error code enum value. + * + * \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" - }; +SBG_COMMON_LIB_API const char *sbgErrorCodeToString(SbgErrorCode errorCode); - assert(errorCode < SBG_ARRAY_SIZE(sbgErrorCodeString)); - return sbgErrorCodeString[errorCode]; +#ifdef __cplusplus } +#endif -#endif /* SBG_ERROR_CODES_H */ +#endif // SBG_ERROR_CODES_H diff --git a/common/sbgTypes.h b/common/sbgTypes.h index aa76780..bfe79c6 100644 --- a/common/sbgTypes.h +++ b/common/sbgTypes.h @@ -1,15 +1,15 @@ /*! - * \file sbgTypes.h - * \ingroup common - * \author SBG Systems - * \date 17 March 2015 + * \file sbgTypes.h + * \ingroup common + * \author SBG Systems + * \date 17 March 2015 * - * \brief Header file that defines all scalar types. + * \brief Header file that defines all scalar types. * * The platform endianness should be defined here. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -44,26 +44,26 @@ //----------------------------------------------------------------------// //- Limits definitions -// //----------------------------------------------------------------------// -#define SBG_MIN_INT_24 (-8388608l) -#define SBG_MAX_INT_24 (8388607l) -#define SBG_MAX_UINT_24 (16777215ul) +#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_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_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) +#define SBG_MIN_INT_56 (-36028797018963967ll - 1) +#define SBG_MAX_INT_56 (36028797018963967ll) +#define SBG_MAX_UINT_56 (72057594037927935ull) //----------------------------------------------------------------------// //- Global typedef -// //----------------------------------------------------------------------// -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. */ +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 -// @@ -74,8 +74,8 @@ typedef uint32_t sbgIpAddress; /*!< Define an IP v4 address stored in 4 */ typedef union _SbgUint8PtrToUint32Ptr { - uint8_t *m_pointerUint8; /*!< Set the address used to access the uint32_t. */ - uint32_t *m_pointerUint32; /*!< Store the unint32 value. */ + uint8_t *m_pointerUint8; /*!< Set the address used to access the uint32_t. */ + uint32_t *m_pointerUint32; /*!< Store the unint32 value. */ } SbgUint8PtrToUint32Ptr; /*! @@ -83,8 +83,8 @@ typedef union _SbgUint8PtrToUint32Ptr */ typedef union _SbgUint8ToInt16 { - int16_t value; - uint8_t buffer[2]; + int16_t value; + uint8_t buffer[2]; } SbgUint8ToInt16; /*! @@ -92,8 +92,8 @@ typedef union _SbgUint8ToInt16 */ typedef union _SbgUint8ToUint16 { - uint16_t value; - uint8_t buffer[2]; + uint16_t value; + uint8_t buffer[2]; } SbgUint8ToUint16; /*! @@ -101,8 +101,8 @@ typedef union _SbgUint8ToUint16 */ typedef union _SbgUint8ToInt32 { - int32_t value; - uint8_t buffer[4]; + int32_t value; + uint8_t buffer[4]; } SbgUint8ToInt32; /*! @@ -110,8 +110,8 @@ typedef union _SbgUint8ToInt32 */ typedef union _SbgUint8ToUint32 { - uint32_t value; - uint8_t buffer[4]; + uint32_t value; + uint8_t buffer[4]; } SbgUint8ToUint32; /*! @@ -119,8 +119,8 @@ typedef union _SbgUint8ToUint32 */ typedef union _SbgUint8ToInt64 { - int64_t value; - uint8_t buffer[8]; + int64_t value; + uint8_t buffer[8]; } SbgUint8ToInt64; /*! @@ -128,8 +128,8 @@ typedef union _SbgUint8ToInt64 */ typedef union _SbgUint8ToUint64 { - uint64_t value; - uint8_t buffer[8]; + uint64_t value; + uint8_t buffer[8]; } SbgUint8ToUint64; /*! @@ -137,9 +137,9 @@ typedef union _SbgUint8ToUint64 */ typedef union _SbgFloatNint { - float valF; - int32_t valI; - uint32_t valU; + float valF; + int32_t valI; + uint32_t valU; } SbgFloatNint; /*! @@ -147,9 +147,9 @@ typedef union _SbgFloatNint */ typedef union _SbgDoubleNint { - double valF; - uint64_t valU; - int64_t valI; + double valF; + uint64_t valU; + int64_t valI; } SbgDoubleNint; /*! @@ -157,7 +157,7 @@ typedef union _SbgDoubleNint */ typedef struct _SbgVector3i { - int32_t v[3]; + int32_t v[3]; } SbgVector3i; /*! @@ -165,32 +165,32 @@ typedef union _SbgDoubleNint */ typedef struct _SbgVector3ll { - int64_t v[3]; + int64_t v[3]; } SbgVector3ll; //----------------------------------------------------------------------// //- DEPRECATED: types definitions -// //----------------------------------------------------------------------// #ifdef SBG_COMMON_USE_DEPRECATED - SBG_DEPRECATED_TYPEDEF(typedef unsigned char uint8); - SBG_DEPRECATED_TYPEDEF(typedef unsigned short uint16); - SBG_DEPRECATED_TYPEDEF(typedef unsigned int uint32); - SBG_DEPRECATED_TYPEDEF(typedef unsigned long long int uint64); - - SBG_DEPRECATED_TYPEDEF(typedef signed char int8); - SBG_DEPRECATED_TYPEDEF(typedef signed short int16); - SBG_DEPRECATED_TYPEDEF(typedef signed int int32); - SBG_DEPRECATED_TYPEDEF(typedef signed long long int int64); - - SBG_DEPRECATED_TYPEDEF(typedef union _SbgUint8PtrToUint32Ptr Uint8PtrToUint32Ptr); - SBG_DEPRECATED_TYPEDEF(typedef union _SbgUint8ToInt16 Uint8ToInt16); - SBG_DEPRECATED_TYPEDEF(typedef union _SbgUint8ToUint16 Uint8ToUint16); - SBG_DEPRECATED_TYPEDEF(typedef union _SbgUint8ToInt32 Uint8ToInt32); - SBG_DEPRECATED_TYPEDEF(typedef union _SbgUint8ToUint32 Uint8ToUint32); - SBG_DEPRECATED_TYPEDEF(typedef union _SbgUint8ToInt64 Uint8ToInt64); - SBG_DEPRECATED_TYPEDEF(typedef union _SbgUint8ToUint64 Uint8ToUint64); - SBG_DEPRECATED_TYPEDEF(typedef union _SbgFloatNint FloatNint); - SBG_DEPRECATED_TYPEDEF(typedef union _SbgDoubleNint DoubleNint); + SBG_DEPRECATED_TYPEDEF(typedef unsigned char uint8); + SBG_DEPRECATED_TYPEDEF(typedef unsigned short uint16); + SBG_DEPRECATED_TYPEDEF(typedef unsigned int uint32); + SBG_DEPRECATED_TYPEDEF(typedef unsigned long long int uint64); + + SBG_DEPRECATED_TYPEDEF(typedef signed char int8); + SBG_DEPRECATED_TYPEDEF(typedef signed short int16); + SBG_DEPRECATED_TYPEDEF(typedef signed int int32); + SBG_DEPRECATED_TYPEDEF(typedef signed long long int int64); + + SBG_DEPRECATED_TYPEDEF(typedef union _SbgUint8PtrToUint32Ptr Uint8PtrToUint32Ptr); + SBG_DEPRECATED_TYPEDEF(typedef union _SbgUint8ToInt16 Uint8ToInt16); + SBG_DEPRECATED_TYPEDEF(typedef union _SbgUint8ToUint16 Uint8ToUint16); + SBG_DEPRECATED_TYPEDEF(typedef union _SbgUint8ToInt32 Uint8ToInt32); + SBG_DEPRECATED_TYPEDEF(typedef union _SbgUint8ToUint32 Uint8ToUint32); + SBG_DEPRECATED_TYPEDEF(typedef union _SbgUint8ToInt64 Uint8ToInt64); + SBG_DEPRECATED_TYPEDEF(typedef union _SbgUint8ToUint64 Uint8ToUint64); + SBG_DEPRECATED_TYPEDEF(typedef union _SbgFloatNint FloatNint); + SBG_DEPRECATED_TYPEDEF(typedef union _SbgDoubleNint DoubleNint); #endif -#endif // SBG_TYPES_H +#endif // SBG_TYPES_H diff --git a/common/splitBuffer/sbgSplitBuffer.h b/common/splitBuffer/sbgSplitBuffer.h index fb32425..0d1e9eb 100644 --- a/common/splitBuffer/sbgSplitBuffer.h +++ b/common/splitBuffer/sbgSplitBuffer.h @@ -1,13 +1,13 @@ /*! - * \file sbgSplitBuffer.h - * \ingroup common - * \author SBG Systems - * \date 19 November 2013 + * \file sbgSplitBuffer.h + * \ingroup common + * \author SBG Systems + * \date 19 November 2013 * - * \brief Helper methods used to handle a splittable buffer. + * \brief Helper methods used to handle a splittable buffer. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -51,195 +51,201 @@ extern "C" { */ 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 */ + 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 -// +//- 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. + * + * \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; + // + // 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. + * + * \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; + // + // 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. + * + * \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; + // + // 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. + * + * \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; - } + // + // 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. + * + * \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; - } + // + // 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. + * + * \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; + 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; } //----------------------------------------------------------------------// @@ -249,4 +255,4 @@ SBG_INLINE size_t sbgSplitBufferGetSubBufferSize(const SbgSplitBuffer *pSplitBuf } #endif -#endif /* SBG_SPLIT_BUFFER_H */ +#endif // SBG_SPLIT_BUFFER_H diff --git a/common/streamBuffer/sbgStreamBuffer.h b/common/streamBuffer/sbgStreamBuffer.h index cebc45f..5adbd38 100644 --- a/common/streamBuffer/sbgStreamBuffer.h +++ b/common/streamBuffer/sbgStreamBuffer.h @@ -1,13 +1,13 @@ /*! - * \file sbgStreamBuffer.h - * \ingroup common - * \author SBG Systems - * \date 02 January 2013 + * \file sbgStreamBuffer.h + * \ingroup common + * \author SBG Systems + * \date 02 January 2013 * - * \brief Used to read/write data from/to a memory buffer stream. + * \brief Used to read/write data from/to a memory buffer stream. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -50,4 +50,4 @@ extern "C" { } #endif -#endif /* SBG_STREAM_BUFFER_H */ +#endif // SBG_STREAM_BUFFER_H diff --git a/common/streamBuffer/sbgStreamBufferBE.h b/common/streamBuffer/sbgStreamBufferBE.h index b78173f..37a7572 100644 --- a/common/streamBuffer/sbgStreamBufferBE.h +++ b/common/streamBuffer/sbgStreamBufferBE.h @@ -1,13 +1,13 @@ /*! - * \file sbgStreamBufferBE.h - * \ingroup common - * \author SBG Systems - * \date 17 February 2015 + * \file sbgStreamBufferBE.h + * \ingroup common + * \author SBG Systems + * \date 17 February 2015 * - * \brief Specific method of stream buffer for little endian readings/writings. + * \brief Specific method of stream buffer for little endian readings/writings. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -42,1124 +42,1124 @@ /*! * 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. + * \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]; - - 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; + int16_t bytesValues[2]; + + 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. + * \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]; - - 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; + uint16_t bytesValues[2]; + + 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. + * \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) { - SbgUint8ToInt32 value; - - 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; + SbgUint8ToInt32 value; + + 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. + * \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) { - SbgUint8ToUint32 value; - - 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; + SbgUint8ToUint32 value; + + 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. + * \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]; - - 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; + int32_t bytesValues[4]; + + 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. + * \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]; - - 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; + uint32_t bytesValues[4]; + + 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. + * \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) { - SbgUint8ToInt64 value; - - 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; + SbgUint8ToInt64 value; + + 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. + * \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) { - SbgUint8ToUint64 value; - - 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; + SbgUint8ToUint64 value; + + 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. + * \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) { - SbgUint8ToInt64 value; - - 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; + SbgUint8ToInt64 value; + + 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. + * \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) { - SbgUint8ToUint64 value; - - 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; + SbgUint8ToUint64 value; + + 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. + * \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) { - SbgUint8ToInt64 value; - - 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; + SbgUint8ToInt64 value; + + 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. + * \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) { - SbgUint8ToUint64 value; - - 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; + SbgUint8ToUint64 value; + + 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. + * \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; - - 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; + int64_t lowPart; + int64_t highPart; + + 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. + * \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; - - 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; + uint64_t lowPart; + uint64_t highPart; + + 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. + * \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) { - assert(pHandle); + assert(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); + // + // 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. + * \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; + uint64_t size; - assert(pHandle); + assert(pHandle); - // - // Just call the read method for uint64_t - // - size = sbgStreamBufferReadUint64BE(pHandle); + // + // 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))); + // + // 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; + // + // 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. + * \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) { - SbgFloatNint floatInt; - - 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; + SbgFloatNint floatInt; + + 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. + * \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) { - SbgDoubleNint doubleInt; - - 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; + SbgDoubleNint doubleInt; + + 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; } //----------------------------------------------------------------------// @@ -1169,756 +1169,756 @@ SBG_INLINE double sbgStreamBufferReadDoubleBE(SbgStreamBuffer *pHandle) /*! * 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. + * \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) { - 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; + 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. + * \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) { - 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; + 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. + * \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) { - 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; + 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. + * \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) { - 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; + 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. + * \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) { - 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; + 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. + * \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) { - 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; + 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 uint48 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. + * \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 sbgStreamBufferWriteUint48BE(SbgStreamBuffer *pHandle, uint64_t value) { - assert(pHandle); - assert(value < ((uint64_t)1 << 48)); - - // - // 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)) - { - // - // 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); - #else - *(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 - } - else - { - // - // We are accessing a data that is outside the stream buffer - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - return pHandle->errorCode; + assert(pHandle); + assert(value < ((uint64_t)1 << 48)); + + // + // 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)) + { + // + // 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); + #else + *(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 + } + 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. + * \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) { - 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; + 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. + * \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) { - 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; + 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. + * \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) { - assert(pHandle); + assert(pHandle); - // - // Make sure the provided size_t value doesn't exceed a uint32_t storage - // - assert(value <= UINT32_MAX); + // + // 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); + // + // 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. + * \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); + // + // 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. + * \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) { - SbgFloatNint floatInt; - - 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; + SbgFloatNint floatInt; + + 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. + * \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) { - SbgDoubleNint doubleInt; - - 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; + SbgDoubleNint doubleInt; + + 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 + * \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; - - 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 %zu bytes into a buffer of %zu bytes.", stringLength, maxSize); - } - } - - return pHandle->errorCode; + size_t stringLength; + + 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 %zu bytes into a buffer of %zu 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. + * \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; - - 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; + size_t stringLength; + + 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 */ +#endif // SBG_STREAM_BUFFER_BE_H diff --git a/common/streamBuffer/sbgStreamBufferCommon.h b/common/streamBuffer/sbgStreamBufferCommon.h index e6c8af4..fb1fb5a 100644 --- a/common/streamBuffer/sbgStreamBufferCommon.h +++ b/common/streamBuffer/sbgStreamBufferCommon.h @@ -1,14 +1,14 @@ /*! - * \file sbgStreamBufferCommon.h - * \ingroup common - * \author SBG Systems - * \date 02 January 2013 + * \file sbgStreamBufferCommon.h + * \ingroup common + * \author SBG Systems + * \date 02 January 2013 * - * \brief Used to read/write data from/to a memory buffer stream. + * \brief Used to read/write data from/to a memory buffer stream. + * + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license - * * 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 @@ -26,7 +26,7 @@ * 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. - * + * * \endlicense */ @@ -36,139 +36,139 @@ #include //----------------------------------------------------------------------// -//- General definitions -// +//- General definitions -// //----------------------------------------------------------------------// /*! - * The default method should read and write using the platform endianness + * 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 + /*! + * 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 sbgStreamBufferReadUint24 sbgStreamBufferReadUint24BE + #define sbgStreamBufferReadInt24 sbgStreamBufferReadInt24BE - #define sbgStreamBufferReadUint32 sbgStreamBufferReadUint32BE - #define sbgStreamBufferReadInt32 sbgStreamBufferReadInt32BE + #define sbgStreamBufferReadUint32 sbgStreamBufferReadUint32BE + #define sbgStreamBufferReadInt32 sbgStreamBufferReadInt32BE - #define sbgStreamBufferReadUint40 sbgStreamBufferReadUint40BE - #define sbgStreamBufferReadInt40 sbgStreamBufferReadInt40BE + #define sbgStreamBufferReadUint40 sbgStreamBufferReadUint40BE + #define sbgStreamBufferReadInt40 sbgStreamBufferReadInt40BE - #define sbgStreamBufferReadUint48 sbgStreamBufferReadUint48BE - #define sbgStreamBufferReadInt48 sbgStreamBufferReadInt48BE + #define sbgStreamBufferReadUint48 sbgStreamBufferReadUint48BE + #define sbgStreamBufferReadInt48 sbgStreamBufferReadInt48BE - #define sbgStreamBufferReadUint56 sbgStreamBufferReadUint56BE - #define sbgStreamBufferReadInt56 sbgStreamBufferReadInt56BE + #define sbgStreamBufferReadUint56 sbgStreamBufferReadUint56BE + #define sbgStreamBufferReadInt56 sbgStreamBufferReadInt56BE - #define sbgStreamBufferReadUint64 sbgStreamBufferReadUint64BE - #define sbgStreamBufferReadInt64 sbgStreamBufferReadInt64BE + #define sbgStreamBufferReadUint64 sbgStreamBufferReadUint64BE + #define sbgStreamBufferReadInt64 sbgStreamBufferReadInt64BE - #define sbgStreamBufferReadSizeT32 sbgStreamBufferReadSizeT32BE - #define sbgStreamBufferReadSizeT64 sbgStreamBufferReadSizeT64BE + #define sbgStreamBufferReadSizeT32 sbgStreamBufferReadSizeT32BE + #define sbgStreamBufferReadSizeT64 sbgStreamBufferReadSizeT64BE - #define sbgStreamBufferReadFloat sbgStreamBufferReadFloatBE - #define sbgStreamBufferReadDouble sbgStreamBufferReadDoubleBE + #define sbgStreamBufferReadFloat sbgStreamBufferReadFloatBE + #define sbgStreamBufferReadDouble sbgStreamBufferReadDoubleBE - #define sbgStreamBufferWriteUint16 sbgStreamBufferWriteUint16BE - #define sbgStreamBufferWriteInt16 sbgStreamBufferWriteInt16BE + #define sbgStreamBufferWriteUint16 sbgStreamBufferWriteUint16BE + #define sbgStreamBufferWriteInt16 sbgStreamBufferWriteInt16BE - #define sbgStreamBufferWriteUint24 sbgStreamBufferWriteUint24BE - #define sbgStreamBufferWriteInt24 sbgStreamBufferWriteInt24BE + #define sbgStreamBufferWriteUint24 sbgStreamBufferWriteUint24BE + #define sbgStreamBufferWriteInt24 sbgStreamBufferWriteInt24BE - #define sbgStreamBufferWriteUint32 sbgStreamBufferWriteUint32BE - #define sbgStreamBufferWriteInt32 sbgStreamBufferWriteInt32BE + #define sbgStreamBufferWriteUint32 sbgStreamBufferWriteUint32BE + #define sbgStreamBufferWriteInt32 sbgStreamBufferWriteInt32BE - #define sbgStreamBufferWriteUint64 sbgStreamBufferWriteUint64BE - #define sbgStreamBufferWriteInt64 sbgStreamBufferWriteInt64BE + #define sbgStreamBufferWriteUint64 sbgStreamBufferWriteUint64BE + #define sbgStreamBufferWriteInt64 sbgStreamBufferWriteInt64BE - #define sbgStreamBufferWriteSizeT32 sbgStreamBufferWriteSizeT32BE - #define sbgStreamBufferWriteSizeT64 sbgStreamBufferWriteSizeT64BE + #define sbgStreamBufferWriteSizeT32 sbgStreamBufferWriteSizeT32BE + #define sbgStreamBufferWriteSizeT64 sbgStreamBufferWriteSizeT64BE - #define sbgStreamBufferWriteFloat sbgStreamBufferWriteFloatBE - #define sbgStreamBufferWriteDouble sbgStreamBufferWriteDoubleBE + #define sbgStreamBufferWriteFloat sbgStreamBufferWriteFloatBE + #define sbgStreamBufferWriteDouble sbgStreamBufferWriteDoubleBE - #define sbgStreamBufferReadString sbgStreamBufferReadStringBE - #define sbgStreamBufferWriteString sbgStreamBufferWriteStringBE + #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 + /*! + * 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 sbgStreamBufferReadUint24 sbgStreamBufferReadUint24LE + #define sbgStreamBufferReadInt24 sbgStreamBufferReadInt24LE - #define sbgStreamBufferReadUint32 sbgStreamBufferReadUint32LE - #define sbgStreamBufferReadInt32 sbgStreamBufferReadInt32LE + #define sbgStreamBufferReadUint32 sbgStreamBufferReadUint32LE + #define sbgStreamBufferReadInt32 sbgStreamBufferReadInt32LE - #define sbgStreamBufferReadUint40 sbgStreamBufferReadUint40LE - #define sbgStreamBufferReadInt40 sbgStreamBufferReadInt40LE + #define sbgStreamBufferReadUint40 sbgStreamBufferReadUint40LE + #define sbgStreamBufferReadInt40 sbgStreamBufferReadInt40LE - #define sbgStreamBufferReadUint48 sbgStreamBufferReadUint48LE - #define sbgStreamBufferReadInt48 sbgStreamBufferReadInt48LE + #define sbgStreamBufferReadUint48 sbgStreamBufferReadUint48LE + #define sbgStreamBufferReadInt48 sbgStreamBufferReadInt48LE - #define sbgStreamBufferReadUint56 sbgStreamBufferReadUint56LE - #define sbgStreamBufferReadInt56 sbgStreamBufferReadInt56LE + #define sbgStreamBufferReadUint56 sbgStreamBufferReadUint56LE + #define sbgStreamBufferReadInt56 sbgStreamBufferReadInt56LE - #define sbgStreamBufferReadUint64 sbgStreamBufferReadUint64LE - #define sbgStreamBufferReadInt64 sbgStreamBufferReadInt64LE + #define sbgStreamBufferReadUint64 sbgStreamBufferReadUint64LE + #define sbgStreamBufferReadInt64 sbgStreamBufferReadInt64LE - #define sbgStreamBufferReadSizeT32 sbgStreamBufferReadSizeT32LE - #define sbgStreamBufferReadSizeT64 sbgStreamBufferReadSizeT64LE + #define sbgStreamBufferReadSizeT32 sbgStreamBufferReadSizeT32LE + #define sbgStreamBufferReadSizeT64 sbgStreamBufferReadSizeT64LE - #define sbgStreamBufferReadFloat sbgStreamBufferReadFloatLE - #define sbgStreamBufferReadDouble sbgStreamBufferReadDoubleLE + #define sbgStreamBufferReadFloat sbgStreamBufferReadFloatLE + #define sbgStreamBufferReadDouble sbgStreamBufferReadDoubleLE - #define sbgStreamBufferWriteUint16 sbgStreamBufferWriteUint16LE - #define sbgStreamBufferWriteInt16 sbgStreamBufferWriteInt16LE + #define sbgStreamBufferWriteUint16 sbgStreamBufferWriteUint16LE + #define sbgStreamBufferWriteInt16 sbgStreamBufferWriteInt16LE - #define sbgStreamBufferWriteUint24 sbgStreamBufferWriteUint24LE - #define sbgStreamBufferWriteInt24 sbgStreamBufferWriteInt24LE + #define sbgStreamBufferWriteUint24 sbgStreamBufferWriteUint24LE + #define sbgStreamBufferWriteInt24 sbgStreamBufferWriteInt24LE - #define sbgStreamBufferWriteUint32 sbgStreamBufferWriteUint32LE - #define sbgStreamBufferWriteInt32 sbgStreamBufferWriteInt32LE + #define sbgStreamBufferWriteUint32 sbgStreamBufferWriteUint32LE + #define sbgStreamBufferWriteInt32 sbgStreamBufferWriteInt32LE - #define sbgStreamBufferWriteUint64 sbgStreamBufferWriteUint64LE - #define sbgStreamBufferWriteInt64 sbgStreamBufferWriteInt64LE + #define sbgStreamBufferWriteUint64 sbgStreamBufferWriteUint64LE + #define sbgStreamBufferWriteInt64 sbgStreamBufferWriteInt64LE - #define sbgStreamBufferWriteSizeT32 sbgStreamBufferWriteSizeT32LE - #define sbgStreamBufferWriteSizeT64 sbgStreamBufferWriteSizeT64LE + #define sbgStreamBufferWriteSizeT32 sbgStreamBufferWriteSizeT32LE + #define sbgStreamBufferWriteSizeT64 sbgStreamBufferWriteSizeT64LE - #define sbgStreamBufferWriteFloat sbgStreamBufferWriteFloatLE - #define sbgStreamBufferWriteDouble sbgStreamBufferWriteDoubleLE + #define sbgStreamBufferWriteFloat sbgStreamBufferWriteFloatLE + #define sbgStreamBufferWriteDouble sbgStreamBufferWriteDoubleLE - #define sbgStreamBufferReadString sbgStreamBufferReadStringLE - #define sbgStreamBufferWriteString sbgStreamBufferWriteStringLE + #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 +#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 -// @@ -179,8 +179,8 @@ */ typedef enum _SbgSBMode { - SB_MODE_READ, /*!< This stream buffer can perform read operations. */ - SB_MODE_WRITE /*!< This stream buffer can perform write operations. */ + SB_MODE_READ, /*!< This stream buffer can perform read operations. */ + SB_MODE_WRITE /*!< This stream buffer can perform write operations. */ } SbgSBMode; /*! @@ -188,10 +188,10 @@ typedef enum _SbgSBMode */ 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. */ + 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; /*! @@ -199,11 +199,11 @@ typedef enum _SbgSBSeekOrigin */ 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. */ + 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; //----------------------------------------------------------------------// @@ -213,96 +213,96 @@ typedef struct _SbgStreamBuffer /*! * 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. + * \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) { - 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; + 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. + * \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) { - 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; + 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 + * \param[in] pHandle Pointer to a valid Stream Buffer handle + * \return Last stream buffer error code */ SBG_INLINE SbgErrorCode sbgStreamBufferGetLastError(const SbgStreamBuffer *pHandle) { - assert(pHandle); + assert(pHandle); - // - // Return error code - // - return pHandle->errorCode; + // + // 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 + * \param[in] pHandle Pointer to a valid Stream Buffer handle */ SBG_INLINE void sbgStreamBufferClearLastError(SbgStreamBuffer *pHandle) { - assert(pHandle); + assert(pHandle); - // - // Return error code - // - pHandle->errorCode = SBG_NO_ERROR; + // + // Return error code + // + pHandle->errorCode = SBG_NO_ERROR; } /*! @@ -312,17 +312,17 @@ SBG_INLINE void sbgStreamBufferClearLastError(SbgStreamBuffer *pHandle) * 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. + * \param[in] pHandle Valid handle on a stream buffer. + * \return The allocated size of the linked buffer in bytes. */ SBG_INLINE size_t sbgStreamBufferGetSize(const SbgStreamBuffer *pHandle) { - assert(pHandle); + assert(pHandle); - // - // Return the linked buffer size - // - return pHandle->bufferSize; + // + // Return the linked buffer size + // + return pHandle->bufferSize; } /*! @@ -332,17 +332,17 @@ SBG_INLINE size_t sbgStreamBufferGetSize(const SbgStreamBuffer *pHandle) * 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. + * \param[in] pHandle Valid handle on a stream buffer. + * \return The current cursor position in bytes. */ SBG_INLINE size_t sbgStreamBufferGetLength(const SbgStreamBuffer *pHandle) { - assert(pHandle); + 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); + // + // Return the number of bytes between the begin of the stream and the current pointer + // + return ((size_t)pHandle->pCurrentPtr - (size_t)pHandle->pBufferPtr); } /*! @@ -351,135 +351,125 @@ SBG_INLINE size_t sbgStreamBufferGetLength(const SbgStreamBuffer *pHandle) * 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. + * \param[in] pHandle Valid handle on a stream buffer. + * \return The space available in this stream buffer in bytes. */ SBG_INLINE size_t sbgStreamBufferGetSpace(const SbgStreamBuffer *pHandle) { - assert(pHandle); + assert(pHandle); - // - // Return the space left in bytes - // - return sbgStreamBufferGetSize(pHandle) - sbgStreamBufferGetLength(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. + * \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) { - 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; + 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; + } + 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; + } + } + } + + 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. + * \param[in] pHandle Valid handle on a stream buffer. + * \return Current offset in bytes from the beginning. */ SBG_INLINE size_t sbgStreamBufferTell(const SbgStreamBuffer *pHandle) { - assert(pHandle); + assert(pHandle); - return (size_t)pHandle->pCurrentPtr - (size_t)pHandle->pBufferPtr; + 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. + * \param[in] pHandle Valid handle on a stream buffer. + * \return Pointer on the begining of the internal buffer. */ SBG_INLINE void *sbgStreamBufferGetLinkedBuffer(const SbgStreamBuffer *pHandle) { - assert(pHandle); + assert(pHandle); - return pHandle->pBufferPtr; + return pHandle->pBufferPtr; } /*! - * Returns a pointer on the internal buffer at the current cursor. + * 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. + * \param[in] pHandle Valid handle on a stream buffer. + * \return Pointer on the current cursor of the internal buffer. */ SBG_INLINE void *sbgStreamBufferGetCursor(const SbgStreamBuffer *pHandle) { - assert(pHandle); + assert(pHandle); - return pHandle->pCurrentPtr; + return pHandle->pCurrentPtr; } //----------------------------------------------------------------------// @@ -489,173 +479,173 @@ SBG_INLINE void *sbgStreamBufferGetCursor(const SbgStreamBuffer *pHandle) /*! * 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. + * \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) { - 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; + 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. + * \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) { - 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; + 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. + * \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) { - 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; + 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. + * \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) { - 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; + 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; } //----------------------------------------------------------------------// @@ -665,167 +655,167 @@ SBG_INLINE SbgErrorCode sbgStreamBufferReadBuffer(SbgStreamBuffer *pHandle, void /*! * 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. + * \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) { - 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; + 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. + * \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) { - 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; + 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. + * \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) { - 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; + 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] numBytesToWrite Number of bytes to write to the stream buffer. - * \return SBG_NO_ERROR if the data has been written. + * \param[in] pHandle Valid stream buffer handle that supports write operations. + * \param[out] pBuffer Buffer to write into the stream buffer. + * \param[in] numBytesToWrite 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) { - 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; + 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/common/streamBuffer/sbgStreamBufferLE.h b/common/streamBuffer/sbgStreamBufferLE.h index 2a43ea9..b4cd409 100644 --- a/common/streamBuffer/sbgStreamBufferLE.h +++ b/common/streamBuffer/sbgStreamBufferLE.h @@ -1,13 +1,13 @@ /*! - * \file sbgStreamBufferLE.h - * \ingroup common - * \author SBG Systems - * \date 17 February 2015 + * \file sbgStreamBufferLE.h + * \ingroup common + * \author SBG Systems + * \date 17 February 2015 * - * \brief Specific method of stream buffer for little endian readings/writings. + * \brief Specific method of stream buffer for little endian readings/writings. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -42,1124 +42,1124 @@ /*! * 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. + * \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]; - - 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; + int16_t bytesValues[2]; + + 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. + * \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]; - - 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; + uint16_t bytesValues[2]; + + 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. + * \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) { - SbgUint8ToInt32 value; - - 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; + SbgUint8ToInt32 value; + + 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. + * \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) { - SbgUint8ToUint32 value; - - 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; + SbgUint8ToUint32 value; + + 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. + * \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]; - - 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; + int32_t bytesValues[4]; + + 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. + * \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]; - - 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; + uint32_t bytesValues[4]; + + 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. + * \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) { - SbgUint8ToInt64 value; - - 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; + SbgUint8ToInt64 value; + + 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. + * \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) { - SbgUint8ToUint64 value; - - 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; + SbgUint8ToUint64 value; + + 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. + * \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) { - SbgUint8ToInt64 value; - - 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; + SbgUint8ToInt64 value; + + 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. + * \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) { - SbgUint8ToUint64 value; - - 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; + SbgUint8ToUint64 value; + + 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. + * \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) { - SbgUint8ToInt64 value; - - 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; + SbgUint8ToInt64 value; + + 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. + * \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) { - SbgUint8ToUint64 value; - - 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; + SbgUint8ToUint64 value; + + 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. + * \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; - - 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; + int64_t lowPart; + int64_t highPart; + + 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. + * \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; - - 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; + uint64_t lowPart; + uint64_t highPart; + + 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. + * \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) { - assert(pHandle); + assert(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); + // + // 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. + * \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; + uint64_t size; - assert(pHandle); + assert(pHandle); - // - // Just call the read method for uint64_t - // - size = sbgStreamBufferReadUint64LE(pHandle); + // + // 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))); + // + // 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; + // + // 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. + * \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) { - SbgFloatNint floatInt; - - 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; + SbgFloatNint floatInt; + + 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. + * \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) { - SbgDoubleNint doubleInt; - - 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; + SbgDoubleNint doubleInt; + + 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; } //----------------------------------------------------------------------// @@ -1169,754 +1169,754 @@ SBG_INLINE double sbgStreamBufferReadDoubleLE(SbgStreamBuffer *pHandle) /*! * 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. + * \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) { - 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; + 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. + * \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) { - 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; + 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. + * \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) { - 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; + 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. + * \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) { - 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; + 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. + * \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) { - 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; + 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. + * \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) { - 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; + 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 uint48 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. + * \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 sbgStreamBufferWriteUint48LE(SbgStreamBuffer *pHandle, uint64_t value) { - assert(pHandle); - assert(value < ((uint64_t)1 << 48)); - - // - // 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)) - { - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - *(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); - #endif - } - else - { - // - // We are accessing a data that is outside the stream buffer - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - return pHandle->errorCode; + assert(pHandle); + assert(value < ((uint64_t)1 << 48)); + + // + // 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)) + { + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + *(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); + #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. + * \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) { - 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; + 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. + * \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) { - 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; + 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. + * \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) { - assert(pHandle); + assert(pHandle); - // - // Make sure the provided size_t value doesn't exceed a uint32_t storage - // - assert(value <= UINT32_MAX); + // + // 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); + // + // 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. + * \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) { - assert(pHandle); + assert(pHandle); - // - // Call the write method to store a uint64_t - // - return sbgStreamBufferWriteUint64LE(pHandle, (uint64_t)value); + // + // 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. + * \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) { - SbgFloatNint floatInt; - - 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; + SbgFloatNint floatInt; + + 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. + * \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) { - SbgDoubleNint doubleInt; - - 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; + SbgDoubleNint doubleInt; + + 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 +* \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; - - 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 %zu bytes into a buffer of %zu bytes.", stringLength, maxSize); - } - } - - return pHandle->errorCode; + size_t stringLength; + + 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 %zu bytes into a buffer of %zu 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. +* \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; - - 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; + size_t stringLength; + + 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 */ +#endif // SBG_STREAM_BUFFER_LE_H diff --git a/common/string/sbgString.c b/common/string/sbgString.c index b411b3d..fba8c7a 100644 --- a/common/string/sbgString.c +++ b/common/string/sbgString.c @@ -12,37 +12,58 @@ // Private functions // //----------------------------------------------------------------------// +/*! + * Reset the state of a string. + * + * The content of the string is reset to the empty string, using its internal + * buffer for storage. This function doesn't check if the string buffer refers + * to dynamically allocated memory, and it is therefore the responsibility of + * the caller to handle that memory before calling this function. + * + * \param[in] pString String. + */ +static void sbgStringReset(SbgString *pString) +{ + assert(pString); + + pString->internalBuffer[0] = '\0'; + + pString->pBuffer = pString->internalBuffer; + pString->capacity = sizeof(pString->internalBuffer); + pString->length = 0; +} + /*! * Count the number of leading zeros in an unsigned integer. * - * \param[in] value Integer value, must not be zero. - * \return Number of leading zeros. + * \param[in] value Integer value, must not be zero. + * \return Number of leading zeros. */ static size_t sbgStringCountLeadingZeros(size_t value) { - size_t result; + size_t result; - assert(value != 0); + assert(value != 0); #if defined(__GNUC__) || defined(__clang__) - result = __builtin_clzl((unsigned long)value); + result = __builtin_clzl((unsigned long)value); #else - size_t count; - size_t tmp; + size_t count; + size_t tmp; - tmp = value; - count = 0; + tmp = value; + count = 0; - while (tmp != 0) - { - tmp >>= 1; - count++; - } + while (tmp != 0) + { + tmp >>= 1; + count++; + } - result = (sizeof(size_t) * 8) - count; + result = (sizeof(size_t) * 8) - count; #endif // defined(__GNUC__) || defined(__clang__) - return result; + return result; } /*! @@ -52,41 +73,47 @@ static size_t sbgStringCountLeadingZeros(size_t value) * * The size must not be zero. * - * \param[in] pString String. - * \param[in] size Size, in bytes. - * \param[out] pCapacity Capacity, in bytes. - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \param[in] size Size, in bytes. + * \param[out] pCapacity Capacity, in bytes. + * \return SBG_NO_ERROR if successful. */ static SbgErrorCode sbgStringComputeCapacity(const SbgString *pString, size_t size, size_t *pCapacity) { - SbgErrorCode errorCode; + SbgErrorCode errorCode; + size_t nrLeadingZeros; + + assert(pString); - assert(pString); + SBG_UNUSED_PARAMETER(pString); - SBG_UNUSED_PARAMETER(pString); + // + // The computed capacity is the power-of-two immediately strictly greater than the size. + // In other words, it's the value with a single bit set where the index of that bit is one + // more than the MSB bit of the size. + // + // If the size already requires all bits of the size_t type to be encoded, then there's no + // extra bit available to encode the capacity. + // + // Values that would require more than half of the address space are very likely a bug, + // so just refuse them. + // - // - // The computed capacity is the power-of-two equal to or immediately greater than the size. - // In other words, it's the value with a single bit set where the index of that bit is one - // more than the MSB bit of the size, unless the size is already a power-of-two. - // - // If the size already requires all bits of the size_t type to be encoded, then there's no - // extra bit available to encode the capacity. - // + nrLeadingZeros = sbgStringCountLeadingZeros(size); - if (size <= ((size_t)1 << ((sizeof(size_t) * 8) - 1))) - { - *pCapacity = (size_t)1 << ((sizeof(size_t) * 8) - sbgStringCountLeadingZeros(size)); + if (nrLeadingZeros != 0) + { + *pCapacity = (size_t)1 << ((sizeof(size_t) * 8) - sbgStringCountLeadingZeros(size)); - errorCode = SBG_NO_ERROR; - } - else - { - errorCode = SBG_INVALID_PARAMETER; - SBG_LOG_ERROR(errorCode, "unable to compute capacity"); - } + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_INVALID_PARAMETER; + SBG_LOG_ERROR(errorCode, "unable to compute capacity"); + } - return errorCode; + return errorCode; } /*! @@ -99,169 +126,218 @@ static SbgErrorCode sbgStringComputeCapacity(const SbgString *pString, size_t si * If an allocation error occurs, and the new size is smaller than the current size, the * operation is considered successful. * - * \param[in] pString String. - * \param[in] size Required size, in bytes. - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \param[in] size Required size, in bytes. + * \return SBG_NO_ERROR if successful. */ static SbgErrorCode sbgStringResizeBuffer(SbgString *pString, size_t size) { - SbgErrorCode errorCode; - - assert(pString); - assert(!pString->readOnly); - - if (pString->isStatic) - { - if (size <= pString->capacity) - { - errorCode = SBG_NO_ERROR; - } - else - { - errorCode = SBG_BUFFER_OVERFLOW; - SBG_LOG_ERROR(errorCode, "insufficient static space for string: requested %zu, capacity %zu", size, pString->capacity); - } - } - else - { - size_t capacity; - - // - // XXX If a user performs frequent operations on a string that change its size - // below and above the size of the internal buffer, the performance impact of - // copying the content to and from the internal buffer would cause hysteresis. - // - // There is no special handling of this situation as it is assumed that the - // internal buffer is small enough that the cost of the copy operation is - // negligible. - // - - errorCode = sbgStringComputeCapacity(pString, size, &capacity); - - if (errorCode == SBG_NO_ERROR) - { - if (capacity != pString->capacity) - { - if (capacity <= sizeof(pString->internalBuffer)) - { - if (pString->pBuffer != pString->internalBuffer) - { - memcpy(pString->internalBuffer, pString->pBuffer, capacity); - free(pString->pBuffer); - - pString->pBuffer = pString->internalBuffer; - pString->capacity = sizeof(pString->internalBuffer); - } - } - else - { - if (pString->pBuffer == pString->internalBuffer) - { - char *pBuffer; - - pBuffer = malloc(capacity); - - if (pBuffer) - { - memcpy(pBuffer, pString->internalBuffer, pString->capacity); - pString->pBuffer = pBuffer; - pString->capacity = capacity; - } - else - { - errorCode = SBG_MALLOC_FAILED; - SBG_LOG_ERROR(errorCode, "unable to allocate buffer"); - } - } - else - { - char *pBuffer; - - pBuffer = realloc(pString->pBuffer, capacity); - - if (pBuffer) - { - pString->pBuffer = pBuffer; - pString->capacity = capacity; - } - else - { - if (capacity < pString->capacity) - { - errorCode = SBG_NO_ERROR; - SBG_LOG_WARNING(SBG_MALLOC_FAILED, "unable to reallocate buffer"); - } - else - { - errorCode = SBG_MALLOC_FAILED; - SBG_LOG_ERROR(errorCode, "unable to reallocate buffer"); - } - } - } - } - } - } - } - - return errorCode; + SbgErrorCode errorCode; + + assert(pString); + assert(!pString->readOnly); + + if (pString->isStatic) + { + if (size <= pString->capacity) + { + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_BUFFER_OVERFLOW; + SBG_LOG_ERROR(errorCode, "insufficient static space for string: requested %zu, capacity %zu", size, pString->capacity); + } + } + else + { + size_t capacity; + + // + // XXX If a user performs frequent operations on a string that change its size + // below and above the size of the internal buffer, the performance impact of + // copying the content to and from the internal buffer would cause hysteresis. + // + // There is no special handling of this situation as it is assumed that the + // internal buffer is small enough that the cost of the copy operation is + // negligible. + // + + errorCode = sbgStringComputeCapacity(pString, size, &capacity); + + if (errorCode == SBG_NO_ERROR) + { + if (capacity != pString->capacity) + { + if (capacity <= sizeof(pString->internalBuffer)) + { + if (pString->pBuffer != pString->internalBuffer) + { + memcpy(pString->internalBuffer, pString->pBuffer, capacity); + free(pString->pBuffer); + + pString->pBuffer = pString->internalBuffer; + pString->capacity = sizeof(pString->internalBuffer); + } + } + else + { + if (pString->pBuffer == pString->internalBuffer) + { + char *pBuffer; + + pBuffer = malloc(capacity); + + if (pBuffer) + { + memcpy(pBuffer, pString->internalBuffer, pString->capacity); + pString->pBuffer = pBuffer; + pString->capacity = capacity; + } + else + { + errorCode = SBG_MALLOC_FAILED; + SBG_LOG_ERROR(errorCode, "unable to allocate buffer"); + } + } + else + { + char *pBuffer; + + pBuffer = realloc(pString->pBuffer, capacity); + + if (pBuffer) + { + pString->pBuffer = pBuffer; + pString->capacity = capacity; + } + else + { + if (capacity < pString->capacity) + { + errorCode = SBG_NO_ERROR; + SBG_LOG_WARNING(SBG_MALLOC_FAILED, "unable to reallocate buffer"); + } + else + { + errorCode = SBG_MALLOC_FAILED; + SBG_LOG_ERROR(errorCode, "unable to reallocate buffer"); + } + } + } + } + } + } + } + + return errorCode; } /*! * Get a pointer to a character in a string. * - * \param[in] pString String. - * \param[in] index Index. - * \return Pointer to the character denoted by the index. + * \param[in] pString String. + * \param[in] index Index. + * \return Pointer to the character denoted by the index. */ static char * sbgStringGetCharPtr(const SbgString *pString, size_t index) { - assert(pString); - assert(index < pString->length); + assert(pString); + assert(index < pString->length); - return &pString->pBuffer[index]; + return &pString->pBuffer[index]; } /*! * Check if the start and end indexes of a substring are valid. * - * \param[in] pString String. - * \param[in] startIndex Start index of the substring. - * \param[in] endIndex End index of the substring. - * \return True if these indexes are valid. + * \param[in] pString String. + * \param[in] startIndex Start index of the substring. + * \param[in] endIndex End index of the substring. + * \return True if these indexes are valid. */ static bool sbgStringIndexesAreValid(const SbgString *pString, size_t startIndex, size_t endIndex) { - assert(pString); + assert(pString); - return ((startIndex <= endIndex) && (endIndex <= (pString->length))); + return ((startIndex <= endIndex) && (endIndex <= (pString->length))); } /*! * Assign a buffer of characters to a string. * - * \param[in] pString String. - * \param[in] pBuffer Buffer. - * \param[in] length Length of the buffer, in bytes. - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \param[in] pBuffer Buffer. + * \param[in] length Length of the buffer, in bytes. + * \return SBG_NO_ERROR if successful. */ static SbgErrorCode sbgStringAssignBuffer(SbgString *pString, const char *pBuffer, size_t length) { - assert(pString); - assert(pBuffer); + assert(pString); + assert(pBuffer); - if (pString->errorCode == SBG_NO_ERROR) - { - pString->errorCode = sbgStringResizeBuffer(pString, length + 1); + if (pString->errorCode == SBG_NO_ERROR) + { + pString->errorCode = sbgStringResizeBuffer(pString, length + 1); - if (pString->errorCode == SBG_NO_ERROR) - { - memcpy(pString->pBuffer, pBuffer, length) ; - pString->pBuffer[length] = '\0'; - pString->length = length; - } - } + if (pString->errorCode == SBG_NO_ERROR) + { + memcpy(pString->pBuffer, pBuffer, length) ; + pString->pBuffer[length] = '\0'; + pString->length = length; + } + } - return pString->errorCode; + return pString->errorCode; +} + +/*! + * Move a buffer of characters to a string. + * + * If moving is impossible, this function performs a copy. + * + * The buffer must have been allocated using a standard C allocation function, + * such as malloc or strdup. + * + * On return, if successful, the buffer is owned by the string. + * + * \param[in] pString String. + * \param[in] pBuffer Buffer. + * \param[in] length Length of the buffer, in bytes. + * \return SBG_NO_ERROR if successful. + */ +static SbgErrorCode sbgStringMoveBuffer(SbgString *pString, char *pBuffer, size_t length) +{ + assert(pString); + assert(pBuffer); + + if (pString->errorCode == SBG_NO_ERROR) + { + if (pString->isStatic) + { + SbgErrorCode errorCode; + + errorCode = sbgStringAssignBuffer(pString, pBuffer, length); + + if (errorCode == SBG_NO_ERROR) + { + free(pBuffer); + } + } + else + { + if (pString->pBuffer != pString->internalBuffer) + { + free(pString->pBuffer); + } + + pString->pBuffer = pBuffer; + pString->capacity = length + 1; + pString->length = length; + } + } + + return pString->errorCode; } /*! @@ -269,48 +345,48 @@ static SbgErrorCode sbgStringAssignBuffer(SbgString *pString, const char *pBuffe * * This function doesn't resize the string buffer. * - * \param[in] pString String. + * \param[in] pString String. */ static bool sbgStringTrimLeftCommon(SbgString *pString) { - size_t index; - bool trimmed; + size_t index; + bool trimmed; - assert(pString); - assert(!pString->readOnly); + assert(pString); + assert(!pString->readOnly); - index = pString->length; + index = pString->length; - for (size_t i = 0; i < pString->length; i++) - { - char *pCharPtr; + for (size_t i = 0; i < pString->length; i++) + { + char *pCharPtr; - pCharPtr = sbgStringGetCharPtr(pString, i); + pCharPtr = sbgStringGetCharPtr(pString, i); - if (!isspace((unsigned char)*pCharPtr)) - { - index = i; - break; - } - } + if (!isspace((unsigned char)*pCharPtr)) + { + index = i; + break; + } + } - if (index != 0) - { - size_t length; + if (index != 0) + { + size_t length; - length = pString->length - index; + length = pString->length - index; - memmove(pString->pBuffer, &pString->pBuffer[index], length + 1); - pString->length = length; + memmove(pString->pBuffer, &pString->pBuffer[index], length + 1); + pString->length = length; - trimmed = true; - } - else - { - trimmed = false; - } + trimmed = true; + } + else + { + trimmed = false; + } - return trimmed; + return trimmed; } /*! @@ -318,95 +394,95 @@ static bool sbgStringTrimLeftCommon(SbgString *pString) * * This function doesn't resize the string buffer. * - * \param[in] pString String. + * \param[in] pString String. */ static bool sbgStringTrimRightCommon(SbgString *pString) { - size_t index; - bool trimmed; + size_t index; + bool trimmed; - assert(pString); - assert(!pString->readOnly); + assert(pString); + assert(!pString->readOnly); - index = pString->length; + index = pString->length; - for (size_t i = 0; i < pString->length; i++) - { - char *pCharPtr; + for (size_t i = 0; i < pString->length; i++) + { + char *pCharPtr; - pCharPtr = sbgStringGetCharPtr(pString, pString->length - i - 1); + pCharPtr = sbgStringGetCharPtr(pString, pString->length - i - 1); - if (!isspace((unsigned char)*pCharPtr)) - { - index = i; - break; - } - } + if (!isspace((unsigned char)*pCharPtr)) + { + index = i; + break; + } + } - if (index != 0) - { - size_t length; + if (index != 0) + { + size_t length; - length = pString->length - index; + length = pString->length - index; - pString->pBuffer[length] = '\0'; - pString->length = length; + pString->pBuffer[length] = '\0'; + pString->length = length; - trimmed = true; - } - else - { - trimmed = false; - } + trimmed = true; + } + else + { + trimmed = false; + } - return trimmed; + return trimmed; } /*! * Compare a string to a buffer of characters, ignoring case differences. * - * \param[in] pString String. - * \param[in] pBuffer Buffer. - * \param[in] length Length of the buffer, in bytes. - * \return The return value is 0 if the string value is equal to the buffer value, - * less than 0 if the string value is less than the buffer value, - * greater than 0 if the string value is greater than the buffer value. + * \param[in] pString String. + * \param[in] pBuffer Buffer. + * \param[in] length Length of the buffer, in bytes. + * \return The return value is 0 if the string value is equal to the buffer value, + * less than 0 if the string value is less than the buffer value, + * greater than 0 if the string value is greater than the buffer value. */ static int32_t sbgStringCompareIgnoreCaseCommon(const SbgString *pString, const char *pBuffer, size_t length) { - int32_t result; - size_t minLength; + int32_t result; + size_t minLength; - assert(pString); - assert(pBuffer); + assert(pString); + assert(pBuffer); - result = 0; + result = 0; - if (length > pString->length) - { - minLength = pString->length; - } - else - { - minLength = length; - } + if (length > pString->length) + { + minLength = pString->length; + } + else + { + minLength = length; + } - for (size_t i = 0; i <= minLength; i++) - { - unsigned char c1; - unsigned char c2; + for (size_t i = 0; i <= minLength; i++) + { + unsigned char c1; + unsigned char c2; - c1 = (unsigned char)tolower((unsigned char)pString->pBuffer[i]); - c2 = (unsigned char)tolower((unsigned char)pBuffer[i]); + c1 = (unsigned char)tolower((unsigned char)pString->pBuffer[i]); + c2 = (unsigned char)tolower((unsigned char)pBuffer[i]); - if (c1 != c2) - { - result = c1 - c2; - break; - } - } + if (c1 != c2) + { + result = c1 - c2; + break; + } + } - return result; + return result; } //----------------------------------------------------------------------// @@ -415,319 +491,310 @@ static int32_t sbgStringCompareIgnoreCaseCommon(const SbgString *pString, const SBG_COMMON_LIB_API void sbgStringConstructEmpty(SbgString *pString) { - assert(pString); - - pString->internalBuffer[0] = '\0'; + sbgStringReset(pString); - pString->pBuffer = pString->internalBuffer; - pString->capacity = sizeof(pString->internalBuffer); - pString->length = 0; + pString->readOnly = false; + pString->isStatic = false; - pString->readOnly = false; - pString->isStatic = false; - - pString->errorCode = SBG_NO_ERROR; + pString->errorCode = SBG_NO_ERROR; } SBG_COMMON_LIB_API SbgErrorCode sbgStringConstruct(SbgString *pString, const SbgString *pSourceString) { - sbgStringConstructEmpty(pString); + sbgStringConstructEmpty(pString); - if (pSourceString) - { - sbgStringAssign(pString, pSourceString); - } + if (pSourceString) + { + sbgStringAssign(pString, pSourceString); + } - return pString->errorCode; + return pString->errorCode; } SBG_COMMON_LIB_API SbgErrorCode sbgStringConstructCString(SbgString *pString, const char *pCString) { - sbgStringConstructEmpty(pString); - return sbgStringAssignCString(pString, pCString); + sbgStringConstructEmpty(pString); + return sbgStringAssignCString(pString, pCString); } SBG_COMMON_LIB_API SbgErrorCode sbgStringConstructVF(SbgString *pString, const char *pFormat, va_list args) { - sbgStringConstructEmpty(pString); - return sbgStringAssignVF(pString, pFormat, args); + sbgStringConstructEmpty(pString); + return sbgStringAssignVF(pString, pFormat, args); } SBG_COMMON_LIB_API SbgErrorCode sbgStringConstructF(SbgString *pString, const char *pFormat, ...) { - SbgErrorCode errorCode; - va_list args; + SbgErrorCode errorCode; + va_list args; - sbgStringConstructEmpty(pString); + sbgStringConstructEmpty(pString); - va_start(args, pFormat); - errorCode = sbgStringAssignVF(pString, pFormat, args); - va_end(args); + va_start(args, pFormat); + errorCode = sbgStringAssignVF(pString, pFormat, args); + va_end(args); - return errorCode; + return errorCode; } SBG_COMMON_LIB_API SbgErrorCode sbgStringConstructStatic(SbgString *pString, const char *pCString, char *pBuffer, size_t size) { - assert(pString); - assert(pBuffer); + assert(pString); + assert(pBuffer); - pString->pBuffer = pBuffer; - pString->capacity = size; + pString->pBuffer = pBuffer; + pString->capacity = size; - pString->readOnly = false; - pString->isStatic = true; + pString->readOnly = false; + pString->isStatic = true; - pString->errorCode = SBG_NO_ERROR; + pString->errorCode = SBG_NO_ERROR; - if (pCString) - { - sbgStringAssignCString(pString, pCString); - } - else - { - pString->length = strlen(pBuffer); - } + if (pCString) + { + sbgStringAssignCString(pString, pCString); + } + else + { + pString->length = strlen(pBuffer); + } - return pString->errorCode; + return pString->errorCode; } SBG_COMMON_LIB_API void sbgStringConstructReadOnly(SbgString *pString, const char *pCString) { - assert(pString); - assert(pCString); + assert(pString); + assert(pCString); - pString->pBuffer = (char *)pCString; - pString->length = strlen(pCString); - pString->capacity = pString->length + 1; + pString->pBuffer = (char *)pCString; + pString->length = strlen(pCString); + pString->capacity = pString->length + 1; - pString->readOnly = true; - pString->isStatic = true; + pString->readOnly = true; + pString->isStatic = true; - pString->errorCode = SBG_NO_ERROR; + pString->errorCode = SBG_NO_ERROR; } SBG_COMMON_LIB_API void sbgStringDestroy(SbgString *pString) { - assert(pString); + assert(pString); - if (!pString->isStatic) - { - if (pString->pBuffer != pString->internalBuffer) - { - free(pString->pBuffer); - } - } + if (!pString->isStatic && (pString->pBuffer != pString->internalBuffer)) + { + free(pString->pBuffer); + } } SBG_COMMON_LIB_API int32_t sbgStringCompare(const SbgString *pString, const SbgString *pOtherString) { - assert(pString); - assert(pOtherString); + assert(pString); + assert(pOtherString); - return (int32_t)strcmp(pString->pBuffer, pOtherString->pBuffer); + return (int32_t)strcmp(pString->pBuffer, pOtherString->pBuffer); } SBG_COMMON_LIB_API int32_t sbgStringCompareCString(const SbgString *pString, const char *pCString) { - assert(pString); - assert(pCString); + assert(pString); + assert(pCString); - return (int32_t)strcmp(pString->pBuffer, pCString); + return (int32_t)strcmp(pString->pBuffer, pCString); } SBG_COMMON_LIB_API int32_t sbgStringCompareIgnoreCase(const SbgString *pString, const SbgString *pOtherString) { - assert(pOtherString); + assert(pOtherString); - return sbgStringCompareIgnoreCaseCommon(pString, pOtherString->pBuffer, pOtherString->length); + return sbgStringCompareIgnoreCaseCommon(pString, pOtherString->pBuffer, pOtherString->length); } SBG_COMMON_LIB_API int32_t sbgStringCompareIgnoreCaseCString(const SbgString *pString, const char *pCString) { - assert(pCString); + assert(pCString); - return sbgStringCompareIgnoreCaseCommon(pString, pCString, strlen(pCString)); + return sbgStringCompareIgnoreCaseCommon(pString, pCString, strlen(pCString)); } SBG_COMMON_LIB_API size_t sbgStringHash(const SbgString *pString) { - size_t hash; + size_t hash; - assert(pString); + assert(pString); - // - // The algorithm is a simple polynomial accumulation : - // s[0]*31^(n-1) + s[1]*31^(n-2) + ... + s[n-1]. - // - // See java.lang.String.hashCode(). - // + // + // The algorithm is a simple polynomial accumulation : + // s[0]*31^(n-1) + s[1]*31^(n-2) + ... + s[n-1]. + // + // See java.lang.String.hashCode(). + // - hash = 0; + hash = 0; - for (size_t i = 0; i < pString->length; i++) - { - char *pCharPtr; + for (size_t i = 0; i < pString->length; i++) + { + char *pCharPtr; - pCharPtr = sbgStringGetCharPtr(pString, i); + pCharPtr = sbgStringGetCharPtr(pString, i); - hash = ((hash << 5) - hash) + *pCharPtr; - } + hash = ((hash << 5) - hash) + *pCharPtr; + } - return hash; + return hash; } SBG_COMMON_LIB_API size_t sbgStringGetLength(const SbgString *pString) { - assert(pString); + assert(pString); - return pString->length; + return pString->length; } SBG_COMMON_LIB_API const char *sbgStringGetCString(const SbgString *pString) { - assert(pString); + assert(pString); - return pString->pBuffer; + return pString->pBuffer; } SBG_COMMON_LIB_API SbgErrorCode sbgStringCharAt(const SbgString *pString, size_t index, char *pChar) { - SbgErrorCode errorCode; + SbgErrorCode errorCode; - assert(pString); - assert(pChar); + assert(pString); + assert(pChar); - if (index < pString->length) - { - char *pCharPtr; + if (index < pString->length) + { + char *pCharPtr; - pCharPtr = sbgStringGetCharPtr(pString, index); - *pChar = *pCharPtr; + pCharPtr = sbgStringGetCharPtr(pString, index); + *pChar = *pCharPtr; - errorCode = SBG_NO_ERROR; - } - else - { - errorCode = SBG_INVALID_PARAMETER; - } + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_INVALID_PARAMETER; + } - return errorCode; + return errorCode; } SBG_COMMON_LIB_API size_t sbgStringFindCString(const SbgString *pString, const char *pCString) { - char *pChar; + char *pChar; - assert(pString); - assert(pCString); + assert(pString); + assert(pCString); - pChar = strstr(pString->pBuffer, pCString); + pChar = strstr(pString->pBuffer, pCString); - if (pChar) - { - return (size_t)(pChar - pString->pBuffer); - } - else - { - return SIZE_MAX; - } + if (pChar) + { + return (size_t)(pChar - pString->pBuffer); + } + else + { + return SIZE_MAX; + } } SBG_COMMON_LIB_API size_t sbgStringFind(const SbgString *pString, const SbgString *pOtherString) { - assert(pOtherString); + assert(pOtherString); - return sbgStringFindCString(pString, pOtherString->pBuffer); + return sbgStringFindCString(pString, pOtherString->pBuffer); } SBG_COMMON_LIB_API SbgErrorCode sbgStringSubstring(const SbgString *pString, size_t startIndex, size_t endIndex, SbgString *pSubstring) { - SbgErrorCode errorCode; + SbgErrorCode errorCode; - if (sbgStringIndexesAreValid(pString, startIndex, endIndex)) - { - size_t length; + if (sbgStringIndexesAreValid(pString, startIndex, endIndex)) + { + size_t length; - length = endIndex - startIndex; + length = endIndex - startIndex; - errorCode = sbgStringAssignBuffer(pSubstring, &pString->pBuffer[startIndex], length); - } - else - { - errorCode = SBG_INVALID_PARAMETER; - } + errorCode = sbgStringAssignBuffer(pSubstring, &pString->pBuffer[startIndex], length); + } + else + { + errorCode = SBG_INVALID_PARAMETER; + } - return errorCode; + return errorCode; } SBG_COMMON_LIB_API bool sbgStringStartsWith(const SbgString *pString, const char *pCString) { - size_t cStringLength; - bool match; + size_t cStringLength; + bool match; - assert(pString); - assert(pCString); + assert(pString); + assert(pCString); - cStringLength = strlen(pCString); + cStringLength = strlen(pCString); - if (cStringLength <= pString->length) - { - int result; + if (cStringLength <= pString->length) + { + int result; - result = memcmp(pString->pBuffer, pCString, cStringLength); + result = memcmp(pString->pBuffer, pCString, cStringLength); - if (result == 0) - { - match = true; - } - else - { - match = false; - } - } - else - { - match = false; - } + if (result == 0) + { + match = true; + } + else + { + match = false; + } + } + else + { + match = false; + } - return match; + return match; } SBG_COMMON_LIB_API bool sbgStringEndsWith(const SbgString *pString, const char *pCString) { - size_t cStringLength; - bool match; + size_t cStringLength; + bool match; - assert(pString); - assert(pCString); + assert(pString); + assert(pCString); - cStringLength = strlen(pCString); + cStringLength = strlen(pCString); - if (cStringLength <= pString->length) - { - size_t endIndex; - int result; + if (cStringLength <= pString->length) + { + size_t endIndex; + int result; - endIndex = pString->length - cStringLength; + endIndex = pString->length - cStringLength; - result = memcmp(&pString->pBuffer[endIndex], pCString, cStringLength); + result = memcmp(&pString->pBuffer[endIndex], pCString, cStringLength); - if (result == 0) - { - match = true; - } - else - { - match = false; - } - } - else - { - match = false; - } + if (result == 0) + { + match = true; + } + else + { + match = false; + } + } + else + { + match = false; + } - return match; + return match; } //----------------------------------------------------------------------// @@ -736,331 +803,404 @@ SBG_COMMON_LIB_API bool sbgStringEndsWith(const SbgString *pString, const char * SBG_COMMON_LIB_API SbgErrorCode sbgStringGetLastError(const SbgString *pString) { - assert(pString); + assert(pString); - return pString->errorCode; + return pString->errorCode; } SBG_COMMON_LIB_API void sbgStringClearLastError(SbgString *pString) { - assert(pString); + assert(pString); - pString->errorCode = SBG_NO_ERROR; + pString->errorCode = SBG_NO_ERROR; } SBG_COMMON_LIB_API SbgErrorCode sbgStringSetCharAt(SbgString *pString, size_t index, char c) { - SbgErrorCode errorCode; + SbgErrorCode errorCode; - assert(pString); + assert(pString); - if ((index < pString->length) && (c != '\0')) - { - char *pCharPtr; + if ((index < pString->length) && (c != '\0')) + { + char *pCharPtr; - pCharPtr = sbgStringGetCharPtr(pString, index); - *pCharPtr = c; + pCharPtr = sbgStringGetCharPtr(pString, index); + *pCharPtr = c; - errorCode = SBG_NO_ERROR; - } - else - { - errorCode = SBG_INVALID_PARAMETER; - } + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_INVALID_PARAMETER; + } - return errorCode; + return errorCode; } SBG_COMMON_LIB_API SbgErrorCode sbgStringAppend(SbgString *pString, const SbgString *pAppendString) { - assert(pString); - assert(pAppendString); + assert(pString); + assert(pAppendString); - if (pString->errorCode == SBG_NO_ERROR) - { - size_t length; + if (pString->errorCode == SBG_NO_ERROR) + { + size_t length; - length = pString->length + pAppendString->length; + length = pString->length + pAppendString->length; - pString->errorCode = sbgStringResizeBuffer(pString, length + 1); + pString->errorCode = sbgStringResizeBuffer(pString, length + 1); - if (pString->errorCode == SBG_NO_ERROR) - { - memcpy(&pString->pBuffer[pString->length], pAppendString->pBuffer, pAppendString->length + 1); - pString->length = length; - } - } + if (pString->errorCode == SBG_NO_ERROR) + { + memcpy(&pString->pBuffer[pString->length], pAppendString->pBuffer, pAppendString->length + 1); + pString->length = length; + } + } - return pString->errorCode; + return pString->errorCode; } SBG_COMMON_LIB_API SbgErrorCode sbgStringAppendCString(SbgString *pString, const char *pCString) { - assert(pString); - assert(pCString); + assert(pString); + assert(pCString); - if (pString->errorCode == SBG_NO_ERROR) - { - size_t cStringLength; - size_t length; + if (pString->errorCode == SBG_NO_ERROR) + { + size_t cStringLength; + size_t length; - cStringLength = strlen(pCString); - length = pString->length + cStringLength; + cStringLength = strlen(pCString); + length = pString->length + cStringLength; - pString->errorCode = sbgStringResizeBuffer(pString, length + 1); + pString->errorCode = sbgStringResizeBuffer(pString, length + 1); - if (pString->errorCode == SBG_NO_ERROR) - { - memcpy(&pString->pBuffer[pString->length], pCString, cStringLength + 1); - pString->length = length; - } - } + if (pString->errorCode == SBG_NO_ERROR) + { + memcpy(&pString->pBuffer[pString->length], pCString, cStringLength + 1); + pString->length = length; + } + } - return pString->errorCode; + return pString->errorCode; } SBG_COMMON_LIB_API SbgErrorCode sbgStringAppendVF(SbgString *pString, const char *pFormat, va_list args) { - assert(pString); - assert(!pString->readOnly); - assert(pFormat); + assert(pString); + assert(!pString->readOnly); + assert(pFormat); - if (pString->errorCode == SBG_NO_ERROR) - { - va_list argsCopy; - int result; - size_t remainingSize; - size_t newLength; + if (pString->errorCode == SBG_NO_ERROR) + { + va_list argsCopy; + int result; + size_t remainingSize; + size_t newLength; - remainingSize = pString->capacity - pString->length; + remainingSize = pString->capacity - pString->length; - va_copy(argsCopy, args); - result = vsnprintf(&pString->pBuffer[pString->length], remainingSize, pFormat, argsCopy); - assert(result >= 0); - va_end(argsCopy); + va_copy(argsCopy, args); + result = vsnprintf(&pString->pBuffer[pString->length], remainingSize, pFormat, argsCopy); + assert(result >= 0); + va_end(argsCopy); - newLength = pString->length + (size_t)result; + newLength = pString->length + (size_t)result; - if ((size_t)result < remainingSize) - { - pString->length = newLength; - } - else - { - pString->errorCode = sbgStringResizeBuffer(pString, newLength + 1); + if ((size_t)result < remainingSize) + { + pString->length = newLength; + } + else + { + pString->errorCode = sbgStringResizeBuffer(pString, newLength + 1); - if (pString->errorCode == SBG_NO_ERROR) - { - remainingSize = pString->capacity - pString->length; - vsnprintf(&pString->pBuffer[pString->length], remainingSize, pFormat, args); - pString->length = newLength; - } - } - } + if (pString->errorCode == SBG_NO_ERROR) + { + remainingSize = pString->capacity - pString->length; + vsnprintf(&pString->pBuffer[pString->length], remainingSize, pFormat, args); + pString->length = newLength; + } + } + } - return pString->errorCode; + return pString->errorCode; } SBG_COMMON_LIB_API SbgErrorCode sbgStringAppendF(SbgString *pString, const char *pFormat, ...) { - assert(pString); - assert(pFormat); + assert(pString); + assert(pFormat); - if (pString->errorCode == SBG_NO_ERROR) - { - va_list args; + if (pString->errorCode == SBG_NO_ERROR) + { + va_list args; - va_start(args, pFormat); - sbgStringAppendVF(pString, pFormat, args); - va_end(args); - } + va_start(args, pFormat); + sbgStringAppendVF(pString, pFormat, args); + va_end(args); + } - return pString->errorCode; + return pString->errorCode; } SBG_COMMON_LIB_API SbgErrorCode sbgStringAssign(SbgString *pString, const SbgString *pAssignString) { - assert(pAssignString); + assert(pAssignString); - return sbgStringAssignBuffer(pString, pAssignString->pBuffer, pAssignString->length); + return sbgStringAssignBuffer(pString, pAssignString->pBuffer, pAssignString->length); } SBG_COMMON_LIB_API SbgErrorCode sbgStringAssignCString(SbgString *pString, const char *pCString) { - assert(pCString); + assert(pCString); - return sbgStringAssignBuffer(pString, pCString, strlen(pCString)); + return sbgStringAssignBuffer(pString, pCString, strlen(pCString)); } SBG_COMMON_LIB_API SbgErrorCode sbgStringAssignVF(SbgString *pString, const char *pFormat, va_list args) { - assert(pString); - assert(pFormat); + assert(pString); + assert(pFormat); - if (pString->errorCode == SBG_NO_ERROR) - { - va_list argsCopy; - int result; + if (pString->errorCode == SBG_NO_ERROR) + { + va_list argsCopy; + int result; - va_copy(argsCopy, args); - result = vsnprintf(pString->pBuffer, pString->capacity, pFormat, argsCopy); - assert(result >= 0); - va_end(argsCopy); + va_copy(argsCopy, args); + result = vsnprintf(pString->pBuffer, pString->capacity, pFormat, argsCopy); + assert(result >= 0); + va_end(argsCopy); - if ((size_t)result < pString->capacity) - { - pString->length = (size_t)result; - } - else - { - pString->errorCode = sbgStringResizeBuffer(pString, (size_t)result + 1); + if ((size_t)result < pString->capacity) + { + pString->length = (size_t)result; + } + else + { + pString->errorCode = sbgStringResizeBuffer(pString, (size_t)result + 1); - if (pString->errorCode == SBG_NO_ERROR) - { - vsnprintf(pString->pBuffer, pString->capacity, pFormat, args); - pString->length = (size_t)result; - } - } - } + if (pString->errorCode == SBG_NO_ERROR) + { + vsnprintf(pString->pBuffer, pString->capacity, pFormat, args); + pString->length = (size_t)result; + } + } + } - return pString->errorCode; + return pString->errorCode; } SBG_COMMON_LIB_API SbgErrorCode sbgStringAssignF(SbgString *pString, const char *pFormat, ...) { - assert(pString); - assert(pFormat); + assert(pString); + assert(pFormat); + + if (pString->errorCode == SBG_NO_ERROR) + { + va_list args; + + va_start(args, pFormat); + sbgStringAssignVF(pString, pFormat, args); + va_end(args); + } + + return pString->errorCode; +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringMove(SbgString *pString, SbgString *pSourceString) +{ + SbgErrorCode errorCode; + + assert(pSourceString); + + errorCode = pSourceString->errorCode; + + if (errorCode == SBG_NO_ERROR) + { + if (pSourceString->isStatic || (pSourceString->pBuffer == pSourceString->internalBuffer)) + { + errorCode = sbgStringAssign(pString, pSourceString); + + if (errorCode == SBG_NO_ERROR) + { + sbgStringClear(pSourceString); + } + } + else + { + errorCode = sbgStringMoveBuffer(pString, pSourceString->pBuffer, pSourceString->length); + + if (errorCode == SBG_NO_ERROR) + { + sbgStringReset(pSourceString); + } + } + } + + return errorCode; +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringMoveCString(SbgString *pString, char *pCString) +{ + assert(pCString); + + return sbgStringMoveBuffer(pString, pCString, strlen(pCString)); +} + +SBG_COMMON_LIB_API char * sbgStringExtract(SbgString *pString) +{ + char *pBuffer = NULL; + + assert(pString); + + if (pString->errorCode == SBG_NO_ERROR) + { + if (pString->isStatic || (pString->pBuffer == pString->internalBuffer)) + { + size_t size; + + size = pString->length + 1; + pBuffer = malloc(size); + + if (pBuffer) + { + memcpy(pBuffer, pString->pBuffer, pString->length + 1); - if (pString->errorCode == SBG_NO_ERROR) - { - va_list args; + sbgStringClear(pString); + } + } + else + { + pBuffer = pString->pBuffer; - va_start(args, pFormat); - sbgStringAssignVF(pString, pFormat, args); - va_end(args); - } + sbgStringReset(pString); + } + } - return pString->errorCode; + return pBuffer; } SBG_COMMON_LIB_API SbgErrorCode sbgStringClear(SbgString *pString) { - assert(pString); + assert(pString); - if (pString->errorCode == SBG_NO_ERROR) - { - sbgStringAssignCString(pString, ""); - } + if (pString->errorCode == SBG_NO_ERROR) + { + sbgStringAssignCString(pString, ""); + } - return pString->errorCode; + return pString->errorCode; } SBG_COMMON_LIB_API SbgErrorCode sbgStringToUpperCase(SbgString *pString) { - assert(pString); - assert(!pString->readOnly); + assert(pString); + assert(!pString->readOnly); - if (pString->errorCode == SBG_NO_ERROR) - { - for (size_t i = 0; i < pString->length; i++) - { - pString->pBuffer[i] = (char)toupper((unsigned char)pString->pBuffer[i]); - } - } + if (pString->errorCode == SBG_NO_ERROR) + { + for (size_t i = 0; i < pString->length; i++) + { + pString->pBuffer[i] = (char)toupper((unsigned char)pString->pBuffer[i]); + } + } - return pString->errorCode; + return pString->errorCode; } SBG_COMMON_LIB_API SbgErrorCode sbgStringToLowerCase(SbgString *pString) { - assert(pString); - assert(!pString->readOnly); + assert(pString); + assert(!pString->readOnly); - if (pString->errorCode == SBG_NO_ERROR) - { - for (size_t i = 0; i < pString->length; i++) - { - pString->pBuffer[i] = (char)tolower((unsigned char)pString->pBuffer[i]); - } - } + if (pString->errorCode == SBG_NO_ERROR) + { + for (size_t i = 0; i < pString->length; i++) + { + pString->pBuffer[i] = (char)tolower((unsigned char)pString->pBuffer[i]); + } + } - return pString->errorCode; + return pString->errorCode; } SBG_COMMON_LIB_API SbgErrorCode sbgStringTrimLeft(SbgString *pString) { - assert(pString); + assert(pString); - if (pString->errorCode == SBG_NO_ERROR) - { - bool trimmed; + if (pString->errorCode == SBG_NO_ERROR) + { + bool trimmed; - trimmed = sbgStringTrimLeftCommon(pString); + trimmed = sbgStringTrimLeftCommon(pString); - if (trimmed) - { - sbgStringResizeBuffer(pString, pString->length + 1); - } - } + if (trimmed) + { + sbgStringResizeBuffer(pString, pString->length + 1); + } + } - return pString->errorCode; + return pString->errorCode; } SBG_COMMON_LIB_API SbgErrorCode sbgStringTrimRight(SbgString *pString) { - assert(pString); + assert(pString); - if (pString->errorCode == SBG_NO_ERROR) - { - bool trimmed; + if (pString->errorCode == SBG_NO_ERROR) + { + bool trimmed; - trimmed = sbgStringTrimRightCommon(pString); + trimmed = sbgStringTrimRightCommon(pString); - if (trimmed) - { - sbgStringResizeBuffer(pString, pString->length + 1); - } - } + if (trimmed) + { + sbgStringResizeBuffer(pString, pString->length + 1); + } + } - return pString->errorCode; + return pString->errorCode; } SBG_COMMON_LIB_API SbgErrorCode sbgStringTrim(SbgString *pString) { - assert(pString); + assert(pString); - if (pString->errorCode == SBG_NO_ERROR) - { - bool trimmedLeft; - bool trimmedRight; + if (pString->errorCode == SBG_NO_ERROR) + { + bool trimmedLeft; + bool trimmedRight; - trimmedLeft = sbgStringTrimLeftCommon(pString); - trimmedRight = sbgStringTrimRightCommon(pString); + trimmedLeft = sbgStringTrimLeftCommon(pString); + trimmedRight = sbgStringTrimRightCommon(pString); - if (trimmedLeft || trimmedRight) - { - sbgStringResizeBuffer(pString, pString->length + 1); - } - } + if (trimmedLeft || trimmedRight) + { + sbgStringResizeBuffer(pString, pString->length + 1); + } + } - return pString->errorCode; + return pString->errorCode; } SBG_COMMON_LIB_API void sbgStringTruncate(SbgString *pString, size_t length) { - assert(pString); - assert(!pString->readOnly); + assert(pString); + assert(!pString->readOnly); - if (pString->errorCode == SBG_NO_ERROR) - { - if (length < pString->length) - { - pString->pBuffer[length] = '\0'; - pString->length = length; + if (pString->errorCode == SBG_NO_ERROR) + { + if (length < pString->length) + { + pString->pBuffer[length] = '\0'; + pString->length = length; - sbgStringResizeBuffer(pString, length + 1); - } - } + sbgStringResizeBuffer(pString, length + 1); + } + } } //----------------------------------------------------------------------// @@ -1069,639 +1209,639 @@ SBG_COMMON_LIB_API void sbgStringTruncate(SbgString *pString, size_t length) SBG_COMMON_LIB_API SbgErrorCode sbgStringFromInt8(SbgString *pString, int8_t value) { - assert(pString); - assert(!pString->readOnly); + assert(pString); + assert(!pString->readOnly); - if (pString->errorCode == SBG_NO_ERROR) - { - size_t length; - size_t oldCapacity; - int result; + if (pString->errorCode == SBG_NO_ERROR) + { + size_t length; + size_t oldCapacity; + int result; - result = snprintf(pString->pBuffer, pString->capacity, "%" PRId8, value); + result = snprintf(pString->pBuffer, pString->capacity, "%" PRId8, value); - assert(result >= 0); + assert(result >= 0); - length = (size_t)result; + length = (size_t)result; - oldCapacity = pString->capacity; + oldCapacity = pString->capacity; - pString->errorCode = sbgStringResizeBuffer(pString, length + 1); + pString->errorCode = sbgStringResizeBuffer(pString, length + 1); - if (pString->errorCode == SBG_NO_ERROR) - { - if (length >= oldCapacity) - { - snprintf(pString->pBuffer, pString->capacity, "%" PRId8, value); - } + if (pString->errorCode == SBG_NO_ERROR) + { + if (length >= oldCapacity) + { + snprintf(pString->pBuffer, pString->capacity, "%" PRId8, value); + } - pString->length = (size_t)length; - } - } + pString->length = (size_t)length; + } + } - return pString->errorCode; + return pString->errorCode; } SBG_COMMON_LIB_API SbgErrorCode sbgStringToInt8(const SbgString *pString, int8_t *pValue) { - SbgErrorCode errorCode; - int result; - int8_t value; + SbgErrorCode errorCode; + int result; + int8_t value; - assert(pString); + assert(pString); - result = sscanf(pString->pBuffer, "%" SCNd8, &value); + result = sscanf(pString->pBuffer, "%" SCNd8, &value); - if (result == 1) - { - *pValue = value; + if (result == 1) + { + *pValue = value; - errorCode = SBG_NO_ERROR; - } - else - { - errorCode = SBG_ERROR; - } + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_ERROR; + } - return errorCode; + return errorCode; } SBG_COMMON_LIB_API SbgErrorCode sbgStringFromUint8(SbgString *pString, uint8_t value) { - assert(pString); - assert(!pString->readOnly); + assert(pString); + assert(!pString->readOnly); - if (pString->errorCode == SBG_NO_ERROR) - { - size_t length; - size_t oldCapacity; - int result; + if (pString->errorCode == SBG_NO_ERROR) + { + size_t length; + size_t oldCapacity; + int result; - result = snprintf(pString->pBuffer, pString->capacity, "%" PRIu8, value); + result = snprintf(pString->pBuffer, pString->capacity, "%" PRIu8, value); - assert(result >= 0); + assert(result >= 0); - length = (size_t)result; + length = (size_t)result; - oldCapacity = pString->capacity; + oldCapacity = pString->capacity; - pString->errorCode = sbgStringResizeBuffer(pString, length + 1); + pString->errorCode = sbgStringResizeBuffer(pString, length + 1); - if (pString->errorCode == SBG_NO_ERROR) - { - if (length >= oldCapacity) - { - snprintf(pString->pBuffer, pString->capacity, "%" PRIu8, value); - } + if (pString->errorCode == SBG_NO_ERROR) + { + if (length >= oldCapacity) + { + snprintf(pString->pBuffer, pString->capacity, "%" PRIu8, value); + } - pString->length = (size_t)length; - } - } + pString->length = (size_t)length; + } + } - return pString->errorCode; + return pString->errorCode; } SBG_COMMON_LIB_API SbgErrorCode sbgStringToUint8(const SbgString *pString, uint8_t *pValue) { - SbgErrorCode errorCode; - int result; - uint8_t value; + SbgErrorCode errorCode; + int result; + uint8_t value; - assert(pString); + assert(pString); - result = sscanf(pString->pBuffer, "%" SCNu8, &value); + result = sscanf(pString->pBuffer, "%" SCNu8, &value); - if (result == 1) - { - *pValue = value; + if (result == 1) + { + *pValue = value; - errorCode = SBG_NO_ERROR; - } - else - { - errorCode = SBG_ERROR; - } + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_ERROR; + } - return errorCode; + return errorCode; } SBG_COMMON_LIB_API SbgErrorCode sbgStringFromInt16(SbgString *pString, int16_t value) { - assert(pString); - assert(!pString->readOnly); + assert(pString); + assert(!pString->readOnly); - if (pString->errorCode == SBG_NO_ERROR) - { - size_t length; - size_t oldCapacity; - int result; + if (pString->errorCode == SBG_NO_ERROR) + { + size_t length; + size_t oldCapacity; + int result; - result = snprintf(pString->pBuffer, pString->capacity, "%" PRId16, value); + result = snprintf(pString->pBuffer, pString->capacity, "%" PRId16, value); - assert(result >= 0); + assert(result >= 0); - length = (size_t)result; + length = (size_t)result; - oldCapacity = pString->capacity; + oldCapacity = pString->capacity; - pString->errorCode = sbgStringResizeBuffer(pString, length + 1); + pString->errorCode = sbgStringResizeBuffer(pString, length + 1); - if (pString->errorCode == SBG_NO_ERROR) - { - if (length >= oldCapacity) - { - snprintf(pString->pBuffer, pString->capacity, "%" PRId16, value); - } + if (pString->errorCode == SBG_NO_ERROR) + { + if (length >= oldCapacity) + { + snprintf(pString->pBuffer, pString->capacity, "%" PRId16, value); + } - pString->length = (size_t)length; - } - } + pString->length = (size_t)length; + } + } - return pString->errorCode; + return pString->errorCode; } SBG_COMMON_LIB_API SbgErrorCode sbgStringToInt16(const SbgString *pString, int16_t *pValue) { - SbgErrorCode errorCode; - int result; - int16_t value; + SbgErrorCode errorCode; + int result; + int16_t value; - assert(pString); + assert(pString); - result = sscanf(pString->pBuffer, "%" SCNd16, &value); + result = sscanf(pString->pBuffer, "%" SCNd16, &value); - if (result == 1) - { - *pValue = value; + if (result == 1) + { + *pValue = value; - errorCode = SBG_NO_ERROR; - } - else - { - errorCode = SBG_ERROR; - } + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_ERROR; + } - return errorCode; + return errorCode; } SBG_COMMON_LIB_API SbgErrorCode sbgStringFromUint16(SbgString *pString, uint16_t value) { - assert(pString); - assert(!pString->readOnly); + assert(pString); + assert(!pString->readOnly); - if (pString->errorCode == SBG_NO_ERROR) - { - size_t length; - size_t oldCapacity; - int result; + if (pString->errorCode == SBG_NO_ERROR) + { + size_t length; + size_t oldCapacity; + int result; - result = snprintf(pString->pBuffer, pString->capacity, "%" PRIu16, value); + result = snprintf(pString->pBuffer, pString->capacity, "%" PRIu16, value); - assert(result >= 0); + assert(result >= 0); - length = (size_t)result; + length = (size_t)result; - oldCapacity = pString->capacity; + oldCapacity = pString->capacity; - pString->errorCode = sbgStringResizeBuffer(pString, length + 1); + pString->errorCode = sbgStringResizeBuffer(pString, length + 1); - if (pString->errorCode == SBG_NO_ERROR) - { - if (length >= oldCapacity) - { - snprintf(pString->pBuffer, pString->capacity, "%" PRIu16, value); - } + if (pString->errorCode == SBG_NO_ERROR) + { + if (length >= oldCapacity) + { + snprintf(pString->pBuffer, pString->capacity, "%" PRIu16, value); + } - pString->length = (size_t)length; - } - } + pString->length = (size_t)length; + } + } - return pString->errorCode; + return pString->errorCode; } SBG_COMMON_LIB_API SbgErrorCode sbgStringToUint16(const SbgString *pString, uint16_t *pValue) { - SbgErrorCode errorCode; - int result; - uint16_t value; + SbgErrorCode errorCode; + int result; + uint16_t value; - assert(pString); + assert(pString); - result = sscanf(pString->pBuffer, "%" SCNu16, &value); + result = sscanf(pString->pBuffer, "%" SCNu16, &value); - if (result == 1) - { - *pValue = value; + if (result == 1) + { + *pValue = value; - errorCode = SBG_NO_ERROR; - } - else - { - errorCode = SBG_ERROR; - } + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_ERROR; + } - return errorCode; + return errorCode; } SBG_COMMON_LIB_API SbgErrorCode sbgStringFromInt32(SbgString *pString, int32_t value) { - assert(pString); - assert(!pString->readOnly); + assert(pString); + assert(!pString->readOnly); - if (pString->errorCode == SBG_NO_ERROR) - { - size_t length; - size_t oldCapacity; - int result; + if (pString->errorCode == SBG_NO_ERROR) + { + size_t length; + size_t oldCapacity; + int result; - result = snprintf(pString->pBuffer, pString->capacity, "%" PRId32, value); + result = snprintf(pString->pBuffer, pString->capacity, "%" PRId32, value); - assert(result >= 0); + assert(result >= 0); - length = (size_t)result; + length = (size_t)result; - oldCapacity = pString->capacity; + oldCapacity = pString->capacity; - pString->errorCode = sbgStringResizeBuffer(pString, length + 1); + pString->errorCode = sbgStringResizeBuffer(pString, length + 1); - if (pString->errorCode == SBG_NO_ERROR) - { - if (length >= oldCapacity) - { - snprintf(pString->pBuffer, pString->capacity, "%" PRId32, value); - } + if (pString->errorCode == SBG_NO_ERROR) + { + if (length >= oldCapacity) + { + snprintf(pString->pBuffer, pString->capacity, "%" PRId32, value); + } - pString->length = (size_t)length; - } - } + pString->length = (size_t)length; + } + } - return pString->errorCode; + return pString->errorCode; } SBG_COMMON_LIB_API SbgErrorCode sbgStringToInt32(const SbgString *pString, int32_t *pValue) { - SbgErrorCode errorCode; - int result; - int32_t value; + SbgErrorCode errorCode; + int result; + int32_t value; - assert(pString); + assert(pString); - result = sscanf(pString->pBuffer, "%" SCNd32, &value); + result = sscanf(pString->pBuffer, "%" SCNd32, &value); - if (result == 1) - { - *pValue = value; + if (result == 1) + { + *pValue = value; - errorCode = SBG_NO_ERROR; - } - else - { - errorCode = SBG_ERROR; - } + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_ERROR; + } - return errorCode; + return errorCode; } SBG_COMMON_LIB_API SbgErrorCode sbgStringFromUint32(SbgString *pString, uint32_t value) { - assert(pString); - assert(!pString->readOnly); + assert(pString); + assert(!pString->readOnly); - if (pString->errorCode == SBG_NO_ERROR) - { - size_t length; - size_t oldCapacity; - int result; + if (pString->errorCode == SBG_NO_ERROR) + { + size_t length; + size_t oldCapacity; + int result; - result = snprintf(pString->pBuffer, pString->capacity, "%" PRIu32, value); + result = snprintf(pString->pBuffer, pString->capacity, "%" PRIu32, value); - assert(result >= 0); + assert(result >= 0); - length = (size_t)result; + length = (size_t)result; - oldCapacity = pString->capacity; + oldCapacity = pString->capacity; - pString->errorCode = sbgStringResizeBuffer(pString, length + 1); + pString->errorCode = sbgStringResizeBuffer(pString, length + 1); - if (pString->errorCode == SBG_NO_ERROR) - { - if (length >= oldCapacity) - { - snprintf(pString->pBuffer, pString->capacity, "%" PRIu32, value); - } + if (pString->errorCode == SBG_NO_ERROR) + { + if (length >= oldCapacity) + { + snprintf(pString->pBuffer, pString->capacity, "%" PRIu32, value); + } - pString->length = (size_t)length; - } - } + pString->length = (size_t)length; + } + } - return pString->errorCode; + return pString->errorCode; } SBG_COMMON_LIB_API SbgErrorCode sbgStringToUint32(const SbgString *pString, uint32_t *pValue) { - SbgErrorCode errorCode; - int result; - uint32_t value; + SbgErrorCode errorCode; + int result; + uint32_t value; - assert(pString); + assert(pString); - result = sscanf(pString->pBuffer, "%" SCNu32, &value); + result = sscanf(pString->pBuffer, "%" SCNu32, &value); - if (result == 1) - { - *pValue = value; + if (result == 1) + { + *pValue = value; - errorCode = SBG_NO_ERROR; - } - else - { - errorCode = SBG_ERROR; - } + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_ERROR; + } - return errorCode; + return errorCode; } SBG_COMMON_LIB_API SbgErrorCode sbgStringFromInt64(SbgString *pString, int64_t value) { - assert(pString); - assert(!pString->readOnly); + assert(pString); + assert(!pString->readOnly); - if (pString->errorCode == SBG_NO_ERROR) - { - size_t length; - size_t oldCapacity; - int result; + if (pString->errorCode == SBG_NO_ERROR) + { + size_t length; + size_t oldCapacity; + int result; - result = snprintf(pString->pBuffer, pString->capacity, "%" PRId64, value); + result = snprintf(pString->pBuffer, pString->capacity, "%" PRId64, value); - assert(result >= 0); + assert(result >= 0); - length = (size_t)result; + length = (size_t)result; - oldCapacity = pString->capacity; + oldCapacity = pString->capacity; - pString->errorCode = sbgStringResizeBuffer(pString, length + 1); + pString->errorCode = sbgStringResizeBuffer(pString, length + 1); - if (pString->errorCode == SBG_NO_ERROR) - { - if (length >= oldCapacity) - { - snprintf(pString->pBuffer, pString->capacity, "%" PRId64, value); - } + if (pString->errorCode == SBG_NO_ERROR) + { + if (length >= oldCapacity) + { + snprintf(pString->pBuffer, pString->capacity, "%" PRId64, value); + } - pString->length = (size_t)length; - } - } + pString->length = (size_t)length; + } + } - return pString->errorCode; + return pString->errorCode; } SBG_COMMON_LIB_API SbgErrorCode sbgStringToInt64(const SbgString *pString, int64_t *pValue) { - SbgErrorCode errorCode; - int result; - int64_t value; + SbgErrorCode errorCode; + int result; + int64_t value; - assert(pString); + assert(pString); - result = sscanf(pString->pBuffer, "%" SCNd64, &value); + result = sscanf(pString->pBuffer, "%" SCNd64, &value); - if (result == 1) - { - *pValue = value; + if (result == 1) + { + *pValue = value; - errorCode = SBG_NO_ERROR; - } - else - { - errorCode = SBG_ERROR; - } + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_ERROR; + } - return errorCode; + return errorCode; } SBG_COMMON_LIB_API SbgErrorCode sbgStringFromUint64(SbgString *pString, uint64_t value) { - assert(pString); - assert(!pString->readOnly); + assert(pString); + assert(!pString->readOnly); - if (pString->errorCode == SBG_NO_ERROR) - { - size_t length; - size_t oldCapacity; - int result; + if (pString->errorCode == SBG_NO_ERROR) + { + size_t length; + size_t oldCapacity; + int result; - result = snprintf(pString->pBuffer, pString->capacity, "%" PRIu64, value); + result = snprintf(pString->pBuffer, pString->capacity, "%" PRIu64, value); - assert(result >= 0); + assert(result >= 0); - length = (size_t)result; + length = (size_t)result; - oldCapacity = pString->capacity; + oldCapacity = pString->capacity; - pString->errorCode = sbgStringResizeBuffer(pString, length + 1); + pString->errorCode = sbgStringResizeBuffer(pString, length + 1); - if (pString->errorCode == SBG_NO_ERROR) - { - if (length >= oldCapacity) - { - snprintf(pString->pBuffer, pString->capacity, "%" PRIu64, value); - } + if (pString->errorCode == SBG_NO_ERROR) + { + if (length >= oldCapacity) + { + snprintf(pString->pBuffer, pString->capacity, "%" PRIu64, value); + } - pString->length = (size_t)length; - } - } + pString->length = (size_t)length; + } + } - return pString->errorCode; + return pString->errorCode; } SBG_COMMON_LIB_API SbgErrorCode sbgStringToUint64(const SbgString *pString, uint64_t *pValue) { - SbgErrorCode errorCode; - int result; - uint64_t value; + SbgErrorCode errorCode; + int result; + uint64_t value; - assert(pString); + assert(pString); - result = sscanf(pString->pBuffer, "%" SCNu64, &value); + result = sscanf(pString->pBuffer, "%" SCNu64, &value); - if (result == 1) - { - *pValue = value; + if (result == 1) + { + *pValue = value; - errorCode = SBG_NO_ERROR; - } - else - { - errorCode = SBG_ERROR; - } + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_ERROR; + } - return errorCode; + return errorCode; } SBG_COMMON_LIB_API SbgErrorCode sbgStringFromFloat(SbgString *pString, float value) { - assert(pString); - assert(!pString->readOnly); + assert(pString); + assert(!pString->readOnly); - if (pString->errorCode == SBG_NO_ERROR) - { - size_t length; - size_t oldCapacity; - int result; + if (pString->errorCode == SBG_NO_ERROR) + { + size_t length; + size_t oldCapacity; + int result; - result = snprintf(pString->pBuffer, pString->capacity, "%f", value); + result = snprintf(pString->pBuffer, pString->capacity, "%f", value); - assert(result >= 0); + assert(result >= 0); - length = (size_t)result; + length = (size_t)result; - oldCapacity = pString->capacity; + oldCapacity = pString->capacity; - pString->errorCode = sbgStringResizeBuffer(pString, length + 1); + pString->errorCode = sbgStringResizeBuffer(pString, length + 1); - if (pString->errorCode == SBG_NO_ERROR) - { - if (length >= oldCapacity) - { - snprintf(pString->pBuffer, pString->capacity, "%f", value); - } + if (pString->errorCode == SBG_NO_ERROR) + { + if (length >= oldCapacity) + { + snprintf(pString->pBuffer, pString->capacity, "%f", value); + } - pString->length = (size_t)length; - } - } + pString->length = (size_t)length; + } + } - return pString->errorCode; + return pString->errorCode; } SBG_COMMON_LIB_API SbgErrorCode sbgStringToFloat(const SbgString *pString, float *pValue) { - SbgErrorCode errorCode; - int result; - float value; + SbgErrorCode errorCode; + int result; + float value; - assert(pString); + assert(pString); - result = sscanf(pString->pBuffer, "%f", &value); + result = sscanf(pString->pBuffer, "%f", &value); - if (result == 1) - { - *pValue = value; + if (result == 1) + { + *pValue = value; - errorCode = SBG_NO_ERROR; - } - else - { - errorCode = SBG_ERROR; - } + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_ERROR; + } - return errorCode; + return errorCode; } SBG_COMMON_LIB_API SbgErrorCode sbgStringFromDouble(SbgString *pString, double value) { - assert(pString); - assert(!pString->readOnly); + assert(pString); + assert(!pString->readOnly); - if (pString->errorCode == SBG_NO_ERROR) - { - size_t length; - size_t oldCapacity; - int result; + if (pString->errorCode == SBG_NO_ERROR) + { + size_t length; + size_t oldCapacity; + int result; - result = snprintf(pString->pBuffer, pString->capacity, "%lf", value); + result = snprintf(pString->pBuffer, pString->capacity, "%lf", value); - assert(result >= 0); + assert(result >= 0); - length = (size_t)result; + length = (size_t)result; - oldCapacity = pString->capacity; + oldCapacity = pString->capacity; - pString->errorCode = sbgStringResizeBuffer(pString, length + 1); + pString->errorCode = sbgStringResizeBuffer(pString, length + 1); - if (pString->errorCode == SBG_NO_ERROR) - { - if (length >= oldCapacity) - { - snprintf(pString->pBuffer, pString->capacity, "%lf", value); - } + if (pString->errorCode == SBG_NO_ERROR) + { + if (length >= oldCapacity) + { + snprintf(pString->pBuffer, pString->capacity, "%lf", value); + } - pString->length = (size_t)length; - } - } + pString->length = (size_t)length; + } + } - return pString->errorCode; + return pString->errorCode; } SBG_COMMON_LIB_API SbgErrorCode sbgStringToDouble(const SbgString *pString, double *pValue) { - SbgErrorCode errorCode; - int result; - double value; + SbgErrorCode errorCode; + int result; + double value; - assert(pString); + assert(pString); - result = sscanf(pString->pBuffer, "%lf", &value); + result = sscanf(pString->pBuffer, "%lf", &value); - if (result == 1) - { - *pValue = value; + if (result == 1) + { + *pValue = value; - errorCode = SBG_NO_ERROR; - } - else - { - errorCode = SBG_ERROR; - } + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_ERROR; + } - return errorCode; + return errorCode; } SBG_COMMON_LIB_API SbgErrorCode sbgStringFromStreamBuffer(SbgString *pString, SbgStreamBuffer *pStream) { - assert(pString); + assert(pString); - if (pString->errorCode == SBG_NO_ERROR) - { - size_t size; + if (pString->errorCode == SBG_NO_ERROR) + { + size_t size; - size = sbgStreamBufferReadSizeT32(pStream); + size = sbgStreamBufferReadSizeT32(pStream); - pString->errorCode = sbgStreamBufferGetLastError(pStream); + pString->errorCode = sbgStreamBufferGetLastError(pStream); - if (pString->errorCode == SBG_NO_ERROR) - { - pString->errorCode = sbgStringResizeBuffer(pString, size); + if (pString->errorCode == SBG_NO_ERROR) + { + pString->errorCode = sbgStringResizeBuffer(pString, size); - if (pString->errorCode == SBG_NO_ERROR) - { - pString->errorCode = sbgStreamBufferReadBuffer(pStream, pString->pBuffer, size); + if (pString->errorCode == SBG_NO_ERROR) + { + pString->errorCode = sbgStreamBufferReadBuffer(pStream, pString->pBuffer, size); - if (pString->errorCode == SBG_NO_ERROR) - { - pString->length = size - 1; - } - } - } - } + if (pString->errorCode == SBG_NO_ERROR) + { + pString->length = size - 1; + } + } + } + } - return pString->errorCode; + return pString->errorCode; } SBG_COMMON_LIB_API SbgErrorCode sbgStringToStreamBuffer(const SbgString *pString, SbgStreamBuffer *pStream) { - SbgErrorCode errorCode; + SbgErrorCode errorCode; - assert(pString); + assert(pString); - errorCode = sbgStreamBufferWriteSizeT32(pStream, pString->length + 1); + errorCode = sbgStreamBufferWriteSizeT32(pStream, pString->length + 1); - if (errorCode == SBG_NO_ERROR) - { - errorCode = sbgStreamBufferWriteBuffer(pStream, pString->pBuffer, pString->length + 1); - } + if (errorCode == SBG_NO_ERROR) + { + errorCode = sbgStreamBufferWriteBuffer(pStream, pString->pBuffer, pString->length + 1); + } - return errorCode; + return errorCode; } //----------------------------------------------------------------------// @@ -1710,63 +1850,63 @@ SBG_COMMON_LIB_API SbgErrorCode sbgStringToStreamBuffer(const SbgString *pString SBG_COMMON_LIB_API void sbgStringIteratorConstruct(SbgStringIterator *pIterator, const SbgString *pString) { - assert(pString); - assert(pIterator); + assert(pString); + assert(pIterator); - pIterator->pCursor = pString->pBuffer; + pIterator->pCursor = pString->pBuffer; } SBG_COMMON_LIB_API SbgErrorCode sbgStringIteratorWalk(SbgStringIterator *pIterator, const char *pSeparators, bool skipEmptyTokens, SbgString *pToken) { - SbgErrorCode errorCode; - - assert(pIterator); - assert(pSeparators); - assert(pToken); - - if (pIterator->pCursor) - { - size_t length; - char *pSeparator; - char *pNext; - - pSeparator = strpbrk(pIterator->pCursor, pSeparators); - - if (pSeparator) - { - length = pSeparator - pIterator->pCursor; - - if (skipEmptyTokens) - { - size_t nrSeparators; - - nrSeparators = strspn(pSeparator, pSeparators); - pNext = &pSeparator[nrSeparators]; - } - else - { - pNext = &pSeparator[1]; - } - } - else - { - length = strlen(pIterator->pCursor); - pNext = NULL; - } - - errorCode = sbgStringAssignBuffer(pToken, pIterator->pCursor, length); - - if (errorCode == SBG_NO_ERROR) - { - pIterator->pCursor = pNext; - } - } - else - { - errorCode = SBG_NOT_READY; - } - - return errorCode; + SbgErrorCode errorCode; + + assert(pIterator); + assert(pSeparators); + assert(pToken); + + if (pIterator->pCursor) + { + size_t length; + char *pSeparator; + char *pNext; + + pSeparator = strpbrk(pIterator->pCursor, pSeparators); + + if (pSeparator) + { + length = pSeparator - pIterator->pCursor; + + if (skipEmptyTokens) + { + size_t nrSeparators; + + nrSeparators = strspn(pSeparator, pSeparators); + pNext = &pSeparator[nrSeparators]; + } + else + { + pNext = &pSeparator[1]; + } + } + else + { + length = strlen(pIterator->pCursor); + pNext = NULL; + } + + errorCode = sbgStringAssignBuffer(pToken, pIterator->pCursor, length); + + if (errorCode == SBG_NO_ERROR) + { + pIterator->pCursor = pNext; + } + } + else + { + errorCode = SBG_NOT_READY; + } + + return errorCode; } //----------------------------------------------------------------------// @@ -1775,66 +1915,63 @@ SBG_COMMON_LIB_API SbgErrorCode sbgStringIteratorWalk(SbgStringIterator *pIterat SBG_COMMON_LIB_API SbgErrorCode sbgStringCopy(char *pDestination, const char *pSource, size_t destMaxSize) { - size_t srcLength; - - // - // Check input arguments - // - assert(pDestination); - assert(pSource); - assert(destMaxSize > 0); - - // - // Get the source string length and add the NULL char - // - srcLength = strlen(pSource) + sizeof(char); - - // - // Make sure the string fits in the provided buffer - // - if (srcLength <= destMaxSize) - { - // - // We can use safely strcpy - // - strcpy(pDestination, pSource); - - return SBG_NO_ERROR; - } - else - { - // - // The destination buffer isn't big enough to hold the provided source string - // Don't forget to save one byte for the NULL terminated char - // - strncpy(pDestination, pSource, destMaxSize-sizeof(char)); - - // - // Append the NULL terminating char - // - pDestination[destMaxSize-1] = '\0'; - - // - // We have a buffer overflow - // - return SBG_BUFFER_OVERFLOW; - } + size_t srcLength; + + assert(pDestination); + assert(pSource); + assert(destMaxSize > 0); + + // + // Get the source string length and add the NULL char + // + srcLength = strlen(pSource) + sizeof(char); + + // + // Make sure the string fits in the provided buffer + // + if (srcLength <= destMaxSize) + { + // + // We can use safely strcpy + // + strcpy(pDestination, pSource); + + return SBG_NO_ERROR; + } + else + { + // + // The destination buffer isn't big enough to hold the provided source string + // Don't forget to save one byte for the NULL terminated char + // + strncpy(pDestination, pSource, destMaxSize-sizeof(char)); + + // + // Append the NULL terminating char + // + pDestination[destMaxSize-1] = '\0'; + + // + // We have a buffer overflow + // + return SBG_BUFFER_OVERFLOW; + } } SBG_COMMON_LIB_API const char *sbgStringFirstValidChar(const char *pInputStr) { - const char *pCurrentStr = pInputStr; + const char *pCurrentStr = pInputStr; - assert(pInputStr); + assert(pInputStr); - // - // Skip any space or tab chars from the beginning of the string - // - while ((*pCurrentStr != '\0') && isspace((unsigned char)*pCurrentStr) ) - { - pCurrentStr += sizeof(char); - } + // + // Skip any space or tab chars from the beginning of the string + // + while ((*pCurrentStr != '\0') && isspace((unsigned char)*pCurrentStr) ) + { + pCurrentStr += sizeof(char); + } - return pCurrentStr; + return pCurrentStr; } diff --git a/common/string/sbgString.h b/common/string/sbgString.h index c03bc19..dfd70b8 100644 --- a/common/string/sbgString.h +++ b/common/string/sbgString.h @@ -1,13 +1,13 @@ /*! - * \file sbgString.h - * \ingroup common - * \author SBG Systems - * \date March 20, 2020 + * \file sbgString.h + * \ingroup common + * \author SBG Systems + * \date March 20, 2020 * - * \brief Character string. + * \brief Character string. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -55,7 +55,7 @@ extern "C" { * This constant must be strictly positive in order to guarantee that no errors can occur * during the construction of an empty string. */ -#define SBG_STRING_INTERNAL_BUFFER_MIN_SIZE (4) +#define SBG_STRING_INTERNAL_BUFFER_MIN_SIZE (4) #if SBG_STRING_INTERNAL_BUFFER_MIN_SIZE == 0 #error "invalid minimum internal buffer size" @@ -64,10 +64,10 @@ extern "C" { /*! * Default size of the internal buffer, in bytes. */ -#define SBG_STRING_INTERNAL_BUFFER_DEFAULT_SIZE (16) +#define SBG_STRING_INTERNAL_BUFFER_DEFAULT_SIZE (16) #ifndef SBG_CONFIG_STRING_INTERNAL_BUFFER_SIZE -#define SBG_CONFIG_STRING_INTERNAL_BUFFER_SIZE SBG_STRING_INTERNAL_BUFFER_DEFAULT_SIZE +#define SBG_CONFIG_STRING_INTERNAL_BUFFER_SIZE SBG_STRING_INTERNAL_BUFFER_DEFAULT_SIZE #elif SBG_CONFIG_STRING_INTERNAL_BUFFER_SIZE < SBG_STRING_INTERNAL_BUFFER_MIN_SIZE #error "invalid internal buffer size" #endif // SBG_CONFIG_STRING_INTERNAL_BUFFER_SIZE @@ -89,21 +89,21 @@ extern "C" { */ typedef struct _SbgString { - /*! - * Internal buffer. - * - * The internal buffer is used for small strings to avoid dynamic allocation. - */ - char internalBuffer[SBG_CONFIG_STRING_INTERNAL_BUFFER_SIZE]; + /*! + * Internal buffer. + * + * The internal buffer is used for small strings to avoid dynamic allocation. + */ + char internalBuffer[SBG_CONFIG_STRING_INTERNAL_BUFFER_SIZE]; - char *pBuffer; /*!< Buffer. */ - size_t capacity; /*!< Capacity of the buffer, in bytes. */ - size_t length; /*!< Length of the string (not including the terminating null character), in bytes. */ + char *pBuffer; /*!< Buffer. */ + size_t capacity; /*!< Capacity of the buffer, in bytes. */ + size_t length; /*!< Length of the string (not including the terminating null character), in bytes. */ - bool readOnly; /*!< True if the string is read-only. */ - bool isStatic; /*!< True if the buffer is statically allocated. */ + bool readOnly; /*!< True if the string is read-only. */ + bool isStatic; /*!< True if the buffer is statically allocated. */ - SbgErrorCode errorCode; /*!< Error code of the last modification operation. */ + SbgErrorCode errorCode; /*!< Error code of the last modification operation. */ } SbgString; /*! @@ -111,7 +111,7 @@ typedef struct _SbgString */ typedef struct _SbgStringIterator { - const char *pCursor; /*!< Cursor. */ + const char *pCursor; /*!< Cursor. */ } SbgStringIterator; //----------------------------------------------------------------------// @@ -124,7 +124,7 @@ typedef struct _SbgStringIterator * The rationale for this constructor is to guarantee that no errors may occur on the * construction of empty strings. * - * \param[in] pString String. + * \param[in] pString String. */ SBG_COMMON_LIB_API void sbgStringConstructEmpty(SbgString *pString); @@ -134,9 +134,9 @@ SBG_COMMON_LIB_API void sbgStringConstructEmpty(SbgString *pString); * The string content is initialized from the given source string. If that string is NULL, * the string is initialized with an empty content. * - * \param[in] pString String. - * \param[in] pSourceString Source string, may be NULL. - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \param[in] pSourceString Source string, may be NULL. + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringConstruct(SbgString *pString, const SbgString *pSourceString); @@ -145,9 +145,9 @@ SBG_COMMON_LIB_API SbgErrorCode sbgStringConstruct(SbgString *pString, const Sbg * * The string content is initialized from the given C null-terminated string. * - * \param[in] pString String. - * \param[in] pCString C null-terminated string. - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \param[in] pCString C null-terminated string. + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringConstructCString(SbgString *pString, const char *pCString); @@ -156,10 +156,10 @@ SBG_COMMON_LIB_API SbgErrorCode sbgStringConstructCString(SbgString *pString, co * * The string content is initialized from formatted data from a variable argument list. * - * \param[in] pString String. - * \param[in] pFormat Format. - * \param[in] args Variable argument list. - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \param[in] pFormat Format. + * \param[in] args Variable argument list. + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringConstructVF(SbgString *pString, const char *pFormat, va_list args) SBG_CHECK_FORMAT(printf, 2, 0); @@ -168,9 +168,9 @@ SBG_COMMON_LIB_API SbgErrorCode sbgStringConstructVF(SbgString *pString, const c * * The string content is initialized from formatted data. * - * \param[in] pString String. - * \param[in] pFormat Format. - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \param[in] pFormat Format. + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringConstructF(SbgString *pString, const char *pFormat, ...) SBG_CHECK_FORMAT(printf, 2, 3); @@ -187,11 +187,11 @@ SBG_COMMON_LIB_API SbgErrorCode sbgStringConstructF(SbgString *pString, const ch * After destroying the string, the buffer contains the content of the string immediately before * destruction. * - * \param[in] pString String. - * \param[in] pCString C null-terminated string, may be NULL. - * \param[in] pBuffer Buffer. - * \param[in] size Buffer size, in bytes. - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \param[in] pCString C null-terminated string, may be NULL. + * \param[in] pBuffer Buffer. + * \param[in] size Buffer size, in bytes. + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringConstructStatic(SbgString *pString, const char *pCString, char *pBuffer, size_t size); @@ -203,111 +203,113 @@ SBG_COMMON_LIB_API SbgErrorCode sbgStringConstructStatic(SbgString *pString, con * The given C null-terminated string provides both the statically allocated buffer * for the string, as well as its content. It is owned by the string and must not be modified. * - * \param[in] pString String. - * \param[in] pCString C null-terminated string. + * \param[in] pString String. + * \param[in] pCString C null-terminated string. */ SBG_COMMON_LIB_API void sbgStringConstructReadOnly(SbgString *pString, const char *pCString); /*! * String destructor. * - * \param[in] pString String. + * \param[in] pString String. */ SBG_COMMON_LIB_API void sbgStringDestroy(SbgString *pString); /*! * Compare a string to another string. * - * \param[in] pString String. - * \param[in] pOtherString Other string. - * \return The return value is 0 if both strings are equal, - * less than 0 if the string value is less than the other string value, - * greater than 0 if the string value is greater than the other string value. + * \param[in] pString String. + * \param[in] pOtherString Other string. + * \return The return value is 0 if both strings are equal, + * less than 0 if the string value is less than the other string value, + * greater than 0 if the string value is greater than the other string value. */ SBG_COMMON_LIB_API int32_t sbgStringCompare(const SbgString *pString, const SbgString *pOtherString); /*! * Compare a string to a C null-terminated string. * - * \param[in] pString String. - * \param[in] pCString C null-terminated string. - * \return The return value is 0 if the string is equal to the C null-terminated string, - * less than 0 if the string value is less than the C null-terminated string value, - * greater than 0 if the string value is greater than the C null-terminated string value. + * \param[in] pString String. + * \param[in] pCString C null-terminated string. + * \return The return value is 0 if the string is equal to the C null-terminated string, + * less than 0 if the string value is less than the C null-terminated string value, + * greater than 0 if the string value is greater than the C null-terminated string value. */ SBG_COMMON_LIB_API int32_t sbgStringCompareCString(const SbgString *pString, const char *pCString); /*! * Compare a string to another string, ignoring case differences. * - * \param[in] pString String. - * \param[in] pOtherString Other string. - * \return The return value is 0 if both strings are equal, - * less than 0 if the string value is less than the other string value, - * greater than 0 if the string value is greater than the other string value. + * \param[in] pString String. + * \param[in] pOtherString Other string. + * \return The return value is 0 if both strings are equal, + * less than 0 if the string value is less than the other string value, + * greater than 0 if the string value is greater than the other string value. */ SBG_COMMON_LIB_API int32_t sbgStringCompareIgnoreCase(const SbgString *pString, const SbgString *pOtherString); /*! * Compare a string to a C null-terminated string, ignoring case differences. * - * \param[in] pString String. - * \param[in] pCString C null-terminated string. - * \return The return value is 0 if the string is equal to the C null-terminated string, - * less than 0 if the string value is less than the C null-terminated string value, - * greater than 0 if the string value is greater than the C null-terminated string value. + * \param[in] pString String. + * \param[in] pCString C null-terminated string. + * \return The return value is 0 if the string is equal to the C null-terminated string, + * less than 0 if the string value is less than the C null-terminated string value, + * greater than 0 if the string value is greater than the C null-terminated string value. */ SBG_COMMON_LIB_API int32_t sbgStringCompareIgnoreCaseCString(const SbgString *pString, const char *pCString); /*! * Compute the hash value of a string. * - * \param[in] pString String. - * \return Hash value. + * \param[in] pString String. + * \return Hash value. */ SBG_COMMON_LIB_API size_t sbgStringHash(const SbgString *pString); /*! * Get the length of a string (not including the terminating null character), in bytes. * - * \param[in] pString String. - * \return Length, in bytes. + * \param[in] pString String. + * \return Length, in bytes. */ SBG_COMMON_LIB_API size_t sbgStringGetLength(const SbgString *pString); /*! * Get a C null-terminated string version of a string. * - * \param[in] pString String. - * \return C null-terminated string. + * The pointer returned remains valid until the next modification operation on the string. + * + * \param[in] pString String. + * \return C null-terminated string. */ SBG_COMMON_LIB_API const char *sbgStringGetCString(const SbgString *pString); /*! * Get a character in a string. * - * \param[in] pString String. - * \param[in] index Index. - * \param[out] pChar Character. - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \param[in] index Index. + * \param[out] pChar Character. + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringCharAt(const SbgString *pString, size_t index, char *pChar); /*! * Get the index of the first occurrence of a string in a string. * - * \param[in] pString String. - * \param[in] pStringToFind String to find. - * \return Index of the first occurrence or SIZE_MAX if not found. + * \param[in] pString String. + * \param[in] pStringToFind String to find. + * \return Index of the first occurrence or SIZE_MAX if not found. */ SBG_COMMON_LIB_API size_t sbgStringFind(const SbgString *pString, const SbgString *pStringToFind); /*! * Get the index of the first occurrence of the given C null-terminated string in a string. * - * \param[in] pString String. - * \param[in] pCString C null-terminated string. - * \return Index of the first occurrence or SIZE_MAX if not found. + * \param[in] pString String. + * \param[in] pCString C null-terminated string. + * \return Index of the first occurrence or SIZE_MAX if not found. */ SBG_COMMON_LIB_API size_t sbgStringFindCString(const SbgString *pString, const char *pCString); @@ -317,29 +319,29 @@ SBG_COMMON_LIB_API size_t sbgStringFindCString(const SbgString *pString, const c * The substring must be constructed before calling this function. If an error occurs, the last error * of the substring is set by this function. * - * \param[in] pString String. - * \param[in] startIndex Start index. - * \param[in] endIndex End index. - * \param[out] pSubstring Substring. - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \param[in] startIndex Start index. + * \param[in] endIndex End index. + * \param[out] pSubstring Substring. + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringSubstring(const SbgString *pString, size_t startIndex, size_t endIndex, SbgString *pSubstring); /*! * Check if a C null-terminated string is at the beginning of a string. * - * \param[in] pString String. - * \param[in] pCString C null-terminated string. - * \return True if the C null-terminated string is at the beginning. + * \param[in] pString String. + * \param[in] pCString C null-terminated string. + * \return True if the C null-terminated string is at the beginning. */ SBG_COMMON_LIB_API bool sbgStringStartsWith(const SbgString *pString, const char *pCString); /*! * Check if a C null-terminated string is at the end of a string. * - * \param[in] pString String. - * \param[in] pCString C null-terminated string. - * \return True if the C null-terminated string is at the end. + * \param[in] pString String. + * \param[in] pCString C null-terminated string. + * \return True if the C null-terminated string is at the end. */ SBG_COMMON_LIB_API bool sbgStringEndsWith(const SbgString *pString, const char *pCString); @@ -350,15 +352,15 @@ SBG_COMMON_LIB_API bool sbgStringEndsWith(const SbgString *pString, const char * /*! * Get the last error of a modification operation on a string. * - * \param[in] pString String. - * \return Last modification error. + * \param[in] pString String. + * \return Last modification error. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringGetLastError(const SbgString *pString); /*! * Clear the last modification operation error of a string. * - * \param[in] pString String. + * \param[in] pString String. */ SBG_COMMON_LIB_API void sbgStringClearLastError(SbgString *pString); @@ -368,28 +370,28 @@ SBG_COMMON_LIB_API void sbgStringClearLastError(SbgString *pString); * The given index must be strictly lower than the string length. The given character * must not be a null character. * - * \param[in] pString String. - * \param[in] index Index. - * \param[in] c Character. - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \param[in] index Index. + * \param[in] c Character. + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringSetCharAt(SbgString *pString, size_t index, char c); /*! * Append a string to another string. * - * \param[in] pString String. - * \param[in] pAppendString String to append. - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \param[in] pAppendString String to append. + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringAppend(SbgString *pString, const SbgString *pAppendString); /*! * Append a C null-terminated string to a string. * - * \param[in] pString String. - * \param[in] pCString C null-terminated string. - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \param[in] pCString C null-terminated string. + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringAppendCString(SbgString *pString, const char *pCString); @@ -398,10 +400,10 @@ SBG_COMMON_LIB_API SbgErrorCode sbgStringAppendCString(SbgString *pString, const * * The string format, as well as the variable argument list, are the same as for vprintf. * - * \param[in] pString String. - * \param[in] pFormat Format. - * \param[in] args Variable argument list. - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \param[in] pFormat Format. + * \param[in] args Variable argument list. + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringAppendVF(SbgString *pString, const char *pFormat, va_list args) SBG_CHECK_FORMAT(printf, 2, 0); @@ -410,27 +412,27 @@ SBG_COMMON_LIB_API SbgErrorCode sbgStringAppendVF(SbgString *pString, const char * * The string format, as well as the variable arguments, are the same as for printf. * - * \param[in] pString String. - * \param[in] pFormat Format. - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \param[in] pFormat Format. + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringAppendF(SbgString *pString, const char *pFormat, ...) SBG_CHECK_FORMAT(printf, 2, 3); /*! * Assign a string to another string. * - * \param[in] pString String. - * \param[in] pAssignString String to assign. - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \param[in] pAssignString String to assign. + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringAssign(SbgString *pString, const SbgString *pAssignString); /*! * Assign a C null-terminated string to a string. * - * \param[in] pString String. - * \param[in] pCString C null-terminated string. - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \param[in] pCString C null-terminated string. + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringAssignCString(SbgString *pString, const char *pCString); @@ -439,10 +441,10 @@ SBG_COMMON_LIB_API SbgErrorCode sbgStringAssignCString(SbgString *pString, const * * The string format, as well as the variable argument list, are the same as for vprintf. * - * \param[in] pString String. - * \param[in] pFormat Format. - * \param[in] args Variable argument list. - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \param[in] pFormat Format. + * \param[in] args Variable argument list. + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringAssignVF(SbgString *pString, const char *pFormat, va_list args) SBG_CHECK_FORMAT(printf, 2, 0); @@ -451,59 +453,95 @@ SBG_COMMON_LIB_API SbgErrorCode sbgStringAssignVF(SbgString *pString, const char * * The string format, as well as the variable arguments, are the same as for printf. * - * \param[in] pString String. - * \param[in] pFormat Format. - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \param[in] pFormat Format. + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringAssignF(SbgString *pString, const char *pFormat, ...) SBG_CHECK_FORMAT(printf, 2, 3); +/*! + * Move content between two strings. + * + * On return, if successful, the source string is empty. + * + * \param[in] pString String. + * \param[in] pSourceString Source string. + * \return SBG_NO_ERROR if successful. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgStringMove(SbgString *pString, SbgString *pSourceString); + +/*! + * Move a dynamically allocated C null-terminated string into a string. + * + * The C string must have been allocated using a standard C allocation function, + * such as malloc() or strdup(). + * + * On return, if successful, the C string buffer is owned by the string. + * + * \param[in] pString String. + * \param[in] pCString C null-terminated string. + * \return SBG_NO_ERROR if successful. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgStringMoveCString(SbgString *pString, char *pCString); + +/*! + * Extract the content of a string. + * + * If successful, the content returned is a C null-terminated string, becomes owned by + * the caller, may be released with free(), and the given string becomes empty. + * + * \param[in] pString String. + * \return C null-terminated string, NULL if an error occurred. + */ +SBG_COMMON_LIB_API char * sbgStringExtract(SbgString *pString); + /*! * Clear a string. * * After returning from this call the string is empty. * - * \param[in] pString String. - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringClear(SbgString *pString); /*! * Convert all the characters of a string to upper case. * - * \param[in] pString String. - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringToUpperCase(SbgString *pString); /*! * Convert all the characters of a string to lower case. * - * \param[in] pString String. - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringToLowerCase(SbgString *pString); /*! * Remove all spacing characters at the beginning of a string. * - * \param[in] pString String. - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringTrimLeft(SbgString *pString); /*! * Remove all spacing characters at the end of a string. * - * \param[in] pString String. - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringTrimRight(SbgString *pString); /*! * Remove all spacing characters at the beginning and the end of a string. * - * \param[in] pString String. - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringTrim(SbgString *pString); @@ -513,8 +551,8 @@ SBG_COMMON_LIB_API SbgErrorCode sbgStringTrim(SbgString *pString); * If the given length is greater than or equal to the length of the string, * this function doesn't modify the string. * - * \param[in] pString String. - * \param[in] length Length, in bytes. + * \param[in] pString String. + * \param[in] length Length, in bytes. */ SBG_COMMON_LIB_API void sbgStringTruncate(SbgString *pString, size_t length); @@ -525,198 +563,198 @@ SBG_COMMON_LIB_API void sbgStringTruncate(SbgString *pString, size_t length); /*! * Convert and assign a 8-bits signed integer value to a string. * - * \param[in] pString String. - * \param[in] value Value. - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \param[in] value Value. + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringFromInt8(SbgString *pString, int8_t value); /*! * Convert a string to a 8-bits signed integer value. * - * \param[in] pString String. - * \param[out] pValue Value. - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \param[out] pValue Value. + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringToInt8(const SbgString *pString, int8_t *pValue); /*! * Convert and assign a 8-bits unsigned integer value to a string. * - * \param[in] pString String. - * \param[in] value Value - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \param[in] value Value + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringFromUint8(SbgString *pString, uint8_t value); /*! * Convert a string to a 8-bits unsigned integer value. * - * \param[in] pString String. - * \param[out] pValue Value. - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \param[out] pValue Value. + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringToUint8(const SbgString *pString, uint8_t *pValue); /*! * Convert and assign a 16-bits signed integer value to a string. * - * \param[in] pString String. - * \param[in] value Value. - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \param[in] value Value. + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringFromInt16(SbgString *pString, int16_t value); /*! * Convert a string to a 16-bits signed integer value. * - * \param[in] pString String. - * \param[out] pValue Value. - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \param[out] pValue Value. + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringToInt16(const SbgString *pString, int16_t *pValue); /*! * Convert and assign a 16-bits unsigned integer value to a string. * - * \param[in] pString String. - * \param[in] value Value - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \param[in] value Value + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringFromUint16(SbgString *pString, uint16_t value); /*! * Convert a string to a 16-bits unsigned integer value. * - * \param[in] pString String. - * \param[out] pValue Value. - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \param[out] pValue Value. + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringToUint16(const SbgString *pString, uint16_t *pValue); /*! * Convert and assign a 32-bits signed integer value to a string. * - * \param[in] pString String. - * \param[in] value Value. - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \param[in] value Value. + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringFromInt32(SbgString *pString, int32_t value); /*! * Convert a string to a 32-bits signed integer value. * - * \param[in] pString String. - * \param[out] pValue Value. - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \param[out] pValue Value. + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringToInt32(const SbgString *pString, int32_t *pValue); /*! * Convert and assign a 32-bits unsigned integer value to a string. * - * \param[in] pString String. - * \param[in] value Value - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \param[in] value Value + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringFromUint32(SbgString *pString, uint32_t value); /*! * Convert a string to a 32-bits unsigned integer value. * - * \param[in] pString String. - * \param[out] pValue Value. - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \param[out] pValue Value. + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringToUint32(const SbgString *pString, uint32_t *pValue); /*! * Convert and assign a 64-bits signed integer value to a string. * - * \param[in] pString String. - * \param[in] value Value. - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \param[in] value Value. + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringFromInt64(SbgString *pString, int64_t value); /*! * Convert a string to a 64-bits signed integer value. * - * \param[in] pString String. - * \param[out] pValue Value. - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \param[out] pValue Value. + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringToInt64(const SbgString *pString, int64_t *pValue); /*! * Convert and assign a 64-bits unsigned integer value to a string. * - * \param[in] pString String. - * \param[in] value Value - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \param[in] value Value + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringFromUint64(SbgString *pString, uint64_t value); /*! * Convert a string to a 64-bits unsigned integer value. * - * \param[in] pString String. - * \param[out] pValue Value. - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \param[out] pValue Value. + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringToUint64(const SbgString *pString, uint64_t *pValue); /*! * Convert and assign a float value to a string. * - * \param[in] pString String. - * \param[in] value Value. - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \param[in] value Value. + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringFromFloat(SbgString *pString, float value); /*! * Convert a string to a float value. * - * \param[in] pString String. - * \param[out] pValue Value. - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \param[out] pValue Value. + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringToFloat(const SbgString *pString, float *pValue); /*! * Convert and assign a double value to a string. * - * \param[in] pString String. - * \param[in] value Value. - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \param[in] value Value. + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringFromDouble(SbgString *pString, double value); /*! * Convert a string to a double value. * - * \param[in] pString String. - * \param[out] pValue Value. - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \param[out] pValue Value. + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringToDouble(const SbgString *pString, double *pValue); /*! * Read a string from a stream buffer. * - * \param[in] pString String. - * \param[in] pStream Stream buffer. - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \param[in] pStream Stream buffer. + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringFromStreamBuffer(SbgString *pString, SbgStreamBuffer *pStream); /*! * Write a string to a stream buffer. * - * \param[in] pString String. - * \param[out] pStream Stream buffer. - * \return SBG_NO_ERROR if successful. + * \param[in] pString String. + * \param[out] pStream Stream buffer. + * \return SBG_NO_ERROR if successful. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringToStreamBuffer(const SbgString *pString, SbgStreamBuffer *pStream); @@ -729,8 +767,8 @@ SBG_COMMON_LIB_API SbgErrorCode sbgStringToStreamBuffer(const SbgString *pString * * If the given string is modified while the iterator is used, the behavior is undefined. * - * \param[in] pIterator String iterator. - * \param[in] pString String. + * \param[in] pIterator String iterator. + * \param[in] pString String. */ SBG_COMMON_LIB_API void sbgStringIteratorConstruct(SbgStringIterator *pIterator, const SbgString *pString); @@ -740,12 +778,12 @@ SBG_COMMON_LIB_API void sbgStringIteratorConstruct(SbgStringIterator *pIterator, * The token string must be constructed before calling this function. If an error occurs, the last error * of the token string is set by this function. * - * \param[in] pIterator String iterator. - * \param[in] pSeparators C null-terminated string containing separator characters. - * \param[in] skipEmptyTokens If true, empty tokens are skipped. - * \param[out] pToken Token. - * \return SBG_NO_ERROR if successful, - * SBG_NOT_READY if there are no more tokens. + * \param[in] pIterator String iterator. + * \param[in] pSeparators C null-terminated string containing separator characters. + * \param[in] skipEmptyTokens If true, empty tokens are skipped. + * \param[out] pToken Token. + * \return SBG_NO_ERROR if successful, + * SBG_NOT_READY if there are no more tokens. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringIteratorWalk(SbgStringIterator *pIterator, const char *pSeparators, bool skipEmptyTokens, SbgString *pToken); @@ -760,19 +798,19 @@ SBG_COMMON_LIB_API SbgErrorCode sbgStringIteratorWalk(SbgStringIterator *pIterat * If the provided output buffer is too small, the method will return a valid NULL terminated C string in destination. * This condition will be returned using SBG_BUFFER_OVERFLOW error code. * - * \param[out] pDestination The destination string that should hold the copied input string. - * \param[in] pSource The source NULL terminated C string to copy to the destination. - * \param[in] destMaxSize The destination buffer size including the null terminating char. - * \return SBG_NO_ERROR if the source string has been copied to destination - * SBG_BUFFER_OVERFLOW if the destination is too small to hold the source string. + * \param[out] pDestination The destination string that should hold the copied input string. + * \param[in] pSource The source NULL terminated C string to copy to the destination. + * \param[in] destMaxSize The destination buffer size including the null terminating char. + * \return SBG_NO_ERROR if the source string has been copied to destination + * SBG_BUFFER_OVERFLOW if the destination is too small to hold the source string. */ SBG_COMMON_LIB_API SbgErrorCode sbgStringCopy(char *pDestination, const char *pSource, size_t destMaxSize); /*! * Skip all spacing characters from the beginning of a string. * - * \param[in] pInputStr Pointer on a read only string to trim. - * \return Pointer to the first useful char. + * \param[in] pInputStr Pointer on a read only string to trim. + * \return Pointer to the first useful char. */ SBG_COMMON_LIB_API const char *sbgStringFirstValidChar(const char *pInputStr); diff --git a/common/swap/sbgSwap.h b/common/swap/sbgSwap.h index 72026bd..8785c3d 100644 --- a/common/swap/sbgSwap.h +++ b/common/swap/sbgSwap.h @@ -1,13 +1,13 @@ /*! - * \file sbgSwap.h - * \ingroup common - * \author SBG Systems - * \date 14 January 2013 + * \file sbgSwap.h + * \ingroup common + * \author SBG Systems + * \date 14 January 2013 * - * \brief Set of functions used to swap numbers. + * \brief Set of functions used to swap numbers. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -47,95 +47,96 @@ extern "C" { //----------------------------------------------------------------------// /*! - * Swap a uint16_t number. - * - * \param[in] x The uint16_t to swap. - * \return The swapped value. + * 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)); + return ((x<<8)|(x>>8)); } /*! - * Swap a uint32_t number. - * \param[in] x The uint32_t to swap. - * \return The swapped value. + * 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)); + 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. + * 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; + 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. + * \param[in] val The float to swap. + * \return The swapped value. */ SBG_INLINE float sbgSwapFloat(float val) { - SbgFloatNint 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; + SbgFloatNint 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. + * \param[in] val The double to swap. + * \return The swapped value. */ SBG_INLINE double sbgSwapDouble(double val) { - SbgDoubleNint 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; + SbgDoubleNint 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; } //----------------------------------------------------------------------// @@ -145,4 +146,4 @@ SBG_INLINE double sbgSwapDouble(double val) } #endif -#endif /* SBG_SWAP_H */ +#endif // SBG_SWAP_H diff --git a/common/version/sbgVersion.c b/common/version/sbgVersion.c index a24c339..a7e9a76 100644 --- a/common/version/sbgVersion.c +++ b/common/version/sbgVersion.c @@ -5,55 +5,105 @@ // Local headers #include "sbgVersion.h" +//----------------------------------------------------------------------// +//- Private methods -// +//----------------------------------------------------------------------// + +/*! + * Convert a qualifier from a string to the corresponding enum value. + * + * \param[in] pString The string to convert. + * \param[out] pValue Converted value. + * \return SBG_NO_ERROR if converted successfully. + */ +static SbgErrorCode sbgVersionQualifierFromString(const char *pString, SbgVersionQualifier *pValue) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + + assert(pString); + assert(pValue); + + if (!strcmp(pString, "dev")) + { + *pValue = SBG_VERSION_QUALIFIER_DEV; + } + else if (!strcmp(pString, "alpha")) + { + *pValue = SBG_VERSION_QUALIFIER_ALPHA; + } + else if (!strcmp(pString, "beta")) + { + *pValue = SBG_VERSION_QUALIFIER_BETA; + } + else if (!strcmp(pString, "rc")) + { + *pValue = SBG_VERSION_QUALIFIER_RC; + } + else if (!strcmp(pString, "stable")) + { + *pValue = SBG_VERSION_QUALIFIER_STABLE; + } + else if (!strcmp(pString, "hotfix")) + { + *pValue = SBG_VERSION_QUALIFIER_HOT_FIX; + } + else + { + errorCode = SBG_INVALID_PARAMETER; + } + + return errorCode; +} + //----------------------------------------------------------------------// //- Constructor -// //----------------------------------------------------------------------// SBG_COMMON_LIB_API void sbgVersionCreateBasic(SbgVersion *pVersion, uint8_t major, uint8_t minor, uint8_t revision, uint8_t build) { - assert(pVersion); - assert(major < 128); + assert(pVersion); + assert(major <= 127); - pVersion->softwareScheme = false; + pVersion->softwareScheme = false; - pVersion->major = major; - pVersion->minor = minor; - pVersion->rev = revision; - pVersion->build = build; + pVersion->major = major; + pVersion->minor = minor; + pVersion->rev = revision; + pVersion->build = build; - pVersion->qualifier = SBG_VERSION_QUALIFIER_DEV; + pVersion->qualifier = SBG_VERSION_QUALIFIER_DEV; } SBG_COMMON_LIB_API void sbgVersionCreateSoftware(SbgVersion *pVersion, uint8_t major, uint8_t minor, uint16_t build, SbgVersionQualifier qualifier) { - assert(pVersion); - assert(major < 64); - assert(minor < 64); + assert(pVersion); + assert(major <= 63); + assert(minor <= 63); - pVersion->softwareScheme = true; + pVersion->softwareScheme = true; - pVersion->major = major; - pVersion->minor = minor; - pVersion->build = build; - pVersion->qualifier = qualifier; + pVersion->major = major; + pVersion->minor = minor; + pVersion->build = build; + pVersion->qualifier = qualifier; - pVersion->rev = 0; + pVersion->rev = 0; } SBG_COMMON_LIB_API void sbgVersionConstructCopy(SbgVersion *pVersion, const SbgVersion *pOtherVersion) { - assert(pVersion); - assert(pOtherVersion); - - // - // Copy each member individually to avoid warnings about reading uninitialized bytes. - // - pVersion->softwareScheme = pOtherVersion->softwareScheme; - pVersion->qualifier = pOtherVersion->qualifier; - pVersion->major = pOtherVersion->major; - pVersion->minor = pOtherVersion->minor; - pVersion->rev = pOtherVersion->rev; - pVersion->build = pOtherVersion->build; + assert(pVersion); + assert(pOtherVersion); + + // + // Copy each member individually to avoid warnings about reading uninitialized bytes. + // + pVersion->softwareScheme = pOtherVersion->softwareScheme; + pVersion->qualifier = pOtherVersion->qualifier; + pVersion->major = pOtherVersion->major; + pVersion->minor = pOtherVersion->minor; + pVersion->rev = pOtherVersion->rev; + pVersion->build = pOtherVersion->build; } //----------------------------------------------------------------------// @@ -62,88 +112,91 @@ SBG_COMMON_LIB_API void sbgVersionConstructCopy(SbgVersion *pVersion, const SbgV SBG_COMMON_LIB_API void sbgVersionDecode(uint32_t encodedVersion, SbgVersion *pVersionInfo) { - 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; - } + 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; + } } SBG_COMMON_LIB_API uint32_t sbgVersionEncode(const SbgVersion *pVersionInfo) { - uint32_t encodedVersion = 0; - - 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 validity - // - assert(pVersionInfo->major <= 127); - - // - // Encode the basic version information - // - encodedVersion = SBG_VERSION_BASIC(pVersionInfo->major, pVersionInfo->minor, pVersionInfo->rev, pVersionInfo->build); - } - - return encodedVersion; + uint32_t encodedVersion = 0; + + 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); + assert(pVersionInfo->minor <= 63); + assert(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 validity + // + assert(pVersionInfo->major <= 127); + assert(pVersionInfo->build <= UINT8_MAX); + + // + // Encode the basic version information + // + encodedVersion = SBG_VERSION_BASIC(pVersionInfo->major, pVersionInfo->minor, pVersionInfo->rev, pVersionInfo->build); + } + + return encodedVersion; } //----------------------------------------------------------------------// @@ -152,150 +205,144 @@ SBG_COMMON_LIB_API uint32_t sbgVersionEncode(const SbgVersion *pVersionInfo) SBG_COMMON_LIB_API int32_t sbgVersionCompare(const SbgVersion *pVersionA, const SbgVersion *pVersionB, SbgVersionCmpThreshold threshold) { - int32_t result; - - assert(pVersionA); - assert(pVersionB); - - // - // Check that the two versions are using the same scheme - // - assert(pVersionA->softwareScheme == pVersionB->softwareScheme); - - // - // Do the comparaison according to the selected threshold - // Start by compairing the major field - // - result = pVersionA->major - pVersionB->major; - - // - // Test if we have to also compare the minor field - // - if ( (result == 0) && ((threshold == SBG_VERSION_CMP_THRESHOLD_MINOR) || (threshold == SBG_VERSION_CMP_THRESHOLD_REVISION) || (threshold == SBG_VERSION_CMP_THRESHOLD_BUILD) || (threshold == SBG_VERSION_CMP_THRESHOLD_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) && ((threshold == SBG_VERSION_CMP_THRESHOLD_REVISION) || (threshold == SBG_VERSION_CMP_THRESHOLD_BUILD) || (threshold == SBG_VERSION_CMP_THRESHOLD_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) && ((threshold == SBG_VERSION_CMP_THRESHOLD_BUILD) || (threshold == SBG_VERSION_CMP_THRESHOLD_QUALIFIER)) ) - { - // - // Compare the build field - // - result = pVersionA->build - pVersionB->build; - - // - // Test if we have to also compare the qualifier field - // - if ( (result == 0) && (threshold == SBG_VERSION_CMP_THRESHOLD_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; + int32_t result; + + assert(pVersionA); + assert(pVersionB); + + // + // Check that the two versions are using the same scheme + // + assert(pVersionA->softwareScheme == pVersionB->softwareScheme); + + // + // Do the comparaison according to the selected threshold + // Start by compairing the major field + // + result = pVersionA->major - pVersionB->major; + + // + // Test if we have to also compare the minor field + // + if ( (result == 0) && ((threshold == SBG_VERSION_CMP_THRESHOLD_MINOR) || (threshold == SBG_VERSION_CMP_THRESHOLD_REVISION) || (threshold == SBG_VERSION_CMP_THRESHOLD_BUILD) || (threshold == SBG_VERSION_CMP_THRESHOLD_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) && ((threshold == SBG_VERSION_CMP_THRESHOLD_REVISION) || (threshold == SBG_VERSION_CMP_THRESHOLD_BUILD) || (threshold == SBG_VERSION_CMP_THRESHOLD_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) && ((threshold == SBG_VERSION_CMP_THRESHOLD_BUILD) || (threshold == SBG_VERSION_CMP_THRESHOLD_QUALIFIER)) ) + { + // + // Compare the build field + // + result = pVersionA->build - pVersionB->build; + + // + // Test if we have to also compare the qualifier field + // + if ( (result == 0) && (threshold == SBG_VERSION_CMP_THRESHOLD_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; } SBG_COMMON_LIB_API int32_t sbgVersionCompareEncoded(uint32_t versionA, uint32_t versionB, SbgVersionCmpThreshold threshold) { - SbgVersion versionAInfo; - SbgVersion versionBInfo; - - // - // Decode the versions - // - sbgVersionDecode(versionA, &versionAInfo); - sbgVersionDecode(versionB, &versionBInfo); - - // - // Do the comparaison - // - return sbgVersionCompare(&versionAInfo, &versionBInfo, threshold); + SbgVersion versionAInfo; + SbgVersion versionBInfo; + + sbgVersionDecode(versionA, &versionAInfo); + sbgVersionDecode(versionB, &versionBInfo); + + return sbgVersionCompare(&versionAInfo, &versionBInfo, threshold); } 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)); + assert(pLowerVersion); + assert(pHigherVersion); + assert(pVersion); + + // + // Use the encoded version to speed up the comparaison + // + return sbgVersionIsWithinRangeEncoded(sbgVersionEncode(pLowerVersion), sbgVersionEncode(pHigherVersion), sbgVersionEncode(pVersion)); } 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; - } + // + // 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; + } } //----------------------------------------------------------------------// @@ -304,105 +351,87 @@ SBG_COMMON_LIB_API int32_t sbgVersionIsWithinRangeEncoded(uint32_t lowerVersion, SBG_COMMON_LIB_API SbgErrorCode sbgVersionToString(const SbgVersion *pVersionInfo, char *pBuffer, size_t sizeOfBuffer) { - SbgErrorCode errorCode = SBG_NO_ERROR; - - assert(pVersionInfo); - assert(pBuffer); - assert(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; + SbgErrorCode errorCode = SBG_NO_ERROR; + + assert(pVersionInfo); + assert(pBuffer); + assert(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 + { + 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) + { + sprintf(pBuffer, "%u.%u.%u.%u", pVersionInfo->major, pVersionInfo->minor, pVersionInfo->rev, pVersionInfo->build); + } + else + { + errorCode = SBG_BUFFER_OVERFLOW; + } + } + + return errorCode; } SBG_COMMON_LIB_API SbgErrorCode sbgVersionToStringEncoded(uint32_t version, char *pBuffer, size_t sizeOfBuffer) { - SbgVersion versionInfo; + SbgVersion versionInfo; - // - // Decode the versions - // - sbgVersionDecode(version, &versionInfo); + sbgVersionDecode(version, &versionInfo); - // - // Return the version as a string - // - return sbgVersionToString(&versionInfo, pBuffer, sizeOfBuffer); + return sbgVersionToString(&versionInfo, pBuffer, sizeOfBuffer); } //----------------------------------------------------------------------// @@ -411,151 +440,155 @@ SBG_COMMON_LIB_API SbgErrorCode sbgVersionToStringEncoded(uint32_t version, char SBG_COMMON_LIB_API SbgErrorCode sbgVersionFromString(const char *pVersionStr, SbgVersion *pVersionInfo) { - SbgErrorCode errorCode = SBG_NO_ERROR; - char qualifierStr[32]; - unsigned int major; - unsigned int minor; - unsigned int rev; - unsigned int build; - - assert(pVersionStr); - assert(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; + SbgErrorCode errorCode = SBG_INVALID_PARAMETER; + uint32_t dotCount = 0; + uint32_t dashCount = 0; + + assert(pVersionStr); + assert(pVersionInfo); + + memset(pVersionInfo, 0x00, sizeof(SbgVersion)); + + // + // Pre-parse the input string to count the number of '.' and '-' chars + // It is used to quickly evaluate the expected version format + // + for (size_t i = 0; pVersionStr[i] != '\0'; i++) + { + if (pVersionStr[i] == '.') + { + dotCount++; + } + else if (pVersionStr[i] == '-') + { + dashCount++; + } + } + + // + // Try to parse each version format + // + if (dashCount == 0) + { + if (dotCount == 3) + { + unsigned int major; + unsigned int minor; + unsigned int rev; + unsigned int build; + + if (sscanf(pVersionStr, "%u.%u.%u.%u", &major, &minor, &rev, &build) == 4) + { + if ( (major <= 127) && (minor <= UINT8_MAX) && (rev <= UINT8_MAX) && (build <= UINT8_MAX) ) + { + pVersionInfo->softwareScheme = false; + pVersionInfo->major = (uint8_t)major; + pVersionInfo->minor = (uint8_t)minor; + pVersionInfo->rev = (uint8_t)rev; + pVersionInfo->build = (uint16_t)build; + + errorCode = SBG_NO_ERROR; + } + } + } + else if (dotCount == 2) + { + unsigned int major; + unsigned int minor; + unsigned int rev; + + if (sscanf(pVersionStr, "%u.%u.%u", &major, &minor, &rev) == 3) + { + if ( (major <= 127) && (minor <= UINT8_MAX) && (rev <= UINT8_MAX) ) + { + pVersionInfo->softwareScheme = false; + pVersionInfo->major = (uint8_t)major; + pVersionInfo->minor = (uint8_t)minor; + pVersionInfo->rev = (uint8_t)rev; + + errorCode = SBG_NO_ERROR; + } + } + } + else if (dotCount == 1) + { + unsigned int major; + unsigned int minor; + + if (sscanf(pVersionStr, "%u.%u", &major, &minor) == 2) + { + if ( (major <= 127) && (minor <= UINT8_MAX) ) + { + pVersionInfo->softwareScheme = false; + pVersionInfo->major = (uint8_t)major; + pVersionInfo->minor = (uint8_t)minor; + + errorCode = SBG_NO_ERROR; + } + } + } + else if (dotCount == 0) + { + unsigned int major; + + if (sscanf(pVersionStr, "%u", &major) == 1) + { + if (major <= 127) + { + pVersionInfo->softwareScheme = false; + pVersionInfo->major = (uint8_t)major; + + errorCode = SBG_NO_ERROR; + } + } + } + } + else if ( (dashCount == 1) && (dotCount == 2) ) + { + char qualifierStr[32]; + unsigned int major; + unsigned int minor; + unsigned int build; + + if (sscanf(pVersionStr, "%u.%u.%u-%s", &major, &minor, &build, qualifierStr) == 4) + { + if ( (major <= 63) && (minor <= 63) && (build <= UINT16_MAX) ) + { + pVersionInfo->softwareScheme = true; + pVersionInfo->major = (uint8_t)major; + pVersionInfo->minor = (uint8_t)minor; + pVersionInfo->build = (uint16_t)build; + + errorCode = sbgVersionQualifierFromString(qualifierStr, &pVersionInfo->qualifier); + } + } + } + + return errorCode; } SBG_COMMON_LIB_API SbgErrorCode sbgVersionFromStringEncoded(const char *pVersionStr, uint32_t *pVersion) { - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgVersion versionInfo; - - assert(pVersionStr); - assert(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; + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgVersion versionInfo; + + assert(pVersionStr); + assert(pVersion); + + // + // Parse the version from a string + // + errorCode = sbgVersionFromString(pVersionStr, &versionInfo); + + if (errorCode == SBG_NO_ERROR) + { + *pVersion = sbgVersionEncode(&versionInfo); + } + else + { + *pVersion = 0; + } + + return errorCode; } diff --git a/common/version/sbgVersion.h b/common/version/sbgVersion.h index ce4888a..75b8ceb 100644 --- a/common/version/sbgVersion.h +++ b/common/version/sbgVersion.h @@ -1,10 +1,10 @@ /*! - * \file sbgVersion.h - * \ingroup common - * \author SBG Systems - * \date April 23, 2015 + * \file sbgVersion.h + * \ingroup common + * \author SBG Systems + * \date April 23, 2015 * - * \brief Helper methods and definitions used to handle version. + * \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. @@ -18,10 +18,11 @@ * Basic version is used for hardware or file formats and is the legacy one for software. * This versions is defined as a `Major`.`Minor`.`Revision`.`Build` * Each element is stored in an uint8_t. - * The Major version should NEVER go above 31 + * The Major version can range from 0 to 127. + * The Minor, Revision and Build from 0 to 255. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -58,19 +59,19 @@ extern "C" { //----------------------------------------------------------------------// //- 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 (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_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_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_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. */ +#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. @@ -79,12 +80,12 @@ extern "C" { */ 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. */ + 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; /*! @@ -94,21 +95,21 @@ typedef enum _SbgVersionQualifier */ typedef enum _SbgVersionCmpThreshold { - SBG_VERSION_CMP_THRESHOLD_MAJOR = 0, /*!< Compare only the major field. */ - SBG_VERSION_CMP_THRESHOLD_MINOR = 1, /*!< Compare the major and minor fields. */ - SBG_VERSION_CMP_THRESHOLD_REVISION = 2, /*!< Compare the major, minor and revision fields (only for basic versions). */ - SBG_VERSION_CMP_THRESHOLD_BUILD = 3, /*!< Compare the major, minor, revision and build fields. */ - SBG_VERSION_CMP_THRESHOLD_QUALIFIER = 4 /*!< Compare the major, minor, revision, build and qualifier fields (only for software scheme). */ + SBG_VERSION_CMP_THRESHOLD_MAJOR = 0, /*!< Compare only the major field. */ + SBG_VERSION_CMP_THRESHOLD_MINOR = 1, /*!< Compare the major and minor fields. */ + SBG_VERSION_CMP_THRESHOLD_REVISION = 2, /*!< Compare the major, minor and revision fields (only for basic versions). */ + SBG_VERSION_CMP_THRESHOLD_BUILD = 3, /*!< Compare the major, minor, revision and build fields. */ + SBG_VERSION_CMP_THRESHOLD_QUALIFIER = 4 /*!< Compare the major, minor, revision, build and qualifier fields (only for software scheme). */ } SbgVersionCmpThreshold; /* * DEPRECATED compatibility definitions */ -#define SBG_VERSION_CMP_THRESOLD_MAJOR SBG_VERSION_CMP_THRESHOLD_MAJOR -#define SBG_VERSION_CMP_THRESOLD_MINOR SBG_VERSION_CMP_THRESHOLD_MINOR -#define SBG_VERSION_CMP_THRESOLD_REVISION SBG_VERSION_CMP_THRESHOLD_REVISION -#define SBG_VERSION_CMP_THRESOLD_BUILD SBG_VERSION_CMP_THRESHOLD_BUILD -#define SBG_VERSION_CMP_THRESOLD_QUALIFIER SBG_VERSION_CMP_THRESHOLD_QUALIFIER +#define SBG_VERSION_CMP_THRESOLD_MAJOR SBG_VERSION_CMP_THRESHOLD_MAJOR +#define SBG_VERSION_CMP_THRESOLD_MINOR SBG_VERSION_CMP_THRESHOLD_MINOR +#define SBG_VERSION_CMP_THRESOLD_REVISION SBG_VERSION_CMP_THRESHOLD_REVISION +#define SBG_VERSION_CMP_THRESOLD_BUILD SBG_VERSION_CMP_THRESHOLD_BUILD +#define SBG_VERSION_CMP_THRESOLD_QUALIFIER SBG_VERSION_CMP_THRESHOLD_QUALIFIER /*! * This structure contains all the fields provided by a version number. @@ -116,12 +117,12 @@ typedef enum _SbgVersionCmpThreshold */ 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 */ + 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; //----------------------------------------------------------------------// @@ -131,31 +132,31 @@ typedef struct _SbgVersion /*! * Compile a version number using the basic scheme based on major, minor, revision and build information * - * \param[in] major The major version between 0 to 127. - * \param[in] minor 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. + * \param[in] major The major version between 0 to 127. + * \param[in] minor 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))) ) +#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 minor version between 0 to 63. - * \param[in] build The build version between 0 to 65535. - * \param[in] qualifier The version qualifier within the SbgVersionQualifier enum. - * \return The encoded version number. + * \param[in] major The major version between 0 to 63. + * \param[in] minor The minor version between 0 to 63. + * \param[in] build The build 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) ) +#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) ) //----------------------------------------------------------------------// //- Constructor -// //----------------------------------------------------------------------// @@ -163,30 +164,30 @@ typedef struct _SbgVersion /*! * Create a basic scheme version given the components major.minor.rev.build * - * \param[out] pVersion Version instance - * \param[in] major The major component between 0 to 127 - * \param[in] minor The minor component between 0 to 255 - * \param[in] revision The revision component between 0 to 255 - * \param[in] build The build component between 0 to 255 + * \param[out] pVersion Version instance + * \param[in] major The major component between 0 to 127 + * \param[in] minor The minor component between 0 to 255 + * \param[in] revision The revision component between 0 to 255 + * \param[in] build The build component between 0 to 255 */ SBG_COMMON_LIB_API void sbgVersionCreateBasic(SbgVersion *pVersion, uint8_t major, uint8_t minor, uint8_t revision, uint8_t build); /*! * Create a software scheme version given the components major.minor.build.qualifier * - * \param[out] pVersion Version instance - * \param[in] major The major component between 0 to 63 - * \param[in] minor The minor component between 0 to 63 - * \param[in] build The build component between 0 to 65535 - * \param[in] qualifier The version qualifier enum value. + * \param[out] pVersion Version instance + * \param[in] major The major component between 0 to 63 + * \param[in] minor The minor component between 0 to 63 + * \param[in] build The build component between 0 to 65535 + * \param[in] qualifier The version qualifier enum value. */ SBG_COMMON_LIB_API void sbgVersionCreateSoftware(SbgVersion *pVersion, uint8_t major, uint8_t minor, uint16_t build, SbgVersionQualifier qualifier); /*! * Version copy constructor. * - * \param[in] pVersion Version. - * \param[in] pOtherVersion Version to copy from. + * \param[in] pVersion Version. + * \param[in] pOtherVersion Version to copy from. */ SBG_COMMON_LIB_API void sbgVersionConstructCopy(SbgVersion *pVersion, const SbgVersion *pOtherVersion); @@ -197,16 +198,16 @@ SBG_COMMON_LIB_API void sbgVersionConstructCopy(SbgVersion *pVersion, const SbgV /*! * 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. + * \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. + * \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); @@ -217,22 +218,22 @@ SBG_COMMON_LIB_API uint32_t sbgVersionEncode(const SbgVersion *pVersionInfo); /*! * 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. + * \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; + SbgVersion decodedVersion; - // - // Decode the version first - // - sbgVersionDecode(encodedVersion, &decodedVersion); + // + // Decode the version first + // + sbgVersionDecode(encodedVersion, &decodedVersion); - // - // Returns if the version is using a software scheme - // - return decodedVersion.softwareScheme; + // + // Returns if the version is using a software scheme + // + return decodedVersion.softwareScheme; } //----------------------------------------------------------------------// @@ -246,10 +247,10 @@ SBG_INLINE bool sbgVersionIsUsingSoftwareScheme(uint32_t encodedVersion) * 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] threshold The comparaison threshold 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 threshold). + * \param[in] pVersionA The first version to compare. + * \param[in] pVersionB The second version to compare. + * \param[in] threshold The comparaison threshold 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 threshold). */ SBG_COMMON_LIB_API int32_t sbgVersionCompare(const SbgVersion *pVersionA, const SbgVersion *pVersionB, SbgVersionCmpThreshold threshold); @@ -260,10 +261,10 @@ SBG_COMMON_LIB_API int32_t sbgVersionCompare(const SbgVersion *pVersionA, const * 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] threshold The comparaison threshold 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 threshold). + * \param[in] versionA The first compiled version to compare. + * \param[in] versionB The second compiled version to compare. + * \param[in] threshold The comparaison threshold 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 threshold). */ SBG_COMMON_LIB_API int32_t sbgVersionCompareEncoded(uint32_t versionA, uint32_t versionB, SbgVersionCmpThreshold threshold); @@ -271,12 +272,12 @@ SBG_COMMON_LIB_API int32_t sbgVersionCompareEncoded(uint32_t versionA, uint32_t * 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 + * \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); @@ -284,12 +285,12 @@ SBG_COMMON_LIB_API int32_t sbgVersionIsWithinRange(const SbgVersion *pLowerVersi * 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 + * \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); @@ -301,22 +302,22 @@ SBG_COMMON_LIB_API int32_t sbgVersionIsWithinRangeEncoded(uint32_t lowerVersion, /*! * 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. + * \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, size_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. + * \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, size_t sizeOfBuffer); @@ -325,22 +326,30 @@ SBG_COMMON_LIB_API SbgErrorCode sbgVersionToStringEncoded(uint32_t version, char //----------------------------------------------------------------------// /*! - * Convert a human readable string to a version structure. + * Converts a human-readable string to a version structure. + * + * This method automatically supports both basic and software version schemes. + * For the basic scheme, partial revisions are accepted, such as Major.Minor. * - * \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. + * \param[in] pString The string containing the version to convert. + * \param[out] pVersion Pointer to a version structure where the parsed version information will be stored. + * \return SBG_NO_ERROR if the version information was successfully converted from the string. + * Otherwise, an error code is returned. */ -SBG_COMMON_LIB_API SbgErrorCode sbgVersionFromString(const char *pVersionStr, SbgVersion *pVersionInfo); +SBG_COMMON_LIB_API SbgErrorCode sbgVersionFromString(const char *pString, SbgVersion *pVersion); /*! - * Convert an encoded version number to a human readable string. + * Converts a human-readable string to an encoded version integer. * - * \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. + * This method automatically supports both basic and software version schemes. + * For the basic scheme, partial revisions are accepted, such as Major.Minor. + * + * \param[in] pString The string containing the version to convert. + * \param[out] pVersion Pointer to an encoded version integer where the parsed version information will be stored. + * \return SBG_NO_ERROR if the version information was successfully converted from the string. + * Otherwise, an error code is returned. */ -SBG_COMMON_LIB_API SbgErrorCode sbgVersionFromStringEncoded(const char *pVersionStr, uint32_t *pVersion); +SBG_COMMON_LIB_API SbgErrorCode sbgVersionFromStringEncoded(const char *pString, uint32_t *pVersion); //----------------------------------------------------------------------// //- Legacy version system methods -// @@ -360,11 +369,11 @@ SBG_DEPRECATED(SBG_INLINE uint8_t SBG_VERSION_GET_BUILD(uint32_t encodedVersion) * * Compile a version number using the basic scheme based on major, minor, revision and build information * - * \param[in] major The major version between 0 to 127. - * \param[in] minor 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. + * \param[in] major The major version between 0 to 127. + * \param[in] minor 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) { @@ -379,22 +388,22 @@ SBG_INLINE uint32_t SBG_VERSION(uint8_t major, uint8_t minor, uint8_t rev, uint8 * * Extract a basic version scheme using legacy methods. * - * \param[in] encodedVersion The encoded version to extract the major version. - * \return The major version. + * \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; + SbgVersion versionInfo; - // - // Decode the version information - // - sbgVersionDecode(encodedVersion, &versionInfo); + // + // Decode the version information + // + sbgVersionDecode(encodedVersion, &versionInfo); - // - // Return the major version - // - return versionInfo.major; + // + // Return the major version + // + return versionInfo.major; } /*! @@ -402,22 +411,22 @@ SBG_INLINE uint8_t SBG_VERSION_GET_MAJOR(uint32_t encodedVersion) * * Extract a basic version scheme using legacy methods. * - * \param[in] encodedVersion The encoded version to extract the major version. - * \return The major version. + * \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; + SbgVersion versionInfo; - // - // Decode the version information - // - sbgVersionDecode(encodedVersion, &versionInfo); + // + // Decode the version information + // + sbgVersionDecode(encodedVersion, &versionInfo); - // - // Return the major version - // - return versionInfo.minor; + // + // Return the major version + // + return versionInfo.minor; } /*! @@ -425,22 +434,22 @@ SBG_INLINE uint8_t SBG_VERSION_GET_MINOR(uint32_t encodedVersion) * * Extract a basic version scheme using legacy methods. * - * \param[in] encodedVersion The encoded version to extract the major version. - * \return The major version. + * \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; + SbgVersion versionInfo; - // - // Decode the version information - // - sbgVersionDecode(encodedVersion, &versionInfo); + // + // Decode the version information + // + sbgVersionDecode(encodedVersion, &versionInfo); - // - // Return the major version - // - return versionInfo.rev; + // + // Return the major version + // + return versionInfo.rev; } /*! @@ -448,22 +457,22 @@ SBG_INLINE uint8_t SBG_VERSION_GET_REV(uint32_t encodedVersion) * * Extract a basic version scheme using legacy methods * - * \param[in] encodedVersion The encoded version to extract the major version. - * \return The major version. + * \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; + SbgVersion versionInfo; - // - // Decode the version information - // - sbgVersionDecode(encodedVersion, &versionInfo); + // + // Decode the version information + // + sbgVersionDecode(encodedVersion, &versionInfo); - // - // Return the major version - // - return (uint8_t)versionInfo.build; + // + // Return the major version + // + return (uint8_t)versionInfo.build; } @@ -471,4 +480,4 @@ SBG_INLINE uint8_t SBG_VERSION_GET_BUILD(uint32_t encodedVersion) } #endif -#endif // SBG_VERSION_H +#endif // SBG_VERSION_H diff --git a/doc/binaryCommands.md b/doc/binaryCommands.md new file mode 100644 index 0000000..c73ce07 --- /dev/null +++ b/doc/binaryCommands.md @@ -0,0 +1,253 @@ +# sbgECom Binary Commands + +SBG Systems products support a question/answer protocol to execute commands and actions through the sbgECom binary protocol. +As of ELLIPSE firmware v3 and for all High Performance INS products, these commands primarily encapsulate [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi/) GET/POST calls over serial interfaces. + +For Ethernet-capable devices, the sbgECom binary commands are generally unnecessary, as traditional HTTP protocols should be used for all GET/POST requests. + +All device actions, settings, and information are now managed via the [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi/) rather than through sbgECom binary commands. + +> [!NOTE] +> ELLIPSE firmware version 3.x still supports legacy sbgECom binary commands from version 2.x to facilitate migration. +> However, users are encouraged to transition to the [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi/) to leverage new features. + +## General Principle + +When a command is issued from the host to the device, the device may respond with an acknowledgment (ACK/NACK), or a specific payload. +If a specific payload is returned, it shares the command identifier with the initial request, differing only in the payload data. + +The sbgECom commands can be either accessed over standard serial interfaces or over virtual Ethernet interfaces. + +### Serial Interface + +SBG Systems products are equipped with multiple serial interfaces for both data output and device configuration. +High Performance INS devices feature up to five serial interfaces (Port A to Port E), whereas PULSE IMUs may only have a single interface. + +`sbgECom` commands are supported exclusively on Port A, meaning that all device configuration must be performed through this serial interface. +Other serial interfaces are restricted to outputting `sbgECom` messages and do not accept configuration commands. + +> [!NOTE] +> For detailed information and limitations regarding each serial interface, please refer to the product's hardware manual. +> Ensure that the serial interface is not overloaded to maintain proper transmission of sbgECom commands. + +### Ethernet Interface + +High Performance INS products offer Ethernet connectivity with standard HTTP GET/POST support to access the [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi/). +This is the recommended method for configuring HPINS products using the JSON-based RestAPI. + +Additionally, standard sbgECom commands can be used over the virtual Ethernet interface ETH 0. +The other Ethernet interfaces (ETH 1 to ETH 4) are designated for outputting sbgECom messages or other supported protocols, such as NMEA. + +Each virtual Ethernet interface provides a full-duplex input and output channel and can be configured for either TCP/IP (server or client) or UDP. +The UDP option minimizes latency by eliminating readback but does not guarantee successful data transmission or reception. +Both TCP/IP and UDP communications are secured with standard Ethernet CRC layers, ensuring data integrity. + +Furthermore, High Performance INS devices automatically create an additional ETH CMD interface dedicated to sbgECom command communications. +This interface is hardcoded to use UDP with a `52140` listen port and `52414` transmit port, and it is always active. + +> [!WARNING] +> When using Ethernet, ensure that the INS is not receiving broadcast network traffic, as it may overload the CPU. +> Broadcasting is generally not recommended in production environments. +> If broadcasting cannot be avoided and results in a high volume of data being sent to the INS, consider isolating the INS on a separate sub-network. + +## Commands Overview + +Below is a list of all available commands for ELLIPSE v3 and High Performance INS products: + +| Name (Msg ID) | Description | +|--------------------------------------------------------------------|---------------------------------------------------------------------------------------------| +| [SBG_ECOM_CMD_ACK (00)](#SBG_ECOM_CMD_ACK) | Sends an ACK or NACK in response to a received command. | +| [SBG_ECOM_CMD_SETTINGS_ACTION (01)](#SBG_ECOM_CMD_SETTINGS_ACTION) | Save settings, restore defaults, or reboot (use sbgInsRestApi instead). | +| [SBG_ECOM_CMD_INFO (04)](#SBG_ECOM_CMD_INFO) | Retrieves device information, such as hardware code and serial number. | +| [SBG_ECOM_CMD_API_POST (47)](#SBG_ECOM_CMD_API_POST) | Executes a POST request to a resource endpoint via sbgInsRestApi (update settings/actions). | +| [SBG_ECOM_CMD_API_GET (48)](#SBG_ECOM_CMD_API_GET) | Executes a GET request to a resource endpoint via sbgInsRestApi (query settings/status). | + +> [!NOTE] +> For ELLIPSE firmware versions 2.x and earlier, refer to the [Firmware Reference Manual PDF](https://support.sbg-systems.com/sc/dev/latest/firmware-documentation). + +## General Commands + +### SBG_ECOM_CMD_ACK (Class: 0x10 | ID: 00) {#SBG_ECOM_CMD_ACK} + +This specific command is only sent by the device to acknowledge that a specific command has been properly executed or not. +This frame returns which command is being acknowledged as well as the status (ie successful ACK or an error has occurred NACK). + +Sending this command to the device has no effect. + +#### Command Details + +- **Compatibility:** All products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.0-blue) ![HPINS](https://img.shields.io/badge/HPINS-1.0-blue) ![PULSE](https://img.shields.io/badge/PULSE-1.0-blue) +- **Payload Size:** 4 + +##### Answer + +If the device receives a command that only returns a OK/NOK status or a command that is mal formatted or invalid, the device should answer with the following payload: + +| Field | Description | Format | Size | Offset | +|-----------------------------|-----------------------------------------------------------------------------------|--------|------|--------| +| cmdId | The command identifier this ACK/NACK applies to. | uint8 | 1 | 0 | +| classId | The message class identifier this ACK/NACK applies to. | uint8 | 1 | 1 | +| errorCode | Associated error code (see [SbgErrorCode](#SbgErrorCode)). | uint16 | 4 | 2 | + +### SBG_ECOM_CMD_SETTINGS_ACTION (Class: 0x10 | ID: 01) {#SBG_ECOM_CMD_SETTINGS_ACTION} + +This command provides special settings actions such as saving settings or restoring default settings. +User calls this command with the payload as described below. + +#### Command Details + +- **Compatibility:** ELLIPSE, HPINS +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.0-blue) ![HPINS](https://img.shields.io/badge/HPINS-1.0-blue) +- **Payload Size:** 1 + +##### Query + +The host should send to the device the following payload: + +| Field | Description | Format | Size | Offset | +|-----------------------------|-----------------------------------------------------------------------------------|--------|------|--------| +| action | The action to execute as defined in the enum below. | uint8 | 1 | 0 | + +###### Enum Actions + +| Name | Value | Description +|-----------------------------------|-------|---------------------------------------------------------------------------| +| SBG_ECOM_REBOOT_ONLY | 0 | Software reboot the device. | +| SBG_ECOM_SAVE_SETTINGS | 1 | Save current settings to FLASH and reboot the device. | +| SBG_ECOM_RESTORE_DEFAULT_SETTINGS | 2 | Restore default settings and reboot the device. | + +##### Answer + +If the command is received and processed successfully, the device should answer with an ACK. + +> [!NOTE] +> Please update your implementation to use new sbgInsRestApi `api/v1/settings/default`, `api/v1/settings/save` and `api/v1/system/reboot` endpoints. + +### SBG_ECOM_CMD_INFO (Class: 0x10 | ID: 04) {#SBG_ECOM_CMD_INFO} + +Query the device information such as product code, serial number, firmware version and so on. +Send the command with an empty payload to get the following answer: + +#### Command Details + +- **Compatibility:** ELLIPSE, HPINS +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.0-blue) ![HPINS](https://img.shields.io/badge/HPINS-1.0-blue) +- **Payload Size:** 52 + +##### Answer + +If the command is received and processed successfully, the device should answer with the following payload: + +| Field | Description | Format | Size | Offset | +|-----------------------------|-----------------------------------------------------------------------------------|--------|------|--------| +| productCode | The device product code such as ELLIPSE-D-G4A3-B1 | string | 32 | 0 | +| serialNumber | The device unique serial number | uint32 | 4 | 32 | +| mnfVersion | Procedure and tools version used to manufacture and validate the product. | uint32 | 4 | 36 | +| mnfYear | The device product code such as ELLIPSE-D-G4A3-B1 | uint16 | 2 | 40 | +| mnfMonth | The device product code such as ELLIPSE-D-G4A3-B1 | uint8 | 1 | 42 | +| mnfDay | The device product code such as ELLIPSE-D-G4A3-B1 | uint8 | 1 | 43 | +| hwRevision | The device hardware revision (Major.Minor.0.0) | uint32 | 4 | 44 | +| swRevision | The device firmware version using | uint32 | 4 | 48 | + +The productCode field is a NULL terminated C String. All versions fields should be interpreted as described in this [section](#VERSION_TYPE). + +> [!NOTE] +> Please update your implementation to use new sbgInsRestApi `api/v1/info` endpoint to query device information. + +## GET/POST (sbgInsRestApi) +High Performance INS products as well as new SBG Systems products use a new configuration and commands layer based on REST API concepts. REST API are very common in Internet of Things (IoT) devices and rely on Ethernet/HTTP connections. + +The REST API is based on JSON (JavaScript Object Notation) specification only. The JSON is widely used in the industry and there are plenty of libraries available to manipulate JSON. + +SBG Systems products’ offer REST API configuration and commands over Ethernet links using standard HTTP GET and POST methods. This manual doesn’t cover the sbgInsRest API usage and definition so please read the dedicated sbgInsRest API manual for more information. + +The REST API can also be accessed and used over serial connections. This is this part that is covered in this documentation. The sbgECom uses two dedicated commands to execute GET and POST requests over a standard serial link. + +### SBG_ECOM_CMD_API_POST (Class: 0x10 | ID: 47) {#SBG_ECOM_CMD_API_POST} + +Execute a POST method on a specified sbgInsRestApi path. +The POST method is used to update a setting value or to execute a command. + +Some POST paths require a body (ie a payload) while others, such as commands for a system reboot, have no body at all. + +The example below, ask the device to save settings in FLASH memory: + +```sh +/api/v1/settings/save +``` + +#### Command Details + +- **Compatibility:** All products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-3.0-blue) ![HPINS](https://img.shields.io/badge/HPINS-4.0-blue) ![PULSE](https://img.shields.io/badge/PULSE-1.0-blue) +- **Payload Size:** variable + +##### Query + +The host should send to the device the following payload: + +| Field | Description | Format | Size | Offset | +|-----------------------------|-----------------------------------------------------------------------------------|--------|------|--------| +| path | The path string to execute POST method on. | string | - | - | +| query | The query parameter string or empty string if not used. | string | - | - | +| body | The JSON body string or empty string if not used. | string | - | - | + +##### Answer + +If the command is received and processed successfully, the device should answer with the following payload: + +| Field | Description | Format | Size | Offset | +|-----------------------------|-----------------------------------------------------------------------------------|--------|------|--------| +| statusCode | HTTP return code for the POST request. 200 for success. | uint16 | 2 | 0 | +| jsonPayload | Variable JSON payload returned by the device. | string | - | 2 | + +> [!NOTE] +> All strings such as path, query and body fields have to be NULL terminated. + +> [!NOTE] +> The device might answer with payload exceeding the standard 4096 frame size. In this case, the extended frame format is used. + +### SBG_ECOM_CMD_API_GET (Class: 0x10 | ID: 48) {#SBG_ECOM_CMD_API_GET} + +Used to GET a resource from the device on a sbgInsRestApi path. +For example, you can query the device settings or the device information / status using this command. + +An optional query parameters string might be provided as shown in the example below: + +```sh +/api/v1/settings?delta=true&format=pretty +``` + +This path query only the settings that have changed from the device default settings and pretty print the returned JSON. +The path itself is `/api/v1/settings` with two query parameters {delta} and {format}. + +#### Command Details + +- **Compatibility:** All products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-3.0-blue) ![HPINS](https://img.shields.io/badge/HPINS-4.0-blue) ![PULSE](https://img.shields.io/badge/PULSE-1.0-blue) +- **Payload Size:** variable + +##### Query + +The host should send to the device the following payload: + +| Field | Description | Format | Size | Offset | +|-----------------------------|-----------------------------------------------------------------------------------|--------|------|--------| +| path | The path string to execute GET method on. | string | - | - | +| query | The query parameter string or empty string if not used. | string | - | - | + +##### Answer + +If the command is received and processed successfully, the device should answer with the following payload: + +| Field | Description | Format | Size | Offset | +|-----------------------------|-----------------------------------------------------------------------------------|--------|------|--------| +| statusCode | HTTP return code for the GET request. 200 for success. | uint16 | 2 | 0 | +| jsonPayload | Variable JSON payload returned by the device. | string | - | 2 | + +> [!NOTE] +> All strings such as path and query fields have to be NULL terminated. + +> [!NOTE] +> The device might answer with payload exceeding the standard 4096 frame size. In this case, the extended frame format is used. diff --git a/doc/binaryMessages.md b/doc/binaryMessages.md new file mode 100644 index 0000000..8efa0be --- /dev/null +++ b/doc/binaryMessages.md @@ -0,0 +1,1540 @@ +# sbgECom Binary Messages {#binaryMessages} + +The `SBG_ECOM_CLASS_LOG_ECOM_0 (0x01)` message class encompasses all binary messages that can be output by the device. +In addition to outputting data, some messages can also be used to input aiding information, such as the [SBG_ECOM_LOG_AIR_DATA (36)](#SBG_ECOM_LOG_AIR_DATA). + +The sbgECom binary logs are designed for high data rates with minimal payload size. +This protocol is ideal for both real-time stabilization/navigation and post-processing applications. + +Each message encapsulates a coherent set of data and status information that can be fully interpreted without requiring additional context. +Users can configure the output rate for each message based on their specific requirements, optimizing the management of transmission bandwidth. + +## Message Timestamping + +Each log includes a monotonic timestamp in microseconds, representing the time since the sensor was powered up. +This timestamp can be converted to GPS Time of Week (ToW) or UTC time using the [SBG_ECOM_LOG_UTC_TIME](#SBG_ECOM_LOG_UTC_TIME) message. + +When valid GPS time is available from a GNSS receiver, the `SBG_ECOM_LOG_UTC_TIME` message provides both the timestamp and the corresponding GPS/UTC time. +This allows for straightforward conversion of any message timestamp to GPS/UTC time. + +The following code sample demonstrates how to convert a message timestamp (`msgTimeStampUs`) to absolute GPS Time of Week in milliseconds. +The variable `lastUtcMsg` holds the latest valid `SBG_ECOM_LOG_UTC_TIME` information received. + +```C + const uint32_t gpsTimeOfWeekMaxMs = 60ul*60ul*24ul*7ul*1000ul; + + int32_t deltaTimeUs = newMsg.timeStamp - lastUtcMsg.timeStamp; + int32_t newMsgTowMs = lastUtcMsg.gpsTimeOfWeek + deltaTimeUs/1000; + + if (newMsgTowMs < 0) + { + newMsgTowMs += gpsTimeOfWeekMaxMs; + } + else if (newMsgTowMs >= gpsTimeOfWeekMaxMs) + { + newMsgTowMs -= gpsTimeOfWeekMaxMs; + } +``` + +> [!NOTE] +> Timestamp to UTC time conversion is implemented in the `sbgBasicLogger` CLI tool. +> For reference, you can review the `CLoggerContext` C++ implementation. + +## Messages overview + +The following list, provides a quick overview of all available logs for this message class. +It briefly describe which parameters are contained in each output log. + +| Name (Msg ID) | Description | +|--------------------------------------------------------------------------|----------------------------------------------------------------------------------------| +| [SBG_ECOM_LOG_STATUS (01)](#SBG_ECOM_LOG_STATUS) | Status general, clock, com aiding, solution, heave | +| [SBG_ECOM_LOG_UTC_TIME (02)](#SBG_ECOM_LOG_UTC_TIME) | Provides UTC time reference | +| [SBG_ECOM_LOG_IMU_DATA (03)](#SBG_ECOM_LOG_IMU_DATA) | DEPRECATED: Synchronous body IMU rates and accelerations | +| [SBG_ECOM_LOG_IMU_SHORT (44)](#SBG_ECOM_LOG_IMU_SHORT) | Body IMU rates and accelerations. Synchronous or asynchronous depending of the INS. | +| [SBG_ECOM_LOG_EKF_EULER (06)](#SBG_ECOM_LOG_EKF_EULER) | Includes roll, pitch, yaw and their accuracies on each axis | +| [SBG_ECOM_LOG_EKF_QUAT (07)](#SBG_ECOM_LOG_EKF_QUAT) | Includes the 4 quaternions values | +| [SBG_ECOM_LOG_EKF_NAV (08)](#SBG_ECOM_LOG_EKF_NAV) | Position and velocities in NED coordinates with the accuracies on each axis | +| [SBG_ECOM_LOG_EKF_VEL_BODY (54)](#SBG_ECOM_LOG_EKF_VEL_BODY) | INS velocity expressed in body/vehicle frame. | +| [SBG_ECOM_LOG_EKF_ROT_ACCEL_BODY (52)](#SBG_ECOM_LOG_EKF_ROT_ACCEL_BODY) | Corrected Earth rate and gravity free rates and accelerations in body/vehicle frame. | +| [SBG_ECOM_LOG_EKF_ROT_ACCEL_NED (53)](#SBG_ECOM_LOG_EKF_ROT_ACCEL_NED) | Corrected Earth rate and gravity free rates and accelerations in NED navigation frame. | +| [SBG_ECOM_LOG_SHIP_MOTION (09)](#SBG_ECOM_LOG_SHIP_MOTION) | Real time heave, surge, sway, accelerations and velocity | +| [SBG_ECOM_LOG_SHIP_MOTION_HP (32)](#SBG_ECOM_LOG_SHIP_MOTION_HP) | Delayed heave, surge, sway, accelerations and velocity | +| [SBG_ECOM_LOG_MAG (04)](#SBG_ECOM_LOG_MAG) | Magnetic data with associated accelerometer on each axis | +| [SBG_ECOM_LOG_MAG_CALIB (05)](#SBG_ECOM_LOG_MAG_CALIB) | Magnetometer calibration data (raw buffer) | +| [SBG_ECOM_LOG_GPS1_VEL (13)](#SBG_ECOM_LOG_GPSX_VEL) | GNSS velocity from primary receiver | +| [SBG_ECOM_LOG_GPS1_POS (14)](#SBG_ECOM_LOG_GPSX_POS) | GNSS position from primary receiver | +| [SBG_ECOM_LOG_GPS1_HDT (15)](#SBG_ECOM_LOG_GPSX_HDT) | GNSS true heading from primary receiver | +| [SBG_ECOM_LOG_GPS1_RAW (31)](#SBG_ECOM_LOG_GPSX_RAW) | GNSS raw data from primary receiver | +| [SBG_ECOM_LOG_GPS1_SAT (50)](#SBG_ECOM_LOG_GPSX_SAT) | GNSS satellite in view information from primary receiver | +| [SBG_ECOM_LOG_GPS2_VEL (16)](#SBG_ECOM_LOG_GPSX_VEL) | GNSS velocity from secondary receiver | +| [SBG_ECOM_LOG_GPS2_POS (17)](#SBG_ECOM_LOG_GPSX_POS) | GNSS position from secondary receiver | +| [SBG_ECOM_LOG_GPS2_HDT (18)](#SBG_ECOM_LOG_GPSX_HDT) | GNSS true heading from secondary receiver | +| [SBG_ECOM_LOG_GPS2_RAW (38)](#SBG_ECOM_LOG_GPSX_RAW) | GNSS raw data from secondary receiver | +| [SBG_ECOM_LOG_GPS2_SAT (51)](#SBG_ECOM_LOG_GPSX_SAT) | GNSS satellite in view information from secondary receiver | +| [SBG_ECOM_LOG_ODO_VEL (19)](#SBG_ECOM_LOG_ODO_VEL) | Provides odometer velocity measured by the device | +| [SBG_ECOM_LOG_AIR_DATA (36)](#SBG_ECOM_LOG_AIR_DATA) | Barometric altimeter input/output for airdata support | +| [SBG_ECOM_LOG_DEPTH (47)](#SBG_ECOM_LOG_DEPTH) | Depth sensor measurement log used for subsea navigation | +| [SBG_ECOM_LOG_DVL_BOTTOM_TRACK (29)](#SBG_ECOM_LOG_DVL_XXXX) | Doppler Velocity Log for bottom tracking data | +| [SBG_ECOM_LOG_DVL_WATER_TRACK (30)](#SBG_ECOM_LOG_DVL_XXXX) | Doppler Velocity log for water layer data | +| [SBG_ECOM_LOG_USBL (37)](#SBG_ECOM_LOG_USBL) | Raw USBL position data for subsea navigation | +| [SBG_ECOM_LOG_EVENT_A (24)](#SBG_ECOM_LOG_EVENT_XXXX) | Event marker sent when a signal is detected on Sync In A pin | +| [SBG_ECOM_LOG_EVENT_B (25)](#SBG_ECOM_LOG_EVENT_XXXX) | Event marker sent when a signal is detected on Sync In B pin | +| [SBG_ECOM_LOG_EVENT_C (26)](#SBG_ECOM_LOG_EVENT_XXXX) | Event marker sent when a signal is detected on Sync In C pin | +| [SBG_ECOM_LOG_EVENT_D (27)](#SBG_ECOM_LOG_EVENT_XXXX) | Event marker sent when a signal is detected on Sync In D pin | +| [SBG_ECOM_LOG_EVENT_E (28)](#SBG_ECOM_LOG_EVENT_XXXX) | Event marker sent when a signal is detected on Sync In E pin | +| [SBG_ECOM_LOG_EVENT_OUT_A (45)](#SBG_ECOM_LOG_EVENT_XXXX) | Event marker used to timestamp each generated Sync Out A signal | +| [SBG_ECOM_LOG_EVENT_OUT_B (46)](#SBG_ECOM_LOG_EVENT_XXXX) | Event marker used to timestamp each generated Sync Out B signal | +| [SBG_ECOM_LOG_DIAG (48)](#SBG_ECOM_LOG_DIAG) | Returns error, warning, info and debug messages | +| [SBG_ECOM_LOG_RTCM_RAW (49)](#SBG_ECOM_LOG_RTCM_RAW) | RTCM/NTRIP RAW data stream that can be used in post-processing | +| [SBG_ECOM_LOG_PTP_STATUS (57)](#SBG_ECOM_LOG_PTP_STATUS) | Precise Time Protocol (PTP) status and metric values. | + +## Messages Availability + +In each message definition, you'll find details about the availability of the message for specific products and the firmware version in which it was introduced. +Some messages are specific to certain product capabilities, such as INS-only features or marine options. + +For devices running up-to-date firmware, the new [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi) can be used to easily query the list of supported output logs. +This can be done using a GET request to the following endpoint: + +```sh +sbgEComApi -s COM1 -r 115200 -g /api/v1/messages/std +``` + +## Messages Versions + +SBG Systems continuously improves its products by adding new features and outputs. +This includes introducing new sbgECom binary messages and updating existing ones with additional fields and data. + +### Payload versions + +Each message payload definition specifies whether new fields have been added in various sbgECom releases. +It is essential to determine the sbgECom version implemented by your device, as described [here](#SBG_ECOM_VERSION_DEF). + +For example, the [SBG_ECOM_LOG_STATUS](#SBG_ECOM_LOG_STATUS) output log has been updated with two new fields over time, as shown below: + +| Field | Description | Unit | Format | Size | Offset | +|-----------------------------|-----------------------------------------------------------------------------------|------|--------|------|--------| +| TIME_STAMP | Time since sensor is powered up | µs | uint32 | 4 | 0 | +| GENERAL_STATUS | General device status (see [STATUS_GENERAL_STATUS](#STATUS_GENERAL_STATUS)) | - | uint16 | 2 | 4 | +| COM_STATUS_2 | Additional communication status (see [STATUS_COM_STATUS_2](#STATUS_COM_STATUS_2)) | - | uint16 | 2 | 6 | +| COM_STATUS | Communication status (see [STATUS_COM_STATUS](#STATUS_COM_STATUS)) | - | uint32 | 4 | 8 | +| AIDING_STATUS | Aiding equipment status (see [STATUS_AIDING_STATUS](#STATUS_AIDING_STATUS)) | - | uint32 | 4 | 12 | +| RESERVED_2 | Reserved status field for future use | - | uint32 | 4 | 16 | +| RESERVED_3 | Reserved field for future use | - | uint16 | 2 | 20 | +| UP_TIME | System up time since the power on - 0 if N/A (added v1.7) | s | uint32 | 4 | 22 | +| CPU_USAGE | Main CPU usage in percent - 0xFF if N/A (added v5.0) | % | uint8 | 1 | 23 | + +Each new field addition corresponds to a specific sbgECom version, allowing developers to understand the evolution of the message and implement backward compatibility when necessary. + +### Backward/Upward Compatibility + +New fields may be added to the end of a message payload over time, but fields are never removed to maintain backward binary compatibility. +If a field becomes obsolete, it will be marked as reserved or deprecated. + +If you use an older sbgECom implementation to parse messages from a firmware with a newer sbgECom protocol, the parsing should still function correctly. +However, you will not have access to any new fields or features introduced in the latest firmware. + +The sbgECom protocol implementation ensures compatibility by checking if a message payload meets the minimum required size. +Any additional fields beyond this size are parsed optionally, based on the remaining payload data. + +For reference on this mechanism, you can review the `SBG_ECOM_LOG_STATUS` message parsing implementation in the sbgECom C library. + +> [!WARNING] +> If you implement your own sbgECom protocol and message parsing, avoid checking for exact equality in the received message payload size. +> Instead, follow the sbgECom message payload parsing C implementation to ensure upward compatibility. +## Additional Messages + +This sections covers output logs used to report device information, settings and for troubleshooting. + +### SBG_ECOM_LOG_DIAG (48) {#SBG_ECOM_LOG_DIAG} + +This log is sent by the device whenever an internal error, warning, or informational message is generated. + +- **Message Name (ID):** `SBG_ECOM_LOG_DIAG (48)` +- **Compatibility:** AHRS/INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-3.0-blue) ![HPINS](https://img.shields.io/badge/HPINS-3.0-blue) ![PULSE](https://img.shields.io/badge/PULSE-1.0-blue) +- **Payload Size:** Variable (6 bytes + message length) + +| Field | Description | Unit | Format | Size | Offset | +|-------------|----------------------------------------------------------------|-------|--------|------------|--------| +| TIME_STAMP | Time since the sensor was powered up | µs | uint32 | 4 | 0 | +| TYPE | Type of message reported (see [TYPE](#DIAG_TYPE)) | - | uint8 | 1 | 4 | +| ERROR_CODE | Error code, if applicable (see [SbgErrorCode](#SbgErrorCode)) | - | uint8 | 1 | 5 | +| MESSAGE | Detailed message as a NULL-terminated C string | - | chars | [0-4080] | 6 | + +#### TYPE Definition {#DIAG_TYPE} + +| Value | Name | Description | +|-------|---------------------------|--------------------------------------------------------------------------------| +| 0 | SBG_DEBUG_LOG_TYPE_ERROR | Indicates an error; should not occur during normal operation | +| 1 | SBG_DEBUG_LOG_TYPE_WARNING| Indicates a warning; may occur during normal operation | +| 2 | SBG_DEBUG_LOG_TYPE_INFO | Provides informational content, useful for troubleshooting | +| 3 | SBG_DEBUG_LOG_TYPE_DEBUG | Debug information, available only with specific firmware | + +## Status and Time + +In this section you can find all sbgECom logs related to status reporting. + +### SBG_ECOM_LOG_STATUS (01) {#SBG_ECOM_LOG_STATUS} + +This output combines all system status data, divided into several categories: General, Communications, Aiding. + +This log is useful for advanced status information. + +- **Message Name (ID):** `SBG_ECOM_LOG_STATUS (01)` +- **Compatibility:** All products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.0-blue) ![HPINS](https://img.shields.io/badge/HPINS-1.0-blue) ![PULSE](https://img.shields.io/badge/PULSE-1.0-blue) +- **Payload Size:** 27 bytes + +| Field | Description | Unit | Format | Size | Offset | +|-----------------------------|-----------------------------------------------------------------------------------|------|--------|------|--------| +| TIME_STAMP | Time since sensor is powered up | µs | uint32 | 4 | 0 | +| GENERAL_STATUS | General device status (see [STATUS_GENERAL_STATUS](#STATUS_GENERAL_STATUS)) | - | uint16 | 2 | 4 | +| COM_STATUS_2 | Additional communication status (see [STATUS_COM_STATUS_2](#STATUS_COM_STATUS_2)) | - | uint16 | 2 | 6 | +| COM_STATUS | Communication status (see [STATUS_COM_STATUS](#STATUS_COM_STATUS)) | - | uint32 | 4 | 8 | +| AIDING_STATUS | Aiding equipment status (see [STATUS_AIDING_STATUS](#STATUS_AIDING_STATUS)) | - | uint32 | 4 | 12 | +| RESERVED_2 | Reserved status field for future use | - | uint32 | 4 | 16 | +| RESERVED_3 | Reserved field for future use | - | uint16 | 2 | 20 | +| UP_TIME | System up time since the power on - 0 if N/A (added v1.7) | s | uint32 | 4 | 22 | +| CPU_USAGE | Main CPU usage in percent - 0xFF if N/A (added v5.0) | % | uint8 | 1 | 23 | + +> [!WARNING] +> The `COM_STATUS_2` field has been introduced in sbgECom version 4.0. In previous versions, this field was reserved and not used. +> Please ensure that you only utilize the additional communication status flags with products running firmware that supports sbgECom version 4.0 or later. + +#### General device status (GENERAL_STATUS field) {#STATUS_GENERAL_STATUS} + +Provides general device status and information such as the power supplies (main, IMU, GNSS), settings, temperature and data-logger. + +| Bit | Name | Type | Description | +|-----|---------------------------------|------|-------------------------------------------------------| +| 0 | SBG_ECOM_GENERAL_MAIN_POWER_OK | Mask | Set if main power supply is OK. | +| 1 | SBG_ECOM_GENERAL_IMU_POWER_OK | Mask | Set if IMU power supply is OK. | +| 2 | SBG_ECOM_GENERAL_GPS_POWER_OK | Mask | Set if GPS power supply is OK. | +| 3 | SBG_ECOM_GENERAL_SETTINGS_OK | Mask | Set if settings are correctly loaded. | +| 4 | SBG_ECOM_GENERAL_TEMPERATURE_OK | Mask | Set if temperature is within specified limits. | +| 5 | SBG_ECOM_GENERAL_DATALOGGER_OK | Mask | Set if the data-logger is working correctly. | +| 6 | SBG_ECOM_GENERAL_CPU_OK | Mask | Set if the CPU headroom is correct. | + +#### Communication status (COM_STATUS field) {#STATUS_COM_STATUS} + +Provide information on ports, tells is they are valid or saturated. + +| Bit | Name | Type | Description | +|------|--------------------------|------|---------------------------------------------------------------| +| 0 | SBG_ECOM_PORTA_VALID | Mask | Set to 0 in case of low level communication error. | +| 1 | SBG_ECOM_PORTB_VALID | Mask | Set to 0 in case of low level communication error. | +| 2 | SBG_ECOM_PORTC_VALID | Mask | Set to 0 in case of low level communication error. | +| 3 | SBG_ECOM_PORTD_VALID | Mask | Set to 0 in case of low level communication error. | +| 4 | SBG_ECOM_PORTE_VALID | Mask | Set to 0 in case of low level communication error. | +| 5 | SBG_ECOM_PORTA_RX_OK | Mask | Set to 0 in case of saturation on PORT A input. | +| 6 | SBG_ECOM_PORTA_TX_OK | Mask | Set to 0 in case of saturation on PORT A output. | +| 7 | SBG_ECOM_PORTB_RX_OK | Mask | Set to 0 in case of saturation on PORT B input. | +| 8 | SBG_ECOM_PORTB_TX_OK | Mask | Set to 0 in case of saturation on PORT B output. | +| 9 | SBG_ECOM_PORTC_RX_OK | Mask | Set to 0 in case of saturation on PORT C input. | +| 10 | SBG_ECOM_PORTC_TX_OK | Mask | Set to 0 in case of saturation on PORT C output. | +| 11 | SBG_ECOM_PORTD_RX_OK | Mask | Set to 0 in case of saturation on PORT D input. | +| 12 | SBG_ECOM_PORTD_TX_OK | Mask | Set to 0 in case of saturation on PORT D output. | +| 13 | SBG_ECOM_PORTE_RX_OK | Mask | Set to 0 in case of saturation on PORT E input. | +| 14 | SBG_ECOM_PORTE_TX_OK | Mask | Set to 0 in case of saturation on PORT E output. | +| 15 | SBG_ECOM_ETH0_VALID | Mask | Set to 0 if the ETH 0 interface is either closed or in error. | +| 16 | SBG_ECOM_ETH1_VALID | Mask | Set to 0 if the ETH 1 interface is either closed or in error. | +| 17 | SBG_ECOM_ETH2_VALID | Mask | Set to 0 if the ETH 2 interface is either closed or in error. | +| 18 | SBG_ECOM_ETH3_VALID | Mask | Set to 0 if the ETH 3 interface is either closed or in error. | +| 19 | SBG_ECOM_ETH4_VALID | Mask | Set to 0 if the ETH 4 interface is either closed or in error. | +| 25 | SBG_ECOM_CAN_VALID | Mask | Set to 0 in case of low level communication error. | +| 26 | SBG_ECOM_CAN_RX_OK | Mask | Set to 0 in case of saturation on CAN Bus input buffer. | +| 27 | SBG_ECOM_CAN_TX_OK | Mask | Set to 0 in case of saturation on CAN Bus output buffer. | +| 28-30| SBG_ECOM_CAN_BUS | Enum | CAN Bus status (see [SBG_ECOM_CAN_BUS](#SBG_ECOM_CAN_BUS)). | + +##### CAN Bus States {#SBG_ECOM_CAN_BUS} + +Reports CAN bus status: + +| Value | Name | Description | +|-------|----------------------------|---------------------------------------------------| +| 0 | SBG_ECOM_CAN_BUS_OFF | Bus OFF operation due to too many errors. | +| 1 | SBG_ECOM_CAN_BUS_TX_RX_ERR | Transmit or receive error. | +| 2 | SBG_ECOM_CAN_BUS_OK | The CAN bus is working correctly. | +| 3 | SBG_ECOM_CAN_BUS_ERROR | A general error has occurred on the CAN bus. | + +#### Communication status (COM_STATUS_2 field) {#STATUS_COM_STATUS_2} + +Provide additional status on interfaces such as Ethernet. + +| Bit | Name | Type | Description | +|-----|-------------------------|------|--------------------------------------------------------------| +| 0 | SBG_ECOM_COM2_ETH0_RX_OK| Mask | Set to 0 in case of error on ETH0 input such as saturation. | +| 1 | SBG_ECOM_COM2_ETH0_TX_OK| Mask | Set to 0 in case of error on ETH0 output such as saturation. | +| 2 | SBG_ECOM_COM2_ETH1_RX_OK| Mask | Set to 0 in case of error on ETH1 input such as saturation. | +| 3 | SBG_ECOM_COM2_ETH1_TX_OK| Mask | Set to 0 in case of error on ETH1 output such as saturation. | +| 4 | SBG_ECOM_COM2_ETH2_RX_OK| Mask | Set to 0 in case of error on ETH2 input such as saturation. | +| 5 | SBG_ECOM_COM2_ETH2_TX_OK| Mask | Set to 0 in case of error on ETH2 output such as saturation. | +| 6 | SBG_ECOM_COM2_ETH3_RX_OK| Mask | Set to 0 in case of error on ETH3 input such as saturation. | +| 7 | SBG_ECOM_COM2_ETH3_TX_OK| Mask | Set to 0 in case of error on ETH3 output such as saturation. | +| 8 | SBG_ECOM_COM2_ETH4_RX_OK| Mask | Set to 0 in case of error on ETH4 input such as saturation. | +| 9 | SBG_ECOM_COM2_ETH4_TX_OK| Mask | Set to 0 in case of error on ETH4 output such as saturation. | + +#### INS aiding status (AIDING_STATUS field) {#STATUS_AIDING_STATUS} + +Tells which aiding data is received. + +| Bit | Name | Type | Description | +|-----|--------------------------------|------|----------------------------------------------------------| +| 0 | SBG_ECOM_AIDING_GPS1_POS_RECV | Mask | Set to 1 when valid GPS 1 position data is received | +| 1 | SBG_ECOM_AIDING_GPS1_VEL_RECV | Mask | Set to 1 when valid GPS 1 velocity data is received | +| 2 | SBG_ECOM_AIDING_GPS1_HDT_RECV | Mask | Set to 1 when valid GPS 1 true heading data is received | +| 3 | SBG_ECOM_AIDING_GPS1_UTC_RECV | Mask | Set to 1 when valid GPS 1 UTC time data is received | +| 4 | SBG_ECOM_AIDING_GPS2_POS_RECV | Mask | Set to 1 when valid GPS 2 position data is received | +| 5 | SBG_ECOM_AIDING_GPS2_VEL_RECV | Mask | Set to 1 when valid GPS 2 velocity data is received | +| 6 | SBG_ECOM_AIDING_GPS2_HDT_RECV | Mask | Set to 1 when valid GPS 2 true heading data is received | +| 7 | SBG_ECOM_AIDING_GPS2_UTC_RECV | Mask | Set to 1 when valid GPS 2 UTC time data is received | +| 8 | SBG_ECOM_AIDING_MAG_RECV | Mask | Set to 1 when valid Magnetometer data is received | +| 9 | SBG_ECOM_AIDING_ODO_RECV | Mask | Set to 1 when Odometer pulse is received | +| 10 | SBG_ECOM_AIDING_DVL_RECV | Mask | Set to 1 when valid DVL data is received | +| 11 | SBG_ECOM_AIDING_USBL_RECV | Mask | Set to 1 when valid USBL data is received | +| 12 | SBG_ECOM_AIDING_DEPTH_RECV | Mask | Set to 1 when valid Depth sensor data is received | +| 13 | SBG_ECOM_AIDING_AIR_DATA_RECV | Mask | Set to 1 when valid altitude and/or airspeed is received | + +### SBG_ECOM_LOG_UTC_TIME (02) {#SBG_ECOM_LOG_UTC_TIME} + +Provides UTC time reference and internal clock estimation metrics. + +This message provides a time correspondence between the device `TIME_STAMP` value and the actual UTC Time. +You should use this information to timestamp data to an absolute UTC or GPS time reference. + +- **Message Name (ID):** `SBG_ECOM_LOG_UTC_TIME (02)` +- **Compatibility:** All products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.0-blue) ![HPINS](https://img.shields.io/badge/HPINS-1.0-blue) ![PULSE](https://img.shields.io/badge/PULSE-1.0-blue) +- **Payload Size:** 33 bytes + +| Field | Description | Unit | Format | Size | Offset | +|-----------------------------|-----------------------------------------------------------------------------------|------|--------|------|--------| +| TIME_STAMP | Time since sensor is powered up | µs | uint32 | 4 | 0 | +| TIME_STATUS | UTC time and clock sync status (see [TIME_STATUS](#TIME_STATUS)) | - | uint16 | 2 | 4 | +| YEAR | Year | year | uint16 | 2 | 6 | +| MONTH | Month in Year [1 ... 12] | month| uint8 | 1 | 8 | +| DAY | Day in Month [1 … 31] | d | uint8 | 1 | 9 | +| HOUR | Hour in day [0 … 23] | h | uint8 | 1 | 10 | +| MIN | Minute in hour [0 … 59] | min | uint8 | 1 | 11 | +| SEC | Second in minute [0 … 60] | s | uint8 | 1 | 12 | +| NANOSEC | Nanosecond of second | ns | uint32 | 4 | 13 | +| GPS_TOW | GPS Time of week | ms | uint32 | 4 | 17 | +| CLK_BIAS_STD | Estimated clock bias 1-σ standard deviation – Added in v4.0 | s | float | 4 | 21 | +| CLK_SF_ERROR_STD | Estimated scale factor 1-σ standard deviation – Added in v4.0 | % | float | 4 | 25 | +| CLK_RESIDUAL_ERR | Measured error between GNSS PPS and internal clock – Added in v4.0 | s | float | 4 | 29 | + +> [!NOTE] +> The `SEC` field can report 60 seconds only when a leap second is inserted. + +> [!WARNING] +> The UTC time status has changed in latest protocol versions (sbgECom 4.0 or later). +> Once the UTC time is initialized and known it stays in this state. + +#### Time and Clock status (TIME_STATUS field) {#TIME_STATUS} + +Provide information on the internal clock estimation and UTC status. + +| Bit | Name | Type | Description | +|------|----------------------------------|------|-------------------------------------------------------------------------------| +| 0 | HAS_CLOCK_INPUT | Mask | Set if a valid input clock signal (PPS) is detected. | +| [1-4]| CLOCK_STATE | Enum | Define the internal clock estimation status (see [CLOCK_STATE](#CLOCK_STATE)) | +| 5 | UTC_IS_ACCURATE | Mask | Set if the reported UTC time is accurate (i.e., aligned with PPS). | +| [6-9]| UTC_STATUS | Enum | Define the UTC validity status (see [UTC_STATUS](#UTC_STATUS)) | + +##### Clock State (CLOCK_STATE field) {#CLOCK_STATE} + +List internal clock alignment and correction state values: + +| Value | Name | Description | +|-------|-----------------------------------|--------------------------------------------------------------------------------------| +| 0 | SBG_ECOM_CLOCK_STATE_ERROR | An error has occurred on the clock estimation. | +| 1 | SBG_ECOM_CLOCK_STATE_FREE_RUNNING | The clock is based on the internal crystal using latest estimated clock bias/sf. | +| 2 | SBG_ECOM_CLOCK_STATE_STEERING | A PPS has been detected and the clock is converging to it. | +| 3 | SBG_ECOM_CLOCK_STATE_VALID | The clock has converged to the PPS or is still considered to be accurate. | + +##### UTC Status (UTC_STATUS field) {#UTC_STATUS} + +List available UTC enum status: + +| Value | Name | Description | +|-------|-----------------------------------|--------------------------------------------------------------------------------------| +| 0 | SBG_ECOM_UTC_STATUS_INVALID | The UTC time is not known and invalid. The firmware initial date time is used. | +| 1 | SBG_ECOM_UTC_STATUS_NO_LEAP_SEC | UTC time initialized but leap second is not known. Firmware leap second is used. | +| 2 | SBG_ECOM_UTC_STATUS_INITIALIZED | UTC time initialized with a fully resolved leap second information. | + +### SBG_ECOM_LOG_PTP_STATUS (57) {#SBG_ECOM_LOG_PTP_STATUS} + +Message that provides status and metrics regarding the embedded Precise Time Protocol (PTP) service. +It can report the time difference between the local INS time and an external GrandMaster clock. + +The PTP service can operate in two different modes: +- `Master`: If the INS is the best available clock, it will broadcast PTP messages and act as a Grandmaster clock. +- `Passive`: Only listen for a PTP Grandmaster clock and reports the time delta between internal INS clock and PTP Grandmaster clock. + +The message reporting the PTP status and accuracy is detailed below: + +- **Message Name (ID):** `SBG_ECOM_LOG_PTP_STATUS (57)` +- **Compatibility:** High Performance INS +- **Firmware:** ![HPINS](https://img.shields.io/badge/HPINS-5.3-blue) +- **Payload Size:** 76 bytes + +| Field | Description | Unit | Format | Size | Offset | +|----------------------------|------------------------------------------------------------------------------------|------|--------|------|--------| +| TIME_STAMP | Time since sensor is powered up. | µs | uint32 | 4 | 0 | +| STATE | Main PTP state (see [PTP_STATE](#PTP_STATE)). | - | uint8 | 1 | 4 | +| TIME_SCALE | Internal time scale (see [PTP_TIME_SCALE](#PTP_TIME_SCALE)). | - | uint8 | 1 | 5 | +| TIME_SCALE_OFFSET | Internal time scale offset, in seconds. | s | double | 8 | 6 | +| LOCAL_CLOCK_IDENTITY | Local clock identity, UINT64_MAX if invalid. | - | uint64 | 4 | 14 | +| LOCAL_CLOCK_PRIORITY1 | Local clock priority1 attribute. | - | uint8 | 4 | 22 | +| LOCAL_CLOCK_PRIORITY2 | Local clock priority2 attribute. | - | uint8 | 4 | 23 | +| LOCAL_CLOCK_CLASS | Local clock class attribute. | - | uint8 | 4 | 24 | +| LOCAL_CLOCK_ACCURACY | Local clock accuracy. | - | uint8 | 4 | 25 | +| LOCAL_CLOCK_LOG2_VARIANCE | Local clock variance expressed as an exponential (base 2). | - | uint16 | 4 | 26 | +| LOCAL_CLOCK_TIME_SOURCE | Local clock time source. | - | uint8 | 4 | 28 | +| MASTER_CLOCK_IDENTITY | Master clock identity, UINT64_MAX if invalid. | - | uint64 | 4 | 29 | +| MASTER_CLOCK_PRIORITY1 | Master clock priority1 attribute. | - | uint8 | 4 | 37 | +| MASTER_CLOCK_PRIORITY2 | Master clock priority2 attribute. | - | uint8 | 4 | 38 | +| MASTER_CLOCK_CLASS | Master clock class attribute. | - | uint8 | 4 | 39 | +| MASTER_CLOCK_ACCURACY | Master clock accuracy. | - | uint8 | 4 | 40 | +| MASTER_CLOCK_LOG2_VARIANCE | Master clock variance expressed as an exponential (base 2). | - | uint16 | 4 | 41 | +| MASTER_CLOCK_TIME_SOURCE | Master clock time source. | - | uint8 | 4 | 43 | +| MASTER_IP_ADDRESS | Master clock IP address, UINT32_MAX if invalid. | - | uint32 | 4 | 44 | +| MEAN_PATH_DELAY | Mean path delay to/from the master clock, NaN if not available. | s | float | 4 | 48 | +| MEAN_PATH_DELAY_STD_DEV | Mean path delay standard deviation, NaN if not available. | s | float | 4 | 52 | +| CLOCK_OFFSET | Offset between the local and master clocks, NaN if not available. | s | double | 8 | 56 | +| CLOCK_OFFSET_STD_DEV | Master clock offset standard deviation, NaN if not available. | s | float | 4 | 64 | +| CLOCK_FREQ_OFFSET | Offset between the frequency of the local and master clocks, NaN if not available. | Hz | float | 4 | 68 | +| CLOCK_FREQ_OFFSET_STD_DEV | Frequency offset standard deviation, NaN if not available. | Hz | float | 4 | 72 | + +> [!NOTE] +> The local clock members are valid if and only if the local clock identity is valid. +> Similarly, the master clock members are valid if and only if the master clock identity is valid. + +#### PTP State (STATE field) {#PTP_STATE} + +General PTP operating mode and status. + +| Value | Name | Description | +|-------|---------------------------------|------------------------------------| +| 0 | SBG_ECOM_LOG_PTP_STATE_DISABLED | PTP is disabled. | +| 1 | SBG_ECOM_LOG_PTP_STATE_FAULTY | The device is in the faulty state. | +| 2 | SBG_ECOM_LOG_PTP_STATE_MASTER | The device is the domain master. | +| 3 | SBG_ECOM_LOG_PTP_STATE_PASSIVE | The device is passive. | + +#### PTP Time Scale (TIME_SCALE field) {#PTP_TIME_SCALE} + +This field specifies the time scale used as the reference for broadcasting PTP time. +The reference time can also include a positive or negative offset value specified by `TIME_SCALE_OFFSET`. + +The IEEE 1588 standard specifies TAI (International Atomic Time) as the standard time scale for PTP (Precision Time Protocol). + +| Value | Name | Description | +|-------|---------------------------------|--------------------------------------------------------| +| 0 | SBG_ECOM_LOG_PTP_TIME_SCALE_TAI | Reference time is TAI (International Atomic Time) | +| 1 | SBG_ECOM_LOG_PTP_TIME_SCALE_UTC | Reference time is UTC (Coordinated Universal Time) | +| 2 | SBG_ECOM_LOG_PTP_TIME_SCALE_GPS | Reference time is GPS (Global Positioning System Time) | +## IMU Measurements + +This section details the messages used to report Inertial Measurement Unit (IMU) data as measured by the device's accelerometers and gyroscopes. +IMU measurements represent physical quantities, specifically accelerations in (m/s²) and angular rates in (rad/s) expressed in the device's body frame (X, Y, Z axes). + +All IMU measurements are factory-calibrated to compensate for thermal drift, alignment errors, scale factors, and non-linearity. +The IMU data is ready for use in an Inertial Navigation System (INS) filter to compute the device's orientation and navigation solution. + +Additionally, if any sensor frame alignment adjustments are made (such as rotating the device within a vehicle's frame), these will affect the reported IMU data. + +> [!NOTE] +> For vehicle rotation rates and accelerations compensated for gravity, earth rotation rate and residual errors, please check [SBG_ECOM_LOG_EKF_ROT_ACCEL_BODY](#SBG_ECOM_LOG_EKF_ROT_ACCEL_BODY) message. + +### IMU Data Synchronization Methods + +Inertial Measurement Unit (IMU) data can be processed either synchronously or asynchronously, depending on the product series. +The synchronization method determines how IMU data is collected, processed, and output in relation to the main INS computation loop. + +#### Synchronous IMU + +For products like the ELLIPSE series and IMUs such as the PULSE-40, IMU data is synchronized with the main computation loop of the device. +Users can configure the output rate down to 1 Hz, with the device automatically downsampling the data using a moving average filter. + +For instance, if the main sampling loop runs at 1 kHz and the output is set to 200 Hz, the device averages 5 samples to produce each output value. +Consequently, if 200 Hz is requested for both IMU and Extended Kalman Filter (EKF) data, exactly 200 messages per second for both IMU and EKF will be received. + +#### Asynchronous IMU + +High Performance INS products, such as the Navsight, Quanta, Ekinox, and Apogee series, operate with asynchronous IMU data. +In these systems, the IMU and INS do not share a common clock. +The IMU runs on its own clock, typically sampling at 200 Hz, while the INS operates its main processing loop also at 200 Hz but aligned with the GNSS time. + +During each INS processing cycle, the latest IMU data messages are transmitted. +Depending on the timing discrepancies between the IMU and INS clocks, there may be 0, 1, or 2 IMU messages per INS processing cycle. + +For example, if the system is set to output 200 Hz EKF data and IMU data triggered on a new data event, exactly 200 EKF messages per second will be received, while the number of IMU messages may slightly vary, influenced by the IMU clock's offset relative to the INS clock. + +### SBG_ECOM_LOG_IMU_DATA (03) ![DEPRECATED](https://img.shields.io/badge/DEPRECATED-red) {#SBG_ECOM_LOG_IMU_DATA} + +The message is **deprecated** and should not be used anymore. +It returns calibrated IMU data expressed in body frame as well as the IMU temperature. + +- **Message Name (ID):** `SBG_ECOM_LOG_IMU_DATA (03)` +- **Compatibility:** All products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.0-blue) ![HPINS](https://img.shields.io/badge/HPINS-1.0-blue) +- **Payload Size:** 58 bytes + +| Field | Description | Unit | Format | Size | Offset | +|-----------------------------|-----------------------------------------------------------------------------------|-------|--------|------|--------| +| TIME_STAMP | Time since sensor is powered up | µs | uint32 | 4 | 0 | +| IMU_STATUS | IMU status (see [IMU_DATA_STATUS](#IMU_DATA_STATUS)) | - | uint16 | 2 | 4 | +| ACCEL_0_X | Acceleration along the X axis in the body frame. | m/s² | float | 4 | 6 | +| ACCEL_0_Y | Acceleration along the Y axis in the body frame | m/s² | float | 4 | 10 | +| ACCEL_0_Z | Acceleration along the Z axis in the body frame | m/s² | float | 4 | 14 | +| GYRO_0_X | Rotation rate along the X axis in the body frame | rad/s | float | 4 | 18 | +| GYRO_0_Y | Rotation rate along the Y axis in the body frame | rad/s | float | 4 | 22 | +| GYRO_0_Z | Rotation rate along the Z axis in the body frame | rad/s | float | 4 | 26 | +| TEMPERATURE | IMU internal average temperature. | °C | float | 4 | 30 | +| ACCEL_1_X | Acceleration along the X axis in the body frame | m/s² | float | 4 | 34 | +| ACCEL_1_Y | Acceleration along the Y axis in the body frame | m/s² | float | 4 | 38 | +| ACCEL_1_Z | Acceleration along the Z axis in the body frame | m/s² | float | 4 | 42 | +| GYRO_1_X | Rotation rate along the X axis in the body frame | rad/s | float | 4 | 46 | +| GYRO_1_Y | Rotation rate along the Y axis in the body frame | rad/s | float | 4 | 50 | +| GYRO_1_Z | Rotation rate along the Z axis in the body frame | rad/s | float | 4 | 54 | + +> [!NOTE] +> The fields `ACCEL_0_*` and `ACCEL_1_*` as well as `GYRO_0_*` and `GYRO_1_*` provide identical values. They are maintained for backward compatibility purposes. + +> [!WARNING] +> Do not use this log for post-processing with software like Qinertia or for computing an INS solution. +> Instead, refer to the new [SBG_ECOM_LOG_IMU_SHORT](#SBG_ECOM_LOG_IMU_SHORT) message. + +#### IMU Data Status {#IMU_DATA_STATUS} + +The IMU_DATA_STATUS field is a bitmask that indicates the health and status of the IMU. +Each bit represents a specific status flag, providing detailed information on various components of the IMU system. + +| Bit | Name | Description | +|---------|---------------------------------|--------------------------------------------------------------------------------| +| 0 (LSB) | SBG_ECOM_IMU_COM_OK | Set to 1 if communication with the IMU is working properly. | +| 1 | SBG_ECOM_IMU_STATUS_BIT | Set to 1 if the IMU passes internal BIT, including calibration and CPU checks. | +| 2 | SBG_ECOM_IMU_ACCEL_X_BIT | Set to 1 if the X-axis accelerometer passes PBIT and CBIT. | +| 3 | SBG_ECOM_IMU_ACCEL_Y_BIT | Set to 1 if the Y-axis accelerometer passes PBIT and CBIT. | +| 4 | SBG_ECOM_IMU_ACCEL_Z_BIT | Set to 1 if the Z-axis accelerometer passes PBIT and CBIT. | +| 5 | SBG_ECOM_IMU_GYRO_X_BIT | Set to 1 if the X-axis gyroscope passes PBIT and CBIT. | +| 6 | SBG_ECOM_IMU_GYRO_Y_BIT | Set to 1 if the Y-axis gyroscope passes PBIT and CBIT. | +| 7 | SBG_ECOM_IMU_GYRO_Z_BIT | Set to 1 if the Z-axis gyroscope passes PBIT and CBIT. | +| 8 | SBG_ECOM_IMU_ACCELS_IN_RANGE | Set to 1 if all accelerometers are operating within the specified range. | +| 9 | SBG_ECOM_IMU_GYROS_IN_RANGE | Set to 1 if all gyroscopes are operating within the specified range. | + +> [!NOTE] +> **PBIT (Power-On Built-In Tests):** These tests are conducted once when the IMU is powered on, ensuring that all essential systems are functioning correctly at startup. +> **CBIT (Continuous Built-In Tests):** These tests are performed continuously during operation, monitoring the health and status of the IMU components in real-time. + +### SBG_ECOM_LOG_IMU_SHORT (22) {#SBG_ECOM_LOG_IMU_SHORT} + +The `SBG_ECOM_LOG_IMU_SHORT` message reports IMU data, including accelerations and rotation rates expressed in the IMU body frame, in a compact and efficient format. + +For High Performance INS, this log is asynchronous and can only be triggered on a *New Data* event. +For ELLIPSE v3 and IMUs like the PULSE-40, this log is synchronous ans can be output at any rate from 1 Hz up to the maximum rate supported by the device. +Typically 1 kHz for ELLIPSE v3 and up to 2 kHz for IMU-only products like the PULSE-40. + +To ensure the best dynamic range and LSB accuracy, measurements are provided using int32 integers with a fixed scaling. +For high rotation rates above ~1833°/s, a different high range scaling is applied. + +> [!NOTE] +> This message supersedes the deprecated [SBG_ECOM_LOG_IMU_DATA](#SBG_ECOM_LOG_IMU_DATA) message. + +- **Message Name (ID):** `SBG_ECOM_LOG_IMU_SHORT (22)` +- **Compatibility:** All products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) ![PULSE](https://img.shields.io/badge/PULSE-1.0-blue) +- **Payload Size:** 32 bytes + +| Field | Description | Unit | Format | Size | Offset | +|-----------------------------|-----------------------------------------------------------------------------------|-------|--------|------|--------| +| TIME_STAMP | Time since sensor is powered up | µs | uint32 | 4 | 0 | +| IMU_STATUS | IMU status (see [IMU_STATUS](#IMU_STATUS)) | - | uint16 | 2 | 4 | +| ACCELERATION_X | Acceleration along the X axis in the body frame (in LSB). | m/s² | int32 | 4 | 6 | +| ACCELERATION_Y | Acceleration along the Y axis in the body frame (in LSB). | m/s² | int32 | 4 | 10 | +| ACCELERATION_Z | Acceleration along the Z axis in the body frame (in LSB). | m/s² | int32 | 4 | 14 | +| RATE_X | Rotation rate along the X axis in the body frame (in LSB). | rad/s | int32 | 4 | 18 | +| RATE_Y | Rotation rate along the Y axis in the body frame (in LSB). | rad/s | int32 | 4 | 22 | +| RATE_Z | Rotation rate along the Z axis in the body frame (in LSB). | rad/s | int32 | 4 | 26 | +| TEMPERATURE | IMU internal average temperature (in LSB). | °C | int16 | 2 | 30 | + +#### Converting to Physical Quantities + +##### Temperature +The temperature is using a fixed scaling of 256 LSB for 1°C: *temperature = temperatureLSB / 256* + +##### Accelerations + +The accelerations are using a fixed scaling of 1048576 LSB for 1 m/s²: *acceleration = accelerationLSB / 1048576* + +##### Rotation Rates + +The rotation rates are using two different scaling: + - *Standard Scaling*: 67108864 LSB for 1 rad/s if `SBG_ECOM_IMU_GYROS_USE_HIGH_SCALE` flag is **not** set. + - *High Range Scaling*: 12304174 LSB for 1 rad/s if `SBG_ECOM_IMU_GYROS_USE_HIGH_SCALE` flag **is** set. + +The device automatically switches to high range scaling when any rotation rate component exceeds approximately 1833°/s. + +The following code snippet illustrates how to convert rotation rates from LSB to rad/s: + +```C + if (status & SBG_ECOM_IMU_GYROS_USE_HIGH_SCALE) + { + rate = rateLSB / 12304174.0; + } + else + { + rate = rateLSB / 67108864.0; + } +``` + +> [!NOTE] +> Most SBG Systems products, such as the ELLIPSE, have a rotation rate range below 1000°/s and thus only use the standard scaling. + +#### IMU Status {#IMU_STATUS} + +The IMU_STATUS field is a bitmask that indicates the health and status of the IMU. +Each bit represents a specific status flag, providing detailed information on various components of the IMU system. + +| Bit | Name | Description | +|---------|-----------------------------------|-------------------------------------------------------------------------------| +| 0 (LSB) | SBG_ECOM_IMU_COM_OK | Set if communication with the IMU is working properly. | +| 1 | SBG_ECOM_IMU_STATUS_BIT | Set if the IMU passes internal BIT, including calibration and CPU checks. | +| 2 | SBG_ECOM_IMU_ACCEL_X_BIT | Set if the X-axis accelerometer passes PBIT and CBIT. | +| 3 | SBG_ECOM_IMU_ACCEL_Y_BIT | Set if the Y-axis accelerometer passes PBIT and CBIT. | +| 4 | SBG_ECOM_IMU_ACCEL_Z_BIT | Set if the Z-axis accelerometer passes PBIT and CBIT. | +| 5 | SBG_ECOM_IMU_GYRO_X_BIT | Set if the X-axis gyroscope passes PBIT and CBIT. | +| 6 | SBG_ECOM_IMU_GYRO_Y_BIT | Set if the Y-axis gyroscope passes PBIT and CBIT. | +| 7 | SBG_ECOM_IMU_GYRO_Z_BIT | Set if the Z-axis gyroscope passes PBIT and CBIT. | +| 8 | SBG_ECOM_IMU_ACCELS_IN_RANGE | Set if all accelerometers are operating within the specified range. | +| 9 | SBG_ECOM_IMU_GYROS_IN_RANGE | Set if all gyroscopes are operating within the specified range. | +| 10 | SBG_ECOM_IMU_GYROS_USE_HIGH_SCALE | Set if the high scale scale factor is being used for rotation rates. | + +> [!NOTE] +> **PBIT (Power-On Built-In Tests):** These tests are conducted once when the IMU is powered on, ensuring that all essential systems are functioning correctly at startup. +> **CBIT (Continuous Built-In Tests):** These tests are performed continuously during operation, monitoring the health and status of the IMU components in real-time. +## Attitude, Velocity, Position + +The following logs present the output of the navigation unit, vehicle attitude (represented by Euler angles and quaternions), velocity, and position data. +These logs share common status flags and enumerations, which are detailed in the subsequent sections. + +Additionally, this section includes logs that provide compensated vehicle accelerations and rotation rates. +Unlike raw IMU data, these measurements have been processed by the INS Extended Kalman Filter to correct for sensor biases and scale factors. + +Furthermore, the effects of Earth's gravity and rotation rate are also removed, providing data that accurately reflects the vehicle's true dynamics. + +### SOLUTION_STATUS {#SOLUTION_STATUS} + +This status provides information about the Extended Kalman Filter (EKF), indicating which aiding data is used and the current solution mode. + +| Bit | Name | Description | +|-------|-------------------------------|------------------------------------------------------------------------------------| +| [0-3] | SBG_ECOM_SOLUTION_MODE | Indicates the Kalman filter computation mode (see [SOLUTION_MODE](#SOLUTION_MODE)) | +| 4 | SBG_ECOM_SOL_ATTITUDE_VALID | Set if attitude data is reliable (Roll/Pitch error within defined criteria). | +| 5 | SBG_ECOM_SOL_HEADING_VALID | Set if heading data is reliable (Heading error within defined criteria). | +| 6 | SBG_ECOM_SOL_VELOCITY_VALID | Set if velocity data is reliable (Velocity error within defined criteria). | +| 7 | SBG_ECOM_SOL_POSITION_VALID | Set if position data is reliable (Position error within defined criteria). | +| 8 | SBG_ECOM_SOL_VERT_REF_USED | Set if the vertical reference is used in the solution. | +| 9 | SBG_ECOM_SOL_MAG_REF_USED | Set if magnetometer is used in the solution. | +| 10 | SBG_ECOM_SOL_GPS1_VEL_USED | Set if GNSS 1 velocity is used in the solution. | +| 11 | SBG_ECOM_SOL_GPS1_POS_USED | Set if GNSS 1 position is used in the solution. | +| 13 | SBG_ECOM_SOL_GPS1_HDT_USED | Set if GNSS 1 true heading is used in the solution. | +| 14 | SBG_ECOM_SOL_GPS2_VEL_USED | Set if GNSS 2 velocity is used in the solution. | +| 15 | SBG_ECOM_SOL_GPS2_POS_USED | Set if GNSS 2 position is used in the solution. | +| 17 | SBG_ECOM_SOL_GPS2_HDT_USED | Set if GNSS 2 true heading is used in the solution. | +| 18 | SBG_ECOM_SOL_ODO_USED | Set if odometer velocity is used in the solution. | +| 19 | SBG_ECOM_SOL_DVL_BT_USED | Set if DVL bottom tracking velocity is used in the solution. | +| 20 | SBG_ECOM_SOL_DVL_WT_USED | Set if DVL water layer velocity is used in the solution. | +| 24 | SBG_ECOM_SOL_USBL_USED | Set if USBL position is used in the solution. | +| 25 | SBG_ECOM_SOL_AIR_DATA_USED | Set if altitude or true airspeed is used in the solution. | +| 26 | SBG_ECOM_SOL_ZUPT_USED | Set if a Zero Velocity Update (ZUPT) is is used in the solution. | +| 27 | SBG_ECOM_SOL_ALIGN_VALID | Set if sensor alignment and residual sensors errors have fully converged. | +| 28 | SBG_ECOM_SOL_DEPTH_USED | Set if depth sensor is used in the solution (subsea navigation). | +| 29 | SBG_ECOM_SOL_ZARU_USED | Set if a Zero Angular Rate Update (ZARU) is used in the solution. | + +#### SOLUTION_MODE {#SOLUTION_MODE} + +| Value | Name | Description | +|-------|---------------------------------|-----------------------------------------------------------------------------| +| 0 | SBG_ECOM_SOL_MODE_UNINITIALIZED | Kalman filter is not initialized; data is invalid. | +| 1 | SBG_ECOM_SOL_MODE_VERTICAL_GYRO | Computes roll and pitch; heading and position drift freely. | +| 2 | SBG_ECOM_SOL_MODE_AHRS | Full orientation with heading reference; navigation data may drift. | +| 3 | SBG_ECOM_SOL_MODE_NAV_VELOCITY | Deprecated. Not in use. | +| 4 | SBG_ECOM_SOL_MODE_NAV_POSITION | Full computation of attitude, velocity, and position. | + +> [!NOTE] +> The *Used in Solution* flags have a 3s timeout mechanism for easier interpretation. +> For instance, the SBG_ECOM_SOL_ODO_USED flag remains valid for 3 seconds after the odometer was last used in the solution. + +### SBG_ECOM_LOG_EKF_EULER (06) {#SBG_ECOM_LOG_EKF_EULER} + +This log provides the Euler angles (roll, pitch, yaw) and their associated accuracies, along with the AHRS/INS solution status. + +- **Message Name (ID):** `SBG_ECOM_LOG_EKF_EULER (06)` +- **Compatibility:** AHRS/INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.0-blue) ![HPINS](https://img.shields.io/badge/HPINS-1.0-blue) +- **Payload Size:** 32 bytes + +| Field | Description | Unit | Format | Size | Offset | +|-----------------|---------------------------------------------------------------|-------|--------|------|--------| +| TIME_STAMP | Time since the sensor was powered up | µs | uint32 | 4 | 0 | +| ROLL | Roll angle | rad | float | 4 | 4 | +| PITCH | Pitch angle | rad | float | 4 | 8 | +| YAW | Yaw angle (geographic heading) | rad | float | 4 | 12 | +| ROLL_ACC | 1σ standard deviation of roll angle accuracy | rad | float | 4 | 16 | +| PITCH_ACC | 1σ standard deviation of pitch angle accuracy | rad | float | 4 | 20 | +| YAW_ACC | 1σ standard deviation of yaw angle accuracy | rad | float | 4 | 24 | +| SOLUTION_STATUS | INS solution status (see [SOLUTION_STATUS](#SOLUTION_STATUS)) | - | uint32 | 4 | 28 | + +### SBG_ECOM_LOG_EKF_QUAT (07) {#SBG_ECOM_LOG_EKF_QUAT} + +This log provides the INS orientation using quaternions which avoid gimbal lock and are valid for all orientations. +It also includes the standard deviation accuracies for roll, pitch, and yaw angles, along with the AHRS/INS solution status. + +- **Message Name (ID):** `SBG_ECOM_LOG_EKF_QUAT (07)` +- **Compatibility:** AHRS/INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.0-blue) ![HPINS](https://img.shields.io/badge/HPINS-1.0-blue) +- **Payload Size:** 36 bytes + +| Field | Description | Unit | Format | Size | Offset | +|-----------------|---------------------------------------------------------------|-------|--------|------|--------| +| TIME_STAMP | Time since the sensor was powered up | µs | uint32 | 4 | 0 | +| Q0 | First quaternion parameter (W component) | - | float | 4 | 4 | +| Q1 | Second quaternion parameter (X component) | - | float | 4 | 8 | +| Q2 | Third quaternion parameter (Y component) | - | float | 4 | 12 | +| Q3 | Fourth quaternion parameter (Z component) | - | float | 4 | 16 | +| ROLL_ACC | 1σ standard deviation of roll angle accuracy | rad | float | 4 | 20 | +| PITCH_ACC | 1σ standard deviation of pitch angle accuracy | rad | float | 4 | 24 | +| YAW_ACC | 1σ standard deviation of yaw angle accuracy | rad | float | 4 | 28 | +| SOLUTION_STATUS | INS solution status (see [SOLUTION_STATUS](#SOLUTION_STATUS)) | - | uint32 | 4 | 32 | + +### SBG_ECOM_LOG_EKF_NAV (08) {#SBG_ECOM_LOG_EKF_NAV} + +This log provides the INS velocity in the North-East-Down (NED) coordinate system and position in Latitude, Longitude, and Altitude. +It also includes accuracy parameters for each measurement along with the INS solution status. + +- **Message Name (ID):** `SBG_ECOM_LOG_EKF_NAV (08)` +- **Compatibility:** INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.0-blue) ![HPINS](https://img.shields.io/badge/HPINS-1.0-blue) +- **Payload Size:** 72 bytes + +| Field | Description | Unit | Format | Size | Offset | +|-----------------|---------------------------------------------------------------|-------|--------|------|--------| +| TIME_STAMP | Time since the sensor was powered up | µs | uint32 | 4 | 0 | +| VELOCITY_N | Velocity in the North direction | m/s | float | 4 | 4 | +| VELOCITY_E | Velocity in the East direction | m/s | float | 4 | 8 | +| VELOCITY_D | Velocity in the Down direction | m/s | float | 4 | 12 | +| VELOCITY_N_ACC | 1σ standard deviation of velocity accuracy in North direction | m/s | float | 4 | 16 | +| VELOCITY_E_ACC | 1σ standard deviation of velocity accuracy in East direction | m/s | float | 4 | 20 | +| VELOCITY_D_ACC | 1σ standard deviation of velocity accuracy in Down direction | m/s | float | 4 | 24 | +| LATITUDE | Geographic latitude, positive North. | ° | double | 8 | 28 | +| LONGITUDE | Geographic longitude, positive East. | ° | double | 8 | 36 | +| ALTITUDE | Altitude above Mean Sea Level (MSL), positive upward. | m | double | 8 | 44 | +| UNDULATION | Difference between geoid and ellipsoid altitude | m | float | 4 | 52 | +| LATITUDE_ACC | 1σ standard deviation of latitude accuracy | m | float | 4 | 56 | +| LONGITUDE_ACC | 1σ standard deviation of longitude accuracy | m | float | 4 | 60 | +| ALTITUDE_ACC | 1σ standard deviation of vertical position accuracy | m | float | 4 | 64 | +| SOLUTION_STATUS | INS solution status (see [SOLUTION_STATUS](#SOLUTION_STATUS)) | - | uint32 | 4 | 68 | + +> [!NOTE] +> The undulation can be used to compute the height above ellipsoid from the MSL altitude. +> Please use the following formula: *height = altitude + undulation* + +### SBG_ECOM_LOG_EKF_VEL_BODY (54) {#SBG_ECOM_LOG_EKF_VEL_BODY} + +This log provides the INS velocity expressed in the body/vehicle frame. + +When the INS is correctly configured, the body frame aligns with the vehicle frame. +For instance, in a car, the X-axis points forward, and the Y-axis points to the right. + +- **Message Name (ID):** `SBG_ECOM_LOG_EKF_VEL_BODY (54)` +- **Compatibility:** INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-3.0-blue) ![HPINS](https://img.shields.io/badge/HPINS-4.0-blue) +- **Payload Size:** 32 bytes + +| Field | Description | Unit | Format | Size | Offset | +|-----------------|--------------------------------------------------------------------|-------|--------|------|--------| +| TIME_STAMP | Time since the sensor was powered up | µs | uint32 | 4 | 0 | +| SOLUTION_STATUS | INS solution status (see [SOLUTION_STATUS](#SOLUTION_STATUS)) | - | uint32 | 4 | 4 | +| VELOCITY_X | Velocity along the X-axis (forward direction in vehicle frame) | m/s | float | 4 | 8 | +| VELOCITY_Y | Velocity along the Y-axis (right direction in vehicle frame) | m/s | float | 4 | 12 | +| VELOCITY_Z | Velocity along the Z-axis (downward direction in vehicle frame) | m/s | float | 4 | 16 | +| VELOCITY_X_ACC | 1σ standard deviation of velocity accuracy along the X-axis | m/s | float | 4 | 20 | +| VELOCITY_Y_ACC | 1σ standard deviation of velocity accuracy along the Y-axis | m/s | float | 4 | 24 | +| VELOCITY_Z_ACC | 1σ standard deviation of velocity accuracy along the Z-axis | m/s | float | 4 | 28 | + +### SBG_ECOM_LOG_EKF_ROT_ACCEL_BODY (52) {#SBG_ECOM_LOG_EKF_ROT_ACCEL_BODY} + +This log provides the corrected rotation rates and accelerations in the INS body/vehicle frame. +The data is compensated by the Navigation Filter for sensor bias, scale factor, earth rotation, and gravity. + +- **Message Name (ID):** `SBG_ECOM_LOG_EKF_ROT_ACCEL_BODY (52)` +- **Compatibility:** INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-3.0-blue) ![HPINS](https://img.shields.io/badge/HPINS-4.0-blue) +- **Payload Size:** 32 bytes + +| Field | Description | Unit | Format | Size | Offset | +|-----------------|--------------------------------------------------------------------|-------|--------|------|--------| +| TIME_STAMP | Time since the sensor was powered up | µs | uint32 | 4 | 0 | +| SOLUTION_STATUS | INS solution status (see [SOLUTION_STATUS](#SOLUTION_STATUS)) | - | uint32 | 4 | 4 | +| RATE_X | Rotation rate around the X axis | rad/s | float | 4 | 8 | +| RATE_Y | Rotation rate around the Y axis | rad/s | float | 4 | 12 | +| RATE_Z | Rotation rate around the Z axis | rad/s | float | 4 | 16 | +| ACCELERATION_X | Acceleration along the X axis | m/s² | float | 4 | 20 | +| ACCELERATION_Y | Acceleration along the Y axis | m/s² | float | 4 | 24 | +| ACCELERATION_Z | Acceleration along the Z axis | m/s² | float | 4 | 28 | + +> [!NOTE] +> This log is properly decimated based on the selected output rate to prevent aliasing issues. + +### SBG_ECOM_LOG_EKF_ROT_ACCEL_NED (53) {#SBG_ECOM_LOG_EKF_ROT_ACCEL_NED} + +This log provides the corrected rotation rates and accelerations in the North, East, Down (NED) navigation frame. +The data is compensated by the Navigation Filter for sensor bias, scale factor, earth rotation, and gravity. + +This log is particularly useful for determining a vehicle's turn rate, such as the down rotation rate. + +- **Message Name (ID):** `SBG_ECOM_LOG_EKF_ROT_ACCEL_NED (53)` +- **Compatibility:** INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-3.0-blue) ![HPINS](https://img.shields.io/badge/HPINS-4.0-blue) +- **Payload Size:** 32 bytes + +| Field | Description | Unit | Format | Size | Offset | +|-----------------|--------------------------------------------------------------------|-------|--------|------|--------| +| TIME_STAMP | Time since the sensor was powered up | µs | uint32 | 4 | 0 | +| SOLUTION_STATUS | INS solution status (see [SOLUTION_STATUS](#SOLUTION_STATUS)) | - | uint32 | 4 | 4 | +| RATE_N | Rotation rate around the North axis in the navigation frame | rad/s | float | 4 | 8 | +| RATE_E | Rotation rate around the East axis in the navigation frame | rad/s | float | 4 | 12 | +| RATE_D | Rotation rate around the Down axis in the navigation frame | rad/s | float | 4 | 16 | +| ACCELERATION_N | Acceleration along the North axis in the navigation frame | m/s² | float | 4 | 20 | +| ACCELERATION_E | Acceleration along the East axis in the navigation frame | m/s² | float | 4 | 24 | +| ACCELERATION_D | Acceleration along the Down axis in the navigation frame | m/s² | float | 4 | 28 | + +> [!NOTE] +> This log is properly decimated based on the selected output rate to prevent aliasing issues. + +> [!WARNING] +> Rotation rates around the North and East navigation axes are not commonly used. +> In most cases, rotation rates in the body X and Y axes are preferable. +## Surge, Sway, Heave + +This section covers output measurements specific to marine applications, including Surge, Sway, and Heave. + +High-performance INS units with marine options support both real-time and delayed heave measurements. +The ELLIPSE Series, however, offers real-time heave measurements only, without surge or sway channels. + +Real-time heave delivers immediate data, whereas delayed heave offers higher accuracy with a 150-second time delay. + +Real-time and delayed ship motion outputs share common conventions, message structures, status flags, and enumerations, as detailed below. + +### Output frame and monitoring point + +Ship motion measurements are defined in a vessel-specific coordinate frame: +- **Heave**: Vertical displacement, (positive downward) +- **Surge**: Longitudinal displacement (positive toward the bow/forward). +- **Sway**: Transverse displacement (positive toward the starboard side/right). + +The heave measurements are affected by the selected output monitoring point and will be re-located accordingly. +However, surge/sway/velocity and accelerations values are only valid when output at the IMU monitoring point. + +### Ship Motion Status {#SHIP_MOTION_STATUS} + +The status field indicates the validity and availability of the ship motion output fields. +It is crucial to check this status to determine which fields are active and if the data is valid. + +| Bit | Name | Description | +|-----|------------------------------------|---------------------------------------------------------------------------------| +| 0 | SBG_ECOM_HEAVE_VALID | Set if heave has stabilized (see [SBG_ECOM_HEAVE_VALID](#SBG_ECOM_HEAVE_VALID)) | +| 1 | SBG_ECOM_HEAVE_VEL_AIDED | Set if the heave output is compensated for transient accelerations | +| 2 | SBG_ECOM_HEAVE_SURGE_SWAY_INCLUDED | Set if surge and sway components are included in this output | +| 3 | SBG_ECOM_HEAVE_PERIOD_INCLUDED | Set if the swell period is provided | +| 4 | SBG_ECOM_HEAVE_PERIOD_VALID | Set if the swell period data is valid | +| 5 | SBG_ECOM_HEAVE_SWELL_MODE | Set if real-time heave uses swell mode computations | + +#### SBG_ECOM_HEAVE_VALID flag {#SBG_ECOM_HEAVE_VALID} + +The `SBG_ECOM_HEAVE_VALID` flag indicates whether the heave measurement is valid and stable, as it relies on high-pass filtering that requires convergence. +This flag is cleared under the following conditions: + - At startup or before convergence + - After a turn occurs without velocity aiding + - When heave measurements reach extreme values + - If a significant step occurs, necessitating filter re-convergence + - In the event of an internal system failure + +### SBG_ECOM_LOG_SHIP_MOTION (09) {#SBG_ECOM_LOG_SHIP_MOTION} + +This log provides real-time ship motion data, including main heave period, surge, sway, heave, velocity, and accelerations. + +- **Message Name (ID):** `SBG_ECOM_LOG_SHIP_MOTION (09)` +- **Compatibility:** AHRS/INS with marine/hydro options +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.0-blue) ![HPINS](https://img.shields.io/badge/HPINS-1.0-blue) +- **Payload Size:** 46 bytes + +| Field | Description | Unit | Format | Size | Offset | +|---------------|---------------------------------------------------------------------------|-------|--------|------|--------| +| TIME_STAMP | Time since the sensor was powered up | µs | uint32 | 4 | 0 | +| HEAVE_PERIOD | Main heave period | s | float | 4 | 4 | +| SURGE | Surge longitudinal displacement (positive forward) | m | float | 4 | 8 | +| SWAY | Sway lateral displacement (positive right) | m | float | 4 | 12 | +| HEAVE | Heave vertical displacement (positive down) | m | float | 4 | 16 | +| ACCEL_X | Longitudinal acceleration (positive forward) | m/s² | float | 4 | 20 | +| ACCEL_Y | Lateral acceleration (positive right) | m/s² | float | 4 | 24 | +| ACCEL_Z | Vertical acceleration (positive down) | m/s² | float | 4 | 28 | +| VEL_X | Longitudinal velocity (positive forward) | m/s | float | 4 | 32 | +| VEL_Y | Lateral velocity (positive right) | m/s | float | 4 | 36 | +| VEL_Z | Vertical velocity (positive down) | m/s | float | 4 | 40 | +| STATUS | Ship motion output status (see [SHIP_MOTION_STATUS](#SHIP_MOTION_STATUS)) | - | uint32 | 4 | 28 | + +> [!NOTE] +> ELLIPSE series only computes and outputs HEAVE_PERIOD, HEAVE, ACCEL_Z and VEL_Z values. +> All other fields are set to zero and reported as invalid in the `STATUS` bitmask. + +> [!WARNING] +> Surge, sway, velocities and accelerations values are only valid when output at the IMU monitoring point. + +### SBG_ECOM_LOG_SHIP_MOTION_HP (32) {#SBG_ECOM_LOG_SHIP_MOTION_HP} + +This log provides delayed heave measurements, offering higher accuracy compared to real-time at the cost of approximately 150 seconds of delayed output. + +The timestamp reported in the message reflects the actual time of data validity. +The data corresponds to conditions roughly 150 seconds earlier than other logs transmitted at the same time. + +- **Message Name (ID):** `SBG_ECOM_LOG_SHIP_MOTION_HP (32)` +- **Compatibility:** High Performance INS (marine option) +- **Firmware:** ![HPINS](https://img.shields.io/badge/HPINS-2.0-blue) +- **Payload Size:** 46 bytes + +| Field | Description | Unit | Format | Size | Offset | +|---------------|---------------------------------------------------------------------------|-------|--------|------|--------| +| TIME_STAMP | Time since the sensor was powered up | µs | uint32 | 4 | 0 | +| HEAVE_PERIOD | Reserved for future use. Always reported as 0 | s | float | 4 | 4 | +| SURGE | Reserved for future use. Always reported as 0 | m | float | 4 | 8 | +| SWAY | Reserved for future use. Always reported as 0 | m | float | 4 | 12 | +| HEAVE | Heave vertical displacement (positive down) | m | float | 4 | 16 | +| ACCEL_X | Reserved for future use. Always reported as 0 | m/s² | float | 4 | 20 | +| ACCEL_Y | Reserved for future use. Always reported as 0 | m/s² | float | 4 | 24 | +| ACCEL_Z | Vertical acceleration (positive down) | m/s² | float | 4 | 28 | +| VEL_X | Reserved for future use. Always reported as 0 | m/s | float | 4 | 32 | +| VEL_Y | Reserved for future use. Always reported as 0 | m/s | float | 4 | 36 | +| VEL_Z | Vertical velocity (positive down) | m/s | float | 4 | 40 | +| STATUS | Ship motion output status (see [SHIP_MOTION_STATUS](#SHIP_MOTION_STATUS)) | - | uint32 | 4 | 28 | + +> [!NOTE] +> The `SBG_ECOM_LOG_SHIP_MOTION_HP` message only computes and outputs delayed heave, vertical velocity and acceleration. +> All other fields are reported as 0 and the `STATUS` bitmask is set accordingly. + +> [!WARNING] +> Vertical velocity and acceleration are only valid when output at the IMU monitoring point. +## GNSS aiding (PVT, HDT) + +The following logs provide the navigation unit's output, including Euler angles, quaternions, velocity and position. +These logs share common status flags and enumerations, detailed below. + +### GNSS Velocity {#SBG_ECOM_LOG_GPSX_VEL} + +This section details the velocity and course information from the primary or secondary GNSS receiver. +The timestamp reflects the actual time of the GNSS velocity data, independent of the main processing loop. + +- **Message Name (ID):** `SBG_ECOM_LOG_GPS1_VEL (13)`, `SBG_ECOM_LOG_GPS2_VEL (16)` +- **Compatibility:** INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.0-blue) ![HPINS](https://img.shields.io/badge/HPINS-1.0-blue) +- **Payload Size:** 44 bytes + +| Field | Description | Unit | Format | Size | Offset | +|-----------------|-------------------------------------------------------------------------------------|-------|---------|------|--------| +| TIME_STAMP | Time since sensor was powered up | µs | uint32 | 4 | 0 | +| STATUS_TYPE | Velocity solution type and status (see [GPS_VEL_STATUS_TYPE](#GPS_VEL_STATUS_TYPE)) | - | uint32 | 4 | 4 | +| TOW | GPS Time of Week | ms | uint32 | 4 | 8 | +| VEL_N | Velocity in North direction | m/s | float | 4 | 12 | +| VEL_E | Velocity in East direction | m/s | float | 4 | 16 | +| VEL_D | Velocity in Down direction | m/s | float | 4 | 20 | +| VEL_ACC_N | 1σ Accuracy in North direction | m/s | float | 4 | 24 | +| VEL_ACC_E | 1σ Accuracy in East direction | m/s | float | 4 | 28 | +| VEL_ACC_D | 1σ Accuracy in Down direction | m/s | float | 4 | 32 | +| COURSE | True direction of motion over ground | ° | float | 4 | 36 | +| COURSE_ACC | 1σ course accuracy | ° | float | 4 | 40 | + +#### Status and Solution Type Field {#GPS_VEL_STATUS_TYPE} + +The `STATUS_TYPE` field provides both the GNSS velocity solution status and the type of computed velocity, encoded as two separate enumerations. + +| Bits | Type | Name | Description | +|--------|-------|--------------------------|------------------------------------------------------------------| +| [0-5] | Enum | SBG_ECOM_GPS_VEL_STATUS | The GPS velocity status (see [GPS_VEL_STATUS](#GPS_VEL_STATUS)). | +| [6-11] | Enum | SBG_ECOM_GPS_VEL_TYPE | The GPS velocity type (see [GPS_VEL_STATUS](#GPS_VEL_STATUS)). | + +##### GNSS Velocity Status Enumeration {#GPS_VEL_STATUS} + +Indicates the general status of the GNSS regarding velocity computation. + +| Value | Name | Description | +|-------|------------------------------|----------------------------------------------| +| 0 | SBG_ECOM_VEL_SOL_COMPUTED | A valid solution has been computed. | +| 1 | SBG_ECOM_VEL_INSUFFICIENT_OBS| Not enough valid SVs to compute a solution. | +| 2 | SBG_ECOM_VEL_INTERNAL_ERROR | An internal error has occurred. | +| 3 | SBG_ECOM_VEL_LIMIT | Velocity limit exceeded. | + +##### GNSS Velocity Type Enumeration {#GPS_VEL_TYPE} + +Specifies the type of the computed velocity solution. + +| Value | Name | Description | +|-------|----------------------------|------------------------------------------------------------------------------------------------------------| +| 0 | SBG_ECOM_VEL_NO_SOLUTION | No valid velocity solution available. | +| 1 | SBG_ECOM_VEL_UNKNOWN_TYPE | An unknown solution type has been computed. | +| 2 | SBG_ECOM_VEL_DOPPLER | A Doppler velocity has been computed. | +| 3 | SBG_ECOM_VEL_DIFFERENTIAL | A velocity has been determined by calculating the difference between two sequential position measurements. | + +> [!NOTE] +> Ensure that both the GNSS velocity solution status and type are checked to validate the output velocity data. + +### GNSS Position {#SBG_ECOM_LOG_GPSX_POS} + +This section provides position information from the primary or secondary GNSS receiver. +The timestamp indicates the actual GPS position data time, not synchronized with the main processing loop. + +- **Message Name (ID):** `SBG_ECOM_LOG_GPS1_POS (14)`, `SBG_ECOM_LOG_GPS2_POS (17)` +- **Compatibility:** INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.0-blue) ![HPINS](https://img.shields.io/badge/HPINS-1.0-blue) +- **Payload Size:** 59 bytes + +| Field | Description | Unit | Format | Size | Offset | +|-----------------|--------------------------------------------------------------------------------------------|-------|---------|------|--------| +| TIME_STAMP | Time since the sensor was powered up | µs | uint32 | 4 | 0 | +| STATUS_TYPE | Position solution type and status (see [GPS_POS_STATUS_TYPE](#GPS_POS_STATUS_TYPE)) | - | uint32 | 4 | 4 | +| TOW | GNSS Time of Week | ms | uint32 | 4 | 8 | +| LATITUDE | Latitude, positive North | ° | double | 8 | 12 | +| LONGITUDE | Longitude, positive East | ° | double | 8 | 20 | +| ALTITUDE | Altitude Above Mean Sea Level | m | double | 8 | 28 | +| UNDULATION | Altitude difference between the geoid and the Ellipsoid | m | float | 4 | 36 | +| LAT_ACC | 1σ Latitude Accuracy | m | float | 4 | 40 | +| LONG_ACC | 1σ Longitude Accuracy | m | float | 4 | 44 | +| ALTI_ACC | 1σ Altitude Accuracy | m | float | 4 | 48 | +| NUM_SV_USED | Number of space vehicles used in GNSS solution - 0xFF if N/A (since v1.4) | - | uint8 | 1 | 52 | +| BASE_STATION_ID | ID of the DGPS/RTK base station in use - 0xFFFF if N/A (since v1.4) | - | uint16 | 2 | 53 | +| DIFF_AGE | Differential data age - 0xFFFF if N/A (since v1.4) | 0.01s | uint16 | 2 | 55 | +| NUM_SV_TRACKED | Number of tracked SV - 0xFF if N/A (since v4.0) | - | uint8 | 1 | 57 | +| STATUS_EXT | Interference/spoofing status (see [GPS_POS_STATUS_EXT](#GPS_POS_STATUS_EXT)). (since v4.0) | - | uint32 | 4 | 58 | + +#### Status and Solution Type Field {#GPS_POS_STATUS_TYPE} + +The `STATUS_TYPE` field provides both the GNSS position solution status and the type of computed position, encoded as two separate enumerations. +It also includes bitmasks to reports which signals are being used to compute the solution. + +| Bits | Type | Name | Description | +|--------|-------|---------------------------------|---------------------------------------------------------------------| +| [0-5] | Enum | SBG_ECOM_GPS_POS_STATUS | GNSS position status (see [GPS_POS_STATUS](#GPS_POS_STATUS)) | +| [6-11] | Enum | SBG_ECOM_GPS_POS_TYPE | GNSS position type (see [GPS_POS_TYPE](#GPS_POS_TYPE)) | +| 12 | Mask | SBG_ECOM_GPS_POS_GPS_L1_USED | Set if GPS L1CA/L1P is used in the solution | +| 13 | Mask | SBG_ECOM_GPS_POS_GPS_L2_USED | Set if GPS L2P/L2C is used in the solution | +| 14 | Mask | SBG_ECOM_GPS_POS_GPS_L5_USED | Set if GPS L5 is used in the solution | +| 15 | Mask | SBG_ECOM_GPS_POS_GLO_L1_USED | Set if GLONASS L1CA is used in the solution | +| 16 | Mask | SBG_ECOM_GPS_POS_GLO_L2_USED | Set if GLONASS L2C/L2P is used in the solution | +| 17 | Mask | SBG_ECOM_GPS_POS_GLO_L3_USED | Set if GLONASS L3 is used in the solution | +| 18 | Mask | SBG_ECOM_GPS_POS_GAL_E1_USED | Set if Galileo E1 is used in the solution | +| 19 | Mask | SBG_ECOM_GPS_POS_GAL_E5A_USED | Set if Galileo E5a is used in the solution | +| 20 | Mask | SBG_ECOM_GPS_POS_GAL_E5B_USED | Set if Galileo E5b is used in the solution | +| 21 | Mask | SBG_ECOM_GPS_POS_GAL_E5ALT_USED | Set if Galileo E5 AltBoc is used in the solution | +| 22 | Mask | SBG_ECOM_GPS_POS_GAL_E6_USED | Set if Galileo E6 is used in the solution | +| 23 | Mask | SBG_ECOM_GPS_POS_BDS_B1_USED | Set if BeiDou B1 is used in the solution | +| 24 | Mask | SBG_ECOM_GPS_POS_BDS_B2_USED | Set if BeiDou B2 is used in the solution | +| 25 | Mask | SBG_ECOM_GPS_POS_BDS_B3_USED | Set if BeiDou B3 is used in the solution | +| 26 | Mask | SBG_ECOM_GPS_POS_QZSS_L1_USED | Set if QZSS L1CA is used in the solution | +| 27 | Mask | SBG_ECOM_GPS_POS_QZSS_L2_USED | Set if QZSS L2C is used in the solution | +| 28 | Mask | SBG_ECOM_GPS_POS_QZSS_L5_USED | Set if QZSS L5 is used in the solution | + +##### GNSS Position Status Enumeration {#GPS_POS_STATUS} + +Specifies the type of the computed velocity solution. + +| Value | Name | Description | +|-------|--------------------------------|------------------------------------------------| +| 0 | SBG_ECOM_POS_SOL_COMPUTED | A valid solution has been computed | +| 1 | SBG_ECOM_POS_INSUFFICIENT_OBS | Not enough valid SV to compute a solution | +| 2 | SBG_ECOM_POS_INTERNAL_ERROR | An internal error has occurred | +| 3 | SBG_ECOM_POS_HEIGHT_LIMIT | The height limit has been exceeded | + +##### GNSS Position Type Enumeration {#GPS_POS_TYPE} + +Specifies the type of the computed velocity solution. + +| Value | Name | Description | +|-------|----------------------------|----------------------------------------------------| +| 0 | SBG_ECOM_POS_NO_SOLUTION | No valid solution available | +| 1 | SBG_ECOM_POS_UNKNOWN_TYPE | An unknown solution type has been computed | +| 2 | SBG_ECOM_POS_SINGLE | Single point solution position | +| 3 | SBG_ECOM_POS_PSRDIFF | Standard Pseudorange Differential Solution (DGPS) | +| 4 | SBG_ECOM_POS_SBAS | SBAS satellite used for differential corrections | +| 5 | SBG_ECOM_POS_OMNISTAR | Omnistar VBS Position (L1 sub-meter) | +| 6 | SBG_ECOM_POS_RTK_FLOAT | Floating RTK ambiguity solution (20 cm RTK) | +| 7 | SBG_ECOM_POS_RTK_INT | Integer RTK ambiguity solution (2 cm RTK) | +| 8 | SBG_ECOM_POS_PPP_FLOAT | Precise Point Positioning with float ambiguities | +| 9 | SBG_ECOM_POS_PPP_INT | Precise Point Positioning with fixed ambiguities | +| 10 | SBG_ECOM_POS_FIXED | Fixed location solution position | + +> [!NOTE] +> Ensure that both the GNSS position solution status and type are checked to validate the output position data. + +##### STATUS_EXT Definition {#GPS_POS_STATUS_EXT} + +The `STATUS_EXT` field provides additional status details, including interference monitoring and spoofing detection. +The field encodes several enumerations as described below: + +| Bits | Type | Name | Description | +|--------|-------|-------------------------------|----------------------------------------------------------------------------------------------| +| [0-3] | Enum | SBG_ECOM_GNSS_IFM_STATUS | GNSS interference monitoring and mitigation (see [GNSS_IFM_STATUS](#GNSS_IFM_STATUS)). | +| [4-7] | Enum | SBG_ECOM_GNSS_SPOOFING_STATUS | GNSS spoofing monitoring and mitigation (see [GNSS_SPOOFING_STATUS](#GNSS_SPOOFING_STATUS)). | +| [8-11] | Enum | SBG_ECOM_GNSS_OSNMA_STATUS | Galileo OSNMA authentication status (see [GNSS_OSNMA_STATUS](#GNSS_OSNMA_STATUS)). | + +##### Interference Monitoring Status Enumeration {#GNSS_IFM_STATUS} + +| Value | Name | Description | +|-------|--------------------------------|-------------------------------------------------------------------------| +| 0 | IFM_STATUS_ERROR | The interference monitoring system is in error and not operational | +| 1 | IFM_STATUS_UNKNOWN | Interference monitoring is disabled or not available | +| 2 | IFM_STATUS_CLEAN | Interference monitored and the environment is clean | +| 3 | IFM_STATUS_MITIGATED | Interference detected and mitigated, PVT is valid | +| 4 | IFM_STATUS_CRITICAL | Interference detected and not mitigated, PVT is invalid | + +##### Spoofing Status Enumeration {#GNSS_SPOOFING_STATUS} + +| Value | Name | Description | +|-------|--------------------------------|-------------------------------------------------------------------------| +| 0 | SPOOFING_STATUS_ERROR | Spoofing detection system error, not operational | +| 1 | SPOOFING_STATUS_UNKNOWN | Spoofing detection disabled or not available | +| 2 | SPOOFING_STATUS_CLEAN | Spoofing detection enabled, no spoofing detected | +| 3 | SPOOFING_STATUS_SINGLE | Probable spoofing detected by one method | +| 4 | SPOOFING_STATUS_MULTIPLE | Confirmed spoofing detected by multiple methods (Galileo OSNMA, RAIM). | + +##### OSNMA Status Enumeration {#GNSS_OSNMA_STATUS} + +| Value | Name | Description | +|-------|--------------------------------|-------------------------------------------------------------------------| +| 0 | OSNMA_STATUS_ERROR | OSNMA is in error state and doesn't work. | +| 1 | OSNMA_STATUS_DISABLED | OSNMA is either disabled or not available. | +| 2 | OSNMA_STATUS_INITIALIZING | OSNMA initialization in progress. | +| 3 | OSNMA_STATUS_WAITING_NTP | Awaiting valid time from NTP server for strict mode operation. | +| 4 | OSNMA_STATUS_VALID | Actively authenticating Galileo signals, no spoofing. | +| 5 | OSNMA_STATUS_SPOOFED | Spoofing detected on Galileo signals. | + +### GNSS True Heading {#SBG_ECOM_LOG_GPSX_HDT} + +Provides true heading data from the primary or secondary dual antenna GNSS receiver. +The timestamp represents the actual time of the GPS true heading data. + +- **Message Name (ID):** `SBG_ECOM_LOG_GPS1_HDT (15)`, `SBG_ECOM_LOG_GPS2_HDT (18)` +- **Compatibility:** INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.0-blue) ![HPINS](https://img.shields.io/badge/HPINS-1.0-blue) +- **Payload Size:** 32 bytes + +| Field | Description | Unit | Format | Size | Offset | +|------------------|------------------------------------------------------------------------------|-------|---------|------|--------| +| TIME_STAMP | Time since the sensor was powered up | µs | uint32 | 4 | 0 | +| STATUS | GNSS True Heading status (see [GPS_HDT_STATUS_BIT](#GPS_HDT_STATUS_BIT)) | - | uint16 | 2 | 4 | +| TOW | GPS Time of Week | ms | uint32 | 4 | 6 | +| TRUE_HEADING | True heading angle (0 to 360°) | ° | float | 4 | 10 | +| TRUE_HEADING_ACC | 1σ True heading estimated accuracy (0 to 360°) | ° | float | 4 | 14 | +| PITCH | Pitch angle from the master to the rover (-90 to +90°) | ° | float | 4 | 18 | +| PITCH_ACC | 1σ pitch estimated accuracy (0 to 90°) | ° | float | 4 | 22 | +| BASELINE | Distance between main and auxiliary antenna (since v1.11) | m | float | 4 | 26 | +| NUM_SV_TRACKED | Number of space vehicles tracked for true heading - 0xFF if N/A (since v4.0) | - | uint8 | 1 | 30 | +| NUM_SV_USED | Number of SV used in true heading solution - 0xFF if N/A (since v4.0) | - | uint8 | 1 | 31 | + +#### Status and Validity {#GPS_HDT_STATUS_BIT} + +The `STATUS` field indicates the status of the GNSS true heading solution along with additional validity information. + +| Bits | Type | Name | Description | +|-------|-------|----------------------------------|------------------------------------------------------------------| +| [0-5] | Enum | SBG_ECOM_GPS_HDT_STATUS | GNSS true heading status (see [GPS_HDT_STATUS](#GPS_HDT_STATUS)) | +| 6 | Mask | SBG_ECOM_GPS_HDT_BASELINE_VALID | Set if the baseline length field is valid | + +##### GNSS True Heading Status Enumeration {#GPS_HDT_STATUS} + +| Value | Name | Description | +|-------|------------------------------|----------------------------------------------------| +| 0 | SBG_ECOM_HDT_SOL_COMPUTED | A valid solution has been computed | +| 1 | SBG_ECOM_HDT_INSUFFICIENT_OBS| Not enough valid SV to compute a solution | +| 2 | SBG_ECOM_HDT_INTERNAL_ERROR | An internal error has occurred | +| 3 | SBG_ECOM_HDT_HEIGHT_LIMIT | The height limit has been exceeded | + +### GNSS Raw Data {#SBG_ECOM_LOG_GPSX_RAW} + +This log is used to store raw GPS data for post-processing purposes. +It directly stores untouched binary messages from the GNSS receiver, relevant for post-processing. + +Each message can contain up to 4086 bytes of raw GNSS data to fit within a standard sbgECom protocol frame. + +- **Message Name (ID):** `SBG_ECOM_LOG_GPS1_RAW (31)`, `SBG_ECOM_LOG_GPS2_RAW (38)` +- **Compatibility:** INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.1-blue) ![HPINS](https://img.shields.io/badge/HPINS-1.1-blue) +- **Payload Size:** Variable (up to 4086) + +| Field | Description | Unit | Format | Size | Offset | +|-------------|----------------------------------------------------------|------|--------|------------|--------| +| RAW_BUFFER | Buffer storing GNSS raw data as returned by the receiver | - | void | [0-4086] | 0 | + +### GNSS Space Vehicles Information {#SBG_ECOM_LOG_GPSX_SAT} + +List of all space vehicles in view by the primary or secondary GNSS receiver antenna. +This message is typically sent every second with the latest information from the GNSS receiver. + +It includes details on the constellation, identifier/PRN, elevation, azimuth, and health status of each satellite, along with tracked signals. + +- **Message Name (ID):** `SBG_ECOM_LOG_GPS1_SAT (50)`, `SBG_ECOM_LOG_GPS2_SAT (51)` +- **Compatibility:** INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-3.0-blue) ![HPINS](https://img.shields.io/badge/HPINS-4.1-blue) +- **Payload Size:** Variable (up to 4086) + +| Field | Description | Unit | Format | Size | Offset | +|--------------|-----------------------------------------------------------------|-------|--------|------------|--------| +| timeStamp | Time since sensor is powered up | µs | uint32 | 4 | 0 | +| reserved | Reserved, do not use | - | uint32 | 4 | 4 | +| nrSatellites | Number of reported satellites 0 to 64 (see [SatData](#SatData)) | - | uint8 | 1 | 8 | + +#### Repeated Group: SatData (nrSatellites times) {#SatData} + +This block provides information for a satellite such as elevation, satellite id etc. +It is repeated `nrSatellites` times. + +| Field | Description | Unit | Format | Size | Offset | +|--------------|-------------------------------------------------------------------|------|--------|--------|--------| +| satelliteId | RINEX Satellite number (see [GNSS_SAT_ID](#GNSS_SAT_ID)) | - | uint8 | 1 | - | +| elevation | Satellite elevation (-90 to +90°) | ° | int8 | 1 | - | +| azimuth | Satellite azimuth (0 to 359°) | ° | uint16 | 2 | - | +| satFlags | Satellite status flags (see [satFlags](#satFlags)) | - | uint16 | 2 | - | +| nrSignals | Number of reported signals 0 to 8 (see [SignalData](#SignalData)) | - | uint8 | 1 | - | + +##### satFlags Definition {#satFlags} + +| Bits | Type | Name | Description | +|--------|-------|--------------------|--------------------------------------------------------------------------| +| [0-2] | Enum | Tracking status | Tracking Status (see [SAT_TRACKING_STATUS](#SAT_TRACKING_STATUS)). | +| [3-4] | Enum | Health status | Health Status (see [SAT_TRACKING_STATUS](#SAT_TRACKING_STATUS)). | +| [5-6] | Enum | Elevation status | Elevation Status (see [SAT_TRACKING_STATUS](#SAT_TRACKING_STATUS)). | +| [7-10] | Enum | Constellation | Satellite constellation (see [GNSS_CONSTELLATION](#GNSS_CONSTELLATION)). | + +#### Repeated Group: SignalData (nrSignals times) {#SignalData} + +This block provides information for a specific signal of a satellite. +It is repeated `nrSignals` for a satellite. + +| Field | Description | Unit | Format | Size | Offset | +|--------------|------------------------------------------------------------|------|--------|------|--------| +| signalId | Signal unique identifier (see [GNSS_SAT_ID](#GNSS_SAT_ID)) | - | uint8 | 1 | - | +| sigFlags | Signal status flags (see [sigFlags](#sigFlags)) | - | uint8 | 1 | - | +| snr | Signal-to-Noise ratio | dB | uint8 | 1 | - | + +##### sigFlags Definition {#sigFlags} + +| Bits | Type | Name | Description | +|--------|-------|------------------|-----------------------------------------------------------------------| +| [0-2] | Enum | Tracking status | Tracking Status (see [SAT_TRACKING_STATUS](#SAT_TRACKING_STATUS)). | +| [3-4] | Enum | Health status | Health Status (see [SAT_TRACKING_STATUS](#SAT_TRACKING_STATUS)). | +| 5 | Mask | SNR valid | Set if the Signal-to-Noise Ratio information is available. | + +#### Common Definitions + +The definitions below are shared across [SatData](#SatData) and [SignalData](#SignalData) blocks. + +##### Tracking Status Enum {#SAT_TRACKING_STATUS} + +| Value | Name | Description | +|-------|------------------------------------------------|-------------------------------------------------------| +| 0 | SBG_ECOM_SAT_TRACKING_STATUS_UNKNOWN | Unknown tracking status (e.g., no signal or idle) | +| 1 | SBG_ECOM_SAT_TRACKING_STATUS_SEARCHING | Signal being searched, not yet usable | +| 2 | SBG_ECOM_SAT_TRACKING_STATUS_TRACKING_UNKNOWN | Signal tracked, use in solution unknown | +| 3 | SBG_ECOM_SAT_TRACKING_STATUS_TRACKING_NOT_USED | Signal tracked, not used in the solution | +| 4 | SBG_ECOM_SAT_TRACKING_STATUS_TRACKING_REJECTED | Signal tracked but rejected (e.g., due to jamming) | +| 5 | SBG_ECOM_SAT_TRACKING_STATUS_TRACKING_USED | Signal tracked and used in the GNSS solution | + +##### Health Status Enum {#SAT_HEALTH_STATUS} + +| Value | Name | Description | +|-------|------------------------------------------------|-------------------------------------------------------| +| 0 | SBG_ECOM_SAT_HEALTH_STATUS_UNKNOWN | Satellite or signal health is unknown | +| 1 | SBG_ECOM_SAT_HEALTH_STATUS_HEALTHY | The satellite or signal is healthy | +| 2 | SBG_ECOM_SAT_HEALTH_STATUS_UNHEALTHY | The satellite or signal is unhealthy | + +##### Elevation Status Enum {#SAT_ELEVATION_STATUS} + +| Value | Name | Description | +|-------|------------------------------------------------|-------------------------------------------------------| +| 0 | SBG_ECOM_SAT_ELEVATION_STATUS_UNKNOWN | Elevation status unknown | +| 1 | SBG_ECOM_SAT_ELEVATION_STATUS_SETTING | The satellite elevation is setting | +| 2 | SBG_ECOM_SAT_ELEVATION_STATUS_RISING | The satellite elevation is rising | +## Aiding Equipments + +The following logs provide the navigation unit's output, including Euler angles, quaternions, velocity and position. +These logs share common status flags and enumerations, detailed below. + +### SBG_ECOM_LOG_MAG (04) {#SBG_ECOM_LOG_MAG} + +This log provides calibrated magnetometer data along with associated accelerometer readings. +When an internal magnetometer is used, the accelerometer data is sourced from the internal IMU. + +- **Message Name (ID):** `SBG_ECOM_LOG_MAG (04)` +- **Compatibility:** ELLIPSE AHRS/INS +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.0-blue) +- **Payload Size:** 30 bytes + +| Field | Description | Unit | Format | Size | Offset | +|-------------|-------------------------------------------------------------------|-------|--------|------|--------| +| TIME_STAMP | Time since the sensor was powered up | µs | uint32 | 4 | 0 | +| MAG_STATUS | Magnetometer status bitmask (see [MAG_STATUS](#MAG_STATUS)) | - | uint16 | 2 | 4 | +| MAG_X | Magnetic field along the X axis in the body frame. | a.u | float | 4 | 6 | +| MAG_Y | Magnetic field along the Y axis in the body frame. | a.u | float | 4 | 10 | +| MAG_Z | Magnetic field along the Z axis in the body frame. | a.u | float | 4 | 14 | +| ACCEL_X | Acceleration along the X axis in the body frame. | m/s² | float | 4 | 18 | +| ACCEL_Y | Acceleration along the Y axis in the body frame. | m/s² | float | 4 | 22 | +| ACCEL_Z | Acceleration along the Z axis in the body frame. | m/s² | float | 4 | 26 | + +> [!NOTE] +> Magnetic field measurements are expressed in Arbitrary Units (A.U.) as they are used to determine magnetic heading. +> After successful magnetic calibration, the magnetic field values are normalized to one. + +#### MAG_STATUS Definition {#MAG_STATUS} + +| Bit | Name | Description | +|-----|----------------------------------|---------------------------------------------------------| +| 0 | SBG_ECOM_MAG_MAG_X_BIT | Set if the magnetometer X-axis passes the self-test | +| 1 | SBG_ECOM_MAG_MAG_Y_BIT | Set if the magnetometer Y-axis passes the self-test | +| 2 | SBG_ECOM_MAG_MAG_Z_BIT | Set if the magnetometer Z-axis passes the self-test | +| 3 | SBG_ECOM_MAG_ACCEL_X_BIT | Set if the accelerometer X-axis passes the self-test | +| 4 | SBG_ECOM_MAG_ACCEL_Y_BIT | Set if the accelerometer Y-axis passes the self-test | +| 5 | SBG_ECOM_MAG_ACCEL_Z_BIT | Set if the accelerometer Z-axis passes the self-test | +| 6 | SBG_ECOM_MAG_MAGS_IN_RANGE | Set if the magnetometer is not saturated | +| 7 | SBG_ECOM_MAG_ACCELS_IN_RANGE | Set if the accelerometer is not saturated | +| 8 | SBG_ECOM_MAG_CALIBRATION_OK | Set if the magnetometer appears to be calibrated | + +### SBG_ECOM_LOG_MAG_CALIB (05) {#SBG_ECOM_LOG_MAG_CALIB} + +This log provides a raw buffer for magnetic calibration procedures, such as those performed using the sbgCenter software. + +- **Message Name (ID):** `SBG_ECOM_LOG_MAG_CALIB (05)` +- **Compatibility:** ELLIPSE AHRS/INS +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.0-blue) +- **Payload Size:** 22 bytes + +| Field | Description | Unit | Format | Size | Offset | +|---------------|-----------------------------------------------------------------|-------|----------|--------|--------| +| TIME_STAMP | Time since the sensor was powered up | µs | uint32 | 4 | 0 | +| RESERVED | Reserved field for future use | - | uint16 | 2 | 4 | +| BUFFER | Raw magnetic calibration data buffer | - | uint8[] | 16 | 6 | + +### SBG_ECOM_LOG_ODO_VEL (19) {#SBG_ECOM_LOG_ODO_VEL} + +This log provides the external velocity aiding expressed in meters per second, measured along the IMU/vehicle X axis (direction of travel). +This velocity can be sourced from various speed sensors, such as a quadrature/pulse odometer or integrated CAN bus vehicles odometer. + +The log is asynchronous and is sent whenever new velocity aiding information is available, such as when an odometer pulse is detected. + +- **Message Name (ID):** `SBG_ECOM_LOG_ODO_VEL (19)` +- **Compatibility:** INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.0-blue) ![HPINS](https://img.shields.io/badge/HPINS-1.0-blue) +- **Payload Size:** 10 bytes + +| Field | Description | Unit | Format | Size | Offset | +|--------------|--------------------------------------------------------------------|-------|--------|------|--------| +| TIME_STAMP | Time since the sensor was powered up | µs | uint32 | 4 | 0 | +| ODO_STATUS | Odometer velocity status (see [ODO_STATUS](#ODO_STATUS)) | - | uint16 | 2 | 4 | +| ODO_VEL | Velocity along the odometer direction | m/s | float | 4 | 6 | + +**Timestamp Considerations:** +This log is asynchronous, with the timestamp indicating the actual time of the velocity data, rather than the time when the log was generated. +Consequently, the timestamp may slightly precede other data, such as IMU readings, that are sent simultaneously. + +#### ODO_STATUS Definition {#ODO_STATUS} + +| Bit | Name | Description | +|-----|------------------------|------------------------------------------------------------------------------------| +| 0 | SBG_ECOM_ODO_REAL_MEAS | Set if the log is from a real pulse measurement, 0 for virtual ZUPT. | +| 1 | SBG_ECOM_ODO_TIME_SYNC | Set if the velocity information is correctly time-synchronized. | + +### SBG_ECOM_LOG_AIR_DATA (36) {#SBG_ECOM_LOG_AIR_DATA} + +The airdata log provides barometric altitude above a reference level and true airspeed. +The altitude is typically measured using a barometer, while the true airspeed is obtained from a pitot tube. + +By default, the altitude is referenced to a standard 1013.25 hPa zero level pressure. + +This log serves both as an input and output. +It is sent by the unit when new internal airdata information is available. +It can also be used to inject external altitude and true airspeed aiding information. + +**Timestamp Considerations:** +This log is asynchronous, with the timestamp indicating the actual time of the altitude/airspeed measurement, rather than the time when the log was generated. + +When used as an input for external altitude aiding, the `TIME_STAMP` field may either represent an absolute time or a measurement delay, depending on the status flags. +The measurement delay allows the INS to calculate an absolute timestamp based on the reception time, transmission delay, and the specified delay. + +- **Message Name (ID):** `SBG_ECOM_LOG_AIR_DATA (36)` +- **Compatibility:** ELLIPSE AHRS/INS +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-2.3-blue) +- **Payload Size:** 26 bytes + +| Field | Description | Unit | Format | Size | Offset | +|--------------------|--------------------------------------------------------------------|--------|--------|------|--------| +| TIME_STAMP / DELAY | Time since sensor was powered up or measurement delay | µs | uint32 | 4 | 0 | +| AIRDATA_STATUS | Airdata information status (see [AIRDATA_STATUS](#AIRDATA_STATUS)) | - | uint16 | 2 | 4 | +| PRESSURE_ABS | Raw absolute pressure measured by the barometer sensor | Pa | float | 4 | 6 | +| ALTITUDE | Altitude computed from barometric altimeter | m | float | 4 | 10 | +| PRESSURE_DIFF | Raw differential pressure measured by the pitot tube | Pa | float | 4 | 14 | +| TRUE_AIRSPEED | True airspeed measured by the pitot tube | m/s | float | 4 | 18 | +| AIR_TEMPERATURE | Outside air temperature used for airspeed computations | °C | float | 4 | 22 | + +#### AIRDATA_STATUS Definition {#AIRDATA_STATUS} + +| Bit | Name | Description | +|-----|---------------------------------------|------------------------------------------------------------------------------| +| 0 | SBG_ECOM_AIR_DATA_TIME_IS_DELAY | Set if `TIME_STAMP` represents a measurement delay instead of absolute time. | +| 1 | SBG_ECOM_AIR_DATA_PRESSURE_ABS_VALID | Set if the absolute pressure field is filled and valid. | +| 2 | SBG_ECOM_AIR_DATA_ALTITUDE_VALID | Set if the barometric altitude field is filled and valid. | +| 3 | SBG_ECOM_AIR_DATA_PRESSURE_DIFF_VALID | Set if the differential pressure field is filled and valid. | +| 4 | SBG_ECOM_AIR_DATA_AIRSPEED_VALID | Set if the true airspeed field is filled and valid. | +| 5 | SBG_ECOM_AIR_DATA_TEMPERATURE_VALID | Set if the outside air temperature field is filled and valid. | + +### SBG_ECOM_LOG_DVL_BOTTOM/WATER_TRACK {#SBG_ECOM_LOG_DVL_XXXX} + +This log records velocity data from a Doppler Velocity Log (DVL), either from bottom tracking or water tracking. +The timestamp indicates the actual time of the DVL velocity data, rather than being synchronized with the main loop. + +- **Message Name (ID):** `SBG_ECOM_LOG_DVL_BOTTOM_TRACK (29)`, `SBG_ECOM_LOG_DVL_WATER_TRACK (30)` +- **Compatibility:** INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) ![HPINS](https://img.shields.io/badge/HPINS-1.1-blue) +- **Payload Size:** 30 bytes + +| Field | Description | Unit | Format | Size | Offset | +|--------------------|------------------------------------------------------------------|-------|--------|------|--------| +| TIME_STAMP | Time since the sensor was powered up | µs | uint32 | 4 | 0 | +| DVL_STATUS | DVL velocity status bitmask (see [DVL_STATUS](#DVL_STATUS)) | - | uint16 | 2 | 4 | +| VELOCITY_X | Velocity in the X direction, DVL instrument frame | m/s | float | 4 | 6 | +| VELOCITY_Y | Velocity in the Y direction, DVL instrument frame | m/s | float | 4 | 10 | +| VELOCITY_Z | Velocity in the Z direction, DVL instrument frame | m/s | float | 4 | 14 | +| VELOCITY_QUALITY_X | Quality of the X velocity measurement | m/s | float | 4 | 18 | +| VELOCITY_QUALITY_Y | Quality of the Y velocity measurement | m/s | float | 4 | 22 | +| VELOCITY_QUALITY_Z | Quality of the Z velocity measurement | m/s | float | 4 | 26 | + +#### DVL_STATUS Definition {#DVL_STATUS} + +| Bit | Name | Description | +|-----|----------------------------|--------------------------------------------------------------------| +| 0 | SBG_ECOM_DVL_VELOCITY_VALID| Set if the DVL measured a valid velocity | +| 1 | SBG_ECOM_DVL_TIME_SYNC | Set if the data is accurately timestamped using Sync In/Out | + +### SBG_ECOM_LOG_DEPTH (47) {#SBG_ECOM_LOG_DEPTH} + +This log captures pressure and depth information from a subsea depth sensor. +The timestamp reflects the actual time of the depth measurement, rather than being synchronized with the main loop. + +- **Message Name (ID):** `SBG_ECOM_LOG_DEPTH (47)` +- **Compatibility:** High Performance INS +- **Firmware:** ![HPINS](https://img.shields.io/badge/HPINS-1.3-blue) +- **Payload Size:** 14 bytes + +| Field | Description | Unit | Format | Size | Offset | +|---------------|-----------------------------------------------------------------|-------|--------|------|--------| +| TIME_STAMP | Time since the sensor was powered up or measurement delay | µs | uint32 | 4 | 0 | +| DEPTH_STATUS | Depth sensor status bitmask (see [DEPTH_STATUS](#DEPTH_STATUS)) | - | uint16 | 2 | 4 | +| PRESSURE_ABS | Absolute water pressure measured | Pa | float | 4 | 6 | +| DEPTH | Measured depth, positive upward | m | float | 4 | 10 | + +#### DEPTH_STATUS Definition {#DEPTH_STATUS} + +| Bit | Name | Description | +|-----|-----------------------------------|------------------------------------------------------------------| +| 0 | SBG_ECOM_DEPTH_TIME_IS_DELAY | Set if the timestamp represents a delay instead of absolute time | +| 1 | SBG_ECOM_DEPTH_PRESSURE_ABS_VALID | Set if the pressure field is valid | +| 2 | SBG_ECOM_DEPTH_ALTITUDE_VALID | Set if the depth field is valid | + +### SBG_ECOM_LOG_USBL (37) {#SBG_ECOM_LOG_USBL} + +This log retrieves the position information as determined by a USBL (Ultra-Short Baseline) beacon. +The timestamp indicates the actual time of the positioning data, rather than being synchronized with the main loop. + +- **Message Name (ID):** `SBG_ECOM_LOG_USBL (37)` +- **Compatibility:** High Performance INS +- **Firmware:** ![HPINS](https://img.shields.io/badge/HPINS-1.3-blue) +- **Payload Size:** 38 bytes + +| Field | Description | Unit | Format | Size | Offset | +|---------------|----------------------------------------------------------------|--------|--------|------|--------| +| TIME_STAMP | Time since the sensor was powered up | µs | uint32 | 4 | 0 | +| USBL_STATUS | USBL system status bitmask (see [USBL_STATUS](#USBL_STATUS)) | - | uint16 | 2 | 4 | +| LATITUDE | Latitude in degrees, positive north | ° | double | 8 | 6 | +| LONGITUDE | Longitude in degrees, positive east | ° | double | 8 | 14 | +| DEPTH | Depth below mean sea level, positive down | m | float | 4 | 22 | +| LATITUDE_STD | 1σ standard deviation of latitude accuracy | m | float | 4 | 26 | +| LONGITUDE_STD | 1σ standard deviation of longitude accuracy | m | float | 4 | 30 | +| DEPTH_STD | 1σ standard deviation of depth accuracy | m | float | 4 | 34 | + +#### USBL_STATUS Definition {#USBL_STATUS} + +| Bit | Name | Description | +|-----|------------------------------|--------------------------------------------------------------------| +| 0 | SBG_ECOM_USBL_TIME_SYNC | Set if the USBL data is accurately time-synchronized | +| 1 | SBG_ECOM_USBL_POSITION_VALID | Set if the USBL data represents a valid 2D position | +| 2 | SBG_ECOM_USBL_DEPTH_VALID | Set if the USBL data has valid depth information | + +> [!NOTE] +> USBL aiding is not currently supported by the INS filter. +> However, the unit can accept external USBL aiding data and log it for later post-processing. +## Event Markers + +SBG Systems products support multiple input and output events, facilitating precise timestamping and synchronization of various equipment. +For example, an electrical signal can timestamp the exact moment a picture is taken. + +SBG Systems' INS can align their internal clock with GPS time, providing a reliable solution for synchronizing devices and measurements to a common clock. + +### SBG_ECOM_LOG_EVENT_X (In/Out) {#SBG_ECOM_LOG_EVENT_XXXX} + +The Event In/Out log is designed to efficiently timestamp events at up to 1 kHz, while being transmitted at a maximum rate of 200 Hz. +This log can report up to five events detected in the last 5 ms, minimizing transmission overhead. + +Depending on the product options, input/output signals such as Sync A, B, C, D, and E may be available, each associated with a corresponding message. + +#### Operating Principle + +The TIME_STAMP field marks the time of the first event received in the last 5 ms. +Subsequent events in this window are recorded with time offsets, optimizing log size. + +Additionally, SBG Systems products can generate output event markers, providing both an electrical sync out signal and a corresponding output log for precise timing. + +**Example:** +If three events occur within 5 ms: +- The first event's time is stored in `TIME_STAMP`. +- The second event's time is `TIME_STAMP + TIME_OFFSET_0`. +- The third event's time is `TIME_STAMP + TIME_OFFSET_1`. + +Unused time offset fields are set to 0, and `EVENT_STATUS` indicates which fields are valid. + +- **Message Name (ID):** `SBG_ECOM_LOG_EVENT_A (24)`, `SBG_ECOM_LOG_EVENT_B (25)`, `SBG_ECOM_LOG_EVENT_C (26)`, `SBG_ECOM_LOG_EVENT_D (27)`, `SBG_ECOM_LOG_EVENT_E (28)`, `SBG_ECOM_LOG_EVENT_OUT_A (45)`, `SBG_ECOM_LOG_EVENT_OUT_B (46)` +- **Compatibility:** All products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.0-blue) ![HPINS](https://img.shields.io/badge/HPINS-1.0-blue) ![PULSE](https://img.shields.io/badge/PULSE-1.0-blue) +- **Payload Size:** 14 bytes + +| Field | Description | Unit | Format | Size | Offset | +|---------------|----------------------------------------------------------------|-------|--------|------|--------| +| TIME_STAMP | Time since the sensor was powered up | µs | uint32 | 4 | 0 | +| EVENT_STATUS | Status bitmask (see [EVENT_STATUS](#EVENT_STATUS)) | - | uint16 | 2 | 4 | +| TIME_OFFSET_0 | Time offset for the second received event | µs | uint16 | 2 | 6 | +| TIME_OFFSET_1 | Time offset for the third received event | µs | uint16 | 2 | 8 | +| TIME_OFFSET_2 | Time offset for the fourth received event | µs | uint16 | 2 | 10 | +| TIME_OFFSET_3 | Time offset for the fifth received event | µs | uint16 | 2 | 12 | + +#### EVENT_STATUS Definition {#EVENT_STATUS} + +| Bit | Name | Description | +|-----|-------------------------------|---------------------------------------------------------------------------| +| 0 | SBG_ECOM_EVENT_OVERFLOW | Set if events are received at a rate higher than 1 kHz | +| 1 | SBG_ECOM_EVENT_OFFSET_0_VALID | Set if at least two events have been received | +| 2 | SBG_ECOM_EVENT_OFFSET_1_VALID | Set if at least three events have been received | +| 3 | SBG_ECOM_EVENT_OFFSET_2_VALID | Set if at least four events have been received | +| 4 | SBG_ECOM_EVENT_OFFSET_3_VALID | Set if five events have been received | + +> [!NOTE] +> The device can handle event markers at up to 1 kHz. Excessive events may overload the internal CPU, decreasing performance and reliability. + +> [!WARNING] +> Never leave an activated Sync In signal unconnected, as noise on the line may trigger spurious events at high rates. diff --git a/doc/binaryProtocol.md b/doc/binaryProtocol.md new file mode 100644 index 0000000..64ef266 --- /dev/null +++ b/doc/binaryProtocol.md @@ -0,0 +1,253 @@ +# sbgECom Binary Protocol + +The sbgECom protocol is designed for compact and secure communication, with its binary format and 16-bit CRC for data integrity. +This makes it highly efficient for inertial navigation communications, which require high throughput and data reliability. + +Using the sbgECom binary protocol is recommended to fully access a device's features and achieve the highest level of integration with host systems. + +> [!NOTE] +> All multi-byte fields use little-endian organization, consistent with the rest of the sbgECom protocol. + +## Standard Frame + +The standard frame is the original format used since the sbgECom protocol's introduction in 2013. +It is synchronized using Sync 1 (0xFF) and Sync 2 (0x5A) bytes, with the frame end marked by the ETX (0x33) byte. + +This frame supports payloads of up to 4086 bytes, resulting in a maximum total frame size of 4096 bytes. + +### Frame Definition + +The standard frame structure is as follows: + +| Field | SYNC 1 | SYNC 2 | MSG | CLASS | LENGTH | DATA | CRC | ETX | +|--------------|------------|------------|-------------|---------------|------------------------|--------------|-------------|--------- | +| Size (bytes) | 1 | 1 | 1 | 1 | 2 | 0 to 4086 | 2 | 1 | +| Description | Start Byte | Sync Byte | Message ID | Message Class | Length of DATA section | Payload data | 16 bit CRC | End Byte | +| Value | 0xFF | 0x5A | - | - | - | - | - | 0x33 | + +### Fields Details + +Please find below more explanations on each field used in the frame protocol: + + - **SYNC 1:** First synchronization byte, always 0xFF. + - **SYNC 2:** Second synchronization byte, always 0x5A. + - **MSG:** Message identifier within the message class. + - **CLASS:** Message class identifier with MSB bit set to 0 (see [Message Class](#msgClass)). + - **LENGTH:** Data length in bytes, from 0 to 4086. + - **DATA:** Full payload as a raw binary stream (optional if `LENGTH` is 0). + - **CRC:** 16 bits CRC computed from the `MSG` to `DATA` fields (see [CRC Definition](#crcDefinition)). + - **ETX:** End-of-frame byte, always 0x33. + +> [!NOTE] +> **Third-Party Frames:** Some third-party frames may not conform to this format. Specific frame formats are defined separately, and users must handle different protocols accordingly. + +## Extended Large Frame + +The sbgECom low-level protocol layer supports payloads of up to 4086 bytes. Since sbgECom version 3.x, introduced in January 2022, the protocol has been extended to support larger payloads, primarily for the new [SBG_ECOM_CMD_API_GET](#SBG_ECOM_CMD_API_GET) and [SBG_ECOM_CMD_API_POST](#SBG_ECOM_CMD_API_POST) commands. + +### Extended Protocol Usage + +The extended large frame protocol is designed for commands that require payloads exceeding the standard maximum size of 4086 bytes. +These extended frames are used exclusively for commands and are not applied to binary output messages, even if the data size exceeds 4086 bytes. + +sbgECom commands follow a question/answer scheme between the host and the device, where the device sends the complete response at once, without interleaving other output messages. +If a large response is required, the extended large frames are sent sequentially, ensuring that no other sbgECom output messages are transmitted in the meantime. + +For binary output messages with potentially large data, such as [SBG_ECOM_LOG_GPSX_RAW](#SBG_ECOM_LOG_GPSX_RAW), the data is divided into smaller individual chunks. +This approach prevents large data bursts and minimizes delays for other *time-critical* outputs on the serial interface. +In these cases, it is the responsibility of the application layer to reassemble the payloads as needed. + +The sbgECom library manages all these mechanisms automatically, and SBG Systems strongly recommends using the sbgECom library implementation when possible. + +### Backward Compatibility + +This protocol extension is fully backward compatible. If the new [SBG_ECOM_CMD_API_GET](#SBG_ECOM_CMD_API_GET) and [SBG_ECOM_CMD_API_POST](#SBG_ECOM_CMD_API_POST) commands are not used, there is no need to update existing implementations. + +### Large Frame Definition + +To support payloads larger than 4086 bytes, a pagination system is used, with a Transmission ID (`TX ID`) field added to improve robustness and prevent frame mixing. + +A large frame is identified when the MSB of the CLASS field is set to 1 (i.e., 0x80). The large frame structure is as follows: + +| Field | SYNC 1 | SYNC 2 | MSG | CLASS | LENGTH | TX ID | PAGE IDX | NR PAGES | DATA | CRC | ETX | +|-------------|------------|-----------|------------|---------------|-----------------|-------------|------------|-------------|--------------|------------|----------| +| Size (bytes)| 1 | 1 | 1 | 1 | 2 | 1 | 2 | 2 | 0 to 4081 | 2 | 1 | +| Description | Start Byte | Sync Byte | Message ID | Message Class | Data length + 5 | Transfer ID | Page Index | Pages Count | Payload data | 16 bit CRC | End Byte | +| Value | 0xFF | 0x5A | - | 0b1xxxxxxx | - | - | - | - | - | - | 0x33 | + +> [!WARNING] +> Large frames are identified by setting the MSB in the CLASS field to 1 (i.e., 0x80). + +### Fields Details + +Please find below more explanations on each field used in the large frame protocol: + + - **SYNC 1:** First synchronization byte, always 0xFF. + - **SYNC 2:** Second synchronization byte, always 0x5A. + - **MSG:** Message identifier within the message class. + - **CLASS:** Message class identifier with MSB bit set to 1 (see [Message Class](#msgClass)). + - **LENGTH:** Total length in bytes, including `TX ID`, `PAGE IDX`, `NR PAGES` and `DATA`. + - **TX ID**: Transmission identifier, incremented with each new transmission. + - **PAGE IDX**: Zero-based index of the current page being transmitted. + - **NR PAGES**: Total number of pages in the transmission. + - **DATA**: Partial payload as a raw binary stream, to be reassembled by the host. + - **CRC:** 16 bits CRC computed from the `MSG` to `DATA` fields (see [CRC Definition](#crcDefinition)). + - **ETX:** End-of-frame byte, always 0x33. + +> [!NOTE] +> Large frames are only used for `SBG_ECOM_CMD_API_GET` and `SBG_ECOM_CMD_API_POST` commands, not for binary output messages. + +> [!NOTE] +> The theoretical maximum payload size is 65535 pages, yielding up to 255 MB of data. However, in practice, it is rare to exceed 10 pages. + +## Messages ID/Classes {#msgClass} + +The sbgECom protocol uses a combination of a Message Class and a Message ID within that class to identify each message or command. + +The table below lists all the available message classes used in the sbgECom protocol: + +| Class | ID | Description | +|----------------------------------|------|------------------------------------------------------------------------------------------------------------| +| SBG_ECOM_CLASS_LOG_ECOM_0 | 0x00 | Primary sbgECom binary output messages used for reading data and status. | +| SBG_ECOM_CLASS_LOG_ECOM_1 | 0x01 | Reserved, not used currently. | +| SBG_ECOM_CLASS_LOG_NMEA_0 | 0x02 | Standard NMEA output messages generated from the AHRS/INS solution (e.g. *GGA*, *HDT*, *VTG*). | +| SBG_ECOM_CLASS_LOG_NMEA_1 | 0x03 | Proprietary output messages using NMEA-style formatting, typically starting with `$P` (e.g. *PRDID* log). | +| SBG_ECOM_CLASS_LOG_THIRD_PARTY_0 | 0x04 | Third party output messages using binary or ASCII custom formats (e.g *TSS1* log). | +| SBG_ECOM_CLASS_LOG_NMEA_GNSS | 0x05 | Standard NMEA messages generated directly from GNSS data, rather than the INS (*GGA*, *RMC*, *ZDA*). | +| SBG_ECOM_CLASS_CMD_0 | 0x10 | Main sbgECom binary commands for configuration and queries. | + +## CRC Definition {#crcDefinition} + +The sbgECom protocol uses a 16-bit Cyclic Redundancy Check (CRC) to detect corrupted messages. + +The CRC is calculated over the `MSG` field up to the end of the `DATA` field. +The polynomial used is *0x8408*, with an initial value of *0*. + +To optimize the CRC computation, the sbgECom library includes an efficient C implementation located in the file: `sbgECom/common/crc/sbgCrc.c`. +This implementation leverages a lookup table for faster processing. + +For reference, below is a basic (non-optimized) C implementation of the 16-bit CRC calculation: + +```C +/*! + * Compute a CRC for a specified buffer. + * + * \param[in] pBuffer Read only buffer to compute the CRC on. + * \param[in] bufferSize Buffer size in bytes. + * \return The computed 16 bit CRC. + */ +uint16_t calcCRC(const void *pBuffer, size_t bufferSize) +{ + const uint8_t *pBytesArray = (const uint8_t*)pBuffer; + uint16_t poly = 0x8408; + uint16_t crc = 0; + uint8_t carry; + + assert(pBuffer); + + for (size_t j = 0; j < bufferSize; j++) + { + crc = crc ^ pBytesArray[j]; + + for (size_t i = 0; i < 8; i++) + { + carry = crc & 1; + crc = crc / 2; + + if (carry) + { + crc = crc^poly; + } + } + } + + return crc; +} +``` + +## sbgECom Protocol Version {#SBG_ECOM_VERSION_DEF} + +Determining the sbgECom protocol version supported by your device's firmware is crucial for compatibility and to access latest features. +This section explains how to identify the sbgECom version your device supports and details the versioning policy. + +### Versioning Policy + +The sbgECom protocol version is denoted by a `Major.Minor` format, derived from the full sbgECom C library version (`Major.Minor.Build-Qualifier`). + + - **Major Version:** Incremented for changes that may require protocol or C API implementation updates indicating potential breaking changes. + - **Minor Version:** Incremented for backward-compatible changes, ensuring that no action is required from users. + +> [!WARNING] +> While SBG Systems strives to maintain forward and backward compatibility across versions, it is the customer's responsibility to re-qualify their integration following any firmware or sbgECom updates. + +### Protocol Version via sbgInsRestApi + +For ELLIPSE v3 (firmware v3 and above), High Performance INS (HPINS), and PULSE IMUs, the [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi) provides an easy way to retrieve the sbgECom protocol version supported by your device. + +To retrieve the sbgECom protocol version, execute a GET request on the `api/v1/info` endpoint. You can do this in several ways: + - **Ethernet Capable Products:** Use a web browser or a tool like `curl` on the url `http://DEVICE_IP/api/v1/info`. + - **Serial Only Products:** Use the sbgEComApi CLI tool: `sbgEComApi -s COM1 -r 115200 -g api/v1/info`. + - **Programmatically:** Implement the sbgECom `SBG_ECOM_CMD_API_GET` command or perform HTTP GET requests to interact with the REST API. + +The response will include: + - **eComVersion:** Indicates the sbgECom protocol and library version (Major.Minor). + - **apiVersion:** Indicates the version of the [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi) implemented on the device. + +Example JSON response: + +```json +{ + "productCode": "EKINOX-UG-00", + "serialNumber": "000000001", + "hwRevision": "1.0.0.0", + "mnfVersion": "5.0.2275-stable", + "mnfDate": "2023-10-11", + "macAddress": "98:5C:93:00:00:00", + "productType": "ekinoxUGV1", + "fmwVersion": "5.3.2436-dev", + "btVersion": "3.6.9-stable", + "eComVersion": "4.0", + "apiVersion": "1.3" +} +``` + +For more details, refer to the [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi) documentation. + +### Protocol Version for Legacy Products + +For older products or firmware versions, refer to the tables below to determine the corresponding sbgECom version for a specific product and firmware version. + +#### ELLIPSE Series + +The following table provides information on the sbgECom protocol version supported by each ELLIPSE firmware release before version `3.0`: + +| Date | Firmware Version | sbgECom Version | +|------------|------------------|-----------------| +| 2024-03-26 | 2.6 | 3.2 | +| 2022-12-09 | 2.5 | 3.2 | +| 2022-06-09 | 2.4 | 3.2 | +| 2022-01-05 | 2.3 | 3.1 | +| 2021-08-19 | 2.2 | 2.1 | +| 2020-12-01 | 2.1 | 2.0 | +| 2020-07-20 | 2.0 | 2.0 | +| 2019-09-16 | 1.7 | 1.11 | +| 2018-11-08 | 1.6 | 1.10 | +| 2018-07-18 | 1.5 | 1.8 | + +#### High Performance INS (HPINS) + +The following table details the sbgECom protocol version supported by each High Performance INS product firmware release before version `5.0`: + +| Date | Firmware Version | sbgECom Version | +|------------|------------------|-----------------| +| 2023-04-04 | 4.2 | 3.2 | +| 2022-07-08 | 4.0 | 3.1 | +| 2021-10-06 | 3.1 | 2.0 | +| 2020-07-17 | 3.0 | 2.0 | +| 2019-10-11 | 2.1 | 1.11 | +| 2019-12-17 | 2.0 | 1.10 | +| 2017-12-06 | 1.5 | 1.7 | + +> [!NOTE] +> SBG Systems offers a free firmware update policy for the entire product lifetime. +> Please update your device to the latest firmware whenever possible to ensure compatibility and access to the latest features. diff --git a/doc/canProtocol.md b/doc/canProtocol.md new file mode 100644 index 0000000..4a789fd --- /dev/null +++ b/doc/canProtocol.md @@ -0,0 +1,964 @@ +# CAN Protocol Specifications + +The CAN protocol in SBG Systems devices offers extensive support for nearly all the information provided by the `sbgECom` serial protocol. +Although the CAN and serial protocols are not identical, they share many definitions and data structures to ensure consistency. + +The CAN interface is primarily designed for outputting messages and can support external aiding equipment such as CAN-based odometers. +However, the CAN interface cannot be used to configure the device. For configuration purposes, standard serial or Ethernet interfaces must be utilized. + +## CAN Conventions & Definitions + +### CAN 2.0 A/B Interface + +The protocol described in this documentation is used to communicate with the AHRS/INS over a Controller Area Network (CAN) bus. +The CAN bus is a message-based protocol originally designed for automotive applications but is now widely used across various industries. + +SBG Systems' CAN implementation supports both CAN 2.0A and CAN 2.0B standards, ensuring maximum compatibility with third-party equipment and CAN software. +The CAN bus implementation adheres to the CiA (CAN in Automation) DS-102 standard for quantum and timing definitions. + +The following standard CAN bus bitrates are supported: +- 1 000 kBit/s +- 800 kBit/s +- 500 kBit/s +- 250 kBit/s +- 200 kBit/s +- 125 kBit/s +- 100 kBit/s +- 50 kBit/s +- 25 kBit/s +- 20 kBit/s +- 10 kBit/s + +A maximum of 8 bytes per frame are transmitted, both standard (11 bits) and extended (29 bits) identifiers are supported. + +> [!WARNING] +> SBG Systems' products do not include a 120 Ohm termination resistor; it is the user's responsibility to implement this component. + +### Status and Enum Definitions + +All status indicators, enumerations, and bitmasks used in the CAN messages are consistent with those in the [sbgECom Binary Messages](#binaryMessages) definitions. +To ensure simplicity and maintainability, please refer to the [sbgECom Binary Messages](#binaryMessages) for detailed definitions of status flags, bitmasks, and enumerations. + +### Value Scaling and Units + +To maximize compatibility with third-party CAN software, each parameter in the CAN messages can be adjusted using a simple offset and scale factor. +For example, if a parameter has a scale factor of $10^{-2}$, the value returned by the device should be multiplied by $10^{-2}$ (or divided by 100) to convert it to the correct unit specified in the message. + +### Message Identification + +Each CAN message is assigned a unique identifier, encoded using 11 bits for standard CAN 2.0A messages or 29 bits for extended CAN 2.0B messages. +- **Standard CAN messages**: Message IDs range from `0x000` to `0x7FF`, allowing for 2,048 unique identifiers. +- **Extended CAN messages**: IDs can go up to `0x1FFFFFF`, offering over 536 million unique identifiers. + +By default, all CAN message IDs are disabled to prevent conflicts with other devices on the same network. +When configuring CAN message outputs, it is crucial to carefully select IDs to avoid collisions with other devices' IDs. + +## CAN Messages Overview + +The following section provides an overview of all available CAN messages with a brief description and default CAN identifier. + +Because CAN payloads are limited to 8 bytes only, consistent data might be spread across several CAN messages. +For example, the two CAN messages `SBG_ECAN_MSG_UTC_0` and `SBG_ECAN_MSG_UTC_1` form a consistent dataset that should be interpreted together. + +| Name (Log ID) | Description | +|---------------------------------------------------------------------------------|-----------------------------------------------------------------------------| +| [SBG_ECAN_MSG_STATUS_01 (0x100)](#SBG_ECAN_MSG_STATUS_01) | General device status and information - part 1/3. | +| [SBG_ECAN_MSG_STATUS_02 (0x101)](#SBG_ECAN_MSG_STATUS_02) | General device status and information - part 2/3. | +| [SBG_ECAN_MSG_STATUS_03 (0x102)](#SBG_ECAN_MSG_STATUS_03) | General device status and information - part 3/3. | +| [SBG_ECAN_MSG_UTC_0 (0x110)](#SBG_ECAN_MSG_UTC_0) | INS clock and UTC time reference - part 1/2. | +| [SBG_ECAN_MSG_UTC_1 (0x111)](#SBG_ECAN_MSG_UTC_1) | INS clock and UTC time reference - part 2/2. | +| [SBG_ECAN_MSG_IMU_INFO (0x120)](#SBG_ECAN_MSG_IMU_INFO) | IMU timestamp, status and temperature. | +| [SBG_ECAN_MSG_IMU_ACCEL (0x121)](#SBG_ECAN_MSG_IMU_ACCEL) | IMU body X,Y,Z accelerometers. | +| [SBG_ECAN_MSG_IMU_GYRO (0x122)](#SBG_ECAN_MSG_IMU_GYRO) | IMU body X,Y,Z rotation rates. | +| [SBG_ECAN_MSG_IMU_DELTA_VEL (0x123)](#SBG_ECAN_MSG_IMU_ACCEL) | Deprecated - same as `SBG_ECAN_MSG_IMU_ACCEL`. | +| [SBG_ECAN_MSG_IMU_DELTA_ANGLE (0x124)](#SBG_ECAN_MSG_IMU_GYRO) | Deprecated - same as `SBG_ECAN_MSG_IMU_GYRO`. | +| [SBG_ECAN_MSG_EKF_INFO (0x130)](#SBG_ECAN_MSG_EKF_INFO) | AHRS/INS solution timestamp. | +| [SBG_ECAN_MSG_EKF_QUAT (0x131)](#SBG_ECAN_MSG_EKF_QUAT) | AHRS/INS attitude using quaternion representation. | +| [SBG_ECAN_MSG_EKF_EULER (0x132)](#SBG_ECAN_MSG_EKF_EULER) | AHRS/INS attitude using Euler angles representation. | +| [SBG_ECAN_MSG_EKF_ORIENTATION_ACC (0x133)](#SBG_ECAN_MSG_EKF_ORIENTATION_ACC) | AHRS/INS attitude standard deviation accuracy. | +| [SBG_ECAN_MSG_EKF_POS (0x134)](#SBG_ECAN_MSG_EKF_POS) | INS latitude/longitude position. | +| [SBG_ECAN_MSG_EKF_ALTITUDE (0x135)](#SBG_ECAN_MSG_EKF_ALTITUDE) | INS altitude above MSL and undulation. | +| [SBG_ECAN_MSG_EKF_POS_ACC (0x136)](#SBG_ECAN_MSG_EKF_POS_ACC) | INS position standard deviation accuracy. | +| [SBG_ECAN_MSG_EKF_VEL_NED (0x137)](#SBG_ECAN_MSG_EKF_VEL_NED) | INS velocity in North, East, Down navigation frame. | +| [SBG_ECAN_MSG_EKF_VEL_NED_ACC (0x138)](#SBG_ECAN_MSG_EKF_VEL_NED_ACC) | INS velocity standard deviation accuracy in the NED frame. | +| [SBG_ECAN_MSG_EKF_VEL_BODY (0x139)](#SBG_ECAN_MSG_EKF_VEL_BODY) | INS velocity X, Y, Z body frame. | +| [SBG_ECAN_MSG_AUTO_TRACK_SLIP_CURV (0x220)](#SBG_ECAN_MSG_AUTO_TRACK_SLIP_CURV) | Automotive-specific track angle, slip angle, and curvature radius. | +| [SBG_ECAN_MSG_SHIP_MOTION_INFO (0x140)](#SBG_ECAN_MSG_SHIP_MOTION_INFO) | Marine-specific heave, surge, sway timestamp, and status. | +| [SBG_ECAN_MSG_SHIP_MOTION_0 (0x141)](#SBG_ECAN_MSG_SHIP_MOTION_0) | Surge, sway, and heave measurements. | +| [SBG_ECAN_MSG_SHIP_MOTION_1 (0x145)](#SBG_ECAN_MSG_SHIP_MOTION_1) | Surge, sway, and heave acceleration measurements. | +| [SBG_ECAN_MSG_SHIP_MOTION_2 (0x149)](#SBG_ECAN_MSG_SHIP_MOTION_2) | Surge, sway, and heave velocity measurements. | +| [SBG_ECAN_MSG_SHIP_MOTION_HP_INFO (0x14A)](#SBG_ECAN_MSG_SHIP_MOTION_HP_INFO) | Delayed heave timestamp and status. | +| [SBG_ECAN_MSG_SHIP_MOTION_HP_0 (0x14B)](#SBG_ECAN_MSG_SHIP_MOTION_HP_0) | Delayed heave measurement (surge and sway are not computed). | +| [SBG_ECAN_MSG_SHIP_MOTION_HP_1 (0x14C)](#SBG_ECAN_MSG_SHIP_MOTION_HP_1) | Vessel's acceleration used to compute delayed heave. | +| [SBG_ECAN_MSG_SHIP_MOTION_HP_2 (0x14D)](#SBG_ECAN_MSG_SHIP_MOTION_HP_2) | Delayed heave velocity (surge and sway are not computed). | +| [SBG_ECAN_MSG_MAG_0 (0x150)](#SBG_ECAN_MSG_MAG_0) | Magnetometer aiding timestamp and status. | +| [SBG_ECAN_MSG_MAG_1 (0x151)](#SBG_ECAN_MSG_MAG_1) | Normalized X, Y, Z magnetic field values. | +| [SBG_ECAN_MSG_MAG_2 (0x152)](#SBG_ECAN_MSG_MAG_2) | Accelerometers X, Y, Z aligned with magnetometer frame. | +| [SBG_ECAN_MSG_ODO_INFO (0x160)](#SBG_ECAN_MSG_ODO_INFO) | Wheel speed odometer velocity timestamp and status. | +| [SBG_ECAN_MSG_ODO_VEL (0x161)](#SBG_ECAN_MSG_ODO_VEL) | Wheel speed odometer velocity measurement. | +| [SBG_ECAN_MSG_AIR_DATA_INFO (0x162)](#SBG_ECAN_MSG_AIR_DATA_INFO) | AirData aiding timestamp, status and air temperature. | +| [SBG_ECAN_MSG_AIR_DATA_ALTITUDE (0x163)](#SBG_ECAN_MSG_AIR_DATA_ALTITUDE) | AirData static pressure and barometric altitude. | +| [SBG_ECAN_MSG_AIR_DATA_AIRSPEED (0x164)](#SBG_ECAN_MSG_AIR_DATA_AIRSPEED) | AirData dynamic pressure and true airspeed. | +| [SBG_ECAN_MSG_DEPTH_INFO (0x166)](#SBG_ECAN_MSG_DEPTH_INFO) | Subsea depth pressure sensor timestamp and status. | +| [SBG_ECAN_MSG_DEPTH_ALTITUDE (0x167)](#SBG_ECAN_MSG_DEPTH_ALTITUDE) | Subsea water pressure value and depth value. | +| [SBG_ECAN_MSG_GPS1_VEL_INFO (0x170)](#SBG_ECAN_MSG_GPSX_VEL_INFO) | GNSS module 1 velocity timestamp and status. | +| [SBG_ECAN_MSG_GPS1_VEL (0x171)](#SBG_ECAN_MSG_GPSX_VEL) | GNSS module 1 velocity North, East, Down values. | +| [SBG_ECAN_MSG_GPS1_VEL_ACC (0x172)](#SBG_ECAN_MSG_GPSX_VEL_ACC) | GNSS module 1 velocity NED standard deviation accuracies. | +| [SBG_ECAN_MSG_GPS1_COURSE (0x173)](#SBG_ECAN_MSG_GPSX_COURSE) | GNSS module 1 course over ground and accuracy. | +| [SBG_ECAN_MSG_GPS1_POS_INFO (0x174)](#SBG_ECAN_MSG_GPSX_POS_INFO) | GNSS module 1 position timestamp and status. | +| [SBG_ECAN_MSG_GPS1_POS (0x175)](#SBG_ECAN_MSG_GPSX_POS) | GNSS module 1 position latitude and longitude. | +| [SBG_ECAN_MSG_GPS1_POS_ALT (0x176)](#SBG_ECAN_MSG_GPSX_POS_ALT) | GNSS module 1 altitude MSL, undulation and information. | +| [SBG_ECAN_MSG_GPS1_POS_ACC (0x177)](#SBG_ECAN_MSG_GPSX_POS_ACC) | GNSS module 1 position accuracy and information. | +| [SBG_ECAN_MSG_GPS1_HDT_INFO (0x178)](#SBG_ECAN_MSG_GPSX_HDT_INFO) | GNSS module 1 dual antenna timestamp and status. | +| [SBG_ECAN_MSG_GPS1_HDT (0x179)](#SBG_ECAN_MSG_GPSX_HDT) | GNSS module 1 dual antenna true heading, pitch angle, and accuracies. | +| [SBG_ECAN_MSG_GPS2_VEL_INFO (0x180)](#SBG_ECAN_MSG_GPSX_VEL_INFO) | GNSS module 2 velocity timestamp and status. | +| [SBG_ECAN_MSG_GPS2_VEL (0x181)](#SBG_ECAN_MSG_GPSX_VEL) | GNSS module 2 velocity North, East, Down values. | +| [SBG_ECAN_MSG_GPS2_VEL_ACC (0x182)](#SBG_ECAN_MSG_GPSX_VEL_ACC) | GNSS module 2 velocity NED standard deviation accuracies. | +| [SBG_ECAN_MSG_GPS2_COURSE (0x183)](#SBG_ECAN_MSG_GPSX_COURSE) | GNSS module 2 course over ground and accuracy. | +| [SBG_ECAN_MSG_GPS2_POS_INFO (0x184)](#SBG_ECAN_MSG_GPSX_POS_INFO) | GNSS module 2 position timestamp and status. | +| [SBG_ECAN_MSG_GPS2_POS (0x185)](#SBG_ECAN_MSG_GPSX_POS) | GNSS module 2 position latitude and longitude. | +| [SBG_ECAN_MSG_GPS2_POS_ALT (0x186)](#SBG_ECAN_MSG_GPSX_POS_ALT) | GNSS module 2 altitude MSL, undulation and information. | +| [SBG_ECAN_MSG_GPS2_POS_ACC (0x187)](#SBG_ECAN_MSG_GPSX_POS_ACC) | GNSS module 2 position accuracy and information. | +| [SBG_ECAN_MSG_GPS2_HDT_INFO (0x188)](#SBG_ECAN_MSG_GPSX_HDT_INFO) | GNSS module 2 dual antenna timestamp and status. | +| [SBG_ECAN_MSG_GPS2_HDT (0x189)](#SBG_ECAN_MSG_GPSX_HDT) | GNSS module 2 dual antenna true heading, pitch angle, and accuracies. | +| [SBG_ECAN_MSG_EVENT_INFO_A (0x200)](#SBG_ECAN_MSG_EVENT_INFO_X) | Sync In A event marker timestamp and status. | +| [SBG_ECAN_MSG_EVENT_TIME_A (0x201)](#SBG_ECAN_MSG_EVENT_TIME_X) | Additional Sync In A event marker offset from primary timestamp. | +| [SBG_ECAN_MSG_EVENT_INFO_B (0x202)](#SBG_ECAN_MSG_EVENT_INFO_X) | Sync In B event marker timestamp and status. | +| [SBG_ECAN_MSG_EVENT_TIME_B (0x203)](#SBG_ECAN_MSG_EVENT_TIME_X) | Additional Sync In B event marker offset from primary timestamp. | +| [SBG_ECAN_MSG_EVENT_INFO_C (0x204)](#SBG_ECAN_MSG_EVENT_INFO_X) | Sync In C event marker timestamp and status. | +| [SBG_ECAN_MSG_EVENT_TIME_C (0x205)](#SBG_ECAN_MSG_EVENT_TIME_X) | Additional Sync In C event marker offset from primary timestamp. | +| [SBG_ECAN_MSG_EVENT_INFO_D (0x206)](#SBG_ECAN_MSG_EVENT_INFO_X) | Sync In D event marker timestamp and status. | +| [SBG_ECAN_MSG_EVENT_TIME_D (0x207)](#SBG_ECAN_MSG_EVENT_TIME_X) | Additional Sync In D event marker offset from primary timestamp. | +| [SBG_ECAN_MSG_EVENT_INFO_E (0x208)](#SBG_ECAN_MSG_EVENT_INFO_X) | Sync In E event marker timestamp and status. | +| [SBG_ECAN_MSG_EVENT_TIME_E (0x209)](#SBG_ECAN_MSG_EVENT_TIME_X) | Additional Sync In E event marker offset from primary timestamp. | +## General Status Messages + +These outputs aggregate system status data into six categories: General, Clock, Communications, Aiding, Solution, and Heave. +To obtain a comprehensive status overview, the CAN messages `SBG_ECAN_MSG_STATUS_01`, `SBG_ECAN_MSG_STATUS_02`, and `SBG_ECAN_MSG_STATUS_03` should be used in combination. + +### SBG_ECAN_MSG_STATUS_01 (0x100) {#SBG_ECAN_MSG_STATUS_01} + +The `SBG_ECAN_MSG_STATUS_01` message provides general status information about the device. + +- **Message Name (ID):** `SBG_ECAN_MSG_STATUS_01 (0x100)` +- **Compatibility:** All products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 8 bytes + +| Field | Description | Unit | Format | Size | Offset | +|------------------|--------------------------------------------------------------------------------|------|--------|------|--------| +| TIME_STAMP | Time since the sensor was powered on | µs | uint32 | 4 | 0 | +| GENERAL_STATUS | General status (see [STATUS_GENERAL_STATUS](#STATUS_GENERAL_STATUS)) | - | uint16 | 2 | 4 | +| CLOCK_STATUS | UTC time and clock sync status (see [TIME_STATUS](#TIME_STATUS)) | - | uint16 | 2 | 6 | + +### SBG_ECAN_MSG_STATUS_02 (0x101) {#SBG_ECAN_MSG_STATUS_02} + +The `SBG_ECAN_MSG_STATUS_02` message provides communication and aiding equipment status. + +- **Message Name (ID):** `SBG_ECAN_MSG_STATUS_02 (0x101)` +- **Compatibility:** All products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 8 bytes + +| Field | Description | Unit | Format | Size | Offset | +|------------------|--------------------------------------------------------------------------------|------|--------|------|--------| +| COM_STATUS | Communication status (see [STATUS_COM_STATUS](#STATUS_COM_STATUS)) | - | uint32 | 4 | 0 | +| AIDING_STATUS | Status of aiding equipment (see [STATUS_AIDING_STATUS](#STATUS_AIDING_STATUS)) | - | uint32 | 4 | 4 | + +### SBG_ECAN_MSG_STATUS_03 (0x102) {#SBG_ECAN_MSG_STATUS_03} + +The `SBG_ECAN_MSG_STATUS_03` message provides the status of the INS solution and ship motion outputs. + +- **Message Name (ID):** `SBG_ECAN_MSG_STATUS_03 (0x102)` +- **Compatibility:** All products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 6 bytes + +| Field | Description | Unit | Format | Size | Offset | +|------------------|--------------------------------------------------------------------------------|------|--------|------|--------| +| SOLUTION_STATUS | INS solution status (see [SOLUTION_STATUS](#SOLUTION_STATUS)) | - | uint32 | 4 | 0 | +| HEAVE_STATUS | Ship motion output status (see [SHIP_MOTION_STATUS](#SHIP_MOTION_STATUS)) | - | uint16 | 2 | 4 | + +> [!NOTE] +> All enumerations and bitmasks are consistent with those used in standard sbgECom output logs. + +## UTC Time Output Messages + +These messages provide a reference to UTC time, offering a time correlation between the device's `TIME_STAMP` value and the actual UTC time. +These frames are essential if you need to timestamp all data with an absolute UTC or GPS time reference. + +### SBG_ECAN_MSG_UTC_0 (0x110) {#SBG_ECAN_MSG_UTC_0} + +The `SBG_ECAN_MSG_UTC_0` message provides the time since the sensor was powered up and the GPS Time of Week (TOW). + +- **Message Name (ID):** `SBG_ECAN_MSG_UTC_0 (0x110)` +- **Compatibility:** All products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 8 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|--------------|---------------------------------------|---------|------|--------|------|--------| +| TIME_STAMP | Time since the sensor was powered on | 1 | µs | uint32 | 4 | 0 | +| GPS_TOW | GPS Time of Week | 1 | ms | uint32 | 4 | 4 | + +### SBG_ECAN_MSG_UTC_1 (0x111) {#SBG_ECAN_MSG_UTC_1} + +The `SBG_ECAN_MSG_UTC_1` message provides the detailed UTC date and time information. + +- **Message Name (ID):** `SBG_ECAN_MSG_UTC_1 (0x111)` +- **Compatibility:** All products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 8 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|-------------|----------------------------------------------------------------|---------|------|--------|------|--------| +| YEAR | Year within the century (e.g., '10' for 2010) | 1 | - | uint8 | 1 | 0 | +| MONTH | Month in Year [1 ... 12] | 1 | - | uint8 | 1 | 1 | +| DAY | Day in Month [1 ... 31] | 1 | - | uint8 | 1 | 2 | +| HOUR | Hour in Day [0 ... 23] | 1 | - | uint8 | 1 | 3 | +| MIN | Minute in Hour [0 ... 59] | 1 | - | uint8 | 1 | 4 | +| SEC | Second in Minute [0 ... 60] (Note: 60 indicates a leap second) | 1 | s | uint8 | 1 | 5 | +| MICRO_SEC | Microseconds within the current second | 100 | µs | uint16 | 2 | 6 | +## Inertial Data Output Messages + +These CAN logs output the IMU calibrated accelerometer and gyroscope values expressed in the X,Y,Z body frame. + +> [!NOTE] +> It is generally not recommended to use the CAN bus for high-frequency data output, particularly for IMU data. +> When multiple nodes are present on the bus, there is an increased risk of message delays, duplication, or loss. + +### SBG_ECAN_MSG_IMU_INFO (0x120) {#SBG_ECAN_MSG_IMU_INFO} + +The `SBG_ECAN_MSG_IMU_INFO` message provides basic IMU information including the timestamp, IMU status, and temperature. + +- **Message Name (ID):** `SBG_ECAN_MSG_IMU_INFO (0x120)` +- **Compatibility:** All products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 8 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|--------------|---------------------------------------|---------|-------|--------|------|--------| +| TIME_STAMP | Time since the sensor was powered on | 1 | µs | uint32 | 4 | 0 | +| IMU_STATUS | IMU status bitmask | - | - | uint16 | 2 | 4 | +| TEMPERATURE | IMU temperature | 10^-2 | °C | int16 | 2 | 6 | + +> [!NOTE] +> Please refer to the sbgECom serial log IMU_STATUS definition for more details on the status bitmask. + +### SBG_ECAN_MSG_IMU_ACCEL (0x121) {#SBG_ECAN_MSG_IMU_ACCEL} + +The `SBG_ECAN_MSG_IMU_ACCEL` message provides the calibrated accelerometer readings for the X, Y, and Z axes. + +- **Message Name (ID):** `SBG_ECAN_MSG_IMU_ACCEL (0x121)` +- **Compatibility:** All products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 6 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|---------|-------------------|---------|---------|--------|------|--------| +| ACCEL_X | Acceleration X | 10^-2 | m/s² | int16 | 2 | 0 | +| ACCEL_Y | Acceleration Y | 10^-2 | m/s² | int16 | 2 | 2 | +| ACCEL_Z | Acceleration Z | 10^-2 | m/s² | int16 | 2 | 4 | + +### SBG_ECAN_MSG_IMU_GYRO (0x122) {#SBG_ECAN_MSG_IMU_GYRO} + +The `SBG_ECAN_MSG_IMU_GYRO` message provides the calibrated gyroscope readings for the X, Y, and Z axes. + +- **Message Name (ID):** `SBG_ECAN_MSG_IMU_GYRO (0x122)` +- **Compatibility:** All products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 6 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|---------|------------------|---------|-----------|--------|------|--------| +| GYRO_X | Rate of turn X | 10^-3 | rad/s | int16 | 2 | 0 | +| GYRO_Y | Rate of turn Y | 10^-3 | rad/s | int16 | 2 | 2 | +| GYRO_Z | Rate of turn Z | 10^-3 | rad/s | int16 | 2 | 4 | +## EKF Output Messages + +These CAN logs output all parameters computed by the device's Extended Kalman Filter (EKF), such as the 3D attitude, velocity, and position. + +### SBG_ECAN_MSG_EKF_INFO (0x130) {#SBG_ECAN_MSG_EKF_INFO} + +The `SBG_ECAN_MSG_EKF_INFO` message provides basic EKF information, including the timestamp of the EKF solution. + +- **Message Name (ID):** `SBG_ECAN_MSG_EKF_INFO (0x130)` +- **Compatibility:** All products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 4 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|-------------|--------------------------------------|---------|-------|--------|------|--------| +| TIME_STAMP | Time since the sensor was powered on | 1 | µs | uint32 | 4 | 0 | + +### SBG_ECAN_MSG_EKF_QUAT (0x131) {#SBG_ECAN_MSG_EKF_QUAT} + +The `SBG_ECAN_MSG_EKF_QUAT` message provides the orientation of the device using a quaternion representation. + +- **Message Name (ID):** `SBG_ECAN_MSG_EKF_QUAT (0x131)` +- **Compatibility:** All products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 8 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|--------|--------------------------------------|---------|------|--------|------|--------| +| Q0 | Orientation quaternion, q0 component | 1/32767 | - | int16 | 2 | 0 | +| Q1 | Orientation quaternion, q1 component | 1/32767 | - | int16 | 2 | 2 | +| Q2 | Orientation quaternion, q2 component | 1/32767 | - | int16 | 2 | 4 | +| Q3 | Orientation quaternion, q3 component | 1/32767 | - | int16 | 2 | 6 | + +### SBG_ECAN_MSG_EKF_EULER (0x132) {#SBG_ECAN_MSG_EKF_EULER} + +The `SBG_ECAN_MSG_EKF_EULER` message provides the orientation of the device using Euler angles representation. + +- **Message Name (ID):** `SBG_ECAN_MSG_EKF_EULER (0x132)` +- **Compatibility:** All products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 6 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|-------|--------------------------------|---------|------|--------|------|--------| +| ROLL | Roll angle [-π to +π] | 10^-4 | rad | int16 | 2 | 0 | +| PITCH | Pitch angle [-π/2 to +π/2] | 10^-4 | rad | int16 | 2 | 2 | +| YAW | Yaw angle [-π to +π] | 10^-4 | rad | int16 | 2 | 4 | + +### SBG_ECAN_MSG_EKF_ORIENTATION_ACC (0x133) {#SBG_ECAN_MSG_EKF_ORIENTATION_ACC} + +The `SBG_ECAN_MSG_EKF_ORIENTATION_ACC` message provides the standard deviation accuracy of the orientation measurements. + +- **Message Name (ID):** `SBG_ECAN_MSG_EKF_ORIENTATION_ACC (0x133)` +- **Compatibility:** All products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 6 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|------------|-------------------------------------|---------|------|--------|------|--------| +| ROLL_ACC | 1σ Roll angle accuracy [0 to +π] | 10^-4 | rad | uint16 | 2 | 0 | +| PITCH_ACC | 1σ Pitch angle accuracy [0 to +π/2] | 10^-4 | rad | uint16 | 2 | 2 | +| YAW_ACC | 1σ Yaw angle accuracy [0 to +π] | 10^-4 | rad | uint16 | 2 | 4 | + +### SBG_ECAN_MSG_EKF_POS (0x134) {#SBG_ECAN_MSG_EKF_POS} + +The `SBG_ECAN_MSG_EKF_POS` message provides the INS position data, consisting of latitude and longitude expressed in the WGS84 datum. + +- **Message Name (ID):** `SBG_ECAN_MSG_EKF_POS (0x134)` +- **Compatibility:** All products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 8 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|-----------|--------------------------------|---------|------|--------|------|--------| +| LATITUDE | Latitude angle, positive north | 10^-7 | ° | int32 | 4 | 0 | +| LONGITUDE | Longitude angle, positive east | 10^-7 | ° | int32 | 4 | 4 | + +### SBG_ECAN_MSG_EKF_ALTITUDE (0x135) {#SBG_ECAN_MSG_EKF_ALTITUDE} + +The `SBG_ECAN_MSG_EKF_ALTITUDE` message provides the altitude above Mean Sea Level (MSL) and the undulation. + +- **Message Name (ID):** `SBG_ECAN_MSG_EKF_ALTITUDE (0x135)` +- **Compatibility:** All products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 6 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|------------|-------------------------------------------------|---------|------|--------|------|--------| +| ALTITUDE | Altitude above Mean Sea Level | 10^-3 | m | int32 | 4 | 0 | +| UNDULATION | Altitude difference between geoid and ellipsoid | 0.005 | m | int16 | 2 | 4 | + +### SBG_ECAN_MSG_EKF_POS_ACC (0x136) {#SBG_ECAN_MSG_EKF_POS_ACC} + +The `SBG_ECAN_MSG_EKF_POS_ACC` message provides the standard deviation accuracy of the position data. + +- **Message Name (ID):** `SBG_ECAN_MSG_EKF_POS_ACC (0x136)` +- **Compatibility:** All products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 6 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|---------------|------------------------------------|---------|------|--------|------|--------| +| LATITUDE_ACC | 1σ Latitude accuracy | 10^-2 | m | uint16 | 2 | 0 | +| LONGITUDE_ACC | 1σ Longitude accuracy | 10^-2 | m | uint16 | 2 | 2 | +| ALTITUDE_ACC | 1σ Vertical Position accuracy | 10^-2 | m | uint16 | 2 | 4 | + +### SBG_ECAN_MSG_EKF_VEL_NED (0x137) {#SBG_ECAN_MSG_EKF_VEL_NED} + +The `SBG_ECAN_MSG_EKF_VEL_NED` message provides INS velocity data in the North, East, and Down directions. + +- **Message Name (ID):** `SBG_ECAN_MSG_EKF_VEL_NED (0x137)` +- **Compatibility:** All products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 6 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|---------------|--------------------------------|---------|--------|--------|------|--------| +| VELOCITY_N | Velocity in North direction | 10^-2 | m/s | int16 | 2 | 0 | +| VELOCITY_E | Velocity in East direction | 10^-2 | m/s | int16 | 2 | 2 | +| VELOCITY_D | Velocity in Down direction | 10^-2 | m/s | int16 | 2 | 4 | + +### SBG_ECAN_MSG_EKF_VEL_NED_ACC (0x138) {#SBG_ECAN_MSG_EKF_VEL_NED_ACC} + +The `SBG_ECAN_MSG_EKF_VEL_NED_ACC` message provides the 1σ accuracy of velocity data in the North, East, and Down directions. + +- **Message Name (ID):** `SBG_ECAN_MSG_EKF_VEL_NED_ACC (0x138)` +- **Compatibility:** All products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 6 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|-----------------|-----------------------------------------|---------|--------|--------|------|--------| +| VELOCITY_ACC_N | 1σ Velocity in North direction accuracy | 10^-2 | m/s | uint16 | 2 | 0 | +| VELOCITY_ACC_E | 1σ Velocity in East direction accuracy | 10^-2 | m/s | uint16 | 2 | 2 | +| VELOCITY_ACC_D | 1σ Velocity in Down direction accuracy | 10^-2 | m/s | uint16 | 2 | 4 | + +### SBG_ECAN_MSG_EKF_VEL_BODY (0x139) {#SBG_ECAN_MSG_EKF_VEL_BODY} + +The `SBG_ECAN_MSG_EKF_VEL_BODY` message provides INS velocity data in the body X, Y, and Z frame. + +- **Message Name (ID):** `SBG_ECAN_MSG_EKF_VEL_BODY (0x139)` +- **Compatibility:** All products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 6 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|---------------|----------------------------------|---------|--------|--------|------|--------| +| VELOCITY_X | Velocity in body/INS X direction | 10^-2 | m/s | int16 | 2 | 0 | +| VELOCITY_Y | Velocity in body/INS Y direction | 10^-2 | m/s | int16 | 2 | 2 | +| VELOCITY_Z | Velocity in body/INS Z direction | 10^-2 | m/s | int16 | 2 | 4 | +## Automotive Outputs + +All messages in this section are specifically designed for automotive applications. + +### SBG_ECAN_MSG_AUTO_TRACK_SLIP_CURV (0x220) {#SBG_ECAN_MSG_AUTO_TRACK_SLIP_CURV} + +The `SBG_ECAN_MSG_AUTO_TRACK_SLIP_CURV` message provides critical automotive parameters such as track angle, slip angle, and curvature radius. + +- **Message Name (ID):** `SBG_ECAN_MSG_AUTO_TRACK_SLIP_CURV (0x220)` +- **Compatibility:** Automotive applications +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 7 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|------------------|----------------------------------------------------------|---------|--------|--------|------|--------| +| ANGLE_TRACK | Track course angle/direction of travel [-π to +π] | 10^-4 | rad | int16 | 2 | 0 | +| ANGLE_SLIP | Vehicle slip angle [-π to +π] | 10^-4 | rad | int16 | 2 | 2 | +| CURVATURE_RADIUS | Curvature radius in meters based on down rotation rate | 10^-2 | m | uint16 | 2 | 4 | +| AUTO_STATUS | Status bitmask as defined in [AUTO_STATUS](#AUTO_STATUS) | - | - | uint8 | 1 | 6 | + +#### AUTO_STATUS Definition {#AUTO_STATUS} + +This field indicates the validity of the automotive data. + +| Bit | Name | Description | +|-----|-------------------------------|-------------------------------------------| +| 0 | SBG_ECOM_AUTO_TRACK_VALID | Set to 1 if the track angle is valid | +| 1 | SBG_ECOM_AUTO_SLIP_VALID | Set to 1 if the slip angle is valid | +| 2 | SBG_ECOM_AUTO_CURVATURE_VALID | Set to 1 if the curvature radius is valid | +## Ship Motion Data + +Ship motion data represents the 3D displacement, velocities, and accelerations of a vessel. +The device can output heave, surge, and sway measurements, along with the main heave period. + +Some devices support both real-time and delayed heave values, which share the same message format. + +### SBG_ECAN_MSG_SHIP_MOTION_INFO (0x140) {#SBG_ECAN_MSG_SHIP_MOTION_INFO} + +The `SBG_ECAN_MSG_SHIP_MOTION_INFO` message provide information about the ship's motion, including the main heave period and the ship's motion status. + +- **Message Name (ID):** `SBG_ECAN_MSG_SHIP_MOTION_INFO (0x140)` +- **Compatibility:** Heave capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 8 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|---------------|--------------------------------------------------------------------|---------|-------|--------|------|--------| +| TIME_STAMP | Time since sensor is powered up | 1 | µs | uint32 | 4 | 0 | +| PERIOD | Main heave period | 10^-2 | s | uint16 | 2 | 4 | +| HEAVE_STATUS | Ship motion status (see [SHIP_MOTION_STATUS](#SHIP_MOTION_STATUS)) | - | - | uint16 | 2 | 6 | + +### SBG_ECAN_MSG_SHIP_MOTION_0 (0x141) {#SBG_ECAN_MSG_SHIP_MOTION_0} + +The `SBG_ECAN_MSG_SHIP_MOTION_0` message reports surge, sway, and heave motion measurements. + +- **Message Name (ID):** `SBG_ECAN_MSG_SHIP_MOTION_0 (0x141)` +- **Compatibility:** Heave capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 6 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|---------------|--------------------------------------------------------------------|---------|-------|--------|------|--------| +| SURGE | Surge motion (positive forward) | 10^-3 | m | int16 | 2 | 0 | +| SWAY | Sway motion (positive right) | 10^-3 | m | int16 | 2 | 2 | +| HEAVE | Heave motion (positive down) | 10^-3 | m | int16 | 2 | 4 | + +### SBG_ECAN_MSG_SHIP_MOTION_1 (0x145) {#SBG_ECAN_MSG_SHIP_MOTION_1} + +The `SBG_ECAN_MSG_SHIP_MOTION_1` message reports longitudinal, lateral, and vertical accelerations. + +- **Message Name (ID):** `SBG_ECAN_MSG_SHIP_MOTION_1 (0x145)` +- **Compatibility:** Heave capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 6 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|---------------|--------------------------------------------------------------------|---------|-------|--------|------|--------| +| ACCEL_X | Longitudinal acceleration (forward) | 10^-2 | m/s² | int16 | 2 | 0 | +| ACCEL_Y | Lateral acceleration (right) | 10^-2 | m/s² | int16 | 2 | 2 | +| ACCEL_Z | Vertical acceleration (down) | 10^-2 | m/s² | int16 | 2 | 4 | + +### SBG_ECAN_MSG_SHIP_MOTION_2 (0x149) {#SBG_ECAN_MSG_SHIP_MOTION_2} + +The `SBG_ECAN_MSG_SHIP_MOTION_2` message reports the velocities in the longitudinal, lateral, and vertical directions. + +- **Message Name (ID):** `SBG_ECAN_MSG_SHIP_MOTION_2 (0x149)` +- **Compatibility:** Heave capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 6 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|---------------|--------------------------------------------------------------------|---------|-------|--------|------|--------| +| VEL_X | Longitudinal velocity (positive forward) | 10^-2 | m/s | int16 | 2 | 0 | +| VEL_Y | Lateral velocity (positive right) | 10^-2 | m/s | int16 | 2 | 2 | +| VEL_Z | Vertical velocity (positive down) | 10^-2 | m/s | int16 | 2 | 4 | + +### SBG_ECAN_MSG_SHIP_MOTION_HP_INFO (0x14A) {#SBG_ECAN_MSG_SHIP_MOTION_HP_INFO} + +The SBG_ECAN_MSG_SHIP_MOTION_HP_INFO` message provide information about the delayed heave timestamp, period and status. + +The timestamp reported in the message reflects the actual time of data validity. +The data corresponds to conditions roughly 150 seconds earlier than other logs transmitted at the same time. + +- **Message Name (ID):** `SBG_ECAN_MSG_SHIP_MOTION_HP_INFO (0x14A)` +- **Compatibility:** HPINS with hydro option +- **Firmware:** ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 8 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|---------------|--------------------------------------------------------------------|---------|-------|--------|------|--------| +| TIME_STAMP | Time since sensor is powered up | 1 | µs | uint32 | 4 | 0 | +| PERIOD | Main heave period | 10^-2 | s | uint16 | 2 | 4 | +| HEAVE_STATUS | Ship motion status (see [SHIP_MOTION_STATUS](#SHIP_MOTION_STATUS)) | - | - | uint16 | 2 | 6 | + +### SBG_ECAN_MSG_SHIP_MOTION_HP_0 (0x14B) {#SBG_ECAN_MSG_SHIP_MOTION_HP_0} + +The `SBG_ECAN_MSG_SHIP_MOTION_HP_0` message reports delayed heave measurement. +Surge and sway quantities are not computed by the delayed heave algorithm. + +- **Message Name (ID):** `SBG_ECAN_MSG_SHIP_MOTION_HP_0 (0x14B)` +- **Compatibility:** HPINS with hydro option +- **Firmware:** ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 6 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|---------------|--------------------------------------------------------------------|---------|-------|--------|------|--------| +| SURGE | Not available for delayed heave. | 10^-3 | m | int16 | 2 | 0 | +| SWAY | Not available for delayed heave. | 10^-3 | m | int16 | 2 | 2 | +| HEAVE | Delayed heave motion (positive down) | 10^-3 | m | int16 | 2 | 4 | + +### SBG_ECAN_MSG_SHIP_MOTION_HP_1 (0x14C) {#SBG_ECAN_MSG_SHIP_MOTION_HP_1} + +The `SBG_ECAN_MSG_SHIP_MOTION_HP_1` message reports longitudinal, lateral, and vertical accelerations. + +- **Message Name (ID):** `SBG_ECAN_MSG_SHIP_MOTION_HP_1 (0x14C)` +- **Compatibility:** HPINS with hydro option +- **Firmware:** ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 6 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|---------------|--------------------------------------------------------------------|---------|-------|--------|------|--------| +| ACCEL_X | Longitudinal acceleration (forward) | 10^-2 | m/s² | int16 | 2 | 0 | +| ACCEL_Y | Lateral acceleration (right) | 10^-2 | m/s² | int16 | 2 | 2 | +| ACCEL_Z | Vertical acceleration (down) | 10^-2 | m/s² | int16 | 2 | 4 | + +### SBG_ECAN_MSG_SHIP_MOTION_HP_2 (0x14D) {#SBG_ECAN_MSG_SHIP_MOTION_HP_2} + +The `SBG_ECAN_MSG_SHIP_MOTION_HP_2` message reports the delayed heave velocity (vertical). +Surge and sway velocities are not computed by the delayed heave algorithm. + +- **Message Name (ID):** `SBG_ECAN_MSG_SHIP_MOTION_HP_2 (0x14D)` +- **Compatibility:** Heave capable products +- **Firmware:** ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 6 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|---------------|--------------------------------------------------------------------|---------|-------|--------|------|--------| +| VEL_X | Longitudinal velocity (positive forward) | 10^-2 | m/s | int16 | 2 | 0 | +| VEL_Y | Lateral velocity (positive right) | 10^-2 | m/s | int16 | 2 | 2 | +| VEL_Z | Vertical velocity (positive down) | 10^-2 | m/s | int16 | 2 | 4 | +## Magnetometer Output + +These CAN logs provide fully calibrated and normalized magnetometer values in arbitrary units (a.u.), along with associated accelerometer measurements. +If properly calibrated, the norm of the magnetic vector should equal 1. + +### SBG_ECAN_MSG_MAG_0 (0x150) {#SBG_ECAN_MSG_MAG_0} + +The `SBG_ECAN_MSG_MAG_0` message provides the timestamp and status of the magnetometer. + +- **Message Name (ID):** `SBG_ECAN_MSG_MAG_0 (0x150)` +- **Compatibility:** All products with magnetometers +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) +- **Payload Size:** 6 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|------------------|--------------------------------------------------------------------------|---------|-------|--------|------|--------| +| TIME_STAMP | Time since sensor is powered up | 1 | µs | uint32 | 4 | 0 | +| MAG_STATUS | Magnetometer status bit-mask (see [MAG_STATUS](#MAG_STATUS)) | - | - | uint16 | 2 | 4 | + +### SBG_ECAN_MSG_MAG_1 (0x151) {#SBG_ECAN_MSG_MAG_1} + +The `SBG_ECAN_MSG_MAG_1` message reports the magnetometer outputs on the X, Y, and Z axes. + +- **Message Name (ID):** `SBG_ECAN_MSG_MAG_1 (0x151)` +- **Compatibility:** All products with magnetometers +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) +- **Payload Size:** 6 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|------------------|--------------------------------------------------------------------------|---------|-------|--------|------|--------| +| MAG_X | Magnetometer output, X axis | 10^-3 | a.u. | int16 | 2 | 0 | +| MAG_Y | Magnetometer output, Y axis | 10^-3 | a.u. | int16 | 2 | 2 | +| MAG_Z | Magnetometer output, Z axis | 10^-3 | a.u. | int16 | 2 | 4 | + +### SBG_ECAN_MSG_MAG_2 (0x152) {#SBG_ECAN_MSG_MAG_2} + +The `SBG_ECAN_MSG_MAG_2` message provides the accelerometer measurements aligned with the magnetometer frame. + +- **Message Name (ID):** `SBG_ECAN_MSG_MAG_2 (0x152)` +- **Compatibility:** All products with magnetometers +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) +- **Payload Size:** 6 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|------------------|--------------------------------------------------------------------------|---------|-------|--------|------|--------| +| ACC_X | Accelerometer output, X axis | 10^-2 | m/s² | int16 | 2 | 0 | +| ACC_Y | Accelerometer output, Y axis | 10^-2 | m/s² | int16 | 2 | 2 | +| ACC_Z | Accelerometer output, Z axis | 10^-2 | m/s² | int16 | 2 | 4 | + +## Odometer Output + +These CAN logs provide information on odometer status, time since reset, and raw velocity using detected odometer pulses. +The `TIME_STAMP` field in these messages is not necessarily aligned with the main loop as it dates the actual odometer data. + +### SBG_ECAN_MSG_ODO_INFO (0x160) {#SBG_ECAN_MSG_ODO_INFO} + +The `SBG_ECAN_MSG_ODO_INFO` message provides odometer status and timestamp information. + +- **Message Name (ID):** `SBG_ECAN_MSG_ODO_INFO (0x160)` +- **Compatibility:** INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 6 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|------------------|--------------------------------------------------------------------------|---------|-------|--------|------|--------| +| TIME_STAMP | Time since sensor is powered up | 1 | µs | uint32 | 4 | 0 | +| ODO_STATUS | Odometer velocity status bit-mask (see [ODO_STATUS](#ODO_STATUS)) | - | - | uint16 | 2 | 4 | + +### SBG_ECAN_MSG_ODO_VEL (0x161) {#SBG_ECAN_MSG_ODO_VEL} + +The `SBG_ECAN_MSG_ODO_VEL` message reports the velocity in the odometer direction. + +- **Message Name (ID):** `SBG_ECAN_MSG_ODO_VEL (0x161)` +- **Compatibility:** INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 2 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|------------------|--------------------------------------------------------------------------|---------|-------|--------|------|--------| +| VELOCITY | Velocity in odometer direction | 10^-2 | m/s | int16 | 2 | 0 | + +## AirData Output + +These CAN logs provide information related to the AirData module, including barometric pressure, altitude, and true airspeed measurements. + +### SBG_ECAN_MSG_AIR_DATA_INFO (0x162) {#SBG_ECAN_MSG_AIR_DATA_INFO} + +The `SBG_ECAN_MSG_AIR_DATA_INFO` message provides general status information for the AirData module, including the outside air temperature. + +- **Message Name (ID):** `SBG_ECAN_MSG_AIR_DATA_INFO (0x162)` +- **Compatibility:** INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 7 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|------------------|--------------------------------------------------------------------------|---------|-------|--------|------|--------| +| TIME_STAMP | Time since sensor is powered up | 1 | µs | uint32 | 4 | 0 | +| AIR_DATA_STATUS | AirData module and sensor status (see [AIRDATA_STATUS](#AIRDATA_STATUS)) | - | - | uint8 | 1 | 4 | +| AIR_TEMPERATURE | Outside air temperature | 10^-2 | °C | int16 | 2 | 5 | + +### SBG_ECAN_MSG_AIR_DATA_ALTITUDE (0x163) {#SBG_ECAN_MSG_AIR_DATA_ALTITUDE} + +The `SBG_ECAN_MSG_AIR_DATA_ALTITUDE` message provides barometric pressure and altitude information. + +- **Message Name (ID):** `SBG_ECAN_MSG_AIR_DATA_ALTITUDE (0x163)` +- **Compatibility:** INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 8 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|------------------|--------------------------------------------------------------------------|---------|-------|--------|------|--------| +| PRESSURE_ABS | Absolute barometric pressure | 10^-3 | Pa | uint32 | 4 | 0 | +| ALTITUDE | Barometric altitude (positive upward) | 10^-3 | m | int32 | 4 | 4 | + +### SBG_ECAN_MSG_AIR_DATA_AIRSPEED (0x164) {#SBG_ECAN_MSG_AIR_DATA_AIRSPEED} + +The `SBG_ECAN_MSG_AIR_DATA_AIRSPEED` message provides information about the differential pressure measured by a pitot tube and the true airspeed. + +- **Message Name (ID):** `SBG_ECAN_MSG_AIR_DATA_AIRSPEED (0x164)` +- **Compatibility:** INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 6 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|------------------|--------------------------------------------------------------------------|---------|-------|--------|------|--------| +| PRESSURE_DIFF | Differential pressure measured by a pitot tube | 10^-3 | Pa | int32 | 4 | 0 | +| AIRSPEED | True airspeed (positive forward) | 10^-2 | m/s | int16 | 2 | 4 | + +Here is the conversion of the Depth Sensor CAN messages using the standard template: + +## Depth Sensor Output + +These CAN logs provide information related to the subsea depth sensor module, including absolute water pressure and depth measurements. + +### SBG_ECAN_MSG_DEPTH_INFO (0x166) {#SBG_ECAN_MSG_DEPTH_INFO} + +The `SBG_ECAN_MSG_DEPTH_INFO` message provides general status information for the depth sensor module. + +- **Message Name (ID):** `SBG_ECAN_MSG_DEPTH_INFO (0x166)` +- **Compatibility:** INS capable products +- **Firmware:** Not Supported - Please contact SBG Systems +- **Payload Size:** 5 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|------------------|--------------------------------------------------------------------------|---------|-------|--------|------|--------| +| TIME_STAMP | Time since sensor is powered up | 1 | µs | uint32 | 4 | 0 | +| DEPTH_STATUS | Depth module and sensor status (see [DEPTH_STATUS](#DEPTH_STATUS)) | - | - | uint8 | 1 | 4 | + +### SBG_ECAN_MSG_DEPTH_ALTITUDE (0x167) {#SBG_ECAN_MSG_DEPTH_ALTITUDE} + +The `SBG_ECAN_MSG_DEPTH_ALTITUDE` message provides information about the absolute water pressure and the altitude measured by the depth sensor. + +- **Message Name (ID):** `SBG_ECAN_MSG_DEPTH_ALTITUDE (0x167)` +- **Compatibility:** INS capable products +- **Firmware:** Not Supported - Please contact SBG Systems +- **Payload Size:** 8 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|------------------|--------------------------------------------------------------------------|---------|-------|--------|------|--------| +| PRESSURE_ABS | Absolute water pressure | 10^-1 | Pa | uint32 | 4 | 0 | +| DEPTH | Altitude from depth sensor (positive upward) | 10^-3 | m | int32 | 4 | 4 | + +## GNSS Velocity Outputs + +These logs provide the GNSS velocity, velocity accuracy, course, and status as computed by the primary or secondary GNSS receiver. +The `TIME_STAMP` in these messages indicates the exact time of the GNSS velocity data and may not align with the main loop. + +### SBG_ECAN_MSG_GPSX_VEL_INFO (0x170 - 0x180) {#SBG_ECAN_MSG_GPSX_VEL_INFO} + +The `SBG_ECAN_MSG_GPSX_VEL_INFO` message provides information about the GNSS velocity status and timestamp for either the primary (GPS1) or secondary (GPS2) GNSS receiver. + +- **Message Name (ID):** `SBG_ECAN_MSG_GPS1_VEL_INFO (0x170)`, `SBG_ECAN_MSG_GPS2_VEL_INFO (0x180)` +- **Compatibility:** INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 8 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|------------------|----------------------------------------------------------------------------------------|---------|-------|--------|------|--------| +| TIME_STAMP | Time since sensor is powered up | 1 | µs | uint32 | 4 | 0 | +| GPS_VEL_STATUS | GPS velocity fix and status bit-mask (see [GPS_VEL_STATUS_TYPE](#GPS_VEL_STATUS_TYPE)) | - | - | uint32 | 4 | 4 | + +### SBG_ECAN_MSG_GPSX_VEL (0x171 - 0x181) {#SBG_ECAN_MSG_GPSX_VEL} + +The `SBG_ECAN_MSG_GPSX_VEL` message provides the GNSS velocity in the North, East, and Down directions. + +- **Message Name (ID):** `SBG_ECAN_MSG_GPS1_VEL (0x171)`, `SBG_ECAN_MSG_GPS2_VEL (0x181)` +- **Compatibility:** INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 6 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|------------------|----------------------------------------------------------------------------------------|---------|-------|--------|------|--------| +| VEL_N | Velocity in North direction | 10^-2 | m/s | int16 | 2 | 0 | +| VEL_E | Velocity in East direction | 10^-2 | m/s | int16 | 2 | 2 | +| VEL_D | Velocity in Down direction | 10^-2 | m/s | int16 | 2 | 4 | + +### SBG_ECAN_MSG_GPSX_VEL_ACC (0x172 - 0x182) {#SBG_ECAN_MSG_GPSX_VEL_ACC} + +The `SBG_ECAN_MSG_GPSX_VEL_ACC` message provides the 1σ accuracy of the GNSS velocity in the North, East, and Down directions. + +- **Message Name (ID):** `SBG_ECAN_MSG_GPS1_VEL_ACC (0x172)`, `SBG_ECAN_MSG_GPS2_VEL_ACC (0x182)` +- **Compatibility:** INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 6 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|------------------|----------------------------------------------------------------------------------------|---------|-------|--------|------|--------| +| VEL_ACC_N | 1σ Accuracy in North direction | 10^-2 | m/s | uint16 | 2 | 0 | +| VEL_ACC_E | 1σ Accuracy in East direction | 10^-2 | m/s | uint16 | 2 | 2 | +| VEL_ACC_D | 1σ Accuracy in Down direction | 10^-2 | m/s | uint16 | 2 | 4 | + +### SBG_ECAN_MSG_GPSX_COURSE (0x173 - 0x183) {#SBG_ECAN_MSG_GPSX_COURSE} + +The `SBG_ECAN_MSG_GPSX_COURSE` message provides the true direction of motion over the ground and its 1σ accuracy. + +- **Message Name (ID):** `SBG_ECAN_MSG_GPS1_COURSE (0x173)`, `SBG_ECAN_MSG_GPS2_COURSE (0x183)` +- **Compatibility:** INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 4 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|------------------|----------------------------------------------------------------------------------------|---------|-------|--------|------|--------| +| COURSE | True direction of motion over ground (0 to 360°) | 10^-2 | ° | uint16 | 2 | 0 | +| COURSE_ACC | 1σ Course accuracy | 10^-2 | ° | uint16 | 2 | 2 | + +## GNSS Position Outputs + +These logs provide the GNSS position, position accuracy, and status as computed by the primary or secondary GNSS receiver. +The `TIME_STAMP` in these messages indicates the exact time of the GNSS positioning data and may not align with the main loop. + +### SBG_ECAN_MSG_GPSX_POS_INFO (0x174 - 0x184) {#SBG_ECAN_MSG_GPSX_POS_INFO} + +The `SBG_ECAN_MSG_GPSX_POS_INFO` message provides information about the GNSS position status and timestamp for either the primary (GPS1) or secondary (GPS2) GNSS receiver. + +- **Message Name (ID):** `SBG_ECAN_MSG_GPS1_POS_INFO (0x174)`, `SBG_ECAN_MSG_GPS2_POS_INFO (0x184)` +- **Compatibility:** All products with GNSS capability +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 8 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|------------------|----------------------------------------------------------------------------------------|---------|-------|--------|------|--------| +| TIME_STAMP | Time since sensor is powered up | 1 | µs | uint32 | 4 | 0 | +| GPS_POS_STATUS | GPS position fix and status bit-mask (see [GPS_POS_STATUS_TYPE](#GPS_POS_STATUS_TYPE)) | - | - | uint32 | 4 | 4 | + +### SBG_ECAN_MSG_GPSX_POS (0x175 - 0x185) {#SBG_ECAN_MSG_GPSX_POS} + +The `SBG_ECAN_MSG_GPSX_POS` message provides the GNSS latitude and longitude. + +- **Message Name (ID):** `SBG_ECAN_MSG_GPS1_POS (0x175)`, `SBG_ECAN_MSG_GPS2_POS (0x185)` +- **Compatibility:** All products with GNSS capability +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 8 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|------------------|----------------------------------------------------------------------------------------|---------|-------|--------|------|--------| +| LATITUDE | Latitude, positive North | 10^-7 | ° | int32 | 4 | 0 | +| LONGITUDE | Longitude, positive East | 10^-7 | ° | int32 | 4 | 4 | + +### SBG_ECAN_MSG_GPSX_POS_ALT (0x176 - 0x186) {#SBG_ECAN_MSG_GPSX_POS_ALT} + +The `SBG_ECAN_MSG_GPSX_POS_ALT` message provides the GNSS altitude, undulation, number of satellites in view, and the age of differential corrections. + +- **Message Name (ID):** `SBG_ECAN_MSG_GPS1_POS_ALT (0x176)`, `SBG_ECAN_MSG_GPS2_POS_ALT (0x186)` +- **Compatibility:** All products with GNSS capability +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 8 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|------------------|----------------------------------------------------------------------------------------|---------|-------|--------|------|--------| +| ALTITUDE | Altitude Above Mean Sea Level | 10^-3 | m | int32 | 4 | 0 | +| UNDULATION | Altitude difference between the geoid and the ellipsoid | 0.005 | m | int16 | 2 | 4 | +| NUM_SV | Number of space vehicles used in solution | - | - | uint8 | 1 | 6 | +| DIFF_CORR_AGE | Age of differential corrections (set to 0xFF if not available) | 1 | s | uint8 | 1 | 7 | + +### SBG_ECAN_MSG_GPSX_POS_ACC (0x177 - 0x187) {#SBG_ECAN_MSG_GPSX_POS_ACC} + +The `SBG_ECAN_MSG_GPSX_POS_ACC` message provides the 1σ accuracy of the GNSS latitude, longitude, and altitude. + +- **Message Name (ID):** `SBG_ECAN_MSG_GPS1_POS_ACC (0x177)`, `SBG_ECAN_MSG_GPS2_POS_ACC (0x187)` +- **Compatibility:** All products with GNSS capability +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 8 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|------------------|----------------------------------------------------------------------------------------|---------|-------|--------|------|--------| +| LAT_ACC | 1σ Latitude Accuracy | 10^-2 | m | uint16 | 2 | 0 | +| LONG_ACC | 1σ Longitude Accuracy | 10^-2 | m | uint16 | 2 | 2 | +| ALT_ACC | 1σ Altitude Accuracy | 10^-2 | m | uint16 | 2 | 4 | +| BASE_STATION_ID | ID of the base station used (set to 0xFFFF if not available) | - | - | uint16 | 2 | 6 | + +### GNSS True Heading Outputs + +These logs provide the GNSS true heading and pitch solutions along with their respective accuracy and status as computed by the dual antenna GNSS receiver. +The `TIME_STAMP` in these messages indicates the exact time of the GNSS true heading data and may not align with the main loop. + +### SBG_ECAN_MSG_GPSX_HDT_INFO (0x178 - 0x188) {#SBG_ECAN_MSG_GPSX_HDT_INFO} + +The `SBG_ECAN_MSG_GPSX_HDT_INFO` message provides information about the GNSS true heading status and timestamp for either the primary (GPS1) or secondary (GPS2) GNSS receiver. + +- **Message Name (ID):** `SBG_ECAN_MSG_GPS1_HDT_INFO (0x178)`, `SBG_ECAN_MSG_GPS2_HDT_INFO (0x188)` +- **Compatibility:** All products with dual antenna GNSS capability +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 6 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|------------------|----------------------------------------------------------------------------------------|---------|-------|--------|------|--------| +| TIME_STAMP | Time since sensor is powered up | 1 | µs | uint32 | 4 | 0 | +| GPS_HDT_STATUS | GPS True Heading status bit-mask (see [GPS_HDT_STATUS_BIT](#GPS_HDT_STATUS_BIT)) | - | - | uint16 | 2 | 4 | + +### SBG_ECAN_MSG_GPSX_HDT (0x179 - 0x189) {#SBG_ECAN_MSG_GPSX_HDT} + +The `SBG_ECAN_MSG_GPSX_HDT` message provides the GNSS true heading, pitch angle, and their respective accuracies. + +- **Message Name (ID):** `SBG_ECAN_MSG_GPS1_HDT (0x179)`, `SBG_ECAN_MSG_GPS2_HDT (0x189)` +- **Compatibility:** All products with dual antenna GNSS capability +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 8 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|------------------|----------------------------------------------------------------------------------------|---------|-------|--------|------|--------| +| TRUE_HEADING | True heading angle (0 to 360°) | 10^-2 | ° | uint16 | 2 | 0 | +| TRUE_HEADING_ACC | 1σ True heading estimated accuracy (0 to 360°) | 10^-2 | ° | uint16 | 2 | 2 | +| PITCH | Pitch angle from the master to the rover | 10^-2 | ° | int16 | 2 | 4 | +| PITCH_ACC | 1σ Pitch estimated accuracy | 10^-2 | ° | uint16 | 2 | 6 | +## Event Markers + +SBG Systems Inertial Systems can detect event markers at up to 1 kHz on synchronization input signals, such as Sync A, Sync B, Sync C, Sync D, and Sync E. +For each input synchronization signal, the device outputs a binary log that captures the timestamp of each received event within the last 5 milliseconds. +The maximum output rate for these logs is 200 Hz. + +The `TIME_STAMP` field records the time of the first event detected within the 5 ms window. +Subsequent events within this period are captured using `TIME_OFFSET` fields, which store the time difference between the `TIME_STAMP` and each subsequent event. +This approach optimizes log size by reducing redundant timestamp data. + +**Example:** + +If three events are received within the last 5 ms: +- The first event's time is recorded directly in `TIME_STAMP`. +- The second event's time is recorded as `TIME_STAMP + TIME_OFFSET_0`. +- The third event's time is recorded as `TIME_STAMP + TIME_OFFSET_1`. + +If there are fewer events, the remaining `TIME_OFFSET` fields are set to 0. The `EVENT_STATUS` flag indicates which `TIME_OFFSET` fields are valid. + +### SBG_ECAN_MSG_EVENT_INFO_X (0x200 - 0x207) {#SBG_ECAN_MSG_EVENT_INFO_X} + +This message provides the timestamp and status of event markers. + +- **Message Name (ID):** `SBG_ECAN_MSG_EVENT_INFO_A (0x200)`, `SBG_ECAN_MSG_EVENT_INFO_B (0x202)`, `SBG_ECAN_MSG_EVENT_INFO_C (0x204)`, `SBG_ECAN_MSG_EVENT_INFO_D (0x206)`, `SBG_ECAN_MSG_EVENT_INFO_E (0x207)` +- **Compatibility:** All products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 6 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|------------------|--------------------------------------------------------------------------|---------|-------|--------|------|--------| +| TIME_STAMP | Time since the sensor is powered up | 1 | µs | uint32 | 4 | 0 | +| EVENT_STATUS | Status bitmask (see [EVENT_STATUS](#EVENT_STATUS)) | - | - | uint16 | 2 | 4 | + +### SBG_ECAN_MSG_EVENT_TIME_X (0x201 - 0x209) {#SBG_ECAN_MSG_EVENT_TIME_X} + +This message logs the time offsets for events received after the initial event within a 5 ms window. + +- **Message Name (ID):** `SBG_ECAN_MSG_EVENT_TIME_A (0x201)`, `SBG_ECAN_MSG_EVENT_TIME_B (0x203)`, `SBG_ECAN_MSG_EVENT_TIME_C (0x205)`, `SBG_ECAN_MSG_EVENT_TIME_D (0x207)`, `SBG_ECAN_MSG_EVENT_TIME_E (0x209)` +- **Compatibility:** All products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.7-blue) ![HPINS](https://img.shields.io/badge/HPINS-2.1-blue) +- **Payload Size:** 8 bytes + +| Field | Description | Scaling | Unit | Format | Size | Offset | +|------------------|--------------------------------------------------------------------------|---------|-------|--------|------|--------| +| TIME_OFFSET_0 | Time offset for the second received event | 1 | µs | uint16 | 2 | 0 | +| TIME_OFFSET_1 | Time offset for the third received event | 1 | µs | uint16 | 2 | 2 | +| TIME_OFFSET_2 | Time offset for the fourth received event | 1 | µs | uint16 | 2 | 4 | +| TIME_OFFSET_3 | Time offset for the fifth received event | 1 | µs | uint16 | 2 | 6 | + +> [!NOTE] +> The system can detect event markers at rates up to 1 kHz. However, excessive event rates may overload the internal CPU, leading to decreased performance and reliability. + +> [!WARNING] +> Never leave an activated Sync In signal unconnected, as noise on the line may trigger spurious events at very high rates, potentially causing issues with system performance. diff --git a/doc/commonDefs.md b/doc/commonDefs.md new file mode 100644 index 0000000..2042cf2 --- /dev/null +++ b/doc/commonDefs.md @@ -0,0 +1,383 @@ +# General definitions + +General types, enums, values definitions that are used across the sbgECom protocol documentation and sbgECom C library implementation. + +## Endianness + +The sbgECom binary protocol uses the little-endian data format. +However, some third-party binary messages may use the big-endian format. These instances are explicitly noted in this documentation. + +The sbgECom communication library is designed to be compatible with both little-endian and big-endian platforms. +You can configure the host platform's endianness by setting the macro `SBG_CONFIG_BIG_ENDIAN` to `1`. + +> [!NOTE] +> For ASCII or NMEA messages, the platform's endianness does not impact the parsing or generation of these messages. + +## Types Definitions + +This section defines common types that are using through the documentation and protocol. + +### Scalar Types + +The following fundamental scalar types are used throughout the protocol documentation: + +| Type | Description | +|--------|-------------------------------------------------------------------------------------------| +| Mask | An unsigned integer variable used to store a set of bit masks. | +| Enum | A set of named integer values representing a status or state. | +| bool | 8-bit boolean; `0x00` is FALSE, `0x01` is TRUE. | +| uint8 | 8-bit unsigned integer. | +| int8 | 8-bit signed integer. | +| uint16 | 16-bit unsigned integer. | +| int16 | 16-bit signed integer. | +| uint32 | 32-bit unsigned integer. | +| int32 | 32-bit signed integer. | +| uint64 | 64-bit unsigned integer. | +| int64 | 64-bit signed integer. | +| float | 32-bit single-precision floating point, standard IEEE 754 format. | +| double | 64-bit double-precision floating point, standard IEEE 754 format. | +| void[] | Data buffer with variable length. | +| string | Null-terminated ASCII C string. The maximum size of the string is defined in the message. | + +> [!NOTE] +> The `Mask` and `Enum` types do not have a predefined size in bytes. Refer to the specific message or command payload documentation for the corresponding size. + +> [!NOTE] +> The [] symbol denotes an array of scalar types. For example, float[] indicates an array of single-precision floating point values. + +### Mathematical Types + +Please find below the documentation for 3D mathematical vectors and 3x3 matrices used through the protocol documentation: + +### Vector Objects + +Vectors are stored as a 1D array of `float` or `double` components, with the components arranged sequentially in memory. + +Mathematically, a 3D vector *V* with components *x*, *y*, *z* can be represented as: + +``` + / x \ + V = | y | + \ z / +``` + +In memory, the vector is stored as: + +```C +[x, y, z] +``` + +### Matrix Objects + +Matrices are stored as a 1D array of `float` or `double` elements, organized in a column-major format. +This means that the elements of the matrix are stored in memory column by column. + +For example, a 3x3 matrix *M* with elements aij (where *i* is the row index and *j* is the column index) is represented as: + +``` + / a11 a12 a13 \ + M = | a21 a22 a23 | + \ a31 a32 a33 / +``` + +In memory, this matrix is stored as: + +```C +[a11, a21, a31, a12, a22, a32, a13, a23, a33] +``` + +In this layout, the first three elements correspond to the first column of the matrix, the next three elements correspond to the second column, and the last three elements correspond to the third column. + +### Version/Revision Type {#VERSION_TYPE} + +Some commands and messages contain version/revision numbers encoded as 32-bit unsigned integers. +These are used to track different versions of hardware and software components. + +There are two schemes, each with distinct usage and interpretation: + - **Basic Revision**: Used for hardware revisions, following a simple `A.B.C.D` format. + - **Software Version**: Used for firmware and software, following a `Major.Minor.Build-Qualifier` format. + +The most significant bit (MSB) of the version number determines the scheme in use: +- **MSB = 0**: Indicates the basic revision scheme. +- **MSB = 1**: Indicates the software version scheme. + +#### Basic Revision Scheme + +This scheme follows the `A.B.C.D` format and is used for versioning hardware and components, excluding firmware or software. + +The basic revision scheme is organized in memory as follows: + +| Field | Size (bits) | Definition | +|---------|-------------|-----------------------------------------------------------------------------| +| SCHEME | 1 (MSB) | Set to 0 for basic revision scheme. | +| A | 7 | A version number (0 to 127). | +| B | 8 | B version number (0 to 255). | +| C | 8 | C version number (0 to 255). | +| D | 8 | D version number (0 to 255). | + +> [!NOTE] +> It is common to use only `A.B` as `Major.Minor` for hardware revisions, leaving `C.D` as zero. + +#### Software Version Scheme + +This scheme uses the `Major.Minor.Build-Qualifier` format and applies to all firmware and software items. + +The software version scheme is organized in memory as follows: + +| Field | Size (bits) | Definition | +|-----------|-------------|---------------------------------------------------------------------------| +| SCHEME | 1 (MSB) | Set to 1 for software version scheme. | +| QUALIFIER | 3 | Software release qualifier (see [VERSION_QUALIFIER](#VERSION_QUALIFIER)). | +| MAJOR | 6 | Major version number (0 to 63). | +| MINOR | 6 | Minor version number (0 to 63). | +| BUILD | 16 | Build number (0 to 65535). | + +##### Version Qualifier Enum {#VERSION_QUALIFIER} + +The `QUALIFIER` field indicates the build type for a firmware release: + +| Name | Value | Description | +|-------------------------------|-------|----------------------------------------------------------------------------------------------| +| SBG_VERSION_QUALIFIER_DEV | 0 | Development build (pre-alpha). Used for testing new technologies. | +| SBG_VERSION_QUALIFIER_ALPHA | 1 | Alpha version, may be unstable, missing features, API can change. | +| SBG_VERSION_QUALIFIER_BETA | 2 | Beta version, feature-freeze, can be unstable, API should not change. | +| SBG_VERSION_QUALIFIER_RC | 3 | Release Candidate, feature-freeze, no known bugs, API is stable. | +| SBG_VERSION_QUALIFIER_STABLE | 4 | Stable release, suitable for production environments. | +| SBG_VERSION_QUALIFIER_HOT_FIX | 5 | Bug fixes applied to a stable version, under validation before releasing a new stable build. | + +### IPv4 Address Type + +The sbgECom protocol defines the type `sbgIpAddress` to handle an IPv4 address. +The underlying type is a `uint32` that stores the four 8-bit IP address fields in big-endian order to comply with Ethernet endianness. + +As a result, the IP address 192.168.1.2 (A.B.C.D) is organized in memory as follows: + +#### Little Endian Platform + +| Byte Index | 0 (LSB) | 1 | 2 | 3 (MSB) | +|--------------|---------|-------|-------|---------| +| Decimal | 192 | 168 | 1 | 2 | +| Hex | 0xC0 | 0xA8 | 0x01 | 0x02 | + +**uint32 value**: `33 663 168` (in decimal). + +#### Big Endian Platform + +| Byte Index | 0 (MSB) | 1 | 2 | 3 (LSB) | +|------------|---------|-------|-------|---------| +| Decimal | 192 | 168 | 1 | 2 | +| Hex | 0xC0 | 0xA8 | 0x01 | 0x02 | + +**uint32 value**: `3 232 235 778` (in decimal). + +## Error Code Enumeration {#SbgErrorCode} + +Through the protocol documentation and sbgECom library implementation, the following error codes are being used: + +| Error Code | Value | Description | +|-------------------------------------|-------|-------------------------------------------------------------------------------------------| +| SBG_NO_ERROR | 0 | The command has been properly executed. | +| SBG_ERROR | 1 | Command could not be executed properly due to a generic error. | +| SBG_NULL_POINTER | 2 | A pointer is null and it is not expected. | +| SBG_INVALID_CRC | 3 | A frame with an invalid CRC has been received. | +| SBG_INVALID_FRAME | 4 | The received frame is invalid and mal-formatted. | +| SBG_TIME_OUT | 5 | An operation has timed out and no answer has been received. | +| SBG_WRITE_ERROR | 6 | Some data couldn't be written over an interface (serial/file/ethernet). | +| SBG_READ_ERROR | 7 | Some data couldn't be read over an interface (serial/file/ethernet). | +| SBG_BUFFER_OVERFLOW | 8 | A buffer is too small to contain so much data. | +| SBG_INVALID_PARAMETER | 9 | A parameter has a non valid value. | +| SBG_NOT_READY | 10 | An operation or request couldn't be completed such as no data received or device is busy. | +| SBG_MALLOC_FAILED | 11 | Failed to allocate a buffer (out of memory). | +| SBG_DEVICE_NOT_FOUND | 16 | A resource or device couldn't be found. | +| SBG_OPERATION_CANCELLED | 17 | An operation has been cancelled by the user. | +| SBG_NOT_CONTINUOUS_FRAME | 18 | We have received a frame that is part of a sequence but are missing the end. | +| SBG_INCOMPATIBLE_HARDWARE | 19 | The command cannot be executed because of hardware incompatibility. | +| SBG_INVALID_VERSION | 20 | Command cannot be executed because of software incompatibility. | + +## Conventions & Units + +SBG Systems IMU, AHRS and INS devices adhere to the International System of Units (SI) where applicable. +The device coordinate frame is defined as North East Down (NED). + +| Physical Quantity | Unit Description | +|---------------------|--------------------------------------------------------------------------| +| Angle | Radians (rad), representing roll, pitch, and yaw. | +| Rotational Speed | Radians per second (rad/s or rad.s⁻¹). | +| Acceleration | Meters per second squared (m/s² or m.s⁻²). | +| Velocity | Meters per second (m/s or m.s⁻¹). | +| Latitude | Degrees, positive values indicate North, negative values indicate South. | +| Longitude | Degrees, positive values indicate East, negative values indicate West. | +| Altitude | Meters, altitude above Mean Sea Level (MSL), positive upward. | +| Height | Meters, height above the WGS-84 Ellipsoid, positive upward. | +| Heave | Vertical displacement, positive downward. | +| Surge | Longitudinal displacement, positive toward the vessel's bow. | +| Sway | Transverse displacement, positive toward the vessel's starboard side. | + +## GNSS Definitions +This section covers common definitions related to GNSS used by the sbgECom binary protocol. + +### Constellations {#GNSS_CONSTELLATION} +Below are the enum definitions for all GNSS constellations. + +| Enum | Value | Description | +|-----------------------------------|-------|----------------------------------| +| SBG_ECOM_CONSTELLATION_ID_UNKNOWN | 0 | Unknown | +| SBG_ECOM_CONSTELLATION_ID_GPS | 1 | GPS | +| SBG_ECOM_CONSTELLATION_ID_GLONASS | 2 | GLONASS | +| SBG_ECOM_CONSTELLATION_ID_GALILEO | 3 | Galileo | +| SBG_ECOM_CONSTELLATION_ID_BEIDOU | 4 | BeiDou | +| SBG_ECOM_CONSTELLATION_ID_QZSS | 5 | QZSS | +| SBG_ECOM_CONSTELLATION_ID_SBAS | 6 | SBAS | +| SBG_ECOM_CONSTELLATION_ID_IRNSS | 7 | IRNSS | +| SBG_ECOM_CONSTELLATION_ID_LBAND | 8 | L-Band | + +### Satellite Id {#GNSS_SAT_ID} + +The sbgECom protocol uses the RINEX satellite Id definition to identify each space vehicle tracked by a GNSS receiver. +The satellite Id definition depends on the GNSS constellation: +- **PRN** for GPS, Galileo, BeiDou, IRNSS +- **Slot number** for GLONASS +- **PRN-100** for SBAS +- **PRN-192** for QZSS + +### Signals {#GNSS_SIGNAL_ID} +Below are the enum definitions for GNSS signals. + +#### GPS constellation (10 to 39) +| Enum | Value | +|--------------------------------------|-------| +| SBG_ECOM_SIGNAL_ID_UNKNOWN | 0 | +| SBG_ECOM_SIGNAL_ID_GPS_L1C_DP | 10 | +| SBG_ECOM_SIGNAL_ID_GPS_L1C_D | 11 | +| SBG_ECOM_SIGNAL_ID_GPS_L1C_P | 12 | +| SBG_ECOM_SIGNAL_ID_GPS_L1_W | 13 | +| SBG_ECOM_SIGNAL_ID_GPS_L1_CA | 14 | +| SBG_ECOM_SIGNAL_ID_GPS_L1P | 15 | +| SBG_ECOM_SIGNAL_ID_GPS_L1_PY | 16 | +| SBG_ECOM_SIGNAL_ID_GPS_L1M | 17 | +| SBG_ECOM_SIGNAL_ID_GPS_L2C_ML | 18 | +| SBG_ECOM_SIGNAL_ID_GPS_L2C_L | 19 | +| SBG_ECOM_SIGNAL_ID_GPS_L2_SEMICL | 20 | +| SBG_ECOM_SIGNAL_ID_GPS_L2_W | 21 | +| SBG_ECOM_SIGNAL_ID_GPS_L2_CA | 22 | +| SBG_ECOM_SIGNAL_ID_GPS_L2C_M | 23 | +| SBG_ECOM_SIGNAL_ID_GPS_L2_PY | 24 | +| SBG_ECOM_SIGNAL_ID_GPS_L2M | 25 | +| SBG_ECOM_SIGNAL_ID_GPS_L2P | 26 | +| SBG_ECOM_SIGNAL_ID_GPS_L5_IQ | 27 | +| SBG_ECOM_SIGNAL_ID_GPS_L5_I | 28 | +| SBG_ECOM_SIGNAL_ID_GPS_L5_Q | 29 | + +#### GLONASS constellation (40 to 59) +| Enum | Value | +|--------------------------------------|-------| +| SBG_ECOM_SIGNAL_ID_GLONASS_G1_P | 40 | +| SBG_ECOM_SIGNAL_ID_GLONASS_G1_CA | 41 | +| SBG_ECOM_SIGNAL_ID_GLONASS_G2_P | 42 | +| SBG_ECOM_SIGNAL_ID_GLONASS_G2_CA | 43 | +| SBG_ECOM_SIGNAL_ID_GLONASS_G3_I | 44 | +| SBG_ECOM_SIGNAL_ID_GLONASS_G3_Q | 45 | +| SBG_ECOM_SIGNAL_ID_GLONASS_G3_IQ | 46 | + +#### Galileo constellation (60 to 99) +| Enum | Value | +|--------------------------------------|-------| +| SBG_ECOM_SIGNAL_ID_GALILEO_E1_BC | 60 | +| SBG_ECOM_SIGNAL_ID_GALILEO_E1_C | 61 | +| SBG_ECOM_SIGNAL_ID_GALILEO_E1_B | 62 | +| SBG_ECOM_SIGNAL_ID_GALILEO_E1_A | 63 | +| SBG_ECOM_SIGNAL_ID_GALILEO_E1_ABC | 64 | +| SBG_ECOM_SIGNAL_ID_GALILEO_E5B_IQ | 65 | +| SBG_ECOM_SIGNAL_ID_GALILEO_E5B_I | 66 | +| SBG_ECOM_SIGNAL_ID_GALILEO_E5B_Q | 67 | +| SBG_ECOM_SIGNAL_ID_GALILEO_E5A_IQ | 68 | +| SBG_ECOM_SIGNAL_ID_GALILEO_E5A_I | 69 | +| SBG_ECOM_SIGNAL_ID_GALILEO_E5A_Q | 70 | +| SBG_ECOM_SIGNAL_ID_GALILEO_E5_IQ | 71 | +| SBG_ECOM_SIGNAL_ID_GALILEO_E5_I | 72 | +| SBG_ECOM_SIGNAL_ID_GALILEO_E5_Q | 73 | +| SBG_ECOM_SIGNAL_ID_GALILEO_E6_BC | 74 | +| SBG_ECOM_SIGNAL_ID_GALILEO_E6_C | 75 | +| SBG_ECOM_SIGNAL_ID_GALILEO_E6_B | 76 | +| SBG_ECOM_SIGNAL_ID_GALILEO_E6_ABC | 77 | +| SBG_ECOM_SIGNAL_ID_GALILEO_E6_A | 78 | + +#### Beidou constellation (100 to 149) +| Enum | Value | +|--------------------------------------|-------| +| SBG_ECOM_SIGNAL_ID_BEIDOU_B1IQ | 100 | +| SBG_ECOM_SIGNAL_ID_BEIDOU_B1I | 101 | +| SBG_ECOM_SIGNAL_ID_BEIDOU_B1Q | 102 | +| SBG_ECOM_SIGNAL_ID_BEIDOU_B1C_P | 103 | +| SBG_ECOM_SIGNAL_ID_BEIDOU_B1C_DP | 104 | +| SBG_ECOM_SIGNAL_ID_BEIDOU_B1C_D | 105 | +| SBG_ECOM_SIGNAL_ID_BEIDOU_B1A_P | 106 | +| SBG_ECOM_SIGNAL_ID_BEIDOU_B1A_DP | 107 | +| SBG_ECOM_SIGNAL_ID_BEIDOU_B1A_D | 108 | +| SBG_ECOM_SIGNAL_ID_BEIDOU_B2IQ | 109 | +| SBG_ECOM_SIGNAL_ID_BEIDOU_B2I | 110 | +| SBG_ECOM_SIGNAL_ID_BEIDOU_B2A_P | 111 | +| SBG_ECOM_SIGNAL_ID_BEIDOU_B2A_DP | 112 | +| SBG_ECOM_SIGNAL_ID_BEIDOU_B2A_D | 113 | +| SBG_ECOM_SIGNAL_ID_BEIDOU_B2Q | 114 | +| SBG_ECOM_SIGNAL_ID_BEIDOU_B2B_P | 115 | +| SBG_ECOM_SIGNAL_ID_BEIDOU_B2B_DP | 116 | +| SBG_ECOM_SIGNAL_ID_BEIDOU_B2B_D | 117 | +| SBG_ECOM_SIGNAL_ID_BEIDOU_B2AB_P | 118 | +| SBG_ECOM_SIGNAL_ID_BEIDOU_B2AB_DP | 119 | +| SBG_ECOM_SIGNAL_ID_BEIDOU_B2AB_D | 120 | +| SBG_ECOM_SIGNAL_ID_BEIDOU_B3IQ | 121 | +| SBG_ECOM_SIGNAL_ID_BEIDOU_B3I | 122 | +| SBG_ECOM_SIGNAL_ID_BEIDOU_B3Q | 123 | +| SBG_ECOM_SIGNAL_ID_BEIDOU_B3A_D | 124 | +| SBG_ECOM_SIGNAL_ID_BEIDOU_B3A_P | 125 | +| SBG_ECOM_SIGNAL_ID_BEIDOU_B3A_DP | 126 | + +#### QZSS constellation (150 to 179) +| Enum | Value | +|--------------------------------------|-------| +| SBG_ECOM_SIGNAL_ID_QZSS_L1C_DP | 150 | +| SBG_ECOM_SIGNAL_ID_QZSS_L1C_D | 151 | +| SBG_ECOM_SIGNAL_ID_QZSS_L1C_P | 152 | +| SBG_ECOM_SIGNAL_ID_QZSS_L1_CA | 153 | +| SBG_ECOM_SIGNAL_ID_QZSS_L1_SAIF | 154 | +| SBG_ECOM_SIGNAL_ID_QZSS_L1_SB | 155 | +| SBG_ECOM_SIGNAL_ID_QZSS_L2C_ML | 156 | +| SBG_ECOM_SIGNAL_ID_QZSS_L2C_L | 157 | +| SBG_ECOM_SIGNAL_ID_QZSS_L2C_M | 158 | +| SBG_ECOM_SIGNAL_ID_QZSS_L5_IQ | 159 | +| SBG_ECOM_SIGNAL_ID_QZSS_L5_I | 160 | +| SBG_ECOM_SIGNAL_ID_QZSS_L5_Q | 161 | +| SBG_ECOM_SIGNAL_ID_QZSS_L5S_IQ | 162 | +| SBG_ECOM_SIGNAL_ID_QZSS_L5S_I | 163 | +| SBG_ECOM_SIGNAL_ID_QZSS_L5S_Q | 164 | +| SBG_ECOM_SIGNAL_ID_QZSS_L6_P | 165 | +| SBG_ECOM_SIGNAL_ID_QZSS_L6_DP | 166 | +| SBG_ECOM_SIGNAL_ID_QZSS_L6_D | 167 | +| SBG_ECOM_SIGNAL_ID_QZSS_L6_E | 168 | +| SBG_ECOM_SIGNAL_ID_QZSS_L6_DE | 169 | + +#### SBAS system (180 to 199) +| Enum | Value | +|--------------------------------------|-------| +| SBG_ECOM_SIGNAL_ID_SBAS_L1_CA | 180 | +| SBG_ECOM_SIGNAL_ID_SBAS_L5_I | 181 | +| SBG_ECOM_SIGNAL_ID_SBAS_L5_Q | 182 | +| SBG_ECOM_SIGNAL_ID_SBAS_L5_IQ | 183 | + +#### IRNSS / NAVIC constellation (200 to 219) +| Enum | Value | +|--------------------------------------|-------| +| SBG_ECOM_SIGNAL_ID_IRNSS_L5_A | 200 | +| SBG_ECOM_SIGNAL_ID_IRNSS_L5_B | 201 | +| SBG_ECOM_SIGNAL_ID_IRNSS_L5_C | 202 | +| SBG_ECOM_SIGNAL_ID_IRNSS_L5_BC | 203 | +| SBG_ECOM_SIGNAL_ID_IRNSS_S9_A | 204 | +| SBG_ECOM_SIGNAL_ID_IRNSS_S9_B | 205 | +| SBG_ECOM_SIGNAL_ID_IRNSS_S9_C | 206 | +| SBG_ECOM_SIGNAL_ID_IRNSS_S9_BC | 207 | + +#### L-Band system (220 to 230) +| Enum | Value | +|--------------------------------------|-------| +| SBG_ECOM_SIGNAL_ID_LBAND | 220 | diff --git a/doc/gettingStarted.md b/doc/gettingStarted.md new file mode 100644 index 0000000..9be3406 --- /dev/null +++ b/doc/gettingStarted.md @@ -0,0 +1,330 @@ +# Getting Started + +This guide will help you quickly get started with the sbgECom ANSI C Library and the [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi/) to configure your device and read data. +It also explains how to use the CLI tools, [sbgEComApi](#sbgEComApi) and [sbgBasicLogger](#sbgBasicLogger), to configure your device and log data to CSV files. + +## Before You Begin + +SBG Systems products use a proprietary binary format, sbgECom, for data output and device communication. +Device configuration is managed using the modern, JSON-based [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi/), which provides a portable and easy-to-integrate solution. + +### sbgECom Library Overview + +The sbgECom library is the C implementation of the sbgECom binary protocol used by the device to send data messages. +It also provides specific commands, such as [SBG_ECOM_CMD_API_GET](#SBG_ECOM_CMD_API_GET) and [SBG_ECOM_CMD_API_POST](#SBG_ECOM_CMD_API_POST), to support [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi/) configuration. + +For example, the [SBG_ECOM_LOG_EKF_EULER](#SBG_ECOM_LOG_EKF_EULER) message returns the AHRS/INS roll, pitch, and yaw angles. + +### sbgInsRestApi Overview + +The [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi/) is a JSON-based RESTful API that handles device configuration and monitoring. +Standard HTTP GET and POST requests are used to interact with the device via various endpoints (URLs). + +For example, entering ``http:///api/v1/settings`` in a web browser retrieves all the device settings in JSON format. + +- **Ethernet Connections**: For devices like HPINS with Ethernet interfaces, standard HTTP GET/POST requests are used to access the [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi/). +- **Serial Connections**: For devices like ELLIPSE v3 with serial interfaces, the sbgECom binary protocol is used to transport HTTP GET/POST requests over the interface using [SBG_ECOM_CMD_API_GET](#SBG_ECOM_CMD_API_GET) and [SBG_ECOM_CMD_API_POST](#SBG_ECOM_CMD_API_POST). + +> [!NOTE] +> ELLIPSE v1 and v2 do not support configuration via sbgInsRestApi and rely only on deprecated sbgECom configuration commands. + +## Building sbgECom Library + +The sbgECom library is designed for easy integration within your projects. Written in clean, well-documented C code with embedded systems in mind. + +It is strongly recommended to use the sbgECom library for proper protocol support and future-proofing against updates. + +> [!NOTE] +> SBG Systems doesn't provide the sbgECom as a pre-compiled static or dynamic library. + +### Dependencies & Requirements + +The sbgECom library and code samples have no third-party dependencies and are written in standard C99. +However, the CLI tools [sbgBasicLogger](#sbgBasicLogger) and [sbgEComApi](#sbgEComApi) are written in C++ (14) and depend on [Argtable3](https://github.com/argtable/argtable3). + +SBG Systems has validated the following tool-chain and libraries: + - \>= CMake 3.0 + - \>= GNU GCC 8 (any platform) + - \>= AppleClang 13 (Mac OS X) + - \>= Visual Studio 2015 or MSBuild equivalent (Windows) + - \>= [Argtable3](https://github.com/argtable/argtable3) (for building CLI tools) + - \>= Git (to fetch [Argtable3](https://github.com/argtable/argtable3)) + +### Platform support + +The sbgECom library can be easily ported to any platform with minimal changes. + +To adapt the library to your specific platform, you'll need to provide or implement a few low-level methods and configurations: + - **Platform Configuration**: Set the platform-specific configurations in `sbgECom/common/sbgConfig.h` to tailor the library to your environment. + - **Time and Sleep Functions**: Implement the functions `sbgGetTime` and `sbgSleep` in `sbgECom/common/platform/sbgPlatform.c` to handle timing and sleep operations according to your platform's requirements. + - **Logging Callback**: Install a callback using `sbgCommonLibSetLogCallback` to track and manage library error, warning, and info logs. + - **Communication Interfaces**: Provide custom implementations for any required communication interfaces in the `sbgECom/common/interfaces/` directory to ensure data transmission and reception on your platform. + +### Building sbgECom + +To build the sbgECom static library, code examples, and command-line tools, go to the `sbgECom` directory and type: + +```sh +cmake -Bbuild -DBUILD_EXAMPLES=ON -DBUILD_TOOLS=ON +cmake --build build --config Debug +``` + +The compiled library, examples, and tools will be located in the `sbgECom/build/Debug` folder. + +> [!NOTE] +> Disable deprecated macros by adding `-DUSE_DEPRECATED_MACROS=OFF` to avoid using outdated defines, macros, and enum values. + +### Installing sbgECom + +To install the compiled sbgECom library on your system, use the following command: + +```sh +cmake --install build +``` + +This will install the static library, headers and cmake files into the default system installation path (e.g., /usr/local on Linux/macOS or C:/Program Files on Windows). + +If you want to customize the installation directory, specify it using --prefix option in the cmake command: + +```sh +cmake --install build --prefix /path/to/install +``` + +Make sure to set the installation path in your system's PATH or library path (LD_LIBRARY_PATH on Linux, DYLD_LIBRARY_PATH on macOS, or environment variables on Windows) if needed. + +## Command Line Tools + +SBG Systems provides two CLI (Command Line Interface) tools to facilitate evaluation and integration. + +You can find the C/C++ source code for each tool in the `sbgECom/tools` directory. +Pre-compiled 64-bit binaries for Windows, Linux, and Mac OS X platforms are available in the `sbgECom/bin/tools` directory. + +### sbgBasicLogger {#sbgBasicLogger} + +The `sbgBasicLogger` tool parses sbgECom messages from a serial or Ethernet interface and writes the log content to CSV files without altering the device configuration. +It can also read sbgECom messages from a binary file, making it useful for converting sbgECom binary streams to text files. + +Additionally, `sbgBasicLogger` can extract raw GNSS data streams, RTCM streams, device settings, information, and error logs. + +For example, the following command displays received messages on the terminal: + +```sh +sbgBasicLogger -s COM1 -r 115200 -p +``` + +Refer to the [sbgBasicLogger README](https://github.com/SBG-Systems/sbgECom/blob/main/tools/sbgBasicLogger/README.md) for more details. + +### sbgEComApi {#sbgEComApi} + +The `sbgEComApi` tool provides easy access to the [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi/) over serial or Ethernet UDP interfaces. +It enables GET and POST queries through command-line arguments, making it ideal for configuring devices like HPINS, ELLIPSE v3, or PULSE using simple bash scripts. + +For example, the following command fetches all device settings as JSON content: + +```sh +sbgEComApi.exe -s COM1 -r 115200 -g /api/v1/settings +``` + +Refer to the [sbgEComApi README](https://github.com/SBG-Systems/sbgECom/blob/main/tools/sbgEComApi/README.md) for more details. + +## Code Examples + +SBG Systems provides several simple C code examples to help you quickly get started with the sbgECom library. +Each example includes both the source code and pre-compiled binaries. + +- **Source Code**: Located in the `sbgECom/examples` directory. +- **Binaries**: Available in the `sbgECom/bin/examples` folder, compiled for 64-bit systems. + +### sbgEComExample {#sbgEComExample} + +This C code sample demonstrates the usage of the `sbgECom` library and the [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi/). +This example supports both serial connections and Ethernet UDP communication. + +**Functionality**: + - Detects a device using `sbgECom` and `sbgInsRestApi`. + - Configures the device to output data at a rate of 10 Hz. + - Displays the received data on the console. + +**Compatibility**: + - ELLIPSE devices (firmware v3 and above) + - High Performance INS (HPINS) products + - PULSE Inertial Measurement Units (IMUs) + +The code is written in plain ANSI C and is designed to work on any POSIX and Windows system. +With minor modifications, it can also run on bare-metal devices. + +The example source file is located in the `sbgECom/examples/sbgEComExample` directory. + +#### Usage Examples + +- **ELLIPSE v3 (Serial)**: ELLIPSE devices communicate at 115200 bps over the default serial port A. + + ```sh + sbgEComExample -s COM1 -r 115200 + ``` + +- **HPINS (Ethernet UDP)**: Uses an Ethernet UDP connection with the device's default output port set to 1234 and input port set to 5678. + The IP address is typically acquired via DHCP. In this example, assume the device IP address is 192.168.1.1: + + ```sh + sbgEComExample -a 192.168.1.1 -I 1234 -O 5678 + ``` + +- **PULSE (Serial)**: PULSE IMUs communicate at 921600 bps by default: + + ```sh + sbgEComExample serial -s COM1 -r 921600 + ``` + +> [!NOTE] +> HPINS devices also support serial communication on Port A with a default baud rate of 115200 bps. + +### ellipseLegacy + +This legacy C code sample demonstrates the usage of `sbgECom` for ELLIPSE devices running firmware v2 or earlier. +It uses only the legacy configuration commands and does not rely on the newer [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi/). + +The example source file is located in the `sbgECom/examples/ellipseLegacy` directory. + +#### Usage Example +To run the example for an ELLIPSE device communicating at 115200 bps over the default serial port A: + +```sh + ellipseLegacy COM1 115200 +``` + +### Magnetic Calibration (sbgInsRestApi) + +The [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi/) provides easy access to the ELLIPSE on-board magnetic field calibration. + +This guide demonstrates how to use the sbgEComApi CLI tool to interact with the [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi/). +Alternatively, you can implement this procedure using the [SBG_ECOM_CMD_API_GET](#SBG_ECOM_CMD_API_GET) and [SBG_ECOM_CMD_API_POST](#SBG_ECOM_CMD_API_POST) sbgECom commands. + +In this example, we focus on a 3D on-board magnetic calibration. +For a 2D calibration, simply change the `mode` parameter to `2d` in the payload and ensure that the device is rotated only horizontally during the magnetic field acquisition. + +#### Step 1: Start Magnetic Field Acquisition + +Clear any previously acquired magnetic field data and prepare the device for a new 3D magnetic field acquisition: + +```sh +sbgEComApi.exe -s COM1 -r 115200 api/v1/magnetometer/calibration/start -p -b {\"mode\":\"3d\"} +``` + +#### Step 2: Compute Magnetic Calibration + +Slowly rotate the unit around each IMU axis to acquire representative magnetic field data. +Once enough data has been collected, compute the new magnetic calibration parameters: + +```sh +sbgEComApi.exe -s COM1 -r 115200 api/v1/magnetometer/calibration -g +``` + +If successful, you should receive output similar to the following: + +```json +{ + "status":"success", + "quality":"medium", + "trust":"high", + "maxNumPoints":1000, + "numPointsTotal":1769, + "numPointsUsed":783, + "is2dCalibration":false, + "enoughPts":true, + "rollMotionValid":true, + "pitchMotionValid":true, + "yawMotionValid":true, + "alignmentNumPtsTotal":533, + "alignmentNumPtsUsed":533, + "alignmentQuality":0.028159335255622864, + "alignmentEnoughPts":true, + "alignmentKeptEnoughPts":true, + "hardIronCorrection":[ + 0.10573061555624008, + 0.16595759987831116, + -0.25122913718223572 + ], + "softIronCorrection":[ + 1.3221145868301392, + -0.077110230922698975, + 0.098171010613441467, + 0.012074913829565048, + 1.1894962787628174, + 0.01619420014321804, + 0.031276974827051163, + 0.11012999713420868, + 1.2979389429092407 + ], + "ellipsoidMatrix":[ + -0.25842645764350891, + -0.73661297559738159, + 0.3940565288066864, + -0.46735265851020813, + 0.41018226742744446, + 0.4602622389793396, + -0.53718864917755127, + -0.069976426661014557, + -0.48310169577598572 + ], + "beforeMeanError":0.16253998875617981, + "beforeStdError":4.72862672805786, + "beforeMaxError":10.900124549865723, + "afterMeanError":0.00320892222225666, + "afterStdError":0.79626452922821045, + "afterMaxError":2.2561728954315186, + "meanAccuracy":0.0052680773660540581, + "stdAccuracy":0.01319846510887146, + "maxAccuracy":0.0277938898652792 +} +``` + +> [!NOTE] +> If you are not satisfied with the results, you can continue acquiring magnetic field data and recompute the calibration until the desired accuracy is achieved. + +#### Step 3: Apply the New Magnetic Calibration + +Extract the `hardIronCorrection` and `softIronCorrection` parameters from the calibration results to update the magnetic calibration. +Ensure that the `calibrationMode` field is correctly set to reflect the method used, whether `2d` or `3d`. + +```sh +sbgEComApi.exe -s COM1 -r 115200 api/v1/settings/aiding/magnetometer -p -b {\"hardIronCorrection\":[0.1057,0.1660,-0.2512],\"softIronCorrection\":[1.322114587,-0.077110231,0.098171011,0.012074914,1.189496279,0.016194200,0.031276975,0.110129997,1.297938943],\"calibrationMode\":\"3d\"} +``` + +> [!NOTE] +> In the example above, the values have been rounded to reduce the command line length. + +#### Step 4: Save the New Magnetic Calibration + +This step is essential to store the new magnetic calibration settings in the device's FLASH memory. +The new calibration will only take effect after the device has been rebooted: + +```sh +sbgEComApi.exe -s COM1 -r 115200 api/v1/settings/save -p +sbgEComApi.exe -s COM1 -r 115200 api/v1/system/reboot -p +``` + +### Magnetic Calibration (Legacy) + +This minimal C example demonstrates how to use the ELLIPSE on-board magnetic calibration using the legacy sbgECom commands. + +This example is relevant for legacy code that has not yet migrated to the new [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi/). +It is also applicable to ELLIPSE v1/v2 devices that do not support the new [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi/). + +The example source file can be found in the `sbgECom/examples/onBoardMagCalib` directory. + +You can try this example by typing the following command line: + +```sh +onBoardMagCalib COM1 115200 +``` + +### Air Data Input + +A simple C example illustrating how to send air data aiding measurements to an ELLIPSE using the sbgECom binary protocol and library. + +The following command will send air data aiding to the device while simultaneously reading output messages on the same serial interface: + +```sh +airDataInput COM1 115200 +``` diff --git a/doc/inputProtocols.md b/doc/inputProtocols.md new file mode 100644 index 0000000..4409cb5 --- /dev/null +++ b/doc/inputProtocols.md @@ -0,0 +1,183 @@ +## External Aiding Protocols + +Inertial Navigation Systems (INS) can interface with various external equipment to enhance their accuracy by integrating aiding information. +The most common external aiding device is a GNSS receiver, which typically provides data using the NMEA standard protocol. +However, for optimal accuracy and robustness, SBG Systems also supports native binary protocols from leading third-party equipment manufacturers. + +This section provides details on the supported third-party equipment and protocols, as well as their integration with SBG Systems' INS products. +Note that the availability of these input protocols varies by product family and type and you should check your product dedicated user manual. + +### sbgECom Input Protocol + +The sbgECom protocol allows the INS to receive specific aiding information, such as external velocity or altitude (airdata). +This binary protocol is compact and secure, making it ideal for injecting real-time aiding data. + +The following sbgECom logs are accepted based on the aiding configuration: + +- [SBG_ECOM_LOG_AIR_DATA (36)]{#SBG_ECOM_LOG_AIR_DATA}: Used by the AirData module to inject external barometric altitude and true airspeed. + +> [!NOTE] +> Please refer to the dedicated Operating Handbooks for more details and code samples. + +### NMEA Protocol + +The NMEA protocol is commonly used for GPS aiding data in a read-only mode. +The following NMEA sentences are expected for real-time INS integration: + +| Message | Rate | Required | Description | +|---------------------------|------|----------|--------------------------------------------------------------------------------------------------------------| +| **GGA** | 5 Hz | Required | GNSS position solution and type. | +| **GST** | 5 Hz | Required | GNSS position accuracy. | +| **RMC** | 5 Hz | Required | GNSS velocity and course. | +| **HDT** | 5 Hz | Optional | Dual antenna true heading solution. | +| **GSV** | 5 Hz | Required | Satellites in view with SNR, azimuth and elevation. | +| **ZDA** | 1 Hz | Required | GNSS UTC time and date information associated with the PPS signal. | + +> [!NOTE] +> Refer to the NMEA GNSS Integration Operating Handbook for detailed information. + +### UBX Protocol + +The UBX binary protocol from u-blox delivers optimal performance when integrating an external u-blox GNSS receiver with the INS. +SBG Systems INS devices support u-blox generations 8, 9, and 10, as well as high-precision u-blox products like the F9P. + +This protocol allows the INS to interface with both primary and secondary positioning engines. +It enabled a single u-blox F9P receiver to compute both a true heading solution and a standalone PVT (Position, Velocity, Time) solution. + +| Message | Rate | Required | Description | +|-----------------------|------|----------|--------------------------------------------------------------------------------------------------------------| +| **UBX-NAV-DOP** | 1 Hz | Optional | Returns GNSS solution Dilution of Precision. Used primarily for NMEA-like outputs. | +| **UBX-NAV-RELPOSNED** | 5 Hz | Optional | Provides relative position between GNSS 1 and 2 for true heading determination. | +| **UBX-NAV-HPPOSLLH** | 5 Hz | Optional | High-precision geodetic position solution, essential for achieving RTK-level accuracy. | +| **UBX-NAV-PVT** | 5 Hz | Required | Standard precision navigation solution, including position, velocity, and time. | +| **UBX-NAV-SIG** | 1 Hz | Optional | Detailed signal information for each satellite. | +| **UBX-NAV-SAT** | 1 Hz | Optional | Provides satellite data, including satellites in view, with azimuth and elevation. | +| **UBX-NAV-STATUS** | 1 Hz | Optional | Reports receiver navigation status, useful for detecting and indicating spoofing events. | +| **UBX-NAV2-DOP** | 1 Hz | Optional | Same as UBX_NAV_DOP but for the second positioning engine. | +| **UBX-NAV2-PVT** | 5 Hz | Optional | Same as UBX_NAV_PVT but for the second positioning engine. | +| **UBX-NAV2-SIG** | 1 Hz | Optional | Same as UBX_NAV_SIG but for the second positioning engine. | +| **UBX-NAV2-SAT** | 1 Hz | Optional | Same as UBX_NAV_SAT but for the second positioning engine. | +| **UBX-NAV2-STATUS** | 1 Hz | Optional | Same as UBX_NAV_STATUS but for the second positioning engine. | +| **UBX-MON-RF** | 1 Hz | Optional | RF information used to detect and report interface mitigation status, such as jamming. | +| **UBX-RXM-RAWX** | 1 Hz | Optional | Multi-GNSS raw measurements supporting post-processing. | +| **UBX-RXM-SFRBX** | 1 Hz | Optional | Broadcast navigation data subframe, used to extract broadcasted Ephemeris data. | +| **UBX-RXM-RTCM** | 1 Hz | Optional | Status of the input real-time RTCM stream. | + +> [!WARNING] +> Support for external u-blox GNSS receivers is limited to ELLIPSE products only. + +### Novatel Binary Protocol + +The Novatel binary protocol is used to achieve the best performance when connecting an external Novatel GNSS receiver to the INS. +The following messages are supported for real-time INS integration: + +| Message | Rate | Required | Description | +|-------------------------|----------|--------------|------------------------------------------------------------------------------------------------------------------| +| **BESTPOS** | 5 Hz | Required | Best real-time position solution and accuracy. | +| **PSRXYZ** | 5 Hz | Required | Real-time doppler velocity and accuracy. | +| **HEADING** | 5 Hz | Optional | Dual antenna true heading and pitch angles with accuracies. | +| **TIME** | 1 Hz | Required | GNSS UTC time and date information associated with the PPS signal. | +| **SATVIS2** | 1 Hz | Optional | Satellite in view with azimuth/elevation and tracked signals. | + +Additionally, the following messages are stored as part of the GNSS RAW stream to support post-processing: + - **RANGECMP** + - **RANGECMP2** + - **RANGECMP4** + - **RAWEPHEM** + - **GPSEPHEMERIS** + - **IONUTC** + - **GLOEPHEMERIS** + - **QZSSEPHEMERIS** + - **BDSEPHEMERIS** + - **GALEPHEMERIS** + - **GALFNAVEPHEMERIS** + - **GALINAVEPHEMERIS** + - **BDSIONO** + - **QZSSIONUTC** + - **GALIONO** + +> [!NOTE] +> Refer to the Novatel GNSS Integration Operating Handbook for more information. + +### Septentrio SBF Binary Protocol + +The INS integrates with the Septentrio SBF binary protocol for GNSS data aiding. +The following messages are supported for real-time INS integration: + +| Message | Rate | Required | Description | +|-------------------------|----------|--------------|------------------------------------------------------------------------------------------------------------------| +| **PVTGeodetic** | 5 Hz | Required | Real-time position and velocity solution. | +| **PosCovGeodetic** | 5 Hz | Required | Real-time position solution accuracy. | +| **VelCovGeodetic** | 5 Hz | Required | Real-time velocity solution accuracy. | +| **AttEuler** | 5 Hz | Optional | Dual antenna true heading and pitch angles. | +| **AttCovEuler** | 5 Hz | Optional | Accuracy of dual antenna true heading and pitch angles. | +| **AuxAntPositions** | 1 Hz | Optional | Baseline measurement for dual antenna systems. | +| **ReceiverTime** | 1 Hz | Required | GNSS UTC time and date information associated with the PPS signal. | +| **DOP** | 1 Hz | Optional | GNSS Dilution of Precision. | +| **ChannelStatus** | 1 Hz | Optional | Reports all space vehicles in view with tracked/used signals. | +| **RFStatus** | 1 Hz | Optional | Reports interference (jamming) detections and mitigation. | +| **GALAuthStatus** | 1 Hz | Optional | Reports Galileo OSNMA authentication/spoofing status. | + +Additionally, the following messages are stored as part of the GNSS RAW stream to support post-processing: + + - **MeasEpoch** + - **Meas3Ranges** + - **Meas3CN0HiRes** + - **Meas3Doppler** + - **Meas3PP** + - **Meas3MP** + - **EndOfMeas** + - **GPSNav** + - **GPSIon** + - **GPSUtc** + - **GLONav** + - **GLOTime** + - **GALNav** + - **GALIon** + - **GALUtc** + - **GALGstGps** + - **BDSNav** + - **BDSIon** + - **BDSUtc** + - **QZSNav** + +> [!NOTE] +> Refer to the Septentrio GNSS Integration Operating Handbook for more details. + +### Trimble GSOF Binary Protocol + +The Trimble General Serial Output Format (GSOF) provides the necessary information for the INS to compute a real-time navigation solution. +For post-processing, RT-27 frame parsing and storage are supported. + +The following GSOF messages are supported: + +| Message | Rate | Required | Description | +|---------------------------|------|----------|--------------------------------------------------------------------------------------------------------------| +| **PositionTime (0x01)** | 5 Hz | Required | Time of the navigation solution. | +| **LatLongHeight (0x02)** | 5 Hz | Required | Latitude, longitude, and altitude above the ellipsoid. | +| **Velocity (0x08)** | 5 Hz | Required | 3D velocity information. | +| **PositionSigma (0x0C)** | 5 Hz | Required | The real time PVT solution accuracy. | +| **PositionType (0x26)** | 5 Hz | Optional | The type of real time PVT solution. | +| **AttitudeInfo (0x1B)** | 5 Hz | Optional | Dual antenna-based heading. | +| **CurrentUtcTime (0x10)** | 1 Hz | Required | GNSS UTC time and date information associated with the PPS signal. | +| **BriefAllSv (0x21)** | 1 Hz | Optional | Space vehicle information brief information. | +| **SvInfoAll (0x22)** | 1 Hz | Optional | Space vehicle information detailed information - recommended. | +| **SvInfoFull (0x30)** | 1 Hz | Optional | Space vehicle information detailed information over several pages. | + +> [!NOTE] +> Refer to the Trimble GNSS Integration Operating Handbook for more details. + +### Teledyne RDI PD0 Protocol + +The PD0 binary frame is a Teledyne RDI proprietary log that outputs DVL data, including bottom tracking, water tracking, and water profiling. +It is accepted as input for DVL aiding purposes. + +### Teledyne RDI PD6 Protocol + +The PD6 ASCII frame is a Teledyne RDI proprietary log that outputs DVL data, such as bottom tracking and water tracking. +It is accepted as input for DVL aiding purposes. + +### Teledyne RDI Wayfinder Protocol + +The Wayfinder protocol, a Teledyne RDI proprietary protocol for their new miniature DVL, supports bottom tracking velocity information. +It is accepted as input for DVL aiding purposes. diff --git a/doc/introduction.md b/doc/introduction.md new file mode 100644 index 0000000..f133e4d --- /dev/null +++ b/doc/introduction.md @@ -0,0 +1,105 @@ +# Introduction + +The sbgECom protocol is a binary protocol specifically designed for [SBG Systems](https://www.sbg-systems.com/) IMU, AHRS, and INS products. +It includes commands for device configuration and output messages for reading data and status. + +The sbgECom Library, the official C implementation of this protocol, is provided under the [MIT License](#license), enabling customers to accelerate their development processes. +The library's usage is demonstrated with clear C code samples, and useful CLI tools with source code are also provided to facilitate evaluation and integration. + +## Scope + +This documentation is essential for anyone integrating SBG Systems' devices, developing custom software applications, or needing an in-depth understanding of the sbgECom protocol. + +The documentation covers the following items: + +- **Frame Protocol Layer**: Detailed descriptions of protocol framing, including packet structures and error handling. +- **Configuration Commands**: A guide to the commands and methods used for configuring device settings and querying information or status. +- **Message Formats**: Specifications for messages output by devices, detailing data formats and field definitions. +- **C Library Doxygen**: Automatically generated documentation for the C library, covering API details, data structures, and functions. +- **C Code Samples**: Example C code demonstrating common use cases and best practices with the sbgECom library. +- **CLI Tools**: Information on command-line tools for interacting with and configuring devices. +- **Migration Guidelines**: Guidance on transitioning between different versions of the sbgECom protocol and firmware, ensuring smooth upgrades and compatibility. +- **CAN Messages**: Description of CAN message data definitions to quickly interface your product with CAN software or loggers. + +## Resources + +For more information and detailed documentation, please refer to the following resources: + +- **sbgECom Documentation**: Access all versions of the sbgECom protocol and library documentation online [here](https://developer.sbg-systems.com/sbgECom). +- **sbgInsRestApi Documentation**: Refer to the [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi/) documentation for configuring products using a modern and easy-to-use JSON-based RestAPI. +- **SBG Systems Support Center**: Visit the [Support Center](https://support.sbg-systems.com) for user manual, knowledge bases, FAQs, quick start guides and integration support. +- **SBG Systems Website**: Visit the [SBG Systems](https://www.sbg-systems.com) website for product information, FAQs, leaflets, and more. +- **CAN Messages Files**: All CAN messages are defined in `dbc` ([Vector](https://www.vector.com)) and `dbf` ([BusMaster](https://rbei-etas.github.io/busmaster/)) formats in the `sbgECom/can` directory. + +## Supported Products + +SBG Systems offers a range of products, from miniature Inertial Measurement Units (IMUs) to survey-grade Inertial Navigation Systems (INS). +The sbgECom protocol, documentation, and C library implementation are shared across all these products. + +The product lines can be organized into the following main families, each with common features and firmware: + +- **ELLIPSE Series v1/v2**: Miniature AHRS/INS launched in 2014, now superseded by ELLIPSE v3. Configured using sbgECom binary commands; does not support the [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi/). +- **ELLIPSE Micro**: Ultra-small, cost-effective IMU/AHRS. Configured using sbgECom binary commands; does not support the [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi/). +- **ELLIPSE Series v3**: The latest generation of ELLIPSE AHRS/INS launched in 2020 with a more powerful CPU. Supports the [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi/) starting with firmware v3.0 (2024). +- **High Performance INS (HPINS)**: Includes EKINOX, APOGEE, QUANTA, and NAVSIGHT series. These are feature-rich, survey-grade INS products with Ethernet, data logging, and web interface capabilities. They share the same architecture and support the [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi/). +- **PULSE IMUs**: IMUs including gyroscopes and accelerometers, with fewer features and configurations compared to INS. Supports the [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi/). + +> [!WARNING] +> This documentation primarily covers HPINS running firmware v4.x and above, as well as ELLIPSE v3 running firmware v3.x and above. +> For ELLIPSE Series v1/v2, ELLIPSE Micro, or devices running older firmware, please refer to the previous [Firmware Reference Manual](https://support.sbg-systems.com/sc/dev/latest/firmware-documentation) and [sbgECom library v4.0](https://developer.sbg-systems.com/sbgECom/4.0/). + +## Configuration Overview + +The sbgECom protocol and [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi/) work together to provide comprehensive solutions for device configuration and data output. + +### Device Configuration + +The [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi/) is a JSON-based REST API that supersedes sbgECom for configuring devices. +It is the primary method for programmatically configuring products in newer integrations. + +- **Ethernet Interfaces:** For products with Ethernet capabilities, such as the HPINS family, standard HTTP GET/POST methods are used to interact with the API. +- **Serial Interfaces:** For devices like the ELLIPSE v3, which use serial interfaces, the sbgECom protocol provides two specific commands, [SBG_ECOM_CMD_API_GET](#SBG_ECOM_CMD_API_GET) and [SBG_ECOM_CMD_API_POST](#SBG_ECOM_CMD_API_POST), to encapsulate [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi/) GET/POST requests. + +### Reading Data and Status + +The sbgECom protocol implements [binary messages](#binaryMessages) for high data rate and efficient retrieval of measurements, information, and status. +These messages are optimized for real-time applications, offering a secure and efficient binary format ideal for embedded systems. + +These binary messages are also used in post-processing workflows, such as with Qinertia software, and for validation and troubleshooting. + +> [!NOTE] +> The [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi/) offers endpoints for querying information, status, and data at low frequencies (1Hz or less). +> It should not be used for high-frequency data acquisition due to potential CPU overuse and the large data volume generated by JSON format. + +### Configuration Method per Family + +The table below summarizes the usage of sbgECom and the availability of sbgInsRestApi across different device families: + +| Product | sbgECom Usage | sbgInsRestApi | Notes | +|---------------|------------------|---------------|--------------------------------------------------------------------------------------------------------| +| ELLIPSE v1/v2 | Conf. & Messages | Not Available | Latest firmware 2.6 - Support ended in 2024. | +| ELLIPSE Micro | Conf. & Messages | Not Available | Configuration with sbgECom commands only, product still actively supported. | +| ELLIPSE v3 | Messages Only | Available | sbgInsRestApi over serial interface only - Since firmware v3.0 | +| HPINS | Messages Only | Available | sbgInsRestApi through standard HTTP GET/POST methods and over serial interfaces - Since firmware v4.0. | +| PULSE | Messages Only | Available | sbgInsRestApi over serial interface only. | + +## Maintenance and Contribution + +This documentation and the sbgECom C Library are developed and maintained by SBG Systems SAS. +For assistance, feedback, or to report issues, please contact us at [support@sbg-systems.com](mailto:support@sbg-systems.com). + +The sbgECom C Library and code sample is available on GitHub as a public repository. +While you can open issues to report bugs or suggest improvements, please do not submit pull requests as we do not use GitHub for development purposes and cannot merge them. + +## Support + +If you need assistance with the sbgECom C Library or have any questions, please refer to the following resources: + +1. **Documentation**: Detailed documentation is available within the repository and covers the usage, installation, and API of the sbgECom library. + +2. **Reporting Issues**: If you encounter any bugs or have feature requests, please open an issue on the [GitHub issues page](https://github.com/SBG-Systems/sbgECom/issues). Our team will review and address it as soon as possible. + +3. **Email Support**: For more direct support, including technical questions or troubleshooting, contact us at [support@sbg-systems.com](mailto:support@sbg-systems.com). Please include as much detail as possible in your request to help us assist you effectively. + +4. **Online Support:** You can also visit our [Support Page](https://www.sbg-systems.com/support/) for additional resources, including product manuals, FAQs, and contact information for our technical support team. + diff --git a/doc/migrations.md b/doc/migrations.md index d978838..941910c 100644 --- a/doc/migrations.md +++ b/doc/migrations.md @@ -1,6 +1,225 @@ # Migrations These pages help you easily migrate your code from previous sbgECom major versions. +## ELLIPSE Firmware v3 + +ELLIPSE firmware v3 is a complete rewrite from scratch over firmware v2. +This new firmware brings massive performance and reliability improvements for VG, AHRS and INS operating modes as well as magnetic heading and heave accuracy. + +This new firmware also improve the user experience with the new [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi/) configuration API already used by High Performance INS products. + +Finally, the new firmware v3 is only available for ELLIPSE hardware generation 3 thanks to a more powerful CPU and more available RAM. +ELLIPSE v1 and v2 are not concerned by this new firmware with only maintenance support on firmware branch v2.x. + +### New Features + +The main new features or changes introduced by the firmware v3 are: + - Latest generation INS filter algorithms + - Latest generation heave filters + - New magnetic heading aiding implementation + - New on-board magnetic field hard/soft iron compensation routines + - New [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi/) configuration API + - New settings streaming over serial interface + - Up to 1 kHz output rate for IMU data + - Up to 2 Mbit/s serial interface baudrates + - New configuration, operating modes and more + +### Compatibility + +The firmware v3 introduces the new [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi/) as the preferred method to configure the device. +However, the legacy sbgECom configuration commands are still supported to ease firmware migration. + +For most users, existing sbgECom implementations should work with new ELLIPSE firmware v3 without requiring code modifications. +However minor compatibility issues might arose between the ELLIPSE firmware v3 and previous sbgECom versions (before v5). + +When possible, ELLIPSE firmware v3 new features and options have been added to the legacy sbgECom configuration commands. +It should help users benefits from the new features and improvements brought by the ELLIPSE firmware v3 with limited code updates. + +> [!NOTE] +> Even if sbgECom legacy configuration commands have been extended to support some ELLIPSE v3 new features and options, SBG Systems recommends migrating to the new [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi/) configuration API. + +### Serial Interface + +The ELLIPSE firmware v3 allow up to 1 kHz IMU output on serial interface as well as settings streaming. +To better support high throughput, the maximum allowable baudrate has been increased from 921600 bps to 2 Mbit/s. + +> [!WARNING] +> Make sure your hardware and cables are able to cope with such high baudrates. + +### Precise Mechanical Installation + +Starting with ELLIPSE firmware v3, all lever arms and alignment configurations are treated as precisely measured and entered. +In previous firmware versions (v2), the system refined these parameters online, which came at the cost of reliability and accuracy. + +This online estimation feature has been disabled in firmware v3 to ensure more repeatable and robust performance. +The following parameters are affected: +- GNSS primary lever arm +- GNSS secondary lever arm (HDT) +- DVL lever arm + +However, accurately measuring lever arms and alignment parameters during initial product setup can be challenging. +To simplify this process, SBG Systems has developed a free software, the **Qinertia Lever Arm Estimation** tool. + +The Qinertia Lever Arm Estimation tool uses forward/backward post-processing to deliver highly accurate and repeatable estimations. +For detailed instructions, please refer to the specific documentation available on the [Support Center](https://support.sbg-systems.com). + +### Magnetometers + +#### External Magnetometer + +ELLIPSE firmware v3 adds a new option to accept external magnetometer modules. +The command `SBG_ECOM_CMD_AIDING_ASSIGNMENT` has been updated accordingly and `SBG_ECOM_MAG_MODEL_ECOM_NORMAL` model has been added. + +#### Error Model + +The integration of magnetometers within the AHRS/INS solution and their rejection handling has been completely reworked. +As a result, the magnetic error mode `SBG_ECOM_MAG_MODEL_NOISY_MAG_TOLERANT` is no longer relevant. + +The updated magnetometer interference rejection and mitigation algorithm provides improved accuracy, even in magnetically disturbed environments, without sacrificing heading robustness. + +#### 2D/3D Magnetic Calibration + +The new magnetometer implementation requires the AHRS/INS filter to know if the magnetic field has been calibrated using a 2D or 3D procedure. +This new settings is available through the sbgInsRestApi at `api/v1/settings/aiding/magnetometers/calibrationMode` endpoint. + +Legacy sbgECom configuration command `SBG_ECOM_CMD_SET_MAG_CALIB` has been extended to include the 2D/3D calibration mode information. +The new method `sbgEComCmdMagSetCalibData2` has been introduced to provide the mode information. + +## From sbgECom v4.x + +sbgECom v5.x brings several modifications and improvements to support ELLIPSE Firmware v3. +This new version also includes several new outputs, fields as well as deprecates some enums or definitions. + +The sbgECom library and protocol documentation has been also completely rewritten. +This guide should help you update your code base to latest sbgECom 5.x for the new ELLIPSE firmware v3. + +> [!WARNING] +> The sbgECom v4.x supports latest ELLIPSE running firmware v3, HPINS and PULSE products. +> For ELLIPSE running firmware v2 or earlier, it is recommended to use sbgECom v3.x. + +### Documentation + +This version introduce a new unified sbgECom protocol and library documentation. +This documentation supersede the previous [Firmware Reference Manual](https://support.sbg-systems.com/sc/dev/latest/firmware-documentation) PDF. + +The new documentation focuses only on latest products and firmware and doesn't cover ELLIPSE v1/v2 anymore. +You should use the previous Firmware Reference Manual for these products. + +### New DVL model + +ELLIPSE firmware v3 now supports a new DVL mode: `SBG_ECOM_DVL_MODEL_NORTEK` + +### Output Event Triggers + +ELLIPSE firmware v3 introduces new Sync Out trigger options to support up to 1 kHz output rates. +To ease migration to firmware v3, the following new definitions have been added to legacy sbgECom configuration commands: + - `SBG_ECOM_SYNC_OUT_MODE_DIV_5`: Trigger a Sync Out pulse at 40 Hz + - `SBG_ECOM_SYNC_OUT_MODE_DIV_100`: Trigger a Sync Out pulse at 2 Hz + - `SBG_ECOM_SYNC_OUT_MODE_1_MS`: Trigger a Sync Out pulse at 1000 Hz + - `SBG_ECOM_SYNC_OUT_MODE_2_MS`: Trigger a Sync Out pulse at 500 Hz + - `SBG_ECOM_SYNC_OUT_MODE_4_MS`: Trigger a Sync Out pulse at 250 Hz + +> [!WARNING] +> These new trigger enum values are only supported starting with ELLIPSE firmware v3. + +### Output Message Triggers + +ELLIPSE firmware v3 introduces new Output Message trigger options to support up to 1 kHz output rates. +To ease migration to firmware v3, the following new definitions have been added to legacy sbgECom configuration commands: + - `SBG_ECOM_OUTPUT_MODE_DIV_100`: Output a message at 2 Hz + - `SBG_ECOM_SYNC_OUT_MODE_1_MS`: Output a message at 1000 Hz + - `SBG_ECOM_SYNC_OUT_MODE_2_MS`: Output a message at 500 Hz + - `SBG_ECOM_SYNC_OUT_MODE_4_MS`: Output a message at 250 Hz + +> [!WARNING] +> These new trigger enum values are only supported starting with ELLIPSE firmware v3. + +### NMEA Output From GNSS + +A new output message class `SBG_ECOM_CLASS_LOG_NMEA_GNSS` has been added to support NMEA messages from the internal GNSS receiver instead of from the INS filter. +The following new messages are available in this class: + - `SBG_ECOM_LOG_NMEA_GNSS_GGA` + - `SBG_ECOM_LOG_NMEA_GNSS_RMC` + - `SBG_ECOM_LOG_NMEA_GNSS_HDT` + - `SBG_ECOM_LOG_NMEA_GNSS_VTG` + +These definitions have been added to support this new feature using the legacy sbgECom configuration commands. +Of course, SBG Systems recommends using the sbgInsRestApi for new code implementations. + +### Magnetometers Aiding + +To support external magnetometers aiding, a reserved field has been used in the `SBG_ECOM_CMD_AIDING_ASSIGNMENT` command payload to define the new magnetometer module assignment. +However, in sbgECom before 4.0 and before, this field was set to zero resulting in a Port A assignment instead of internal. + +> [!WARNING] +> The command `SBG_ECOM_CMD_AIDING_ASSIGNMENT` might create compatibility issues when used with sbgECom 4.x or before. + +### Magnetometer Models + +The `SBG_ECOM_MAG_MODEL_NOISY_MAG_TOLERANT` error model is not relevant anymore and has been removed. + +The `SBG_ECOM_MAG_MODEL_NORMAL` model has been replaced by `SBG_ECOM_MAG_MODEL_INTERNAL_NORMAL`. +The `SBG_ECOM_MAG_MODEL_ECOM_NORMAL` has been added to support external magnetometers using the sbgECom protocol ([SBG_ECOM_LOG_MAG](#SBG_ECOM_LOG_MAG) message). + +### Magnetometer Calibration + +The hard and soft iron calibration module has been improved as well as how the magnetic field is fused in the AHRS/INS filter. + +The command `SBG_ECOM_CMD_SET_MAG_CALIB` has a new field to indicate if the magnetic calibration is a 2D or 3D one. +This new field is available with the new `sbgEComCmdMagSetCalibData2` method that supersede `sbgEComCmdMagSetCalibData`. + +The command `SBG_ECOM_CMD_START_MAG_CALIB` bandwidth parameter is not used anymore and has no effect. +The method `sbgEComCmdMagStartCalib` is updated accordingly to reflect this change. + +### New Motion Profiles + +Two new motion profiles have been added that tunes the INS filter: + - `SBG_ECOM_MOTION_PROFILE_OFF_ROAD_VEHICLE` + - `SBG_ECOM_MOTION_PROFILE_UNDERWATER` + +### New Output Messages + +#### SBG_ECOM_LOG_PTP_STATUS + +The new [SBG_ECOM_LOG_PTP_STATUS](#SBG_ECOM_LOG_PTP_STATUS) log returns advanced Precise Time Protocol status information for HPINS products. +When the INS doesn't act as the GrandMaster Clock on the network, this message is used to track the delta time between the INS and an external Grand Master Clock. + +#### SBG_ECOM_LOG_SESSION_INFO + +Returns in small chunks and continuously the device current configuration and information. +Used to retrieve the device configuration from output messages binary stream over any serial or Ethernet interface. + +Only available on ELLIPSE firmware v3 and HPINS running firmware 5.3 and above. + +The new class `SbgEComSessionInfoCtx` offers a convenient way to reassemble each [SBG_ECOM_LOG_SESSION_INFO](#SBG_ECOM_LOG_SESSION_INFO) chunk and to get the device information and settings as JSON format. + +### Updated Output Messages + +### SBG_ECOM_LOG_STATUS +Added new CPU usage in percent field + +#### SBG_ECOM_LOG_EFK_EULER and SBG_ECOM_LOG_EFK_QUAT +Added magDeclination and magInclination information +New `sbgEComLogEkfEulerGetMagneticHeading` getter to return INS magnetic heading instead of geographic. + +### SBG_ECOM_LOG_IMU_SHORT +Added a high range scale factor for gyroscopes to support rotation rates above 1833°/s +Transparent if you use the getter `sbgEComLogImuShortGetDeltaAngle` otherwise, please use it. + +### Updated Output Triggers + +The following messages have new output triggers options and are automatically migrated. + +| sbgECom ID | sbgECom Trigger | RestApi Trigger | +|---------------------------|-----------------|-----------------------| +| SBG_ECOM_LOG_IMU_SHORT | New Data | Converted to 200 Hz | +| SBG_ECOM_LOG_MAG_CALIB | 1 Hz to 50 Hz | Converted to New Data | + +### Deprecations + +The message [SBG_ECOM_LOG_IMU_DATA](#SBG_ECOM_LOG_IMU_DATA) is deprecated and [SBG_ECOM_LOG_IMU_SHORT](#SBG_ECOM_LOG_IMU_SHORT) should be used instead. +The message `SBG_ECOM_LOG_FAST_IMU_DATA` has been removed from ELLIPSE firmware v3 and [SBG_ECOM_LOG_IMU_SHORT](#SBG_ECOM_LOG_IMU_SHORT) should be used instead. + ## From sbgECom v3.x sbgECom v4.x is a major improvement over sbgECom v3.x in term of features, code quality and organization. @@ -17,7 +236,7 @@ sbgECom namespace conventions were not aligned with SBG Systems quality standard Most sbgECom binary logs struct, enum and function definitions have been updated to comply with strict namespace. The legacy names have been kept and indicated to be deprecated to avoid breaking changes. -Almost all the source files located in the `src/logs` directory have been reworked. +Almost all the source files located in the `sbgECom/src/logs` directory have been reworked. Legacy sbgECom v3 style: @@ -108,4 +327,5 @@ The method `sbgEComCmdSensorGetMotionProfileInfo` has been replaced by `sbgEComC ### Legacy IG-500 sbgCom -sbgECom version 2.x drop the legacy IG-500 support so the methods `sbgEComCmdOutputGetLegacyConf` and `sbgEComCmdOutputSetLegacyConf` are removed. Please update your code to use sbgECom protocol instead. +sbgECom version 2.x drop the legacy IG-500 support so the methods `sbgEComCmdOutputGetLegacyConf` and `sbgEComCmdOutputSetLegacyConf` are removed. +Please update your code to use sbgECom protocol instead. diff --git a/doc/nmeaMessages.md b/doc/nmeaMessages.md new file mode 100644 index 0000000..8af7948 --- /dev/null +++ b/doc/nmeaMessages.md @@ -0,0 +1,1084 @@ +# NMEA Protocol & Messages {#nmeaMsg} + +SBG Systems AHRS and INS support input and output standard NMEA protocol. +The NMEA protocol is a an well known and simple ASCII based format that is widely used in different industry. + +Using NMEA output messages allows straightforward and easy integration in a wide range of products and software without requiring any development. + +However, NMEA outputs are less efficient as sbgECom binary messages and don't give access to all the features, options and status of SBG Systems products. + +## NMEA Protocol Overview + +The implemented NMEA sentences are based on NMEA 0183 Version 4.1. + +### NMEA Sentence Structure + +The NMEA sentence structure is detailed below with an example: + +``` +$GPZDA,201530.00,04,07,2002,00,00*60 +$[,,]* +``` + +| Value | Description | Example | +|--------------|-------------------------------------------------------------------------------------------------------------|-------------------------------| +| `$` | All frames start with a dollar sign (`$`). | `$` | +| `` | Talker ID, e.g., `GP` for GPS, `GL` for GLONASS, etc. | `GP` | +| `` | Sentence type, message name. | `ZDA` | +| `[,]` | Array of parameters separated by commas (`,`). | `,201530.00,04,07,2002,00,00` | +| `*` | Two characters 8-bit hex value checksum following `*` separator. (see []()). | `*60` | +| `` | All frames end with a carriage return and line feed. | `` | + +**Talker ID** +The NMEA talker ID can be configured for each output interface. +When NMEA messages are used to input aiding data such as an external GNSS, the AHRS/INS simply ignore the Talker ID field. + +**Empty Fields** +Each data field is comma-separated. When a field value is not defined or not available, the field is left empty, as shown below: + +``` +$GPZDA,,,,,,*XX +``` + +### NMEA Checksum + +The checksum for an NMEA sentence is calculated by XORing all characters between the `$` and `*` (exclusive). +The following C code snippet demonstrates how to compute this checksum: + +```c +/*! + * Computes the NMEA 0183 checksum for a given NMEA sentence. + * + * The input sentence should be a complete NMEA message up to the checksum separator '*' + * (e.g., "$GPZDA,,,,,,*"). + * + * \param[in] pSentence Pointer to the NMEA sentence (NULL-terminated C string). + * \return The computed checksum as an 8-bit unsigned integer. + */ +uint8_t nmea0183Checksum(const char *pSentence) +{ + uint8_t checksum = 0; + + assert(pSentence); + + // + // Start at index 1 to skip the leading '$' + // + for (size_t i = 1; (pSentence[i] != '*') && (pSentence[i] != '\0'); i++) + { + checksum ^= (uint8_t)pSentence[i]; + } + + return checksum; +} +``` + +### NMEA Types & Conventions + +To simplify NMEA message definitions, we use specific conventions for representing both integer and decimal numbers. + +#### Integer Numbers + +- **Format**: Integer numbers are represented using the character `i`. + The number of `i` characters indicates the maximum number of digits that can be used to represent the integer. +- **Negative Integers**: The character `-` is prepended to denote a negative integer. +- **Example**: The format `iii` could represent the following integers: `-234`, `13`, `-3`. + +#### Decimal Numbers + +- **Format**: Decimal numbers are represented using the character `f`. + The character `.` is used to separate the integer part from the decimal part. + The number of `f` characters indicates the maximum number of digits that can be used for both the integer and decimal parts. +- **Sign**: The characters `-` or `+` can be prepended to represent negative or positive decimal numbers, respectively. +- **Example**: The format `ff.fff` could represent the following decimal numbers: `-34.2`, `1.205`, `24.126`. + +#### Time Convention + +- **Format**: Time in NMEA messages is always expressed in UTC. It follows the format `hhmmss.ss`. +- **Example**: The time `125430.0` corresponds to `12:54` and `30.00` seconds in UTC. + +#### Date Convention + +- **Format**: Date in NMEA messages is expressed in the format `ddmmyy`, where: + - `dd` is the day of the month (01 to 31), + - `mm` is the month (01 to 12), + - `yy` is the last two digits of the year. +- **Example**: The date `070422` corresponds to `07 April 2022`. + +#### Latitude and Longitude + +- **Format**: Latitude and longitude are represented in the format `ddmm.mmmm`, where: + - `dd` is degrees (00 to 90), + - `mm.mmmm` is minutes with a fractional part, + - It is followed by the hemisphere identifier `N` for North, `S` for South, `E` for East or `W` for West. +- **Example**: The latitude `4807.038,N` corresponds to `48 degrees, 07.038 minutes North`. + +#### Status Indicators + +- **Format**: Status indicators are typically represented by a single letter or number to denote the status of data or operations. + Common examples include: + - `A` for Active or valid data, + - `V` for Void or invalid data. +- **Example**: In a GGA sentence, the character `A` indicates that the GPS fix is active and the data is valid. + +### Quality Indicators + +In NMEA messages, quality indicators traditionally reflect the accuracy of a GNSS fix. +However, when the data source is an INS, these indicators are interpreted differently based on the position standard deviation of the INS solution. +The following table illustrates how these quality indicators are mapped from INS-derived horizontal accuracy to the NMEA standard. + +When the data source is a GNSS, the standard DOP and quality indicator definitions apply. + +| Horizontal Accuracy | NMEA Fix Type | NMEA Status | GGA Quality | GGK Quality | RMC/VTG Mode | RMC Nav. Status | +|---------------------|----------------|-------------|-------------|-------------|--------------|-----------------| +| > 100 m | Invalid Fix | Not valid | 0 | 0 | N | V | +| < 100 m | Dead Reckoning | Caution | 6 | 1 | E | C | +| < 10 m | Standalone Fix | Safe | 1 | 4 | A | S | +| < 1.2 m | DGPS Fix | Safe | 2 | 2 | D | S | +| < 30 cm | Floating RTK | Safe | 5 | 3 | F | S | +| < 10 cm | Fixed RTK | Safe | 4 | 2 | R | S | + +### DOP Computation + +DOP (Dilution of Precision) is a quality indicator traditionally associated with GNSS, reflecting the geometry of the satellites and its impact on positional accuracy. +When using an INS, DOP has no intrinsic meaning because the INS does not rely on satellite geometry. +Instead, the INS provides more reliable quality indicators directly related to the estimated position standard deviation. + +However, since some software still relies on DOP values to filter out outliers and inaccurate solutions, the INS can output a DOP value based on the estimated position standard deviation. +This allows for backward compatibility with systems that use DOP for quality assessment, even though more modern and accurate indicators are available from the INS. + +SBG Systems products implements the following computation to convert a position standard deviation to an horizontal DOP value: + +$$DOP_{std} = \sqrt{North_{std}^2+East_{std}^2}$$ +## Standard NMEA Messages + +The `SBG_ECOM_CLASS_LOG_NMEA_0 (0x02)` message class encompasses all standard NMEA 0183 messages that can be output by the device. +These messages implements the NMEA 0183 version 4.1 and can be used for GNSS drop in replacement in some integrations. + +All NMEA outputs generated by the INS are based on the Inertial Navigation Solution and are not related directly to aiding data such as from a GNSS receiver. + +For instance, even if a GNSS receiver is providing an RTK solution, if the INS is not aligned the product will output GGA messages with a quality indicator of 0 (invalid). This behavior is expected as an INS can still deliver RTK like accuracy solutions even during GNSS outages. + +### Messages overview + +The following list, provides a quick overview of all available logs for this message class. +It briefly describe which parameters are contained in each output log. + +| Name (Msg ID) | Description | +|------------------------------------------------------|----------------------------------------------------------------------------------------| +| [SBG_ECOM_LOG_NMEA_GGA (00)](#SBG_ECOM_LOG_NMEA_GGA) | Latitude, Longitude, Altitude, Quality indicator. | +| [SBG_ECOM_LOG_NMEA_RMC (01)](#SBG_ECOM_LOG_NMEA_RMC) | Latitude, Longitude, velocity, course over ground. | +| [SBG_ECOM_LOG_NMEA_ZDA (02)](#SBG_ECOM_LOG_NMEA_ZDA) | INS UTC date and time. | +| [SBG_ECOM_LOG_NMEA_HDT (03)](#SBG_ECOM_LOG_NMEA_HDT) | Heading from true geographic North. | +| [SBG_ECOM_LOG_NMEA_GST (04)](#SBG_ECOM_LOG_NMEA_GST) | GPS Pseudorange Noise Statistics. | +| [SBG_ECOM_LOG_NMEA_VBW (05)](#SBG_ECOM_LOG_NMEA_VBW) | Water referenced and ground referenced speed data. | +| [SBG_ECOM_LOG_NMEA_DPT (07)](#SBG_ECOM_LOG_NMEA_DPT) | Depth sensor output. | +| [SBG_ECOM_LOG_NMEA_VTG (08)](#SBG_ECOM_LOG_NMEA_VTG) | Track an Speed over the ground. | +| [SBG_ECOM_LOG_NMEA_ROT (09)](#SBG_ECOM_LOG_NMEA_ROT) | Rate and direction of turn. | +| [SBG_ECOM_LOG_NMEA_GSV (10)](#SBG_ECOM_LOG_NMEA_GSV) | GNSS Satellites in View with azimuth, elevation and SNR information. | + +--- + +### SBG_ECOM_LOG_NMEA_GGA (00) {#SBG_ECOM_LOG_NMEA_GGA} + +The `SBG_ECOM_LOG_NMEA_GGA` message reports the Inertial Navigation System (INS) position and time in NMEA GGA format. + +#### Message Format + +``` +$GPGGA,,,,,,0,00,20.0,,,,,,*7A +$GPGGA,000010.00,4852.10719,N,00209.42313,E,0,00,0.0,-44.7,M,0.0,M,,,*63 +``` + +#### Message Structure + +- **Message Name (ID):** `SBG_ECOM_LOG_NMEA_GGA (00)` +- **Compatibility:** INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.0-blue) ![HPINS](https://img.shields.io/badge/HPINS-1.0-blue) + +| Field | Name | Format | Description | +|-------|------------------|----------------|-------------------------------------------------------------------------| +| 0 | $##GGA | string | Message ID – GGA frame | +| 1 | Time | hhmmss.ss | UTC Time, current time | +| 2 | Latitude | ddmm.mmmmmmmm | Latitude: degree + minutes with 8 decimal digits | +| 3 | N/S | char | North / South indicator | +| 4 | Longitude | dddmm.mmmmmmmm | Longitude: degree + minutes with 8 decimal digits | +| 5 | E/W | char | East / West indicator | +| 6 | Quality | i | Solution quality (see definition in Quality indicators) | +| 7 | SV used | ii | Number of satellites used in solution | +| 8 | Horizontal DOP | ff.f | Horizontal dilution of precision (see definition in Quality indicators) | +| 9 | Altitude MSL | ffff.fff | Altitude above Mean Sea Level in meters | +| 10 | M | M | Altitude unit (Meters) fixed field. | +| 11 | Undulation | fff.fff | Geoidal separation between WGS-84 and MSL in meters. | +| 12 | M | M | Units for geoidal separation (Meters) fixed field. | +| 13 | Diff. Age | s.s | Age of differential corrections in seconds, only filled if applicable. | +| 14 | Diff. station ID | rrrr | Differential station ID, only filled if applicable. | +| 15 | Check sum | *cs | XOR of all previous bytes except $ | +| 16 | End of frame | | Carriage return and line feed | + +> [!NOTE] +> The position quality and DOP are calculated using the INS position's estimated standard deviation, and are not directly related to the GNSS solution. + +--- + +### SBG_ECOM_LOG_NMEA_RMC (01) {#SBG_ECOM_LOG_NMEA_RMC} + +The `SBG_ECOM_LOG_NMEA_RMC` message provides the minimum recommended GNSS data, which includes a Kalman-enhanced 2D position, velocity, course over ground, and quality indicators. + +#### Message Format + +``` +$GPRMC,,V,,,,,,,,,,N,V*29 +$GPRMC,010802.26,A,4852.13326,N,00209.49001,E,0.2,195.49,290512,,,A*67 +``` + +#### Message Structure + +- **Message Name (ID):** `SBG_ECOM_LOG_NMEA_RMC (01)` +- **Compatibility:** INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.0-blue) ![HPINS](https://img.shields.io/badge/HPINS-1.0-blue) + +| Field | Name | Format | Description | +|-------|--------------|----------------|-------------------------------------------------------------------------| +| 0 | $##RMC | string | Message ID – RMC frame | +| 1 | Time | hhmmss.ss | UTC Time, current time | +| 2 | Status | char | Status field: A = Valid data, V = Invalid data | +| 3 | Latitude | ddmm.mmmmmmmm | Latitude: degree + minutes with 8 decimal digits | +| 4 | N/S | char | North / South indicator | +| 5 | Longitude | dddmm.mmmmmmmm | Longitude: degree + minutes with 8 decimal digits | +| 6 | E/W | char | East / West indicator | +| 7 | Speed | fff.f | Speed over ground in knots | +| 8 | Course | fff.f | Course over ground in degrees [0; 360] | +| 9 | Date | ddmmyy | UTC day, month, year | +| 10 | Variation | fff.ff | Magnetic variation value in degrees [0; 180] | +| 11 | E/W | char | Direction of magnetic variation (East / West) | +| 12 | Mode | char | Position mode indicator (see definition in Quality indicators section) | +| 13 | Nav Status | char | Navigational status indicator (see definition in Quality indicators) | +| 14 | Check sum | *cs | XOR of all previous bytes except $ | +| 15 | End of frame | | Carriage return and line feed | + +> [!NOTE] +> The position mode and navigational status are calculated using the INS position's estimated standard deviation and are not directly related to the GNSS solution. + +--- + +### SBG_ECOM_LOG_NMEA_ZDA (02) {#SBG_ECOM_LOG_NMEA_ZDA} + +The `SBG_ECOM_LOG_NMEA_ZDA` message contains UTC time and date information. + +#### Message Format + +``` +$GPZDA,,,,,,*48 +$GPZDA,201530.00,04,07,2002,00,00*60 +``` + +#### Message Structure + +- **Message Name (ID):** `SBG_ECOM_LOG_NMEA_ZDA (02)` +- **Compatibility:** INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.0-blue) ![HPINS](https://img.shields.io/badge/HPINS-1.0-blue) + +| Field | Name | Format | Description | +|-------|----------|-------------|------------------------------------------------------| +| 0 | $##ZDA | string | Message ID – ZDA frame | +| 1 | Time | hhmmss.ss | UTC Time, current time | +| 2 | Day | dd | Day of month [01 - 31] | +| 3 | Month | mm | Month of year [01 - 12] | +| 4 | Year | yyyy | Year (4 digits) | +| 5 | Ltzh | 0 | Local zone hours (not supported, fixed 00) | +| 6 | Ltzh | 0 | Local zone minutes (not supported, fixed 00) | +| 7 | Check sum| *cs | XOR of all previous bytes except $ | +| 8 | End of frame | | Carriage return and line feed | + +--- + +### SBG_ECOM_LOG_NMEA_HDT (03) {#SBG_ECOM_LOG_NMEA_HDT} + +The `SBG_ECOM_LOG_NMEA_HDT` message outputs the INS Kalman filtered true heading value. The true heading is the direction the vehicle is pointing, which is not necessarily the same as the direction of travel (course over ground). + +#### Message Format + +``` +$GPHDT,,T*1B +$GPHDT,191.94,T*01 +``` + +#### Message Structure + +- **Message Name (ID):** `SBG_ECOM_LOG_NMEA_HDT (03)` +- **Compatibility:** INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.0-blue) ![HPINS](https://img.shields.io/badge/HPINS-1.0-blue) + +| Field | Name | Format | Description | +|-------|----------|-------------|------------------------------------------------------| +| 0 | $##HDT | string | Message ID – HDT frame | +| 1 | Heading | fff.ff | True heading in degrees [0 - 360] | +| 2 | T | char | T indicates true heading | +| 3 | Check sum| *cs | XOR of all previous bytes except $ | +| 4 | End of frame | | Carriage return and line feed | + +--- + +### SBG_ECOM_LOG_NMEA_GST (04) {#SBG_ECOM_LOG_NMEA_GST} + +The `SBG_ECOM_LOG_NMEA_GST` message provides detailed position error statistics of the Kalman filtered position solution. +It is important to note that the data reflects the estimated inertial position and not the GNSS quality fix directly. + +#### Message Format + +``` +$GPGST,,,,,,,,*57 +$GPGST,172814.00,,0.023,0.020,273.62,0.023,0.015,0.031*46 +``` + +#### Message Structure + +- **Message Name (ID):** `SBG_ECOM_LOG_NMEA_GST (04)` +- **Compatibility:** INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.0-blue) ![HPINS](https://img.shields.io/badge/HPINS-1.0-blue) + +| Field | Name | Format | Description | +|-------------------|------------------|---------------|-------------------------------------------------------------------------| +| 0 | $##GST | string | Message ID – GST frame | +| 1 | Time | hhmmss.ss | UTC Time, current time | +| 2 | psrResidual | NULL | RMS value of pseudorange residuals. Always NULL, not supported. | +| 3 | sMajorAxisError | fff.fff | Error ellipse semi-major axis 1 sigma error, in meters | +| 4 | sMinorAxisError | fff.fff | Error ellipse semi-minor axis 1 sigma error, in meters | +| 5 | errorEllipseAng | fff.ff | Error ellipse orientation, degrees from true north [0 - 360] | +| 6 | latError | fff.fff | Latitude 1 sigma error, in meters | +| 7 | longError | fff.fff | Longitude 1 sigma error, in meters | +| 8 | altError | fff.fff | Height 1 sigma error, in meters | +| 9 | Check sum | *cs | XOR of all previous bytes except $ | +| 10 | End of frame | | Carriage return and line feed | + +--- + +### SBG_ECOM_LOG_NMEA_VBW (05) {#SBG_ECOM_LOG_NMEA_VBW} + +The `SBG_ECOM_LOG_NMEA_VBW` message outputs Ground and Water speed from the INS data fusion algorithm. The data are expressed in the INS (body) frame. +This log is filled correctly only if the INS receives valid bottom tracking and water tracking DVL data. Bottom and water velocities are used by the Inertial Navigation System Kalman filter to estimate the water current and compute the water speed in the vessel coordinate frame. + +#### Message Format + +``` +$GPVBW,,,,,,,,*54 +$GPVBW,0.312,0.910,A,0.410,0.950,A*55 +``` + +#### Message Structure + +- **Message Name (ID):** `SBG_ECOM_LOG_NMEA_VBW (05)` +- **Compatibility:** INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.0-blue) ![HPINS](https://img.shields.io/badge/HPINS-1.0-blue) + +| Field | Name | Format | Description | +|--------------------|------------------|---------------|-------------------------------------------------------------------------| +| 0 | $##VBW | string | Message ID – VBW frame | +| 1 | longWaterSpeed | fff.fff | Longitudinal water speed, knots (positive forward) | +| 2 | transvWaterSpeed | fff.fff | Transverse water speed, knots (positive right) | +| 3 | waterSpeedValid | char | Status: Water speed, A = Data valid, V = Invalid | +| 4 | longGroundSpeed | fff.fff | Longitudinal ground speed, knots (positive forward) | +| 5 | transvGroundSpeed| fff.fff | Transverse ground speed, knots (positive right) | +| 6 | groundSpeedValid | char | Status: Ground speed, A = Data valid, V = Invalid | +| 7 | Check sum | *cs | XOR of all previous bytes except $ | +| 8 | End of frame | | Carriage return and line feed | + +--- + +### SBG_ECOM_LOG_NMEA_DPT (07) {#SBG_ECOM_LOG_NMEA_DPT} + +The `SBG_ECOM_LOG_NMEA_DPT` message outputs the depth in meters as received by the device from an external pressure sensor. +This value is untouched by the device and is basically a copy of the received data. This log is filled correctly only if valid pressure sensor data are fed to the INS. + +#### Message Format + +``` +$GPDPT,,,*7B +$GPDPT,21.393,,*6F +``` + +#### Message Structure + +- **Message Name (ID):** `SBG_ECOM_LOG_NMEA_DPT (07)` +- **Compatibility:** INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.0-blue) ![HPINS](https://img.shields.io/badge/HPINS-1.0-blue) + +| Field | Name | Format | Description | +|--------------------|------------------|---------------|-------------------------------------------------------------------------| +| 0 | $##DPT | string | Message ID – DPT frame | +| 1 | Depth | fff.fff | Depth below surface in meters (positive down) | +| 2 | Offset | fff.fff | Offset from transducer. Not supported, always empty. | +| 3 | Range Scale | fff.fff | Maximum range scale in use. Not supported, always empty. | +| 4 | Check sum | *cs | XOR of all previous bytes except $ | +| 5 | End of frame | | Carriage return and line feed | + +--- + +### SBG_ECOM_LOG_NMEA_VTG (08) {#SBG_ECOM_LOG_NMEA_VTG} + +The `SBG_ECOM_LOG_NMEA_VTG` message reports the track and velocity over ground as computed by the Kalman filter. +It includes a quality indicator similar to the RMC message. + +#### Message Format + +``` +$GPVTG,,,,,,,,,N*30 +$GPVTG,256.31,T,256.44,M,45.401,N,84.084,K,N*2A +``` + +#### Message Structure + +- **Message Name (ID):** `SBG_ECOM_LOG_NMEA_VTG (08)` +- **Compatibility:** INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.0-blue) ![HPINS](https://img.shields.io/badge/HPINS-1.0-blue) + +| Field | Name | Format | Description | +|-------|----------------|------------|-------------------------------------------------------------------------| +| 0 | $##VTG | string | Message ID – VTG frame | +| 1 | Course True | fff.ff | True course over ground in degrees [0; 360] | +| 2 | T | char | Course over ground is relative to true north | +| 3 | Course Magnetic| fff.ff | Magnetic course over ground in degrees [0; 360] | +| 4 | M | char | Course over ground is relative to magnetic north | +| 5 | Speed Knots | fff.fff | Ground speed in knots | +| 6 | N | char | The speed is expressed in knots | +| 7 | Speed Km/h | fff.fff | Ground speed in kilometers per hour | +| 8 | K | char | The speed is expressed in kilometers per hour | +| 9 | Mode | char | Position mode indicator (see definition in Quality indicators section) | +| 10 | Check sum | *cs | XOR of all previous bytes except $ | +| 11 | End of frame | | Carriage return and line feed | + +> [!NOTE] +> The position mode indicator is computed using the INS position's estimated standard deviation, and is not directly related to the GNSS solution. + +--- + +### SBG_ECOM_LOG_NMEA_ROT (09) {#SBG_ECOM_LOG_NMEA_ROT} + +The `SBG_ECOM_LOG_NMEA_ROT` message outputs the INS rate of turn around the down axis. This is not the IMU gyroscope Z value but rather the true heading derivative. +This log is filled correctly only if the INS has valid roll, pitch, and heading values. + +#### Message Format + +``` +$GPROT,,V*55 +$GPROT,31.61,A*55 +``` + +#### Message Structure + +- **Message Name (ID):** `SBG_ECOM_LOG_NMEA_ROT (09)` +- **Compatibility:** INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.0-blue) ![HPINS](https://img.shields.io/badge/HPINS-1.0-blue) + +| Field | Name | Format | Description | +|--------------------|------------------|---------------|-------------------------------------------------------------------------| +| 0 | $##ROT | string | Message ID – ROT frame | +| 1 | Rate | fff.ff | Down rate of turn in degrees/minute (positive clockwise) or empty if invalid | +| 2 | Status | - | ‘A’ if the rate of turn is valid or ‘V’ if invalid | +| 3 | Check sum | *cs | XOR of all previous bytes except $ | +| 4 | End of frame | | Carriage return and line feed | + +--- + +### SBG_ECOM_LOG_NMEA_GSV (10) {#SBG_ECOM_LOG_NMEA_GSV} + +The `SBG_ECOM_LOG_NMEA_GSV` message reports GNSS satellites in view. For each satellite, information such as elevation, azimuth, and SNR is provided. + +#### Multi-Constellations + +The NMEA talker ID is used to output satellite information for each constellation: + +- **GPGSV:** Lists all visible GPS and SBAS satellites with associated information. +- **GLGSV:** Lists all visible GLONASS satellites with associated information. +- **GAGSV:** Lists all visible Galileo satellites with associated information. +- **GBGSV:** Lists all visible BeiDou satellites with associated information. +- **GIGSV:** Lists all visible NavIC satellites with associated information. +- **GQGSV:** Lists all visible QZSS satellites with associated information. + +> [!NOTE] +> SBG Systems products do not output GNGSV messages to comply with NMEA standards. + +#### Message Re-Assembly + +GSV messages are sent using multiple NMEA frames to comply with the 80-character limit. Each GSV message contains the total number of messages as well as the current message number. Using this information, it is possible to reassemble the GSV message and get all visible satellites for each constellation. + +**Empty Message Example** + +The following message is sent by the INS when there is no GNSS reception: +``` +$GPGSV,1,1,00,,,,*79 +``` + +**Multi-Constellation Example** + +The following example shows a typical GSV reporting when tracking multi-constellation satellites in a clean environment. Each constellation contains multiple GSV messages sent sequentially. An empty line has been intentionally inserted between each constellation for readability; this empty line is not output by the product. + +``` +$GPGSV,5,1,19,09,78,059,50,02,12,323,40,30,12,186,43,03,10,109,39*75 +$GPGSV,5,2,19,20,26,297,41,04,39,060,44,07,38,162,47,11,46,297,47*76 +$GPGSV,5,3,19,06,62,239,49,16,06,066,,26,08,041,,29,01,348,*7D +$GPGSV,5,4,19,19,06,223,,49,34,176,45,61,21,130,38,38,31,204,44*74 +$GPGSV,5,5,19,36,27,143,44,34,33,189,,39,10,112,36*4C + +$GLGSV,3,1,10,67,64,030,51,77,30,105,37,84,18,336,36,66,18,070,45*68 +$GLGSV,3,2,10,76,29,048,48,82,30,210,48,68,46,284,49,83,52,268,40*62 +$GLGSV,3,3,10,69,01,265,,78,01,152,*63 + +$GAGSV,3,1,10,08,70,254,49,26,23,319,40,24,10,035,38,25,13,087,38*6B +$GAGSV,3,2,10,15,33,183,44,07,17,247,41,03,54,063,46,13,59,258,48*6B +$GAGSV,3,3,10,05,03,066,,02,00,130,*6F + +$GBGSV,4,1,15,41,62,074,,23,19,149,43,24,15,038,41,05,13,117,37*62 +$GBGSV,4,2,15,27,19,212,46,29,16,323,42,33,11,088,42,09,19,046,38*61 +$GBGSV,4,3,15,56,22,046,,25,36,097,48,30,33,267,48,32,57,285,50*6C +$GBGSV,4,4,15,31,04,087,,20,05,273,,06,03,043,*55 +``` + +#### Message Structure + +- **Message Name (ID):** `SBG_ECOM_LOG_NMEA_GSV (10)` +- **Compatibility:** GNSS-capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-1.0-blue) ![HPINS](https://img.shields.io/badge/HPINS-1.0-blue) + +| Field | Name | Format | Description | +|-------|-----------------|----------|------------------------------------------------------------------------------------------| +| 0 | $##GSV | string | Message ID – GSV frame | +| 1 | Total Msg Count | d | Total number of messages to reassemble for this constellation (1-9) | +| 2 | Msg Number | d | Current message being transmitted for this constellation (1-9) | +| 3 | Sv Count | dd | Total number of satellites in view for this constellation | +| 4 | PRN/ID | dd | Satellite PRN numbers or ID depending on the constellation | +| 5 | Elevation | dd | Satellite elevation in degrees [-90 to +90°] | +| 6 | Azimuth | ddd | Satellite azimuth in degrees [000 to +359°] | +| 7 | SNR | dd | Signal Noise Ratio (C/No) in dB [00 to 99 dB] - Empty if satellite is not tracked | +| ... | ... | ... | Repetition of PRN, Elevation, Azimuth, and SNR fields for each satellite in this message | +| 8 | Check sum | *cs | XOR of all previous bytes except $ | +| 9 | End of frame | | Carriage return and line feed | + +> [!NOTE] +> If a satellite is tracked but the SNR is unknown, a default SNR of 60 dB is reported. + +--- +## Proprietary NMEA Messages + +The `SBG_ECOM_CLASS_LOG_NMEA_1 (0x03)` message class includes all proprietary NMEA messages and other non-standard messages that utilize NMEA formatting. +These messages primarily provide Kalman filtered data, including navigation, velocity, and attitude information. + +The logs are generated mainly from the AHRS/INS solution, rather than directly from GNSS data. + +### Messages overview + +The following list, provides a quick overview of all available logs for this message class. +It briefly describe which parameters are contained in each output log. + +| Name (MSG ID) | Description | +|--------------------------------------------------------------|---------------------------------------------------------------------------------------------------| +| [SBG_ECOM_LOG_NMEA_1_PSBGI (01)](#SBG_ECOM_LOG_NMEA_1_PSBGI) | SBG Systems proprietary sentence. Contains rotation rates and accelerations. | +| [SBG_ECOM_LOG_NMEA_1_PSBGA (13)](#SBG_ECOM_LOG_NMEA_1_PSBGA) | SBG Systems proprietary sentence. Contains EKF attitude and status. | +| [SBG_ECOM_LOG_NMEA_1_PSBGB (04)](#SBG_ECOM_LOG_NMEA_1_PSBGB) | SBG Systems proprietary sentence. Contains attitude, heading, heave, angular rates, and velocity. | +| [SBG_ECOM_LOG_NMEA_1_PRDID (00)](#SBG_ECOM_LOG_NMEA_1_PRDID) | RDI proprietary sentence. Reports pitch, roll, and heading. | +| [SBG_ECOM_LOG_NMEA_1_PASHR (02)](#SBG_ECOM_LOG_NMEA_1_PASHR) | Proprietary NMEA-like message. Reports roll, pitch, heading, and heave. | +| [SBG_ECOM_LOG_NMEA_1_WASSP (12)](#SBG_ECOM_LOG_NMEA_1_WASSP) | Proprietary NMEA-like message for WASSP. Reports roll, pitch, heading, and heave. | +| [SBG_ECOM_LOG_NMEA_1_PHINF (05)](#SBG_ECOM_LOG_NMEA_1_PHINF) | iXblue NMEA-like log used to output status information. | +| [SBG_ECOM_LOG_NMEA_1_PHTRO (06)](#SBG_ECOM_LOG_NMEA_1_PHTRO) | iXblue NMEA-like log used to output attitude and ship motion. | +| [SBG_ECOM_LOG_NMEA_1_PHLIN (07)](#SBG_ECOM_LOG_NMEA_1_PHLIN) | iXblue NMEA-like log used to output roll and pitch. | +| [SBG_ECOM_LOG_NMEA_1_PHOCT (08)](#SBG_ECOM_LOG_NMEA_1_PHOCT) | iXblue NMEA-like log used to output surge, sway, and heave. | +| [SBG_ECOM_LOG_NMEA_1_INDYN (09)](#SBG_ECOM_LOG_NMEA_1_INDYN) | iXblue NMEA-like log used to output position, heading, attitude, rate, and velocity. | +| [SBG_ECOM_LOG_NMEA_1_GGK (10)](#SBG_ECOM_LOG_NMEA_1_GGK) | Trimble NMEA-like log containing time, latitude, longitude, and ellipsoidal height. | + +--- + +### SBG_ECOM_LOG_NMEA_1_PSBGI (00) {#SBG_ECOM_LOG_NMEA_1_PSBGI} + +The `SBG_ECOM_LOG_NMEA_1_PSBGI` message is an SBG Systems proprietary NMEA log that provides accelerations and rotation rates in the body frame, along with accurate UTC timestamping. + +#### Message Format + +``` +$PSBGI,003944.74,-0.08,0.07,0.00,-0.02,0.06,-9.72,*42 +``` + +#### Message Structure + +- **Message Name (ID):** `SBG_ECOM_LOG_NMEA_1_PSBGI (01)` +- **Compatibility:** All products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-X.X-blue) ![HPINS](https://img.shields.io/badge/HPINS-X.X-blue) + +| Field | Name | Format | Description | +|-------|--------------|--------------|-----------------------------------------------------------------------| +| 0 | $PSBGI | string | Message ID – SBG Systems proprietary NMEA identifier | +| 1 | timeUTC | hhmmss.sss | Current UTC time | +| 2 | gyroX | fff.fff | X Rotation rate in °/s from -999.999 to +999.999 | +| 3 | gyroY | fff.fff | Y Rotation rate in °/s from -999.999 to +999.999 | +| 4 | gyroZ | fff.fff | Z Rotation rate in °/s from -999.999 to +999.999 | +| 5 | accelX | fff.fff | X acceleration in m/s² from -999.999 to +999.999 | +| 6 | accelY | fff.fff | Y acceleration in m/s² from -999.999 to +999.999 | +| 7 | accelZ | fff.fff | Z acceleration in m/s² from -999.999 to +999.999 | +| 8 | Check sum | *cs | XOR of all previous bytes except `$` | +| 9 | End of frame | | Carriage return and line feed | + +> [!NOTE] +> The outputs are direct measurements from the Inertial Measurement Unit (IMU). They are not compensated for sensor bias, earth rotation rate, or gravity. + +--- + +### SBG_ECOM_LOG_NMEA_1_PSBGA (13) {#SBG_ECOM_LOG_NMEA_1_PSBGA} + +The `SBG_ECOM_LOG_NMEA_1_PSBGA` is an SBG Systems proprietary NMEA log that provides UTC time of day, heading, roll, pitch, and associated status and standard deviations. + +#### Message Format + +``` +$PSBGA,155513.685,V,13.684,-63.139,269.130,0.024,0.006,0.196,p,v,v*74 +``` + +#### Message Structure + +- **Message Name (ID):** `SBG_ECOM_LOG_NMEA_1_PSBGA (13)` +- **Compatibility:** INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-X.X-blue) ![HPINS](https://img.shields.io/badge/HPINS-X.X-blue) + +| Field | Name | Format | Description | +|-------|-----------------|------------|---------------------------------------------------------------------------------------------------------------| +| 0 | $PSBGA | string | Message ID – SBG Systems proprietary NMEA identifier | +| 1 | timeUTC | hhmmss.sss | Current UTC time | +| 2 | utcStatus | char | UTC time validity indicator:
‘i’ = Invalid
‘v’ = Valid with known leap second
‘d’ = Unknown leap second
Status is uppercase if UTC is synchronized with a PPS signal. | +| 3 | roll | fff.fff | Roll angle in decimal degrees, positive port side up [-180; 180°]. | +| 4 | pitch | fff.fff | Pitch angle in decimal degrees, positive bow up [-90; 90°]. | +| 5 | heading | fff.fff | True Heading angle in decimal degrees [0; 360°]. | +| 6 | rollStd | fff.fff | Roll standard deviation in decimal degrees [0; 180°]. | +| 7 | pitchStd | fff.fff | Pitch standard deviation in decimal degrees [0; 180°]. | +| 8 | headingStd | fff.fff | True Heading standard deviation in decimal degrees [0; 180°]. | +| 9 | solType | char | EKF smart solution type (see [Smart Solution Type](#smart-solution-type)). | +| 10 | rollPitchStatus | char | Roll and Pitch validity flag:
‘i’ = Invalid (Standard deviation above threshold)
‘v’ = Optimal accuracy | +| 11 | headingStatus | char | Heading validity flag:
‘i’ = Invalid (Standard deviation above threshold)
‘v’ = Optimal accuracy | +| 12 | Check sum | *cs | XOR of all previous bytes except `$`. | +| 13 | End of frame | | Carriage return and line feed | + +#### Smart Solution Type {#smart-solution-type} + +This status indicates the operating mode of the navigation filter and the type of input position or velocity being used. +It also denotes whether the INS is aligned, using lowercase or uppercase characters: + +- **Lowercase**: INS is not yet aligned. +- **Uppercase**: INS is aligned and delivering optimal accuracy. + +| Value | Name | Description | +|-------|-------------------|----------------------------------------------------------------------------------------------| +| a | Uninitialized | EKF filter is not yet initialized. All measurements are invalid. | +| b | Vertical Gyro | EKF filter only computes valid roll and pitch information using the gravity reference. | +| c | AHRS | EKF filter computes roll, pitch, and heading using either magnetometers or dual GNSS. | +| d | Nav Inertial | Full INS solution by integrating accelerations – inertial only. | +| e | Nav Vel. Const | Full INS solution by integrating accelerations with vehicle constraints applied. | +| f | Nav Odometer | Full INS solution by integrating accelerations and odometer velocity. | +| g | Nav ZUPT | Full INS solution with Zero Velocity Updates to limit position drift. | +| h | Nav DVL | Full INS solution by integrating accelerations and DVL velocity. | +| i | Nav GNSS Vel. | Full INS solution using GNSS velocity but not the position. | +| j | Nav USBL | Full INS solution using USBL position. | +| k | Nav GNSS Unkw. | Full INS solution using unknown GNSS position type. | +| l | Nav Single Point | Full INS solution using GNSS single point position type. | +| m | Nav DGPS | Full INS solution using GNSS DGPS position type. | +| n | Nav SBAS | Full INS solution using GNSS SBAS position type. | +| o | Nav RTK Float | Full INS solution using GNSS RTK with float ambiguities position type. | +| p | Nav RTK Fixed | Full INS solution using GNSS RTK with fixed ambiguities position type. | +| q | Nav PPP Float | Full INS solution using GNSS PPP with float ambiguities position type. | +| r | Nav PPP Fixed | Full INS solution using GNSS PPP with fixed ambiguities position type. | + +> [!NOTE] +> If the INS is not fully aligned, the solution type is reported in lowercase. Once aligned, the solution type is reported in uppercase. + +--- + +### SBG_ECOM_LOG_NMEA_1_PSBGB (04) {#SBG_ECOM_LOG_NMEA_1_PSBGB} + +The `SBG_ECOM_LOG_NMEA_1_PSBGB` is an SBG Systems proprietary NMEA log that provides UTC time of day, heading, roll, pitch, heave, angular rates, and body velocities, along with standard deviations and status indicators. + +#### Message Format + +``` +$PSBGB,1,000344.000,0,3.529,-12.821,6.122,0.101,0.098,10.117,0,0,0.004,0.050,2,0.772,0.004,‑0.017,1.043,4.476,0.171,866.025,0,*53 +``` + +#### Message Structure + +- **Message Name (ID):** `SBG_ECOM_LOG_NMEA_1_PSBGB (04)` +- **Compatibility:** INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-X.X-blue) ![HPINS](https://img.shields.io/badge/HPINS-X.X-blue) + +| Field | Name | Format | Description | +|-------|------------------|--------------|-----------------------------------------------------------------------------| +| 0 | $PSBGB | string | Message ID – SBG Systems proprietary NMEA identifier | +| 1 | Version | char | Version of this message – typically `1` | +| 2 | timeUTC | hhmmss.sss | Current UTC time | +| 3 | utcStatus | char | UTC time validity indicator:
‘0’ = Invalid
‘1’ = Valid and PPS synchronized
‘2’ = Valid but no PPS synchronized
‘3’ = Unknown leap second and PPS synchronized
‘4’ = Unknown leap second and no PPS synchronized | +| 4 | roll | fff.fff | Roll angle in decimal degrees, positive port side up [-180; 180°]. | +| 5 | pitch | fff.fff | Pitch angle in decimal degrees, positive bow up [-90; 90°]. | +| 6 | heading | fff.fff | True Heading angle, in decimal degrees [0; 360°]. | +| 7 | rollStd | fff.fff | Roll standard deviation in decimal degrees [0; 180°]. | +| 8 | pitchStd | fff.fff | Pitch standard deviation in decimal degrees [0; 180°]. | +| 9 | headingStd | fff.fff | True Heading standard deviation in decimal degrees [0; 180°]. | +| 10 | rollPitchStatus | char | Roll and Pitch validity flag:
‘0’ = Invalid (Standard deviation above threshold)
‘1’ = Optimal accuracy
‘2’ = Degraded accuracy, alignment in progress | +| 11 | headingStatus | char | Heading validity flag:
‘0’ = Invalid (Standard deviation above threshold)
‘1’ = Optimal accuracy
‘2’ = Degraded accuracy, alignment in progress | +| 12 | heave | ff.fff | Heave, in meters, positive down. | +| 13 | heaveStd | ff.fff | Heave standard deviation in meters. – Fixed to 5 cm | +| 14 | heaveStatus | char | Heave status flag:
‘0’ = Invalid or initializing
‘1’ = Valid and velocity aided
‘2’ = Valid, standalone | +| 15 | rateX | fff.fff | Compensated body X angular rate in °/s, positive port side up. | +| 16 | rateY | fff.fff | Compensated body Y angular rate in °/s, positive bow up. | +| 17 | rateZ | fff.fff | Compensated body Z angular rate in °/s, positive clockwise. | +| 18 | velocityX | fff.fff | Velocity in X body axis in m/s, positive forward. | +| 19 | velocityY | fff.fff | Velocity in Y body axis in m/s, positive starboard. | +| 20 | velocityZ | fff.fff | Velocity in Z body axis in m/s, positive down. | +| 21 | velocityStd | fff.fff | Norm of X, Y, and Z velocity standard deviation in m/s. | +| 22 | velocityStatus | char | Velocity validity flag:
‘0’ = Invalid (Standard deviation above threshold)
‘1’ = Optimal accuracy
‘2’ = Degraded accuracy, alignment in progress | +| 23 | Check sum | *cs | XOR of all previous bytes except `$`. | +| 24 | End of frame | | Carriage return and line feed | + +--- + +### SBG_ECOM_LOG_NMEA_1_PRDID (00) {#SBG_ECOM_LOG_NMEA_1_PRDID} + +The `SBG_ECOM_LOG_NMEA_1_PRDID` message is a Teledyne RDI proprietary message that outputs the vessel's pitch, roll, and true heading angles in degrees. +It uses an NMEA-style formatting. + +#### Message Format + +``` +$PRDID,-012.39,+002.14,366.91*7A +``` + +#### Message Structure + +- **Message Name (ID):** `SBG_ECOM_LOG_NMEA_1_PRDID (00)` +- **Compatibility:** AHRS/INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-X.X-blue) ![HPINS](https://img.shields.io/badge/HPINS-X.X-blue) + +| Field | Name | Format | Description | +|-------|--------------|----------|------------------------------------------------------------------------| +| 0 | $PRDID | string | Message ID – Teledyne RDI proprietary NMEA identifier | +| 1 | Pitch | fff.ff | Signed vessel pitch in degrees, positive bow up. | +| 2 | Roll | fff.ff | Signed vessel roll in degrees, positive port up. | +| 3 | Heading | fff.ff | Vessel true heading in degrees [0 - 360] | +| 4 | Check sum | *cs | XOR of all previous bytes except `$` | +| 5 | End of frame | | Carriage return and line feed | + +--- + +### SBG_ECOM_LOG_NMEA_1_PASHR (02) {#SBG_ECOM_LOG_NMEA_1_PASHR} + +The `SBG_ECOM_LOG_NMEA_1_PASHR` message is a proprietary NMEA log that provides roll, pitch, heading, and heave outputs. + +#### Message Format + +``` +$PASHR,,,T,,,,,,,0,1*21 +$PASHR,123816.80,312.95,T,-000.83,-000.42,-000.01,0.234,0.224,0.298,1,0*09 +``` + +#### Message Structure + +- **Message Name (ID):** `SBG_ECOM_LOG_NMEA_1_PASHR (02)` +- **Compatibility:** AHRS/INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-X.X-blue) ![HPINS](https://img.shields.io/badge/HPINS-X.X-blue) + +| Field | Name | Format | Description | +|-------|------------|------------|-----------------------------------------------------------------------------| +| 0 | $PASHR | string | Message ID – Proprietary NMEA identifier | +| 1 | timeUTC | hhmmss.ss | Current UTC time – empty if invalid | +| 2 | heading | fff.ff | Heading angle, in decimal degrees [0 – 360] – empty if invalid | +| 3 | T | char | True Heading | +| 4 | roll | fff.ff | Roll in decimal degrees. From [-180 – 180°] – empty if invalid | +| 5 | pitch | fff.ff | Pitch in decimal degrees. From [-90 – 90°] – empty if invalid | +| 6 | heave | fff.ff | Heave, in meters, positive down – empty if invalid | +| 7 | rollStd | fff.ff | Roll angle standard deviation in decimal degrees – empty if invalid | +| 8 | pitchStd | fff.ff | Pitch angle standard deviation in decimal degrees – empty if invalid | +| 9 | headingStd | fff.ff | Heading angle standard deviation in decimal degrees – empty if invalid | +| 10 | posStatus | char | Position Quality status
0 = No position
1 = All non-RTK fixed integer positions
2 = RTK fixed integer position | +| 11 | imuStatus | char | IMU & Sensor Status
0 = IMU is working correctly
1 = IMU sensor error | +| 12 | Check sum | *cs | XOR of all previous bytes except `$` | +| 13 | End of frame | | Carriage return and line feed | + +--- + +### SBG_ECOM_LOG_NMEA_1_WASSP (12) {#SBG_ECOM_LOG_NMEA_1_WASSP} + +The `SBG_ECOM_LOG_NMEA_1_WASSP` message is a proprietary NMEA log that provides roll, pitch, heading, and heave outputs. +This message shares the same structure as the `PASHR` message but has slight differences, such as the heave sign being positive up. + +#### Message Format + +``` +$PASHR,,,T,,,,,,,0,1*21 +$PASHR,002258.15,320.99,T,+032.46,-008.15,-012.239,0.454,0.095,1.070,1,0*39 +``` + +#### Message Structure + +- **Message Name (ID):** `SBG_ECOM_LOG_NMEA_1_WASSP (12)` +- **Compatibility:** AHRS/INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-X.X-blue) ![HPINS](https://img.shields.io/badge/HPINS-X.X-blue) + +| Field | Name | Format | Description | +|-------|------------|------------|-----------------------------------------------------------------------------| +| 0 | $PASHR | string | Message ID – Proprietary NMEA identifier | +| 1 | timeUTC | hhmmss.ss | Current UTC time – empty if invalid | +| 2 | heading | fff.ff | Heading angle, in decimal degrees [0 – 360] – empty if invalid | +| 3 | T | char | True Heading | +| 4 | roll | fff.ff | Roll in decimal degrees. From [-180 – 180°] – empty if invalid | +| 5 | pitch | fff.ff | Pitch in decimal degrees. From [-90 – 90°] – empty if invalid | +| 6 | heave | fff.fff | Heave, in meters, positive up – empty if invalid | +| 7 | rollStd | fff.ff | Roll angle standard deviation in decimal degrees – empty if invalid | +| 8 | pitchStd | fff.ff | Pitch angle standard deviation in decimal degrees – empty if invalid | +| 9 | headingStd | fff.ff | Heading angle standard deviation in decimal degrees – empty if invalid | +| 10 | posStatus | char | Position Quality status
0 = No position
1 = All non-RTK fixed integer positions
2 = RTK fixed integer position | +| 11 | imuStatus | char | IMU & Sensor Status
0 = IMU is working correctly
1 = IMU sensor error | +| 12 | Check sum | *cs | XOR of all previous bytes except `$` | +| 13 | End of frame | | Carriage return and line feed | + +--- + +### SBG_ECOM_LOG_NMEA_1_PHINF (05) {#SBG_ECOM_LOG_NMEA_1_PHINF} + +The `SBG_ECOM_LOG_NMEA_1_PHINF` message is an Ixblue proprietary NMEA log that outputs the general system status. + +#### Message Format + +``` +$PHINF,08030027*7B +``` + +#### Message Structure + +- **Message Name (ID):** `SBG_ECOM_LOG_NMEA_1_PHINF (05)` +- **Compatibility:** AHRS/INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-X.X-blue) ![HPINS](https://img.shields.io/badge/HPINS-X.X-blue) + +| Field | Name | Format | Description | +|-------|--------------|----------|----------------------------------------------------------------| +| 0 | $PHINF | string | Message ID – Ixblue proprietary NMEA identifier | +| 1 | Status | hhhhhhhh | Hexadecimal value representing the INS status | +| 2 | Check sum | *cs | XOR of all previous bytes except `$` | +| 3 | End of frame | | Carriage return and line feed | + +**STATUS Definition:** + +The `Status` field is a 32-bit word that acts as a built-in test and control system status indicator. +It monitors different modes and sub-part statuses of the system in real-time, setting specific flags as needed. + +Each flag is a bit set to “1” when the status is active (ON) and “0” when inactive (OFF). + +| Bit | Name | Description | +|-----|----------------------------------|------------------------------------------------------------------| +| 0 | IXBLUE_STAT_HEADING_UNVALID | Set when the provided heading is invalid or still converging | +| 1 | IXBLUE_STAT_ROLL_UNVALID | Set when the provided roll is invalid or still converging | +| 2 | IXBLUE_STAT_PITCH_UNVALID | Set when the provided pitch is invalid or still converging | +| 3 | IXBLUE_STAT_HEAVE_INIT | Set when the heave filter is in initialization phase | +| 4 | Reserved | Reserved, not used | +| 5 | IXBLUE_STAT_ALIGNMENT | Set during the alignment phase | +| 6 | IXBLUE_STAT_CONFIG_SAVED | Not implemented | +| 7 | IXBLUE_STAT_COMPUTATION_OVERLOAD | Set when the CPU is overloaded | +| 8 | IXBLUE_STAT_FOG_X1_ANOMALY | Set when the gyroscope X built-in test has failed | +| 9 | IXBLUE_STAT_FOG_X2_ANOMALY | Set when the gyroscope Y built-in test has failed | +| 10 | IXBLUE_STAT_FOG_X3_ANOMALY | Set when the gyroscope Z built-in test has failed | +| 11 | IXBLUE_STAT_FOG_ACQ_ERROR | Set when at least one gyroscope is out of range | +| 12 | IXBLUE_STAT_ACC_X1_ANOMALY | Set when the accelerometer X built-in test has failed | +| 13 | IXBLUE_STAT_ACC_X2_ANOMALY | Set when the accelerometer Y built-in test has failed | +| 14 | IXBLUE_STAT_ACC_X3_ANOMALY | Set when the accelerometer Z built-in test has failed | +| 15 | IXBLUE_STAT_SENSOR_ERROR | Set when at least one sensor is failing or out of range | +| 16 | IXBLUE_STAT_SERIAL_IN_A_ERROR | Set when errors are detected on serial port A RX | +| 17 | IXBLUE_STAT_SERIAL_IN_B_ERROR | Set when errors are detected on serial port B RX | +| 18 | IXBLUE_STAT_SERIAL_IN_C_ERROR | Set when errors are detected on serial port C RX | +| 19 | IXBLUE_STAT_OUTPUT_OVERLOADED | Set when at least one serial output is overloaded | +| 20 | IXBLUE_STAT_SERIAL_OUT_A_FULL | Set when the serial output A is overloaded | +| 21 | IXBLUE_STAT_SERIAL_OUT_B_FULL | Set when the serial output B is overloaded | +| 22 | IXBLUE_STAT_SERIAL_OUT_C_FULL | Set when the serial output C is overloaded | +| 23 | IXBLUE_STAT_SERIAL_OUT_D_FULL | Not implemented | +| 24 | IXBLUE_STAT_MANUAL_LOG_USED | Not implemented | +| 25 | IXBLUE_STAT_MANUAL_LAT_USED | Not implemented | +| 27 | IXBLUE_STAT_HRP_INVALID | Set if at least one roll, pitch, or heading component is invalid | +| 31 | IXBLUE_STAT_RESTART_SYSTEM | Not implemented | + +--- + +### SBG_ECOM_LOG_NMEA_1_PHTRO (06) {#SBG_ECOM_LOG_NMEA_1_PHTRO} + +The `SBG_ECOM_LOG_NMEA_1_PHTRO` message is an Ixblue proprietary NMEA log that outputs the unit's pitch and roll angles. + +#### Message Format + +``` +$PHTRO,0.03,P,0.22,T*56 +``` + +#### Message Structure + +- **Message Name (ID):** `SBG_ECOM_LOG_NMEA_1_PHTRO (06)` +- **Compatibility:** AHRS/INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-X.X-blue) ![HPINS](https://img.shields.io/badge/HPINS-X.X-blue) + +| Field | Name | Format | Description | +|-------|--------------|----------|----------------------------------------------------------------------| +| 0 | $PHTRO | string | Message ID – Ixblue proprietary NMEA identifier | +| 1 | Pitch | x.xx | Pitch angle in degrees | +| 2 | Pitch sign | char | 'M' for bow up and 'P' for bow down | +| 3 | Roll | y.yy | Roll angle in degrees | +| 4 | Roll sign | char | 'B' for port down and 'T' for port up | +| 5 | Check sum | *cs | XOR of all previous bytes except `$` | +| 6 | End of frame | | Carriage return and line feed | + +--- + +### SBG_ECOM_LOG_NMEA_1_PHLIN (07) {#SBG_ECOM_LOG_NMEA_1_PHLIN} + +The `SBG_ECOM_LOG_NMEA_1_PHLIN` message is an Ixblue proprietary NMEA log that outputs surge, sway, and heave data. + +#### Message Format + +``` +$PHLIN,0.03,0.22,-0.15*68 +``` + +#### Message Structure + +- **Message Name (ID):** `SBG_ECOM_LOG_NMEA_1_PHLIN (07)` +- **Compatibility:** Heave capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-X.X-blue) ![HPINS](https://img.shields.io/badge/HPINS-X.X-blue) + +| Field | Name | Format | Description | +|-------|--------------|----------|---------------------------------------------------------------------------------------------------| +| 0 | $PHLIN | string | Message ID – Ixblue proprietary NMEA identifier | +| 1 | Surge | x.xxx | Signed surge in meters (positive forward) | +| 2 | Sway | y.yyy | Signed sway in meters (positive left) | +| 3 | Heave | z.zzz | Signed heave in meters (positive up) | +| 4 | Check sum | *cs | XOR of all previous bytes except `$` | +| 5 | End of frame | | Carriage return and line feed | + +> [!WARNING] +> Ixblue uses different conventions for ship motion measurements. In this frame, both sway and heave values are reversed compared to SBG Systems conventions. + +--- + +### SBG_ECOM_LOG_NMEA_1_PHOCT (08) {#SBG_ECOM_LOG_NMEA_1_PHOCT} + +The `SBG_ECOM_LOG_NMEA_1_PHOCT` message is an Ixblue proprietary NMEA log that outputs time, attitude, heading, and ship motion data. + +#### Message Format + +``` +$PHOCT,01,000201.000,E,00,356.592,E,+000.225,E,+00.039,E,+00.023,T,+00.023,+00.016,+00.003,+00.002,-00.001,+00.000,+0001.96*04 +``` + +#### Message Structure + +- **Message Name (ID):** `SBG_ECOM_LOG_NMEA_1_PHOCT (08)` +- **Compatibility:** Heave capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-X.X-blue) ![HPINS](https://img.shields.io/badge/HPINS-X.X-blue) + +| Field | Name | Format | Description | +|-------|----------------|-----------|------------------------------------------------------------------------------------------| +| 0 | $PHOCT | string | Message ID – Ixblue proprietary NMEA identifier | +| 1 | Protocol Ver. | 01 | Protocol version identifier | +| 2 | Time | hhmmss.ss | UTC Time, current time | +| 3 | UTC Status | G | UTC Time status: 'T'=Valid 'E'=Invalid | +| 4 | Latency | AA | INS latency for heading, roll, pitch (Not implemented, always set to 0) | +| 5 | True Heading | HHH.HHH | True heading in degrees (000.000 to 359.999) | +| 6 | Heading Status | N | True Heading status: 'T'=Valid; 'E'=Invalid; 'I'=Initializing | +| 7 | Roll Angle | RRR.RRR | Roll in degrees (positive if port side up); -180.000 to +180.000 | +| 8 | Roll Status | L | Roll status: 'T'=Valid 'E'=Invalid 'I'=Initializing | +| 9 | Pitch Angle | PP.PPP | Pitch in degrees (positive if bow down); -90.000 to +90.000 | +| 10 | Pitch Status | K | Pitch status: 'T'=Valid 'E'=Invalid 'I'=Initializing | +| 11 | Primary Heave | FF.FFF | Heave at Primary Lever arm in meters (positive up); -99.999 to +99.999 | +| 12 | Heave Status | M | Heave status (also used for surge, sway & speed): 'T'=Valid 'E'=Invalid 'I'=Initializing | +| 13 | Heave | HH.HHH | Heave at desired lever arm in meters (positive up); -99.999 to +99.999 | +| 14 | Surge | SS.SSS | Surge with Lever arms applied in meters (positive forward); -99.999 to +99.999 | +| 15 | Sway | WW.WWW | Sway at desired lever arm in meters (positive left); -99.999 to +99.999 | +| 16 | Heave Speed | ZZ.ZZZ | Heave speed at desired lever arm in m/s (positive up); -99.999 to +99.999 | +| 17 | Surge Speed | YY.YYY | Surge speed at desired lever arm in m/s (positive forward); -99.999 to +99.999 | +| 18 | Sway Speed | XX.XXX | Sway speed at desired lever arm in m/s (positive left); -99.999 to +99.999 | +| 19 | Heading Rate | QQQQ.QQ | Heading rate of turn in °/min (positive clockwise); -9999.99 to +9999.99 | +| 20 | Check sum | *cs | XOR of all previous bytes except `$` | +| 21 | End of frame | | Carriage return and line feed | + +> [!WARNING] +> Ixblue uses different conventions for ship motion measurements. In this frame, both sway and heave values are reversed compared to SBG Systems conventions. + +--- + +### SBG_ECOM_LOG_NMEA_1_INDYN (09) {#SBG_ECOM_LOG_NMEA_1_INDYN} + +The `SBG_ECOM_LOG_NMEA_1_INDYN` message is an Ixblue proprietary NMEA log that outputs position, heading, attitude, attitude rate, and speed data. + +#### Message Format + +``` +$INDYN,48.87949927,1.99962275,0.000,218.714,-0.909,0.291,-0.011,-0.073,-0.024,0.019*6A +``` + +#### Message Structure + +- **Message Name (ID):** `SBG_ECOM_LOG_NMEA_1_INDYN (09)` +- **Compatibility:** INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-X.X-blue) ![HPINS](https://img.shields.io/badge/HPINS-X.X-blue) + +| Field | Name | Format | Description | +|-------|--------------|---------------|-----------------------------------------------------------------------------| +| 0 | $INDYN | string | Message ID – Ixblue proprietary NMEA identifier | +| 1 | Latitude | x.xxxxxxxx | INS latitude in degrees with 8 decimals | +| 2 | Longitude | y.yyyyyyyy | INS longitude in degrees with 8 decimals | +| 3 | Altitude | z.zzz | INS altitude above Mean Sea Level in meters, positive upward | +| 4 | True Heading | h.hhh | True heading in degrees (0.000 to 359.999) | +| 5 | Roll Angle | r.rrr | Roll angle in degrees, positive for port side up (-180.000 to +180.000) | +| 6 | Pitch Angle | p.ppp | Pitch angle in degrees, positive when bow down (-90.000 to +90.000) | +| 7 | Heading Rate | a.aaa | Heading rate in °/s (positive when heading increases) | +| 8 | Roll Rate | b.bbb | Roll rate in °/s (positive when roll increases) | +| 9 | Pitch Rate | c.ccc | Pitch rate in °/s (positive when pitch increases) | +| 10 | Ground Speed | s.sss | Horizon speed in m/s (positive toward the bow) | +| 11 | Check sum | *cs | XOR of all previous bytes except `$` | +| 12 | End of frame | | Carriage return and line feed | + +> [!NOTE] +> The roll, pitch, and heading rates are not the same as the gyroscope values. Gyroscope values are expressed in the body frame (X, Y, Z) and are not unbiased. + +> [!WARNING] +> In this message, Ixblue uses a different convention for orientation. The pitch angle is reversed compared to SBG Systems conventions. + +--- + +### SBG_ECOM_LOG_NMEA_1_GGK (10) {#SBG_ECOM_LOG_NMEA_1_GGK} + +The `SBG_ECOM_LOG_NMEA_1_GGK` message is a Trimble proprietary NMEA log that outputs time, position, and GNSS-related quality indicators. + +#### Message Format + +``` +$PTNL,GGK,,,,,,,0,00,,,M*30 +$PTNL,GGK,161159.00,013020,4854.61758182,N,00210.08881241,E,1,07,8.3,EHT140.509,M*75 +``` + +#### Message Structure + +- **Message Name (ID):** `SBG_ECOM_LOG_NMEA_1_GGK (10)` +- **Compatibility:** INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-X.X-blue) ![HPINS](https://img.shields.io/badge/HPINS-X.X-blue) + +| Field | Name | Format | Description | +|-------|--------------|----------------|----------------------------------------------------------------------------------| +| 0 | $PTNL | string | Talker ID – Trimble proprietary NMEA identifier, always `PTNL` | +| 1 | GGK | string | Message ID – Trimble proprietary NMEA identifier, always `GGK` | +| 2 | Time | hhmmss.ss | UTC time of solution, hours must be two chars | +| 3 | Date | mmddyy | UTC date of solution, month must be two chars | +| 4 | Latitude | ddmm.mmmmmmmm | INS latitude in degrees (0 to 90) and decimal minutes (8 decimal digits) | +| 5 | N/S | char | ‘N’ for Northern hemisphere latitude, ‘S’ for Southern hemisphere latitude | +| 6 | Longitude | dddmm.mmmmmmmm | INS longitude in degrees (0 to 180) and decimal minutes (8 decimal digits) | +| 7 | E/W | char | ‘E’ for Eastern longitude, ‘W’ for Western longitude | +| 8 | Pos. Quality | char | Position quality indicator using INS standard deviation (see Quality indicators) | +| 9 | SV used | xx | Number of satellites used in the solution (latest received GNSS fix) | +| 10 | HDOP | z.z | Horizontal Dilution of Precision (1 decimal digit) (see Quality indicators) | +| 11 | Height | EHTx.xxx | Height above ellipsoid with 3 decimal digits and must start with `EHT` | +| 12 | Height Unit | M | Always `M`: Height above ellipsoid measured in meters | +| 13 | Check sum | *cs | XOR of all previous bytes except `$` | +| 14 | End of frame | | Carriage return and line feed | + +> [!NOTE] +> The position quality and DOP are computed using the INS position's estimated standard deviation and are not directly related to the GNSS solution. + +--- diff --git a/doc/thirdPartyMessages.md b/doc/thirdPartyMessages.md new file mode 100644 index 0000000..32dbbb5 --- /dev/null +++ b/doc/thirdPartyMessages.md @@ -0,0 +1,581 @@ +# Third Party Binary Messages + +The `SBG_ECOM_CLASS_LOG_THIRD_PARTY_0 (0x04)` message class consolidates a variety of widely recognized binary outputs from leading third-party hardware and software providers. +By supporting protocols from industry leaders such as Teledyne, Kongsberg, Cobham, KVH, and Crossbow, SBG Systems' inertial navigation systems ensure seamless integration and broad compatibility with various external equipment. + +This support allows for straightforward data exchange and system interoperability, making it easier for users to incorporate SBG Systems' INS products into complex multi-vendor environments. + +## Messages overview + +The following list, provides a quick overview of all available logs for this message class. +It briefly describe which parameters are contained in each output log. + +| Name (MSG ID) | Description | +|------------------------------------------------------------------------------|--------------------------------------------------------------------------------| +| [SBG_ECOM_THIRD_PARTY_TSS1 (00)](#SBG_ECOM_THIRD_PARTY_TSS1) | Latitude, Longitude, Altitude, Quality indicator | +| [SBG_ECOM_THIRD_PARTY_KVH (01)](#SBG_ECOM_THIRD_PARTY_KVH) | KVH Roll, Pitch, and Heading output | +| [SBG_ECOM_THIRD_PARTY_PD0 (02)](#SBG_ECOM_THIRD_PARTY_PD0) | Teledyne RDI PD0 message | +| [SBG_ECOM_THIRD_PARTY_SIMRAD_1000 (03)](#SBG_ECOM_THIRD_PARTY_SIMRAD_1000) | Konsberg EM1000 proprietary frame that outputs Roll, Pitch, and Heading | +| [SBG_ECOM_THIRD_PARTY_SIMRAD_3000 (04)](#SBG_ECOM_THIRD_PARTY_SIMRAD_3000) | Konsberg EM3000 proprietary frame that outputs Roll, Pitch, and Heading | +| [SBG_ECOM_THIRD_PARTY_SEAPATH_B26 (05)](#SBG_ECOM_THIRD_PARTY_SEAPATH_B26) | Konsberg Seapath Binary Log 26 used for MBES FM mode | +| [SBG_ECOM_THIRD_PARTY_DOLOG_HRP (06)](#SBG_ECOM_THIRD_PARTY_DOLOG_HRP) | DOLOG Heading, Roll, Pitch, and HRP rates | +| [SBG_ECOM_THIRD_PARTY_AHRS_500 (07)](#SBG_ECOM_THIRD_PARTY_AHRS_500) | Crossbow AHRS-500 attitude, rate, acceleration, and status | +| [SBG_ECOM_THIRD_PARTY_ADA_01 (08)](#SBG_ECOM_THIRD_PARTY_ADA_01) | ADA-specific Data Packet with IMU/INS/Status data | +| [SBG_ECOM_THIRD_PARTY_AT_ITINS (09)](#SBG_ECOM_THIRD_PARTY_AT_ITINS) | Cobham AVIATOR UAV 200 AT command | +| [SBG_ECOM_THIRD_PARTY_KONGSBERG_MB (10)](#SBG_ECOM_THIRD_PARTY_KONGSBERG_MB) | Kongsberg multi-beam binary aiding log | + +--- + +## SBG_ECOM_THIRD_PARTY_TSS1 (00) {#SBG_ECOM_THIRD_PARTY_TSS1} + +The `SBG_ECOM_THIRD_PARTY_TSS1` message is a proprietary log specifically designed for marine survey applications, providing heave, roll, pitch, sway, and vertical accelerations data. + +The log data is deported to the output lever arms configured for each output interface, ensuring precise measurements at specific points on the vessel. +With the capability to output multiple TSS1 frames across various interfaces, it allows seamless data integration for equipment located in different positions on a vessel. + +### Message Format + +``` +:XXAAAASMHHHHQMRRRRSMPPPP +``` + +### Message Structure + +- **Message Name (ID):** `SBG_ECOM_THIRD_PARTY_TSS1 (00)` +- **Compatibility:** Heave capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-X.X-blue) ![HPINS](https://img.shields.io/badge/HPINS-X.X-blue) + +| Field | Name | Description | +|------------|-----------------------|--------------------------------------------------------------------------------------------------------------------------| +| `:` | Start character | Start of the TSS1 message frame. | +| `XX` | Sway acceleration | Hex value of sway acceleration in 3.835 cm/s², with a range from 0 to 9.81 m/s². | +| `AAAA` | Vertical acceleration | Hex value (2’s complement) of vertical acceleration in 0.0625 cm/s², with a range of –20.48 to +20.48 m/s². | +| `S` | Separator character | Delimiter space character. | +| `M` | Sign of heave | Space if positive, minus if negative. | +| `HHHH` | Heave measurement | ASCII value of heave in centimeters, with a range of –99.99 to +99.99 meters. | +| `Q` | Status flag | See [TSS1 Status Flags](#tss1-status-flags) for details. | +| `M` | Sign of roll | Space if positive, minus if negative. | +| `RRRR` | Roll | Roll in units of 0.01 degrees (e.g., 1000 = 10°), with a range of –99.99° to +99.99°. | +| `S` | Separator character | Delimiter space character. | +| `M` | Sign of pitch | Space if positive, minus if negative. | +| `PPPP` | Pitch | Pitch in units of 0.01 degrees (e.g., 1000 = 10°), with a range of –99.99° to +99.99°. | +| `` | End of frame | Carriage return and line feed, marking the end of the frame. | + +### TSS1 Status Flags {#tss1-status-flags} + +The status flag character identifies the specific algorithms used to compute the heave data. +For instance, it indicates whether the heave computation uses the INS velocity and rate of turn to greatly improve the accuracy of heave measurements during vessel maneuvers. + +| Value | Description | +|-------|----------------------------------------------------------------------------------------------| +| `U` | Unaided mode and stable measurements. | +| `u` | Unaided mode but unstable heave data. | +| `G` | Velocity-aided mode and stable measurements. | +| `g` | Velocity-aided mode but unstable data. | +| `H` | Heading-aided mode and stable measurements. | +| `h` | Heading-aided mode but unstable data. | +| `F` | Both velocity and heading-aided mode and stable measurements. | +| `f` | Both velocity and heading-aided mode but unstable measurements. | + +### Message Example + +``` +:1A4770 -0016H 0429 -0680 +``` + +Please find below a detailed breakdown and explanation of each field: + +| Field | Value | Explanation | +|--------|:-------:|----------------------------------------------------------------------------------------------------------------------------| +| `XX` | 1A | Sway acceleration, which is 0.9971 m/s²
*0x1A (hex) = 26 (decimal), multiplied by 0.03835 m/s² yields 0.9971 m/s²* | +| `AAAA` | 4770 | Heave acceleration, which is 11.43 m/s²
*0x4770 (hex) = 18288 (decimal), multiplied by 0.000625 m/s² yields 11.43 m/s²* | +| `S` | (space) | Delimiter space character. | +| `M` | (minus) | Indicates the heave value is negative. | +| `HHHH` | 0016 | Heave value, which is -16 cm. | +| `Q` | H | Status flag indicating stable heading-aided mode. | +| `M` | (space) | Indicates the roll value is positive. | +| `RRRR` | 0429 | Roll, which is 4.29°. | +| `S` | (space) | Delimiter space character. | +| `M` | (minus) | Indicates the pitch value is negative. | +| `PPPP` | 0680 | Pitch, which is -6.80°. | + +> [!WARNING] +> The TSS1 frame uses different conventions for heave measurements. In this frame, sway is expressed as positive left, and heave is positive up. + +--- + +## SBG_ECOM_THIRD_PARTY_KVH (01) {#SBG_ECOM_THIRD_PARTY_KVH} + +The `SBG_ECOM_THIRD_PARTY_KVH` message is a basic proprietary log that provides pitch, roll, heading, and heading rate data. +This format is straightforward, making it easy to integrate with systems requiring these essential orientation parameters. + +### Message Format +``` +%pitch,roll,heading +``` + +### Message Structure + +- **Message Name (ID):** `SBG_ECOM_THIRD_PARTY_KVH (01)` +- **Compatibility:** AHRS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-X.X-blue) + +| Field | Name | Format | Description | +|-------|--------------|----------|---------------------------------------------------------------------------------------------------------| +| 0 | % | string | Sentence Identifier | +| 1 | pitch | ddd | Pitch angle in tenths of degrees. Divide by 10 to get the pitch in degrees. Positive when bow up. | +| 2 | roll | ddd | Roll angle in tenths of degrees. Divide by 10 to get the roll in degrees. Positive when port side down. | +| 3 | heading | ddd | Heading in tenths of degrees. Values range from 0 to 3600. | +| 4 | End of frame | | End of frame: carriage return and line feed. | + +### Example Frame +``` +%10,-5,3489 +``` + +This example shows a frame where the pitch is 1.0°, the roll is -0.5° (port side down), and the heading is 348.9°. + +> [!WARNING] +> Roll sign is opposite sign of the usual SBG Systems convention. + +## SBG_ECOM_THIRD_PARTY_PD0 (02) {#SBG_ECOM_THIRD_PARTY_PD0} + +The `SBG_ECOM_THIRD_PARTY_PD0` message is a proprietary binary log format from Teledyne RDI, primarily used for outputting Doppler Velocity Log (DVL) data, including bottom tracking, water tracking, and water profiling information. + +This log can be generated by the Inertial Navigation System (INS) only if valid PD0 frames are received from a connected DVL device. +The INS does not modify or supplement the PD0 data; it simply forwards the original PD0 frame as received. +This feature is particularly useful for recording DVL data in the internal datalogger or for real-time output to be processed by third-party software applications. + +For detailed information on the PD0 frame structure and its contents, please refer to the Teledyne RDI documentation. + +## SBG_ECOM_THIRD_PARTY_SIMRAD_1000 (03) {#SBG_ECOM_THIRD_PARTY_SIMRAD_1000} + +The `SBG_ECOM_THIRD_PARTY_SIMRAD_1000` message is a proprietary binary log from Kongsberg, primarily used for transmitting attitude and heave data to echo sounders. +This logs provide roll, pitch, heading, and heave measurements, essential for marine survey applications. + +All fields in this log are stored in Little Endian format (LSB first), with signed fields represented using standard 2's complement. + +The log data is deported to the output lever arms configured for each output interface, ensuring precise measurements at specific points on the vessel. +With the capability to output multiple SIMRAD frames across various interfaces, it allows seamless data integration for equipment located in different positions on a vessel. + +### Message Structure + +- **Message Name (ID):** `SBG_ECOM_THIRD_PARTY_SIMRAD_1000 (03)` +- **Compatibility:** Heave capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-X.X-blue) ![HPINS](https://img.shields.io/badge/HPINS-X.X-blue) +- **Total Size:** 10 bytes + +| Field | Description | Unit | Format | Size | Offset | +|-------------|--------------------------------------------------------|--------|--------|------|--------| +| Sync Byte 1 | Sync Byte 1 = 0x00 | - | uint8 | 1 | 0 | +| Sync Byte 2 | Sync Byte 2 = 0x90 | - | uint8 | 1 | 1 | +| Roll | Roll, positive with port side up, ±179.99° | 0.01° | int16 | 2 | 2 | +| Pitch | Pitch, positive with bow up, ±89.99° | 0.01° | int16 | 2 | 4 | +| Heave | Heave, positive up, ±99.99 meters | cm | int16 | 2 | 6 | +| Heading | Heading, positive clockwise, [0 to 359.99°] | 0.01° | int16 | 2 | 8 | + +> [!WARNING] +> The Simrad 1000 message uses a different convention from SBG Systems' standard. In this frame, the heave measurement is positive upward, which is the opposite of the SBG Systems' convention. + + +## SBG_ECOM_THIRD_PARTY_SIMRAD_3000 (04) {#SBG_ECOM_THIRD_PARTY_SIMRAD_3000} + +The `SBG_ECOM_THIRD_PARTY_SIMRAD_3000` message is similar to the [SBG_ECOM_THIRD_PARTY_SIMRAD_1000](#SBG_ECOM_THIRD_PARTY_SIMRAD_1000) message. +The only difference is the first field that provide the heave/INS status. + +Please refer to [SBG_ECOM_THIRD_PARTY_SIMRAD_1000](#SBG_ECOM_THIRD_PARTY_SIMRAD_1000) message for more information. + +### Message Structure + +- **Message Name (ID):** `SBG_ECOM_THIRD_PARTY_SIMRAD_3000 (04)` +- **Compatibility:** Heave capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-X.X-blue) ![HPINS](https://img.shields.io/badge/HPINS-X.X-blue) +- **Total Size:** 10 bytes + +| Field | Description | Unit | Format | Size | Offset | +|-------------|-----------------------------------------------------------------------|--------|--------|------|--------| +| Status | Solution status (see [Simrad 3000 Status](#simrad-3000-status-flags)) | - | uint8 | 1 | 0 | +| Sync Byte 2 | Sync Byte 2 = 0x90 | - | uint8 | 1 | 1 | +| Roll | Roll, positive with port side up, ±179.99° | 0.01° | int16 | 2 | 2 | +| Pitch | Pitch, positive with bow up, ±89.99° | 0.01° | int16 | 2 | 4 | +| Heave | Heave, positive up, ±99.99 meters | cm | int16 | 2 | 6 | +| Heading | Heading, positive clockwise, [0 to 359.99°] | 0.01° | int16 | 2 | 8 | + +> [!WARNING] +> The Simrad 3000 message uses a different convention from SBG Systems' standard. In this frame, the heave measurement is positive upward, which is the opposite of the SBG Systems' convention. + +### Simrad 3000 Status Flags {#simrad-3000-status-flags} + +The status flag in the Simrad 3000 protocol is used to indicate the quality and validity of the orientation and heave data. + +| Value | Description | +|-------|----------------------------------------------------------------------------| +| 0x90 | Valid measurements with full accuracy | +| 0x91 | Valid measurements with reduced accuracy (unaided mode) | +| 0x9A | Invalid measurements; device is aligning, or the heave filter is unsettled | +| 0xA0 | Error reported in the motion sensor | + +## SBG_ECOM_THIRD_PARTY_SEAPATH_B26 (05) {#SBG_ECOM_THIRD_PARTY_SEAPATH_B26} + +The `SBG_ECOM_THIRD_PARTY_SEAPATH_B26` message is a proprietary binary log from Kongsberg Seapath. +It provides marine related data including INS position, velocity, attitude, heave, and rotation rates. + +This log is primarily used with Kongsberg MBES systems to support the FM mode, facilitating precise marine survey operations. + +All fields in this log are stored in Big Endian format (MSB first). Signed data uses the 2's complement representation. + +### Message Structure + +- **Message Name (ID):** `SBG_ECOM_THIRD_PARTY_SEAPATH_B26 (05)` +- **Compatibility:** INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-X.X-blue) ![HPINS](https://img.shields.io/badge/HPINS-X.X-blue) +- **Total Size:** 52 bytes + +| Field | Description | Unit | Format | Size | Offset | +|-------------------------------------|------------------------------------------------------------------------|---------------|--------|------|--------| +| Header 1 | First header value, always 0xAA | - | uint8 | 1 | 0 | +| Header 2 | Second header value, always 0x55 | - | uint8 | 1 | 1 | +| Time, Seconds | UTC Time in seconds that timestamps the INS data | Seconds | int32 | 4 | 2 | +| Time, Fraction of Sec. | UTC Time fraction of a second that timestamps the INS data | 0.0001 sec | uint16 | 2 | 6 | +| Latitude | INS latitude, positive North of the Equator | 2^30 = 90° | int32 | 4 | 8 | +| Longitude | INS longitude, positive East of Greenwich | 2^30 = 90° | int32 | 4 | 12 | +| Height | INS altitude above ellipsoid | Centimeters | int32 | 4 | 16 | +| Real-time Heave | Heave, positive down | Centimeters | int16 | 2 | 20 | +| North Velocity | INS North velocity, positive North | cm/s | int16 | 2 | 22 | +| East Velocity | INS East velocity, positive East | cm/s | int16 | 2 | 24 | +| Down Velocity | INS Down velocity, positive Down | cm/s | int16 | 2 | 26 | +| Roll | Vessel roll angle, positive with port side up | 2^14 = 90° | int16 | 2 | 28 | +| Pitch | Vessel pitch angle, positive with bow up | 2^14 = 90° | int16 | 2 | 30 | +| Heading | Vessel true heading, positive clockwise [0 to 359.99°] | 2^14 = 90° | uint16 | 2 | 32 | +| Vessel X Rate | Vessel body X rotation rate, positive with port side up | 2^14 = 90 °/s | int16 | 2 | 34 | +| Vessel Y Rate | Vessel body Y rotation rate, positive with bow up | 2^14 = 90 °/s | int16 | 2 | 36 | +| Vessel Z Rate | Vessel body Z rotation rate, positive clockwise | 2^14 = 90 °/s | int16 | 2 | 38 | +| Delayed Heave Time, Seconds | UTC Time in seconds that timestamps the delayed heave information | Seconds | int32 | 4 | 40 | +| Delayed Heave Time, Fraction of Sec.| UTC Time fraction of a second that timestamps the delayed heave info | 0.0001 sec | uint16 | 2 | 44 | +| Delayed Heave | Delayed heave, positive down | Centimeters | int16 | 2 | 46 | +| Status Word | Device status bitmask (see [Status Word](#seapath-b26-status)) | - | uint16 | 2 | 48 | +| Checksum | CRC of all bytes excluding header and checksum | - | uint16 | 2 | 50 | + +> [!WARNING] +> The Seapath Binary 26 message uses a different convention for heave measurements, where heave is positive downward. + +### Time Format +The UTC time is divided into an integer seconds part and a fractional seconds part. +The integer part is UNIX time counted from 1970-01-01 UTC. + +### Checksum + +Checksum is calculated as a 16-bit Block Cyclic Redundancy Check of all bytes except the header and checksum. +The CRC algorithm uses the polynomial 0x8408 with an initial value of 0xFFFF. + +### Seapath Binary 26 Status Word {#seapath-b26-status} + +The Status Word consists of 16 single-bit flags, numbered from 0 to 15, where position 0 is the least significant bit. +If a bit is set (1), it indicates: + +| Bit Position | Description | +|--------------|----------------------------------------------------------------------------------------------| +| 0 | Reduced horizontal position and velocity performance. Data is valid, but INS is not aligned. | +| 1 | Invalid horizontal position and velocity data as per user-defined quality thresholds. | +| 2 | Reduced heave and vertical velocity performance. Heave is valid but not aided by INS. | +| 3 | Invalid heave and vertical velocity data. The heave filter is not converged or has diverged. | +| 4 | Reduced roll and pitch performance. Data is valid, but the INS is not aligned. | +| 5 | Invalid roll and pitch data as per user-defined quality thresholds. | +| 6 | Reduced heading performance. Heading is valid, but the INS is not aligned. | +| 7 | Invalid heading data as per user-defined quality thresholds. | +| 8 | Invalid delayed heave data. The delayed heave filter has not converged. | + +> [!NOTE] +> Validity thresholds for each parameter can be configured by the user in the device settings. + +## SBG_ECOM_THIRD_PARTY_DOLOG_HRP (06) {#SBG_ECOM_THIRD_PARTY_DOLOG_HRP} + +The `SBG_ECOM_THIRD_PARTY_DOLOG_HRP` message is a proprietary binary log that provides Euler angles (roll, pitch, heading) and vehicle/body rotation rates. +This message is commonly used in stabilization systems where high-throughput attitude and rotational measurements are essential. + +All fields in this log are stored in Big Endian format (MSB first), and signed data uses the 2's complement representation. + +### Message Structure + +- **Message Name (ID):** `SBG_ECOM_THIRD_PARTY_DOLOG_HRP (06)` +- **Compatibility:** AHRS/INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-X.X-blue) ![HPINS](https://img.shields.io/badge/HPINS-X.X-blue) +- **Total Size:** 16 bytes + +| Field | Description | Unit | Format | Size | Offset | +|----------|---------------------------------------------------------------------------|-------------|--------|------|--------| +| Header | Start of sentence – always 0x02 | - | uint8 | 1 | 0 | +| Status | Status bitmask (see [Status Bitmask](#dolog-hrp-status-bitmask)) | - | uint8 | 1 | 1 | +| Heading | Heading angle [0° to 359.99°] | 2^15 = 180° | uint16 | 2 | 2 | +| Roll | Roll angle, positive with port side up [-90 to 89.99°] | 2^15 = 90° | int16 | 2 | 4 | +| Pitch | Pitch angle, positive with bow up [-90 to 89.99°] | 2^15 = 90° | int16 | 2 | 6 | +| Z Rate | Rate about the body/vehicle Z axis, positive clockwise [-45 to +44.99°/s] | 2^15 = 45°/s | int16 | 2 | 8 | +| X Rate | Rate about the body/vehicle X axis, positive clockwise [-45 to +44.99°/s] | 2^15 = 45°/s | int16 | 2 | 10 | +| Y Rate | Rate about the body/vehicle Y axis, positive clockwise [-45 to +44.99°/s] | 2^15 = 45°/s | int16 | 2 | 12 | +| Checksum | Negative sum of all the bytes before the checksum, ignoring overflow | - | uint8 | 1 | 14 | +| Footer | End of frame byte – always 0x03 | - | uint8 | 1 | 15 | + +### Status Bitmask {#dolog-hrp-status-bitmask} + +The Status Word is an 8-bit integer with various single-bit flags, numbered from 0 to 7, where position 0 is the least significant bit. +Each bit indicates specific status information: + +| Bit Position | Description | +|--------------|---------------------------------------------------------------------------------------------| +| 0 | Set if the Heading, Pitch & Roll measurements are valid (based on user-defined thresholds). | +| 1-3 | Not used | +| 4 | Set if the device is aligned and ready to deliver full accuracy data | +| 5 | Set if an error has been detected on the IMU (accelerometers or gyroscopes) | +| 6-7 | Not used | + +## SBG_ECOM_THIRD_PARTY_AHRS_500 (07) {#SBG_ECOM_THIRD_PARTY_AHRS_500} + +The `SBG_ECOM_THIRD_PARTY_AHRS_500` message is a proprietary binary log that ensures compatibility with the Crossbow AHRS-500 series. +This log outputs essential data such as attitude (roll, pitch, yaw), rotation rates, accelerations, and device status, enabling seamless integration with AHRS systems. + +All values are stored in Big Endian format (MSB first), with 16-bit signed integers represented in 2's complement. +While this message mirrors Crossbow's format, SBG Systems has implemented certain conversions to align with its AHRS/INS technology. + +Each data packet begins with a two-byte header (`0xAA 0x55`) and ends with a two-byte checksum, ensuring data integrity. +The checksum is computed by summing all packet contents except the header and checksum bytes, and then taking the result modulo `0xFFFF`. + +### Message Structure + +- **Message Name (ID):** `SBG_ECOM_THIRD_PARTY_AHRS_500 (07)` +- **Compatibility:** AHRS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-X.X-blue) ![HPINS](https://img.shields.io/badge/HPINS-X.X-blue) +- **Total Size:** 26 bytes + +| Field | Description | Resolution | Unit | Format | Size | Offset | +|------------|-------------------------------------------------------------------------------|------------|--------|--------|------|--------| +| Header #1 | First header byte, always `0xAA` | - | - | uint8 | 1 | 0 | +| Header #2 | Second header byte, always `0x55` | - | - | uint8 | 1 | 1 | +| Roll | Roll angle [-180° to 180°] | 180/2^15 | ° | int16 | 2 | 2 | +| Pitch | Pitch angle [-90° to 90°] | 180/2^15 | ° | int16 | 2 | 4 | +| Heading | Heading angle [-180° to 180°] | 180/2^15 | ° | int16 | 2 | 6 | +| Rate X | X body rotation rate [-1200°/s to +1200°/s] | 1200/2^15 | °/s | int16 | 2 | 8 | +| Rate Y | Y body rotation rate [-1200°/s to +1200°/s] | 1200/2^15 | °/s | int16 | 2 | 10 | +| Rate Z | Z body rotation rate [-1200°/s to +1200°/s] | 1200/2^15 | °/s | int16 | 2 | 12 | +| Accel X | X body acceleration [-15g to +15g], where 1g = 9.80 m/s² | 15/2^15 | G | int16 | 2 | 14 | +| Accel Y | Y body acceleration [-15g to +15g], where 1g = 9.80 m/s² | 15/2^15 | G | int16 | 2 | 16 | +| Accel Z | Z body acceleration [-15g to +15g], where 1g = 9.80 m/s² | 15/2^15 | G | int16 | 2 | 18 | +| Model # | Model number, always set to 226 | - | - | uint16 | 2 | 20 | +| Status BIT | Device status, see [Status BIT definitions](#status-bit-definitions) | - | - | uint16 | 2 | 22 | +| Checksum | Checksum computed as described above. | - | - | uint16 | 2 | 24 | + +> [!NOTE] +> SBG Systems products and the AHRS-500 both use the North-East-Down frame convention. Euler angles, rotation rates, and accelerations are consistent with SBG Systems' standards. + +### Status BIT Definitions {#status-bit-definitions} + +The Status BIT field contains several status indicators to monitor the health of the product. +A bit set to `1` indicates an error or performance degradation, while a bit set to `0` reports normal or optimal operation. + +Please note that due to architectural differences, Crossbow AHRS-500 status may not directly translate to SBG Systems products. + +| Bit | Name | Description | +|-----|--------------------|--------------------------------------------------------------------------------------------------------| +| 0 | Hard Failure | `0`: Unit is functioning correctly; `1`: Fatal error detected, do not use unit data | +| 1 | Soft Failure | Not implemented, always `0` | +| 2 | Reserved | Reserved, always `0` | +| 3 | Power Failure | Not implemented, always `0` | +| 4 | Comm Error | `0`: Port A communication normal; `1`: Port A communication errors or saturation | +| 5 | Reboot Detect | Not implemented, always `0` | +| 6 | Calibration Failure| `0`: Sensor calibration valid; `1`: Sensor calibration corrupted, data invalid | +| 7 | External Aiding | `0`: External air data aiding; `1`: External air data not present or not used in the AHRS/INS solution | +| 8 | Coordinated Turn | Not implemented, always `0` | +| 9 | Reserved | Reserved, always `0` | +| 10 | Algorithm Status | `0`: Roll and Pitch valid, alignment done; `1`: Alignment in progress or Roll/Pitch invalid | +| 11 | Mag Align Status | `0`: Heading valid and aligned; `1`: Heading invalid, free drifting or not aligned | +| 12 | Reserved | Reserved, always `0` | +| 13 | Mag Calibration | `0`: Magnetometers valid, used in AHRS/INS solution; `1`: Magnetometers not used or poorly calibrated | +| 14 | Reserved | Reserved, always `0` | +| 15 | Remote Mag | Not implemented, always `0` | + +Here's the improved and reformatted version of the ADA Data Packet message: + +## SBG_ECOM_THIRD_PARTY_ADA_01 (08) {#SBG_ECOM_THIRD_PARTY_ADA_01} + +The `SBG_ECOM_THIRD_PARTY_ADA_01` message is a proprietary binary log that encapsulates all INS, IMU, and status data within a single, compact frame. +This message is designed for efficient processing on low-end platforms by using only integer representations with specific scaling. + +All values are stored in Big Endian format (MSB first) and are encoded using 16/32-bit signed integers in 2's complement representation or unsigned integers. +The status indicators align with those defined in standard sbgECom logs, ensuring consistent behavior across different log types. + +The data packet begins with a two-byte header (`0xAA 0x5A`) and includes a two-byte checksum to ensure data integrity. +The checksum is calculated as the sum of all packet contents (excluding the header and checksum bytes) modulo `0xFFFF`. + +### Message Structure + +- **Message Name (ID):** `SBG_ECOM_THIRD_PARTY_ADA_01 (08)` +- **Compatibility:** INS and IMU capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-X.X-blue) ![HPINS](https://img.shields.io/badge/HPINS-X.X-blue) +- **Total Size:** 100 bytes + +| Field | Description | Resolution | Unit | Format | Size | Offset | +|-----------------|-----------------------------------------------------------------------|-------------|---------|--------|------|--------| +| Header #1 | First header byte, always `0xAA` | - | - | uint8 | 1 | 0 | +| Header #2 | Second header byte, always `0x5A` | - | - | uint8 | 1 | 1 | +| Roll | INS Roll angle [-180° to 180°] | 180/2^15 | ° | int16 | 2 | 2 | +| Pitch | INS Pitch angle [-90° to 90°] | 180/2^15 | ° | int16 | 2 | 4 | +| Heading | INS Heading angle [-180° to 180°] | 180/2^15 | ° | int16 | 2 | 6 | +| Rate X | X body rotation rate [-1200°/s to +1200°/s] | 1200/2^15 | °/s | int16 | 2 | 8 | +| Rate Y | Y body rotation rate [-1200°/s to +1200°/s] | 1200/2^15 | °/s | int16 | 2 | 10 | +| Rate Z | Z body rotation rate [-1200°/s to +1200°/s] | 1200/2^15 | °/s | int16 | 2 | 12 | +| Accel X | X body acceleration [-15g to +15g], with 1g = 9.80 m/s² | 15/2^15 | G | int16 | 2 | 14 | +| Accel Y | Y body acceleration [-15g to +15g], with 1g = 9.80 m/s² | 15/2^15 | G | int16 | 2 | 16 | +| Accel Z | Z body acceleration [-15g to +15g], with 1g = 9.80 m/s² | 15/2^15 | G | int16 | 2 | 18 | +| Temperature | Internal INS temperature [-50° to +100°] | 100/2^15 | °C | int16 | 2 | 20 | +| Velocity N | INS North Velocity [-1500 m/s to +1500 m/s] | 1500/2^31 | m/s | int32 | 4 | 22 | +| Velocity E | INS East Velocity [-1500 m/s to +1500 m/s] | 1500/2^31 | m/s | int32 | 4 | 26 | +| Velocity D | INS Down Velocity [-1500 m/s to +1500 m/s] | 1500/2^31 | m/s | int32 | 4 | 30 | +| Latitude | INS Latitude positive North [-90° to +90°] | 180/2^31 | ° | int32 | 4 | 34 | +| Longitude | INS Longitude positive East [-180° to +180°] | 180/2^31 | ° | int32 | 4 | 38 | +| Altitude | INS Altitude above MSL positive upward [-100000 m to +100000 m] | 100000/2^31 | m | int32 | 4 | 42 | +| Roll Std | INS Roll accuracy 1σ standard deviation [0° to +180°] | 180/2^15 | ° | uint16 | 2 | 46 | +| Pitch Std | INS Pitch accuracy 1σ standard deviation [0° to +180°] | 180/2^15 | ° | uint16 | 2 | 48 | +| Heading Std | INS Heading accuracy 1σ standard deviation [0° to +180°] | 180/2^15 | ° | uint16 | 2 | 50 | +| Velocity N Std. | INS North Velocity accuracy 1σ standard deviation [0 m/s to +200 m/s] | 100/2^15 | m/s | uint16 | 2 | 52 | +| Velocity E Std. | INS East Velocity accuracy 1σ standard deviation [0 m/s to +200 m/s] | 100/2^15 | m/s | uint16 | 2 | 54 | +| Velocity D Std. | INS Down Velocity accuracy 1σ standard deviation [0 m/s to +200 m/s] | 100/2^15 | m/s | uint16 | 2 | 56 | +| Latitude Std. | INS Latitude accuracy 1σ standard deviation [0 m to +3276 m] | 0.05 | m | uint16 | 2 | 58 | +| Longitude Std. | INS Longitude accuracy 1σ standard deviation [0 m to +3276 m] | 0.05 | m | uint16 | 2 | 60 | +| Altitude Std. | INS Altitude accuracy 1σ standard deviation [0 m to +3276 m] | 0.05 | m | uint16 | 2 | 62 | +| Timestamp | Free running time since INS powered up | - | µs | uint32 | 4 | 64 | +| General Status | See GENERAL_STATUS definition | - | - | uint16 | 2 | 68 | +| Com Status | See COM_STATUS definition | - | - | uint32 | 4 | 70 | +| Aiding Status | See AIDING_STATUS definition | - | - | uint32 | 4 | 74 | +| Up Time | INS system up time since power on | - | s | uint32 | 4 | 78 | +| IMU Status | See IMU_STATUS definition | - | - | uint16 | 2 | 82 | +| Solution Status | See SOLUTION_STATUS definition | - | - | uint32 | 4 | 84 | +| Mag X | X body magnetic field in Arbitrary Unit (AU) | 1/2^10 | AU | int16 | 2 | 88 | +| Mag Y | Y body magnetic field in Arbitrary Unit (AU) | 1/2^10 | AU | int16 | 2 | 90 | +| Mag Z | Z body magnetic field in Arbitrary Unit (AU) | 1/2^10 | AU | int16 | 2 | 92 | +| Reserved | Reserved bytes for future use | - | - | - | 4 | 94 | +| Checksum | Checksum computed as described above. | - | - | uint16 | 2 | 98 | + +> [!NOTE] +> SBG Systems products and ADA Data Packet messages use the same conventions and frame definitions, ensuring seamless integration. + +## SBG_ECOM_THIRD_PARTY_AT_ITINS (09) {#SBG_ECOM_THIRD_PARTY_AT_ITINS} + +The `SBG_ECOM_THIRD_PARTY_AT_ITINS` message is a proprietary ASCII log designed to provide all essential navigation and orientation data required by the AVIATOR UAV 200 Satcom terminal. +The Cobham Aviator UAV 200 utilizes SBG Systems' INS data to precisely align with the satellite, thereby enhancing transmission range and reliability. + +The message begins with the header `AT_ITINS=` and ends with ``. +Each parameter is separated by a comma, and the total message length does not exceed 128 characters. + +### Example Message +``` +AT_ITINS=48.9102,2.1677,66,112,2020-06-17/14:10:15,0,0,0,-1.6,0.2,0.1,127.3,126.3,0.1,0.2,0.0 +``` + +### Message Structure + +- **Message Name (ID):** `SBG_ECOM_THIRD_PARTY_AT_ITINS (09)` +- **Compatibility:** INS capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-X.X-blue) ![HPINS](https://img.shields.io/badge/HPINS-X.X-blue) +- **Maximum Length:** 128 characters + +| Field | Description | Decimal Digits | Unit | +|-----------------|-----------------------------------------------------|----------------|-------| +| AT_ITINS= | AT-Command header | - | - | +| latitude | INS latitude, positive North [-90 to +90] | 4 | ° | +| longitude | INS longitude, positive East [-180 to +180] | 4 | ° | +| baroAltitude | Barometric altitude [-500 to +25000] | 0 | m | +| height | INS altitude above ellipsoid [-500 to +25000] | 0 | m | +| UTC Time | UTC Time as a string: `yyyy-mm-dd/hh:mm:ss` | - | - | +| velocityNorth | INS north velocity, positive north [-500 to +500] | 0 | m/s | +| velocityEast | INS east velocity, positive east [-500 to +500] | 0 | m/s | +| gndSpeed | INS ground 2D velocity [0 to +500] | 0 | m/s | +| trackAngle | INS track course, positive clockwise [-180 to +180] | 1 | ° | +| roll | INS roll angle, positive right up [-180 to +180] | 1 | ° | +| pitch | INS pitch angle, positive nose up [-180 to +180] | 1 | ° | +| heading | INS heading, positive clockwise [-180 to +180] | 1 | ° | +| magHeading | Magnetic heading, positive clockwise [-180 to +180] | 1 | ° | +| rollRate | Roll rate, positive right up [-90 to +90] | 1 | °/s | +| pitchRate | Pitch rate, positive nose up [-90 to +90] | 1 | °/s | +| yawRate | Yaw rate, positive clockwise [-90 to +90] | 1 | °/s | + +> [!NOTE] +> The roll, pitch, and yaw rates are expressed in the device’s body/vehicle frame. +> These measurements differ from IMU gyroscope outputs as they are compensated by the INS filter for residual sensor biases, scale factors, and the Earth’s rotation rate. + +> [!NOTE] +> Fields that are unavailable or invalid are represented with a value of zero. + +## SBG_ECOM_THIRD_PARTY_KONGSBERG_MB (10) {#SBG_ECOM_THIRD_PARTY_KONGSBERG_MB} + +The `SBG_ECOM_THIRD_PARTY_KONGSBERG_MB` message is a proprietary binary log from Kongsberg MBES. +This log is primarily used to support FM mode by integrating INS and heave data, which is essential for precise marine survey operations. + +All fields in this log are stored in Little Endian format (LSB first). Signed data uses the 2's complement representation. + +### Message Structure + +- **Message Name (ID):** `SBG_ECOM_THIRD_PARTY_KONGSBERG_MB (10)` +- **Compatibility:** Heave capable products +- **Firmware:** ![ELLIPSE](https://img.shields.io/badge/ELLIPSE-X.X-blue) ![HPINS](https://img.shields.io/badge/HPINS-X.X-blue) +- **Total Size:** 132 bytes + +| Field | Description | Unit | Format | Size | Offset | +|-------------------------|-----------------------------------------------------------------|-------------|--------|------|--------| +| Start ID | 4 chars header, always set to `#KMB` | - | char | 4 | 0 | +| Datagram Length | Message length, always set to 132 bytes | - | uint16 | 2 | 4 | +| Datagram Version | Message version, always set to 1 | - | uint16 | 2 | 6 | +| UTC Seconds | UTC time in seconds since epoch 1970-01-01 | Seconds | uint32 | 4 | 8 | +| UTC Nanoseconds | UTC time second decimal part in nanoseconds | Nanoseconds | uint32 | 4 | 12 | +| Status Word | Status and quality (see [KMB status](#kmb-status-word)) | - | uint32 | 4 | 16 | +| Latitude | INS latitude, positive North of the Equator | Degree | double | 8 | 20 | +| Longitude | INS longitude, positive East of Greenwich | Degree | double | 8 | 28 | +| Ellipsoid Height | INS height above ellipsoid, positive upward | Meter | float | 4 | 36 | +| Roll | INS roll angle, positive with port side up | Degree | float | 4 | 40 | +| Pitch | INS pitch angle, positive with bow up | Degree | float | 4 | 44 | +| Heading | INS true heading | Degree | float | 4 | 48 | +| Heave | Real-time heave, positive downward | Meter | float | 4 | 52 | +| Roll Rate | Vessel roll rate, positive with port side up | °/s | float | 4 | 56 | +| Pitch Rate | Vessel pitch rate, positive with bow up | °/s | float | 4 | 60 | +| Yaw Rate | Vessel yaw rate, positive clockwise | °/s | float | 4 | 64 | +| North Velocity | INS north velocity, positive North | m/s | float | 4 | 68 | +| East Velocity | INS east velocity, positive East | m/s | float | 4 | 72 | +| Down Velocity | INS down velocity, positive Down | m/s | float | 4 | 76 | +| Latitude Error | INS estimated 1-sigma latitude error | Meter | float | 4 | 80 | +| Longitude Error | INS estimated 1-sigma longitude error | Meter | float | 4 | 84 | +| Height Error | INS estimated 1-sigma height error | Meter | float | 4 | 88 | +| Roll Error | INS estimated 1-sigma roll error | Degree | float | 4 | 92 | +| Pitch Error | INS estimated 1-sigma pitch error | Degree | float | 4 | 96 | +| Heading Error | INS estimated 1-sigma heading error | Degree | float | 4 | 100 | +| Heave Error | INS estimated 1-sigma real-time heave error | Meter | float | 4 | 104 | +| North Acceleration | INS north acceleration (gravity-free) | m/s² | float | 4 | 108 | +| East Acceleration | INS east acceleration (gravity-free) | m/s² | float | 4 | 112 | +| Down Acceleration | INS down acceleration (gravity-free) | m/s² | float | 4 | 116 | +| Delayed Heave UTC Sec | UTC time integer seconds for delayed heave data | Seconds | uint32 | 4 | 120 | +| Delayed Heave UTC Ns | UTC time fractional seconds part for delayed heave data | Nanoseconds | uint32 | 4 | 124 | +| Delayed Heave | Delayed heave value, positive downward | Meter | float | 4 | 128 | + +### Time Format + +The UTC time is divided into an integer seconds part and a fractional second part. The integer part is UNIX time counted from 1970-01-01 UTC. + +### KMB Status Word {#kmb-status-word} + +The Status Word consists of 32 single-bit flags, numbered from 0 to 31, where position 0 is the least significant bit. +A bit set to `1` indicates the following: + +| Bit Position | Description | +|--------------|----------------------------------------------------------------------------------------------| +| 0 | Invalid INS position and velocity data, as defined by user quality thresholds. | +| 1 | Invalid INS roll and pitch data, as defined by user quality thresholds. | +| 2 | Invalid INS heading data, as defined by user quality thresholds. | +| 3 | Invalid heave and vertical velocity data. The heave filter is not converged or has diverged. | +| 4 | Invalid INS acceleration data (NED). Same behavior as invalid INS position and velocity. | +| 5 | Invalid delayed heave data. The delayed heave filter has not converged. | +| 16 | Reduced INS position and velocity data. Data is valid but the INS is not aligned yet. | +| 17 | Reduced INS roll and pitch data. Data is valid but the INS is not aligned yet. | +| 18 | Reduced INS heading data. Data is valid but the INS is not aligned yet. | +| 19 | Reduced heave and vertical velocity data. The heave is valid but not aided by INS. | +| 20 | Reduced INS acceleration data (NED). Same behavior as reduced INS position and velocity. | +| 21 | Reduced delayed heave data. The delayed heave is valid but not aided by INS. | + +> [!NOTE] +> Validity thresholds for each parameter can be configured by the user in the device settings. diff --git a/examples/airDataInput/src/airDataInput.c b/examples/airDataInput/src/airDataInput.c deleted file mode 100644 index f1f323f..0000000 --- a/examples/airDataInput/src/airDataInput.c +++ /dev/null @@ -1,263 +0,0 @@ -/*! - * \file ellipseMinimal.c - * \author SBG Systems - * \date 28/03/2014 - * - * \brief Send AirData aiding mesurements to an ELLIPSE and read back data - * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license - * - * 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. - * - * \endlicense - */ - -// sbgCommonLib headers -#include - -// sbgECom headers -#include - -//----------------------------------------------------------------------// -//- Private methids -// -//----------------------------------------------------------------------// - -/*! - * Generate a random float number between min and max. - * - * \param[in] randMin Minimum random value to generate. - * \param[in] randMax Maximum random value to generate. - * \return Float randome value between min and max. - */ -static float airDataInputRandFloat(float randMin, float randMax) -{ - assert(randMin <= randMax); - - if (randMin == randMax) - { - return randMin; - } - else - { - return (randMax - randMin) * ((float)rand() / RAND_MAX) + randMin; - } -} - -/*! - * Send an AirData packet to the device with random values. - * - * \param[in] pHandle SbgECom handle - * \return SBG_NO_ERROR if the packet has been sent successfully. - */ -static SbgErrorCode airDataInputSendOneLog(SbgEComHandle *pHandle) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgEComLogAirData airDataLog; - uint8_t outputBuffer[64]; - SbgStreamBuffer outputStream; - - assert(pHandle); - - // - // Create a random AirData struct - // - memset(&airDataLog, 0x00, sizeof(airDataLog)); - - // - // We consider a fixed delay of 10 ms - // - airDataLog.timeStamp = 10000; - airDataLog.status |= SBG_ECOM_AIR_DATA_TIME_IS_DELAY; - - // - // We create a random altitude between 0 to 8000 meters - // - airDataLog.altitude = airDataInputRandFloat(0.0f, 8000.0f); - airDataLog.status |= SBG_ECOM_AIR_DATA_ALTITUDE_VALID; - - // - // We create a random airspeed between 0 to 12 m.s^-1 - // - airDataLog.trueAirspeed = airDataInputRandFloat(0.0f, 12.0f); - airDataLog.status |= SBG_ECOM_AIR_DATA_AIRPSEED_VALID; - - // - // Write the payload - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - - errorCode = sbgEComLogAirDataWriteToStream(&airDataLog, &outputStream); - - if (errorCode == SBG_NO_ERROR) - { - // - // Send the sbgECom log to the device - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_AIR_DATA, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); - - if (errorCode != SBG_NO_ERROR) - { - SBG_LOG_ERROR(errorCode, "Unable to send the AirData log"); - } - } - else - { - SBG_LOG_ERROR(errorCode, "Unable to write the AirData payload."); - } - - return errorCode; -} - -/*! - * Execute the airDataInput example given an opened and valid interface. - * - * \param[in] pInterface Interface used to communicate with the device. - * \return SBG_NO_ERROR if successful. - */ -static SbgErrorCode airDataInputProcess(SbgInterface *pInterface) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgEComHandle comHandle; - - assert(pInterface); - - // - // Create the sbgECom library and associate it with the created interfaces - // - errorCode = sbgEComInit(&comHandle, pInterface); - - // - // Test that the sbgECom has been initialized - // - if (errorCode == SBG_NO_ERROR) - { - // - // Welcome message - // - printf("Welcome to the AirDataInput example.\n"); - printf("sbgECom version %s\n\n", SBG_E_COM_VERSION_STR); - - // - // Loop until the user kill the process - // - while (1) - { - // - // Try to read a frame - // - errorCode = sbgEComHandle(&comHandle); - - // - // Test if we have to release some CPU (no frame received) - // - if (errorCode == SBG_NOT_READY) - { - // - // Send one AirData log - // - if (airDataInputSendOneLog(&comHandle) == SBG_NO_ERROR) - { - SBG_LOG_DEBUG("Airdata log sent!"); - } - else - { - SBG_LOG_WARNING(errorCode, "Unable to send AirData log"); - } - - // - // Wait for 100 ms to only send AirData at 10 Hz - // - sbgSleep(100); - } - else - { - SBG_LOG_ERROR(errorCode, "Unable to process incoming sbgECom logs"); - } - } - - // - // Close the sbgECom library - // - sbgEComClose(&comHandle); - } - else - { - SBG_LOG_ERROR(errorCode, "Unable to initialize the sbgECom library"); - } - - return errorCode; -} - -//----------------------------------------------------------------------// -// Main program // -//----------------------------------------------------------------------// - -/*! - * Program entry point usage: airDataInput COM1 921600 - * - * \param[in] argc Number of input arguments. - * \param[in] argv Input arguments as an array of strings. - * \return EXIT_SUCCESS if successful. - */ -int main(int argc, char** argv) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgInterface sbgInterface; - int exitCode; - - SBG_UNUSED_PARAMETER(argc); - SBG_UNUSED_PARAMETER(argv); - - if (argc == 3) - { - // - // Create a serial interface to communicate with the PULSE - // - errorCode = sbgInterfaceSerialCreate(&sbgInterface, argv[1], atoi(argv[2])); - - if (errorCode == SBG_NO_ERROR) - { - errorCode = airDataInputProcess(&sbgInterface); - - if (errorCode == SBG_NO_ERROR) - { - exitCode = EXIT_SUCCESS; - } - else - { - exitCode = EXIT_FAILURE; - } - - sbgInterfaceDestroy(&sbgInterface); - } - else - { - SBG_LOG_ERROR(errorCode, "unable to open serial interface"); - exitCode = EXIT_FAILURE; - } - } - else - { - printf("Invalid input arguments, usage: airDataInput SERIAL_DEVICE SERIAL_BAUDRATE\n"); - exitCode = EXIT_FAILURE; - } - - return exitCode; -} diff --git a/examples/airDataInput/src/main.c b/examples/airDataInput/src/main.c new file mode 100644 index 0000000..b7fec02 --- /dev/null +++ b/examples/airDataInput/src/main.c @@ -0,0 +1,263 @@ +/*! + * \file main.c + * \author SBG Systems + * \date 28/03/2014 + * + * \brief Send AirData aiding mesurements to an ELLIPSE and read back data + * + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * 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. + * + * \endlicense + */ + +// sbgCommonLib headers +#include + +// sbgECom headers +#include + +//----------------------------------------------------------------------// +//- Private methids -// +//----------------------------------------------------------------------// + +/*! + * Generate a random float number between min and max. + * + * \param[in] randMin Minimum random value to generate. + * \param[in] randMax Maximum random value to generate. + * \return Float randome value between min and max. + */ +static float airDataInputRandFloat(float randMin, float randMax) +{ + assert(randMin <= randMax); + + if (randMin == randMax) + { + return randMin; + } + else + { + return (randMax - randMin) * ((float)rand() / RAND_MAX) + randMin; + } +} + +/*! + * Send an AirData packet to the device with random values. + * + * \param[in] pHandle SbgECom handle + * \return SBG_NO_ERROR if the packet has been sent successfully. + */ +static SbgErrorCode airDataInputSendOneLog(SbgEComHandle *pHandle) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComLogAirData airDataLog; + uint8_t outputBuffer[64]; + SbgStreamBuffer outputStream; + + assert(pHandle); + + // + // Create a random AirData struct + // + memset(&airDataLog, 0x00, sizeof(airDataLog)); + + // + // We consider a fixed delay of 10 ms + // + airDataLog.timeStamp = 10000; + airDataLog.status |= SBG_ECOM_AIR_DATA_TIME_IS_DELAY; + + // + // We create a random altitude between 0 to 8000 meters + // + airDataLog.altitude = airDataInputRandFloat(0.0f, 8000.0f); + airDataLog.status |= SBG_ECOM_AIR_DATA_ALTITUDE_VALID; + + // + // We create a random airspeed between 0 to 12 m.s^-1 + // + airDataLog.trueAirspeed = airDataInputRandFloat(0.0f, 12.0f); + airDataLog.status |= SBG_ECOM_AIR_DATA_AIRPSEED_VALID; + + // + // Write the payload + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + + errorCode = sbgEComLogAirDataWriteToStream(&airDataLog, &outputStream); + + if (errorCode == SBG_NO_ERROR) + { + // + // Send the sbgECom log to the device + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_AIR_DATA, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + if (errorCode != SBG_NO_ERROR) + { + SBG_LOG_ERROR(errorCode, "Unable to send the AirData log"); + } + } + else + { + SBG_LOG_ERROR(errorCode, "Unable to write the AirData payload."); + } + + return errorCode; +} + +/*! + * Execute the airDataInput example given an opened and valid interface. + * + * \param[in] pInterface Interface used to communicate with the device. + * \return SBG_NO_ERROR if successful. + */ +static SbgErrorCode airDataInputProcess(SbgInterface *pInterface) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComHandle comHandle; + + assert(pInterface); + + // + // Create the sbgECom library and associate it with the created interfaces + // + errorCode = sbgEComInit(&comHandle, pInterface); + + // + // Test that the sbgECom has been initialized + // + if (errorCode == SBG_NO_ERROR) + { + // + // Welcome message + // + printf("Welcome to the AirDataInput example.\n"); + printf("sbgECom version %s\n\n", SBG_E_COM_VERSION_STR); + + // + // Loop until the user kill the process + // + while (1) + { + // + // Try to read a frame + // + errorCode = sbgEComHandle(&comHandle); + + // + // Test if we have to release some CPU (no frame received) + // + if (errorCode == SBG_NOT_READY) + { + // + // Send one AirData log + // + if (airDataInputSendOneLog(&comHandle) == SBG_NO_ERROR) + { + SBG_LOG_DEBUG("Airdata log sent!"); + } + else + { + SBG_LOG_WARNING(errorCode, "Unable to send AirData log"); + } + + // + // Wait for 100 ms to only send AirData at 10 Hz + // + sbgSleep(100); + } + else + { + SBG_LOG_ERROR(errorCode, "Unable to process incoming sbgECom logs"); + } + } + + // + // Close the sbgECom library + // + sbgEComClose(&comHandle); + } + else + { + SBG_LOG_ERROR(errorCode, "Unable to initialize the sbgECom library"); + } + + return errorCode; +} + +//----------------------------------------------------------------------// +// Main program // +//----------------------------------------------------------------------// + +/*! + * Program entry point usage: airDataInput COM1 921600 + * + * \param[in] argc Number of input arguments. + * \param[in] argv Input arguments as an array of strings. + * \return EXIT_SUCCESS if successful. + */ +int main(int argc, char** argv) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgInterface sbgInterface; + int exitCode; + + SBG_UNUSED_PARAMETER(argc); + SBG_UNUSED_PARAMETER(argv); + + if (argc == 3) + { + // + // Create a serial interface to communicate with the PULSE + // + errorCode = sbgInterfaceSerialCreate(&sbgInterface, argv[1], atoi(argv[2])); + + if (errorCode == SBG_NO_ERROR) + { + errorCode = airDataInputProcess(&sbgInterface); + + if (errorCode == SBG_NO_ERROR) + { + exitCode = EXIT_SUCCESS; + } + else + { + exitCode = EXIT_FAILURE; + } + + sbgInterfaceDestroy(&sbgInterface); + } + else + { + SBG_LOG_ERROR(errorCode, "unable to open serial interface"); + exitCode = EXIT_FAILURE; + } + } + else + { + printf("Invalid input arguments, usage: airDataInput SERIAL_DEVICE SERIAL_BAUDRATE\n"); + exitCode = EXIT_FAILURE; + } + + return exitCode; +} diff --git a/examples/ellipseLegacy/src/main.c b/examples/ellipseLegacy/src/main.c new file mode 100644 index 0000000..060855e --- /dev/null +++ b/examples/ellipseLegacy/src/main.c @@ -0,0 +1,283 @@ +/*! + * \file main.c + * \author SBG Systems + * \date 28/03/2014 + * + * \brief C example that showcase ELLIPSE configuration and log parsing. + * + * This small example demonstrates how to initialize the sbgECom library + * to read data from an Ellipse using callbacks. + * + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * 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. + * + * \endlicense + */ + +// sbgCommonLib headers +#include +#include + +// sbgECom headers +#include + +//----------------------------------------------------------------------// +//- 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] 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. + */ +static SbgErrorCode onLogReceived(SbgEComHandle *pHandle, SbgEComClass msgClass, SbgEComMsgId msg, const SbgEComLogUnion *pLogData, void *pUserArg) +{ + assert(pLogData); + + SBG_UNUSED_PARAMETER(pHandle); + SBG_UNUSED_PARAMETER(pUserArg); + + if (msgClass == SBG_ECOM_CLASS_LOG_ECOM_0) + { + // + // Handle separately each received data according to the log ID + // + switch (msg) + { + case SBG_ECOM_LOG_EKF_EULER: + // + // Simply display euler angles in real time + // + printf("Euler Angles: %3.1f\t%3.1f\t%3.1f\tStd Dev:%3.1f\t%3.1f\t%3.1f \r", + sbgRadToDegf(pLogData->ekfEulerData.euler[0]), sbgRadToDegf(pLogData->ekfEulerData.euler[1]), sbgRadToDegf(pLogData->ekfEulerData.euler[2]), + sbgRadToDegf(pLogData->ekfEulerData.eulerStdDev[0]), sbgRadToDegf(pLogData->ekfEulerData.eulerStdDev[1]), sbgRadToDegf(pLogData->ekfEulerData.eulerStdDev[2])); + break; + default: + break; + } + } + + return SBG_NO_ERROR; +} + +/*! + * Get and print product info. + * + * \param[in] pECom SbgECom instance. + * \return SBG_NO_ERROR if successful. + */ +static SbgErrorCode getAndPrintProductInfo(SbgEComHandle *pECom) +{ + SbgErrorCode errorCode; + SbgEComDeviceInfo deviceInfo; + + assert(pECom); + + // + // Get device information + // + errorCode = sbgEComCmdGetInfo(pECom, &deviceInfo); + + // + // Display device information if no error + // + if (errorCode == SBG_NO_ERROR) + { + char calibVersionStr[32]; + char hwRevisionStr[32]; + char fmwVersionStr[32]; + + sbgVersionToStringEncoded(deviceInfo.calibationRev, calibVersionStr, sizeof(calibVersionStr)); + sbgVersionToStringEncoded(deviceInfo.hardwareRev, hwRevisionStr, sizeof(hwRevisionStr)); + sbgVersionToStringEncoded(deviceInfo.firmwareRev, fmwVersionStr, sizeof(fmwVersionStr)); + + printf(" Serial Number: %09"PRIu32"\n", deviceInfo.serialNumber); + printf(" Product Code: %s\n", deviceInfo.productCode); + printf(" Hardware Revision: %s\n", hwRevisionStr); + printf(" Firmware Version: %s\n", fmwVersionStr); + printf(" Calib. Version: %s\n", calibVersionStr); + printf("\n"); + } + else + { + SBG_LOG_WARNING(errorCode, "Unable to retrieve device information"); + } + + return errorCode; +} + +/*! + * Execute the ellipseMinimal example given an opened and valid interface. + * + * \param[in] pInterface Interface used to communicate with the device. + * \return SBG_NO_ERROR if successful. + */ +static SbgErrorCode ellipseMinimalProcess(SbgInterface *pInterface) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComHandle comHandle; + + assert(pInterface); + + // + // Create the sbgECom library and associate it with the created interfaces + // + errorCode = sbgEComInit(&comHandle, pInterface); + + // + // Test that the sbgECom has been initialized + // + if (errorCode == SBG_NO_ERROR) + { + // + // Welcome message + // + printf("Welcome to the ELLIPSE minimal example.\n"); + printf("sbgECom version %s\n\n", SBG_E_COM_VERSION_STR); + + // + // Query and display produce info, don't stop if there is an error + // + getAndPrintProductInfo(&comHandle); + + // + // Showcase how to configure some output logs to 25 Hz, don't stop if there is an error + // + errorCode = sbgEComCmdOutputSetConf(&comHandle, SBG_ECOM_OUTPUT_PORT_A, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_IMU_DATA, SBG_ECOM_OUTPUT_MODE_DIV_8); + + if (errorCode != SBG_NO_ERROR) + { + SBG_LOG_WARNING(errorCode, "Unable to configure SBG_ECOM_LOG_IMU_DATA log"); + } + + errorCode = sbgEComCmdOutputSetConf(&comHandle, SBG_ECOM_OUTPUT_PORT_A, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_EKF_EULER, SBG_ECOM_OUTPUT_MODE_DIV_8); + + if (errorCode != SBG_NO_ERROR) + { + SBG_LOG_WARNING(errorCode, "Unable to configure SBG_ECOM_LOG_EKF_EULER log"); + } + + // + // Define callbacks for received data and display header + // + sbgEComSetReceiveLogCallback(&comHandle, onLogReceived, NULL); + printf("Euler Angles display with estimated standard deviation - degrees\n"); + + // + // Loop until the user exist + // + while (1) + { + // + // Try to read a frame + // + errorCode = sbgEComHandle(&comHandle); + + // + // Test if we have to release some CPU (no frame received) + // + if (errorCode == SBG_NOT_READY) + { + // + // Release CPU + // + sbgSleep(1); + } + else + { + SBG_LOG_ERROR(errorCode, "Unable to process incoming sbgECom logs"); + } + } + + // + // Close the sbgECom library + // + sbgEComClose(&comHandle); + } + else + { + SBG_LOG_ERROR(errorCode, "Unable to initialize the sbgECom library"); + } + + return errorCode; +} + +//----------------------------------------------------------------------// +// Main program // +//----------------------------------------------------------------------// + +/*! + * Program entry point usage: ellipseMinimal COM1 921600 + * + * \param[in] argc Number of input arguments. + * \param[in] argv Input arguments as an array of strings. + * \return EXIT_SUCCESS if successful. + */ +int main(int argc, char** argv) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgInterface sbgInterface; + int exitCode; + + SBG_UNUSED_PARAMETER(argc); + SBG_UNUSED_PARAMETER(argv); + + if (argc == 3) + { + // + // Create a serial interface to communicate with the PULSE + // + errorCode = sbgInterfaceSerialCreate(&sbgInterface, argv[1], atoi(argv[2])); + + if (errorCode == SBG_NO_ERROR) + { + errorCode = ellipseMinimalProcess(&sbgInterface); + + if (errorCode == SBG_NO_ERROR) + { + exitCode = EXIT_SUCCESS; + } + else + { + exitCode = EXIT_FAILURE; + } + + sbgInterfaceDestroy(&sbgInterface); + } + else + { + SBG_LOG_ERROR(errorCode, "unable to open serial interface"); + exitCode = EXIT_FAILURE; + } + } + else + { + printf("Invalid input arguments, usage: ellipseMinimal SERIAL_DEVICE SERIAL_BAUDRATE\n"); + exitCode = EXIT_FAILURE; + } + + return exitCode; +} diff --git a/examples/ellipseMinimal/src/ellipseMinimal.c b/examples/ellipseMinimal/src/ellipseMinimal.c deleted file mode 100644 index 6b5f137..0000000 --- a/examples/ellipseMinimal/src/ellipseMinimal.c +++ /dev/null @@ -1,283 +0,0 @@ -/*! - * \file ellipseMinimal.c - * \author SBG Systems - * \date 28/03/2014 - * - * \brief C example that simply opens an Ellipse interface and reads Euler Angles from it. - * - * This small example demonstrates how to initialize the sbgECom library - * to read data from an Ellipse using callbacks. - * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license - * - * 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. - * - * \endlicense - */ - -// sbgCommonLib headers -#include -#include - -// sbgECom headers -#include - -//----------------------------------------------------------------------// -//- 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] 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. - */ -static SbgErrorCode onLogReceived(SbgEComHandle *pHandle, SbgEComClass msgClass, SbgEComMsgId msg, const SbgEComLogUnion *pLogData, void *pUserArg) -{ - assert(pLogData); - - SBG_UNUSED_PARAMETER(pHandle); - SBG_UNUSED_PARAMETER(pUserArg); - - if (msgClass == SBG_ECOM_CLASS_LOG_ECOM_0) - { - // - // Handle separately each received data according to the log ID - // - switch (msg) - { - case SBG_ECOM_LOG_EKF_EULER: - // - // Simply display euler angles in real time - // - printf("Euler Angles: %3.1f\t%3.1f\t%3.1f\tStd Dev:%3.1f\t%3.1f\t%3.1f \r", - sbgRadToDegf(pLogData->ekfEulerData.euler[0]), sbgRadToDegf(pLogData->ekfEulerData.euler[1]), sbgRadToDegf(pLogData->ekfEulerData.euler[2]), - sbgRadToDegf(pLogData->ekfEulerData.eulerStdDev[0]), sbgRadToDegf(pLogData->ekfEulerData.eulerStdDev[1]), sbgRadToDegf(pLogData->ekfEulerData.eulerStdDev[2])); - break; - default: - break; - } - } - - return SBG_NO_ERROR; -} - -/*! - * Get and print product info. - * - * \param[in] pECom SbgECom instance. - * \return SBG_NO_ERROR if successful. - */ -static SbgErrorCode getAndPrintProductInfo(SbgEComHandle *pECom) -{ - SbgErrorCode errorCode; - SbgEComDeviceInfo deviceInfo; - - assert(pECom); - - // - // Get device information - // - errorCode = sbgEComCmdGetInfo(pECom, &deviceInfo); - - // - // Display device information if no error - // - if (errorCode == SBG_NO_ERROR) - { - char calibVersionStr[32]; - char hwRevisionStr[32]; - char fmwVersionStr[32]; - - sbgVersionToStringEncoded(deviceInfo.calibationRev, calibVersionStr, sizeof(calibVersionStr)); - sbgVersionToStringEncoded(deviceInfo.hardwareRev, hwRevisionStr, sizeof(hwRevisionStr)); - sbgVersionToStringEncoded(deviceInfo.firmwareRev, fmwVersionStr, sizeof(fmwVersionStr)); - - printf(" Serial Number: %09"PRIu32"\n", deviceInfo.serialNumber); - printf(" Product Code: %s\n", deviceInfo.productCode); - printf(" Hardware Revision: %s\n", hwRevisionStr); - printf(" Firmware Version: %s\n", fmwVersionStr); - printf(" Calib. Version: %s\n", calibVersionStr); - printf("\n"); - } - else - { - SBG_LOG_WARNING(errorCode, "Unable to retrieve device information"); - } - - return errorCode; -} - -/*! - * Execute the ellipseMinimal example given an opened and valid interface. - * - * \param[in] pInterface Interface used to communicate with the device. - * \return SBG_NO_ERROR if successful. - */ -static SbgErrorCode ellipseMinimalProcess(SbgInterface *pInterface) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgEComHandle comHandle; - - assert(pInterface); - - // - // Create the sbgECom library and associate it with the created interfaces - // - errorCode = sbgEComInit(&comHandle, pInterface); - - // - // Test that the sbgECom has been initialized - // - if (errorCode == SBG_NO_ERROR) - { - // - // Welcome message - // - printf("Welcome to the ELLIPSE minimal example.\n"); - printf("sbgECom version %s\n\n", SBG_E_COM_VERSION_STR); - - // - // Query and display produce info, don't stop if there is an error - // - getAndPrintProductInfo(&comHandle); - - // - // Showcase how to configure some output logs to 25 Hz, don't stop if there is an error - // - errorCode = sbgEComCmdOutputSetConf(&comHandle, SBG_ECOM_OUTPUT_PORT_A, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_IMU_DATA, SBG_ECOM_OUTPUT_MODE_DIV_8); - - if (errorCode != SBG_NO_ERROR) - { - SBG_LOG_WARNING(errorCode, "Unable to configure SBG_ECOM_LOG_IMU_DATA log"); - } - - errorCode = sbgEComCmdOutputSetConf(&comHandle, SBG_ECOM_OUTPUT_PORT_A, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_EKF_EULER, SBG_ECOM_OUTPUT_MODE_DIV_8); - - if (errorCode != SBG_NO_ERROR) - { - SBG_LOG_WARNING(errorCode, "Unable to configure SBG_ECOM_LOG_EKF_EULER log"); - } - - // - // Define callbacks for received data and display header - // - sbgEComSetReceiveLogCallback(&comHandle, onLogReceived, NULL); - printf("Euler Angles display with estimated standard deviation - degrees\n"); - - // - // Loop until the user exist - // - while (1) - { - // - // Try to read a frame - // - errorCode = sbgEComHandle(&comHandle); - - // - // Test if we have to release some CPU (no frame received) - // - if (errorCode == SBG_NOT_READY) - { - // - // Release CPU - // - sbgSleep(1); - } - else - { - SBG_LOG_ERROR(errorCode, "Unable to process incoming sbgECom logs"); - } - } - - // - // Close the sbgECom library - // - sbgEComClose(&comHandle); - } - else - { - SBG_LOG_ERROR(errorCode, "Unable to initialize the sbgECom library"); - } - - return errorCode; -} - -//----------------------------------------------------------------------// -// Main program // -//----------------------------------------------------------------------// - -/*! - * Program entry point usage: ellipseMinimal COM1 921600 - * - * \param[in] argc Number of input arguments. - * \param[in] argv Input arguments as an array of strings. - * \return EXIT_SUCCESS if successful. - */ -int main(int argc, char** argv) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgInterface sbgInterface; - int exitCode; - - SBG_UNUSED_PARAMETER(argc); - SBG_UNUSED_PARAMETER(argv); - - if (argc == 3) - { - // - // Create a serial interface to communicate with the PULSE - // - errorCode = sbgInterfaceSerialCreate(&sbgInterface, argv[1], atoi(argv[2])); - - if (errorCode == SBG_NO_ERROR) - { - errorCode = ellipseMinimalProcess(&sbgInterface); - - if (errorCode == SBG_NO_ERROR) - { - exitCode = EXIT_SUCCESS; - } - else - { - exitCode = EXIT_FAILURE; - } - - sbgInterfaceDestroy(&sbgInterface); - } - else - { - SBG_LOG_ERROR(errorCode, "unable to open serial interface"); - exitCode = EXIT_FAILURE; - } - } - else - { - printf("Invalid input arguments, usage: ellipseMinimal SERIAL_DEVICE SERIAL_BAUDRATE\n"); - exitCode = EXIT_FAILURE; - } - - return exitCode; -} diff --git a/examples/ellipseOnboardMagCalib/src/ellipseOnboardMagCalib.c b/examples/ellipseOnboardMagCalib/src/ellipseOnboardMagCalib.c deleted file mode 100644 index 8d9f42b..0000000 --- a/examples/ellipseOnboardMagCalib/src/ellipseOnboardMagCalib.c +++ /dev/null @@ -1,556 +0,0 @@ -/*! - * \file ellipseOnboardMagCalib.c - * \author SBG Systems - * \date 30/06/2014 - * - * \brief C example that demonstrates the onboard magnetic calibration procedure. - * - * The onboard magnetic calibration is done with the three following steps: - * - Acquire some magnetic field data - * - Compute a magnetic calibration - * - Apply the newly computed magnetic calibration - * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license - * - * 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. - * - * \endlicense - */ - -// sbgCommonLib headers -#include -#include - -// sbgECom headers -#include - -//----------------------------------------------------------------------// -//- Private methods -// -//----------------------------------------------------------------------// - -/*! - * Returns the last char entered by the user just before the user presses the enter key. - * - * \return The key code entered by the user just before he has pressed the 'enter' key - * or -1 if only 'enter' has been pressed. - */ -static int32_t getUserChoice(void) -{ - int userChoice = -1; - - // - // Just drain getc input to only return the latest char just before the enter key is pressed - // - for (;;) - { - int keyPressed; - - keyPressed = getc(stdin); - - if (keyPressed != '\n') - { - userChoice = keyPressed; - } - else if (keyPressed == '\n') - { - break; - } - } - - return userChoice; -} - -/*! - * Ask the user for a 2D or 3D calibration. - * - * \return The calibration mode to use. - */ -static SbgEComMagCalibMode askCalibrationMode(void) -{ - SbgEComMagCalibMode mode; - int32_t keyPressed; - - for (;;) - { - printf("Would you like to perform a 2D or 3D calibration?\n" - " 1) For a 2D calibration\n" - " 2) For a 3D calibration\n" - "Select 1 or 2 and press enter: "); - - keyPressed = getUserChoice(); - - if (keyPressed == '1') - { - mode = SBG_ECOM_MAG_CALIB_MODE_2D; - break; - } - else if (keyPressed == '2') - { - mode = SBG_ECOM_MAG_CALIB_MODE_3D; - break; - } - else - { - printf("Invalid choice, please retry.\n"); - } - } - - return mode; -} - -/*! - * Display magnetic calibration results on the console. - * - * \param[in] mode Define which magnetic calibration type has been performed. It could be 3D or 2D - * \param[in] pMagCalibResults Pointer on a read only magnetic calibration results structure. - */ -static void displayMagCalibResults(SbgEComMagCalibMode mode, const SbgEComMagCalibResults *pMagCalibResults) -{ - // - // Display the magnetic calibration results - // - printf("\n======== Magnetic calibration report ========\n"); - - // - // Convert the quality indicator to human readable output - // - switch (pMagCalibResults->quality) - { - case SBG_ECOM_MAG_CALIB_QUAL_OPTIMAL: - printf("Quality:\t\toptimal\n"); - break; - case SBG_ECOM_MAG_CALIB_QUAL_GOOD: - printf("Quality:\t\tgood\n"); - break; - case SBG_ECOM_MAG_CALIB_QUAL_POOR: - printf("Quality:\t\tpoor\n"); - break; - default: - printf("Quality:\t\tundefined\n"); - } - - // - // Convert the confidence indicator to human readable output - // - switch (pMagCalibResults->confidence) - { - case SBG_ECOM_MAG_CALIB_TRUST_HIGH: - printf("Confidence:\t\thigh\n"); - break; - case SBG_ECOM_MAG_CALIB_TRUST_MEDIUM: - printf("Confidence:\t\tmedium\n"); - break; - case SBG_ECOM_MAG_CALIB_TRUST_LOW: - printf("Confidence:\t\tlow\n"); - break; - default: - printf("Confidence:\t\tundefined\n"); - } - - // - // Print advanced status - // - printf("Advanced Status:\n"); - if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_NOT_ENOUGH_POINTS) - { - printf("\t- Not enough valid points. Maybe you are moving too fast.\n"); - } - if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_TOO_MUCH_DISTORTIONS) - { - printf("\t- Unable to find a calibration solution. Maybe there are too much non static distortions.\n"); - } - if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_ALIGNMENT_ISSUE) - { - printf("\t- The magnetic calibration has troubles to correct the magnetometers and inertial frame alignment.\n"); - } - - // - // Test if we have a 2D or 3D calibration mode - // - if (mode == SBG_ECOM_MAG_CALIB_MODE_2D) - { - // - // In 2D mode, a X or Y motion issue means we have too much motion - // - if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_X_MOTION_ISSUE) - { - printf("\t- Too much roll motion for a 2D magnetic calibration.\n"); - } - if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_Y_MOTION_ISSUE) - { - printf("\t- Too much pitch motion for a 2D magnetic calibration.\n"); - } - } - else - { - // - // In 3D mode, a X or Y motion issue means we have not enough motion - // - if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_X_MOTION_ISSUE) - { - printf("\t- Not enough roll motion for a 3D magnetic calibration.\n"); - } - if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_Y_MOTION_ISSUE) - { - printf("\t- Not enough pitch motion for a 3D magnetic calibration.\n"); - } - } - - // - // Test if we had enough yaw motion to compute a calibration - // - if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_Z_MOTION_ISSUE) - { - // - // Test if we are in - printf("\t- Not enough yaw motion to compute a valid magnetic calibration.\n"); - } - - // - // Display the number of points used to compute the magnetic calibration - // - printf("\n"); - printf("Used Points:\t%"PRIu16"\n", pMagCalibResults->numPoints); - printf("Max Points:\t%"PRIu16"\n", pMagCalibResults->maxNumPoints); - - // - // Display magnetic field deviation errors - // - printf( "\n" - "---------------------------------------------\n" - "- Magnetic field deviation report -\n" - "---------------------------------------------\n"); - - printf("\t\tMean\tStd\tMax\n"); - printf("Before\t\t%0.2f\t%0.2f\t%0.2f\n", pMagCalibResults->beforeMeanError, pMagCalibResults->beforeStdError, pMagCalibResults->beforeMaxError); - printf("After\t\t%0.2f\t%0.2f\t%0.2f\n", pMagCalibResults->afterMeanError, pMagCalibResults->afterStdError, pMagCalibResults->afterMaxError); - printf("Accuracy (deg)\t%0.2f\t%0.2f\t%0.2f\n", sbgRadToDegf(pMagCalibResults->meanAccuracy), sbgRadToDegf(pMagCalibResults->stdAccuracy), sbgRadToDegf(pMagCalibResults->maxAccuracy)); - printf("\n================ END REPORT ================\n"); -} - -/*! - * Get and print product info. - * - * \param[in] pECom SbgECom instance. - * \return SBG_NO_ERROR if successful. - */ -static SbgErrorCode getAndPrintProductInfo(SbgEComHandle *pECom) -{ - SbgErrorCode errorCode; - SbgEComDeviceInfo deviceInfo; - - assert(pECom); - - // - // Get device information - // - errorCode = sbgEComCmdGetInfo(pECom, &deviceInfo); - - // - // Display device information if no error - // - if (errorCode == SBG_NO_ERROR) - { - char calibVersionStr[32]; - char hwRevisionStr[32]; - char fmwVersionStr[32]; - - sbgVersionToStringEncoded(deviceInfo.calibationRev, calibVersionStr, sizeof(calibVersionStr)); - sbgVersionToStringEncoded(deviceInfo.hardwareRev, hwRevisionStr, sizeof(hwRevisionStr)); - sbgVersionToStringEncoded(deviceInfo.firmwareRev, fmwVersionStr, sizeof(fmwVersionStr)); - - printf(" Serial Number: %09"PRIu32"\n", deviceInfo.serialNumber); - printf(" Product Code: %s\n", deviceInfo.productCode); - printf(" Hardware Revision: %s\n", hwRevisionStr); - printf(" Firmware Version: %s\n", fmwVersionStr); - printf(" Calib. Version: %s\n", calibVersionStr); - printf("\n"); - } - else - { - SBG_LOG_WARNING(errorCode, "Unable to retrieve device information"); - } - - return errorCode; -} - -/*! - * Compute the magnetic calibration and display the results to the user. - * - * The new magnetic calibration can be applied on the device if apply is set to true. - * Computing a magnetic calibration don't reset the list of acquired points. - * - * \param[in] pHandle sbgEComHandle instance - * \param[in] mode 2D or 3D magnetic calibration mode. - * \param[in] apply Set to true to compute and then apply the magnetic calibration - * Set to false to just compute and display the magnetic calibration results. - * \return SBG_NO_ERROR if a valid magnetic calibration has been computed (and optionally applied). - */ -static SbgErrorCode computeMagneticCalibration(SbgEComHandle *pHandle, SbgEComMagCalibMode mode, bool apply) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgEComMagCalibResults magCalibResults; - - assert(pHandle); - - // - // Try to compute a magnetic calibration and get the results - // - errorCode = sbgEComCmdMagComputeCalib(pHandle, &magCalibResults); - - if (errorCode == SBG_NO_ERROR) - { - // - // Test if the device has computed a valid magnetic calibration - // - if (magCalibResults.quality != SBG_ECOM_MAG_CALIB_QUAL_INVALID) - { - if (apply) - { - // - // Send the new magnetic calibration data - // - errorCode = sbgEComCmdMagSetCalibData(pHandle, magCalibResults.offset, magCalibResults.matrix); - - if (errorCode == SBG_NO_ERROR) - { - // - // Magnetic calibration applied, save and reboot device to use it - // - errorCode = sbgEComCmdSettingsAction(pHandle, SBG_ECOM_SAVE_SETTINGS); - - if (errorCode == SBG_NO_ERROR) - { - // - // Successfully saved & applied magnetic calibration, display results - // - printf("The magnetic calibration has been saved and applied.\n"); - displayMagCalibResults(mode, &magCalibResults); - } - else - { - SBG_LOG_ERROR(errorCode, "Unable to save new magnetic calibration"); - } - } - else - { - SBG_LOG_ERROR(errorCode, "Unable to apply new magnetic calibration."); - } - } - else - { - // - // Display the magnetic calibration status - // - printf("A new magnetic calibration solution has been computed but not applied.\n"); - displayMagCalibResults(mode, &magCalibResults); - } - } - else - { - SBG_LOG_ERROR(errorCode, "Computed magnetic calibration is invalid. Please retry."); - } - } - else - { - SBG_LOG_ERROR(errorCode, "Unable to compute magnetic calibration."); - } - - return errorCode; -} - -/*! - * Process. - * - * \param[in] pInterface Interface. - * \return SBG_NO_ERROR if successful. - */ -static SbgErrorCode ellipseOnBoardMagCalibProcess(SbgInterface *pInterface) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgEComHandle comHandle; - SbgEComMagCalibMode mode; - int keyPressed; - - assert(pInterface); - - // - // Create the sbgECom library and associate it with the created interfaces - // - errorCode = sbgEComInit(&comHandle, pInterface); - - // - // Test that the sbgECom has been initialized - // - if (errorCode == SBG_NO_ERROR) - { - printf("Welcome to the ELLIPSE on-board magnetic calibration example.\n"); - printf("sbgECom version %s\n\n", SBG_E_COM_VERSION_STR); - - // - // Get device information and print them - // - getAndPrintProductInfo(&comHandle); - - // - // Ask the user if he wants a 2D or 3D calibration - // - mode = askCalibrationMode(); - - // - // Flush the serial interface and sbgECom work buffer as the program flow has been interrupted - // - sbgEComPurgeIncoming(&comHandle); - - // - // Start / reset the acquisition of magnetic field data - // Each time this command is called, the device is prepared to acquire a new set of magnetic field data - // You have to specify here if the magnetic field data acquisition will be used to compute a 2D or 3D calibration - // - errorCode = sbgEComCmdMagStartCalib(&comHandle, mode, SBG_ECOM_MAG_CALIB_HIGH_BW); - - // - // Make sure that the magnetic calibration has started - // - if (errorCode == SBG_NO_ERROR) - { - for (;;) - { - // - // The device is now acquiring some magnetic field data. - // Wait for a user input before computing the magnetic calibration - // - if (mode == SBG_ECOM_MAG_CALIB_MODE_3D) - { - printf("\n\nThe device is acquiring magnetic field data for a 3D calibration........\n"); - } - else - { - printf("\n\nThe device is acquiring magnetic field data for a 2D calibration.........\n"); - } - - printf( "Please rotate the device slowly...\n" - "\n" - "You can compute as many magnetic calibration as you want without loosing already acquired points:\n" - " 1) Compute a magnetic calibration but don't apply it\n" - " 2) Compute and apply a magnetic calibration then save and reboot the device\n" - " 3) Stop the current acquisition and quit\n" - "\n" - "Please enter your choice 1, 2 or 3 and press enter : "); - - // - // Wait for user choice - // - keyPressed = getUserChoice(); - - // - // Flush the serial interface and sbgECom work buffer as the program flow has been interrupted - // - sbgEComPurgeIncoming(&comHandle); - - if (keyPressed == '1') - { - computeMagneticCalibration(&comHandle, mode, false); - } - else if (keyPressed == '2') - { - computeMagneticCalibration(&comHandle, mode, true); - break; - } - else if (keyPressed == '3') - { - break; - } - } - } - else - { - SBG_LOG_ERROR(errorCode, "Unable to start the on-board magnetic calibration"); - } - - // - // Close the sbgECom library - // - sbgEComClose(&comHandle); - } - else - { - SBG_LOG_ERROR(errorCode, "Unable to initialize the sbgECom library."); - } - - return errorCode; -} - -//----------------------------------------------------------------------// -// Main program // -//----------------------------------------------------------------------// - -/*! - * Program entry point usage: ellipseOnboardMagCalib COM1 921600 - * - * \param[in] argc Number of input arguments. - * \param[in] argv Input arguments as an array of strings. - * \return EXIT_SUCCESS if successful. - */ -int main(int argc, char **argv) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgInterface sbgInterface; - int exitCode; - - SBG_UNUSED_PARAMETER(argc); - SBG_UNUSED_PARAMETER(argv); - - if (argc == 3) - { - // - // Create a serial interface to communicate with the PULSE - // - errorCode = sbgInterfaceSerialCreate(&sbgInterface, argv[1], atoi(argv[2])); - - if (errorCode == SBG_NO_ERROR) - { - errorCode = ellipseOnBoardMagCalibProcess(&sbgInterface); - - if (errorCode == SBG_NO_ERROR) - { - exitCode = EXIT_SUCCESS; - } - else - { - exitCode = EXIT_FAILURE; - } - - sbgInterfaceDestroy(&sbgInterface); - } - else - { - SBG_LOG_ERROR(errorCode, "Unable to open serial interface"); - exitCode = EXIT_FAILURE; - } - } - else - { - SBG_LOG_ERROR(SBG_ERROR, "Invalid input arguments, usage: pulseMinimal SERIAL_DEVICE SERIAL_BAUDRATE"); - exitCode = EXIT_FAILURE; - } - - return exitCode; -} diff --git a/examples/ellipseOnboardMagCalib/src/main.c b/examples/ellipseOnboardMagCalib/src/main.c new file mode 100644 index 0000000..17bc62e --- /dev/null +++ b/examples/ellipseOnboardMagCalib/src/main.c @@ -0,0 +1,556 @@ +/*! + * \file main.c + * \author SBG Systems + * \date 30/06/2014 + * + * \brief C example that demonstrates the onboard magnetic calibration procedure. + * + * The onboard magnetic calibration is done with the three following steps: + * - Acquire some magnetic field data + * - Compute a magnetic calibration + * - Apply the newly computed magnetic calibration + * + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * 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. + * + * \endlicense + */ + +// sbgCommonLib headers +#include +#include + +// sbgECom headers +#include + +//----------------------------------------------------------------------// +//- Private methods -// +//----------------------------------------------------------------------// + +/*! + * Returns the last char entered by the user just before the user presses the enter key. + * + * \return The key code entered by the user just before he has pressed the 'enter' key + * or -1 if only 'enter' has been pressed. + */ +static int32_t getUserChoice(void) +{ + int userChoice = -1; + + // + // Just drain getc input to only return the latest char just before the enter key is pressed + // + for (;;) + { + int keyPressed; + + keyPressed = getc(stdin); + + if (keyPressed != '\n') + { + userChoice = keyPressed; + } + else if (keyPressed == '\n') + { + break; + } + } + + return userChoice; +} + +/*! + * Ask the user for a 2D or 3D calibration. + * + * \return The calibration mode to use. + */ +static SbgEComMagCalibMode askCalibrationMode(void) +{ + SbgEComMagCalibMode mode; + int32_t keyPressed; + + for (;;) + { + printf("Would you like to perform a 2D or 3D calibration?\n" + " 1) For a 2D calibration\n" + " 2) For a 3D calibration\n" + "Select 1 or 2 and press enter: "); + + keyPressed = getUserChoice(); + + if (keyPressed == '1') + { + mode = SBG_ECOM_MAG_CALIB_MODE_2D; + break; + } + else if (keyPressed == '2') + { + mode = SBG_ECOM_MAG_CALIB_MODE_3D; + break; + } + else + { + printf("Invalid choice, please retry.\n"); + } + } + + return mode; +} + +/*! + * Display magnetic calibration results on the console. + * + * \param[in] mode Define which magnetic calibration type has been performed. It could be 3D or 2D + * \param[in] pMagCalibResults Pointer on a read only magnetic calibration results structure. + */ +static void displayMagCalibResults(SbgEComMagCalibMode mode, const SbgEComMagCalibResults *pMagCalibResults) +{ + // + // Display the magnetic calibration results + // + printf("\n======== Magnetic calibration report ========\n"); + + // + // Convert the quality indicator to human readable output + // + switch (pMagCalibResults->quality) + { + case SBG_ECOM_MAG_CALIB_QUAL_OPTIMAL: + printf("Quality:\t\toptimal\n"); + break; + case SBG_ECOM_MAG_CALIB_QUAL_GOOD: + printf("Quality:\t\tgood\n"); + break; + case SBG_ECOM_MAG_CALIB_QUAL_POOR: + printf("Quality:\t\tpoor\n"); + break; + default: + printf("Quality:\t\tundefined\n"); + } + + // + // Convert the confidence indicator to human readable output + // + switch (pMagCalibResults->confidence) + { + case SBG_ECOM_MAG_CALIB_TRUST_HIGH: + printf("Confidence:\t\thigh\n"); + break; + case SBG_ECOM_MAG_CALIB_TRUST_MEDIUM: + printf("Confidence:\t\tmedium\n"); + break; + case SBG_ECOM_MAG_CALIB_TRUST_LOW: + printf("Confidence:\t\tlow\n"); + break; + default: + printf("Confidence:\t\tundefined\n"); + } + + // + // Print advanced status + // + printf("Advanced Status:\n"); + if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_NOT_ENOUGH_POINTS) + { + printf("\t- Not enough valid points. Maybe you are moving too fast.\n"); + } + if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_TOO_MUCH_DISTORTIONS) + { + printf("\t- Unable to find a calibration solution. Maybe there are too much non static distortions.\n"); + } + if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_ALIGNMENT_ISSUE) + { + printf("\t- The magnetic calibration has troubles to correct the magnetometers and inertial frame alignment.\n"); + } + + // + // Test if we have a 2D or 3D calibration mode + // + if (mode == SBG_ECOM_MAG_CALIB_MODE_2D) + { + // + // In 2D mode, a X or Y motion issue means we have too much motion + // + if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_X_MOTION_ISSUE) + { + printf("\t- Too much roll motion for a 2D magnetic calibration.\n"); + } + if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_Y_MOTION_ISSUE) + { + printf("\t- Too much pitch motion for a 2D magnetic calibration.\n"); + } + } + else + { + // + // In 3D mode, a X or Y motion issue means we have not enough motion + // + if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_X_MOTION_ISSUE) + { + printf("\t- Not enough roll motion for a 3D magnetic calibration.\n"); + } + if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_Y_MOTION_ISSUE) + { + printf("\t- Not enough pitch motion for a 3D magnetic calibration.\n"); + } + } + + // + // Test if we had enough yaw motion to compute a calibration + // + if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_Z_MOTION_ISSUE) + { + // + // Test if we are in + printf("\t- Not enough yaw motion to compute a valid magnetic calibration.\n"); + } + + // + // Display the number of points used to compute the magnetic calibration + // + printf("\n"); + printf("Used Points:\t%"PRIu16"\n", pMagCalibResults->numPoints); + printf("Max Points:\t%"PRIu16"\n", pMagCalibResults->maxNumPoints); + + // + // Display magnetic field deviation errors + // + printf( "\n" + "---------------------------------------------\n" + "- Magnetic field deviation report -\n" + "---------------------------------------------\n"); + + printf("\t\tMean\tStd\tMax\n"); + printf("Before\t\t%0.2f\t%0.2f\t%0.2f\n", pMagCalibResults->beforeMeanError, pMagCalibResults->beforeStdError, pMagCalibResults->beforeMaxError); + printf("After\t\t%0.2f\t%0.2f\t%0.2f\n", pMagCalibResults->afterMeanError, pMagCalibResults->afterStdError, pMagCalibResults->afterMaxError); + printf("Accuracy (deg)\t%0.2f\t%0.2f\t%0.2f\n", sbgRadToDegf(pMagCalibResults->meanAccuracy), sbgRadToDegf(pMagCalibResults->stdAccuracy), sbgRadToDegf(pMagCalibResults->maxAccuracy)); + printf("\n================ END REPORT ================\n"); +} + +/*! + * Get and print product info. + * + * \param[in] pECom SbgECom instance. + * \return SBG_NO_ERROR if successful. + */ +static SbgErrorCode getAndPrintProductInfo(SbgEComHandle *pECom) +{ + SbgErrorCode errorCode; + SbgEComDeviceInfo deviceInfo; + + assert(pECom); + + // + // Get device information + // + errorCode = sbgEComCmdGetInfo(pECom, &deviceInfo); + + // + // Display device information if no error + // + if (errorCode == SBG_NO_ERROR) + { + char calibVersionStr[32]; + char hwRevisionStr[32]; + char fmwVersionStr[32]; + + sbgVersionToStringEncoded(deviceInfo.calibationRev, calibVersionStr, sizeof(calibVersionStr)); + sbgVersionToStringEncoded(deviceInfo.hardwareRev, hwRevisionStr, sizeof(hwRevisionStr)); + sbgVersionToStringEncoded(deviceInfo.firmwareRev, fmwVersionStr, sizeof(fmwVersionStr)); + + printf(" Serial Number: %09"PRIu32"\n", deviceInfo.serialNumber); + printf(" Product Code: %s\n", deviceInfo.productCode); + printf(" Hardware Revision: %s\n", hwRevisionStr); + printf(" Firmware Version: %s\n", fmwVersionStr); + printf(" Calib. Version: %s\n", calibVersionStr); + printf("\n"); + } + else + { + SBG_LOG_WARNING(errorCode, "Unable to retrieve device information"); + } + + return errorCode; +} + +/*! + * Compute the magnetic calibration and display the results to the user. + * + * The new magnetic calibration can be applied on the device if apply is set to true. + * Computing a magnetic calibration don't reset the list of acquired points. + * + * \param[in] pHandle sbgEComHandle instance + * \param[in] mode 2D or 3D magnetic calibration mode. + * \param[in] apply Set to true to compute and then apply the magnetic calibration + * Set to false to just compute and display the magnetic calibration results. + * \return SBG_NO_ERROR if a valid magnetic calibration has been computed (and optionally applied). + */ +static SbgErrorCode computeMagneticCalibration(SbgEComHandle *pHandle, SbgEComMagCalibMode mode, bool apply) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComMagCalibResults magCalibResults; + + assert(pHandle); + + // + // Try to compute a magnetic calibration and get the results + // + errorCode = sbgEComCmdMagComputeCalib(pHandle, &magCalibResults); + + if (errorCode == SBG_NO_ERROR) + { + // + // Test if the device has computed a valid magnetic calibration + // + if (magCalibResults.quality != SBG_ECOM_MAG_CALIB_QUAL_INVALID) + { + if (apply) + { + // + // Send the new magnetic calibration data (for ELLIPSE firmware v3 and above) + // + errorCode = sbgEComCmdMagSetCalibData2(pHandle, magCalibResults.offset, magCalibResults.matrix, mode); + + if (errorCode == SBG_NO_ERROR) + { + // + // Magnetic calibration applied, save and reboot device to use it + // + errorCode = sbgEComCmdSettingsAction(pHandle, SBG_ECOM_SAVE_SETTINGS); + + if (errorCode == SBG_NO_ERROR) + { + // + // Successfully saved & applied magnetic calibration, display results + // + printf("The magnetic calibration has been saved and applied.\n"); + displayMagCalibResults(mode, &magCalibResults); + } + else + { + SBG_LOG_ERROR(errorCode, "Unable to save new magnetic calibration"); + } + } + else + { + SBG_LOG_ERROR(errorCode, "Unable to apply new magnetic calibration."); + } + } + else + { + // + // Display the magnetic calibration status + // + printf("A new magnetic calibration solution has been computed but not applied.\n"); + displayMagCalibResults(mode, &magCalibResults); + } + } + else + { + SBG_LOG_ERROR(errorCode, "Computed magnetic calibration is invalid. Please retry."); + } + } + else + { + SBG_LOG_ERROR(errorCode, "Unable to compute magnetic calibration."); + } + + return errorCode; +} + +/*! + * Process. + * + * \param[in] pInterface Interface. + * \return SBG_NO_ERROR if successful. + */ +static SbgErrorCode ellipseOnBoardMagCalibProcess(SbgInterface *pInterface) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComHandle comHandle; + SbgEComMagCalibMode mode; + int keyPressed; + + assert(pInterface); + + // + // Create the sbgECom library and associate it with the created interfaces + // + errorCode = sbgEComInit(&comHandle, pInterface); + + // + // Test that the sbgECom has been initialized + // + if (errorCode == SBG_NO_ERROR) + { + printf("Welcome to the ELLIPSE on-board magnetic calibration example.\n"); + printf("sbgECom version %s\n\n", SBG_E_COM_VERSION_STR); + + // + // Get device information and print them + // + getAndPrintProductInfo(&comHandle); + + // + // Ask the user if he wants a 2D or 3D calibration + // + mode = askCalibrationMode(); + + // + // Flush the serial interface and sbgECom work buffer as the program flow has been interrupted + // + sbgEComPurgeIncoming(&comHandle); + + // + // Start / reset the acquisition of magnetic field data + // Each time this command is called, the device is prepared to acquire a new set of magnetic field data + // You have to specify here if the magnetic field data acquisition will be used to compute a 2D or 3D calibration + // + errorCode = sbgEComCmdMagStartCalib(&comHandle, mode, SBG_ECOM_MAG_CALIB_HIGH_BW); + + // + // Make sure that the magnetic calibration has started + // + if (errorCode == SBG_NO_ERROR) + { + for (;;) + { + // + // The device is now acquiring some magnetic field data. + // Wait for a user input before computing the magnetic calibration + // + if (mode == SBG_ECOM_MAG_CALIB_MODE_3D) + { + printf("\n\nThe device is acquiring magnetic field data for a 3D calibration........\n"); + } + else + { + printf("\n\nThe device is acquiring magnetic field data for a 2D calibration.........\n"); + } + + printf( "Please rotate the device slowly...\n" + "\n" + "You can compute as many magnetic calibration as you want without loosing already acquired points:\n" + " 1) Compute a magnetic calibration but don't apply it\n" + " 2) Compute and apply a magnetic calibration then save and reboot the device\n" + " 3) Stop the current acquisition and quit\n" + "\n" + "Please enter your choice 1, 2 or 3 and press enter : "); + + // + // Wait for user choice + // + keyPressed = getUserChoice(); + + // + // Flush the serial interface and sbgECom work buffer as the program flow has been interrupted + // + sbgEComPurgeIncoming(&comHandle); + + if (keyPressed == '1') + { + computeMagneticCalibration(&comHandle, mode, false); + } + else if (keyPressed == '2') + { + computeMagneticCalibration(&comHandle, mode, true); + break; + } + else if (keyPressed == '3') + { + break; + } + } + } + else + { + SBG_LOG_ERROR(errorCode, "Unable to start the on-board magnetic calibration"); + } + + // + // Close the sbgECom library + // + sbgEComClose(&comHandle); + } + else + { + SBG_LOG_ERROR(errorCode, "Unable to initialize the sbgECom library."); + } + + return errorCode; +} + +//----------------------------------------------------------------------// +// Main program // +//----------------------------------------------------------------------// + +/*! + * Program entry point usage: ellipseOnboardMagCalib COM1 921600 + * + * \param[in] argc Number of input arguments. + * \param[in] argv Input arguments as an array of strings. + * \return EXIT_SUCCESS if successful. + */ +int main(int argc, char **argv) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgInterface sbgInterface; + int exitCode; + + SBG_UNUSED_PARAMETER(argc); + SBG_UNUSED_PARAMETER(argv); + + if (argc == 3) + { + // + // Create a serial interface to communicate with the PULSE + // + errorCode = sbgInterfaceSerialCreate(&sbgInterface, argv[1], atoi(argv[2])); + + if (errorCode == SBG_NO_ERROR) + { + errorCode = ellipseOnBoardMagCalibProcess(&sbgInterface); + + if (errorCode == SBG_NO_ERROR) + { + exitCode = EXIT_SUCCESS; + } + else + { + exitCode = EXIT_FAILURE; + } + + sbgInterfaceDestroy(&sbgInterface); + } + else + { + SBG_LOG_ERROR(errorCode, "Unable to open serial interface"); + exitCode = EXIT_FAILURE; + } + } + else + { + SBG_LOG_ERROR(SBG_ERROR, "Invalid input arguments, usage: ellipseOnboardMagCalib SERIAL_DEVICE SERIAL_BAUDRATE"); + exitCode = EXIT_FAILURE; + } + + return exitCode; +} diff --git a/examples/hpInsMinimal/src/hpInsMinimal.c b/examples/hpInsMinimal/src/hpInsMinimal.c deleted file mode 100644 index f7ef1cf..0000000 --- a/examples/hpInsMinimal/src/hpInsMinimal.c +++ /dev/null @@ -1,222 +0,0 @@ -/*! - * \file hpInsMinimal.c - * \author SBG Systems - * \date 06/05/2015 - * - * \brief C example to read euler angles from a high performance INS over Ethernet - * - * This small C example demonstrates how to initialize the sbgECom library - * with an Ethernet UDP interface and parse incoming SBG_ECOM_LOG_EKF_EULER logs. - * - * The INS has to be correctly setup to output the SBG_ECOM_LOG_EKF_EULER log - * on the selected UDP port. - * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license - * - * 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. - * - * \endlicense - */ - -// sbgCommonLib headers -#include - -// sbgECom headers -#include - -//----------------------------------------------------------------------// -//- 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] 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. - */ -static SbgErrorCode onLogReceived(SbgEComHandle *pHandle, SbgEComClass msgClass, SbgEComMsgId msg, const SbgEComLogUnion *pLogData, void *pUserArg) -{ - SBG_UNUSED_PARAMETER(pHandle); - SBG_UNUSED_PARAMETER(pUserArg); - - if (msgClass == SBG_ECOM_CLASS_LOG_ECOM_0) - { - // - // Handle separately each received data according to the log ID - // - switch (msg) - { - case SBG_ECOM_LOG_EKF_EULER: - // - // Simply display euler angles in real time - // - printf("Euler Angles: %3.1f\t%3.1f\t%3.1f\tStd Dev:%3.1f\t%3.1f\t%3.1f \n", - sbgRadToDegf(pLogData->ekfEulerData.euler[0]), sbgRadToDegf(pLogData->ekfEulerData.euler[1]), sbgRadToDegf(pLogData->ekfEulerData.euler[2]), - sbgRadToDegf(pLogData->ekfEulerData.eulerStdDev[0]), sbgRadToDegf(pLogData->ekfEulerData.eulerStdDev[1]), sbgRadToDegf(pLogData->ekfEulerData.eulerStdDev[2])); - break; - - default: - break; - } - } - - return SBG_NO_ERROR; -} - -/*! - * Execute the hpInsMinimal example given an opened and valid interface. - * - * \param[in] pInterface Interface used to communicate with the device. - * \return SBG_NO_ERROR if successful. - */ -static SbgErrorCode hpInsMinimalProcess(SbgInterface *pInterface) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgEComHandle comHandle; - - assert(pInterface); - - // - // Create the sbgECom library and associate it with the created interfaces - // - errorCode = sbgEComInit(&comHandle, pInterface); - - // - // Test that the sbgECom has been initialized - // - if (errorCode == SBG_NO_ERROR) - { - // - // Welcome message - // - printf("Welcome to the High Performance INS minimal example.\n"); - printf("sbgECom version %s\n\n", SBG_E_COM_VERSION_STR); - - printf("Euler Angles display with estimated standard deviation.\n"); - - // - // Define callbacks for received data - // - sbgEComSetReceiveLogCallback(&comHandle, onLogReceived, NULL); - - // - // Loop until the user exist - // - while (1) - { - // - // Try to read a frame - // - errorCode = sbgEComHandle(&comHandle); - - // - // Test if we have to release some CPU (no frame received) - // - if (errorCode == SBG_NOT_READY) - { - // - // Release CPU - // - sbgSleep(1); - } - else - { - SBG_LOG_ERROR(errorCode, "Unable to process incoming sbgECom logs"); - } - } - - // - // Close the sbgECom library - // - sbgEComClose(&comHandle); - } - else - { - SBG_LOG_ERROR(errorCode, "Unable to initialize the sbgECom library"); - } - - return errorCode; -} - -//----------------------------------------------------------------------// -// Main program // -//----------------------------------------------------------------------// - -/*! - * Program entry point usage: ekinoxMinimal REMOTE_ID_ADDR REMOTE_UDP_PORT HOST_UDP_PORT - * - * Establish an UDP connection with a High Performance INS product: - * REMOTE_ID_ADDR: is the INS IP address to connect to in the form 192.168.1.1 - * REMOTE_UDP_PORT: is the UDP port on which the INS is listening - * HOST_UDP_PORT: is the UDP port on which the INS is sending data - * - * \param[in] argc Number of input arguments. - * \param[in] argv Input arguments as an array of strings. - * \return EXIT_SUCCESS if successful. - */ -int main(int argc, char **argv) -{ - SbgErrorCode errorCode =SBG_NO_ERROR; - SbgInterface sbgInterface; - int exitCode; - - SBG_UNUSED_PARAMETER(argc); - SBG_UNUSED_PARAMETER(argv); - - if (argc == 4) - { - // - // Create a serial interface to communicate with the PULSE - // - errorCode = sbgInterfaceUdpCreate(&sbgInterface, sbgNetworkIpFromString(argv[1]), atoi(argv[2]), atoi(argv[3])); - - if (errorCode == SBG_NO_ERROR) - { - errorCode = hpInsMinimalProcess(&sbgInterface); - - if (errorCode == SBG_NO_ERROR) - { - exitCode = EXIT_SUCCESS; - } - else - { - exitCode = EXIT_FAILURE; - } - - sbgInterfaceDestroy(&sbgInterface); - } - else - { - SBG_LOG_ERROR(errorCode, "unable to open serial interface"); - exitCode = EXIT_FAILURE; - } - } - else - { - printf("Invalid input arguments, usage: hpInsMinimal REMOTE_ID_ADDR REMOTE_UDP_PORT HOST_UDP_PORT\n"); - exitCode = EXIT_FAILURE; - } - - return exitCode; -} diff --git a/examples/pulseMinimal/src/pulseMinimal.c b/examples/pulseMinimal/src/pulseMinimal.c deleted file mode 100644 index 9afb009..0000000 --- a/examples/pulseMinimal/src/pulseMinimal.c +++ /dev/null @@ -1,410 +0,0 @@ -/*! - * \file pulseMinimal.c - * \author SBG Systems - * \date July 27, 2021 - * - * \brief C example that simply opens a pulse interface and reads IMU data from it. - * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license - * - * 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. - * - * \endlicense - */ - -// sbgCommonLib headers -#include - -// sbgECom headers -#include - -//----------------------------------------------------------------------// -//- Constant definitions -// -//----------------------------------------------------------------------// - -/*! - * IMU short period to configure, in ms. - */ -#define PULSE_MINIMAL_IMU_SHORT_PERIOD (100) - -//----------------------------------------------------------------------// -//- Private methods -// -//----------------------------------------------------------------------// - -/*! - * Callback used to handle received logs. - * - * \param[in] pECom SbgECom instance. - * \param[in] msgClass Class of the received message. - * \param[in] msg Received message ID. - * \param[in] pLogData Received data. - * \param[in] pUserArg Optional user argument. - * \return SBG_NO_ERROR if successful. - */ -static SbgErrorCode pulseMinimalOnLogReceived(SbgEComHandle *pECom, SbgEComClass msgClass, SbgEComMsgId msg, const SbgEComLogUnion *pLogData, void *pUserArg) -{ - SBG_UNUSED_PARAMETER(pECom); - SBG_UNUSED_PARAMETER(pUserArg); - - assert(pLogData); - - if (msgClass == SBG_ECOM_CLASS_LOG_ECOM_0) - { - switch (msg) - { - case SBG_ECOM_LOG_IMU_SHORT: - printf("%" PRIu32 "\t%" PRIu16 "\t%0.3lf\t%0.12lf\t%0.12lf\t%0.12lf\t%0.12lf\t%0.12lf\t%0.12lf\n", - pLogData->imuShort.timeStamp, - pLogData->imuShort.status, - ((double)(pLogData->imuShort.temperature) / 256.0), - ((double)pLogData->imuShort.deltaVelocity[0] / 1048576.0), - ((double)pLogData->imuShort.deltaVelocity[1] / 1048576.0 ), - ((double)pLogData->imuShort.deltaVelocity[2] / 1048576.0 ), - sbgRadToDegd(((double)pLogData->imuShort.deltaAngle[0] / 67108864.0)), - sbgRadToDegd(((double)pLogData->imuShort.deltaAngle[1] / 67108864.0 )), - sbgRadToDegd(((double)pLogData->imuShort.deltaAngle[2] / 67108864.0 ))); - break; - - default: - break; - } - } - - return SBG_NO_ERROR; -} - -/*! - * Receive logs. - * - * \param[in] pECom SbgECom instance. - */ -static void pulseMinimalReceive(SbgEComHandle *pECom) -{ - assert(pECom); - - sbgEComSetReceiveLogCallback(pECom, pulseMinimalOnLogReceived, NULL); - - while (1) - { - SbgErrorCode errorCode; - - errorCode = sbgEComHandle(pECom); - - if (errorCode == SBG_NOT_READY) - { - sbgSleep(1); - } - } -} - -/*! - * Get and print product info. - * - * \param[in] pECom SbgECom instance. - * \return SBG_NO_ERROR if successful. - */ -static SbgErrorCode pulseMinimalGetAndPrintProductInfo(SbgEComHandle *pECom) -{ - SbgEComCmdApiReply reply; - SbgErrorCode errorCode; - - assert(pECom); - - sbgEComCmdApiReplyConstruct(&reply); - - errorCode = sbgEComCmdApiGet(pECom, "/api/v1/info", NULL, &reply); - - if (errorCode == SBG_NO_ERROR) - { - char calibVersion[32]; - char productCode[32]; - char serialNumber[32]; - char hwRevision[32]; - char btVersion[32]; - char fmwVersion[32]; - int ret; - - // - // This is a naive and simplistic way to parse a JSON content. - // It is recommended to use a true JSON parser. - // The cJson library can help you with this. - // - ret = sscanf(reply.pContent, "{" \ - "\"productCode\":\"%[^\"]\"," \ - "\"serialNumber\":\"%[^\"]\"," \ - "\"hwRevision\":\"%[^\"]\"," \ - "\"calibVersion\":\"%[^\"]\"," \ - "\"fmwVersion\":\"%[^\"]\"," \ - "\"btVersion\":\"%[^\"]\"," \ - "}", productCode, serialNumber, hwRevision, calibVersion, fmwVersion, btVersion); - - if (ret == 6) - { - printf(" product code: %s\n", productCode); - printf(" serial number: %s\n", serialNumber); - printf(" hardware revision: %s\n", hwRevision); - printf(" firmware version: %s\n", fmwVersion); - printf(" bootLoader version: %s\n", btVersion); - printf("calibration version: %s\n", calibVersion); - printf("\n"); - } - else - { - errorCode = SBG_INVALID_PARAMETER; - SBG_LOG_ERROR(errorCode, "Received JSON is malformed"); - } - } - else - { - SBG_LOG_ERROR(errorCode, "unable to retrieve product info"); - } - - sbgEComCmdApiReplyDestroy(&reply); - - return errorCode; -} - -/*! - * Parse an error JSON payload to extract each field. - * - * This is a naive implementation only for demonstration purposes. - * It should NOT be used in production environments. - * - * \param[in] pContent The JSON payload to parse - * \param[out] pStatus Extracted 'status' field from the JSON payload - * \param[out] pTitle Extracted 'title' field from the JSON payload - * \param[out] pDetail Extracted 'detail' field from the JSON payload. - */ -static SbgErrorCode pulseMinimalJsonParseError(const char* pContent, uint32_t *pStatus, char *pTitle, size_t titleMaxSize, char *pDetail, size_t detailMaxSize) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - char formatStr[128]; - int ret; - - assert(pContent); - assert(pStatus); - assert(pTitle); - assert(titleMaxSize > 0); - assert(pDetail); - assert(detailMaxSize > 0); - - // - // Create a sscanf format string with built in width to avoid buffer overflows - // This is a naive implementation and sscanf should not be used to correctly address buffer overflows. - // - ret = sprintf(formatStr, "{" \ - "\"status\": %%"PRIu32"," \ - "\"title\":\"%%%zu[^\"]\"," \ - "\"detail\":\"%%%zu[^\"]\"," \ - "}", titleMaxSize, detailMaxSize); - - if (ret > 0) - { - // - // This is a naive and simplistic way to parse a JSON content. - // It is recommended to use a true JSON parser. - // The cJson library can help you with this. - // - ret = sscanf(pContent, formatStr, pStatus, pTitle, pDetail); - - if (ret != 3) - { - errorCode = SBG_INVALID_PARAMETER; - SBG_LOG_ERROR(errorCode, "JSON payload malformed"); - } - } - else - { - errorCode = SBG_ERROR; - SBG_LOG_ERROR(errorCode, "Unable to generate sscanf format string"); - } - - return errorCode; -} - -/*! - * Change IMU short output rate. - * - * \param[in] pECom SbgECom instance. - * \param[in] period IMU short period. - * \return SBG_NO_ERROR if successful. - */ -static SbgErrorCode pulseMinimalChangeImuShortOutputRate(SbgEComHandle *pECom, uint32_t period) -{ - SbgEComCmdApiReply reply; - SbgErrorCode errorCode; - char periodStr[32]; - int ret; - - assert(pECom); - - sbgEComCmdApiReplyConstruct(&reply); - - // - // This is a naive and simplistic way to build a JSON string. - // It is recommended to use a true JSON generator. - // The cJson library can help you with this. - // - ret = snprintf(periodStr, sizeof(periodStr), "\"%"PRIu32"ms\"", period); - - if (ret >= 0) - { - errorCode = sbgEComCmdApiPost(pECom, "/api/v1/settings/output/comA/messages/imuShort", NULL, periodStr, &reply); - - if (errorCode == SBG_NO_ERROR) - { - // - // Check the reply status code that is based on HTTP codes (200 - OK) - // - if (reply.statusCode == 200) - { - SBG_LOG_INFO("IMU Short output period successfully configured"); - } - else - { - uint32_t status; - char title[64]; - char details[128]; - - // - // Parse an error payload using a naive and simplistic approach - // You should rather use a true JSON parser such as cJson library - // - errorCode = pulseMinimalJsonParseError(reply.pContent, &status, title, sizeof(title), details, sizeof(details)); - - if (errorCode == SBG_NO_ERROR) - { - errorCode = SBG_INVALID_PARAMETER; - SBG_LOG_ERROR(errorCode, "unable to configure IMU short log each %"PRIu32" ms", period); - - printf(" status: %"PRIu32"\n", status); - printf(" title: %s\n", title); - printf(" details: %s\n", details); - printf("\n"); - } - } - } - else - { - SBG_LOG_ERROR(errorCode, "unable to configure IMU short log at 10 Hz"); - } - - sbgEComCmdApiReplyDestroy(&reply); - } - else - { - errorCode = SBG_INVALID_PARAMETER; - SBG_LOG_ERROR(errorCode, "unable to parse period (%"PRIu32")", period); - } - - return errorCode; -} - -/*! - * Process. - * - * \param[in] pInterface Interface. - * \return SBG_NO_ERROR if successful. - */ -static SbgErrorCode pulseMinimalProcess(SbgInterface *pInterface) -{ - SbgErrorCode errorCode; - SbgEComHandle comHandle; - - assert(pInterface); - - errorCode = sbgEComInit(&comHandle, pInterface); - - if (errorCode == SBG_NO_ERROR) - { - errorCode = pulseMinimalGetAndPrintProductInfo(&comHandle); - - if (errorCode == SBG_NO_ERROR) - { - errorCode = pulseMinimalChangeImuShortOutputRate(&comHandle, PULSE_MINIMAL_IMU_SHORT_PERIOD); - } - - if (errorCode == SBG_NO_ERROR) - { - pulseMinimalReceive(&comHandle); - } - - sbgEComClose(&comHandle); - } - - return errorCode; -} - -//----------------------------------------------------------------------// -// Main program // -//----------------------------------------------------------------------// - -/*! - * Program entry point usage: pulseMinimal COM1 921600 - * - * \param[in] argc Number of input arguments. - * \param[in] argv Input arguments as an array of strings. - * \return EXIT_SUCCESS if successful. - */ -int main(int argc, char **argv) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgInterface sbgInterface; - int exitCode; - - SBG_UNUSED_PARAMETER(argc); - SBG_UNUSED_PARAMETER(argv); - - if (argc == 3) - { - // - // Create a serial interface to communicate with the PULSE - // - errorCode = sbgInterfaceSerialCreate(&sbgInterface, argv[1], atoi(argv[2])); - - if (errorCode == SBG_NO_ERROR) - { - errorCode = pulseMinimalProcess(&sbgInterface); - - if (errorCode == SBG_NO_ERROR) - { - exitCode = EXIT_SUCCESS; - } - else - { - exitCode = EXIT_FAILURE; - } - - sbgInterfaceDestroy(&sbgInterface); - } - else - { - SBG_LOG_ERROR(errorCode, "unable to open serial interface"); - exitCode = EXIT_FAILURE; - } - } - else - { - printf("Invalid input arguments, usage: pulseMinimal SERIAL_DEVICE SERIAL_BAUDRATE\n"); - exitCode = EXIT_FAILURE; - } - - return exitCode; -} diff --git a/examples/sbgEComExample/src/cJSON.c b/examples/sbgEComExample/src/cJSON.c new file mode 100644 index 0000000..cac1164 --- /dev/null +++ b/examples/sbgEComExample/src/cJSON.c @@ -0,0 +1,3143 @@ +/* + Copyright (c) 2009-2017 Dave Gamble and cJSON contributors + + 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. +*/ + +/* cJSON */ +/* JSON parser in C. */ + +/* disable warnings about old C89 functions in MSVC */ +#if !defined(_CRT_SECURE_NO_DEPRECATE) && defined(_MSC_VER) +#define _CRT_SECURE_NO_DEPRECATE +#endif + +#ifdef __GNUC__ +#pragma GCC visibility push(default) +#endif +#if defined(_MSC_VER) +#pragma warning (push) +/* disable warning about single line comments in system headers */ +#pragma warning (disable : 4001) +#endif + +#include +#include +#include +#include +#include +#include +#include + +#ifdef ENABLE_LOCALES +#include +#endif + +#if defined(_MSC_VER) +#pragma warning (pop) +#endif +#ifdef __GNUC__ +#pragma GCC visibility pop +#endif + +#include "cJSON.h" + +/* define our own boolean type */ +#ifdef true +#undef true +#endif +#define true ((cJSON_bool)1) + +#ifdef false +#undef false +#endif +#define false ((cJSON_bool)0) + +/* define isnan and isinf for ANSI C, if in C99 or above, isnan and isinf has been defined in math.h */ +#ifndef isinf +#define isinf(d) (isnan((d - d)) && !isnan(d)) +#endif +#ifndef isnan +#define isnan(d) (d != d) +#endif + +#ifndef NAN +#ifdef _WIN32 +#define NAN sqrt(-1.0) +#else +#define NAN 0.0/0.0 +#endif +#endif + +typedef struct { + const unsigned char *json; + size_t position; +} error; +static error global_error = { NULL, 0 }; + +CJSON_PUBLIC(const char *) cJSON_GetErrorPtr(void) +{ + return (const char*) (global_error.json + global_error.position); +} + +CJSON_PUBLIC(char *) cJSON_GetStringValue(const cJSON * const item) +{ + if (!cJSON_IsString(item)) + { + return NULL; + } + + return item->valuestring; +} + +CJSON_PUBLIC(double) cJSON_GetNumberValue(const cJSON * const item) +{ + if (!cJSON_IsNumber(item)) + { + return (double) NAN; + } + + return item->valuedouble; +} + +/* This is a safeguard to prevent copy-pasters from using incompatible C and header files */ +#if (CJSON_VERSION_MAJOR != 1) || (CJSON_VERSION_MINOR != 7) || (CJSON_VERSION_PATCH != 18) + #error cJSON.h and cJSON.c have different versions. Make sure that both have the same. +#endif + +CJSON_PUBLIC(const char*) cJSON_Version(void) +{ + static char version[15]; + sprintf(version, "%i.%i.%i", CJSON_VERSION_MAJOR, CJSON_VERSION_MINOR, CJSON_VERSION_PATCH); + + return version; +} + +/* Case insensitive string comparison, doesn't consider two NULL pointers equal though */ +static int case_insensitive_strcmp(const unsigned char *string1, const unsigned char *string2) +{ + if ((string1 == NULL) || (string2 == NULL)) + { + return 1; + } + + if (string1 == string2) + { + return 0; + } + + for(; tolower(*string1) == tolower(*string2); (void)string1++, string2++) + { + if (*string1 == '\0') + { + return 0; + } + } + + return tolower(*string1) - tolower(*string2); +} + +typedef struct internal_hooks +{ + void *(CJSON_CDECL *allocate)(size_t size); + void (CJSON_CDECL *deallocate)(void *pointer); + void *(CJSON_CDECL *reallocate)(void *pointer, size_t size); +} internal_hooks; + +#if defined(_MSC_VER) +/* work around MSVC error C2322: '...' address of dllimport '...' is not static */ +static void * CJSON_CDECL internal_malloc(size_t size) +{ + return malloc(size); +} +static void CJSON_CDECL internal_free(void *pointer) +{ + free(pointer); +} +static void * CJSON_CDECL internal_realloc(void *pointer, size_t size) +{ + return realloc(pointer, size); +} +#else +#define internal_malloc malloc +#define internal_free free +#define internal_realloc realloc +#endif + +/* strlen of character literals resolved at compile time */ +#define static_strlen(string_literal) (sizeof(string_literal) - sizeof("")) + +static internal_hooks global_hooks = { internal_malloc, internal_free, internal_realloc }; + +static unsigned char* cJSON_strdup(const unsigned char* string, const internal_hooks * const hooks) +{ + size_t length = 0; + unsigned char *copy = NULL; + + if (string == NULL) + { + return NULL; + } + + length = strlen((const char*)string) + sizeof(""); + copy = (unsigned char*)hooks->allocate(length); + if (copy == NULL) + { + return NULL; + } + memcpy(copy, string, length); + + return copy; +} + +CJSON_PUBLIC(void) cJSON_InitHooks(cJSON_Hooks* hooks) +{ + if (hooks == NULL) + { + /* Reset hooks */ + global_hooks.allocate = malloc; + global_hooks.deallocate = free; + global_hooks.reallocate = realloc; + return; + } + + global_hooks.allocate = malloc; + if (hooks->malloc_fn != NULL) + { + global_hooks.allocate = hooks->malloc_fn; + } + + global_hooks.deallocate = free; + if (hooks->free_fn != NULL) + { + global_hooks.deallocate = hooks->free_fn; + } + + /* use realloc only if both free and malloc are used */ + global_hooks.reallocate = NULL; + if ((global_hooks.allocate == malloc) && (global_hooks.deallocate == free)) + { + global_hooks.reallocate = realloc; + } +} + +/* Internal constructor. */ +static cJSON *cJSON_New_Item(const internal_hooks * const hooks) +{ + cJSON* node = (cJSON*)hooks->allocate(sizeof(cJSON)); + if (node) + { + memset(node, '\0', sizeof(cJSON)); + } + + return node; +} + +/* Delete a cJSON structure. */ +CJSON_PUBLIC(void) cJSON_Delete(cJSON *item) +{ + cJSON *next = NULL; + while (item != NULL) + { + next = item->next; + if (!(item->type & cJSON_IsReference) && (item->child != NULL)) + { + cJSON_Delete(item->child); + } + if (!(item->type & cJSON_IsReference) && (item->valuestring != NULL)) + { + global_hooks.deallocate(item->valuestring); + item->valuestring = NULL; + } + if (!(item->type & cJSON_StringIsConst) && (item->string != NULL)) + { + global_hooks.deallocate(item->string); + item->string = NULL; + } + global_hooks.deallocate(item); + item = next; + } +} + +/* get the decimal point character of the current locale */ +static unsigned char get_decimal_point(void) +{ +#ifdef ENABLE_LOCALES + struct lconv *lconv = localeconv(); + return (unsigned char) lconv->decimal_point[0]; +#else + return '.'; +#endif +} + +typedef struct +{ + const unsigned char *content; + size_t length; + size_t offset; + size_t depth; /* How deeply nested (in arrays/objects) is the input at the current offset. */ + internal_hooks hooks; +} parse_buffer; + +/* check if the given size is left to read in a given parse buffer (starting with 1) */ +#define can_read(buffer, size) ((buffer != NULL) && (((buffer)->offset + size) <= (buffer)->length)) +/* check if the buffer can be accessed at the given index (starting with 0) */ +#define can_access_at_index(buffer, index) ((buffer != NULL) && (((buffer)->offset + index) < (buffer)->length)) +#define cannot_access_at_index(buffer, index) (!can_access_at_index(buffer, index)) +/* get a pointer to the buffer at the position */ +#define buffer_at_offset(buffer) ((buffer)->content + (buffer)->offset) + +/* Parse the input text to generate a number, and populate the result into item. */ +static cJSON_bool parse_number(cJSON * const item, parse_buffer * const input_buffer) +{ + double number = 0; + unsigned char *after_end = NULL; + unsigned char number_c_string[64]; + unsigned char decimal_point = get_decimal_point(); + size_t i = 0; + + if ((input_buffer == NULL) || (input_buffer->content == NULL)) + { + return false; + } + + /* copy the number into a temporary buffer and replace '.' with the decimal point + * of the current locale (for strtod) + * This also takes care of '\0' not necessarily being available for marking the end of the input */ + for (i = 0; (i < (sizeof(number_c_string) - 1)) && can_access_at_index(input_buffer, i); i++) + { + switch (buffer_at_offset(input_buffer)[i]) + { + case '0': + case '1': + case '2': + case '3': + case '4': + case '5': + case '6': + case '7': + case '8': + case '9': + case '+': + case '-': + case 'e': + case 'E': + number_c_string[i] = buffer_at_offset(input_buffer)[i]; + break; + + case '.': + number_c_string[i] = decimal_point; + break; + + default: + goto loop_end; + } + } +loop_end: + number_c_string[i] = '\0'; + + number = strtod((const char*)number_c_string, (char**)&after_end); + if (number_c_string == after_end) + { + return false; /* parse_error */ + } + + item->valuedouble = number; + + /* use saturation in case of overflow */ + if (number >= INT_MAX) + { + item->valueint = INT_MAX; + } + else if (number <= (double)INT_MIN) + { + item->valueint = INT_MIN; + } + else + { + item->valueint = (int)number; + } + + item->type = cJSON_Number; + + input_buffer->offset += (size_t)(after_end - number_c_string); + return true; +} + +/* don't ask me, but the original cJSON_SetNumberValue returns an integer or double */ +CJSON_PUBLIC(double) cJSON_SetNumberHelper(cJSON *object, double number) +{ + if (number >= INT_MAX) + { + object->valueint = INT_MAX; + } + else if (number <= (double)INT_MIN) + { + object->valueint = INT_MIN; + } + else + { + object->valueint = (int)number; + } + + return object->valuedouble = number; +} + +/* Note: when passing a NULL valuestring, cJSON_SetValuestring treats this as an error and return NULL */ +CJSON_PUBLIC(char*) cJSON_SetValuestring(cJSON *object, const char *valuestring) +{ + char *copy = NULL; + /* if object's type is not cJSON_String or is cJSON_IsReference, it should not set valuestring */ + if ((object == NULL) || !(object->type & cJSON_String) || (object->type & cJSON_IsReference)) + { + return NULL; + } + /* return NULL if the object is corrupted or valuestring is NULL */ + if (object->valuestring == NULL || valuestring == NULL) + { + return NULL; + } + if (strlen(valuestring) <= strlen(object->valuestring)) + { + strcpy(object->valuestring, valuestring); + return object->valuestring; + } + copy = (char*) cJSON_strdup((const unsigned char*)valuestring, &global_hooks); + if (copy == NULL) + { + return NULL; + } + if (object->valuestring != NULL) + { + cJSON_free(object->valuestring); + } + object->valuestring = copy; + + return copy; +} + +typedef struct +{ + unsigned char *buffer; + size_t length; + size_t offset; + size_t depth; /* current nesting depth (for formatted printing) */ + cJSON_bool noalloc; + cJSON_bool format; /* is this print a formatted print */ + internal_hooks hooks; +} printbuffer; + +/* realloc printbuffer if necessary to have at least "needed" bytes more */ +static unsigned char* ensure(printbuffer * const p, size_t needed) +{ + unsigned char *newbuffer = NULL; + size_t newsize = 0; + + if ((p == NULL) || (p->buffer == NULL)) + { + return NULL; + } + + if ((p->length > 0) && (p->offset >= p->length)) + { + /* make sure that offset is valid */ + return NULL; + } + + if (needed > INT_MAX) + { + /* sizes bigger than INT_MAX are currently not supported */ + return NULL; + } + + needed += p->offset + 1; + if (needed <= p->length) + { + return p->buffer + p->offset; + } + + if (p->noalloc) { + return NULL; + } + + /* calculate new buffer size */ + if (needed > (INT_MAX / 2)) + { + /* overflow of int, use INT_MAX if possible */ + if (needed <= INT_MAX) + { + newsize = INT_MAX; + } + else + { + return NULL; + } + } + else + { + newsize = needed * 2; + } + + if (p->hooks.reallocate != NULL) + { + /* reallocate with realloc if available */ + newbuffer = (unsigned char*)p->hooks.reallocate(p->buffer, newsize); + if (newbuffer == NULL) + { + p->hooks.deallocate(p->buffer); + p->length = 0; + p->buffer = NULL; + + return NULL; + } + } + else + { + /* otherwise reallocate manually */ + newbuffer = (unsigned char*)p->hooks.allocate(newsize); + if (!newbuffer) + { + p->hooks.deallocate(p->buffer); + p->length = 0; + p->buffer = NULL; + + return NULL; + } + + memcpy(newbuffer, p->buffer, p->offset + 1); + p->hooks.deallocate(p->buffer); + } + p->length = newsize; + p->buffer = newbuffer; + + return newbuffer + p->offset; +} + +/* calculate the new length of the string in a printbuffer and update the offset */ +static void update_offset(printbuffer * const buffer) +{ + const unsigned char *buffer_pointer = NULL; + if ((buffer == NULL) || (buffer->buffer == NULL)) + { + return; + } + buffer_pointer = buffer->buffer + buffer->offset; + + buffer->offset += strlen((const char*)buffer_pointer); +} + +/* securely comparison of floating-point variables */ +static cJSON_bool compare_double(double a, double b) +{ + double maxVal = fabs(a) > fabs(b) ? fabs(a) : fabs(b); + return (fabs(a - b) <= maxVal * DBL_EPSILON); +} + +/* Render the number nicely from the given item into a string. */ +static cJSON_bool print_number(const cJSON * const item, printbuffer * const output_buffer) +{ + unsigned char *output_pointer = NULL; + double d = item->valuedouble; + int length = 0; + size_t i = 0; + unsigned char number_buffer[26] = {0}; /* temporary buffer to print the number into */ + unsigned char decimal_point = get_decimal_point(); + double test = 0.0; + + if (output_buffer == NULL) + { + return false; + } + + /* This checks for NaN and Infinity */ + if (isnan(d) || isinf(d)) + { + length = sprintf((char*)number_buffer, "null"); + } + else if(d == (double)item->valueint) + { + length = sprintf((char*)number_buffer, "%d", item->valueint); + } + else + { + /* Try 15 decimal places of precision to avoid nonsignificant nonzero digits */ + length = sprintf((char*)number_buffer, "%1.15g", d); + + /* Check whether the original double can be recovered */ + if ((sscanf((char*)number_buffer, "%lg", &test) != 1) || !compare_double((double)test, d)) + { + /* If not, print with 17 decimal places of precision */ + length = sprintf((char*)number_buffer, "%1.17g", d); + } + } + + /* sprintf failed or buffer overrun occurred */ + if ((length < 0) || (length > (int)(sizeof(number_buffer) - 1))) + { + return false; + } + + /* reserve appropriate space in the output */ + output_pointer = ensure(output_buffer, (size_t)length + sizeof("")); + if (output_pointer == NULL) + { + return false; + } + + /* copy the printed number to the output and replace locale + * dependent decimal point with '.' */ + for (i = 0; i < ((size_t)length); i++) + { + if (number_buffer[i] == decimal_point) + { + output_pointer[i] = '.'; + continue; + } + + output_pointer[i] = number_buffer[i]; + } + output_pointer[i] = '\0'; + + output_buffer->offset += (size_t)length; + + return true; +} + +/* parse 4 digit hexadecimal number */ +static unsigned parse_hex4(const unsigned char * const input) +{ + unsigned int h = 0; + size_t i = 0; + + for (i = 0; i < 4; i++) + { + /* parse digit */ + if ((input[i] >= '0') && (input[i] <= '9')) + { + h += (unsigned int) input[i] - '0'; + } + else if ((input[i] >= 'A') && (input[i] <= 'F')) + { + h += (unsigned int) 10 + input[i] - 'A'; + } + else if ((input[i] >= 'a') && (input[i] <= 'f')) + { + h += (unsigned int) 10 + input[i] - 'a'; + } + else /* invalid */ + { + return 0; + } + + if (i < 3) + { + /* shift left to make place for the next nibble */ + h = h << 4; + } + } + + return h; +} + +/* converts a UTF-16 literal to UTF-8 + * A literal can be one or two sequences of the form \uXXXX */ +static unsigned char utf16_literal_to_utf8(const unsigned char * const input_pointer, const unsigned char * const input_end, unsigned char **output_pointer) +{ + long unsigned int codepoint = 0; + unsigned int first_code = 0; + const unsigned char *first_sequence = input_pointer; + unsigned char utf8_length = 0; + unsigned char utf8_position = 0; + unsigned char sequence_length = 0; + unsigned char first_byte_mark = 0; + + if ((input_end - first_sequence) < 6) + { + /* input ends unexpectedly */ + goto fail; + } + + /* get the first utf16 sequence */ + first_code = parse_hex4(first_sequence + 2); + + /* check that the code is valid */ + if (((first_code >= 0xDC00) && (first_code <= 0xDFFF))) + { + goto fail; + } + + /* UTF16 surrogate pair */ + if ((first_code >= 0xD800) && (first_code <= 0xDBFF)) + { + const unsigned char *second_sequence = first_sequence + 6; + unsigned int second_code = 0; + sequence_length = 12; /* \uXXXX\uXXXX */ + + if ((input_end - second_sequence) < 6) + { + /* input ends unexpectedly */ + goto fail; + } + + if ((second_sequence[0] != '\\') || (second_sequence[1] != 'u')) + { + /* missing second half of the surrogate pair */ + goto fail; + } + + /* get the second utf16 sequence */ + second_code = parse_hex4(second_sequence + 2); + /* check that the code is valid */ + if ((second_code < 0xDC00) || (second_code > 0xDFFF)) + { + /* invalid second half of the surrogate pair */ + goto fail; + } + + + /* calculate the unicode codepoint from the surrogate pair */ + codepoint = 0x10000 + (((first_code & 0x3FF) << 10) | (second_code & 0x3FF)); + } + else + { + sequence_length = 6; /* \uXXXX */ + codepoint = first_code; + } + + /* encode as UTF-8 + * takes at maximum 4 bytes to encode: + * 11110xxx 10xxxxxx 10xxxxxx 10xxxxxx */ + if (codepoint < 0x80) + { + /* normal ascii, encoding 0xxxxxxx */ + utf8_length = 1; + } + else if (codepoint < 0x800) + { + /* two bytes, encoding 110xxxxx 10xxxxxx */ + utf8_length = 2; + first_byte_mark = 0xC0; /* 11000000 */ + } + else if (codepoint < 0x10000) + { + /* three bytes, encoding 1110xxxx 10xxxxxx 10xxxxxx */ + utf8_length = 3; + first_byte_mark = 0xE0; /* 11100000 */ + } + else if (codepoint <= 0x10FFFF) + { + /* four bytes, encoding 1110xxxx 10xxxxxx 10xxxxxx 10xxxxxx */ + utf8_length = 4; + first_byte_mark = 0xF0; /* 11110000 */ + } + else + { + /* invalid unicode codepoint */ + goto fail; + } + + /* encode as utf8 */ + for (utf8_position = (unsigned char)(utf8_length - 1); utf8_position > 0; utf8_position--) + { + /* 10xxxxxx */ + (*output_pointer)[utf8_position] = (unsigned char)((codepoint | 0x80) & 0xBF); + codepoint >>= 6; + } + /* encode first byte */ + if (utf8_length > 1) + { + (*output_pointer)[0] = (unsigned char)((codepoint | first_byte_mark) & 0xFF); + } + else + { + (*output_pointer)[0] = (unsigned char)(codepoint & 0x7F); + } + + *output_pointer += utf8_length; + + return sequence_length; + +fail: + return 0; +} + +/* Parse the input text into an unescaped cinput, and populate item. */ +static cJSON_bool parse_string(cJSON * const item, parse_buffer * const input_buffer) +{ + const unsigned char *input_pointer = buffer_at_offset(input_buffer) + 1; + const unsigned char *input_end = buffer_at_offset(input_buffer) + 1; + unsigned char *output_pointer = NULL; + unsigned char *output = NULL; + + /* not a string */ + if (buffer_at_offset(input_buffer)[0] != '\"') + { + goto fail; + } + + { + /* calculate approximate size of the output (overestimate) */ + size_t allocation_length = 0; + size_t skipped_bytes = 0; + while (((size_t)(input_end - input_buffer->content) < input_buffer->length) && (*input_end != '\"')) + { + /* is escape sequence */ + if (input_end[0] == '\\') + { + if ((size_t)(input_end + 1 - input_buffer->content) >= input_buffer->length) + { + /* prevent buffer overflow when last input character is a backslash */ + goto fail; + } + skipped_bytes++; + input_end++; + } + input_end++; + } + if (((size_t)(input_end - input_buffer->content) >= input_buffer->length) || (*input_end != '\"')) + { + goto fail; /* string ended unexpectedly */ + } + + /* This is at most how much we need for the output */ + allocation_length = (size_t) (input_end - buffer_at_offset(input_buffer)) - skipped_bytes; + output = (unsigned char*)input_buffer->hooks.allocate(allocation_length + sizeof("")); + if (output == NULL) + { + goto fail; /* allocation failure */ + } + } + + output_pointer = output; + /* loop through the string literal */ + while (input_pointer < input_end) + { + if (*input_pointer != '\\') + { + *output_pointer++ = *input_pointer++; + } + /* escape sequence */ + else + { + unsigned char sequence_length = 2; + if ((input_end - input_pointer) < 1) + { + goto fail; + } + + switch (input_pointer[1]) + { + case 'b': + *output_pointer++ = '\b'; + break; + case 'f': + *output_pointer++ = '\f'; + break; + case 'n': + *output_pointer++ = '\n'; + break; + case 'r': + *output_pointer++ = '\r'; + break; + case 't': + *output_pointer++ = '\t'; + break; + case '\"': + case '\\': + case '/': + *output_pointer++ = input_pointer[1]; + break; + + /* UTF-16 literal */ + case 'u': + sequence_length = utf16_literal_to_utf8(input_pointer, input_end, &output_pointer); + if (sequence_length == 0) + { + /* failed to convert UTF16-literal to UTF-8 */ + goto fail; + } + break; + + default: + goto fail; + } + input_pointer += sequence_length; + } + } + + /* zero terminate the output */ + *output_pointer = '\0'; + + item->type = cJSON_String; + item->valuestring = (char*)output; + + input_buffer->offset = (size_t) (input_end - input_buffer->content); + input_buffer->offset++; + + return true; + +fail: + if (output != NULL) + { + input_buffer->hooks.deallocate(output); + output = NULL; + } + + if (input_pointer != NULL) + { + input_buffer->offset = (size_t)(input_pointer - input_buffer->content); + } + + return false; +} + +/* Render the cstring provided to an escaped version that can be printed. */ +static cJSON_bool print_string_ptr(const unsigned char * const input, printbuffer * const output_buffer) +{ + const unsigned char *input_pointer = NULL; + unsigned char *output = NULL; + unsigned char *output_pointer = NULL; + size_t output_length = 0; + /* numbers of additional characters needed for escaping */ + size_t escape_characters = 0; + + if (output_buffer == NULL) + { + return false; + } + + /* empty string */ + if (input == NULL) + { + output = ensure(output_buffer, sizeof("\"\"")); + if (output == NULL) + { + return false; + } + strcpy((char*)output, "\"\""); + + return true; + } + + /* set "flag" to 1 if something needs to be escaped */ + for (input_pointer = input; *input_pointer; input_pointer++) + { + switch (*input_pointer) + { + case '\"': + case '\\': + case '\b': + case '\f': + case '\n': + case '\r': + case '\t': + /* one character escape sequence */ + escape_characters++; + break; + default: + if (*input_pointer < 32) + { + /* UTF-16 escape sequence uXXXX */ + escape_characters += 5; + } + break; + } + } + output_length = (size_t)(input_pointer - input) + escape_characters; + + output = ensure(output_buffer, output_length + sizeof("\"\"")); + if (output == NULL) + { + return false; + } + + /* no characters have to be escaped */ + if (escape_characters == 0) + { + output[0] = '\"'; + memcpy(output + 1, input, output_length); + output[output_length + 1] = '\"'; + output[output_length + 2] = '\0'; + + return true; + } + + output[0] = '\"'; + output_pointer = output + 1; + /* copy the string */ + for (input_pointer = input; *input_pointer != '\0'; (void)input_pointer++, output_pointer++) + { + if ((*input_pointer > 31) && (*input_pointer != '\"') && (*input_pointer != '\\')) + { + /* normal character, copy */ + *output_pointer = *input_pointer; + } + else + { + /* character needs to be escaped */ + *output_pointer++ = '\\'; + switch (*input_pointer) + { + case '\\': + *output_pointer = '\\'; + break; + case '\"': + *output_pointer = '\"'; + break; + case '\b': + *output_pointer = 'b'; + break; + case '\f': + *output_pointer = 'f'; + break; + case '\n': + *output_pointer = 'n'; + break; + case '\r': + *output_pointer = 'r'; + break; + case '\t': + *output_pointer = 't'; + break; + default: + /* escape and print as unicode codepoint */ + sprintf((char*)output_pointer, "u%04x", *input_pointer); + output_pointer += 4; + break; + } + } + } + output[output_length + 1] = '\"'; + output[output_length + 2] = '\0'; + + return true; +} + +/* Invoke print_string_ptr (which is useful) on an item. */ +static cJSON_bool print_string(const cJSON * const item, printbuffer * const p) +{ + return print_string_ptr((unsigned char*)item->valuestring, p); +} + +/* Predeclare these prototypes. */ +static cJSON_bool parse_value(cJSON * const item, parse_buffer * const input_buffer); +static cJSON_bool print_value(const cJSON * const item, printbuffer * const output_buffer); +static cJSON_bool parse_array(cJSON * const item, parse_buffer * const input_buffer); +static cJSON_bool print_array(const cJSON * const item, printbuffer * const output_buffer); +static cJSON_bool parse_object(cJSON * const item, parse_buffer * const input_buffer); +static cJSON_bool print_object(const cJSON * const item, printbuffer * const output_buffer); + +/* Utility to jump whitespace and cr/lf */ +static parse_buffer *buffer_skip_whitespace(parse_buffer * const buffer) +{ + if ((buffer == NULL) || (buffer->content == NULL)) + { + return NULL; + } + + if (cannot_access_at_index(buffer, 0)) + { + return buffer; + } + + while (can_access_at_index(buffer, 0) && (buffer_at_offset(buffer)[0] <= 32)) + { + buffer->offset++; + } + + if (buffer->offset == buffer->length) + { + buffer->offset--; + } + + return buffer; +} + +/* skip the UTF-8 BOM (byte order mark) if it is at the beginning of a buffer */ +static parse_buffer *skip_utf8_bom(parse_buffer * const buffer) +{ + if ((buffer == NULL) || (buffer->content == NULL) || (buffer->offset != 0)) + { + return NULL; + } + + if (can_access_at_index(buffer, 4) && (strncmp((const char*)buffer_at_offset(buffer), "\xEF\xBB\xBF", 3) == 0)) + { + buffer->offset += 3; + } + + return buffer; +} + +CJSON_PUBLIC(cJSON *) cJSON_ParseWithOpts(const char *value, const char **return_parse_end, cJSON_bool require_null_terminated) +{ + size_t buffer_length; + + if (NULL == value) + { + return NULL; + } + + /* Adding null character size due to require_null_terminated. */ + buffer_length = strlen(value) + sizeof(""); + + return cJSON_ParseWithLengthOpts(value, buffer_length, return_parse_end, require_null_terminated); +} + +/* Parse an object - create a new root, and populate. */ +CJSON_PUBLIC(cJSON *) cJSON_ParseWithLengthOpts(const char *value, size_t buffer_length, const char **return_parse_end, cJSON_bool require_null_terminated) +{ + parse_buffer buffer = { 0, 0, 0, 0, { 0, 0, 0 } }; + cJSON *item = NULL; + + /* reset error position */ + global_error.json = NULL; + global_error.position = 0; + + if (value == NULL || 0 == buffer_length) + { + goto fail; + } + + buffer.content = (const unsigned char*)value; + buffer.length = buffer_length; + buffer.offset = 0; + buffer.hooks = global_hooks; + + item = cJSON_New_Item(&global_hooks); + if (item == NULL) /* memory fail */ + { + goto fail; + } + + if (!parse_value(item, buffer_skip_whitespace(skip_utf8_bom(&buffer)))) + { + /* parse failure. ep is set. */ + goto fail; + } + + /* if we require null-terminated JSON without appended garbage, skip and then check for a null terminator */ + if (require_null_terminated) + { + buffer_skip_whitespace(&buffer); + if ((buffer.offset >= buffer.length) || buffer_at_offset(&buffer)[0] != '\0') + { + goto fail; + } + } + if (return_parse_end) + { + *return_parse_end = (const char*)buffer_at_offset(&buffer); + } + + return item; + +fail: + if (item != NULL) + { + cJSON_Delete(item); + } + + if (value != NULL) + { + error local_error; + local_error.json = (const unsigned char*)value; + local_error.position = 0; + + if (buffer.offset < buffer.length) + { + local_error.position = buffer.offset; + } + else if (buffer.length > 0) + { + local_error.position = buffer.length - 1; + } + + if (return_parse_end != NULL) + { + *return_parse_end = (const char*)local_error.json + local_error.position; + } + + global_error = local_error; + } + + return NULL; +} + +/* Default options for cJSON_Parse */ +CJSON_PUBLIC(cJSON *) cJSON_Parse(const char *value) +{ + return cJSON_ParseWithOpts(value, 0, 0); +} + +CJSON_PUBLIC(cJSON *) cJSON_ParseWithLength(const char *value, size_t buffer_length) +{ + return cJSON_ParseWithLengthOpts(value, buffer_length, 0, 0); +} + +#define cjson_min(a, b) (((a) < (b)) ? (a) : (b)) + +static unsigned char *print(const cJSON * const item, cJSON_bool format, const internal_hooks * const hooks) +{ + static const size_t default_buffer_size = 256; + printbuffer buffer[1]; + unsigned char *printed = NULL; + + memset(buffer, 0, sizeof(buffer)); + + /* create buffer */ + buffer->buffer = (unsigned char*) hooks->allocate(default_buffer_size); + buffer->length = default_buffer_size; + buffer->format = format; + buffer->hooks = *hooks; + if (buffer->buffer == NULL) + { + goto fail; + } + + /* print the value */ + if (!print_value(item, buffer)) + { + goto fail; + } + update_offset(buffer); + + /* check if reallocate is available */ + if (hooks->reallocate != NULL) + { + printed = (unsigned char*) hooks->reallocate(buffer->buffer, buffer->offset + 1); + if (printed == NULL) { + goto fail; + } + buffer->buffer = NULL; + } + else /* otherwise copy the JSON over to a new buffer */ + { + printed = (unsigned char*) hooks->allocate(buffer->offset + 1); + if (printed == NULL) + { + goto fail; + } + memcpy(printed, buffer->buffer, cjson_min(buffer->length, buffer->offset + 1)); + printed[buffer->offset] = '\0'; /* just to be sure */ + + /* free the buffer */ + hooks->deallocate(buffer->buffer); + buffer->buffer = NULL; + } + + return printed; + +fail: + if (buffer->buffer != NULL) + { + hooks->deallocate(buffer->buffer); + buffer->buffer = NULL; + } + + if (printed != NULL) + { + hooks->deallocate(printed); + printed = NULL; + } + + return NULL; +} + +/* Render a cJSON item/entity/structure to text. */ +CJSON_PUBLIC(char *) cJSON_Print(const cJSON *item) +{ + return (char*)print(item, true, &global_hooks); +} + +CJSON_PUBLIC(char *) cJSON_PrintUnformatted(const cJSON *item) +{ + return (char*)print(item, false, &global_hooks); +} + +CJSON_PUBLIC(char *) cJSON_PrintBuffered(const cJSON *item, int prebuffer, cJSON_bool fmt) +{ + printbuffer p = { 0, 0, 0, 0, 0, 0, { 0, 0, 0 } }; + + if (prebuffer < 0) + { + return NULL; + } + + p.buffer = (unsigned char*)global_hooks.allocate((size_t)prebuffer); + if (!p.buffer) + { + return NULL; + } + + p.length = (size_t)prebuffer; + p.offset = 0; + p.noalloc = false; + p.format = fmt; + p.hooks = global_hooks; + + if (!print_value(item, &p)) + { + global_hooks.deallocate(p.buffer); + p.buffer = NULL; + return NULL; + } + + return (char*)p.buffer; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_PrintPreallocated(cJSON *item, char *buffer, const int length, const cJSON_bool format) +{ + printbuffer p = { 0, 0, 0, 0, 0, 0, { 0, 0, 0 } }; + + if ((length < 0) || (buffer == NULL)) + { + return false; + } + + p.buffer = (unsigned char*)buffer; + p.length = (size_t)length; + p.offset = 0; + p.noalloc = true; + p.format = format; + p.hooks = global_hooks; + + return print_value(item, &p); +} + +/* Parser core - when encountering text, process appropriately. */ +static cJSON_bool parse_value(cJSON * const item, parse_buffer * const input_buffer) +{ + if ((input_buffer == NULL) || (input_buffer->content == NULL)) + { + return false; /* no input */ + } + + /* parse the different types of values */ + /* null */ + if (can_read(input_buffer, 4) && (strncmp((const char*)buffer_at_offset(input_buffer), "null", 4) == 0)) + { + item->type = cJSON_NULL; + input_buffer->offset += 4; + return true; + } + /* false */ + if (can_read(input_buffer, 5) && (strncmp((const char*)buffer_at_offset(input_buffer), "false", 5) == 0)) + { + item->type = cJSON_False; + input_buffer->offset += 5; + return true; + } + /* true */ + if (can_read(input_buffer, 4) && (strncmp((const char*)buffer_at_offset(input_buffer), "true", 4) == 0)) + { + item->type = cJSON_True; + item->valueint = 1; + input_buffer->offset += 4; + return true; + } + /* string */ + if (can_access_at_index(input_buffer, 0) && (buffer_at_offset(input_buffer)[0] == '\"')) + { + return parse_string(item, input_buffer); + } + /* number */ + if (can_access_at_index(input_buffer, 0) && ((buffer_at_offset(input_buffer)[0] == '-') || ((buffer_at_offset(input_buffer)[0] >= '0') && (buffer_at_offset(input_buffer)[0] <= '9')))) + { + return parse_number(item, input_buffer); + } + /* array */ + if (can_access_at_index(input_buffer, 0) && (buffer_at_offset(input_buffer)[0] == '[')) + { + return parse_array(item, input_buffer); + } + /* object */ + if (can_access_at_index(input_buffer, 0) && (buffer_at_offset(input_buffer)[0] == '{')) + { + return parse_object(item, input_buffer); + } + + return false; +} + +/* Render a value to text. */ +static cJSON_bool print_value(const cJSON * const item, printbuffer * const output_buffer) +{ + unsigned char *output = NULL; + + if ((item == NULL) || (output_buffer == NULL)) + { + return false; + } + + switch ((item->type) & 0xFF) + { + case cJSON_NULL: + output = ensure(output_buffer, 5); + if (output == NULL) + { + return false; + } + strcpy((char*)output, "null"); + return true; + + case cJSON_False: + output = ensure(output_buffer, 6); + if (output == NULL) + { + return false; + } + strcpy((char*)output, "false"); + return true; + + case cJSON_True: + output = ensure(output_buffer, 5); + if (output == NULL) + { + return false; + } + strcpy((char*)output, "true"); + return true; + + case cJSON_Number: + return print_number(item, output_buffer); + + case cJSON_Raw: + { + size_t raw_length = 0; + if (item->valuestring == NULL) + { + return false; + } + + raw_length = strlen(item->valuestring) + sizeof(""); + output = ensure(output_buffer, raw_length); + if (output == NULL) + { + return false; + } + memcpy(output, item->valuestring, raw_length); + return true; + } + + case cJSON_String: + return print_string(item, output_buffer); + + case cJSON_Array: + return print_array(item, output_buffer); + + case cJSON_Object: + return print_object(item, output_buffer); + + default: + return false; + } +} + +/* Build an array from input text. */ +static cJSON_bool parse_array(cJSON * const item, parse_buffer * const input_buffer) +{ + cJSON *head = NULL; /* head of the linked list */ + cJSON *current_item = NULL; + + if (input_buffer->depth >= CJSON_NESTING_LIMIT) + { + return false; /* to deeply nested */ + } + input_buffer->depth++; + + if (buffer_at_offset(input_buffer)[0] != '[') + { + /* not an array */ + goto fail; + } + + input_buffer->offset++; + buffer_skip_whitespace(input_buffer); + if (can_access_at_index(input_buffer, 0) && (buffer_at_offset(input_buffer)[0] == ']')) + { + /* empty array */ + goto success; + } + + /* check if we skipped to the end of the buffer */ + if (cannot_access_at_index(input_buffer, 0)) + { + input_buffer->offset--; + goto fail; + } + + /* step back to character in front of the first element */ + input_buffer->offset--; + /* loop through the comma separated array elements */ + do + { + /* allocate next item */ + cJSON *new_item = cJSON_New_Item(&(input_buffer->hooks)); + if (new_item == NULL) + { + goto fail; /* allocation failure */ + } + + /* attach next item to list */ + if (head == NULL) + { + /* start the linked list */ + current_item = head = new_item; + } + else + { + /* add to the end and advance */ + current_item->next = new_item; + new_item->prev = current_item; + current_item = new_item; + } + + /* parse next value */ + input_buffer->offset++; + buffer_skip_whitespace(input_buffer); + if (!parse_value(current_item, input_buffer)) + { + goto fail; /* failed to parse value */ + } + buffer_skip_whitespace(input_buffer); + } + while (can_access_at_index(input_buffer, 0) && (buffer_at_offset(input_buffer)[0] == ',')); + + if (cannot_access_at_index(input_buffer, 0) || buffer_at_offset(input_buffer)[0] != ']') + { + goto fail; /* expected end of array */ + } + +success: + input_buffer->depth--; + + if (head != NULL) { + head->prev = current_item; + } + + item->type = cJSON_Array; + item->child = head; + + input_buffer->offset++; + + return true; + +fail: + if (head != NULL) + { + cJSON_Delete(head); + } + + return false; +} + +/* Render an array to text */ +static cJSON_bool print_array(const cJSON * const item, printbuffer * const output_buffer) +{ + unsigned char *output_pointer = NULL; + size_t length = 0; + cJSON *current_element = item->child; + + if (output_buffer == NULL) + { + return false; + } + + /* Compose the output array. */ + /* opening square bracket */ + output_pointer = ensure(output_buffer, 1); + if (output_pointer == NULL) + { + return false; + } + + *output_pointer = '['; + output_buffer->offset++; + output_buffer->depth++; + + while (current_element != NULL) + { + if (!print_value(current_element, output_buffer)) + { + return false; + } + update_offset(output_buffer); + if (current_element->next) + { + length = (size_t) (output_buffer->format ? 2 : 1); + output_pointer = ensure(output_buffer, length + 1); + if (output_pointer == NULL) + { + return false; + } + *output_pointer++ = ','; + if(output_buffer->format) + { + *output_pointer++ = ' '; + } + *output_pointer = '\0'; + output_buffer->offset += length; + } + current_element = current_element->next; + } + + output_pointer = ensure(output_buffer, 2); + if (output_pointer == NULL) + { + return false; + } + *output_pointer++ = ']'; + *output_pointer = '\0'; + output_buffer->depth--; + + return true; +} + +/* Build an object from the text. */ +static cJSON_bool parse_object(cJSON * const item, parse_buffer * const input_buffer) +{ + cJSON *head = NULL; /* linked list head */ + cJSON *current_item = NULL; + + if (input_buffer->depth >= CJSON_NESTING_LIMIT) + { + return false; /* to deeply nested */ + } + input_buffer->depth++; + + if (cannot_access_at_index(input_buffer, 0) || (buffer_at_offset(input_buffer)[0] != '{')) + { + goto fail; /* not an object */ + } + + input_buffer->offset++; + buffer_skip_whitespace(input_buffer); + if (can_access_at_index(input_buffer, 0) && (buffer_at_offset(input_buffer)[0] == '}')) + { + goto success; /* empty object */ + } + + /* check if we skipped to the end of the buffer */ + if (cannot_access_at_index(input_buffer, 0)) + { + input_buffer->offset--; + goto fail; + } + + /* step back to character in front of the first element */ + input_buffer->offset--; + /* loop through the comma separated array elements */ + do + { + /* allocate next item */ + cJSON *new_item = cJSON_New_Item(&(input_buffer->hooks)); + if (new_item == NULL) + { + goto fail; /* allocation failure */ + } + + /* attach next item to list */ + if (head == NULL) + { + /* start the linked list */ + current_item = head = new_item; + } + else + { + /* add to the end and advance */ + current_item->next = new_item; + new_item->prev = current_item; + current_item = new_item; + } + + if (cannot_access_at_index(input_buffer, 1)) + { + goto fail; /* nothing comes after the comma */ + } + + /* parse the name of the child */ + input_buffer->offset++; + buffer_skip_whitespace(input_buffer); + if (!parse_string(current_item, input_buffer)) + { + goto fail; /* failed to parse name */ + } + buffer_skip_whitespace(input_buffer); + + /* swap valuestring and string, because we parsed the name */ + current_item->string = current_item->valuestring; + current_item->valuestring = NULL; + + if (cannot_access_at_index(input_buffer, 0) || (buffer_at_offset(input_buffer)[0] != ':')) + { + goto fail; /* invalid object */ + } + + /* parse the value */ + input_buffer->offset++; + buffer_skip_whitespace(input_buffer); + if (!parse_value(current_item, input_buffer)) + { + goto fail; /* failed to parse value */ + } + buffer_skip_whitespace(input_buffer); + } + while (can_access_at_index(input_buffer, 0) && (buffer_at_offset(input_buffer)[0] == ',')); + + if (cannot_access_at_index(input_buffer, 0) || (buffer_at_offset(input_buffer)[0] != '}')) + { + goto fail; /* expected end of object */ + } + +success: + input_buffer->depth--; + + if (head != NULL) { + head->prev = current_item; + } + + item->type = cJSON_Object; + item->child = head; + + input_buffer->offset++; + return true; + +fail: + if (head != NULL) + { + cJSON_Delete(head); + } + + return false; +} + +/* Render an object to text. */ +static cJSON_bool print_object(const cJSON * const item, printbuffer * const output_buffer) +{ + unsigned char *output_pointer = NULL; + size_t length = 0; + cJSON *current_item = item->child; + + if (output_buffer == NULL) + { + return false; + } + + /* Compose the output: */ + length = (size_t) (output_buffer->format ? 2 : 1); /* fmt: {\n */ + output_pointer = ensure(output_buffer, length + 1); + if (output_pointer == NULL) + { + return false; + } + + *output_pointer++ = '{'; + output_buffer->depth++; + if (output_buffer->format) + { + *output_pointer++ = '\n'; + } + output_buffer->offset += length; + + while (current_item) + { + if (output_buffer->format) + { + size_t i; + output_pointer = ensure(output_buffer, output_buffer->depth); + if (output_pointer == NULL) + { + return false; + } + for (i = 0; i < output_buffer->depth; i++) + { + *output_pointer++ = '\t'; + } + output_buffer->offset += output_buffer->depth; + } + + /* print key */ + if (!print_string_ptr((unsigned char*)current_item->string, output_buffer)) + { + return false; + } + update_offset(output_buffer); + + length = (size_t) (output_buffer->format ? 2 : 1); + output_pointer = ensure(output_buffer, length); + if (output_pointer == NULL) + { + return false; + } + *output_pointer++ = ':'; + if (output_buffer->format) + { + *output_pointer++ = '\t'; + } + output_buffer->offset += length; + + /* print value */ + if (!print_value(current_item, output_buffer)) + { + return false; + } + update_offset(output_buffer); + + /* print comma if not last */ + length = ((size_t)(output_buffer->format ? 1 : 0) + (size_t)(current_item->next ? 1 : 0)); + output_pointer = ensure(output_buffer, length + 1); + if (output_pointer == NULL) + { + return false; + } + if (current_item->next) + { + *output_pointer++ = ','; + } + + if (output_buffer->format) + { + *output_pointer++ = '\n'; + } + *output_pointer = '\0'; + output_buffer->offset += length; + + current_item = current_item->next; + } + + output_pointer = ensure(output_buffer, output_buffer->format ? (output_buffer->depth + 1) : 2); + if (output_pointer == NULL) + { + return false; + } + if (output_buffer->format) + { + size_t i; + for (i = 0; i < (output_buffer->depth - 1); i++) + { + *output_pointer++ = '\t'; + } + } + *output_pointer++ = '}'; + *output_pointer = '\0'; + output_buffer->depth--; + + return true; +} + +/* Get Array size/item / object item. */ +CJSON_PUBLIC(int) cJSON_GetArraySize(const cJSON *array) +{ + cJSON *child = NULL; + size_t size = 0; + + if (array == NULL) + { + return 0; + } + + child = array->child; + + while(child != NULL) + { + size++; + child = child->next; + } + + /* FIXME: Can overflow here. Cannot be fixed without breaking the API */ + + return (int)size; +} + +static cJSON* get_array_item(const cJSON *array, size_t index) +{ + cJSON *current_child = NULL; + + if (array == NULL) + { + return NULL; + } + + current_child = array->child; + while ((current_child != NULL) && (index > 0)) + { + index--; + current_child = current_child->next; + } + + return current_child; +} + +CJSON_PUBLIC(cJSON *) cJSON_GetArrayItem(const cJSON *array, int index) +{ + if (index < 0) + { + return NULL; + } + + return get_array_item(array, (size_t)index); +} + +static cJSON *get_object_item(const cJSON * const object, const char * const name, const cJSON_bool case_sensitive) +{ + cJSON *current_element = NULL; + + if ((object == NULL) || (name == NULL)) + { + return NULL; + } + + current_element = object->child; + if (case_sensitive) + { + while ((current_element != NULL) && (current_element->string != NULL) && (strcmp(name, current_element->string) != 0)) + { + current_element = current_element->next; + } + } + else + { + while ((current_element != NULL) && (case_insensitive_strcmp((const unsigned char*)name, (const unsigned char*)(current_element->string)) != 0)) + { + current_element = current_element->next; + } + } + + if ((current_element == NULL) || (current_element->string == NULL)) { + return NULL; + } + + return current_element; +} + +CJSON_PUBLIC(cJSON *) cJSON_GetObjectItem(const cJSON * const object, const char * const string) +{ + return get_object_item(object, string, false); +} + +CJSON_PUBLIC(cJSON *) cJSON_GetObjectItemCaseSensitive(const cJSON * const object, const char * const string) +{ + return get_object_item(object, string, true); +} + +CJSON_PUBLIC(cJSON_bool) cJSON_HasObjectItem(const cJSON *object, const char *string) +{ + return cJSON_GetObjectItem(object, string) ? 1 : 0; +} + +/* Utility for array list handling. */ +static void suffix_object(cJSON *prev, cJSON *item) +{ + prev->next = item; + item->prev = prev; +} + +/* Utility for handling references. */ +static cJSON *create_reference(const cJSON *item, const internal_hooks * const hooks) +{ + cJSON *reference = NULL; + if (item == NULL) + { + return NULL; + } + + reference = cJSON_New_Item(hooks); + if (reference == NULL) + { + return NULL; + } + + memcpy(reference, item, sizeof(cJSON)); + reference->string = NULL; + reference->type |= cJSON_IsReference; + reference->next = reference->prev = NULL; + return reference; +} + +static cJSON_bool add_item_to_array(cJSON *array, cJSON *item) +{ + cJSON *child = NULL; + + if ((item == NULL) || (array == NULL) || (array == item)) + { + return false; + } + + child = array->child; + /* + * To find the last item in array quickly, we use prev in array + */ + if (child == NULL) + { + /* list is empty, start new one */ + array->child = item; + item->prev = item; + item->next = NULL; + } + else + { + /* append to the end */ + if (child->prev) + { + suffix_object(child->prev, item); + array->child->prev = item; + } + } + + return true; +} + +/* Add item to array/object. */ +CJSON_PUBLIC(cJSON_bool) cJSON_AddItemToArray(cJSON *array, cJSON *item) +{ + return add_item_to_array(array, item); +} + +#if defined(__clang__) || (defined(__GNUC__) && ((__GNUC__ > 4) || ((__GNUC__ == 4) && (__GNUC_MINOR__ > 5)))) + #pragma GCC diagnostic push +#endif +#ifdef __GNUC__ +#pragma GCC diagnostic ignored "-Wcast-qual" +#endif +/* helper function to cast away const */ +static void* cast_away_const(const void* string) +{ + return (void*)string; +} +#if defined(__clang__) || (defined(__GNUC__) && ((__GNUC__ > 4) || ((__GNUC__ == 4) && (__GNUC_MINOR__ > 5)))) + #pragma GCC diagnostic pop +#endif + + +static cJSON_bool add_item_to_object(cJSON * const object, const char * const string, cJSON * const item, const internal_hooks * const hooks, const cJSON_bool constant_key) +{ + char *new_key = NULL; + int new_type = cJSON_Invalid; + + if ((object == NULL) || (string == NULL) || (item == NULL) || (object == item)) + { + return false; + } + + if (constant_key) + { + new_key = (char*)cast_away_const(string); + new_type = item->type | cJSON_StringIsConst; + } + else + { + new_key = (char*)cJSON_strdup((const unsigned char*)string, hooks); + if (new_key == NULL) + { + return false; + } + + new_type = item->type & ~cJSON_StringIsConst; + } + + if (!(item->type & cJSON_StringIsConst) && (item->string != NULL)) + { + hooks->deallocate(item->string); + } + + item->string = new_key; + item->type = new_type; + + return add_item_to_array(object, item); +} + +CJSON_PUBLIC(cJSON_bool) cJSON_AddItemToObject(cJSON *object, const char *string, cJSON *item) +{ + return add_item_to_object(object, string, item, &global_hooks, false); +} + +/* Add an item to an object with constant string as key */ +CJSON_PUBLIC(cJSON_bool) cJSON_AddItemToObjectCS(cJSON *object, const char *string, cJSON *item) +{ + return add_item_to_object(object, string, item, &global_hooks, true); +} + +CJSON_PUBLIC(cJSON_bool) cJSON_AddItemReferenceToArray(cJSON *array, cJSON *item) +{ + if (array == NULL) + { + return false; + } + + return add_item_to_array(array, create_reference(item, &global_hooks)); +} + +CJSON_PUBLIC(cJSON_bool) cJSON_AddItemReferenceToObject(cJSON *object, const char *string, cJSON *item) +{ + if ((object == NULL) || (string == NULL)) + { + return false; + } + + return add_item_to_object(object, string, create_reference(item, &global_hooks), &global_hooks, false); +} + +CJSON_PUBLIC(cJSON*) cJSON_AddNullToObject(cJSON * const object, const char * const name) +{ + cJSON *null = cJSON_CreateNull(); + if (add_item_to_object(object, name, null, &global_hooks, false)) + { + return null; + } + + cJSON_Delete(null); + return NULL; +} + +CJSON_PUBLIC(cJSON*) cJSON_AddTrueToObject(cJSON * const object, const char * const name) +{ + cJSON *true_item = cJSON_CreateTrue(); + if (add_item_to_object(object, name, true_item, &global_hooks, false)) + { + return true_item; + } + + cJSON_Delete(true_item); + return NULL; +} + +CJSON_PUBLIC(cJSON*) cJSON_AddFalseToObject(cJSON * const object, const char * const name) +{ + cJSON *false_item = cJSON_CreateFalse(); + if (add_item_to_object(object, name, false_item, &global_hooks, false)) + { + return false_item; + } + + cJSON_Delete(false_item); + return NULL; +} + +CJSON_PUBLIC(cJSON*) cJSON_AddBoolToObject(cJSON * const object, const char * const name, const cJSON_bool boolean) +{ + cJSON *bool_item = cJSON_CreateBool(boolean); + if (add_item_to_object(object, name, bool_item, &global_hooks, false)) + { + return bool_item; + } + + cJSON_Delete(bool_item); + return NULL; +} + +CJSON_PUBLIC(cJSON*) cJSON_AddNumberToObject(cJSON * const object, const char * const name, const double number) +{ + cJSON *number_item = cJSON_CreateNumber(number); + if (add_item_to_object(object, name, number_item, &global_hooks, false)) + { + return number_item; + } + + cJSON_Delete(number_item); + return NULL; +} + +CJSON_PUBLIC(cJSON*) cJSON_AddStringToObject(cJSON * const object, const char * const name, const char * const string) +{ + cJSON *string_item = cJSON_CreateString(string); + if (add_item_to_object(object, name, string_item, &global_hooks, false)) + { + return string_item; + } + + cJSON_Delete(string_item); + return NULL; +} + +CJSON_PUBLIC(cJSON*) cJSON_AddRawToObject(cJSON * const object, const char * const name, const char * const raw) +{ + cJSON *raw_item = cJSON_CreateRaw(raw); + if (add_item_to_object(object, name, raw_item, &global_hooks, false)) + { + return raw_item; + } + + cJSON_Delete(raw_item); + return NULL; +} + +CJSON_PUBLIC(cJSON*) cJSON_AddObjectToObject(cJSON * const object, const char * const name) +{ + cJSON *object_item = cJSON_CreateObject(); + if (add_item_to_object(object, name, object_item, &global_hooks, false)) + { + return object_item; + } + + cJSON_Delete(object_item); + return NULL; +} + +CJSON_PUBLIC(cJSON*) cJSON_AddArrayToObject(cJSON * const object, const char * const name) +{ + cJSON *array = cJSON_CreateArray(); + if (add_item_to_object(object, name, array, &global_hooks, false)) + { + return array; + } + + cJSON_Delete(array); + return NULL; +} + +CJSON_PUBLIC(cJSON *) cJSON_DetachItemViaPointer(cJSON *parent, cJSON * const item) +{ + if ((parent == NULL) || (item == NULL)) + { + return NULL; + } + + if (item != parent->child) + { + /* not the first element */ + item->prev->next = item->next; + } + if (item->next != NULL) + { + /* not the last element */ + item->next->prev = item->prev; + } + + if (item == parent->child) + { + /* first element */ + parent->child = item->next; + } + else if (item->next == NULL) + { + /* last element */ + parent->child->prev = item->prev; + } + + /* make sure the detached item doesn't point anywhere anymore */ + item->prev = NULL; + item->next = NULL; + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_DetachItemFromArray(cJSON *array, int which) +{ + if (which < 0) + { + return NULL; + } + + return cJSON_DetachItemViaPointer(array, get_array_item(array, (size_t)which)); +} + +CJSON_PUBLIC(void) cJSON_DeleteItemFromArray(cJSON *array, int which) +{ + cJSON_Delete(cJSON_DetachItemFromArray(array, which)); +} + +CJSON_PUBLIC(cJSON *) cJSON_DetachItemFromObject(cJSON *object, const char *string) +{ + cJSON *to_detach = cJSON_GetObjectItem(object, string); + + return cJSON_DetachItemViaPointer(object, to_detach); +} + +CJSON_PUBLIC(cJSON *) cJSON_DetachItemFromObjectCaseSensitive(cJSON *object, const char *string) +{ + cJSON *to_detach = cJSON_GetObjectItemCaseSensitive(object, string); + + return cJSON_DetachItemViaPointer(object, to_detach); +} + +CJSON_PUBLIC(void) cJSON_DeleteItemFromObject(cJSON *object, const char *string) +{ + cJSON_Delete(cJSON_DetachItemFromObject(object, string)); +} + +CJSON_PUBLIC(void) cJSON_DeleteItemFromObjectCaseSensitive(cJSON *object, const char *string) +{ + cJSON_Delete(cJSON_DetachItemFromObjectCaseSensitive(object, string)); +} + +/* Replace array/object items with new ones. */ +CJSON_PUBLIC(cJSON_bool) cJSON_InsertItemInArray(cJSON *array, int which, cJSON *newitem) +{ + cJSON *after_inserted = NULL; + + if (which < 0 || newitem == NULL) + { + return false; + } + + after_inserted = get_array_item(array, (size_t)which); + if (after_inserted == NULL) + { + return add_item_to_array(array, newitem); + } + + if (after_inserted != array->child && after_inserted->prev == NULL) { + /* return false if after_inserted is a corrupted array item */ + return false; + } + + newitem->next = after_inserted; + newitem->prev = after_inserted->prev; + after_inserted->prev = newitem; + if (after_inserted == array->child) + { + array->child = newitem; + } + else + { + newitem->prev->next = newitem; + } + return true; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_ReplaceItemViaPointer(cJSON * const parent, cJSON * const item, cJSON * replacement) +{ + if ((parent == NULL) || (parent->child == NULL) || (replacement == NULL) || (item == NULL)) + { + return false; + } + + if (replacement == item) + { + return true; + } + + replacement->next = item->next; + replacement->prev = item->prev; + + if (replacement->next != NULL) + { + replacement->next->prev = replacement; + } + if (parent->child == item) + { + if (parent->child->prev == parent->child) + { + replacement->prev = replacement; + } + parent->child = replacement; + } + else + { /* + * To find the last item in array quickly, we use prev in array. + * We can't modify the last item's next pointer where this item was the parent's child + */ + if (replacement->prev != NULL) + { + replacement->prev->next = replacement; + } + if (replacement->next == NULL) + { + parent->child->prev = replacement; + } + } + + item->next = NULL; + item->prev = NULL; + cJSON_Delete(item); + + return true; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_ReplaceItemInArray(cJSON *array, int which, cJSON *newitem) +{ + if (which < 0) + { + return false; + } + + return cJSON_ReplaceItemViaPointer(array, get_array_item(array, (size_t)which), newitem); +} + +static cJSON_bool replace_item_in_object(cJSON *object, const char *string, cJSON *replacement, cJSON_bool case_sensitive) +{ + if ((replacement == NULL) || (string == NULL)) + { + return false; + } + + /* replace the name in the replacement */ + if (!(replacement->type & cJSON_StringIsConst) && (replacement->string != NULL)) + { + cJSON_free(replacement->string); + } + replacement->string = (char*)cJSON_strdup((const unsigned char*)string, &global_hooks); + if (replacement->string == NULL) + { + return false; + } + + replacement->type &= ~cJSON_StringIsConst; + + return cJSON_ReplaceItemViaPointer(object, get_object_item(object, string, case_sensitive), replacement); +} + +CJSON_PUBLIC(cJSON_bool) cJSON_ReplaceItemInObject(cJSON *object, const char *string, cJSON *newitem) +{ + return replace_item_in_object(object, string, newitem, false); +} + +CJSON_PUBLIC(cJSON_bool) cJSON_ReplaceItemInObjectCaseSensitive(cJSON *object, const char *string, cJSON *newitem) +{ + return replace_item_in_object(object, string, newitem, true); +} + +/* Create basic types: */ +CJSON_PUBLIC(cJSON *) cJSON_CreateNull(void) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if(item) + { + item->type = cJSON_NULL; + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateTrue(void) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if(item) + { + item->type = cJSON_True; + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateFalse(void) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if(item) + { + item->type = cJSON_False; + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateBool(cJSON_bool boolean) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if(item) + { + item->type = boolean ? cJSON_True : cJSON_False; + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateNumber(double num) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if(item) + { + item->type = cJSON_Number; + item->valuedouble = num; + + /* use saturation in case of overflow */ + if (num >= INT_MAX) + { + item->valueint = INT_MAX; + } + else if (num <= (double)INT_MIN) + { + item->valueint = INT_MIN; + } + else + { + item->valueint = (int)num; + } + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateString(const char *string) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if(item) + { + item->type = cJSON_String; + item->valuestring = (char*)cJSON_strdup((const unsigned char*)string, &global_hooks); + if(!item->valuestring) + { + cJSON_Delete(item); + return NULL; + } + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateStringReference(const char *string) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item != NULL) + { + item->type = cJSON_String | cJSON_IsReference; + item->valuestring = (char*)cast_away_const(string); + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateObjectReference(const cJSON *child) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item != NULL) { + item->type = cJSON_Object | cJSON_IsReference; + item->child = (cJSON*)cast_away_const(child); + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateArrayReference(const cJSON *child) { + cJSON *item = cJSON_New_Item(&global_hooks); + if (item != NULL) { + item->type = cJSON_Array | cJSON_IsReference; + item->child = (cJSON*)cast_away_const(child); + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateRaw(const char *raw) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if(item) + { + item->type = cJSON_Raw; + item->valuestring = (char*)cJSON_strdup((const unsigned char*)raw, &global_hooks); + if(!item->valuestring) + { + cJSON_Delete(item); + return NULL; + } + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateArray(void) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if(item) + { + item->type=cJSON_Array; + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateObject(void) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item) + { + item->type = cJSON_Object; + } + + return item; +} + +/* Create Arrays: */ +CJSON_PUBLIC(cJSON *) cJSON_CreateIntArray(const int *numbers, int count) +{ + size_t i = 0; + cJSON *n = NULL; + cJSON *p = NULL; + cJSON *a = NULL; + + if ((count < 0) || (numbers == NULL)) + { + return NULL; + } + + a = cJSON_CreateArray(); + + for(i = 0; a && (i < (size_t)count); i++) + { + n = cJSON_CreateNumber(numbers[i]); + if (!n) + { + cJSON_Delete(a); + return NULL; + } + if(!i) + { + a->child = n; + } + else + { + suffix_object(p, n); + } + p = n; + } + + if (a && a->child) { + a->child->prev = n; + } + + return a; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateFloatArray(const float *numbers, int count) +{ + size_t i = 0; + cJSON *n = NULL; + cJSON *p = NULL; + cJSON *a = NULL; + + if ((count < 0) || (numbers == NULL)) + { + return NULL; + } + + a = cJSON_CreateArray(); + + for(i = 0; a && (i < (size_t)count); i++) + { + n = cJSON_CreateNumber((double)numbers[i]); + if(!n) + { + cJSON_Delete(a); + return NULL; + } + if(!i) + { + a->child = n; + } + else + { + suffix_object(p, n); + } + p = n; + } + + if (a && a->child) { + a->child->prev = n; + } + + return a; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateDoubleArray(const double *numbers, int count) +{ + size_t i = 0; + cJSON *n = NULL; + cJSON *p = NULL; + cJSON *a = NULL; + + if ((count < 0) || (numbers == NULL)) + { + return NULL; + } + + a = cJSON_CreateArray(); + + for(i = 0; a && (i < (size_t)count); i++) + { + n = cJSON_CreateNumber(numbers[i]); + if(!n) + { + cJSON_Delete(a); + return NULL; + } + if(!i) + { + a->child = n; + } + else + { + suffix_object(p, n); + } + p = n; + } + + if (a && a->child) { + a->child->prev = n; + } + + return a; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateStringArray(const char *const *strings, int count) +{ + size_t i = 0; + cJSON *n = NULL; + cJSON *p = NULL; + cJSON *a = NULL; + + if ((count < 0) || (strings == NULL)) + { + return NULL; + } + + a = cJSON_CreateArray(); + + for (i = 0; a && (i < (size_t)count); i++) + { + n = cJSON_CreateString(strings[i]); + if(!n) + { + cJSON_Delete(a); + return NULL; + } + if(!i) + { + a->child = n; + } + else + { + suffix_object(p,n); + } + p = n; + } + + if (a && a->child) { + a->child->prev = n; + } + + return a; +} + +/* Duplication */ +CJSON_PUBLIC(cJSON *) cJSON_Duplicate(const cJSON *item, cJSON_bool recurse) +{ + cJSON *newitem = NULL; + cJSON *child = NULL; + cJSON *next = NULL; + cJSON *newchild = NULL; + + /* Bail on bad ptr */ + if (!item) + { + goto fail; + } + /* Create new item */ + newitem = cJSON_New_Item(&global_hooks); + if (!newitem) + { + goto fail; + } + /* Copy over all vars */ + newitem->type = item->type & (~cJSON_IsReference); + newitem->valueint = item->valueint; + newitem->valuedouble = item->valuedouble; + if (item->valuestring) + { + newitem->valuestring = (char*)cJSON_strdup((unsigned char*)item->valuestring, &global_hooks); + if (!newitem->valuestring) + { + goto fail; + } + } + if (item->string) + { + newitem->string = (item->type&cJSON_StringIsConst) ? item->string : (char*)cJSON_strdup((unsigned char*)item->string, &global_hooks); + if (!newitem->string) + { + goto fail; + } + } + /* If non-recursive, then we're done! */ + if (!recurse) + { + return newitem; + } + /* Walk the ->next chain for the child. */ + child = item->child; + while (child != NULL) + { + newchild = cJSON_Duplicate(child, true); /* Duplicate (with recurse) each item in the ->next chain */ + if (!newchild) + { + goto fail; + } + if (next != NULL) + { + /* If newitem->child already set, then crosswire ->prev and ->next and move on */ + next->next = newchild; + newchild->prev = next; + next = newchild; + } + else + { + /* Set newitem->child and move to it */ + newitem->child = newchild; + next = newchild; + } + child = child->next; + } + if (newitem && newitem->child) + { + newitem->child->prev = newchild; + } + + return newitem; + +fail: + if (newitem != NULL) + { + cJSON_Delete(newitem); + } + + return NULL; +} + +static void skip_oneline_comment(char **input) +{ + *input += static_strlen("//"); + + for (; (*input)[0] != '\0'; ++(*input)) + { + if ((*input)[0] == '\n') { + *input += static_strlen("\n"); + return; + } + } +} + +static void skip_multiline_comment(char **input) +{ + *input += static_strlen("/*"); + + for (; (*input)[0] != '\0'; ++(*input)) + { + if (((*input)[0] == '*') && ((*input)[1] == '/')) + { + *input += static_strlen("*/"); + return; + } + } +} + +static void minify_string(char **input, char **output) { + (*output)[0] = (*input)[0]; + *input += static_strlen("\""); + *output += static_strlen("\""); + + + for (; (*input)[0] != '\0'; (void)++(*input), ++(*output)) { + (*output)[0] = (*input)[0]; + + if ((*input)[0] == '\"') { + (*output)[0] = '\"'; + *input += static_strlen("\""); + *output += static_strlen("\""); + return; + } else if (((*input)[0] == '\\') && ((*input)[1] == '\"')) { + (*output)[1] = (*input)[1]; + *input += static_strlen("\""); + *output += static_strlen("\""); + } + } +} + +CJSON_PUBLIC(void) cJSON_Minify(char *json) +{ + char *into = json; + + if (json == NULL) + { + return; + } + + while (json[0] != '\0') + { + switch (json[0]) + { + case ' ': + case '\t': + case '\r': + case '\n': + json++; + break; + + case '/': + if (json[1] == '/') + { + skip_oneline_comment(&json); + } + else if (json[1] == '*') + { + skip_multiline_comment(&json); + } else { + json++; + } + break; + + case '\"': + minify_string(&json, (char**)&into); + break; + + default: + into[0] = json[0]; + json++; + into++; + } + } + + /* and null-terminate. */ + *into = '\0'; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_IsInvalid(const cJSON * const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_Invalid; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_IsFalse(const cJSON * const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_False; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_IsTrue(const cJSON * const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xff) == cJSON_True; +} + + +CJSON_PUBLIC(cJSON_bool) cJSON_IsBool(const cJSON * const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & (cJSON_True | cJSON_False)) != 0; +} +CJSON_PUBLIC(cJSON_bool) cJSON_IsNull(const cJSON * const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_NULL; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_IsNumber(const cJSON * const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_Number; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_IsString(const cJSON * const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_String; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_IsArray(const cJSON * const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_Array; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_IsObject(const cJSON * const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_Object; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_IsRaw(const cJSON * const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_Raw; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_Compare(const cJSON * const a, const cJSON * const b, const cJSON_bool case_sensitive) +{ + if ((a == NULL) || (b == NULL) || ((a->type & 0xFF) != (b->type & 0xFF))) + { + return false; + } + + /* check if type is valid */ + switch (a->type & 0xFF) + { + case cJSON_False: + case cJSON_True: + case cJSON_NULL: + case cJSON_Number: + case cJSON_String: + case cJSON_Raw: + case cJSON_Array: + case cJSON_Object: + break; + + default: + return false; + } + + /* identical objects are equal */ + if (a == b) + { + return true; + } + + switch (a->type & 0xFF) + { + /* in these cases and equal type is enough */ + case cJSON_False: + case cJSON_True: + case cJSON_NULL: + return true; + + case cJSON_Number: + if (compare_double(a->valuedouble, b->valuedouble)) + { + return true; + } + return false; + + case cJSON_String: + case cJSON_Raw: + if ((a->valuestring == NULL) || (b->valuestring == NULL)) + { + return false; + } + if (strcmp(a->valuestring, b->valuestring) == 0) + { + return true; + } + + return false; + + case cJSON_Array: + { + cJSON *a_element = a->child; + cJSON *b_element = b->child; + + for (; (a_element != NULL) && (b_element != NULL);) + { + if (!cJSON_Compare(a_element, b_element, case_sensitive)) + { + return false; + } + + a_element = a_element->next; + b_element = b_element->next; + } + + /* one of the arrays is longer than the other */ + if (a_element != b_element) { + return false; + } + + return true; + } + + case cJSON_Object: + { + cJSON *a_element = NULL; + cJSON *b_element = NULL; + cJSON_ArrayForEach(a_element, a) + { + /* TODO This has O(n^2) runtime, which is horrible! */ + b_element = get_object_item(b, a_element->string, case_sensitive); + if (b_element == NULL) + { + return false; + } + + if (!cJSON_Compare(a_element, b_element, case_sensitive)) + { + return false; + } + } + + /* doing this twice, once on a and b to prevent true comparison if a subset of b + * TODO: Do this the proper way, this is just a fix for now */ + cJSON_ArrayForEach(b_element, b) + { + a_element = get_object_item(a, b_element->string, case_sensitive); + if (a_element == NULL) + { + return false; + } + + if (!cJSON_Compare(b_element, a_element, case_sensitive)) + { + return false; + } + } + + return true; + } + + default: + return false; + } +} + +CJSON_PUBLIC(void *) cJSON_malloc(size_t size) +{ + return global_hooks.allocate(size); +} + +CJSON_PUBLIC(void) cJSON_free(void *object) +{ + global_hooks.deallocate(object); + object = NULL; +} diff --git a/examples/sbgEComExample/src/cJSON.h b/examples/sbgEComExample/src/cJSON.h new file mode 100644 index 0000000..88cf0bc --- /dev/null +++ b/examples/sbgEComExample/src/cJSON.h @@ -0,0 +1,300 @@ +/* + Copyright (c) 2009-2017 Dave Gamble and cJSON contributors + + 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 cJSON__h +#define cJSON__h + +#ifdef __cplusplus +extern "C" +{ +#endif + +#if !defined(__WINDOWS__) && (defined(WIN32) || defined(WIN64) || defined(_MSC_VER) || defined(_WIN32)) +#define __WINDOWS__ +#endif + +#ifdef __WINDOWS__ + +/* When compiling for windows, we specify a specific calling convention to avoid issues where we are being called from a project with a different default calling convention. For windows you have 3 define options: + +CJSON_HIDE_SYMBOLS - Define this in the case where you don't want to ever dllexport symbols +CJSON_EXPORT_SYMBOLS - Define this on library build when you want to dllexport symbols (default) +CJSON_IMPORT_SYMBOLS - Define this if you want to dllimport symbol + +For *nix builds that support visibility attribute, you can define similar behavior by + +setting default visibility to hidden by adding +-fvisibility=hidden (for gcc) +or +-xldscope=hidden (for sun cc) +to CFLAGS + +then using the CJSON_API_VISIBILITY flag to "export" the same symbols the way CJSON_EXPORT_SYMBOLS does + +*/ + +#define CJSON_CDECL __cdecl +#define CJSON_STDCALL __stdcall + +/* export symbols by default, this is necessary for copy pasting the C and header file */ +#if !defined(CJSON_HIDE_SYMBOLS) && !defined(CJSON_IMPORT_SYMBOLS) && !defined(CJSON_EXPORT_SYMBOLS) +#define CJSON_EXPORT_SYMBOLS +#endif + +#if defined(CJSON_HIDE_SYMBOLS) +#define CJSON_PUBLIC(type) type CJSON_STDCALL +#elif defined(CJSON_EXPORT_SYMBOLS) +#define CJSON_PUBLIC(type) __declspec(dllexport) type CJSON_STDCALL +#elif defined(CJSON_IMPORT_SYMBOLS) +#define CJSON_PUBLIC(type) __declspec(dllimport) type CJSON_STDCALL +#endif +#else /* !__WINDOWS__ */ +#define CJSON_CDECL +#define CJSON_STDCALL + +#if (defined(__GNUC__) || defined(__SUNPRO_CC) || defined (__SUNPRO_C)) && defined(CJSON_API_VISIBILITY) +#define CJSON_PUBLIC(type) __attribute__((visibility("default"))) type +#else +#define CJSON_PUBLIC(type) type +#endif +#endif + +/* project version */ +#define CJSON_VERSION_MAJOR 1 +#define CJSON_VERSION_MINOR 7 +#define CJSON_VERSION_PATCH 18 + +#include + +/* cJSON Types: */ +#define cJSON_Invalid (0) +#define cJSON_False (1 << 0) +#define cJSON_True (1 << 1) +#define cJSON_NULL (1 << 2) +#define cJSON_Number (1 << 3) +#define cJSON_String (1 << 4) +#define cJSON_Array (1 << 5) +#define cJSON_Object (1 << 6) +#define cJSON_Raw (1 << 7) /* raw json */ + +#define cJSON_IsReference 256 +#define cJSON_StringIsConst 512 + +/* The cJSON structure: */ +typedef struct cJSON +{ + /* next/prev allow you to walk array/object chains. Alternatively, use GetArraySize/GetArrayItem/GetObjectItem */ + struct cJSON *next; + struct cJSON *prev; + /* An array or object item will have a child pointer pointing to a chain of the items in the array/object. */ + struct cJSON *child; + + /* The type of the item, as above. */ + int type; + + /* The item's string, if type==cJSON_String and type == cJSON_Raw */ + char *valuestring; + /* writing to valueint is DEPRECATED, use cJSON_SetNumberValue instead */ + int valueint; + /* The item's number, if type==cJSON_Number */ + double valuedouble; + + /* The item's name string, if this item is the child of, or is in the list of subitems of an object. */ + char *string; +} cJSON; + +typedef struct cJSON_Hooks +{ + /* malloc/free are CDECL on Windows regardless of the default calling convention of the compiler, so ensure the hooks allow passing those functions directly. */ + void *(CJSON_CDECL *malloc_fn)(size_t sz); + void (CJSON_CDECL *free_fn)(void *ptr); +} cJSON_Hooks; + +typedef int cJSON_bool; + +/* Limits how deeply nested arrays/objects can be before cJSON rejects to parse them. + * This is to prevent stack overflows. */ +#ifndef CJSON_NESTING_LIMIT +#define CJSON_NESTING_LIMIT 1000 +#endif + +/* returns the version of cJSON as a string */ +CJSON_PUBLIC(const char*) cJSON_Version(void); + +/* Supply malloc, realloc and free functions to cJSON */ +CJSON_PUBLIC(void) cJSON_InitHooks(cJSON_Hooks* hooks); + +/* Memory Management: the caller is always responsible to free the results from all variants of cJSON_Parse (with cJSON_Delete) and cJSON_Print (with stdlib free, cJSON_Hooks.free_fn, or cJSON_free as appropriate). The exception is cJSON_PrintPreallocated, where the caller has full responsibility of the buffer. */ +/* Supply a block of JSON, and this returns a cJSON object you can interrogate. */ +CJSON_PUBLIC(cJSON *) cJSON_Parse(const char *value); +CJSON_PUBLIC(cJSON *) cJSON_ParseWithLength(const char *value, size_t buffer_length); +/* ParseWithOpts allows you to require (and check) that the JSON is null terminated, and to retrieve the pointer to the final byte parsed. */ +/* If you supply a ptr in return_parse_end and parsing fails, then return_parse_end will contain a pointer to the error so will match cJSON_GetErrorPtr(). */ +CJSON_PUBLIC(cJSON *) cJSON_ParseWithOpts(const char *value, const char **return_parse_end, cJSON_bool require_null_terminated); +CJSON_PUBLIC(cJSON *) cJSON_ParseWithLengthOpts(const char *value, size_t buffer_length, const char **return_parse_end, cJSON_bool require_null_terminated); + +/* Render a cJSON entity to text for transfer/storage. */ +CJSON_PUBLIC(char *) cJSON_Print(const cJSON *item); +/* Render a cJSON entity to text for transfer/storage without any formatting. */ +CJSON_PUBLIC(char *) cJSON_PrintUnformatted(const cJSON *item); +/* Render a cJSON entity to text using a buffered strategy. prebuffer is a guess at the final size. guessing well reduces reallocation. fmt=0 gives unformatted, =1 gives formatted */ +CJSON_PUBLIC(char *) cJSON_PrintBuffered(const cJSON *item, int prebuffer, cJSON_bool fmt); +/* Render a cJSON entity to text using a buffer already allocated in memory with given length. Returns 1 on success and 0 on failure. */ +/* NOTE: cJSON is not always 100% accurate in estimating how much memory it will use, so to be safe allocate 5 bytes more than you actually need */ +CJSON_PUBLIC(cJSON_bool) cJSON_PrintPreallocated(cJSON *item, char *buffer, const int length, const cJSON_bool format); +/* Delete a cJSON entity and all subentities. */ +CJSON_PUBLIC(void) cJSON_Delete(cJSON *item); + +/* Returns the number of items in an array (or object). */ +CJSON_PUBLIC(int) cJSON_GetArraySize(const cJSON *array); +/* Retrieve item number "index" from array "array". Returns NULL if unsuccessful. */ +CJSON_PUBLIC(cJSON *) cJSON_GetArrayItem(const cJSON *array, int index); +/* Get item "string" from object. Case insensitive. */ +CJSON_PUBLIC(cJSON *) cJSON_GetObjectItem(const cJSON * const object, const char * const string); +CJSON_PUBLIC(cJSON *) cJSON_GetObjectItemCaseSensitive(const cJSON * const object, const char * const string); +CJSON_PUBLIC(cJSON_bool) cJSON_HasObjectItem(const cJSON *object, const char *string); +/* For analysing failed parses. This returns a pointer to the parse error. You'll probably need to look a few chars back to make sense of it. Defined when cJSON_Parse() returns 0. 0 when cJSON_Parse() succeeds. */ +CJSON_PUBLIC(const char *) cJSON_GetErrorPtr(void); + +/* Check item type and return its value */ +CJSON_PUBLIC(char *) cJSON_GetStringValue(const cJSON * const item); +CJSON_PUBLIC(double) cJSON_GetNumberValue(const cJSON * const item); + +/* These functions check the type of an item */ +CJSON_PUBLIC(cJSON_bool) cJSON_IsInvalid(const cJSON * const item); +CJSON_PUBLIC(cJSON_bool) cJSON_IsFalse(const cJSON * const item); +CJSON_PUBLIC(cJSON_bool) cJSON_IsTrue(const cJSON * const item); +CJSON_PUBLIC(cJSON_bool) cJSON_IsBool(const cJSON * const item); +CJSON_PUBLIC(cJSON_bool) cJSON_IsNull(const cJSON * const item); +CJSON_PUBLIC(cJSON_bool) cJSON_IsNumber(const cJSON * const item); +CJSON_PUBLIC(cJSON_bool) cJSON_IsString(const cJSON * const item); +CJSON_PUBLIC(cJSON_bool) cJSON_IsArray(const cJSON * const item); +CJSON_PUBLIC(cJSON_bool) cJSON_IsObject(const cJSON * const item); +CJSON_PUBLIC(cJSON_bool) cJSON_IsRaw(const cJSON * const item); + +/* These calls create a cJSON item of the appropriate type. */ +CJSON_PUBLIC(cJSON *) cJSON_CreateNull(void); +CJSON_PUBLIC(cJSON *) cJSON_CreateTrue(void); +CJSON_PUBLIC(cJSON *) cJSON_CreateFalse(void); +CJSON_PUBLIC(cJSON *) cJSON_CreateBool(cJSON_bool boolean); +CJSON_PUBLIC(cJSON *) cJSON_CreateNumber(double num); +CJSON_PUBLIC(cJSON *) cJSON_CreateString(const char *string); +/* raw json */ +CJSON_PUBLIC(cJSON *) cJSON_CreateRaw(const char *raw); +CJSON_PUBLIC(cJSON *) cJSON_CreateArray(void); +CJSON_PUBLIC(cJSON *) cJSON_CreateObject(void); + +/* Create a string where valuestring references a string so + * it will not be freed by cJSON_Delete */ +CJSON_PUBLIC(cJSON *) cJSON_CreateStringReference(const char *string); +/* Create an object/array that only references it's elements so + * they will not be freed by cJSON_Delete */ +CJSON_PUBLIC(cJSON *) cJSON_CreateObjectReference(const cJSON *child); +CJSON_PUBLIC(cJSON *) cJSON_CreateArrayReference(const cJSON *child); + +/* These utilities create an Array of count items. + * The parameter count cannot be greater than the number of elements in the number array, otherwise array access will be out of bounds.*/ +CJSON_PUBLIC(cJSON *) cJSON_CreateIntArray(const int *numbers, int count); +CJSON_PUBLIC(cJSON *) cJSON_CreateFloatArray(const float *numbers, int count); +CJSON_PUBLIC(cJSON *) cJSON_CreateDoubleArray(const double *numbers, int count); +CJSON_PUBLIC(cJSON *) cJSON_CreateStringArray(const char *const *strings, int count); + +/* Append item to the specified array/object. */ +CJSON_PUBLIC(cJSON_bool) cJSON_AddItemToArray(cJSON *array, cJSON *item); +CJSON_PUBLIC(cJSON_bool) cJSON_AddItemToObject(cJSON *object, const char *string, cJSON *item); +/* Use this when string is definitely const (i.e. a literal, or as good as), and will definitely survive the cJSON object. + * WARNING: When this function was used, make sure to always check that (item->type & cJSON_StringIsConst) is zero before + * writing to `item->string` */ +CJSON_PUBLIC(cJSON_bool) cJSON_AddItemToObjectCS(cJSON *object, const char *string, cJSON *item); +/* Append reference to item to the specified array/object. Use this when you want to add an existing cJSON to a new cJSON, but don't want to corrupt your existing cJSON. */ +CJSON_PUBLIC(cJSON_bool) cJSON_AddItemReferenceToArray(cJSON *array, cJSON *item); +CJSON_PUBLIC(cJSON_bool) cJSON_AddItemReferenceToObject(cJSON *object, const char *string, cJSON *item); + +/* Remove/Detach items from Arrays/Objects. */ +CJSON_PUBLIC(cJSON *) cJSON_DetachItemViaPointer(cJSON *parent, cJSON * const item); +CJSON_PUBLIC(cJSON *) cJSON_DetachItemFromArray(cJSON *array, int which); +CJSON_PUBLIC(void) cJSON_DeleteItemFromArray(cJSON *array, int which); +CJSON_PUBLIC(cJSON *) cJSON_DetachItemFromObject(cJSON *object, const char *string); +CJSON_PUBLIC(cJSON *) cJSON_DetachItemFromObjectCaseSensitive(cJSON *object, const char *string); +CJSON_PUBLIC(void) cJSON_DeleteItemFromObject(cJSON *object, const char *string); +CJSON_PUBLIC(void) cJSON_DeleteItemFromObjectCaseSensitive(cJSON *object, const char *string); + +/* Update array items. */ +CJSON_PUBLIC(cJSON_bool) cJSON_InsertItemInArray(cJSON *array, int which, cJSON *newitem); /* Shifts pre-existing items to the right. */ +CJSON_PUBLIC(cJSON_bool) cJSON_ReplaceItemViaPointer(cJSON * const parent, cJSON * const item, cJSON * replacement); +CJSON_PUBLIC(cJSON_bool) cJSON_ReplaceItemInArray(cJSON *array, int which, cJSON *newitem); +CJSON_PUBLIC(cJSON_bool) cJSON_ReplaceItemInObject(cJSON *object,const char *string,cJSON *newitem); +CJSON_PUBLIC(cJSON_bool) cJSON_ReplaceItemInObjectCaseSensitive(cJSON *object,const char *string,cJSON *newitem); + +/* Duplicate a cJSON item */ +CJSON_PUBLIC(cJSON *) cJSON_Duplicate(const cJSON *item, cJSON_bool recurse); +/* Duplicate will create a new, identical cJSON item to the one you pass, in new memory that will + * need to be released. With recurse!=0, it will duplicate any children connected to the item. + * The item->next and ->prev pointers are always zero on return from Duplicate. */ +/* Recursively compare two cJSON items for equality. If either a or b is NULL or invalid, they will be considered unequal. + * case_sensitive determines if object keys are treated case sensitive (1) or case insensitive (0) */ +CJSON_PUBLIC(cJSON_bool) cJSON_Compare(const cJSON * const a, const cJSON * const b, const cJSON_bool case_sensitive); + +/* Minify a strings, remove blank characters(such as ' ', '\t', '\r', '\n') from strings. + * The input pointer json cannot point to a read-only address area, such as a string constant, + * but should point to a readable and writable address area. */ +CJSON_PUBLIC(void) cJSON_Minify(char *json); + +/* Helper functions for creating and adding items to an object at the same time. + * They return the added item or NULL on failure. */ +CJSON_PUBLIC(cJSON*) cJSON_AddNullToObject(cJSON * const object, const char * const name); +CJSON_PUBLIC(cJSON*) cJSON_AddTrueToObject(cJSON * const object, const char * const name); +CJSON_PUBLIC(cJSON*) cJSON_AddFalseToObject(cJSON * const object, const char * const name); +CJSON_PUBLIC(cJSON*) cJSON_AddBoolToObject(cJSON * const object, const char * const name, const cJSON_bool boolean); +CJSON_PUBLIC(cJSON*) cJSON_AddNumberToObject(cJSON * const object, const char * const name, const double number); +CJSON_PUBLIC(cJSON*) cJSON_AddStringToObject(cJSON * const object, const char * const name, const char * const string); +CJSON_PUBLIC(cJSON*) cJSON_AddRawToObject(cJSON * const object, const char * const name, const char * const raw); +CJSON_PUBLIC(cJSON*) cJSON_AddObjectToObject(cJSON * const object, const char * const name); +CJSON_PUBLIC(cJSON*) cJSON_AddArrayToObject(cJSON * const object, const char * const name); + +/* When assigning an integer value, it needs to be propagated to valuedouble too. */ +#define cJSON_SetIntValue(object, number) ((object) ? (object)->valueint = (object)->valuedouble = (number) : (number)) +/* helper for the cJSON_SetNumberValue macro */ +CJSON_PUBLIC(double) cJSON_SetNumberHelper(cJSON *object, double number); +#define cJSON_SetNumberValue(object, number) ((object != NULL) ? cJSON_SetNumberHelper(object, (double)number) : (number)) +/* Change the valuestring of a cJSON_String object, only takes effect when type of object is cJSON_String */ +CJSON_PUBLIC(char*) cJSON_SetValuestring(cJSON *object, const char *valuestring); + +/* If the object is not a boolean type this does nothing and returns cJSON_Invalid else it returns the new type*/ +#define cJSON_SetBoolValue(object, boolValue) ( \ + (object != NULL && ((object)->type & (cJSON_False|cJSON_True))) ? \ + (object)->type=((object)->type &(~(cJSON_False|cJSON_True)))|((boolValue)?cJSON_True:cJSON_False) : \ + cJSON_Invalid\ +) + +/* Macro for iterating over an array or object */ +#define cJSON_ArrayForEach(element, array) for(element = (array != NULL) ? (array)->child : NULL; element != NULL; element = element->next) + +/* malloc/free objects using the malloc/free functions that have been set with cJSON_InitHooks */ +CJSON_PUBLIC(void *) cJSON_malloc(size_t size); +CJSON_PUBLIC(void) cJSON_free(void *object); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/examples/sbgEComExample/src/main.c b/examples/sbgEComExample/src/main.c new file mode 100644 index 0000000..57c866c --- /dev/null +++ b/examples/sbgEComExample/src/main.c @@ -0,0 +1,719 @@ +/*! + * \file main.c + * \author SBG Systems + * \date July 27, 2021 + * + * \brief C code sample demonstrating the usage of sbgECom and sbgInsRestApi. + * + * This simple C program: + * - Detects a device using sbgECom and sbgInsRestApi. + * - Configures the device to output data at 10 Hz. + * - Displays received data on the console. + * + * The code supports both serial and Ethernet UDP connections. + * It is compatible with: + * - ELLIPSE devices (firmware v3 and above) + * - High Performance INS (HPINS) products + * - PULSE Inertial Measurement Units (IMUs) + * + * + * The code is written in plain ANSI C and is designed to work on any POSIX + * and Windows system. With minor modifications, it can also run on bare-metal devices. + * + * Dependencies: + * - Uses the open-source cJSON library by Dave Gamble (https://github.com/DaveGamble/cJSON) + * to parse incoming JSON replies from the device. + * - Uses the argtable3 library to handle command-line argument parsing. + * + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * 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. + * + * \endlicense + */ + +// Standard Library +#include + +// sbgCommonLib headers +#include + +// Argtable3 headers +#include + +// sbgECom headers +#include + +// Local headers +#include "cJSON.h" +#include "restApiHelper.h" + +//----------------------------------------------------------------------// +//- Constant definitions -// +//----------------------------------------------------------------------// + +/*! + * Program name. + */ +#define PROGRAM_NAME "sbgEComExample" + +/*! + * Time to wait before trying to connect to the device following a software reboot + */ +#define SBG_ECOM_EXAMPLE_BOOT_TIME_MS (500) + +//----------------------------------------------------------------------// +//- Gloval private members -// +//----------------------------------------------------------------------// + +/*! + * Global variable used to gracefull exit the example when the user hit CTRL+C + */ +volatile sig_atomic_t gShouldExit = 0; + +//----------------------------------------------------------------------// +//- Private methods -// +//----------------------------------------------------------------------// + +/*! + * Callback definition used to print log error messages. + * + * \param[in] pFileName The file in which the log was triggered. + * \param[in] pFunctionName The function where the log was triggered. + * \param[in] line The line in the file where the log was triggered. + * \param[in] pCategory Category for this log or "None" if no category has been specified. + * \param[in] logType Associated log message level. + * \param[in] errorCode Associated error code or SBG_NO_ERROR for INFO & DEBUG level logs. + * \param[in] pMessage The message to log. + */ +static void sbgEComExamplePrintMsgLog(const char *pFileName, const char *pFunctionName, uint32_t line, const char *pCategory, SbgDebugLogType type, SbgErrorCode errorCode, const char *pMessage) +{ + const char *pTypeStr; + + assert(pFileName); + assert(pFunctionName); + assert(pCategory); + assert(pMessage); + + SBG_UNUSED_PARAMETER(pFileName); + SBG_UNUSED_PARAMETER(line); + + pTypeStr = sbgDebugLogTypeToStr(type); + + if (errorCode == SBG_NO_ERROR) + { + fprintf(stderr, "%-7s %s: %s\n", pTypeStr, pFunctionName, pMessage); + } + else + { + fprintf(stderr, "%-7s %s %s: %s\n", pTypeStr, sbgErrorCodeToString(errorCode), pFunctionName, pMessage); + } +} + +/*! + * Callback used to handle received sbgECom logs. + * + * \param[in] pECom SbgECom instance. + * \param[in] msgClass Class of the received message. + * \param[in] msg Received message ID. + * \param[in] pLogData Received data. + * \param[in] pUserArg Optional user argument. + * \return SBG_NO_ERROR if successful. + */ +static SbgErrorCode sbgEComExampleOnLogReceived(SbgEComHandle *pECom, SbgEComClass msgClass, SbgEComMsgId msg, const SbgEComLogUnion *pLogData, void *pUserArg) +{ + SBG_UNUSED_PARAMETER(pECom); + SBG_UNUSED_PARAMETER(pUserArg); + + assert(pLogData); + + if (msgClass == SBG_ECOM_CLASS_LOG_ECOM_0) + { + switch (msg) + { + case SBG_ECOM_LOG_IMU_SHORT: + printf("IMU : %" PRIu32 "\t%" PRIu16 "\t%0.3f\t%0.3f\t%0.3f\t%0.3f\t%0.3f\t%0.3f\t%0.3f\n", + pLogData->imuShort.timeStamp, + pLogData->imuShort.status, + sbgEComLogImuShortGetDeltaVelocity(&pLogData->imuShort, 0), + sbgEComLogImuShortGetDeltaVelocity(&pLogData->imuShort, 1), + sbgEComLogImuShortGetDeltaVelocity(&pLogData->imuShort, 2), + sbgRadToDegf(sbgEComLogImuShortGetDeltaAngle(&pLogData->imuShort, 0)), + sbgRadToDegf(sbgEComLogImuShortGetDeltaAngle(&pLogData->imuShort, 1)), + sbgRadToDegf(sbgEComLogImuShortGetDeltaAngle(&pLogData->imuShort, 2)), + sbgEComLogImuShortGetTemperature(&pLogData->imuShort)); + break; + + case SBG_ECOM_LOG_EKF_EULER: + printf("EULER: %" PRIu32 "\t%" PRIu16 "\t%0.3f\t%0.3f\t%0.3f\t%0.3f\t%0.3f\t%0.3f\t%0.3f\n", + pLogData->ekfEulerData.timeStamp, + pLogData->ekfEulerData.status, + sbgRadToDegf(pLogData->ekfEulerData.euler[0]), + sbgRadToDegf(pLogData->ekfEulerData.euler[1]), + sbgRadToDegf(pLogData->ekfEulerData.euler[2]), + sbgRadToDegf(pLogData->ekfEulerData.eulerStdDev[0]), + sbgRadToDegf(pLogData->ekfEulerData.eulerStdDev[1]), + sbgRadToDegf(pLogData->ekfEulerData.eulerStdDev[2]), + sbgRadToDegf(pLogData->ekfEulerData.magDeclination)); + break; + default: + break; + } + } + + return SBG_NO_ERROR; +} + +/*! + * Receive logs and only exit upon user request. + * + * \param[in] pECom SbgECom instance. + */ +static void sbgEComExampleProcessIncoming(SbgEComHandle *pECom) +{ + assert(pECom); + + sbgEComSetReceiveLogCallback(pECom, sbgEComExampleOnLogReceived, NULL); + + while (!gShouldExit) + { + SbgErrorCode errorCode; + + errorCode = sbgEComHandle(pECom); + + if (errorCode == SBG_NOT_READY) + { + sbgSleep(1); + } + } +} + +/*! + * Get and print product info. + * + * \param[in] pECom SbgECom instance. + * \return SBG_NO_ERROR if successful. + */ +static SbgErrorCode sbgEComExamplePrintDeviceInfo(SbgEComHandle *pECom) +{ + SbgEComCmdApiReply reply; + SbgErrorCode errorCode; + + assert(pECom); + + sbgEComPurgeIncoming(pECom); + + sbgEComCmdApiReplyConstruct(&reply); + + errorCode = sbgEComCmdApiGet(pECom, "/api/v1/info", NULL, &reply); + + if (errorCode == SBG_NO_ERROR) + { + cJSON *pJsonNode = cJSON_Parse(reply.pContent); + + if (pJsonNode) + { + DeviceInfo deviceInfo; + + errorCode = restApiHelperParseDeviceInfo(pJsonNode, &deviceInfo); + + if (errorCode == SBG_NO_ERROR) + { + printf(" product code: %s\n", deviceInfo.productCode); + printf(" serial number: %s\n", deviceInfo.serialNumber); + printf(" hardware revision: %s\n", deviceInfo.hwRevision); + printf(" manufacturing version: %s\n", deviceInfo.mnfVersion); + printf(" firmware version: %s\n", deviceInfo.fmwVersion); + printf(" bootLoader version: %s\n", deviceInfo.btVersion); + + printf("\n"); + } + else + { + SBG_LOG_ERROR(errorCode, "Received JSON is malformed"); + } + + cJSON_Delete(pJsonNode); + } + else + { + errorCode = SBG_ERROR; + SBG_LOG_ERROR(errorCode, "Unable to parse the JSON content"); + } + } + else + { + SBG_LOG_ERROR(errorCode, "unable to retrieve product info"); + } + + sbgEComCmdApiReplyDestroy(&reply); + + return errorCode; +} + +/*! + * Save new settings to FLASH memory and reboot the device + * + * \param[in] pECom SbgECom instance. + * \return SBG_NO_ERROR if successful. + */ +static SbgErrorCode sbgEComExampleSaveAndReboot(SbgEComHandle *pECom) +{ + SbgErrorCode errorCode; + SbgEComCmdApiReply reply; + + assert(pECom); + + sbgEComCmdApiReplyConstruct(&reply); + + // + // Save to FLASH memory + // + errorCode = sbgEComCmdApiPost(pECom, "/api/v1/settings/save", NULL, NULL, &reply); + + if (errorCode == SBG_NO_ERROR) + { + // + // Check the reply status code that is based on HTTP codes (200 - OK) + // + if (reply.statusCode == 200) + { + SBG_LOG_DEBUG("settings saved to FLASH memory"); + } + else + { + errorCode = SBG_INVALID_PARAMETER; + SBG_LOG_ERROR(errorCode, "unable to save settings to FLASH memory"); + + restApiHelperPrintErrorDetails(reply.pContent); + } + } + else + { + SBG_LOG_ERROR(errorCode, "unable to save settings to FLASH memory"); + } + + // + // Reboot the device for the new settings to be applied + // + errorCode = sbgEComCmdApiPost(pECom, "/api/v1/system/reboot", NULL, NULL, &reply); + + if (errorCode == SBG_NO_ERROR) + { + // + // Check the reply status code that is based on HTTP codes (200 - OK) + // + if (reply.statusCode == 200) + { + SBG_LOG_INFO("device is rebooting..."); + sbgSleep(SBG_ECOM_EXAMPLE_BOOT_TIME_MS); + } + else + { + errorCode = SBG_INVALID_PARAMETER; + SBG_LOG_ERROR(errorCode, "unable to reboot the device"); + + restApiHelperPrintErrorDetails(reply.pContent); + } + } + else + { + SBG_LOG_ERROR(errorCode, "unable to reboot the device"); + } + + sbgEComCmdApiReplyDestroy(&reply); + + return errorCode; +} + +/*! + * Configures the device to output IMU Short messages on Port A. + * + * This function demonstrates how to modify device settings using the SBG REST API. + * It also verifies the device response to determine if a reboot is required for + * the updated settings to take effect. + * + * \param[in] pECom SbgECom instance. + * \param[out] pNeedReboot Returns true if the applied settings require the device to reboot. + * \return SBG_NO_ERROR if the configuration was successful. + */ +static SbgErrorCode sbgEComExampleConfigurePortA(SbgEComHandle *pECom, bool *pNeedReboot) +{ + SbgEComCmdApiReply reply; + SbgErrorCode errorCode; + + assert(pECom); + assert(pNeedReboot); + + sbgEComCmdApiReplyConstruct(&reply); + + // + // Enable SBG_ECOM_LOG_IMU_SHORT on Port A at 10Hz + // + errorCode = sbgEComCmdApiPost(pECom, "/api/v1/settings/output/comA/messages/imuShort", NULL, "\"100ms\"", &reply); + + if (errorCode == SBG_NO_ERROR) + { + // + // Check if the POST request has been successfully exectued (HTTP code 200 - OK) + // + if (reply.statusCode == 200) + { + errorCode = restApiHelperParseSuccess(reply.pContent, pNeedReboot); + + if (errorCode != SBG_NO_ERROR) + { + SBG_LOG_ERROR(errorCode, "unable to parse answer from the product"); + } + } + else + { + errorCode = SBG_INVALID_PARAMETER; + SBG_LOG_ERROR(errorCode, "unable to configure IMU short log at 10Hz"); + + restApiHelperPrintErrorDetails(reply.pContent); + } + } + else + { + SBG_LOG_ERROR(errorCode, "unable to configure IMU short log at 10Hz"); + } + + sbgEComCmdApiReplyDestroy(&reply); + + return errorCode; +} + +/*! + * Configures the device to output EKF Euler messages on ETH0 + * + * This function demonstrates how to modify device settings using the SBG REST API. + * It also verifies the device response to determine if a reboot is required for + * the updated settings to take effect. + * + * \param[in] pECom SbgECom instance. + * \param[out] pNeedReboot Returns true if the applied settings require the device to reboot. + * \return SBG_NO_ERROR if the configuration was successful. + */ +static SbgErrorCode sbgEComExampleConfigureEth0(SbgEComHandle *pECom, bool *pNeedReboot) +{ + SbgEComCmdApiReply reply; + SbgErrorCode errorCode; + + assert(pECom); + assert(pNeedReboot); + + sbgEComCmdApiReplyConstruct(&reply); + + // + // Enable SBG_ECOM_LOG_EKF_EULER on Eth0 at 10Hz + // + errorCode = sbgEComCmdApiPost(pECom, "/api/v1/settings/output/eth0/messages/ekfEuler", NULL, "\"100ms\"", &reply); + + if (errorCode == SBG_NO_ERROR) + { + // + // Check if the POST request has been successfully exectued (HTTP code 200 - OK) + // + if (reply.statusCode == 200) + { + errorCode = restApiHelperParseSuccess(reply.pContent, pNeedReboot); + + if (errorCode != SBG_NO_ERROR) + { + SBG_LOG_ERROR(errorCode, "unable to parse answer from the product"); + } + } + else + { + errorCode = SBG_INVALID_PARAMETER; + SBG_LOG_ERROR(errorCode, "unable to configure EKF Euler log at 10Hz"); + + restApiHelperPrintErrorDetails(reply.pContent); + } + } + else + { + SBG_LOG_ERROR(errorCode, "unable to configure EKF Euler log at 10Hz"); + } + + sbgEComCmdApiReplyDestroy(&reply); + + return errorCode; +} + +/*! + * Detect and print product information, configure it and read data. + * + * \param[in] pInterface Interface. + * \return SBG_NO_ERROR if successful. + */ +static SbgErrorCode sbgEComExampleExecute(SbgInterface *pInterface) +{ + SbgErrorCode errorCode; + SbgEComHandle comHandle; + + assert(pInterface); + + errorCode = sbgEComInit(&comHandle, pInterface); + + if (errorCode == SBG_NO_ERROR) + { + errorCode = sbgEComExamplePrintDeviceInfo(&comHandle); + + if (errorCode == SBG_NO_ERROR) + { + bool needReboot; + + // + // Showcase device configuration using the sbgInsRestApi. + // + // For serial interface: + // - Assume a PULSE or ELLIPSE device that communicates via PORT A + // - Configure IMU Short output log on Port A at 10Hz + // + // For Ethernet UDP connection: + // - Assume an HPINS device that communicates via ETH0 + // - Configure EKF Euler output on ETH0 at 10Hz + // + if (sbgInterfaceTypeGet(pInterface) == SBG_IF_TYPE_SERIAL) + { + errorCode = sbgEComExampleConfigurePortA(&comHandle, &needReboot); + } + else + { + errorCode = sbgEComExampleConfigureEth0(&comHandle, &needReboot); + } + + // + // If new settings require a reboot to take effect, save to FLASH and reboot the device + // + if ( (errorCode == SBG_NO_ERROR) && (needReboot) ) + { + errorCode = sbgEComExampleSaveAndReboot(&comHandle); + + if (errorCode != SBG_NO_ERROR) + { + SBG_LOG_ERROR(errorCode, "unable to save new settings and reboot the device"); + } + } + } + + if (errorCode == SBG_NO_ERROR) + { + sbgEComPurgeIncoming(&comHandle); + sbgEComExampleProcessIncoming(&comHandle); + } + + sbgEComClose(&comHandle); + } + + return errorCode; +} + +/*! + * Intercept Ctrl+C events and Terminal close events to exit gracefully. + * + * \param[in] signalId Signal identifier that has triggered the event. + */ +static void signalHandler(int signalId) +{ + SBG_UNUSED_PARAMETER(signalId); + + // + // Notify main processing loop to stop the processing + // + gShouldExit = 1; +} + +/*! + * Register all signals events used to exit the logger gracefully + */ +static void registerSignalsEvents() +{ + signal(SIGINT, signalHandler); + signal(SIGABRT, signalHandler); + signal(SIGTERM, signalHandler); + +#ifdef SIGHUP + signal(SIGHUP, signalHandler); +#endif + +#ifdef SIGBREAK + signal(SIGBREAK, signalHandler); +#endif +} + +//----------------------------------------------------------------------// +// Main program entry point // +//----------------------------------------------------------------------// + +/*! + * Program entry point. + * + * \param[in] argc Number of input arguments. + * \param[in] argv Input arguments as an array of strings. + * \return EXIT_SUCCESS if successful. + */ +int main(int argc, char **argv) +{ + int exitCode = EXIT_SUCCESS; + bool printHelp = false; + + struct arg_lit *pHelpArg; + struct arg_lit *pVersionArg; + + struct arg_str *pSerialDeviceArg; + struct arg_int *pSerialBaudrateArg; + + struct arg_str *pUdpAddrArg; + struct arg_int *pUdpPortInArg; + struct arg_int *pUdpPortOutArg; + + struct arg_end *pEndArg; + + // + // TODO: add support for network interfaces. + // + void *argTable[] = + { + pHelpArg = arg_lit0( "h", "help", "display this help and exit"), + pVersionArg = arg_lit0( "v", "version", "display version info and exit"), + + pSerialDeviceArg = arg_str0( "s", "serial-device", "SERIAL_DEVICE", "open a serial interface"), + pSerialBaudrateArg = arg_int0( "r", "serial-baudrate", "SERIAL_BAUDRATE", "serial baudrate"), + + pUdpAddrArg = arg_str0( "a", "addr-ip", "IP address", "open an UDP interface"), + pUdpPortInArg = arg_int0( "I", "udp-port-in", "UDP port in", "UDP port to receive data from (local)"), + pUdpPortOutArg = arg_int0( "O", "udp-port-out", "UDP port out", "UDP port to send data to (remote)"), + + pEndArg = arg_end(20), + }; + + sbgCommonLibSetLogCallback(sbgEComExamplePrintMsgLog); + + if (arg_nullcheck(argTable) == 0) + { + int argError; + + argError = arg_parse(argc, argv, argTable); + + if (pHelpArg->count != 0) + { + printf("Usage: %s", PROGRAM_NAME); + arg_print_syntax(stdout, argTable, "\n\n"); + + + printf("sbgECom simple C example that display device info and print outputs.\n\n"); + printf(" Serial example: %s -s -r \n", PROGRAM_NAME); + printf(" UDP example: %s -a -I -O \n", PROGRAM_NAME); + + puts(""); + + arg_print_glossary(stdout, argTable, " %-50s %s\n"); + } + else if (pVersionArg->count != 0) + { + printf("%s\n", sbgEComGetVersionAsString()); + } + else if (argError == 0) + { + SbgErrorCode errorCode = SBG_INVALID_PARAMETER; + SbgInterface ecomInterface; + bool hasSerialConf = false; + bool hasUdpConf = false; + + if ( (pSerialDeviceArg->count != 0) && (pSerialBaudrateArg->count != 0) ) + { + hasSerialConf = true; + } + + if ( (pUdpAddrArg->count != 0) && (pUdpPortInArg->count != 0) && (pUdpPortOutArg->count != 0) ) + { + hasUdpConf = true; + } + + // + // Can't open at the same time a serial and UDP interface so check it + // + if (hasSerialConf && hasUdpConf) + { + SBG_LOG_ERROR(errorCode, "please select either a serial or an UDP interface no both"); + } + else if (hasSerialConf) + { + errorCode = sbgInterfaceSerialCreate(&ecomInterface, pSerialDeviceArg->sval[0], pSerialBaudrateArg->ival[0]); + } + else if (hasUdpConf) + { + errorCode = sbgInterfaceUdpCreate(&ecomInterface, sbgNetworkIpFromString(pUdpAddrArg->sval[0]), pUdpPortOutArg->ival[0], pUdpPortInArg->ival[0]); + + if (errorCode == SBG_NO_ERROR) + { + // + // Enable connected mode to only send/receive commands to the designed host + // + sbgInterfaceUdpSetConnectedMode(&ecomInterface, true); + } + } + + if (errorCode == SBG_NO_ERROR) + { + printf("Welcome to the sbgECom v%s C code sample.\n", sbgEComGetVersionAsString()); + printf("You can exit this example by pressing the CTRL+C keys.\n"); + puts(""); + + errorCode = sbgEComExampleExecute(&ecomInterface); + + sbgInterfaceDestroy(&ecomInterface); + } + else if (errorCode == SBG_INVALID_PARAMETER) + { + printHelp = true; + exitCode = EXIT_FAILURE; + } + else + { + SBG_LOG_ERROR(errorCode, "unable to open the serial or ethernet interface"); + exitCode = EXIT_FAILURE; + } + } + else + { + printHelp = true; + } + + if (printHelp) + { + arg_print_errors(stderr, pEndArg, PROGRAM_NAME); + fprintf(stderr, "Try '%s --help' for more information.\n", PROGRAM_NAME); + exitCode = EXIT_FAILURE; + } + + arg_freetable(argTable, SBG_ARRAY_SIZE(argTable)); + } + else + { + SBG_LOG_ERROR(SBG_MALLOC_FAILED, "unable to allocate memory"); + exitCode = EXIT_FAILURE; + } + + return exitCode; +} diff --git a/examples/sbgEComExample/src/restApiHelper.c b/examples/sbgEComExample/src/restApiHelper.c new file mode 100644 index 0000000..3160748 --- /dev/null +++ b/examples/sbgEComExample/src/restApiHelper.c @@ -0,0 +1,183 @@ +// sbgCommonLib headers +#include + +// Local headers +#include "cJSON.h" +#include "restApiHelper.h" + +//----------------------------------------------------------------------// +//- Public definitions -// +//----------------------------------------------------------------------// + +SbgErrorCode restApiHelperParseDeviceInfo(const cJSON *pJsonNode, DeviceInfo *pDeviceInfo) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + + assert(pJsonNode); + assert(pDeviceInfo); + + const cJSON *pProductCode = cJSON_GetObjectItemCaseSensitive(pJsonNode, "productCode"); + const cJSON *pSerialNumber = cJSON_GetObjectItemCaseSensitive(pJsonNode, "serialNumber"); + const cJSON *pHwRevision = cJSON_GetObjectItemCaseSensitive(pJsonNode, "hwRevision"); + const cJSON *pMnfVersion = cJSON_GetObjectItemCaseSensitive(pJsonNode, "mnfVersion"); + const cJSON *pCalibVersion = cJSON_GetObjectItemCaseSensitive(pJsonNode, "calibVersion"); + const cJSON *pFmwVersion = cJSON_GetObjectItemCaseSensitive(pJsonNode, "fmwVersion"); + const cJSON *pBtVersion = cJSON_GetObjectItemCaseSensitive(pJsonNode, "btVersion"); + + if (cJSON_IsString(pProductCode) && (pProductCode->valuestring) ) + { + strncpy(pDeviceInfo->productCode, pProductCode->valuestring, sizeof(pDeviceInfo->productCode)); + pDeviceInfo->productCode[sizeof(pDeviceInfo->productCode)-1] = '\0'; + } + else + { + errorCode = SBG_INVALID_PARAMETER; + SBG_LOG_WARNING(errorCode, "'productCode' is malformatted"); + } + + if (cJSON_IsString(pSerialNumber) && (pSerialNumber->valuestring) ) + { + strncpy(pDeviceInfo->serialNumber, pSerialNumber->valuestring, sizeof(pDeviceInfo->serialNumber)); + pDeviceInfo->serialNumber[sizeof(pDeviceInfo->serialNumber)-1] = '\0'; + } + else + { + errorCode = SBG_INVALID_PARAMETER; + SBG_LOG_WARNING(errorCode, "'serialNumber' is malformatted"); + } + + if (cJSON_IsString(pHwRevision) && (pHwRevision->valuestring) ) + { + strncpy(pDeviceInfo->hwRevision, pHwRevision->valuestring, sizeof(pDeviceInfo->hwRevision)); + pDeviceInfo->hwRevision[sizeof(pDeviceInfo->hwRevision)-1] = '\0'; + } + else + { + errorCode = SBG_INVALID_PARAMETER; + SBG_LOG_WARNING(errorCode, "'hwRevision' is malformatted"); + } + + // + // In initial sbgInsRestApi implementation, the field "mnfVersion" was called "calibVersion" + // Handle this specific case to support both implementations + // + // It will be removed in future release + // + if (cJSON_IsString(pMnfVersion) && (pMnfVersion->valuestring) ) + { + strncpy(pDeviceInfo->mnfVersion, pMnfVersion->valuestring, sizeof(pDeviceInfo->mnfVersion)); + pDeviceInfo->mnfVersion[sizeof(pDeviceInfo->mnfVersion)-1] = '\0'; + } + else if (cJSON_IsString(pCalibVersion) && (pCalibVersion->valuestring)) + { + strncpy(pDeviceInfo->mnfVersion, pCalibVersion->valuestring, sizeof(pDeviceInfo->mnfVersion)); + pDeviceInfo->mnfVersion[sizeof(pDeviceInfo->mnfVersion)-1] = '\0'; + } + else + { + errorCode = SBG_INVALID_PARAMETER; + SBG_LOG_WARNING(errorCode, "'mnfVersion' is malformatted"); + } + + if (cJSON_IsString(pFmwVersion) && (pFmwVersion->valuestring) ) + { + strncpy(pDeviceInfo->fmwVersion, pFmwVersion->valuestring, sizeof(pDeviceInfo->fmwVersion)); + pDeviceInfo->fmwVersion[sizeof(pDeviceInfo->fmwVersion)-1] = '\0'; + } + else + { + errorCode = SBG_INVALID_PARAMETER; + SBG_LOG_WARNING(errorCode, "'fmwVersion' is malformatted"); + } + + if (cJSON_IsString(pBtVersion) && (pBtVersion->valuestring) ) + { + strncpy(pDeviceInfo->btVersion, pBtVersion->valuestring, sizeof(pDeviceInfo->btVersion)); + pDeviceInfo->btVersion[sizeof(pDeviceInfo->btVersion)-1] = '\0'; + } + else + { + errorCode = SBG_INVALID_PARAMETER; + SBG_LOG_WARNING(errorCode, "'btVersion' is malformatted"); + } + + return errorCode; +} + +SbgErrorCode restApiHelperPrintErrorDetails(const char *pContent) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + cJSON *pJsonNode; + + assert(pContent); + + pJsonNode = cJSON_Parse(pContent); + + if (pJsonNode) + { + const cJSON *pStatus = cJSON_GetObjectItemCaseSensitive(pJsonNode, "status"); + const cJSON *pTitle = cJSON_GetObjectItemCaseSensitive(pJsonNode, "title"); + const cJSON *pDetail = cJSON_GetObjectItemCaseSensitive(pJsonNode, "detail"); + + if (cJSON_IsNumber(pStatus) && cJSON_IsString(pTitle) && cJSON_IsString(pDetail) ) + { + printf(" status: %"PRIu32"\n", pStatus->valueint); + printf(" title: %s\n", pTitle->valuestring); + printf(" details: %s\n", pDetail->valuestring); + printf("\n"); + } + else + { + errorCode = SBG_INVALID_PARAMETER; + SBG_LOG_WARNING(errorCode, "JSON is malformed"); + } + + cJSON_Delete(pJsonNode); + } + else + { + errorCode = SBG_ERROR; + SBG_LOG_ERROR(errorCode, "Unable to parse the JSON content"); + } + + return errorCode; +} + +SbgErrorCode restApiHelperParseSuccess(const char *pContent, bool *pNeedReboot) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + cJSON *pJsonNode; + + assert(pContent); + assert(pNeedReboot); + + pJsonNode = cJSON_Parse(pContent); + + if (pJsonNode) + { + const cJSON *pJsonNeedReboot = cJSON_GetObjectItemCaseSensitive(pJsonNode, "needReboot"); + + if (cJSON_IsTrue(pJsonNeedReboot)) + { + *pNeedReboot = true; + } + else if (cJSON_IsFalse(pJsonNeedReboot)) + { + *pNeedReboot = false; + } + else + { + errorCode = SBG_INVALID_PARAMETER; + SBG_LOG_WARNING(errorCode, "JSON is malformed"); + } + + cJSON_Delete(pJsonNode); + } + else + { + errorCode = SBG_ERROR; + SBG_LOG_ERROR(errorCode, "Unable to parse the JSON content"); + } + + return errorCode; +} diff --git a/examples/sbgEComExample/src/restApiHelper.h b/examples/sbgEComExample/src/restApiHelper.h new file mode 100644 index 0000000..baf42ab --- /dev/null +++ b/examples/sbgEComExample/src/restApiHelper.h @@ -0,0 +1,95 @@ + +/*! + * \file jsonHelper.h + * \ingroup main + * \author SBG Systems + * \date 2024-11-14 + * + * \brief Provides simple and naive JSON parsing helpers. + * + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * 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. + * + * \endlicense + */ + +#ifndef SBG_JSON_HELPER_H +#define SBG_JSON_HELPER_H + +#ifdef __cplusplus +extern "C" { +#endif + +// sbgCommonLib headers +#include + +// Local headers +#include "cJSON.h" + +//----------------------------------------------------------------------// +//- Public definition -// +//----------------------------------------------------------------------// + +/*! + * Store device information as returned by the sbgInsRestApi 'api/v1/info' endpoint. + */ +typedef struct _DeviceInfo +{ + char productCode[32]; + char serialNumber[32]; + char hwRevision[32]; + char mnfVersion[32]; + char fmwVersion[32]; + char btVersion[32]; +} DeviceInfo; + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * Parse a JSON node that contains the device information and return it. + * + * \param[in] pJsonNode The JSON node to parse the device information from. + * \param[out] pDeviceInfo Parsed device information. + * \return SBG_NO_ERROR if all fields have been parsed successfully. + */ +SbgErrorCode restApiHelperParseDeviceInfo(const cJSON *pJsonNode, DeviceInfo *pDeviceInfo); + +/*! + * Parse an error JSON payload to extract each field and print information. + * + * \param[in] pContent The JSON payload to parse + */ +SbgErrorCode restApiHelperPrintErrorDetails(const char *pContent); + +/*! + * Parse an error JSON payload to extract each field and print information. + * + * \param[in] pContent The JSON payload to parse + */ +SbgErrorCode restApiHelperParseSuccess(const char *pContent, bool *pNeedReboot); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_JSON_HELPER_H diff --git a/src/commands/sbgEComCmd.h b/src/commands/sbgEComCmd.h index 2620e5e..e2cc78f 100644 --- a/src/commands/sbgEComCmd.h +++ b/src/commands/sbgEComCmd.h @@ -1,13 +1,13 @@ /*! - * \file sbgEComCmd.h - * \ingroup commands - * \author SBG Systems - * \date 16 June 2014 + * \file sbgEComCmd.h + * \ingroup commands + * \author SBG Systems + * \date 16 June 2014 * - * \brief Include all available sbgECom commands. + * \brief Include all available sbgECom commands. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -31,8 +31,8 @@ */ /*! - * \defgroup commands Commands - * \brief Configuration and action commands mainly for ELLIPSE products. + * \defgroup commands Commands + * \brief Configuration and action commands mainly for ELLIPSE products. */ #ifndef SBG_ECOM_CMD_H diff --git a/src/commands/sbgEComCmdAdvanced.c b/src/commands/sbgEComCmdAdvanced.c index d0df61c..903a8f8 100644 --- a/src/commands/sbgEComCmdAdvanced.c +++ b/src/commands/sbgEComCmdAdvanced.c @@ -15,304 +15,304 @@ SbgErrorCode sbgEComCmdAdvancedGetConf(SbgEComHandle *pHandle, SbgEComAdvancedConf *pConf) { - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgEComProtocolPayload receivedPayload; - uint32_t trial; - - assert(pHandle); - assert(pConf); - - sbgEComProtocolPayloadConstruct(&receivedPayload); - - // - // 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 = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ADVANCED_CONF, &receivedPayload, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a SBG_ECOM_CMD_ADVANCED_CONF command - // - if (errorCode == SBG_NO_ERROR) - { - SbgStreamBuffer inputStream; - - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); - - // - // Read the mandatory time reference parameter - // - pConf->timeReference = (SbgEComTimeReferenceSrc)sbgStreamBufferReadUint8LE(&inputStream); - errorCode = sbgStreamBufferGetLastError(&inputStream); - - if (errorCode == SBG_NO_ERROR) - { - // - // The GNSS options parameter has been introduced in ELLIPSE firmware v2.2 - // We shouldn't report it as an error for older firmware - // - pConf->gnssOptions = sbgStreamBufferReadUint32LE(&inputStream); - - if (sbgStreamBufferGetLastError(&inputStream) == SBG_NO_ERROR) - { - // - // The NMEA options parameter has been introduced in ELLIPSE firmware v2.3 - // We shouldn't report it as an error for older firmware - // - pConf->nmeaOptions = sbgStreamBufferReadUint32LE(&inputStream); - - if (sbgStreamBufferGetLastError(&inputStream) == SBG_NO_ERROR) - { - errorCode = SBG_NO_ERROR; - } - else - { - pConf->nmeaOptions = 0; - } - } - else - { - pConf->gnssOptions = 0; - } - } - - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - sbgEComProtocolPayloadDestroy(&receivedPayload); - - return errorCode; + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pConf); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // 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 = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ADVANCED_CONF, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a SBG_ECOM_CMD_ADVANCED_CONF command + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // Read the mandatory time reference parameter + // + pConf->timeReference = (SbgEComTimeReferenceSrc)sbgStreamBufferReadUint8LE(&inputStream); + errorCode = sbgStreamBufferGetLastError(&inputStream); + + if (errorCode == SBG_NO_ERROR) + { + // + // The GNSS options parameter has been introduced in ELLIPSE firmware v2.2 + // We shouldn't report it as an error for older firmware + // + pConf->gnssOptions = sbgStreamBufferReadUint32LE(&inputStream); + + if (sbgStreamBufferGetLastError(&inputStream) == SBG_NO_ERROR) + { + // + // The NMEA options parameter has been introduced in ELLIPSE firmware v2.3 + // We shouldn't report it as an error for older firmware + // + pConf->nmeaOptions = sbgStreamBufferReadUint32LE(&inputStream); + + if (sbgStreamBufferGetLastError(&inputStream) == SBG_NO_ERROR) + { + errorCode = SBG_NO_ERROR; + } + else + { + pConf->nmeaOptions = 0; + } + } + else + { + pConf->gnssOptions = 0; + } + } + + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; } SbgErrorCode sbgEComCmdAdvancedSetConf(SbgEComHandle *pHandle, const SbgEComAdvancedConf *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)pConf->timeReference); - sbgStreamBufferWriteUint32LE(&outputStream, pConf->gnssOptions); - sbgStreamBufferWriteUint32LE(&outputStream, pConf->nmeaOptions); - - // - // 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; + 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)pConf->timeReference); + sbgStreamBufferWriteUint32LE(&outputStream, pConf->gnssOptions); + sbgStreamBufferWriteUint32LE(&outputStream, pConf->nmeaOptions); + + // + // 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; } SbgErrorCode sbgEComCmdAdvancedGetThresholds(SbgEComHandle *pHandle, SbgEComValidityThresholds *pConf) { - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgEComProtocolPayload receivedPayload; - uint32_t trial; - - assert(pHandle); - assert(pConf); - - sbgEComProtocolPayloadConstruct(&receivedPayload); - - // - // 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 = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_VALIDITY_THRESHOLDS, &receivedPayload, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a SBG_ECOM_CMD_VALIDITY_THRESHOLDS command - // - if (errorCode == SBG_NO_ERROR) - { - SbgStreamBuffer inputStream; - - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); - - // - // 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; - } - } - - sbgEComProtocolPayloadDestroy(&receivedPayload); - - return errorCode; + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pConf); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // 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 = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_VALIDITY_THRESHOLDS, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a SBG_ECOM_CMD_VALIDITY_THRESHOLDS command + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // 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; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; } SbgErrorCode sbgEComCmdAdvancedSetThresholds(SbgEComHandle *pHandle, const SbgEComValidityThresholds *pConf) { - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[4*sizeof(float)]; - 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; + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[4*sizeof(float)]; + 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/src/commands/sbgEComCmdAdvanced.h b/src/commands/sbgEComCmdAdvanced.h index d5fa38e..c537dd4 100644 --- a/src/commands/sbgEComCmdAdvanced.h +++ b/src/commands/sbgEComCmdAdvanced.h @@ -1,13 +1,13 @@ /*! - * \file sbgEComCmdAdvanced.h - * \ingroup commands - * \author SBG Systems - * \date 11 June 2014 + * \file sbgEComCmdAdvanced.h + * \ingroup commands + * \author SBG Systems + * \date 11 June 2014 * - * \brief Advanced settings related commands. + * \brief Advanced settings related commands. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -52,21 +52,21 @@ extern "C" { */ 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. */ + 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; /*! * List all options for the GNSS bitmask options */ -#define SBG_ECOM_GNSS_OPT_01 (uint32_t)(0x00000001 << 0) /*!< Reserved advanced GNSS option for ELLIPSE-N/D v3 */ +#define SBG_ECOM_GNSS_OPT_01 (uint32_t)(0x00000001 << 0) /*!< Reserved advanced GNSS option for ELLIPSE-N/D v3 */ /*! * List all options for the NMEA bitmask options */ -#define SBG_ECOM_NMEA_OPT_MODE_STD (uint32_t)(0x00000001 << 0) /*!< Output NMEA messages that complies with 82 chars limit */ -#define SBG_ECOM_NMEA_OPT_FORCE_UTC (uint32_t)(0x00000001 << 4) /*!< Always output time in NMEA messages even if invalid */ +#define SBG_ECOM_NMEA_OPT_MODE_STD (uint32_t)(0x00000001 << 0) /*!< Output NMEA messages that complies with 82 chars limit */ +#define SBG_ECOM_NMEA_OPT_FORCE_UTC (uint32_t)(0x00000001 << 4) /*!< Always output time in NMEA messages even if invalid */ //----------------------------------------------------------------------// //- Advanced configurations -// @@ -77,9 +77,9 @@ typedef enum _SbgEComTimeReferenceSrc */ typedef struct _SbgEComAdvancedConf { - SbgEComTimeReferenceSrc timeReference; /*!< Time reference source for clock alignment. */ - uint32_t gnssOptions; /*!< Advanced GNSS options - contact SBG Systems. */ - uint32_t nmeaOptions; /*!< Advanced NMEA output options. */ + SbgEComTimeReferenceSrc timeReference; /*!< Time reference source for clock alignment. */ + uint32_t gnssOptions; /*!< Advanced GNSS options - contact SBG Systems. */ + uint32_t nmeaOptions; /*!< Advanced NMEA output options. */ } SbgEComAdvancedConf; /*! @@ -88,10 +88,10 @@ typedef struct _SbgEComAdvancedConf */ 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) */ + 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; //----------------------------------------------------------------------// @@ -101,27 +101,27 @@ typedef struct _SbgEComValidityThresholds /*! * 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. + * \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. + * \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. + * \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); @@ -129,9 +129,9 @@ SbgErrorCode sbgEComCmdAdvancedGetThresholds(SbgEComHandle *pHandle, SbgEComVali /*! * 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. + * \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); diff --git a/src/commands/sbgEComCmdAirData.c b/src/commands/sbgEComCmdAirData.c index 5ae9a6c..76d36d8 100644 --- a/src/commands/sbgEComCmdAirData.c +++ b/src/commands/sbgEComCmdAirData.c @@ -16,308 +16,308 @@ SbgErrorCode sbgEComCmdAirDataSetModelId(SbgEComHandle *pHandle, SbgEComAirDataModelsIds modelId) { - assert(pHandle); + assert(pHandle); - return sbgEComCmdGenericSetModelId(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_AIRDATA_MODEL_ID, modelId); + 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; + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t modelIdAsUint; - assert(pHandle); - assert(pModelId); + assert(pHandle); + assert(pModelId); - errorCode = sbgEComCmdGenericGetModelId(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_AIRDATA_MODEL_ID, &modelIdAsUint); + errorCode = sbgEComCmdGenericGetModelId(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_AIRDATA_MODEL_ID, &modelIdAsUint); - if (errorCode == SBG_NO_ERROR) - { - *pModelId = (SbgEComAirDataModelsIds)modelIdAsUint; - } + if (errorCode == SBG_NO_ERROR) + { + *pModelId = (SbgEComAirDataModelsIds)modelIdAsUint; + } - return errorCode; + 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 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; - SbgEComProtocolPayload receivedPayload; - uint32_t trial; - - assert(pHandle); - assert(pLeverArm); - - sbgEComProtocolPayloadConstruct(&receivedPayload); - - // - // 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 = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_AIRDATA_LEVER_ARM, &receivedPayload, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a SBG_ECOM_CMD_GNSS_1_LEVER_ARM_ALIGNMENT command - // - if (errorCode == SBG_NO_ERROR) - { - SbgStreamBuffer inputStream; - - // - // Initialize stream buffer to parse the payload - // - sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); - - 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; - } - } - - sbgEComProtocolPayloadDestroy(&receivedPayload); - - return errorCode; + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pLeverArm); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // 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 = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_AIRDATA_LEVER_ARM, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a SBG_ECOM_CMD_GNSS_1_LEVER_ARM_ALIGNMENT command + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to parse the payload + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + 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; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + 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 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; - SbgEComProtocolPayload receivedPayload; - uint32_t trial; - - assert(pHandle); - assert(pRejectConf); - - sbgEComProtocolPayloadConstruct(&receivedPayload); - - // - // 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 = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_AIRDATA_REJECT_MODES, &receivedPayload, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a SBG_ECOM_CMD_GNSS_1_REJECT_MODES command - // - if (errorCode == SBG_NO_ERROR) - { - SbgStreamBuffer inputStream; - - // - // Initialize stream buffer to parse payload - // - sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); - - // - // 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; - } - } - - sbgEComProtocolPayloadDestroy(&receivedPayload); - - return errorCode; + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pRejectConf); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // 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 = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_AIRDATA_REJECT_MODES, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a SBG_ECOM_CMD_GNSS_1_REJECT_MODES command + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to parse payload + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // 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; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; } diff --git a/src/commands/sbgEComCmdAirData.h b/src/commands/sbgEComCmdAirData.h index 2d463f4..a426021 100644 --- a/src/commands/sbgEComCmdAirData.h +++ b/src/commands/sbgEComCmdAirData.h @@ -1,13 +1,13 @@ /*! - * \file sbgEComCmdAirData.h - * \ingroup commands - * \author SBG Systems - * \date 18 February 2019 + * \file sbgEComCmdAirData.h + * \ingroup commands + * \author SBG Systems + * \date 18 February 2019 * - * \brief AirData aiding module configuration commands. + * \brief AirData aiding module configuration commands. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -55,9 +55,9 @@ extern "C" { */ 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. */ + 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; /*! @@ -65,8 +65,8 @@ typedef enum _SbgEComAirDataModelsIds */ typedef struct _SbgEComAirDataRejectionConf { - SbgEComRejectionMode airspeed; /*!< Rejection mode for the true air speed measurement. */ - SbgEComRejectionMode altitude; /*!< Rejection mode for the barometric altitude measurement. */ + SbgEComRejectionMode airspeed; /*!< Rejection mode for the true air speed measurement. */ + SbgEComRejectionMode altitude; /*!< Rejection mode for the barometric altitude measurement. */ } SbgEComAirDataRejectionConf; //----------------------------------------------------------------------// @@ -76,54 +76,54 @@ typedef struct _SbgEComAirDataRejectionConf /*! * 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. + * \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. + * \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. + * \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. + * \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. + * \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. + * \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); diff --git a/src/commands/sbgEComCmdApi.c b/src/commands/sbgEComCmdApi.c index 4766a9b..00ac96f 100644 --- a/src/commands/sbgEComCmdApi.c +++ b/src/commands/sbgEComCmdApi.c @@ -16,12 +16,12 @@ /*! * Status code indicating success. */ -#define SBG_ECOM_CMD_API_STATUS_CODE_OK (200) +#define SBG_ECOM_CMD_API_STATUS_CODE_OK (200) /*! * Status code indicating an internal server error. */ -#define SBG_ECOM_CMD_API_STATUS_CODE_INTERNAL_SERVER_ERROR (500) +#define SBG_ECOM_CMD_API_STATUS_CODE_INTERNAL_SERVER_ERROR (500) //----------------------------------------------------------------------// //- Private methods -// @@ -30,58 +30,58 @@ /*! * Parse the payload of a REST API reply. * - * \param[in] pReply REST API reply. - * \return SBG_NO_ERROR if successful. + * \param[in] pReply REST API reply. + * \return SBG_NO_ERROR if successful. */ static SbgErrorCode sbgEComCmdApiReplyParsePayload(SbgEComCmdApiReply *pReply) { - SbgErrorCode errorCode; - SbgStreamBuffer streamBuffer; - uint16_t statusCode; - - assert(pReply); - - sbgStreamBufferInitForRead(&streamBuffer, sbgEComProtocolPayloadGetBuffer(&pReply->payload), sbgEComProtocolPayloadGetSize(&pReply->payload)); - - statusCode = sbgStreamBufferReadUint16LE(&streamBuffer); - - errorCode = sbgStreamBufferGetLastError(&streamBuffer); - - if (errorCode == SBG_NO_ERROR) - { - const char *pContent; - size_t size; - - pContent = sbgStreamBufferGetCursor(&streamBuffer); - size = sbgStreamBufferGetSpace(&streamBuffer); - - if (size != 0) - { - if (pContent[size - 1] == '\0') - { - pReply->statusCode = statusCode; - pReply->pContent = pContent; - - errorCode = SBG_NO_ERROR; - } - else - { - errorCode = SBG_INVALID_FRAME; - SBG_LOG_ERROR(errorCode, "invalid content format"); - } - } - else - { - errorCode = SBG_INVALID_FRAME; - SBG_LOG_ERROR(errorCode, "invalid content size"); - } - } - else - { - SBG_LOG_ERROR(errorCode, "unable to read status code"); - } - - return errorCode; + SbgErrorCode errorCode; + SbgStreamBuffer streamBuffer; + uint16_t statusCode; + + assert(pReply); + + sbgStreamBufferInitForRead(&streamBuffer, sbgEComProtocolPayloadGetBuffer(&pReply->payload), sbgEComProtocolPayloadGetSize(&pReply->payload)); + + statusCode = sbgStreamBufferReadUint16LE(&streamBuffer); + + errorCode = sbgStreamBufferGetLastError(&streamBuffer); + + if (errorCode == SBG_NO_ERROR) + { + const char *pContent; + size_t size; + + pContent = sbgStreamBufferGetCursor(&streamBuffer); + size = sbgStreamBufferGetSpace(&streamBuffer); + + if (size != 0) + { + if (pContent[size - 1] == '\0') + { + pReply->statusCode = statusCode; + pReply->pContent = pContent; + + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_INVALID_FRAME; + SBG_LOG_ERROR(errorCode, "invalid content format"); + } + } + else + { + errorCode = SBG_INVALID_FRAME; + SBG_LOG_ERROR(errorCode, "invalid content size"); + } + } + else + { + SBG_LOG_ERROR(errorCode, "unable to read status code"); + } + + return errorCode; } //----------------------------------------------------------------------// @@ -90,154 +90,158 @@ static SbgErrorCode sbgEComCmdApiReplyParsePayload(SbgEComCmdApiReply *pReply) void sbgEComCmdApiReplyConstruct(SbgEComCmdApiReply *pReply) { - assert(pReply); + assert(pReply); - sbgEComProtocolPayloadConstruct(&pReply->payload); + sbgEComProtocolPayloadConstruct(&pReply->payload); - pReply->statusCode = SBG_ECOM_CMD_API_STATUS_CODE_INTERNAL_SERVER_ERROR; - pReply->pContent = NULL; + pReply->statusCode = SBG_ECOM_CMD_API_STATUS_CODE_INTERNAL_SERVER_ERROR; + pReply->pContent = NULL; } void sbgEComCmdApiReplyDestroy(SbgEComCmdApiReply *pReply) { - assert(pReply); + assert(pReply); - sbgEComProtocolPayloadDestroy(&pReply->payload); + sbgEComProtocolPayloadDestroy(&pReply->payload); } bool sbgEComCmdApiReplySuccessful(const SbgEComCmdApiReply *pReply) { - assert(pReply); + assert(pReply); - return pReply->statusCode == SBG_ECOM_CMD_API_STATUS_CODE_OK; + return pReply->statusCode == SBG_ECOM_CMD_API_STATUS_CODE_OK; } SbgErrorCode sbgEComCmdApiGet(SbgEComHandle *pHandle, const char *pPath, const char *pQuery, SbgEComCmdApiReply *pReply) { - SbgErrorCode errorCode; - uint8_t *pSendBuffer; - SbgStreamBuffer streamBuffer; - size_t pathSize; - size_t querySize; - size_t size; - - assert(pPath); - - if (!pQuery) - { - pQuery = ""; - } - - pathSize = strlen(pPath) + 1; - querySize = strlen(pQuery) + 1; - size = pathSize + querySize; - - pSendBuffer = malloc(size); - - if (pSendBuffer) - { - sbgStreamBufferInitForWrite(&streamBuffer, pSendBuffer, size); - - sbgStreamBufferWriteBuffer(&streamBuffer, pPath, pathSize); - errorCode = sbgStreamBufferWriteBuffer(&streamBuffer, pQuery, querySize); - assert(errorCode == SBG_NO_ERROR); - - for (uint32_t i = 0; i < pHandle->numTrials; i++) - { - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_API_GET, sbgStreamBufferGetLinkedBuffer(&streamBuffer), sbgStreamBufferGetLength(&streamBuffer)); - - if (errorCode == SBG_NO_ERROR) - { - errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_API_GET, &pReply->payload, pHandle->cmdDefaultTimeOut); - - if (errorCode == SBG_NO_ERROR) - { - errorCode = sbgEComCmdApiReplyParsePayload(pReply); - break; - } - } - else - { - break; - } - } - - free(pSendBuffer); - } - else - { - errorCode = SBG_MALLOC_FAILED; - SBG_LOG_ERROR(errorCode, "unable allocate buffer"); - } - - return errorCode; + SbgErrorCode errorCode; + uint8_t *pSendBuffer; + SbgStreamBuffer streamBuffer; + size_t pathSize; + size_t querySize; + size_t size; + + assert(pHandle); + assert(pPath); + assert(pReply); + + if (!pQuery) + { + pQuery = ""; + } + + pathSize = strlen(pPath) + 1; + querySize = strlen(pQuery) + 1; + size = pathSize + querySize; + + pSendBuffer = malloc(size); + + if (pSendBuffer) + { + sbgStreamBufferInitForWrite(&streamBuffer, pSendBuffer, size); + + sbgStreamBufferWriteBuffer(&streamBuffer, pPath, pathSize); + errorCode = sbgStreamBufferWriteBuffer(&streamBuffer, pQuery, querySize); + assert(errorCode == SBG_NO_ERROR); + + for (uint32_t i = 0; i < pHandle->numTrials; i++) + { + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_API_GET, sbgStreamBufferGetLinkedBuffer(&streamBuffer), sbgStreamBufferGetLength(&streamBuffer)); + + if (errorCode == SBG_NO_ERROR) + { + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_API_GET, &pReply->payload, pHandle->cmdDefaultTimeOut); + + if (errorCode == SBG_NO_ERROR) + { + errorCode = sbgEComCmdApiReplyParsePayload(pReply); + break; + } + } + else + { + break; + } + } + + free(pSendBuffer); + } + else + { + errorCode = SBG_MALLOC_FAILED; + SBG_LOG_ERROR(errorCode, "unable allocate buffer"); + } + + return errorCode; } SbgErrorCode sbgEComCmdApiPost(SbgEComHandle *pHandle, const char *pPath, const char *pQuery, const char *pBody, SbgEComCmdApiReply *pReply) { - SbgErrorCode errorCode; - uint8_t *pSendBuffer; - SbgStreamBuffer streamBuffer; - size_t pathSize; - size_t querySize; - size_t bodySize; - size_t size; - - assert(pPath); - - if (!pQuery) - { - pQuery = ""; - } - - if (!pBody) - { - pBody = ""; - } - - pathSize = strlen(pPath) + 1; - querySize = strlen(pQuery) + 1; - bodySize = strlen(pBody) + 1; - size = pathSize + querySize + bodySize; - - pSendBuffer = malloc(size); - - if (pSendBuffer) - { - sbgStreamBufferInitForWrite(&streamBuffer, pSendBuffer, size); - - sbgStreamBufferWriteBuffer(&streamBuffer, pPath, pathSize); - sbgStreamBufferWriteBuffer(&streamBuffer, pQuery, querySize); - errorCode = sbgStreamBufferWriteBuffer(&streamBuffer, pBody, bodySize); - assert(errorCode == SBG_NO_ERROR); - - for (uint32_t i = 0; i < pHandle->numTrials; i++) - { - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_API_POST, sbgStreamBufferGetLinkedBuffer(&streamBuffer), sbgStreamBufferGetLength(&streamBuffer)); - - if (errorCode == SBG_NO_ERROR) - { - errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_API_POST, &pReply->payload, pHandle->cmdDefaultTimeOut); - - if (errorCode == SBG_NO_ERROR) - { - errorCode = sbgEComCmdApiReplyParsePayload(pReply); - break; - } - } - else - { - break; - } - } - - free(pSendBuffer); - } - else - { - errorCode = SBG_MALLOC_FAILED; - SBG_LOG_ERROR(errorCode, "unable allocate buffer"); - } - - return errorCode; + SbgErrorCode errorCode; + uint8_t *pSendBuffer; + SbgStreamBuffer streamBuffer; + size_t pathSize; + size_t querySize; + size_t bodySize; + size_t size; + + assert(pHandle); + assert(pPath); + assert(pReply); + + if (!pQuery) + { + pQuery = ""; + } + + if (!pBody) + { + pBody = ""; + } + + pathSize = strlen(pPath) + 1; + querySize = strlen(pQuery) + 1; + bodySize = strlen(pBody) + 1; + size = pathSize + querySize + bodySize; + + pSendBuffer = malloc(size); + + if (pSendBuffer) + { + sbgStreamBufferInitForWrite(&streamBuffer, pSendBuffer, size); + + sbgStreamBufferWriteBuffer(&streamBuffer, pPath, pathSize); + sbgStreamBufferWriteBuffer(&streamBuffer, pQuery, querySize); + errorCode = sbgStreamBufferWriteBuffer(&streamBuffer, pBody, bodySize); + assert(errorCode == SBG_NO_ERROR); + + for (uint32_t i = 0; i < pHandle->numTrials; i++) + { + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_API_POST, sbgStreamBufferGetLinkedBuffer(&streamBuffer), sbgStreamBufferGetLength(&streamBuffer)); + + if (errorCode == SBG_NO_ERROR) + { + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_API_POST, &pReply->payload, pHandle->cmdDefaultTimeOut); + + if (errorCode == SBG_NO_ERROR) + { + errorCode = sbgEComCmdApiReplyParsePayload(pReply); + break; + } + } + else + { + break; + } + } + + free(pSendBuffer); + } + else + { + errorCode = SBG_MALLOC_FAILED; + SBG_LOG_ERROR(errorCode, "unable allocate buffer"); + } + + return errorCode; } diff --git a/src/commands/sbgEComCmdApi.h b/src/commands/sbgEComCmdApi.h index ddc697d..2c8fb1a 100644 --- a/src/commands/sbgEComCmdApi.h +++ b/src/commands/sbgEComCmdApi.h @@ -1,13 +1,13 @@ /*! - * \file sbgEComCmdApi.h - * \ingroup commands - * \author SBG Systems - * \date October 14, 2020 + * \file sbgEComCmdApi.h + * \ingroup commands + * \author SBG Systems + * \date October 14, 2020 * - * \brief REST API related commands. + * \brief REST API related commands. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -57,9 +57,9 @@ extern "C" { */ typedef struct _SbgEComCmdApiReply { - SbgEComProtocolPayload payload; /*!< Payload. */ - uint16_t statusCode; /*!< Status code. */ - const char *pContent; /*!< Content. */ + SbgEComProtocolPayload payload; /*!< Payload. */ + uint16_t statusCode; /*!< Status code. */ + const char *pContent; /*!< Content. */ } SbgEComCmdApiReply; //----------------------------------------------------------------------// @@ -69,22 +69,22 @@ typedef struct _SbgEComCmdApiReply /*! * REST API reply constructor. * - * \param[in] pReply REST API reply. + * \param[in] pReply REST API reply. */ void sbgEComCmdApiReplyConstruct(SbgEComCmdApiReply *pReply); /*! * REST API reply destructor. * - * \param[in] pReply REST API reply. + * \param[in] pReply REST API reply. */ void sbgEComCmdApiReplyDestroy(SbgEComCmdApiReply *pReply); /*! * Check if a reply indicates successful command execution. * - * \param[in] pReply REST API reply. - * \return True if the reply indicates successful command execution. + * \param[in] pReply REST API reply. + * \return True if the reply indicates successful command execution. */ bool sbgEComCmdApiReplySuccessful(const SbgEComCmdApiReply *pReply); @@ -93,11 +93,11 @@ bool sbgEComCmdApiReplySuccessful(const SbgEComCmdApiReply *pReply); * * The reply must be destroyed before the next attempt to receive data, either logs or command replies. * - * \param[in] pHandle ECom handle. - * \param[in] pPath URI path component. - * \param[in] pQuery Query string, may be NULL. - * \param[out] pReply Reply. - * \return SBG_NO_ERROR if successful. + * \param[in] pHandle ECom handle. + * \param[in] pPath URI path component. + * \param[in] pQuery Query string, may be NULL. + * \param[out] pReply Reply. + * \return SBG_NO_ERROR if successful. */ SbgErrorCode sbgEComCmdApiGet(SbgEComHandle *pHandle, const char *pPath, const char *pQuery, SbgEComCmdApiReply *pReply); @@ -106,12 +106,12 @@ SbgErrorCode sbgEComCmdApiGet(SbgEComHandle *pHandle, const char *pPath, const c * * The reply must be destroyed before the next attempt to receive data, either logs or command replies. * - * \param[in] pHandle ECom handle. - * \param[in] pPath URI path component. - * \param[in] pQuery Query string, may be NULL. - * \param[in] pBody Body, may be NULL. - * \param[out] pReply Reply. - * \return SBG_NO_ERROR if successful. + * \param[in] pHandle ECom handle. + * \param[in] pPath URI path component. + * \param[in] pQuery Query string, may be NULL. + * \param[in] pBody Body, may be NULL. + * \param[out] pReply Reply. + * \return SBG_NO_ERROR if successful. */ SbgErrorCode sbgEComCmdApiPost(SbgEComHandle *pHandle, const char *pPath, const char *pQuery, const char *pBody, SbgEComCmdApiReply *pReply); diff --git a/src/commands/sbgEComCmdCommon.c b/src/commands/sbgEComCmdCommon.c index 816f278..bc26e36 100644 --- a/src/commands/sbgEComCmdCommon.c +++ b/src/commands/sbgEComCmdCommon.c @@ -14,261 +14,261 @@ SbgErrorCode sbgEComReceiveAnyCmd(SbgEComHandle *pHandle, uint8_t *pMsgClass, uint8_t *pMsgId, void *pData, size_t *pSize, size_t maxSize, uint32_t timeOut) { - SbgErrorCode errorCode; - SbgEComProtocolPayload payload; + SbgErrorCode errorCode; + SbgEComProtocolPayload payload; - assert(pHandle); - assert(pMsgClass); - assert(pMsgId); + assert(pHandle); + assert(pMsgClass); + assert(pMsgId); - sbgEComProtocolPayloadConstruct(&payload); + sbgEComProtocolPayloadConstruct(&payload); - errorCode = sbgEComReceiveAnyCmd2(pHandle, pMsgClass, pMsgId, &payload, timeOut); + errorCode = sbgEComReceiveAnyCmd2(pHandle, pMsgClass, pMsgId, &payload, timeOut); - if (errorCode == SBG_NO_ERROR) - { - size_t size; + if (errorCode == SBG_NO_ERROR) + { + size_t size; - size = sbgEComProtocolPayloadGetSize(&payload); + size = sbgEComProtocolPayloadGetSize(&payload); - if (size <= maxSize) - { - if (pData) - { - const void *pBuffer; + if (size <= maxSize) + { + if (pData) + { + const void *pBuffer; - pBuffer = sbgEComProtocolPayloadGetBuffer(&payload); + pBuffer = sbgEComProtocolPayloadGetBuffer(&payload); - memcpy(pData, pBuffer, size); - } + memcpy(pData, pBuffer, size); + } - if (pSize) - { - *pSize = size; - } - } - else - { - errorCode = SBG_BUFFER_OVERFLOW; - } - } + if (pSize) + { + *pSize = size; + } + } + else + { + errorCode = SBG_BUFFER_OVERFLOW; + } + } - sbgEComProtocolPayloadDestroy(&payload); + sbgEComProtocolPayloadDestroy(&payload); - return errorCode; + return errorCode; } SbgErrorCode sbgEComReceiveAnyCmd2(SbgEComHandle *pHandle, uint8_t *pMsgClass, uint8_t *pMsgId, SbgEComProtocolPayload *pPayload, uint32_t timeOut) { - SbgErrorCode errorCode; - uint32_t start; - - assert(pHandle); - - if (timeOut > 0) - { - start = sbgGetTime(); - } - else - { - // - // Avoid compiler warning - // - start = 0; - } - - for (;;) - { - uint8_t receivedMsgClass; - uint8_t receivedMsgId; - uint32_t now; - - errorCode = sbgEComProtocolReceive2(&pHandle->protocolHandle, &receivedMsgClass, &receivedMsgId, pPayload); - - if (errorCode == SBG_NO_ERROR) - { - if (sbgEComMsgClassIsALog((SbgEComClass)receivedMsgClass)) - { - if (pHandle->pReceiveLogCallback) - { - SbgEComLogUnion logData; - - errorCode = sbgEComLogParse((SbgEComClass)receivedMsgClass, receivedMsgId, sbgEComProtocolPayloadGetBuffer(pPayload), sbgEComProtocolPayloadGetSize(pPayload), &logData); - - if (errorCode == SBG_NO_ERROR) - { - pHandle->pReceiveLogCallback(pHandle, (SbgEComClass)receivedMsgClass, receivedMsgId, &logData, pHandle->pUserArg); - - sbgEComLogCleanup(&logData, (SbgEComClass)receivedMsgClass, (SbgEComMsgId)receivedMsgId); - } - } - } - else - { - if (pMsgClass) - { - *pMsgClass = receivedMsgClass; - } - - if (pMsgId) - { - *pMsgId = receivedMsgId; - } - - break; - } - } - - if (timeOut > 0) - { - // - // Only sleep if the Rx buffer is empty, otherwise we should retry ASAP to drain it - // - if (errorCode == SBG_NOT_READY) - { - sbgSleep(1); - } - - now = sbgGetTime(); - - if ((now - start) >= timeOut) - { - errorCode = SBG_TIME_OUT; - break; - } - } - else - { - if (errorCode == SBG_NO_ERROR) - { - errorCode = SBG_TIME_OUT; - } - else - { - errorCode = SBG_NOT_READY; - } - - break; - } - } - - return errorCode; + SbgErrorCode errorCode; + uint32_t start; + + assert(pHandle); + + if (timeOut > 0) + { + start = sbgGetTime(); + } + else + { + // + // Avoid compiler warning + // + start = 0; + } + + for (;;) + { + uint8_t receivedMsgClass; + uint8_t receivedMsgId; + uint32_t now; + + errorCode = sbgEComProtocolReceive2(&pHandle->protocolHandle, &receivedMsgClass, &receivedMsgId, pPayload); + + if (errorCode == SBG_NO_ERROR) + { + if (sbgEComMsgClassIsALog((SbgEComClass)receivedMsgClass)) + { + if (pHandle->pReceiveLogCallback) + { + SbgEComLogUnion logData; + + errorCode = sbgEComLogParse((SbgEComClass)receivedMsgClass, receivedMsgId, sbgEComProtocolPayloadGetBuffer(pPayload), sbgEComProtocolPayloadGetSize(pPayload), &logData); + + if (errorCode == SBG_NO_ERROR) + { + pHandle->pReceiveLogCallback(pHandle, (SbgEComClass)receivedMsgClass, receivedMsgId, &logData, pHandle->pUserArg); + + sbgEComLogCleanup(&logData, (SbgEComClass)receivedMsgClass, (SbgEComMsgId)receivedMsgId); + } + } + } + else + { + if (pMsgClass) + { + *pMsgClass = receivedMsgClass; + } + + if (pMsgId) + { + *pMsgId = receivedMsgId; + } + + break; + } + } + + if (timeOut > 0) + { + // + // Only sleep if the Rx buffer is empty, otherwise we should retry ASAP to drain it + // + if (errorCode == SBG_NOT_READY) + { + sbgSleep(1); + } + + now = sbgGetTime(); + + if ((now - start) >= timeOut) + { + errorCode = SBG_TIME_OUT; + break; + } + } + else + { + if (errorCode == SBG_NO_ERROR) + { + errorCode = SBG_TIME_OUT; + } + else + { + errorCode = SBG_NOT_READY; + } + + break; + } + } + + return errorCode; } SbgErrorCode sbgEComReceiveCmd(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msgId, void *pData, size_t *pSize, size_t maxSize, uint32_t timeOut) { - SbgErrorCode errorCode; - SbgEComProtocolPayload payload; + SbgErrorCode errorCode; + SbgEComProtocolPayload payload; - sbgEComProtocolPayloadConstruct(&payload); + sbgEComProtocolPayloadConstruct(&payload); - errorCode = sbgEComReceiveCmd2(pHandle, msgClass, msgId, &payload, timeOut); + errorCode = sbgEComReceiveCmd2(pHandle, msgClass, msgId, &payload, timeOut); - if (errorCode == SBG_NO_ERROR) - { - size_t size; + if (errorCode == SBG_NO_ERROR) + { + size_t size; - size = sbgEComProtocolPayloadGetSize(&payload); + size = sbgEComProtocolPayloadGetSize(&payload); - if (size <= maxSize) - { - if (pData) - { - const void *pBuffer; + if (size <= maxSize) + { + if (pData) + { + const void *pBuffer; - pBuffer = sbgEComProtocolPayloadGetBuffer(&payload); + pBuffer = sbgEComProtocolPayloadGetBuffer(&payload); - memcpy(pData, pBuffer, size); - } + memcpy(pData, pBuffer, size); + } - if (pSize) - { - *pSize = size; - } - } - else - { - errorCode = SBG_BUFFER_OVERFLOW; - } - } + if (pSize) + { + *pSize = size; + } + } + else + { + errorCode = SBG_BUFFER_OVERFLOW; + } + } - sbgEComProtocolPayloadDestroy(&payload); + sbgEComProtocolPayloadDestroy(&payload); - return errorCode; + return errorCode; } SbgErrorCode sbgEComReceiveCmd2(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msgId, SbgEComProtocolPayload *pPayload, uint32_t timeOut) { - SbgErrorCode errorCode; - uint32_t start; - - assert(pHandle); - - start = sbgGetTime(); - - for (;;) - { - uint8_t receivedMsgClass; - uint8_t receivedMsgId; - uint32_t now; - - errorCode = sbgEComReceiveAnyCmd2(pHandle, &receivedMsgClass, &receivedMsgId, pPayload, 0); - - if (errorCode == SBG_NO_ERROR) - { - if ((receivedMsgClass == msgClass) && (receivedMsgId == msgId)) - { - break; - } - else if ((receivedMsgClass == SBG_ECOM_CLASS_LOG_CMD_0) && (receivedMsgId == SBG_ECOM_CMD_ACK)) - { - SbgStreamBuffer streamBuffer; - uint8_t ackMsgClass; - uint8_t ackMsgId; - SbgErrorCode ackErrorCode; - - sbgStreamBufferInitForRead(&streamBuffer, sbgEComProtocolPayloadGetBuffer(pPayload), sbgEComProtocolPayloadGetSize(pPayload)); - - ackMsgId = sbgStreamBufferReadUint8(&streamBuffer); - ackMsgClass = sbgStreamBufferReadUint8(&streamBuffer); - ackErrorCode = (SbgErrorCode)sbgStreamBufferReadUint16LE(&streamBuffer); - - errorCode = sbgStreamBufferGetLastError(&streamBuffer); - - if ((errorCode == SBG_NO_ERROR) && (ackMsgClass == msgClass) && (ackMsgId == msgId)) - { - // - // If a successful ACK is expected, the caller should instead explicitly wait for it. - // Receiving a successful ACK that corresponds to the requested class/message is thus an error! - // - if (ackErrorCode != SBG_NO_ERROR) - { - errorCode = ackErrorCode; - } - else - { - errorCode = SBG_ERROR; - } - - break; - } - } - } - else if (errorCode == SBG_NOT_READY) - { - sbgSleep(1); - } - - now = sbgGetTime(); - - if ((now - start) >= timeOut) - { - errorCode = SBG_TIME_OUT; - break; - } - } - - return errorCode; + SbgErrorCode errorCode; + uint32_t start; + + assert(pHandle); + + start = sbgGetTime(); + + for (;;) + { + uint8_t receivedMsgClass; + uint8_t receivedMsgId; + uint32_t now; + + errorCode = sbgEComReceiveAnyCmd2(pHandle, &receivedMsgClass, &receivedMsgId, pPayload, 0); + + if (errorCode == SBG_NO_ERROR) + { + if ((receivedMsgClass == msgClass) && (receivedMsgId == msgId)) + { + break; + } + else if ((receivedMsgClass == SBG_ECOM_CLASS_LOG_CMD_0) && (receivedMsgId == SBG_ECOM_CMD_ACK)) + { + SbgStreamBuffer streamBuffer; + uint8_t ackMsgClass; + uint8_t ackMsgId; + SbgErrorCode ackErrorCode; + + sbgStreamBufferInitForRead(&streamBuffer, sbgEComProtocolPayloadGetBuffer(pPayload), sbgEComProtocolPayloadGetSize(pPayload)); + + ackMsgId = sbgStreamBufferReadUint8(&streamBuffer); + ackMsgClass = sbgStreamBufferReadUint8(&streamBuffer); + ackErrorCode = (SbgErrorCode)sbgStreamBufferReadUint16LE(&streamBuffer); + + errorCode = sbgStreamBufferGetLastError(&streamBuffer); + + if ((errorCode == SBG_NO_ERROR) && (ackMsgClass == msgClass) && (ackMsgId == msgId)) + { + // + // If a successful ACK is expected, the caller should instead explicitly wait for it. + // Receiving a successful ACK that corresponds to the requested class/message is thus an error! + // + if (ackErrorCode != SBG_NO_ERROR) + { + errorCode = ackErrorCode; + } + else + { + errorCode = SBG_ERROR; + } + + break; + } + } + } + else if (errorCode == SBG_NOT_READY) + { + sbgSleep(1); + } + + now = sbgGetTime(); + + if ((now - start) >= timeOut) + { + errorCode = SBG_TIME_OUT; + break; + } + } + + return errorCode; } //----------------------------------------------------------------------// @@ -277,86 +277,86 @@ SbgErrorCode sbgEComReceiveCmd2(SbgEComHandle *pHandle, uint8_t msgClass, uint8_ SbgErrorCode sbgEComWaitForAck(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, uint32_t timeOut) { - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgEComProtocolPayload receivedPayload; - uint8_t ackClass; - uint8_t ackMsg; - - assert(pHandle); - - sbgEComProtocolPayloadConstruct(&receivedPayload); - - // - // Try to receive the ACK and discard any other received log - // - errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ACK, &receivedPayload, timeOut); - - if (errorCode == SBG_NO_ERROR) - { - // - // Validate the received ACK frame - // - if (sbgEComProtocolPayloadGetSize(&receivedPayload) == 2*sizeof(uint16_t)) - { - SbgStreamBuffer inputStream; - - // - // Initialize a stream buffer to parse the received payload - // - sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); - - // - // 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)) - { - errorCode = (SbgErrorCode)sbgStreamBufferReadUint16LE(&inputStream); - } - else - { - errorCode = SBG_INVALID_FRAME; - SBG_LOG_WARNING(errorCode, "received ACK mismatch. expecting %#"PRIx8":%#"PRIx8" got %#"PRIx8":%#"PRIx8, msgClass, msg, ackClass, ackMsg); - } - } - else - { - errorCode = SBG_INVALID_FRAME; - SBG_LOG_WARNING(errorCode, "payload size is invalid for an ACK."); - } - } - - sbgEComProtocolPayloadDestroy(&receivedPayload); - - return errorCode; + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint8_t ackClass; + uint8_t ackMsg; + + assert(pHandle); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Try to receive the ACK and discard any other received log + // + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ACK, &receivedPayload, timeOut); + + if (errorCode == SBG_NO_ERROR) + { + // + // Validate the received ACK frame + // + if (sbgEComProtocolPayloadGetSize(&receivedPayload) == 2*sizeof(uint16_t)) + { + SbgStreamBuffer inputStream; + + // + // Initialize a stream buffer to parse the received payload + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // 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)) + { + errorCode = (SbgErrorCode)sbgStreamBufferReadUint16LE(&inputStream); + } + else + { + errorCode = SBG_INVALID_FRAME; + SBG_LOG_WARNING(errorCode, "received ACK mismatch. expecting %#"PRIx8":%#"PRIx8" got %#"PRIx8":%#"PRIx8, msgClass, msg, ackClass, ackMsg); + } + } + else + { + errorCode = SBG_INVALID_FRAME; + SBG_LOG_WARNING(errorCode, "payload size is invalid for an ACK."); + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; } SbgErrorCode sbgEComSendAck(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, SbgErrorCode cmdError) { - SbgStreamBuffer outputStream; - uint8_t outputBuffer[2*sizeof(uint8_t)+sizeof(uint16_t)]; - - assert(pHandle); - - // - // Initialize a stream buffer to write the command payload - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - - // - // 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)); + SbgStreamBuffer outputStream; + uint8_t outputBuffer[2*sizeof(uint8_t)+sizeof(uint16_t)]; + + assert(pHandle); + + // + // Initialize a stream buffer to write the command payload + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + + // + // 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)); } //----------------------------------------------------------------------// @@ -365,136 +365,136 @@ SbgErrorCode sbgEComSendAck(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t ms 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 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; - SbgEComProtocolPayload receivedPayload; - uint32_t trial; - - assert(pHandle); - assert(pModelId); - - sbgEComProtocolPayloadConstruct(&receivedPayload); - - // - // 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 = sbgEComReceiveCmd2(pHandle, msgClass, msg, &receivedPayload, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a the specified command - // - if (errorCode == SBG_NO_ERROR) - { - SbgStreamBuffer inputStream; - - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); - - // - // 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; - } - } - - sbgEComProtocolPayloadDestroy(&receivedPayload); - - return errorCode; + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pModelId); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // 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 = sbgEComReceiveCmd2(pHandle, msgClass, msg, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a the specified command + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // 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; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; } diff --git a/src/commands/sbgEComCmdCommon.h b/src/commands/sbgEComCmdCommon.h index d0e1d1d..575e5bb 100644 --- a/src/commands/sbgEComCmdCommon.h +++ b/src/commands/sbgEComCmdCommon.h @@ -1,13 +1,13 @@ /*! - * \file sbgEComCmdCommon.h - * \ingroup commands - * \author SBG Systems - * \date 11 June 2014 + * \file sbgEComCmdCommon.h + * \ingroup commands + * \author SBG Systems + * \date 11 June 2014 * - * \brief Definitions and methods common to all commands. + * \brief Definitions and methods common to all commands. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -47,16 +47,16 @@ extern "C" { //- Definitions -// //----------------------------------------------------------------------// -#define SBG_ECOM_DEFAULT_CMD_TIME_OUT (500) /*!< Default time out in ms for commands reception. */ +#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 */ + 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; /*! @@ -64,12 +64,12 @@ typedef enum _SbgEComRejectionMode */ 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. */ + 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; //----------------------------------------------------------------------// @@ -81,17 +81,17 @@ typedef enum _SbgEComAxisDirection * * All binary logs received are handled trough the standard callback system. * - * \param[in] pHandle SbgECom handle. - * \param[out] pMsgClass Message class. - * \param[out] pMsgId Message ID. - * \param[out] pData Data buffer, may be NULL. - * \param[out] pSize Number of bytes received, in bytes, may be NULL. - * \param[in] maxSize Data buffer size, in bytes. - * \param[in] timeOut Time-out, in ms. - * \return SBG_NO_ERROR if successful, - * SBG_TIME_OUT if no command message was received within the specified time out (even if timeOut = 0). - * SBG_NOT_READY to indicate the underlying interface is empty (only applicable when timeOut = 0). - * SBG_BUFFER_OVERFLOW if the payload of the received frame couldn't fit into the buffer, + * \param[in] pHandle SbgECom handle. + * \param[out] pMsgClass Message class. + * \param[out] pMsgId Message ID. + * \param[out] pData Data buffer, may be NULL. + * \param[out] pSize Number of bytes received, in bytes, may be NULL. + * \param[in] maxSize Data buffer size, in bytes. + * \param[in] timeOut Time-out, in ms. + * \return SBG_NO_ERROR if successful, + * SBG_TIME_OUT if no command message was received within the specified time out (even if timeOut = 0). + * SBG_NOT_READY to indicate the underlying interface is empty (only applicable when timeOut = 0). + * SBG_BUFFER_OVERFLOW if the payload of the received frame couldn't fit into the buffer, */ SbgErrorCode sbgEComReceiveAnyCmd(SbgEComHandle *pHandle, uint8_t *pMsgClass, uint8_t *pMsgId, void *pData, size_t *pSize, size_t maxSize, uint32_t timeOut); @@ -109,14 +109,14 @@ SbgErrorCode sbgEComReceiveAnyCmd(SbgEComHandle *pHandle, uint8_t *pMsgClass, ui * Because the payload buffer may directly refer to the protocol work buffer on return, it is only valid until * the next attempt to receive a frame, with any of the receive functions. * - * \param[in] pHandle SbgECom handle. - * \param[out] pMsgClass Message class. - * \param[out] pMsgId Message ID. - * \param[out] pPayload Payload. - * \param[in] timeOut Time-out, in ms. - * \return SBG_NO_ERROR if successful. - * SBG_TIME_OUT if no command message was received within the specified time out (even if timeOut = 0). - * SBG_NOT_READY to indicate the underlying interface is empty (only applicable when timeOut = 0). + * \param[in] pHandle SbgECom handle. + * \param[out] pMsgClass Message class. + * \param[out] pMsgId Message ID. + * \param[out] pPayload Payload. + * \param[in] timeOut Time-out, in ms. + * \return SBG_NO_ERROR if successful. + * SBG_TIME_OUT if no command message was received within the specified time out (even if timeOut = 0). + * SBG_NOT_READY to indicate the underlying interface is empty (only applicable when timeOut = 0). */ SbgErrorCode sbgEComReceiveAnyCmd2(SbgEComHandle *pHandle, uint8_t *pMsgClass, uint8_t *pMsgId, SbgEComProtocolPayload *pPayload, uint32_t timeOut); @@ -127,18 +127,18 @@ SbgErrorCode sbgEComReceiveAnyCmd2(SbgEComHandle *pHandle, uint8_t *pMsgClass, u * * All binary logs received during this time are handled trough the standard callback system. * - * \param[in] pHandle SbgECom handle. - * \param[in] msgClass Message class. - * \param[in] msgId Message ID. - * \param[out] pData Data buffer. - * \param[out] pSize Number of bytes received, in bytes. - * \param[in] maxSize Data buffer size, in bytes. - * \param[in] timeOut Time-out, in ms. - * \return SBG_NO_ERROR if successful, - * SBG_NOT_READY if no command message has been received, - * SBG_BUFFER_OVERFLOW if the payload of the received frame couldn't fit into the buffer, - * SBG_TIME_OUT if no command message was received within the specified time out, - * any error code reported by an ACK message for the given class and ID. + * \param[in] pHandle SbgECom handle. + * \param[in] msgClass Message class. + * \param[in] msgId Message ID. + * \param[out] pData Data buffer. + * \param[out] pSize Number of bytes received, in bytes. + * \param[in] maxSize Data buffer size, in bytes. + * \param[in] timeOut Time-out, in ms. + * \return SBG_NO_ERROR if successful, + * SBG_NOT_READY if no command message has been received, + * SBG_BUFFER_OVERFLOW if the payload of the received frame couldn't fit into the buffer, + * SBG_TIME_OUT if no command message was received within the specified time out, + * any error code reported by an ACK message for the given class and ID. */ SbgErrorCode sbgEComReceiveCmd(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msgId, void *pData, size_t *pSize, size_t maxSize, uint32_t timeOut); @@ -158,15 +158,15 @@ SbgErrorCode sbgEComReceiveCmd(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t * Because the payload buffer may directly refer to the protocol work buffer on return, it is only valid until * the next attempt to receive a frame, with any of the receive functions. * - * \param[in] pHandle SbgECom handle. - * \param[in] msgClass Message class. - * \param[in] msgId Message ID. - * \param[out] pPayload Payload. - * \param[in] timeOut Time-out, in ms. - * \return SBG_NO_ERROR if successful, - * SBG_NOT_READY if no command message has been received - * SBG_TIME_OUT if no command message was received within the specified time out, - * any error code reported by an ACK message for the given class and ID. + * \param[in] pHandle SbgECom handle. + * \param[in] msgClass Message class. + * \param[in] msgId Message ID. + * \param[out] pPayload Payload. + * \param[in] timeOut Time-out, in ms. + * \return SBG_NO_ERROR if successful, + * SBG_NOT_READY if no command message has been received + * SBG_TIME_OUT if no command message was received within the specified time out, + * any error code reported by an ACK message for the given class and ID. */ SbgErrorCode sbgEComReceiveCmd2(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msgId, SbgEComProtocolPayload *pPayload, uint32_t timeOut); @@ -177,22 +177,22 @@ SbgErrorCode sbgEComReceiveCmd2(SbgEComHandle *pHandle, uint8_t msgClass, uint8_ /*! * 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. + * \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. + * \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); @@ -203,22 +203,22 @@ SbgErrorCode sbgEComSendAck(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t ms /*! * 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. + * \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. + * \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); diff --git a/src/commands/sbgEComCmdDvl.c b/src/commands/sbgEComCmdDvl.c index 2972dda..5ce9185 100644 --- a/src/commands/sbgEComCmdDvl.c +++ b/src/commands/sbgEComCmdDvl.c @@ -15,320 +15,320 @@ SbgErrorCode sbgEComCmdDvlSetModelId(SbgEComHandle *pHandle, SbgEComDvlModelsIds modelId) { - assert(pHandle); + assert(pHandle); - return sbgEComCmdGenericSetModelId(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_DVL_MODEL_ID, modelId); + 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; + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t modelIdAsUint; - assert(pHandle); - assert(pModelId); + assert(pHandle); + assert(pModelId); - errorCode = sbgEComCmdGenericGetModelId(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_DVL_MODEL_ID, &modelIdAsUint); + 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; + if (errorCode == SBG_NO_ERROR) + { + *pModelId = (SbgEComDvlModelsIds)modelIdAsUint; + } + + return errorCode; } 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 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; - SbgEComProtocolPayload receivedPayload; - uint32_t trial; - - assert(pHandle); - assert(pDvlInstallation); - - sbgEComProtocolPayloadConstruct(&receivedPayload); - - // - // 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 = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_DVL_INSTALLATION, &receivedPayload, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a SBG_ECOM_CMD_GNSS_1_LEVER_ARM_ALIGNMENT command - // - if (errorCode == SBG_NO_ERROR) - { - SbgStreamBuffer inputStream; - - // - // Initialize stream buffer to parse the payload - // - sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); - - 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; - } - } - - sbgEComProtocolPayloadDestroy(&receivedPayload); - - return errorCode; + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pDvlInstallation); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // 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 = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_DVL_INSTALLATION, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a SBG_ECOM_CMD_GNSS_1_LEVER_ARM_ALIGNMENT command + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to parse the payload + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + 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; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + 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 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; - SbgEComProtocolPayload receivedPayload; - uint32_t trial; - - assert(pHandle); - assert(pRejectConf); - - sbgEComProtocolPayloadConstruct(&receivedPayload); - - // - // 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 = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_DVL_REJECT_MODES, &receivedPayload, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a SBG_ECOM_CMD_GNSS_1_REJECT_MODES command - // - if (errorCode == SBG_NO_ERROR) - { - SbgStreamBuffer inputStream; - - // - // Initialize stream buffer to parse payload - // - sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); - - // - // 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; - } - } - - sbgEComProtocolPayloadDestroy(&receivedPayload); - - return errorCode; + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pRejectConf); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // 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 = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_DVL_REJECT_MODES, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a SBG_ECOM_CMD_GNSS_1_REJECT_MODES command + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to parse payload + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // 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; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; } diff --git a/src/commands/sbgEComCmdDvl.h b/src/commands/sbgEComCmdDvl.h index 1283404..06f91e1 100644 --- a/src/commands/sbgEComCmdDvl.h +++ b/src/commands/sbgEComCmdDvl.h @@ -1,14 +1,14 @@ /*! - * \file sbgEComCmdDvl.h - * \ingroup commands - * \author SBG Systems - * \date 13 December 2018 + * \file sbgEComCmdDvl.h + * \ingroup commands + * \author SBG Systems + * \date 13 December 2018 * - * \brief DVL (Doppler Velocity Logger) aiding module configuration commands. + * \brief DVL (Doppler Velocity Logger) aiding module configuration commands. + * + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license - * * 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 @@ -26,7 +26,7 @@ * 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. - * + * * \endlicense */ @@ -55,27 +55,31 @@ extern "C" { */ typedef enum _SbgEComDvlModelsIds { - SBG_ECOM_DVL_MODEL_GENERIC_PD6 = 202, /*!< Generic DVL using PD6 protocol format. */ - SBG_ECOM_DVL_MODEL_WAYFINDER = 203 /*!< Teledyne Wayfinder DVL using proprietary protocol. */ + SBG_ECOM_DVL_MODEL_GENERIC_PD6 = 202, /*!< Generic DVL using PD6 protocol format. */ + SBG_ECOM_DVL_MODEL_WAYFINDER = 203, /*!< Teledyne Wayfinder DVL using proprietary protocol. */ + SBG_ECOM_DVL_MODEL_NORTEK = 204, /*!< Nortek DVL using proprietary protocol. */ } SbgEComDvlModelsIds; /*! - * DVL mechanical installation parameters such as lever arm and alignment + * DVL (Doppler Velocity Log) mechanical installation parameters, including lever arm and alignment. + * + * \note In ELLIPSE firmware 3.x and above, lever arms are always treated as precise, with no online re-estimation performed. + * The `preciseInstallation` field is deprecated and implicitly considered true. */ 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. */ + float leverArm[3]; /*!< DVL lever arm (X, Y, Z) in meters, expressed from the DVL to the IMU. */ + float alignment[3]; /*!< DVL alignment (roll, pitch, yaw) in radians. */ + bool preciseInstallation; /*!< [DEPRECATED] Indicates whether both the DVL lever arm and alignment are precise and do not require in-run estimation. */ } SbgEComDvlInstallation; /*! - * Holds all necessary information for DVL module data rejection. + * Contains configuration settings 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). */ + 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; //----------------------------------------------------------------------// @@ -85,54 +89,54 @@ typedef struct _SbgEComDvlRejectionConf /*! * 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. + * \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. + * \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. + * \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. + * \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. + * \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. + * \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); diff --git a/src/commands/sbgEComCmdEthernet.c b/src/commands/sbgEComCmdEthernet.c index ea89606..68860af 100644 --- a/src/commands/sbgEComCmdEthernet.c +++ b/src/commands/sbgEComCmdEthernet.c @@ -16,57 +16,57 @@ /*! * 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. + * \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); + 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. + * \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); + 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); } //----------------------------------------------------------------------// @@ -75,202 +75,202 @@ static SbgErrorCode sbgEComEthernetConfParse(SbgStreamBuffer *pInputStream, SbgE SbgErrorCode sbgEComEthernetGetConf(SbgEComHandle *pHandle, SbgEComEthernetConf *pEthernetConf) { - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgEComProtocolPayload receivedPayload; - uint32_t trial; - - assert(pHandle); - assert(pEthernetConf); - - sbgEComProtocolPayloadConstruct(&receivedPayload); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the command with no payload to retrieve 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 = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ETHERNET_CONF, &receivedPayload, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received correctly the answer - // - if (errorCode == SBG_NO_ERROR) - { - SbgStreamBuffer inputStream; - - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); - - // - // 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; - } - } - - sbgEComProtocolPayloadDestroy(&receivedPayload); - - return errorCode; + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pEthernetConf); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the command with no payload to retrieve 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 = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ETHERNET_CONF, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received correctly the answer + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // 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; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; } SbgErrorCode sbgEComEthernetSetConf(SbgEComHandle *pHandle, const SbgEComEthernetConf *pEthernetConf) { - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[24]; - 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; + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[24]; + 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; } SbgErrorCode sbgEComEthernetInfo(SbgEComHandle *pHandle, SbgEComEthernetConf *pEthernetConf) { - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgEComProtocolPayload receivedPayload; - uint32_t trial; - - assert(pHandle); - assert(pEthernetConf); - - sbgEComProtocolPayloadConstruct(&receivedPayload); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the command with no payload to retrieve 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 = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ETHERNET_INFO, &receivedPayload, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received correctly the answer - // - if (errorCode == SBG_NO_ERROR) - { - SbgStreamBuffer inputStream; - - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); - - // - // 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; - } - } - - sbgEComProtocolPayloadDestroy(&receivedPayload); - - return errorCode; + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pEthernetConf); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the command with no payload to retrieve 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 = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ETHERNET_INFO, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received correctly the answer + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // 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; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; } diff --git a/src/commands/sbgEComCmdEthernet.h b/src/commands/sbgEComCmdEthernet.h index 6044fb5..32cf385 100644 --- a/src/commands/sbgEComCmdEthernet.h +++ b/src/commands/sbgEComCmdEthernet.h @@ -1,13 +1,13 @@ /*! - * \file sbgEComCmdEthernet.h - * \ingroup commands - * \author SBG Systems - * \date 14 November 2016 + * \file sbgEComCmdEthernet.h + * \ingroup commands + * \author SBG Systems + * \date 14 November 2016 * - * \brief Ethernet configuration related commands. + * \brief Ethernet configuration related commands. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -52,8 +52,8 @@ extern "C" { */ 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. */ + 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; /*! @@ -61,12 +61,12 @@ typedef enum _SbgEComEthernetMode */ typedef struct _SbgEComEthernetConf { - SbgEComEthernetMode mode; /*!< Define how the device will acquire 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. */ + SbgEComEthernetMode mode; /*!< Define how the device will acquire 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; //----------------------------------------------------------------------// @@ -76,21 +76,21 @@ typedef struct _SbgEComEthernetConf /*! * 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. + * \note This method only returns the Ethernet configuration and NOT the IP address currently used by the device. * You should rather use sbgEComEthernetInfo to retrieve the current assigned IP. * - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pEthernetConf SbgEComEthernetConf struct to hold the read configuration from the device. - * \return SBG_NO_ERROR if the command has been executed successfully. + * \param[in] pHandle A valid sbgECom handle. + * \param[out] pEthernetConf SbgEComEthernetConf struct to hold 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 SbgEComEthernetConf struct to hold the new configuration to apply. - * \return SBG_NO_ERROR if the command has been executed successfully. + * \param[in] pHandle A valid sbgECom handle. + * \param[in] pEthernetConf SbgEComEthernetConf struct to hold the new configuration to apply. + * \return SBG_NO_ERROR if the command has been executed successfully. */ SbgErrorCode sbgEComEthernetSetConf(SbgEComHandle *pHandle, const SbgEComEthernetConf *pEthernetConf); @@ -100,9 +100,9 @@ SbgErrorCode sbgEComEthernetSetConf(SbgEComHandle *pHandle, const SbgEComEtherne * 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 SbgEComEthernetConf struct to hold the read IP settings from the device. - * \return SBG_NO_ERROR if the command has been executed successfully. + * \param[in] pHandle A valid sbgECom handle. + * \param[out] pEthernetConf SbgEComEthernetConf struct to hold the read IP settings from the device. + * \return SBG_NO_ERROR if the command has been executed successfully. */ SbgErrorCode sbgEComEthernetInfo(SbgEComHandle *pHandle, SbgEComEthernetConf *pEthernetConf); diff --git a/src/commands/sbgEComCmdEvent.c b/src/commands/sbgEComCmdEvent.c index 6303e6e..d553f19 100644 --- a/src/commands/sbgEComCmdEvent.c +++ b/src/commands/sbgEComCmdEvent.c @@ -15,280 +15,280 @@ SbgErrorCode sbgEComCmdSyncInGetConf(SbgEComHandle *pHandle, SbgEComSyncInId syncInId, SbgEComSyncInConf *pConf) { - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgEComProtocolPayload receivedPayload; - uint32_t trial; - - assert(pHandle); - assert(pConf); - - sbgEComProtocolPayloadConstruct(&receivedPayload); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - uint8_t syncInIdParam = syncInId; - - // - // Send the command with syncInId as a 1-byte payload - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_SYNC_IN_CONF, &syncInIdParam, sizeof(syncInIdParam)); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_SYNC_IN_CONF, &receivedPayload, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a SBG_ECOM_CMD_SYNC_IN_CONF command - // - if (errorCode == SBG_NO_ERROR) - { - SbgStreamBuffer inputStream; - - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); - - // - // 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; - } - } - - sbgEComProtocolPayloadDestroy(&receivedPayload); - - return errorCode; + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pConf); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + uint8_t syncInIdParam = syncInId; + + // + // Send the command with syncInId as a 1-byte payload + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_SYNC_IN_CONF, &syncInIdParam, sizeof(syncInIdParam)); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_SYNC_IN_CONF, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a SBG_ECOM_CMD_SYNC_IN_CONF command + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // 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; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; } SbgErrorCode sbgEComCmdSyncInSetConf(SbgEComHandle *pHandle, SbgEComSyncInId syncInId, const SbgEComSyncInConf *pConf) { - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[8]; - 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; + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[8]; + 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; } SbgErrorCode sbgEComCmdSyncOutGetConf(SbgEComHandle *pHandle, SbgEComSyncOutId syncOutId, SbgEComSyncOutConf *pConf) { - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgEComProtocolPayload receivedPayload; - uint32_t trial; - - assert(pHandle); - assert(pConf); - - sbgEComProtocolPayloadConstruct(&receivedPayload); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - uint8_t syncOutIdParam = syncOutId; - - // - // Send the command with syncOutId as a 1-byte payload - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_SYNC_OUT_CONF, &syncOutIdParam, sizeof(syncOutIdParam)); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_SYNC_OUT_CONF, &receivedPayload, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a SBG_ECOM_CMD_SYNC_OUT_CONF command - // - if (errorCode == SBG_NO_ERROR) - { - SbgStreamBuffer inputStream; - - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); - - // - // 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; - } - } - - sbgEComProtocolPayloadDestroy(&receivedPayload); - - return errorCode; + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pConf); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + uint8_t syncOutIdParam = syncOutId; + + // + // Send the command with syncOutId as a 1-byte payload + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_SYNC_OUT_CONF, &syncOutIdParam, sizeof(syncOutIdParam)); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_SYNC_OUT_CONF, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a SBG_ECOM_CMD_SYNC_OUT_CONF command + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // 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; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; } SbgErrorCode sbgEComCmdSyncOutSetConf(SbgEComHandle *pHandle, SbgEComSyncOutId syncOutId, const SbgEComSyncOutConf *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)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; + 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)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/src/commands/sbgEComCmdEvent.h b/src/commands/sbgEComCmdEvent.h index 8d6b955..31920ff 100644 --- a/src/commands/sbgEComCmdEvent.h +++ b/src/commands/sbgEComCmdEvent.h @@ -1,13 +1,13 @@ /*! - * \file sbgEComCmdEvent.h - * \ingroup commands - * \author SBG Systems - * \date 11 June 2014 + * \file sbgEComCmdEvent.h + * \ingroup commands + * \author SBG Systems + * \date 11 June 2014 * - * \brief Input/output event markers configuration commands. + * \brief Input/output event markers configuration commands. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -52,10 +52,10 @@ extern "C" { */ 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 */ + 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; /*! @@ -63,10 +63,10 @@ typedef enum _SbgEComSyncInId */ 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). */ + 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; /*! @@ -74,8 +74,8 @@ typedef enum _SbgEComSyncInSensitivity */ typedef enum _SbgEComSyncOutId { - SBG_ECOM_SYNC_OUT_A = 0, /*!< Synchronization output A */ - SBG_ECOM_SYNC_OUT_B = 1 /*!< Synchronization output B */ + SBG_ECOM_SYNC_OUT_A = 0, /*!< Synchronization output A */ + SBG_ECOM_SYNC_OUT_B = 1 /*!< Synchronization output B */ } SbgEComSyncOutId; /*! @@ -83,27 +83,28 @@ typedef enum _SbgEComSyncOutId */ 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 the following products: - - ELLIPSE-N with hardware revisions above 1.2.1.0 - - ELLIPSE-N & ELLIPSE-D Generation 3 or above. - Polarity and duration parameters are ignored with this specific mode. */ - + 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_5 = 5, /*!< Output is generated at 40Hz. (only ELLIPSE fmw v3) */ + 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_100 = 100, /*!< Output is generated at 2Hz. (only ELLIPSE fmw v3). */ + SBG_ECOM_SYNC_OUT_MODE_DIV_200 = 200, /*!< Output is generated at 1Hz. */ + + SBG_ECOM_SYNC_OUT_MODE_1_MS = 1001, /*!< Output is generated at 1000 Hz. (only ELLIPSE fmw v3) */ + SBG_ECOM_SYNC_OUT_MODE_2_MS = 1002, /*!< Output is generated at 500 Hz. (only ELLIPSE fmw v3) */ + SBG_ECOM_SYNC_OUT_MODE_4_MS = 1004, /*!< Output is generated at 250 Hz. (only ELLIPSE fmw v3) */ + + SBG_ECOM_SYNC_OUT_MODE_PPS = 10000, /*!< Pulse Per Second. Same as SBG_ECOM_SYNC_OUT_MODE_DIV_200. */ + 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 the following products: + - ELLIPSE-N with hardware revisions above 1.2.1.0 + - ELLIPSE-N & ELLIPSE-D Generation 3 or above. + Polarity and duration parameters are ignored with this specific mode. */ } SbgEComSyncOutFunction; /*! @@ -111,9 +112,9 @@ typedef enum _SbgEComSyncOutFunction */ 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 */ + 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; //----------------------------------------------------------------------// @@ -125,8 +126,8 @@ typedef enum _SbgEComSyncOutPolarity */ typedef struct _SbgEComSyncInConf { - SbgEComSyncInSensitivity sensitivity; /*!< Sensitivity of the sync in. */ - int32_t delay; /*!< Delay to take into account for the sync in. (in ns)*/ + SbgEComSyncInSensitivity sensitivity; /*!< Sensitivity of the sync in. */ + int32_t delay; /*!< Delay to take into account for the sync in. (in ns)*/ } SbgEComSyncInConf; /*! @@ -134,9 +135,9 @@ typedef struct _SbgEComSyncInConf */ 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). */ + 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; //----------------------------------------------------------------------// @@ -144,42 +145,42 @@ typedef struct _SbgEComSyncOutConf //----------------------------------------------------------------------// /*! - * Retrieve the configuration of a Sync In. + * 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. + * \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. + * 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. + * \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. + * 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. + * \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. + * 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. + * \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); diff --git a/src/commands/sbgEComCmdFeatures.c b/src/commands/sbgEComCmdFeatures.c index efec8d3..dd7f766 100644 --- a/src/commands/sbgEComCmdFeatures.c +++ b/src/commands/sbgEComCmdFeatures.c @@ -15,86 +15,86 @@ SbgErrorCode sbgEComCmdGetFeatures(SbgEComHandle *pHandle, SbgEComFeatures *pFeatures) { - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgEComProtocolPayload receivedPayload; - uint32_t trial; - - assert(pHandle); - assert(pFeatures); + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pFeatures); - sbgEComProtocolPayloadConstruct(&receivedPayload); + sbgEComProtocolPayloadConstruct(&receivedPayload); - // - // 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); + // + // 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 = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_FEATURES, &receivedPayload, pHandle->cmdDefaultTimeOut); + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_FEATURES, &receivedPayload, pHandle->cmdDefaultTimeOut); - // - // Test if we have received a SBG_ECOM_CMD_GPS_FEATURES command - // - if (errorCode == SBG_NO_ERROR) - { - SbgStreamBuffer inputStream; + // + // Test if we have received a SBG_ECOM_CMD_GPS_FEATURES command + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); - // - // 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)); - - // - // Only parse the GNSS firmware version if available - // - if (sbgStreamBufferGetSpace(&inputStream) > 0) - { - sbgStreamBufferReadBuffer(&inputStream, pFeatures->gnssFirmwareVersion, 32 * sizeof(char)); - } - else - { - strcpy(pFeatures->gnssFirmwareVersion, ""); - } - - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } + // + // 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)); + + // + // Only parse the GNSS firmware version if available + // + if (sbgStreamBufferGetSpace(&inputStream) > 0) + { + sbgStreamBufferReadBuffer(&inputStream, pFeatures->gnssFirmwareVersion, 32 * sizeof(char)); + } + else + { + strcpy(pFeatures->gnssFirmwareVersion, ""); + } + + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } - sbgEComProtocolPayloadDestroy(&receivedPayload); - - return errorCode; + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; } diff --git a/src/commands/sbgEComCmdFeatures.h b/src/commands/sbgEComCmdFeatures.h index 0e311db..e2e6fac 100644 --- a/src/commands/sbgEComCmdFeatures.h +++ b/src/commands/sbgEComCmdFeatures.h @@ -1,13 +1,13 @@ /*! - * \file sbgEComCmdFeatures.h - * \ingroup commands - * \author SBG Systems - * \date 19 March 2015 + * \file sbgEComCmdFeatures.h + * \ingroup commands + * \author SBG Systems + * \date 19 March 2015 * - * \brief Commands used to query supported device features. + * \brief Commands used to query supported device features. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -50,39 +50,41 @@ extern "C" { // // 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) */ +#define SBG_ECOM_SENSOR_FEATURE_IMU (0x00000001 << 0) /*!< The device can provide IMU data */ +#define SBG_ECOM_SENSOR_FEATURE_AHRS (0x00000001 << 1) /*!< The device can provide orientation data */ +#define SBG_ECOM_SENSOR_FEATURE_NAVIGATION (0x00000001 << 2) /*!< The device can provide position and velocity data */ +#define SBG_ECOM_SENSOR_FEATURE_SHIP_MOTION (0x00000001 << 3) /*!< The device 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) */ +#define SBG_ECOM_GNSS_SIGNAL_GPS_L1 (0x00000001 << 0) /*!< The GNSS receiver tracks GPS L1 band. */ +#define SBG_ECOM_GNSS_SIGNAL_GPS_L2 (0x00000001 << 1) /*!< The GNSS receiver tracks GPS L2 band. */ +#define SBG_ECOM_GNSS_SIGNAL_GPS_L5 (0x00000001 << 2) /*!< The GNSS receiver tracks GPS L5 band. */ +#define SBG_ECOM_GNSS_SIGNAL_GLONASS_L1 (0x00000001 << 3) /*!< The GNSS receiver tracks GLONASS L1 band. */ +#define SBG_ECOM_GNSS_SIGNAL_GLONASS_L2 (0x00000001 << 4) /*!< The GNSS receiver tracks GLONASS L2 band. */ +#define SBG_ECOM_GNSS_SIGNAL_BEIDOU_B1 (0x00000001 << 5) /*!< The GNSS receiver tracks BEIDOU B1 band. */ +#define SBG_ECOM_GNSS_SIGNAL_BEIDOU_B2 (0x00000001 << 6) /*!< The GNSS receiver tracks BEIDOU B2 band. */ +#define SBG_ECOM_GNSS_SIGNAL_BEIDOU_B3 (0x00000001 << 7) /*!< The GNSS receiver tracks BEIDOU B3 band. */ +#define SBG_ECOM_GNSS_SIGNAL_GALILEO_E1 (0x00000001 << 8) /*!< The GNSS receiver tracks GALILEO E1 band. */ +#define SBG_ECOM_GNSS_SIGNAL_GALILEO_E5 (0x00000001 << 9) /*!< The GNSS receiver tracks GALILEO E5 band. */ +#define SBG_ECOM_GNSS_SIGNAL_GALILEO_E6 (0x00000001 << 10) /*!< The GNSS receiver tracks GALILEO E6 band. */ +#define SBG_ECOM_GNSS_SIGNAL_QZSS (0x00000001 << 11) /*!< The GNSS receiver tracks QZSS signals */ +#define SBG_ECOM_GNSS_SIGNAL_SBAS (0x00000001 << 12) /*!< The GNSS receiver tracks SBAS signals */ +#define SBG_ECOM_GNSS_SIGNAL_L_BAND (0x00000001 << 13) /*!< The 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) */ +#define SBG_ECOM_GNSS_FEATURE_DUAL_ANT (0x00000001 << 0) /*!< The GNSS receiver provides a dual antenna heading */ +#define SBG_ECOM_GNSS_FEATURE_RTK_LIMITED (0x00000001 << 1) /*!< The GNSS receiver has limited RTK accuracy (eg. Trimble RTK 30/30) */ +#define SBG_ECOM_GNSS_FEATURE_RTK (0x00000001 << 2) /*!< The GNSS receiver provides full RTK accuracy */ +#define SBG_ECOM_GNSS_FEATURE_PPP (0x00000001 << 3) /*!< The GNSS receiver provides PPP computations */ +#define SBG_ECOM_GNSS_FEATURE_RAW_DATA (0x00000001 << 4) /*!< The GNSS receiver provides RAW data output */ +#define SBG_ECOM_GNSS_FEATURE_RAIM (0x00000001 << 5) /*!< The GNSS receiver provides Receiver Autonomous Integrity Monitoring */ +#define SBG_ECOM_GNSS_FEATURE_HIGH_SPEED (0x00000001 << 6) /*!< The GNSS receiver has no high speed limitation (> 515m/s) */ +#define SBG_ECOM_GNSS_FEATURE_IM (0x00000001 << 7) /*!< The GNSS receiver is able to detect and mitigate RF interferences. */ +#define SBG_ECOM_GNSS_FEATURE_OSNMA (0x00000001 << 8) /*!< The GNSS receiver implements Galileo OSNMA spoofing detection. */ //----------------------------------------------------------------------// //- Feature commands types definition -// @@ -94,15 +96,15 @@ extern "C" { */ 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 */ - SBG_ECOM_GNSS_TYPE_UBX_F9P = 8, /*!< Ublox ZED-F9P module */ + 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 */ + SBG_ECOM_GNSS_TYPE_UBX_F9P = 8, /*!< Ublox ZED-F9P module */ } SbgEComGnssType; /*! @@ -110,14 +112,14 @@ typedef enum _SbgEComGnssType */ 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) */ - char gnssFirmwareVersion[32]; /*!< String containing the GNSS receiver firmware version ("\0" if unknown) */ + 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) */ + char gnssFirmwareVersion[32]; /*!< String containing the GNSS receiver firmware version ("\0" if unknown) */ } SbgEComFeatures; //----------------------------------------------------------------------// @@ -127,9 +129,9 @@ typedef struct _SbgEComFeatures /*! * 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. + * \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); diff --git a/src/commands/sbgEComCmdGnss.c b/src/commands/sbgEComCmdGnss.c index 91d28c6..c82c8d4 100644 --- a/src/commands/sbgEComCmdGnss.c +++ b/src/commands/sbgEComCmdGnss.c @@ -16,358 +16,358 @@ /*! * 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. + * \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, SbgEComGnssModelsStdIds modelId, SbgEComCmd cmdId) { - assert(pHandle); + assert(pHandle); - return sbgEComCmdGenericSetModelId(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, cmdId, modelId); + return sbgEComCmdGenericSetModelId(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, cmdId, modelId); } /*! * Retrieve GNSS error model id. * - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pModelId Retrieved model id. - * \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. + * \param[in] pHandle A valid sbgECom handle. + * \param[out] pModelId Retrieved model id. + * \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 sbgEComCmdGnssGetModelId(SbgEComHandle *pHandle, SbgEComGnssModelsStdIds *pModelId, SbgEComCmd cmdId) { - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t modelIdAsUint; + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t modelIdAsUint; - assert(pHandle); - assert(pModelId); + assert(pHandle); + assert(pModelId); - errorCode = sbgEComCmdGenericGetModelId(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, cmdId, &modelIdAsUint); + errorCode = sbgEComCmdGenericGetModelId(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, cmdId, &modelIdAsUint); - if (errorCode == SBG_NO_ERROR) - { - *pModelId = (SbgEComGnssModelsStdIds)modelIdAsUint; - } + if (errorCode == SBG_NO_ERROR) + { + *pModelId = (SbgEComGnssModelsStdIds)modelIdAsUint; + } - return errorCode; + return errorCode; } /*! * 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. + * \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; - SbgEComProtocolPayload receivedPayload; - uint32_t trial; - - assert(pHandle); - assert(pGnssInstallation); - - sbgEComProtocolPayloadConstruct(&receivedPayload); - - // - // 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 = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, cmdId, &receivedPayload, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a valid answer - // - if (errorCode == SBG_NO_ERROR) - { - SbgStreamBuffer inputStream; - - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); - - // - // 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; - } - } - - sbgEComProtocolPayloadDestroy(&receivedPayload); - - return errorCode; + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pGnssInstallation); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // 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 = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, cmdId, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a valid answer + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // 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; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + 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. + * \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[64]; - 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; + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[64]; + 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. + * \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; - SbgEComProtocolPayload receivedPayload; - uint32_t trial; - - assert(pHandle); - assert(pRejectConf); - - sbgEComProtocolPayloadConstruct(&receivedPayload); - - // - // 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 = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, cmdId, &receivedPayload, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a SBG_ECOM_CMD_GNSS_1_REJECT_MODES command - // - if (errorCode == SBG_NO_ERROR) - { - SbgStreamBuffer inputStream; - - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); - - // - // 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; - } - } - - sbgEComProtocolPayloadDestroy(&receivedPayload); - - return errorCode; + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pRejectConf); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // 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 = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, cmdId, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a SBG_ECOM_CMD_GNSS_1_REJECT_MODES command + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // 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; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + 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. + * \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[64]; - 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; + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[64]; + 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; } //----------------------------------------------------------------------// @@ -376,47 +376,47 @@ static SbgErrorCode sbgEComCmdGnssSetRejection(SbgEComHandle *pHandle, const Sbg SbgErrorCode sbgEComCmdGnss1SetModelId(SbgEComHandle *pHandle, SbgEComGnssModelsStdIds modelId) { - assert(pHandle); + assert(pHandle); - return sbgEComCmdGnssSetModelId(pHandle, modelId, SBG_ECOM_CMD_GNSS_1_MODEL_ID); + return sbgEComCmdGnssSetModelId(pHandle, modelId, SBG_ECOM_CMD_GNSS_1_MODEL_ID); } SbgErrorCode sbgEComCmdGnss1GetModelId(SbgEComHandle *pHandle, SbgEComGnssModelsStdIds *pModelId) { - assert(pHandle); - assert(pModelId); + assert(pHandle); + assert(pModelId); - return sbgEComCmdGnssGetModelId(pHandle, pModelId, SBG_ECOM_CMD_GNSS_1_MODEL_ID); + return sbgEComCmdGnssGetModelId(pHandle, pModelId, SBG_ECOM_CMD_GNSS_1_MODEL_ID); } SbgErrorCode sbgEComCmdGnss1InstallationGet(SbgEComHandle *pHandle, SbgEComGnssInstallation *pGnssInstallation) { - assert(pHandle); - assert(pGnssInstallation); + assert(pHandle); + assert(pGnssInstallation); - return sbgEComCmdGnssInstallationGet(pHandle, pGnssInstallation, SBG_ECOM_CMD_GNSS_1_INSTALLATION); + return sbgEComCmdGnssInstallationGet(pHandle, pGnssInstallation, SBG_ECOM_CMD_GNSS_1_INSTALLATION); } SbgErrorCode sbgEComCmdGnss1InstallationSet(SbgEComHandle *pHandle, const SbgEComGnssInstallation *pGnssInstallation) { - assert(pHandle); - assert(pGnssInstallation); + assert(pHandle); + assert(pGnssInstallation); - return sbgEComCmdGnssInstallationSet(pHandle, pGnssInstallation, SBG_ECOM_CMD_GNSS_1_INSTALLATION); + return sbgEComCmdGnssInstallationSet(pHandle, pGnssInstallation, SBG_ECOM_CMD_GNSS_1_INSTALLATION); } SbgErrorCode sbgEComCmdGnss1GetRejection(SbgEComHandle *pHandle, SbgEComGnssRejectionConf *pRejectConf) { - assert(pHandle); - assert(pRejectConf); + assert(pHandle); + assert(pRejectConf); - return sbgEComCmdGnssGetRejection(pHandle, pRejectConf, SBG_ECOM_CMD_GNSS_1_REJECT_MODES); + return sbgEComCmdGnssGetRejection(pHandle, pRejectConf, SBG_ECOM_CMD_GNSS_1_REJECT_MODES); } SbgErrorCode sbgEComCmdGnss1SetRejection(SbgEComHandle *pHandle, const SbgEComGnssRejectionConf *pRejectConf) { - assert(pHandle); - assert(pRejectConf); + assert(pHandle); + assert(pRejectConf); - return sbgEComCmdGnssSetRejection(pHandle, pRejectConf, SBG_ECOM_CMD_GNSS_1_REJECT_MODES); + return sbgEComCmdGnssSetRejection(pHandle, pRejectConf, SBG_ECOM_CMD_GNSS_1_REJECT_MODES); } diff --git a/src/commands/sbgEComCmdGnss.h b/src/commands/sbgEComCmdGnss.h index e4bab66..cd21861 100644 --- a/src/commands/sbgEComCmdGnss.h +++ b/src/commands/sbgEComCmdGnss.h @@ -1,13 +1,13 @@ /*! - * \file sbgEComCmdGnss.h - * \ingroup commands - * \author SBG Systems - * \date 11 June 2014 + * \file sbgEComCmdGnss.h + * \ingroup commands + * \author SBG Systems + * \date 11 June 2014 * - * \brief GNSS aiding module configuration commands. + * \brief GNSS aiding module configuration commands. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -55,49 +55,55 @@ extern "C" { */ typedef enum _SbgEComGnssModelsStdIds { - SBG_ECOM_GNSS_MODEL_INTERNAL = 101, /*!< Default internal GNSS for ELLIPSE-N and ELLIPSE-D */ - SBG_ECOM_GNSS_MODEL_NMEA = 102, /*!< ELLIPSE-E to accept an external GNSS using NMEA protocol */ - SBG_ECOM_GNSS_MODEL_UBLOX_GPS_BEIDOU = 103, /*!< Only for ELLIPSE-N hardware 1 & 2 to select GPS+BEIDOU instead of the default GPS+GLONASS */ - SBG_ECOM_GNSS_MODEL_UBLOX_EXTERNAL = 104, /*!< ELLIPSE-E to accept an external Ublox GNSS (receive only - passive) */ - SBG_ECOM_GNSS_MODEL_RESERVED_01 = 105, /*!< Reserved, do not use */ - SBG_ECOM_GNSS_MODEL_NOVATEL_EXTERNAL = 106, /*!< ELLIPSE-E to accept an external Novatel GNSS (receive only - passive) */ - SBG_ECOM_GNSS_MODEL_RESERVED_02 = 107, /*!< Reserved, do not use */ - SBG_ECOM_GNSS_MODEL_RESERVED_03 = 108, /*!< Reserved, do not use */ - SBG_ECOM_GNSS_MODEL_SEPTENTRIO_EXTERNAL = 109, /*!< ELLIPSE-E to accept an external Septentrio GNSS(receive only - passive) */ - SBG_ECOM_GNSS_MODEL_RESERVED_04 = 110 /*!< Reserved, do not use */ + SBG_ECOM_GNSS_MODEL_INTERNAL = 101, /*!< Default internal GNSS for ELLIPSE-N and ELLIPSE-D */ + SBG_ECOM_GNSS_MODEL_NMEA = 102, /*!< ELLIPSE-E to accept an external GNSS using NMEA protocol */ + SBG_ECOM_GNSS_MODEL_UBLOX_GPS_BEIDOU = 103, /*!< Only for ELLIPSE-N hardware 1 & 2 to select GPS+BEIDOU instead of the default GPS+GLONASS */ + SBG_ECOM_GNSS_MODEL_UBLOX_EXTERNAL = 104, /*!< ELLIPSE-E to accept an external Ublox GNSS (receive only - passive) */ + SBG_ECOM_GNSS_MODEL_RESERVED_01 = 105, /*!< Reserved, do not use */ + SBG_ECOM_GNSS_MODEL_NOVATEL_EXTERNAL = 106, /*!< ELLIPSE-E to accept an external Novatel GNSS (receive only - passive) */ + SBG_ECOM_GNSS_MODEL_RESERVED_02 = 107, /*!< Reserved, do not use */ + SBG_ECOM_GNSS_MODEL_RESERVED_03 = 108, /*!< Reserved, do not use */ + SBG_ECOM_GNSS_MODEL_SEPTENTRIO_EXTERNAL = 109, /*!< ELLIPSE-E to accept an external Septentrio GNSS(receive only - passive) */ + SBG_ECOM_GNSS_MODEL_RESERVED_04 = 110 /*!< Reserved, do not use */ } SbgEComGnssModelsStdIds; /*! - * GNSS mechanical installation modes for the dual antenna mode. + * GNSS mechanical installation modes for dual antenna configurations. + * + * \note In ELLIPSE firmware 3.x and above, lever arms are always treated as precise, with no online re-estimation performed. + * The mode `SBG_ECOM_GNSS_INSTALLATION_MODE_DUAL_ROUGH` is internally resolved as `SBG_ECOM_GNSS_INSTALLATION_MODE_DUAL_PRECISE`. */ 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. */ + SBG_ECOM_GNSS_INSTALLATION_MODE_SINGLE = 1, /*!< GNSS operates in single antenna mode; the secondary lever arm is not used. */ + SBG_ECOM_GNSS_INSTALLATION_MODE_DUAL_AUTO = 2, /*!< [Reserved] GNSS uses dual antenna data, but the secondary lever arm is not specified. */ + SBG_ECOM_GNSS_INSTALLATION_MODE_DUAL_ROUGH = 3, /*!< [Deprecated] GNSS uses dual antenna data with an approximate secondary lever arm position. */ + SBG_ECOM_GNSS_INSTALLATION_MODE_DUAL_PRECISE = 4 /*!< GNSS uses dual antenna data with a precisely specified secondary lever arm; no online re-estimation needed. */ } SbgEComGnssInstallationMode; /*! - * GNSS mechanical installation parameters to be used with command SBG_ECOM_CMD_GNSS_#_INSTALLATION + * GNSS mechanical installation parameters for use with the command `SBG_ECOM_CMD_GNSS_#_INSTALLATION`. + * + * \note In ELLIPSE firmware 3.x and above, lever arms are always treated as precise, with no online re-estimation performed. + * The `leverArmPrimaryPrecise` field is deprecated and implicitly considered true. */ 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 leverArmPrimary[3]; /*!< Primary GNSS antenna lever arm in IMU X, Y, Z axes (meters). */ + bool leverArmPrimaryPrecise; /*!< [Deprecated] Indicates whether the primary lever arm is accurately specified; online re-estimation is not required. */ - 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. */ + float leverArmSecondary[3]; /*!< Secondary GNSS antenna lever arm in IMU X, Y, Z axes (meters). */ + SbgEComGnssInstallationMode leverArmSecondaryMode; /*!< Defines the operating mode for the secondary (dual) antenna. */ } SbgEComGnssInstallation; /*! - * Holds all necessary information for GNSS module data rejection. + * Configuration settings 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. */ + SbgEComRejectionMode position; /*!< Rejection mode for GNSS position data. */ + SbgEComRejectionMode velocity; /*!< Rejection mode for GNSS velocity data. */ + SbgEComRejectionMode hdt; /*!< Rejection mode for true heading (HDT). */ } SbgEComGnssRejectionConf; //----------------------------------------------------------------------// @@ -107,54 +113,54 @@ typedef struct _SbgEComGnssRejectionConf /*! * Set GNSS error model id. * - * \param[in] pHandle A valid sbgECom handle. - * \param[in] modelId Model ID to set - * \return SBG_NO_ERROR if the command has been executed successfully. + * \param[in] pHandle A valid sbgECom handle. + * \param[in] modelId Model ID to set + * \return SBG_NO_ERROR if the command has been executed successfully. */ SbgErrorCode sbgEComCmdGnss1SetModelId(SbgEComHandle *pHandle, SbgEComGnssModelsStdIds modelId); /*! * Retrieve GNSS error model id. * - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pModelId Retrieved model id. - * \return SBG_NO_ERROR if the command has been executed successfully. + * \param[in] pHandle A valid sbgECom handle. + * \param[out] pModelId Retrieved model id. + * \return SBG_NO_ERROR if the command has been executed successfully. */ SbgErrorCode sbgEComCmdGnss1GetModelId(SbgEComHandle *pHandle, SbgEComGnssModelsStdIds *pModelId); /*! * 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. + * \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. + * \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] pRejectConf Pointer to a SbgEComGnssRejectionConf struct to hold rejection configuration of the gnss module. - * \return SBG_NO_ERROR if the command has been executed successfully. + * \param[in] pHandle A valid sbgECom handle. + * \param[out] pRejectConf 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] pRejectConf Pointer to a SbgEComGnssRejectionConf struct holding rejection configuration for the gnss module. - * \return SBG_NO_ERROR if the command has been executed successfully. + * \param[in] pHandle A valid sbgECom handle. + * \param[out] pRejectConf 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); diff --git a/src/commands/sbgEComCmdInfo.c b/src/commands/sbgEComCmdInfo.c index fb4598a..ab02631 100644 --- a/src/commands/sbgEComCmdInfo.c +++ b/src/commands/sbgEComCmdInfo.c @@ -15,91 +15,91 @@ SbgErrorCode sbgEComCmdGetInfo(SbgEComHandle *pHandle, SbgEComDeviceInfo *pInfo) { - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgEComProtocolPayload receivedPayload; - uint32_t trial; - + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + - assert(pHandle); - assert(pInfo); + assert(pHandle); + assert(pInfo); - sbgEComProtocolPayloadConstruct(&receivedPayload); + sbgEComProtocolPayloadConstruct(&receivedPayload); - // - // 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); + // + // 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 = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_INFO, &receivedPayload, pHandle->cmdDefaultTimeOut); + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_INFO, &receivedPayload, pHandle->cmdDefaultTimeOut); - // - // Test if we have correctly received a message - // - if (errorCode == SBG_NO_ERROR) - { - // - // Make sure we have received a payload - // - if (sbgEComProtocolPayloadGetSize(&receivedPayload) > 0) - { - SbgStreamBuffer inputStream; + // + // Test if we have correctly received a message + // + if (errorCode == SBG_NO_ERROR) + { + // + // Make sure we have received a payload + // + if (sbgEComProtocolPayloadGetSize(&receivedPayload) > 0) + { + SbgStreamBuffer inputStream; - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); - // - // 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); + // + // 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); + // + // 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; - } - } + 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; + } + } - sbgEComProtocolPayloadDestroy(&receivedPayload); + sbgEComProtocolPayloadDestroy(&receivedPayload); - return errorCode; + return errorCode; } diff --git a/src/commands/sbgEComCmdInfo.h b/src/commands/sbgEComCmdInfo.h index 9ec2004..e8e19f5 100644 --- a/src/commands/sbgEComCmdInfo.h +++ b/src/commands/sbgEComCmdInfo.h @@ -1,13 +1,13 @@ /*! - * \file sbgEComCmdInfo.h - * \ingroup commands - * \author SBG Systems - * \date 11 June 2014 + * \file sbgEComCmdInfo.h + * \ingroup commands + * \author SBG Systems + * \date 11 June 2014 * - * \brief Commands used to query the device information. + * \brief Commands used to query the device information. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -48,7 +48,7 @@ extern "C" { //----------------------------------------------------------------------// /* Misc */ -#define SBG_ECOM_INFO_PRODUCT_CODE_LENGTH (32) +#define SBG_ECOM_INFO_PRODUCT_CODE_LENGTH (32) //----------------------------------------------------------------------// //- Device Info structure -// @@ -59,14 +59,14 @@ extern "C" { */ 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 */ + 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; //----------------------------------------------------------------------// @@ -76,9 +76,9 @@ typedef struct _SbgEComDeviceInfo /*! * 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. + * \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); diff --git a/src/commands/sbgEComCmdInterface.c b/src/commands/sbgEComCmdInterface.c index e0f10b9..7d06cbc 100644 --- a/src/commands/sbgEComCmdInterface.c +++ b/src/commands/sbgEComCmdInterface.c @@ -15,287 +15,285 @@ SbgErrorCode sbgEComCmdInterfaceGetUartConf(SbgEComHandle *pHandle, SbgEComPortId interfaceId, SbgEComInterfaceConf *pConf) { - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgEComProtocolPayload receivedPayload; - uint32_t trial; - - assert(pHandle); - assert(pConf); - - sbgEComProtocolPayloadConstruct(&receivedPayload); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - uint8_t interfaceIdParam = interfaceId; - - // - // Send the command and the interfaceId as a 1-byte payload - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_UART_CONF, &interfaceIdParam, sizeof(interfaceIdParam)); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_UART_CONF, &receivedPayload, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received correctly the answer - // - if (errorCode == SBG_NO_ERROR) - { - SbgStreamBuffer inputStream; - - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); - - // - // 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; - } - } - - sbgEComProtocolPayloadDestroy(&receivedPayload); - - return errorCode; + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pConf); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + uint8_t interfaceIdParam = interfaceId; + + // + // Send the command and the interfaceId as a 1-byte payload + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_UART_CONF, &interfaceIdParam, sizeof(interfaceIdParam)); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_UART_CONF, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received correctly the answer + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // 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; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; } SbgErrorCode sbgEComCmdInterfaceSetUartConf(SbgEComHandle *pHandle, SbgEComPortId interfaceId, const SbgEComInterfaceConf *pConf) { - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[64]; - 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 errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[64]; + 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; - SbgEComProtocolPayload receivedPayload; - uint32_t trial; - - assert(pHandle); - assert(pBitrate); - assert(pMode); - - sbgEComProtocolPayloadConstruct(&receivedPayload); - - // - // 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 = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_CAN_BUS_CONF, &receivedPayload, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a SBG_ECOM_CMD_CAN_BUS_CONF command - // - if (errorCode == SBG_NO_ERROR) - { - SbgStreamBuffer inputStream; - - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); - - // - // 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; - } - } - - sbgEComProtocolPayloadDestroy(&receivedPayload); - - return errorCode; + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pBitrate); + assert(pMode); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // 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 = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_CAN_BUS_CONF, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a SBG_ECOM_CMD_CAN_BUS_CONF command + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // 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; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + 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; + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[3]; + SbgStreamBuffer outputStream; + + assert(pHandle); + + // + // 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/src/commands/sbgEComCmdInterface.h b/src/commands/sbgEComCmdInterface.h index 97567c9..589f3e8 100644 --- a/src/commands/sbgEComCmdInterface.h +++ b/src/commands/sbgEComCmdInterface.h @@ -1,13 +1,13 @@ /*! - * \file sbgEComCmdInterface.h - * \ingroup commands - * \author SBG Systems - * \date 11 June 2014 + * \file sbgEComCmdInterface.h + * \ingroup commands + * \author SBG Systems + * \date 11 June 2014 * - * \brief Commands used to configure device serial, CAN and Ethernet interfaces. + * \brief Commands used to configure device serial, CAN and Ethernet interfaces. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -52,19 +52,19 @@ extern "C" { */ 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. */ + 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; /*! @@ -72,9 +72,9 @@ typedef enum _SbgEComPortId */ 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. */ + 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; //----------------------------------------------------------------------// @@ -86,8 +86,8 @@ typedef enum _SbgEComPortMode */ typedef struct _SbgEComInterfaceConf { - uint32_t baudRate; /*!< The baud rate of the interface. */ - SbgEComPortMode mode; /*!< The mode of the interface. */ + uint32_t baudRate; /*!< The baud rate of the interface. */ + SbgEComPortMode mode; /*!< The mode of the interface. */ } SbgEComInterfaceConf; //----------------------------------------------------------------------// @@ -99,17 +99,17 @@ typedef struct _SbgEComInterfaceConf */ 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. */ + 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; /*! @@ -117,10 +117,10 @@ typedef enum _SbgEComCanBitRate */ 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 + 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; //----------------------------------------------------------------------// @@ -130,40 +130,40 @@ typedef enum _SbgEComCanMode /*! * 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. + * \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. + * \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] pMode Mode of the CAN interface. - * \return SBG_NO_ERROR if the command has been executed successfully. + * \param[in] pHandle A valid sbgECom handle. + * \param[out] pBitrate The bitrate of the CAN interface. + * \param[out] pMode 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] mode Mode of the CAN interface. - * \return SBG_NO_ERROR if the command has been executed successfully. + * \param[in] pHandle A valid sbgECom handle. + * \param[in] bitRate The bitrate of the CAN interface. + * \param[in] mode Mode of the CAN interface. + * \return SBG_NO_ERROR if the command has been executed successfully. */ SbgErrorCode sbgEComCmdInterfaceSetCanConf(SbgEComHandle *pHandle, SbgEComCanBitRate bitRate, SbgEComCanMode mode); diff --git a/src/commands/sbgEComCmdLicense.c b/src/commands/sbgEComCmdLicense.c index ce5a4e8..5cb9e5a 100644 --- a/src/commands/sbgEComCmdLicense.c +++ b/src/commands/sbgEComCmdLicense.c @@ -17,28 +17,28 @@ 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; + 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/src/commands/sbgEComCmdLicense.h b/src/commands/sbgEComCmdLicense.h index 7fb5ea6..7cde78f 100644 --- a/src/commands/sbgEComCmdLicense.h +++ b/src/commands/sbgEComCmdLicense.h @@ -1,13 +1,13 @@ /*! - * \file sbgEComCmdLicense.h - * \ingroup commands - * \author SBG Systems - * \date 25 February 2015 + * \file sbgEComCmdLicense.h + * \ingroup commands + * \author SBG Systems + * \date 25 February 2015 * - * \brief Command used to upload and apply an activation license. + * \brief Command used to upload and apply an activation license. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -52,10 +52,10 @@ extern "C" { * * 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. + * \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); diff --git a/src/commands/sbgEComCmdMag.c b/src/commands/sbgEComCmdMag.c index f3c25cd..58fab48 100644 --- a/src/commands/sbgEComCmdMag.c +++ b/src/commands/sbgEComCmdMag.c @@ -15,236 +15,226 @@ SbgErrorCode sbgEComCmdMagSetModelId(SbgEComHandle *pHandle, SbgEComMagModelsStdId modelId) { - assert(pHandle); - - return sbgEComCmdGenericSetModelId(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_MAGNETOMETER_MODEL_ID, modelId); + assert(pHandle); + + return sbgEComCmdGenericSetModelId(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_MAGNETOMETER_MODEL_ID, modelId); } SbgErrorCode sbgEComCmdMagGetModelId(SbgEComHandle *pHandle, SbgEComMagModelsStdId *pModelId) { - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t modelIdAsUint; + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t modelIdAsUint; - assert(pHandle); - assert(pModelId); + assert(pHandle); + assert(pModelId); - errorCode = sbgEComCmdGenericGetModelId(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_MAGNETOMETER_MODEL_ID, &modelIdAsUint); + errorCode = sbgEComCmdGenericGetModelId(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_MAGNETOMETER_MODEL_ID, &modelIdAsUint); - if (errorCode == SBG_NO_ERROR) - { - *pModelId = (SbgEComMagModelsStdId)modelIdAsUint; - } + if (errorCode == SBG_NO_ERROR) + { + *pModelId = (SbgEComMagModelsStdId)modelIdAsUint; + } - return errorCode; + return errorCode; } SbgErrorCode sbgEComCmdMagSetCalibData(SbgEComHandle *pHandle, const float *pOffset, const float *pMatrix) { - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgStreamBuffer outputStream; - uint8_t outputBuffer[12 * sizeof(float)]; - uint32_t trial; - uint32_t i; - - assert(pHandle); - assert(pOffset); - assert(pMatrix); - - // - // Initialize a stream buffer to write the command payload - // - errorCode = sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - - // - // Write the offset vector - // - sbgStreamBufferWriteFloatLE(&outputStream, pOffset[0]); - sbgStreamBufferWriteFloatLE(&outputStream, pOffset[1]); - sbgStreamBufferWriteFloatLE(&outputStream, pOffset[2]); - - // - // Write the matrix - // - for (i = 0; i < 9; i++) - { - sbgStreamBufferWriteFloatLE(&outputStream, pMatrix[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, 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_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; + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgStreamBuffer outputStream; + uint8_t outputBuffer[12 * sizeof(float)]; + uint32_t trial; + size_t i; + + assert(pHandle); + assert(pOffset); + assert(pMatrix); + + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + + sbgStreamBufferWriteFloatLE(&outputStream, pOffset[0]); + sbgStreamBufferWriteFloatLE(&outputStream, pOffset[1]); + sbgStreamBufferWriteFloatLE(&outputStream, pOffset[2]); + + for (i = 0; i < 9; i++) + { + sbgStreamBufferWriteFloatLE(&outputStream, pMatrix[i]); + } + + errorCode = sbgStreamBufferGetLastError(&outputStream); + + if (errorCode == SBG_NO_ERROR) + { + for (trial = 0; trial < pHandle->numTrials; trial++) + { + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_SET_MAG_CALIB, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + if (errorCode == SBG_NO_ERROR) + { + errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_SET_MAG_CALIB, pHandle->cmdDefaultTimeOut); + + if (errorCode == SBG_NO_ERROR) + { + break; + } + } + else + { + // + // Unable to send the command, stop here + // + break; + } + } + } + + return errorCode; +} + +SbgErrorCode sbgEComCmdMagSetCalibData2(SbgEComHandle *pHandle, const float *pOffset, const float *pMatrix, SbgEComMagCalibMode mode) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgStreamBuffer outputStream; + uint8_t outputBuffer[12 * sizeof(float) + sizeof(uint8_t)]; + uint32_t trial; + size_t i; + + assert(pHandle); + assert(pOffset); + assert(pMatrix); + + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + + sbgStreamBufferWriteFloatLE(&outputStream, pOffset[0]); + sbgStreamBufferWriteFloatLE(&outputStream, pOffset[1]); + sbgStreamBufferWriteFloatLE(&outputStream, pOffset[2]); + + for (i = 0; i < 9; i++) + { + sbgStreamBufferWriteFloatLE(&outputStream, pMatrix[i]); + } + + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)mode); + + errorCode = sbgStreamBufferGetLastError(&outputStream); + + if (errorCode == SBG_NO_ERROR) + { + for (trial = 0; trial < pHandle->numTrials; trial++) + { + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_SET_MAG_CALIB, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + if (errorCode == SBG_NO_ERROR) + { + errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_SET_MAG_CALIB, pHandle->cmdDefaultTimeOut); + + if (errorCode == SBG_NO_ERROR) + { + break; + } + } + else + { + // + // Unable to send the command, stop here + // + break; + } + } + } + + return errorCode; } SbgErrorCode sbgEComCmdMagGetRejection(SbgEComHandle *pHandle, SbgEComMagRejectionConf *pRejectConf) { - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgEComProtocolPayload receivedPayload; - uint32_t trial; - - assert(pHandle); - assert(pRejectConf); - - sbgEComProtocolPayloadConstruct(&receivedPayload); - - // - // 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) - { - SbgStreamBuffer inputStream; - - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_MAGNETOMETER_REJECT_MODE, &receivedPayload, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received frame was OK - // - if (errorCode == SBG_NO_ERROR) - { - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); - - // - // 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; - } - } - - sbgEComProtocolPayloadDestroy(&receivedPayload); - - return errorCode; + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pRejectConf); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + for (trial = 0; trial < pHandle->numTrials; trial++) + { + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_MAGNETOMETER_REJECT_MODE, NULL, 0); + + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_MAGNETOMETER_REJECT_MODE, &receivedPayload, pHandle->cmdDefaultTimeOut); + + if (errorCode == SBG_NO_ERROR) + { + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // Parse received payload + // + pRejectConf->magneticField = (SbgEComRejectionMode)sbgStreamBufferReadUint8LE(&inputStream); + + break; + } + } + else + { + // + // Unable to send the command, stop here + // + break; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; } SbgErrorCode sbgEComCmdMagSetRejection(SbgEComHandle *pHandle, const SbgEComMagRejectionConf *pRejectConf) { - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[64]; - 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; + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[64]; + SbgStreamBuffer outputStream; + + assert(pHandle); + assert(pRejectConf); + + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pRejectConf->magneticField); + + errorCode = sbgStreamBufferGetLastError(&outputStream); + + if (errorCode == SBG_NO_ERROR) + { + for (trial = 0; trial < pHandle->numTrials; trial++) + { + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_MAGNETOMETER_REJECT_MODE, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + if (errorCode == SBG_NO_ERROR) + { + errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_MAGNETOMETER_REJECT_MODE, pHandle->cmdDefaultTimeOut); + + if (errorCode == SBG_NO_ERROR) + { + break; + } + } + else + { + // + // Unable to send the command, stop here + // + break; + } + } + } + + return errorCode; } //----------------------------------------------------------------------// @@ -253,170 +243,129 @@ SbgErrorCode sbgEComCmdMagSetRejection(SbgEComHandle *pHandle, const SbgEComMagR SbgErrorCode sbgEComCmdMagStartCalib(SbgEComHandle *pHandle, SbgEComMagCalibMode mode, SbgEComMagCalibBandwidth bandwidth) { - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgStreamBuffer outputStream; - uint8_t outputBuffer[2]; - uint32_t trial; - - assert(pHandle); - - // - // Initialize a stream buffer to write the command payload - // - errorCode = sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - - // - // Write the calibration mode and bandwidth - // - 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, 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_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; + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgStreamBuffer outputStream; + uint8_t outputBuffer[2]; + uint32_t trial; + + assert(pHandle); + + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)mode); + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)bandwidth); + + errorCode = sbgStreamBufferGetLastError(&outputStream); + + if (errorCode == SBG_NO_ERROR) + { + for (trial = 0; trial < pHandle->numTrials; trial++) + { + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_START_MAG_CALIB, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + if (errorCode == SBG_NO_ERROR) + { + errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_START_MAG_CALIB, pHandle->cmdDefaultTimeOut); + + if (errorCode == SBG_NO_ERROR) + { + break; + } + } + else + { + // + // Unable to send the command, stop here + // + break; + } + } + } + + return errorCode; } SbgErrorCode sbgEComCmdMagComputeCalib(SbgEComHandle *pHandle, SbgEComMagCalibResults *pCalibResults) { - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgEComProtocolPayload receivedPayload; - uint32_t trial; - - assert(pHandle); - assert(pCalibResults); - - sbgEComProtocolPayloadConstruct(&receivedPayload); - - // - // 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 on-board magnetic computation can take some time - // - errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_COMPUTE_MAG_CALIB, &receivedPayload, 5000); - - // - // Test if we have received the correct command - // - if (errorCode == SBG_NO_ERROR) - { - SbgStreamBuffer inputStream; - size_t i; - - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); - - // - // 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; - } - } - - sbgEComProtocolPayloadDestroy(&receivedPayload); - - return errorCode; + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pCalibResults); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + for (trial = 0; trial < pHandle->numTrials; trial++) + { + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_COMPUTE_MAG_CALIB, NULL, 0); + + if (errorCode == SBG_NO_ERROR) + { + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_COMPUTE_MAG_CALIB, &receivedPayload, 5000); + + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + size_t i; + + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // 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); + } + + break; + } + } + else + { + // + // Unable to send the command, stop here + // + break; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; } + +//----------------------------------------------------------------------// +//- Private SBG Systems only definitions -// +//----------------------------------------------------------------------// + diff --git a/src/commands/sbgEComCmdMag.h b/src/commands/sbgEComCmdMag.h index 95db273..9fc312d 100644 --- a/src/commands/sbgEComCmdMag.h +++ b/src/commands/sbgEComCmdMag.h @@ -1,13 +1,13 @@ /*! - * \file sbgEComCmdMag.h - * \ingroup commands - * \author SBG Systems - * \date 11 June 2014 + * \file sbgEComCmdMag.h + * \ingroup commands + * \author SBG Systems + * \date 11 June 2014 * - * \brief Magnetometer aiding module configuration & on-board magnetic calibration commands. + * \brief Magnetometer aiding module configuration & on-board magnetic calibration commands. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -51,68 +51,74 @@ extern "C" { //----------------------------------------------------------------------// /*! - * Define if the on-board magnetic calibration should acquire points for a 3D or 2D calibration. + * Defines the algorithm mode used for compensating magnetometers against soft and hard iron effects. + * + * The 2D calibration mode can only be used by the INS filter when roll and pitch are minimal (± 5°) + * as it assumes a near-horizontal orientation of the sensor. + * + * The 3D calibration mode, in contrast, provides a valid magnetic field compensation for any sensor orientation, + * thereby improving the overall quality and reliability of the magnetic heading data. */ 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. */ + SBG_ECOM_MAG_CALIB_MODE_2D = 1, /*!< Calibration using 2D mode, requiring at least a full 360° and motions less than 5° */ + SBG_ECOM_MAG_CALIB_MODE_3D = 2 /*!< Calibration using 3D mode, accurate in all orientations but needs motions greater than ± 30° */ } SbgEComMagCalibMode; - /*! - * Used to select the expected dynamics during the magnetic calibration. + * Define the expected motions bandwidth during the magnetic field compensation acquisition. + * + * \note This parameter is deprecated and has no effect for ELLIPSE firmware v3.0 and above. + * For ELLIPSE firmware v2.x and before, please use SBG_ECOM_MAG_CALIB_HIGH_BW. */ 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. */ + SBG_ECOM_MAG_CALIB_LOW_BW = 0, /*!< Not used anymore. */ + SBG_ECOM_MAG_CALIB_MEDIUM_BW = 1, /*!< Not used anymore. */ + SBG_ECOM_MAG_CALIB_HIGH_BW = 2 /*!< Not used anymore. */ } SbgEComMagCalibBandwidth; /*! - * General quality indicator of an on-board magnetic calibration. + * General quality indicator of an on-board 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. */ + 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 on-bard magnetic calibration. + * Confidence indicator on results of an on-bard 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. */ + 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 information on the on-board magnetic calibration. + * Status bit masks used to report advanced information on the on-board 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. */ +#define SBG_ECOM_MAG_CALIB_NOT_ENOUGH_POINTS (0x0001u << 0) /*!< Not enough valid magnetic points have been acquired. */ +#define SBG_ECOM_MAG_CALIB_TOO_MUCH_DISTORTIONS (0x0001u << 1) /*!< Unable to compute a magnetic calibration due to magnetic interferences or incorrect data set distribution. */ +#define SBG_ECOM_MAG_CALIB_X_MOTION_ISSUE (0x0001u << 2) /*!< 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 (0x0001u << 3) /*!< 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 (0x0001u << 4) /*!< For a 3D or 2D calibration: not enough motion on Z axis. */ +#define SBG_ECOM_MAG_CALIB_ALIGNMENT_ISSUE (0x0001u << 5) /*!< 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 + * Defines the magnetometers aiding error model and protocol configuration. */ 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 */ + SBG_ECOM_MAG_MODEL_INTERNAL_NORMAL = 201, /*!< Internal magnetometer - implements world leading magnetic rejection and interference mitigation solutions. */ + SBG_ECOM_MAG_MODEL_INTERNAL_RESERVED = 202, /*!< Internal magnetometer - only used for backward compatibility with ELLIPSE firmware v2.x */ + SBG_ECOM_MAG_MODEL_ECOM_NORMAL = 203, /*!< External magnetometer - sbgECom protocol, implements world leading magnetic rejection and interference mitigation solutions. */ } SbgEComMagModelsStdId; //----------------------------------------------------------------------// @@ -124,7 +130,7 @@ typedef enum _SbgEComMagModelsStdIds */ typedef struct _SbgEComMagRejectionConf { - SbgEComRejectionMode magneticField; /*!< Rejection mode for magnetic field. */ + SbgEComRejectionMode magneticField; /*!< Rejection mode for magnetic field. */ } SbgEComMagRejectionConf; /*! @@ -132,26 +138,26 @@ typedef struct _SbgEComMagRejectionConf */ 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. */ + 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; //----------------------------------------------------------------------// @@ -161,49 +167,68 @@ typedef struct _SbgEComMagCalibResults /*! * Set magnetometer error model id. * - * \param[in] pHandle A valid sbgECom handle - * \param[in] modelId Magnetometer model id to set - * \return SBG_NO_ERROR if the command has been executed successfully + * \param[in] pHandle A valid sbgECom handle + * \param[in] modelId Magnetometer model id to set + * \return SBG_NO_ERROR if the command has been executed successfully */ SbgErrorCode sbgEComCmdMagSetModelId(SbgEComHandle *pHandle, SbgEComMagModelsStdId modelId); /*! * Retrieve magnetometer error model id * - * \param[in] pHandle A valid sbgECom handle - * \param[out] pModelId Retrieved magnetometer model id - * \return SBG_NO_ERROR if the command has been executed successfully + * \param[in] pHandle A valid sbgECom handle + * \param[out] pModelId Retrieved magnetometer model id + * \return SBG_NO_ERROR if the command has been executed successfully */ SbgErrorCode sbgEComCmdMagGetModelId(SbgEComHandle *pHandle, SbgEComMagModelsStdId *pModelId); /*! * 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. + * \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. + * \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. + * Set the magnetometers soft and hard iron compensation parameters (Firmware v2.x and earlier). + * + * \note: This command should ONLY be used for ELLIPSE firmware v2 and below. + * Please use sbgEComCmdMagSetCalibData2 for ELLIPSE firmware v3 and after. * - * \param[in] pHandle A valid sbgECom handle. - * \param[in] pOffset Magnetometers calibration offset vector. - * \param[in] pMatrix Magnetometers calibration 3x3 matrix. - * \return SBG_NO_ERROR if the command has been executed successfully. + * \param[in] pHandle A valid sbgECom handle. + * \param[in] pOffset Magnetometers calibration offset vector (array of 3 floats). + * \param[in] pMatrix Magnetometers calibration 3x3 matrix (array of 9 floats). + * \return SBG_NO_ERROR if the command has been executed successfully. */ SbgErrorCode sbgEComCmdMagSetCalibData(SbgEComHandle *pHandle, const float *pOffset, const float *pMatrix); +/*! + * Set the magnetometers soft and hard iron compensation parameters (Firmware v3.x and later). + * + * With the introduction of ELLIPSE firmware version 3, the soft/hard iron compensation algorithm mode information + * is provided to refine magnetometer aiding within the Inertial Navigation System (INS) filter. + * + * \note: Firmware versions 2 and earlier do not support this feature and 'sbgEComCmdMagSetCalibData' method should be used instead. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] pOffset Magnetometers calibration offset vector (array of 3 floats). + * \param[in] pMatrix Magnetometers calibration 3x3 matrix (array of 9 floats). + * \param[in] mode Defines if the magnetic calibration is a 2D or 3D one. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdMagSetCalibData2(SbgEComHandle *pHandle, const float *pOffset, const float *pMatrix, SbgEComMagCalibMode mode); + //----------------------------------------------------------------------// //- Magnetometer on-board calibration commands -// //----------------------------------------------------------------------// @@ -214,10 +239,10 @@ SbgErrorCode sbgEComCmdMagSetCalibData(SbgEComHandle *pHandle, const float *pOff * 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. + * \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 Not used anymore for ELLIPSE firmware v3.0, this value has no effect. + * \return SBG_NO_ERROR if the command has been executed successfully. */ SbgErrorCode sbgEComCmdMagStartCalib(SbgEComHandle *pHandle, SbgEComMagCalibMode mode, SbgEComMagCalibBandwidth bandwidth); @@ -226,12 +251,22 @@ SbgErrorCode sbgEComCmdMagStartCalib(SbgEComHandle *pHandle, SbgEComMagCalibMode * * 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 on-board magnetic calibration results and status. - * \return SBG_NO_ERROR if the command has been executed successfully. + * \param[in] pHandle A valid sbgECom handle. + * \param[out] pCalibResults Pointer on a SbgEComMagCalibResults structure that can hold on-board magnetic calibration results and status. + * \return SBG_NO_ERROR if the command has been executed successfully. */ SbgErrorCode sbgEComCmdMagComputeCalib(SbgEComHandle *pHandle, SbgEComMagCalibResults *pCalibResults); +//----------------------------------------------------------------------// +//- DEPRECATED - Used for backward compatibility -// +//----------------------------------------------------------------------// + +#ifdef SBG_ECOM_USE_DEPRECATED_MACROS + #define SBG_ECOM_MAG_MODEL_NORMAL (SBG_ECOM_MAG_MODEL_INTERNAL_NORMAL) + #define SBG_ECOM_MAG_MODEL_NOISY_MAG_TOLERANT (SBG_ECOM_MAG_MODEL_INTERNAL_RESERVED) +#endif + + #ifdef __cplusplus } #endif diff --git a/src/commands/sbgEComCmdOdo.c b/src/commands/sbgEComCmdOdo.c index 5f07558..d605461 100644 --- a/src/commands/sbgEComCmdOdo.c +++ b/src/commands/sbgEComCmdOdo.c @@ -15,568 +15,566 @@ SbgErrorCode sbgEComCmdOdoGetConf(SbgEComHandle *pHandle, SbgEComOdoConf *pOdometerConf) { - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgEComProtocolPayload receivedPayload; - uint32_t trial; - - assert(pHandle); - assert(pOdometerConf); - - sbgEComProtocolPayloadConstruct(&receivedPayload); - - // - // 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 = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_CONF, &receivedPayload, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a SBG_ECOM_CMD_ODO_CONF command - // - if (errorCode == SBG_NO_ERROR) - { - SbgStreamBuffer inputStream; - - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); - - // - // 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; - } - } - - sbgEComProtocolPayloadDestroy(&receivedPayload); - - return errorCode; + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pOdometerConf); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // 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 = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_CONF, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a SBG_ECOM_CMD_ODO_CONF command + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // 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; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; } SbgErrorCode sbgEComCmdOdoSetConf(SbgEComHandle *pHandle, const SbgEComOdoConf *pOdometerConf) { - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[64]; - 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 errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[64]; + 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 *pLeverArm) { - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgEComProtocolPayload receivedPayload; - uint32_t trial; - - assert(pHandle); - assert(pLeverArm); - - sbgEComProtocolPayloadConstruct(&receivedPayload); - - // - // 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 = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_LEVER_ARM, &receivedPayload, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a correct answer - // - if (errorCode == SBG_NO_ERROR) - { - SbgStreamBuffer inputStream; - - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); - - // - // Read parameters - // - pLeverArm[0] = sbgStreamBufferReadFloatLE(&inputStream); - pLeverArm[1] = sbgStreamBufferReadFloatLE(&inputStream); - pLeverArm[2] = sbgStreamBufferReadFloatLE(&inputStream); - - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - sbgEComProtocolPayloadDestroy(&receivedPayload); - - return errorCode; + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pLeverArm); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // 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 = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_LEVER_ARM, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a correct answer + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // Read parameters + // + pLeverArm[0] = sbgStreamBufferReadFloatLE(&inputStream); + pLeverArm[1] = sbgStreamBufferReadFloatLE(&inputStream); + pLeverArm[2] = sbgStreamBufferReadFloatLE(&inputStream); + + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; } SbgErrorCode sbgEComCmdOdoSetLeverArm(SbgEComHandle *pHandle, const float *pLeverArm) { - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[64]; - SbgStreamBuffer outputStream; - - assert(pHandle); - assert(pLeverArm); - - // - // 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, pLeverArm[0]); - sbgStreamBufferWriteFloatLE(&outputStream, pLeverArm[1]); - sbgStreamBufferWriteFloatLE(&outputStream, pLeverArm[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 errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[64]; + SbgStreamBuffer outputStream; + + assert(pHandle); + assert(pLeverArm); + + // + // 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, pLeverArm[0]); + sbgStreamBufferWriteFloatLE(&outputStream, pLeverArm[1]); + sbgStreamBufferWriteFloatLE(&outputStream, pLeverArm[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; - SbgEComProtocolPayload receivedPayload; - uint32_t trial; - - assert(pHandle); - assert(pRejectConf); - - sbgEComProtocolPayloadConstruct(&receivedPayload); - - // - // 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 = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_REJECT_MODE, &receivedPayload, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a correct answer - // - if (errorCode == SBG_NO_ERROR) - { - SbgStreamBuffer inputStream; - - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); - - // - // 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; - } - } - - sbgEComProtocolPayloadDestroy(&receivedPayload); - - return errorCode; + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pRejectConf); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // 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 = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_REJECT_MODE, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a correct answer + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // 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; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; } SbgErrorCode sbgEComCmdOdoSetRejection(SbgEComHandle *pHandle, const SbgEComOdoRejectionConf *pRejectConf) { - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[64]; - 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 errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[64]; + 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; - SbgEComProtocolPayload receivedPayload; - uint32_t trial; - uint8_t outputBuffer[16]; - SbgStreamBuffer outputStream; - - assert(pHandle); - assert(pOdoCanConf); - assert(canChannel <= UCHAR_MAX); - - sbgEComProtocolPayloadConstruct(&receivedPayload); - - // - // Build the command payload used to ask the CAN odometer configuration for a specific channel - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - 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 = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_CAN_CONF, &receivedPayload, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a SBG_ECOM_CMD_ODO_CAN_SPEED_CONF command - // - if (errorCode == SBG_NO_ERROR) - { - SbgStreamBuffer inputStream; - - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); - - // - // 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; - } - } - - sbgEComProtocolPayloadDestroy(&receivedPayload); - - return errorCode; + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + uint8_t outputBuffer[16]; + SbgStreamBuffer outputStream; + + assert(pHandle); + assert(pOdoCanConf); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Build the command payload used to ask the CAN odometer configuration for a specific channel + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + 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 = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_CAN_CONF, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a SBG_ECOM_CMD_ODO_CAN_SPEED_CONF command + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // 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; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; } SbgErrorCode sbgEComCmdOdoCanSetConf(SbgEComHandle *pHandle, SbgEComCmdOdoCanChannel canChannel, const SbgEComCmdOdoCanConf *pOdoCanConf) { - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[64]; - 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; + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[64]; + SbgStreamBuffer outputStream; + + assert(pHandle); + assert(pOdoCanConf); + + // + // 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/src/commands/sbgEComCmdOdo.h b/src/commands/sbgEComCmdOdo.h index defd28e..583360d 100644 --- a/src/commands/sbgEComCmdOdo.h +++ b/src/commands/sbgEComCmdOdo.h @@ -1,13 +1,13 @@ /*! - * \file sbgEComCmdOdo.h - * \ingroup commands - * \author SBG Systems - * \date 11 June 2014 + * \file sbgEComCmdOdo.h + * \ingroup commands + * \author SBG Systems + * \date 11 June 2014 * - * \brief Odometer / DMI aiding module configuration commands. + * \brief Odometer / DMI aiding module configuration commands. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -55,9 +55,9 @@ extern "C" { */ 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. */ + 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; /*! @@ -65,7 +65,7 @@ typedef struct _SbgEComOdoConf */ typedef struct _SbgEComOdoRejectionConf { - SbgEComRejectionMode velocity; /*!< Rejection mode for velocity. */ + SbgEComRejectionMode velocity; /*!< Rejection mode for velocity. */ } SbgEComOdoRejectionConf; /*! @@ -74,17 +74,17 @@ typedef struct _SbgEComOdoRejectionConf */ 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). */ + 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. */ +#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. @@ -92,16 +92,16 @@ typedef enum _SbgEComCmdOdoCanChannel */ 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. */ + 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). */ + 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 */ + 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; //----------------------------------------------------------------------// @@ -111,74 +111,74 @@ typedef struct _SbgEComCmdOdoCanConf /*! * 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. + * \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. + * \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] pLeverArm Array of three values, one for each axis. - * \return SBG_NO_ERROR if the command has been executed successfully. + * \param[in] pHandle A valid sbgECom handle. + * \param[out] pLeverArm Array of three values, one for each axis. + * \return SBG_NO_ERROR if the command has been executed successfully. */ SbgErrorCode sbgEComCmdOdoGetLeverArm(SbgEComHandle *pHandle, float *pLeverArm); /*! * Set the lever arm applicable for both quadrature or CAN based odometer. * - * \param[in] pHandle A valid sbgECom handle. - * \param[in] pLeverArm Array of three values, one for each axis. - * \return SBG_NO_ERROR if the command has been executed successfully. + * \param[in] pHandle A valid sbgECom handle. + * \param[in] pLeverArm 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 *pLeverArm); /*! * 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. + * \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. + * \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 retrieve 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. + * \param[in] pHandle A valid sbgECom handle. + * \param[in] canChannel The CAN channel to retrieve 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. + * \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); diff --git a/src/commands/sbgEComCmdOutput.c b/src/commands/sbgEComCmdOutput.c index ba95b85..b766ee8 100644 --- a/src/commands/sbgEComCmdOutput.c +++ b/src/commands/sbgEComCmdOutput.c @@ -15,569 +15,569 @@ SbgErrorCode sbgEComCmdOutputGetConf(SbgEComHandle *pHandle, SbgEComOutputPort outputPort, SbgEComClass classId, SbgEComMsgId msgId, SbgEComOutputMode *pConf) { - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgEComProtocolPayload receivedPayload; - uint32_t trial; - uint8_t outputBuffer[5]; - SbgStreamBuffer outputStream; - - assert(pHandle); - assert(pConf); - - sbgEComProtocolPayloadConstruct(&receivedPayload); - - // - // Build payload to send - // - sbgStreamBufferInitForWrite(&outputStream, &outputBuffer, sizeof(outputBuffer)); - - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)outputPort); - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)msgId); - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)classId); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // 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 = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_OUTPUT_CONF, &receivedPayload, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a SBG_ECOM_CMD_OUTPUT_CONF command - // - if (errorCode == SBG_NO_ERROR) - { - SbgStreamBuffer inputStream; - - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); - - // - // Parse the received payload - // - 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; - } - } - - sbgEComProtocolPayloadDestroy(&receivedPayload); - - return errorCode; + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + uint8_t outputBuffer[5]; + SbgStreamBuffer outputStream; + + assert(pHandle); + assert(pConf); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Build payload to send + // + sbgStreamBufferInitForWrite(&outputStream, &outputBuffer, sizeof(outputBuffer)); + + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)outputPort); + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)msgId); + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)classId); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // 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 = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_OUTPUT_CONF, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a SBG_ECOM_CMD_OUTPUT_CONF command + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // Parse the received payload + // + 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; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; } SbgErrorCode sbgEComCmdOutputSetConf(SbgEComHandle *pHandle, SbgEComOutputPort outputPort, SbgEComClass classId, SbgEComMsgId msgId, SbgEComOutputMode conf) { - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[5]; - SbgStreamBuffer outputStream; - - assert(pHandle); - - // - // Build the payload to send - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)outputPort); - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)msgId); - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)classId); - sbgStreamBufferWriteUint16LE(&outputStream, (uint16_t)conf); - - // - // 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_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; - } - } - - return errorCode; + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[5]; + SbgStreamBuffer outputStream; + + assert(pHandle); + + // + // Build the payload to send + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)outputPort); + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)msgId); + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)classId); + sbgStreamBufferWriteUint16LE(&outputStream, (uint16_t)conf); + + // + // 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_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; + } + } + + return errorCode; } SbgErrorCode sbgEComCmdOutputClassGetEnable(SbgEComHandle *pHandle, SbgEComOutputPort outputPort, SbgEComClass classId, bool *pEnable) { - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgEComProtocolPayload receivedPayload; - uint32_t trial; - uint8_t outputBuffer[3]; - SbgStreamBuffer outputStream; - - assert(pHandle); - assert(pEnable); - - sbgEComProtocolPayloadConstruct(&receivedPayload); - - // - // Build payload to send - // - sbgStreamBufferInitForWrite(&outputStream, &outputBuffer, sizeof(outputBuffer)); - - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)outputPort); - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)classId); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // 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 = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_OUTPUT_CLASS_ENABLE, &receivedPayload, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a correct answer - // - if (errorCode == SBG_NO_ERROR) - { - SbgStreamBuffer inputStream; - - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); - - // - // Parse the received payload - // - 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; - } - } - - sbgEComProtocolPayloadDestroy(&receivedPayload); - - return errorCode; + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + uint8_t outputBuffer[3]; + SbgStreamBuffer outputStream; + + assert(pHandle); + assert(pEnable); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Build payload to send + // + sbgStreamBufferInitForWrite(&outputStream, &outputBuffer, sizeof(outputBuffer)); + + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)outputPort); + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)classId); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // 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 = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_OUTPUT_CLASS_ENABLE, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a correct answer + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // Parse the received payload + // + 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; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; } SbgErrorCode sbgEComCmdOutputClassSetEnable(SbgEComHandle *pHandle, SbgEComOutputPort outputPort, SbgEComClass classId, bool enable) { - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[3]; - SbgStreamBuffer outputStream; - - assert(pHandle); - - // - // Build payload to send - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)outputPort); - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)classId); - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)enable); - - // - // 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_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; - } - } - - return errorCode; + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[3]; + SbgStreamBuffer outputStream; + + assert(pHandle); + + // + // Build payload to send + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)outputPort); + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)classId); + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)enable); + + // + // 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_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; + } + } + + return errorCode; } SbgErrorCode sbgEComCmdCanOutputGetConf(SbgEComHandle *pHandle, SbgECanMessageId internalId, SbgEComOutputMode *pMode, uint32_t *pUserId, bool *pExtended) { - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgEComProtocolPayload receivedPayload; - uint32_t trial; - uint8_t outputBuffer[2]; - SbgStreamBuffer outputStream; - - - assert(pHandle); - assert(pMode); - assert(pUserId); - assert(pExtended); - - sbgEComProtocolPayloadConstruct(&receivedPayload); - - // - // Build the payload to send - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - sbgStreamBufferWriteUint16LE(&outputStream, internalId); - - // - // 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_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 = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_CAN_OUTPUT_CONF, &receivedPayload, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a correct answer - // - if (errorCode == SBG_NO_ERROR) - { - SbgStreamBuffer inputStream; - - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); - - // - // Parse the payload - // - 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; - } - } - - sbgEComProtocolPayloadDestroy(&receivedPayload); - - return errorCode; + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + uint8_t outputBuffer[2]; + SbgStreamBuffer outputStream; + + + assert(pHandle); + assert(pMode); + assert(pUserId); + assert(pExtended); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Build the payload to send + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + sbgStreamBufferWriteUint16LE(&outputStream, internalId); + + // + // 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_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 = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_CAN_OUTPUT_CONF, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a correct answer + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // Parse the payload + // + 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; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; } SbgErrorCode sbgEComCmdCanOutputSetConf(SbgEComHandle *pHandle, SbgECanMessageId internalId, SbgEComOutputMode mode, uint32_t userId, bool extended) { - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[9]; - SbgStreamBuffer outputStream; - - assert(pHandle); - - // - // Build the payload to send - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - - sbgStreamBufferWriteUint16LE(&outputStream, (uint16_t)internalId); - sbgStreamBufferWriteUint16LE(&outputStream, (uint16_t)mode); - sbgStreamBufferWriteUint32LE(&outputStream, userId); - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)extended); - - // - // 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_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; + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[9]; + SbgStreamBuffer outputStream; + + assert(pHandle); + + // + // Build the payload to send + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + + sbgStreamBufferWriteUint16LE(&outputStream, (uint16_t)internalId); + sbgStreamBufferWriteUint16LE(&outputStream, (uint16_t)mode); + sbgStreamBufferWriteUint32LE(&outputStream, userId); + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)extended); + + // + // 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_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; } SbgErrorCode sbgEComCmdOutputGetNmeaTalkerId(SbgEComHandle *pHandle, SbgEComOutputPort outputPort, char *pNmeaTalkerId) { - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgEComProtocolPayload receivedPayload; - uint32_t trial; - uint8_t outputBuffer[1]; - SbgStreamBuffer outputStream; - - assert(pHandle); - assert(pNmeaTalkerId); - - sbgEComProtocolPayloadConstruct(&receivedPayload); - - // - // Build the payload to send - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - sbgStreamBufferWriteUint8(&outputStream, outputPort); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the command with the output port as a 1-byte payload - // - 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 = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_NMEA_TALKER_ID, &receivedPayload, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a SBG_ECOM_CMD_NMEA_TALKER_ID command - // - if (errorCode == SBG_NO_ERROR) - { - SbgStreamBuffer inputStream; - - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); - - // - // Read parameters - // - outputPort = (SbgEComOutputPort)sbgStreamBufferReadUint8LE(&inputStream); - pNmeaTalkerId[0] = (char)sbgStreamBufferReadUint8LE(&inputStream); - pNmeaTalkerId[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; - } - } - - sbgEComProtocolPayloadDestroy(&receivedPayload); - - return errorCode; + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + uint8_t outputBuffer[1]; + SbgStreamBuffer outputStream; + + assert(pHandle); + assert(pNmeaTalkerId); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Build the payload to send + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + sbgStreamBufferWriteUint8(&outputStream, outputPort); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the command with the output port as a 1-byte payload + // + 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 = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_NMEA_TALKER_ID, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a SBG_ECOM_CMD_NMEA_TALKER_ID command + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // Read parameters + // + outputPort = (SbgEComOutputPort)sbgStreamBufferReadUint8LE(&inputStream); + pNmeaTalkerId[0] = (char)sbgStreamBufferReadUint8LE(&inputStream); + pNmeaTalkerId[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; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; } SbgErrorCode sbgEComCmdOutputSetNmeaTalkerId(SbgEComHandle *pHandle, SbgEComOutputPort outputPort, const char *pNmeaTalkerId) { - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[3]; - SbgStreamBuffer outputStream; - - assert(pHandle); - assert(pNmeaTalkerId); - - // - // Build the payload to send - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)outputPort); - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)(pNmeaTalkerId[0])); - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)(pNmeaTalkerId[1])); - - // - // 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_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; + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[3]; + SbgStreamBuffer outputStream; + + assert(pHandle); + assert(pNmeaTalkerId); + + // + // Build the payload to send + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)outputPort); + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)(pNmeaTalkerId[0])); + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)(pNmeaTalkerId[1])); + + // + // 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_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/src/commands/sbgEComCmdOutput.h b/src/commands/sbgEComCmdOutput.h index e1e8919..b6343b7 100644 --- a/src/commands/sbgEComCmdOutput.h +++ b/src/commands/sbgEComCmdOutput.h @@ -1,13 +1,13 @@ /*! - * \file sbgEComCmdOutput.h - * \ingroup commands - * \author SBG Systems - * \date 11 June 2014 + * \file sbgEComCmdOutput.h + * \ingroup commands + * \author SBG Systems + * \date 11 June 2014 * - * \brief Commands used to setup logs to output over the device interfaces. + * \brief Commands used to setup logs to output over the device interfaces. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -52,9 +52,9 @@ extern "C" { */ 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 */ + 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; /*! @@ -62,24 +62,32 @@ typedef enum _SbgEComOutputPort */ 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 */ + SBG_ECOM_OUTPUT_MODE_DISABLED = 0, /*!< This output is disabled. */ + SBG_ECOM_OUTPUT_MODE_MAIN_LOOP = 1, /*!< Output is generated at 200 Hz. */ + SBG_ECOM_OUTPUT_MODE_DIV_2 = 2, /*!< Output is generated at 100 Hz. */ + SBG_ECOM_OUTPUT_MODE_DIV_4 = 4, /*!< Output is generated at 50 Hz. */ + SBG_ECOM_OUTPUT_MODE_DIV_5 = 5, /*!< Output is generated at 40 Hz. */ + SBG_ECOM_OUTPUT_MODE_DIV_8 = 8, /*!< Output is generated at 25 Hz. */ + SBG_ECOM_OUTPUT_MODE_DIV_10 = 10, /*!< Output is generated at 20 Hz. */ + SBG_ECOM_OUTPUT_MODE_DIV_20 = 20, /*!< Output is generated at 10 Hz. */ + SBG_ECOM_OUTPUT_MODE_DIV_40 = 40, /*!< Output is generated at 5 Hz. */ + SBG_ECOM_OUTPUT_MODE_DIV_100 = 100, /*!< Output is generated at 2 Hz. (only ELLIPSE fmw v3). */ + SBG_ECOM_OUTPUT_MODE_DIV_200 = 200, /*!< Output is generated at 1 Hz. */ + + SBG_ECOM_OUTPUT_MODE_1_MS = 1001, /*!< Output is generated at 1000 Hz (only ELLIPSE fmw v3). */ + SBG_ECOM_OUTPUT_MODE_2_MS = 1002, /*!< Output is generated at 500 Hz (only ELLIPSE fmw v3). */ + SBG_ECOM_OUTPUT_MODE_4_MS = 1004, /*!< Output is generated at 250 Hz (only ELLIPSE fmw v3). */ + + 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 (deprecated for ELLIPSE fmw v3, only used for IMU FAST). */ } SbgEComOutputMode; /*! @@ -88,12 +96,12 @@ typedef enum _SbgEComOutputMode */ 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. */ + 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; //----------------------------------------------------------------------// @@ -103,90 +111,93 @@ typedef enum _SbgEComOutputMonitoringPoint /*! * Retrieve the configuration of one message for an 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. + * \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 message for an 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. + * \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 if a whole message class is enabled or not for an output interface. * - * \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. + * \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 if a whole message class is enabled or not for an output interface. * - * \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. + * Note: You can use the special classId SBG_ECOM_CLASS_LOG_ALL to disable or enable all classes at once. + * This is only available for ELLIPSE firmware v3 and above. + * + * \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 message for 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. + * \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 message for 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. + * \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 NMEA talker id for an output interface. * - * \param[in] pHandle A valid sbgECom handle. - * \param[in] outputPort The output port of the device for the log concerned. - * \param[out] pNmeaTalkerId A 2-char array to contain the NMEA talker id. - * \return SBG_NO_ERROR if the command has been executed successfully. + * \param[in] pHandle A valid sbgECom handle. + * \param[in] outputPort The output port of the device for the log concerned. + * \param[out] pNmeaTalkerId 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 *pNmeaTalkerId); /*! * Set the NMEA talker id for an output interface. * - * \param[in] pHandle A valid sbgECom handle. - * \param[in] outputPort The output port of the device for the log concerned. - * \param[out] pNmeaTalkerId A 2-char array containing the new NMEA talker id. - * \return SBG_NO_ERROR if the command has been executed successfully. + * \param[in] pHandle A valid sbgECom handle. + * \param[in] outputPort The output port of the device for the log concerned. + * \param[out] pNmeaTalkerId A 2-char array containing the new NMEA talker id. + * \return SBG_NO_ERROR if the command has been executed successfully. */ SbgErrorCode sbgEComCmdOutputSetNmeaTalkerId(SbgEComHandle *pHandle, SbgEComOutputPort outputPort, const char *pNmeaTalkerId); diff --git a/src/commands/sbgEComCmdSensor.c b/src/commands/sbgEComCmdSensor.c index d6fbcf8..26db2fd 100644 --- a/src/commands/sbgEComCmdSensor.c +++ b/src/commands/sbgEComCmdSensor.c @@ -15,470 +15,325 @@ SbgErrorCode sbgEComCmdSensorSetMotionProfileId(SbgEComHandle *pHandle, SbgEComMotionProfileStdIds modelId) { - assert(pHandle); + assert(pHandle); - return sbgEComCmdGenericSetModelId(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_MOTION_PROFILE_ID, modelId); + return sbgEComCmdGenericSetModelId(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_MOTION_PROFILE_ID, modelId); } - SbgErrorCode sbgEComCmdSensorGetMotionProfileId(SbgEComHandle *pHandle, SbgEComMotionProfileStdIds *pModelId) { - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t modelIdAsUint; + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t modelIdAsUint; - assert(pHandle); - assert(pModelId); + assert(pHandle); + assert(pModelId); - errorCode = sbgEComCmdGenericGetModelId(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_MOTION_PROFILE_ID, &modelIdAsUint); + errorCode = sbgEComCmdGenericGetModelId(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_MOTION_PROFILE_ID, &modelIdAsUint); - if (errorCode == SBG_NO_ERROR) - { - *pModelId = (SbgEComMotionProfileStdIds)modelIdAsUint; - } + if (errorCode == SBG_NO_ERROR) + { + *pModelId = (SbgEComMotionProfileStdIds)modelIdAsUint; + } - return errorCode; + return errorCode; } SbgErrorCode sbgEComCmdSensorGetInitCondition(SbgEComHandle *pHandle, SbgEComInitConditionConf *pConf) { - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgEComProtocolPayload receivedPayload; - uint32_t trial; - - assert(pHandle); - assert(pConf); - - sbgEComProtocolPayloadConstruct(&receivedPayload); - - // - // 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 = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_INIT_PARAMETERS, &receivedPayload, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a SBG_ECOM_CMD_INIT_PARAMETERS command - // - if (errorCode == SBG_NO_ERROR) - { - SbgStreamBuffer inputStream; - - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); - - // - // 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; - } - } - - sbgEComProtocolPayloadDestroy(&receivedPayload); - - return errorCode; + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pConf); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + for (trial = 0; trial < pHandle->numTrials; trial++) + { + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_INIT_PARAMETERS, NULL, 0); + + if (errorCode == SBG_NO_ERROR) + { + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_INIT_PARAMETERS, &receivedPayload, pHandle->cmdDefaultTimeOut); + + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + pConf->latitude = sbgStreamBufferReadDoubleLE(&inputStream); + pConf->longitude = sbgStreamBufferReadDoubleLE(&inputStream); + pConf->altitude = sbgStreamBufferReadDoubleLE(&inputStream); + pConf->year = sbgStreamBufferReadUint16LE(&inputStream); + pConf->month = sbgStreamBufferReadUint8LE(&inputStream); + pConf->day = sbgStreamBufferReadUint8LE(&inputStream); + + break; + } + } + else + { + // + // Unable to send the command, stop here + // + break; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; } SbgErrorCode sbgEComCmdSensorSetInitCondition(SbgEComHandle *pHandle, const SbgEComInitConditionConf *pConf) { - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[64]; - 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; + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgStreamBuffer outputStream; + uint8_t outputBuffer[64]; + uint32_t trial; + + assert(pHandle); + assert(pConf); + + for (trial = 0; trial < pHandle->numTrials; trial++) + { + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + + 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); + + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_INIT_PARAMETERS, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + if (errorCode == SBG_NO_ERROR) + { + errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_INIT_PARAMETERS, pHandle->cmdDefaultTimeOut); + + if (errorCode == SBG_NO_ERROR) + { + break; + } + } + else + { + // + // Unable to send the command, stop here + // + break; + } + } + + return errorCode; } SbgErrorCode sbgEComCmdSensorGetAidingAssignment(SbgEComHandle *pHandle, SbgEComAidingAssignConf *pConf) { - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgEComProtocolPayload receivedPayload; - uint32_t trial; - - assert(pHandle); - assert(pConf); - - sbgEComProtocolPayloadConstruct(&receivedPayload); - - // - // 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 = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_AIDING_ASSIGNMENT, &receivedPayload, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a SBG_ECOM_CMD_AIDING_ASSIGNMENT command - // - if (errorCode == SBG_NO_ERROR) - { - SbgStreamBuffer inputStream; - - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); - - // - // 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; - } - } - - sbgEComProtocolPayloadDestroy(&receivedPayload); - - return errorCode; + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pConf); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + for (trial = 0; trial < pHandle->numTrials; trial++) + { + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_AIDING_ASSIGNMENT, NULL, 0); + + if (errorCode == SBG_NO_ERROR) + { + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_AIDING_ASSIGNMENT, &receivedPayload, pHandle->cmdDefaultTimeOut); + + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + pConf->gps1Port = (SbgEComModulePortAssignment)sbgStreamBufferReadUint8LE(&inputStream); + pConf->gps1Sync = (SbgEComModuleSyncAssignment)sbgStreamBufferReadUint8LE(&inputStream); + + // + // Skip the reserved 4 bytes + // + sbgStreamBufferSeek(&inputStream, sizeof(uint32_t), SB_SEEK_CUR_INC); + + 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); + + break; + } + } + else + { + // + // Unable to send the command, stop here + // + break; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; } 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; + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[16]; + SbgStreamBuffer outputStream; + + assert(pHandle); + assert(pConf); + + for (trial = 0; trial < pHandle->numTrials; trial++) + { + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pConf->gps1Port); + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pConf->gps1Sync); + + // + // Skip the reserved 4 bytes + // + sbgStreamBufferWriteUint32LE(&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); + + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_AIDING_ASSIGNMENT, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + if (errorCode == SBG_NO_ERROR) + { + errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_AIDING_ASSIGNMENT, pHandle->cmdDefaultTimeOut); + + if (errorCode == SBG_NO_ERROR) + { + break; + } + } + else + { + // + // Unable to send the command, stop here + // + break; + } + } + + return errorCode; } SbgErrorCode sbgEComCmdSensorGetAlignmentAndLeverArm(SbgEComHandle *pHandle, SbgEComSensorAlignmentInfo *pAlignConf, float *pLeverArm) { - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgEComProtocolPayload receivedPayload; - uint32_t trial; - - assert(pHandle); - assert(pAlignConf); - assert(pLeverArm); - - sbgEComProtocolPayloadConstruct(&receivedPayload); - - // - // 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 = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_IMU_ALIGNMENT_LEVER_ARM, &receivedPayload, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a SBG_ECOM_CMD_IMU_ALIGNMENT command - // - if (errorCode == SBG_NO_ERROR) - { - SbgStreamBuffer inputStream; - - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); - - // - // Read parameters - // - pAlignConf->axisDirectionX = (SbgEComAxisDirection)sbgStreamBufferReadUint8LE(&inputStream); - pAlignConf->axisDirectionY = (SbgEComAxisDirection)sbgStreamBufferReadUint8LE(&inputStream); - pAlignConf->misRoll = sbgStreamBufferReadFloatLE(&inputStream); - pAlignConf->misPitch = sbgStreamBufferReadFloatLE(&inputStream); - pAlignConf->misYaw = sbgStreamBufferReadFloatLE(&inputStream); - pLeverArm[0] = sbgStreamBufferReadFloatLE(&inputStream); - pLeverArm[1] = sbgStreamBufferReadFloatLE(&inputStream); - pLeverArm[2] = sbgStreamBufferReadFloatLE(&inputStream); - - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - sbgEComProtocolPayloadDestroy(&receivedPayload); - - return errorCode; + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pAlignConf); + assert(pLeverArm); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + for (trial = 0; trial < pHandle->numTrials; trial++) + { + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_IMU_ALIGNMENT_LEVER_ARM, NULL, 0); + + if (errorCode == SBG_NO_ERROR) + { + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_IMU_ALIGNMENT_LEVER_ARM, &receivedPayload, pHandle->cmdDefaultTimeOut); + + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + pAlignConf->axisDirectionX = (SbgEComAxisDirection)sbgStreamBufferReadUint8LE(&inputStream); + pAlignConf->axisDirectionY = (SbgEComAxisDirection)sbgStreamBufferReadUint8LE(&inputStream); + pAlignConf->misRoll = sbgStreamBufferReadFloatLE(&inputStream); + pAlignConf->misPitch = sbgStreamBufferReadFloatLE(&inputStream); + pAlignConf->misYaw = sbgStreamBufferReadFloatLE(&inputStream); + pLeverArm[0] = sbgStreamBufferReadFloatLE(&inputStream); + pLeverArm[1] = sbgStreamBufferReadFloatLE(&inputStream); + pLeverArm[2] = sbgStreamBufferReadFloatLE(&inputStream); + + break; + } + } + else + { + // + // Unable to send the command, stop here + // + break; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; } SbgErrorCode sbgEComCmdSensorSetAlignmentAndLeverArm(SbgEComHandle *pHandle, const SbgEComSensorAlignmentInfo *pAlignConf, const float *pLeverArm) { - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[32]; - SbgStreamBuffer outputStream; - - assert(pHandle); - assert(pAlignConf); - assert(pLeverArm); - - // - // 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, pLeverArm[0]); - sbgStreamBufferWriteFloatLE(&outputStream, pLeverArm[1]); - sbgStreamBufferWriteFloatLE(&outputStream, pLeverArm[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; + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[32]; + SbgStreamBuffer outputStream; + + assert(pHandle); + assert(pAlignConf); + assert(pLeverArm); + + for (trial = 0; trial < pHandle->numTrials; trial++) + { + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + + 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, pLeverArm[0]); + sbgStreamBufferWriteFloatLE(&outputStream, pLeverArm[1]); + sbgStreamBufferWriteFloatLE(&outputStream, pLeverArm[2]); + + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_IMU_ALIGNMENT_LEVER_ARM, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + if (errorCode == SBG_NO_ERROR) + { + errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_IMU_ALIGNMENT_LEVER_ARM, pHandle->cmdDefaultTimeOut); + + if (errorCode == SBG_NO_ERROR) + { + break; + } + } + else + { + // + // Unable to send the command, stop here + // + break; + } + } + + return errorCode; } diff --git a/src/commands/sbgEComCmdSensor.h b/src/commands/sbgEComCmdSensor.h index 77970e3..a9ccc76 100644 --- a/src/commands/sbgEComCmdSensor.h +++ b/src/commands/sbgEComCmdSensor.h @@ -1,13 +1,13 @@ /*! - * \file sbgEComCmdSensor.h - * \ingroup commands - * \author SBG Systems - * \date 11 June 2014 + * \file sbgEComCmdSensor.h + * \ingroup commands + * \author SBG Systems + * \date 11 June 2014 * - * \brief Motion profile, aiding assignment & sensor installation commands. + * \brief Motion profile, aiding assignment & sensor installation commands. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -55,13 +55,13 @@ extern "C" { */ typedef enum _SbgEComModulePortAssignment { - SBG_ECOM_MODULE_PORT_A = 0, /*!< Module connected on PORT_A. */ - 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. */ + SBG_ECOM_MODULE_PORT_A = 0, /*!< Module connected on PORT_A. */ + 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; /*! @@ -69,14 +69,14 @@ typedef enum _SbgEComModulePortAssignment */ 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. */ + 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; /*! @@ -84,10 +84,10 @@ typedef enum _SbgEComModuleSyncAssignment */ 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). */ + 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; /*! @@ -95,21 +95,23 @@ typedef enum _SbgEComOdometerPinAssignment */ 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 with strict lateral velocity constraints. */ - 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_PEDESTRIAN = 6, /*!< Pedestrian applications using foot odometer */ - 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 */ - SBG_ECOM_MOTION_PROFILE_STATIC = 9, /*!< Static motion profile that delivers stable results for 27/7 operations. */ - SBG_ECOM_MOTION_PROFILE_TRUCK = 10, /*!< Truck applications with medium lateral velocity constraints. */ - SBG_ECOM_MOTION_PROFILE_RAILWAY = 11 /*!< Train applications with relaxed lateral velocity constraints. */ + 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 with strict lateral velocity constraints. */ + 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_PEDESTRIAN = 6, /*!< Pedestrian applications using foot odometer (only for ELLIPSE firmware v3.x and above). */ + 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 */ + SBG_ECOM_MOTION_PROFILE_STATIC = 9, /*!< Static motion profile that delivers stable results for 27/7 operations. */ + SBG_ECOM_MOTION_PROFILE_TRUCK = 10, /*!< Truck applications with medium lateral velocity constraints (only for ELLIPSE firmware v3.x and above). */ + SBG_ECOM_MOTION_PROFILE_RAILWAY = 11, /*!< Train applications with relaxed lateral velocity constraints (only for ELLIPSE firmware v3.x and above). */ + SBG_ECOM_MOTION_PROFILE_OFF_ROAD_VEHICLE = 12, /*!< Off road vehicle such as Jeeps, expect slide slip and drift (only for ELLIPSE firmware v3.x and above). */ + SBG_ECOM_MOTION_PROFILE_UNDERWATER = 13 /*!< Marine underwater with low dynamics such as for ROV (only for ELLIPSE firmware v3.x and above). */ } SbgEComMotionProfileStdIds; //----------------------------------------------------------------------// -//- Event configurations -// +//- Event configurations -// //----------------------------------------------------------------------// /*! @@ -117,13 +119,13 @@ typedef enum _SbgEComMotionProfileStdIds */ 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. */ + 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 timestamp 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; /*! @@ -131,11 +133,11 @@ typedef struct _SbgEComAidingAssignConf */ 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 */ + 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; /*! @@ -143,12 +145,12 @@ typedef struct _SbgEComSensorAlignmentInfo */ 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 */ + double latitude; /*!< Initial latitude in °, positive north */ + double longitude; /*!< Initial longitude in °, positive east */ + double altitude; /*!< Initial height above ellipsoid in meters, positive up */ + uint16_t year; /*!< Initial Year */ + uint8_t month; /*!< Initial month */ + uint8_t day; /*!< Initial day */ } SbgEComInitConditionConf; //----------------------------------------------------------------------// @@ -158,74 +160,74 @@ typedef struct _SbgEComInitConditionConf /*! * Set the motion profile id used to tune the Kalman Filter to a specific application * - * \param[in] pHandle A valid sbgECom handle. - * \param[in] modelId Motion profile id to set - * \return SBG_NO_ERROR if the command has been executed successfully. + * \param[in] pHandle A valid sbgECom handle. + * \param[in] modelId Motion profile id to set + * \return SBG_NO_ERROR if the command has been executed successfully. */ SbgErrorCode sbgEComCmdSensorSetMotionProfileId(SbgEComHandle *pHandle, SbgEComMotionProfileStdIds modelId); /*! * Retrieve the motion profile id. * - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pModelId Retrieved motion profile id - * \return SBG_NO_ERROR if the command has been executed successfully. + * \param[in] pHandle A valid sbgECom handle. + * \param[out] pModelId Retrieved motion profile id + * \return SBG_NO_ERROR if the command has been executed successfully. */ SbgErrorCode sbgEComCmdSensorGetMotionProfileId(SbgEComHandle *pHandle, SbgEComMotionProfileStdIds *pModelId); /*! * 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. + * \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. + * \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. + * \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. + * \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] pLeverArm 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. + * \param[in] pHandle A valid sbgECom handle. + * \param[out] pAlignConf Pointer to a SbgEComSensorAlignmentInfo struct to hold alignment configuration of the sensor. + * \param[out] pLeverArm 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 *pLeverArm); /*! * 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] pLeverArm Pointer to a table containing lever arm X, Y, Z components in meters. - * \return SBG_NO_ERROR if the command has been executed successfully. + * \param[in] pHandle A valid sbgECom handle. + * \param[in] pAlignConf Pointer to a SbgEComSensorAlignmentInfo struct holding alignment configuration for the sensor. + * \param[in] pLeverArm 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 *pLeverArm); diff --git a/src/commands/sbgEComCmdSettings.c b/src/commands/sbgEComCmdSettings.c index 17c9f3d..dcc99c0 100644 --- a/src/commands/sbgEComCmdSettings.c +++ b/src/commands/sbgEComCmdSettings.c @@ -16,74 +16,74 @@ SbgErrorCode sbgEComCmdSettingsAction(SbgEComHandle *pHandle, SbgEComSettingsAction action) { - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[1]; - SbgStreamBuffer outputStream; + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[1]; + SbgStreamBuffer outputStream; - assert(pHandle); + assert(pHandle); - // - // Build the payload to send - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - sbgStreamBufferWriteUint8(&outputStream, action); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the command and the action - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_SETTINGS_ACTION, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + // + // Build the payload to send + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + sbgStreamBufferWriteUint8(&outputStream, action); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the command and the 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); + // + // 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; + // + // 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 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); + // + // Call function that handle data transfer + // + return sbgEComTransferSend(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_IMPORT_SETTINGS, pBuffer, size); } 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); + // + // 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/src/commands/sbgEComCmdSettings.h b/src/commands/sbgEComCmdSettings.h index 9e33abc..4da15d0 100644 --- a/src/commands/sbgEComCmdSettings.h +++ b/src/commands/sbgEComCmdSettings.h @@ -1,13 +1,13 @@ /*! - * \file sbgEComCmdSettings.h - * \ingroup commands - * \author SBG Systems - * \date 11 June 2014 + * \file sbgEComCmdSettings.h + * \ingroup commands + * \author SBG Systems + * \date 11 June 2014 * - * \brief Import/export/save settings commands. + * \brief Import/export/save settings commands. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -52,9 +52,9 @@ extern "C" { */ 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. */ + 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; //----------------------------------------------------------------------// @@ -65,13 +65,13 @@ typedef enum _SbgEComSettingsAction * Send a command to execute a specific system action to reboot/save/restore default settings. * * 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. + * - 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. + * \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); @@ -80,21 +80,21 @@ SbgErrorCode sbgEComCmdSettingsAction(SbgEComHandle *pHandle, SbgEComSettingsAct * * 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. + * \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. + * \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); diff --git a/src/defs/sbgEComDefsGnss.c b/src/defs/sbgEComDefsGnss.c index b49c283..4e64f19 100644 --- a/src/defs/sbgEComDefsGnss.c +++ b/src/defs/sbgEComDefsGnss.c @@ -13,8 +13,8 @@ */ typedef struct _SbgEComSignalIdDesc { - SbgEComSignalId id; /*!< Signal ID. */ - const char *pName; /*!< Corresponding NULL terminated C string. */ + SbgEComSignalId id; /*!< Signal ID. */ + const char *pName; /*!< Corresponding NULL terminated C string. */ } SbgEComSignalIdDesc; //----------------------------------------------------------------------// @@ -23,367 +23,367 @@ typedef struct _SbgEComSignalIdDesc SbgEComConstellationId sbgEComGetConstellationFromSignalId(SbgEComSignalId signalId) { - SbgEComConstellationId constellationId = SBG_ECOM_CONSTELLATION_ID_UNKNOWN; - - // - // Don't use a default catch to explicitly handle ALL enum values - // - switch (signalId) - { - case SBG_ECOM_SIGNAL_ID_UNKNOWN: - constellationId = SBG_ECOM_CONSTELLATION_ID_UNKNOWN; - break; - - // - // GPS constellation - // - case SBG_ECOM_SIGNAL_ID_GPS_L1C_DP: - case SBG_ECOM_SIGNAL_ID_GPS_L1C_D: - case SBG_ECOM_SIGNAL_ID_GPS_L1C_P: - case SBG_ECOM_SIGNAL_ID_GPS_L1_W: - case SBG_ECOM_SIGNAL_ID_GPS_L1_CA: - case SBG_ECOM_SIGNAL_ID_GPS_L1P: - case SBG_ECOM_SIGNAL_ID_GPS_L1_PY: - case SBG_ECOM_SIGNAL_ID_GPS_L1M: - case SBG_ECOM_SIGNAL_ID_GPS_L2C_ML: - case SBG_ECOM_SIGNAL_ID_GPS_L2C_L: - case SBG_ECOM_SIGNAL_ID_GPS_L2_SEMICL: - case SBG_ECOM_SIGNAL_ID_GPS_L2_W: - case SBG_ECOM_SIGNAL_ID_GPS_L2_CA: - case SBG_ECOM_SIGNAL_ID_GPS_L2C_M: - case SBG_ECOM_SIGNAL_ID_GPS_L2_PY: - case SBG_ECOM_SIGNAL_ID_GPS_L2M: - case SBG_ECOM_SIGNAL_ID_GPS_L2P: - case SBG_ECOM_SIGNAL_ID_GPS_L5_IQ: - case SBG_ECOM_SIGNAL_ID_GPS_L5_I: - case SBG_ECOM_SIGNAL_ID_GPS_L5_Q: - constellationId = SBG_ECOM_CONSTELLATION_ID_GPS; - break; - - // - // GLONASS constellation - // - case SBG_ECOM_SIGNAL_ID_GLONASS_G1_P: - case SBG_ECOM_SIGNAL_ID_GLONASS_G1_CA: - case SBG_ECOM_SIGNAL_ID_GLONASS_G2_P: - case SBG_ECOM_SIGNAL_ID_GLONASS_G2_CA: - case SBG_ECOM_SIGNAL_ID_GLONASS_G3_I: - case SBG_ECOM_SIGNAL_ID_GLONASS_G3_Q: - case SBG_ECOM_SIGNAL_ID_GLONASS_G3_IQ: - constellationId = SBG_ECOM_CONSTELLATION_ID_GLONASS; - break; - - // - // Galileo constellation - // - case SBG_ECOM_SIGNAL_ID_GALILEO_E1_BC: - case SBG_ECOM_SIGNAL_ID_GALILEO_E1_C: - case SBG_ECOM_SIGNAL_ID_GALILEO_E1_B: - case SBG_ECOM_SIGNAL_ID_GALILEO_E1_A: - case SBG_ECOM_SIGNAL_ID_GALILEO_E1_ABC: - case SBG_ECOM_SIGNAL_ID_GALILEO_E5B_IQ: - case SBG_ECOM_SIGNAL_ID_GALILEO_E5B_I: - case SBG_ECOM_SIGNAL_ID_GALILEO_E5B_Q: - case SBG_ECOM_SIGNAL_ID_GALILEO_E5A_IQ: - case SBG_ECOM_SIGNAL_ID_GALILEO_E5A_I: - case SBG_ECOM_SIGNAL_ID_GALILEO_E5A_Q: - case SBG_ECOM_SIGNAL_ID_GALILEO_E5_IQ: - case SBG_ECOM_SIGNAL_ID_GALILEO_E5_I: - case SBG_ECOM_SIGNAL_ID_GALILEO_E5_Q: - case SBG_ECOM_SIGNAL_ID_GALILEO_E6_BC: - case SBG_ECOM_SIGNAL_ID_GALILEO_E6_C: - case SBG_ECOM_SIGNAL_ID_GALILEO_E6_B: - case SBG_ECOM_SIGNAL_ID_GALILEO_E6_ABC: - case SBG_ECOM_SIGNAL_ID_GALILEO_E6_A: - constellationId = SBG_ECOM_CONSTELLATION_ID_GALILEO; - break; - - // - // BeiDou constellation - // - case SBG_ECOM_SIGNAL_ID_BEIDOU_B1IQ: - case SBG_ECOM_SIGNAL_ID_BEIDOU_B1I: - case SBG_ECOM_SIGNAL_ID_BEIDOU_B1Q: - case SBG_ECOM_SIGNAL_ID_BEIDOU_B1C_P: - case SBG_ECOM_SIGNAL_ID_BEIDOU_B1C_DP: - case SBG_ECOM_SIGNAL_ID_BEIDOU_B1C_D: - case SBG_ECOM_SIGNAL_ID_BEIDOU_B1A_P: - case SBG_ECOM_SIGNAL_ID_BEIDOU_B1A_DP: - case SBG_ECOM_SIGNAL_ID_BEIDOU_B1A_D: - case SBG_ECOM_SIGNAL_ID_BEIDOU_B2IQ: - case SBG_ECOM_SIGNAL_ID_BEIDOU_B2I: - case SBG_ECOM_SIGNAL_ID_BEIDOU_B2A_P: - case SBG_ECOM_SIGNAL_ID_BEIDOU_B2A_DP: - case SBG_ECOM_SIGNAL_ID_BEIDOU_B2A_D: - case SBG_ECOM_SIGNAL_ID_BEIDOU_B2Q: - case SBG_ECOM_SIGNAL_ID_BEIDOU_B2B_P: - case SBG_ECOM_SIGNAL_ID_BEIDOU_B2B_DP: - case SBG_ECOM_SIGNAL_ID_BEIDOU_B2B_D: - case SBG_ECOM_SIGNAL_ID_BEIDOU_B2AB_P: - case SBG_ECOM_SIGNAL_ID_BEIDOU_B2AB_DP: - case SBG_ECOM_SIGNAL_ID_BEIDOU_B2AB_D: - case SBG_ECOM_SIGNAL_ID_BEIDOU_B3IQ: - case SBG_ECOM_SIGNAL_ID_BEIDOU_B3I: - case SBG_ECOM_SIGNAL_ID_BEIDOU_B3Q: - case SBG_ECOM_SIGNAL_ID_BEIDOU_B3A_D: - case SBG_ECOM_SIGNAL_ID_BEIDOU_B3A_P: - case SBG_ECOM_SIGNAL_ID_BEIDOU_B3A_DP: - constellationId = SBG_ECOM_CONSTELLATION_ID_BEIDOU; - break; - - // - // QZSS constellation - // - case SBG_ECOM_SIGNAL_ID_QZSS_L1C_DP: - case SBG_ECOM_SIGNAL_ID_QZSS_L1C_D: - case SBG_ECOM_SIGNAL_ID_QZSS_L1C_P: - case SBG_ECOM_SIGNAL_ID_QZSS_L1_CA: - case SBG_ECOM_SIGNAL_ID_QZSS_L1_SAIF: - case SBG_ECOM_SIGNAL_ID_QZSS_L1_SB: - case SBG_ECOM_SIGNAL_ID_QZSS_L2C_ML: - case SBG_ECOM_SIGNAL_ID_QZSS_L2C_L: - case SBG_ECOM_SIGNAL_ID_QZSS_L2C_M: - case SBG_ECOM_SIGNAL_ID_QZSS_L5_IQ: - case SBG_ECOM_SIGNAL_ID_QZSS_L5_I: - case SBG_ECOM_SIGNAL_ID_QZSS_L5_Q: - case SBG_ECOM_SIGNAL_ID_QZSS_L5S_IQ: - case SBG_ECOM_SIGNAL_ID_QZSS_L5S_I: - case SBG_ECOM_SIGNAL_ID_QZSS_L5S_Q: - case SBG_ECOM_SIGNAL_ID_QZSS_L6_P: - case SBG_ECOM_SIGNAL_ID_QZSS_L6_DP: - case SBG_ECOM_SIGNAL_ID_QZSS_L6_D: - case SBG_ECOM_SIGNAL_ID_QZSS_L6_E: - case SBG_ECOM_SIGNAL_ID_QZSS_L6_DE: - constellationId = SBG_ECOM_CONSTELLATION_ID_QZSS; - break; - - // - // SBAS system - // - case SBG_ECOM_SIGNAL_ID_SBAS_L1_CA: - case SBG_ECOM_SIGNAL_ID_SBAS_L5_I: - case SBG_ECOM_SIGNAL_ID_SBAS_L5_Q: - case SBG_ECOM_SIGNAL_ID_SBAS_L5_IQ: - constellationId = SBG_ECOM_CONSTELLATION_ID_SBAS; - break; - - // - // IRNSS constellation - // - case SBG_ECOM_SIGNAL_ID_IRNSS_L5_A: - case SBG_ECOM_SIGNAL_ID_IRNSS_L5_B: - case SBG_ECOM_SIGNAL_ID_IRNSS_L5_C: - case SBG_ECOM_SIGNAL_ID_IRNSS_L5_BC: - case SBG_ECOM_SIGNAL_ID_IRNSS_S9_A: - case SBG_ECOM_SIGNAL_ID_IRNSS_S9_B: - case SBG_ECOM_SIGNAL_ID_IRNSS_S9_C: - case SBG_ECOM_SIGNAL_ID_IRNSS_S9_BC: - constellationId = SBG_ECOM_CONSTELLATION_ID_IRNSS; - break; - - // - // L-Band system - // - case SBG_ECOM_SIGNAL_ID_LBAND: - constellationId = SBG_ECOM_CONSTELLATION_ID_LBAND; - break; - } - - return constellationId; + SbgEComConstellationId constellationId = SBG_ECOM_CONSTELLATION_ID_UNKNOWN; + + // + // Don't use a default catch to explicitly handle ALL enum values + // + switch (signalId) + { + case SBG_ECOM_SIGNAL_ID_UNKNOWN: + constellationId = SBG_ECOM_CONSTELLATION_ID_UNKNOWN; + break; + + // + // GPS constellation + // + case SBG_ECOM_SIGNAL_ID_GPS_L1C_DP: + case SBG_ECOM_SIGNAL_ID_GPS_L1C_D: + case SBG_ECOM_SIGNAL_ID_GPS_L1C_P: + case SBG_ECOM_SIGNAL_ID_GPS_L1_W: + case SBG_ECOM_SIGNAL_ID_GPS_L1_CA: + case SBG_ECOM_SIGNAL_ID_GPS_L1P: + case SBG_ECOM_SIGNAL_ID_GPS_L1_PY: + case SBG_ECOM_SIGNAL_ID_GPS_L1M: + case SBG_ECOM_SIGNAL_ID_GPS_L2C_ML: + case SBG_ECOM_SIGNAL_ID_GPS_L2C_L: + case SBG_ECOM_SIGNAL_ID_GPS_L2_SEMICL: + case SBG_ECOM_SIGNAL_ID_GPS_L2_W: + case SBG_ECOM_SIGNAL_ID_GPS_L2_CA: + case SBG_ECOM_SIGNAL_ID_GPS_L2C_M: + case SBG_ECOM_SIGNAL_ID_GPS_L2_PY: + case SBG_ECOM_SIGNAL_ID_GPS_L2M: + case SBG_ECOM_SIGNAL_ID_GPS_L2P: + case SBG_ECOM_SIGNAL_ID_GPS_L5_IQ: + case SBG_ECOM_SIGNAL_ID_GPS_L5_I: + case SBG_ECOM_SIGNAL_ID_GPS_L5_Q: + constellationId = SBG_ECOM_CONSTELLATION_ID_GPS; + break; + + // + // GLONASS constellation + // + case SBG_ECOM_SIGNAL_ID_GLONASS_G1_P: + case SBG_ECOM_SIGNAL_ID_GLONASS_G1_CA: + case SBG_ECOM_SIGNAL_ID_GLONASS_G2_P: + case SBG_ECOM_SIGNAL_ID_GLONASS_G2_CA: + case SBG_ECOM_SIGNAL_ID_GLONASS_G3_I: + case SBG_ECOM_SIGNAL_ID_GLONASS_G3_Q: + case SBG_ECOM_SIGNAL_ID_GLONASS_G3_IQ: + constellationId = SBG_ECOM_CONSTELLATION_ID_GLONASS; + break; + + // + // Galileo constellation + // + case SBG_ECOM_SIGNAL_ID_GALILEO_E1_BC: + case SBG_ECOM_SIGNAL_ID_GALILEO_E1_C: + case SBG_ECOM_SIGNAL_ID_GALILEO_E1_B: + case SBG_ECOM_SIGNAL_ID_GALILEO_E1_A: + case SBG_ECOM_SIGNAL_ID_GALILEO_E1_ABC: + case SBG_ECOM_SIGNAL_ID_GALILEO_E5B_IQ: + case SBG_ECOM_SIGNAL_ID_GALILEO_E5B_I: + case SBG_ECOM_SIGNAL_ID_GALILEO_E5B_Q: + case SBG_ECOM_SIGNAL_ID_GALILEO_E5A_IQ: + case SBG_ECOM_SIGNAL_ID_GALILEO_E5A_I: + case SBG_ECOM_SIGNAL_ID_GALILEO_E5A_Q: + case SBG_ECOM_SIGNAL_ID_GALILEO_E5_IQ: + case SBG_ECOM_SIGNAL_ID_GALILEO_E5_I: + case SBG_ECOM_SIGNAL_ID_GALILEO_E5_Q: + case SBG_ECOM_SIGNAL_ID_GALILEO_E6_BC: + case SBG_ECOM_SIGNAL_ID_GALILEO_E6_C: + case SBG_ECOM_SIGNAL_ID_GALILEO_E6_B: + case SBG_ECOM_SIGNAL_ID_GALILEO_E6_ABC: + case SBG_ECOM_SIGNAL_ID_GALILEO_E6_A: + constellationId = SBG_ECOM_CONSTELLATION_ID_GALILEO; + break; + + // + // BeiDou constellation + // + case SBG_ECOM_SIGNAL_ID_BEIDOU_B1IQ: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B1I: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B1Q: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B1C_P: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B1C_DP: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B1C_D: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B1A_P: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B1A_DP: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B1A_D: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B2IQ: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B2I: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B2A_P: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B2A_DP: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B2A_D: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B2Q: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B2B_P: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B2B_DP: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B2B_D: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B2AB_P: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B2AB_DP: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B2AB_D: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B3IQ: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B3I: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B3Q: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B3A_D: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B3A_P: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B3A_DP: + constellationId = SBG_ECOM_CONSTELLATION_ID_BEIDOU; + break; + + // + // QZSS constellation + // + case SBG_ECOM_SIGNAL_ID_QZSS_L1C_DP: + case SBG_ECOM_SIGNAL_ID_QZSS_L1C_D: + case SBG_ECOM_SIGNAL_ID_QZSS_L1C_P: + case SBG_ECOM_SIGNAL_ID_QZSS_L1_CA: + case SBG_ECOM_SIGNAL_ID_QZSS_L1_SAIF: + case SBG_ECOM_SIGNAL_ID_QZSS_L1_SB: + case SBG_ECOM_SIGNAL_ID_QZSS_L2C_ML: + case SBG_ECOM_SIGNAL_ID_QZSS_L2C_L: + case SBG_ECOM_SIGNAL_ID_QZSS_L2C_M: + case SBG_ECOM_SIGNAL_ID_QZSS_L5_IQ: + case SBG_ECOM_SIGNAL_ID_QZSS_L5_I: + case SBG_ECOM_SIGNAL_ID_QZSS_L5_Q: + case SBG_ECOM_SIGNAL_ID_QZSS_L5S_IQ: + case SBG_ECOM_SIGNAL_ID_QZSS_L5S_I: + case SBG_ECOM_SIGNAL_ID_QZSS_L5S_Q: + case SBG_ECOM_SIGNAL_ID_QZSS_L6_P: + case SBG_ECOM_SIGNAL_ID_QZSS_L6_DP: + case SBG_ECOM_SIGNAL_ID_QZSS_L6_D: + case SBG_ECOM_SIGNAL_ID_QZSS_L6_E: + case SBG_ECOM_SIGNAL_ID_QZSS_L6_DE: + constellationId = SBG_ECOM_CONSTELLATION_ID_QZSS; + break; + + // + // SBAS system + // + case SBG_ECOM_SIGNAL_ID_SBAS_L1_CA: + case SBG_ECOM_SIGNAL_ID_SBAS_L5_I: + case SBG_ECOM_SIGNAL_ID_SBAS_L5_Q: + case SBG_ECOM_SIGNAL_ID_SBAS_L5_IQ: + constellationId = SBG_ECOM_CONSTELLATION_ID_SBAS; + break; + + // + // IRNSS constellation + // + case SBG_ECOM_SIGNAL_ID_IRNSS_L5_A: + case SBG_ECOM_SIGNAL_ID_IRNSS_L5_B: + case SBG_ECOM_SIGNAL_ID_IRNSS_L5_C: + case SBG_ECOM_SIGNAL_ID_IRNSS_L5_BC: + case SBG_ECOM_SIGNAL_ID_IRNSS_S9_A: + case SBG_ECOM_SIGNAL_ID_IRNSS_S9_B: + case SBG_ECOM_SIGNAL_ID_IRNSS_S9_C: + case SBG_ECOM_SIGNAL_ID_IRNSS_S9_BC: + constellationId = SBG_ECOM_CONSTELLATION_ID_IRNSS; + break; + + // + // L-Band system + // + case SBG_ECOM_SIGNAL_ID_LBAND: + constellationId = SBG_ECOM_CONSTELLATION_ID_LBAND; + break; + } + + return constellationId; } bool sbgEComSignalIdIsValid(uint8_t signalId) { - if ( (signalId == SBG_ECOM_SIGNAL_ID_UNKNOWN) || - (sbgEComGetConstellationFromSignalId(signalId) != SBG_ECOM_CONSTELLATION_ID_UNKNOWN)) - { - return true; - } - else - { - return false; - } + if ( (signalId == SBG_ECOM_SIGNAL_ID_UNKNOWN) || + (sbgEComGetConstellationFromSignalId(signalId) != SBG_ECOM_CONSTELLATION_ID_UNKNOWN)) + { + return true; + } + else + { + return false; + } } const char *sbgEComSignalToStr(SbgEComSignalId signalId) { - static const SbgEComSignalIdDesc signalIdDesc[] = - { - { SBG_ECOM_SIGNAL_ID_UNKNOWN, "unknown" }, - - { SBG_ECOM_SIGNAL_ID_GPS_L1C_DP, "gpsL1C_DP" }, - { SBG_ECOM_SIGNAL_ID_GPS_L1C_D, "gpsL1C_D" }, - { SBG_ECOM_SIGNAL_ID_GPS_L1C_P, "gpsL1C_P" }, - { SBG_ECOM_SIGNAL_ID_GPS_L1_W, "gpsL1_W" }, - { SBG_ECOM_SIGNAL_ID_GPS_L1_CA, "gpsL1_CA" }, - { SBG_ECOM_SIGNAL_ID_GPS_L1P, "gpsL1P" }, - { SBG_ECOM_SIGNAL_ID_GPS_L1_PY, "gpsL1_PY" }, - { SBG_ECOM_SIGNAL_ID_GPS_L1M, "gpsL1M" }, - { SBG_ECOM_SIGNAL_ID_GPS_L2C_ML, "gpsL2C_ML" }, - { SBG_ECOM_SIGNAL_ID_GPS_L2C_L, "gpsL2C_L" }, - { SBG_ECOM_SIGNAL_ID_GPS_L2_SEMICL, "gpsL2_SEMICL" }, - { SBG_ECOM_SIGNAL_ID_GPS_L2_W, "gpsL2_W" }, - { SBG_ECOM_SIGNAL_ID_GPS_L2_CA, "gpsL2_CA" }, - { SBG_ECOM_SIGNAL_ID_GPS_L2C_M, "gpsL2C_M" }, - { SBG_ECOM_SIGNAL_ID_GPS_L2_PY, "gpsL2_PY" }, - { SBG_ECOM_SIGNAL_ID_GPS_L2M, "gpsL2M" }, - { SBG_ECOM_SIGNAL_ID_GPS_L2P, "gpsL2P" }, - { SBG_ECOM_SIGNAL_ID_GPS_L5_IQ, "gpsL5_IQ" }, - { SBG_ECOM_SIGNAL_ID_GPS_L5_I, "gpsL5_I" }, - { SBG_ECOM_SIGNAL_ID_GPS_L5_Q, "gpsL5_Q" }, - - { SBG_ECOM_SIGNAL_ID_GLONASS_G1_P, "glonassG1_P" }, - { SBG_ECOM_SIGNAL_ID_GLONASS_G1_CA, "glonassG1_CA" }, - { SBG_ECOM_SIGNAL_ID_GLONASS_G2_P, "glonassG2_P" }, - { SBG_ECOM_SIGNAL_ID_GLONASS_G2_CA, "glonassG2_CA" }, - { SBG_ECOM_SIGNAL_ID_GLONASS_G3_I, "glonassG3_I" }, - { SBG_ECOM_SIGNAL_ID_GLONASS_G3_Q, "glonassG3_Q" }, - { SBG_ECOM_SIGNAL_ID_GLONASS_G3_IQ, "glonassG3_IQ" }, - - { SBG_ECOM_SIGNAL_ID_GALILEO_E1_BC, "galileoE1_BC" }, - { SBG_ECOM_SIGNAL_ID_GALILEO_E1_C, "galileoE1_C" }, - { SBG_ECOM_SIGNAL_ID_GALILEO_E1_B, "galileoE1_B" }, - { SBG_ECOM_SIGNAL_ID_GALILEO_E1_A, "galileoE1_A" }, - { SBG_ECOM_SIGNAL_ID_GALILEO_E1_ABC, "galileoE1_ABC" }, - { SBG_ECOM_SIGNAL_ID_GALILEO_E5B_IQ, "galileoE5B_IQ" }, - { SBG_ECOM_SIGNAL_ID_GALILEO_E5B_I, "galileoE5B_I" }, - { SBG_ECOM_SIGNAL_ID_GALILEO_E5B_Q, "galileoE5B_Q" }, - { SBG_ECOM_SIGNAL_ID_GALILEO_E5A_IQ, "galileoE5A_IQ" }, - { SBG_ECOM_SIGNAL_ID_GALILEO_E5A_I, "galileoE5A_I" }, - { SBG_ECOM_SIGNAL_ID_GALILEO_E5A_Q, "galileoE5A_Q" }, - { SBG_ECOM_SIGNAL_ID_GALILEO_E5_IQ, "galileoE5_IQ" }, - { SBG_ECOM_SIGNAL_ID_GALILEO_E5_I, "galileoE5_I" }, - { SBG_ECOM_SIGNAL_ID_GALILEO_E5_Q, "galileoE5_Q" }, - { SBG_ECOM_SIGNAL_ID_GALILEO_E6_BC, "galileoE6_BC" }, - { SBG_ECOM_SIGNAL_ID_GALILEO_E6_C, "galileoE6_C" }, - { SBG_ECOM_SIGNAL_ID_GALILEO_E6_B, "galileoE6_B" }, - { SBG_ECOM_SIGNAL_ID_GALILEO_E6_ABC, "galileoE6_ABC" }, - { SBG_ECOM_SIGNAL_ID_GALILEO_E6_A, "galileoE6_A" }, - - { SBG_ECOM_SIGNAL_ID_BEIDOU_B1IQ, "beidouB1IQ" }, - { SBG_ECOM_SIGNAL_ID_BEIDOU_B1I, "beidouB1I" }, - { SBG_ECOM_SIGNAL_ID_BEIDOU_B1Q, "beidouB1Q" }, - { SBG_ECOM_SIGNAL_ID_BEIDOU_B1C_P, "beidouB1C_P" }, - { SBG_ECOM_SIGNAL_ID_BEIDOU_B1C_DP, "beidouB1C_DP" }, - { SBG_ECOM_SIGNAL_ID_BEIDOU_B1C_D, "beidouB1C_D" }, - { SBG_ECOM_SIGNAL_ID_BEIDOU_B1A_P, "beidouB1A_P" }, - { SBG_ECOM_SIGNAL_ID_BEIDOU_B1A_DP, "beidouB1A_DP" }, - { SBG_ECOM_SIGNAL_ID_BEIDOU_B1A_D, "beidouB1A_D" }, - { SBG_ECOM_SIGNAL_ID_BEIDOU_B2IQ, "beidouB2IQ" }, - { SBG_ECOM_SIGNAL_ID_BEIDOU_B2I, "beidouB2I" }, - { SBG_ECOM_SIGNAL_ID_BEIDOU_B2A_P, "beidouB2A_P" }, - { SBG_ECOM_SIGNAL_ID_BEIDOU_B2A_DP, "beidouB2A_DP" }, - { SBG_ECOM_SIGNAL_ID_BEIDOU_B2A_D, "beidouB2A_D" }, - { SBG_ECOM_SIGNAL_ID_BEIDOU_B2Q, "beidouB2Q" }, - { SBG_ECOM_SIGNAL_ID_BEIDOU_B2B_P, "beidouB2B_P" }, - { SBG_ECOM_SIGNAL_ID_BEIDOU_B2B_DP, "beidouB2B_DP" }, - { SBG_ECOM_SIGNAL_ID_BEIDOU_B2B_D, "beidouB2B_D" }, - { SBG_ECOM_SIGNAL_ID_BEIDOU_B2AB_P, "beidouB2AB_P" }, - { SBG_ECOM_SIGNAL_ID_BEIDOU_B2AB_DP, "beidouB2AB_DP" }, - { SBG_ECOM_SIGNAL_ID_BEIDOU_B2AB_D, "beidouB2AB_D" }, - { SBG_ECOM_SIGNAL_ID_BEIDOU_B3IQ, "beidouB3IQ" }, - { SBG_ECOM_SIGNAL_ID_BEIDOU_B3I, "beidouB3I" }, - { SBG_ECOM_SIGNAL_ID_BEIDOU_B3Q, "beidouB3Q" }, - { SBG_ECOM_SIGNAL_ID_BEIDOU_B3A_D, "beidouB3A_D" }, - { SBG_ECOM_SIGNAL_ID_BEIDOU_B3A_P, "beidouB3A_P" }, - { SBG_ECOM_SIGNAL_ID_BEIDOU_B3A_DP, "beidouB3A_DP" }, - - { SBG_ECOM_SIGNAL_ID_QZSS_L1C_DP, "qzssL1C_DP" }, - { SBG_ECOM_SIGNAL_ID_QZSS_L1C_D, "qzssL1C_D" }, - { SBG_ECOM_SIGNAL_ID_QZSS_L1C_P, "qzssL1C_P" }, - { SBG_ECOM_SIGNAL_ID_QZSS_L1_CA, "qzssL1_CA" }, - { SBG_ECOM_SIGNAL_ID_QZSS_L1_SAIF, "qzssL1_SAIF" }, - { SBG_ECOM_SIGNAL_ID_QZSS_L1_SB, "qzssL1_SB" }, - { SBG_ECOM_SIGNAL_ID_QZSS_L2C_ML, "qzssL2C_ML" }, - { SBG_ECOM_SIGNAL_ID_QZSS_L2C_L, "qzssL2C_L" }, - { SBG_ECOM_SIGNAL_ID_QZSS_L2C_M, "qzssL2C_M" }, - { SBG_ECOM_SIGNAL_ID_QZSS_L5_IQ, "qzssL5_IQ" }, - { SBG_ECOM_SIGNAL_ID_QZSS_L5_I, "qzssL5_I" }, - { SBG_ECOM_SIGNAL_ID_QZSS_L5_Q, "qzssL5_Q" }, - { SBG_ECOM_SIGNAL_ID_QZSS_L5S_IQ, "qzssL5S_IQ" }, - { SBG_ECOM_SIGNAL_ID_QZSS_L5S_I, "qzssL5S_I" }, - { SBG_ECOM_SIGNAL_ID_QZSS_L5S_Q, "qzssL5S_Q" }, - { SBG_ECOM_SIGNAL_ID_QZSS_L6_P, "qzssL6_P" }, - { SBG_ECOM_SIGNAL_ID_QZSS_L6_DP, "qzssL6_DP" }, - { SBG_ECOM_SIGNAL_ID_QZSS_L6_D, "qzssL6_D" }, - { SBG_ECOM_SIGNAL_ID_QZSS_L6_E, "qzssL6_E" }, - { SBG_ECOM_SIGNAL_ID_QZSS_L6_DE, "qzssL6_DE" }, - - { SBG_ECOM_SIGNAL_ID_SBAS_L1_CA, "sbasL1_CA" }, - { SBG_ECOM_SIGNAL_ID_SBAS_L5_I, "sbasL5_I" }, - { SBG_ECOM_SIGNAL_ID_SBAS_L5_Q, "sbasL5_Q" }, - { SBG_ECOM_SIGNAL_ID_SBAS_L5_IQ, "sbasL5_IQ" }, - - { SBG_ECOM_SIGNAL_ID_IRNSS_L5_A, "irnssL5_A" }, - { SBG_ECOM_SIGNAL_ID_IRNSS_L5_B, "irnssL5_B" }, - { SBG_ECOM_SIGNAL_ID_IRNSS_L5_C, "irnssL5_C" }, - { SBG_ECOM_SIGNAL_ID_IRNSS_L5_BC, "irnssL5_BC" }, - { SBG_ECOM_SIGNAL_ID_IRNSS_S9_A, "irnssS9_A" }, - { SBG_ECOM_SIGNAL_ID_IRNSS_S9_B, "irnssS9_B" }, - { SBG_ECOM_SIGNAL_ID_IRNSS_S9_C, "irnssS9_C" }, - { SBG_ECOM_SIGNAL_ID_IRNSS_S9_BC, "irnssS9_BC" }, - - { SBG_ECOM_SIGNAL_ID_LBAND, "lband" } - }; - - for (size_t i = 0; i < SBG_ARRAY_SIZE(signalIdDesc); i++) - { - if (signalIdDesc[i].id == signalId) - { - return signalIdDesc[i].pName; - } - } - - // - // Enforce that the first item is the unknown signal ID and return it - // - assert(signalIdDesc[0].id == SBG_ECOM_SIGNAL_ID_UNKNOWN); - return signalIdDesc[0].pName; + static const SbgEComSignalIdDesc signalIdDesc[] = + { + { SBG_ECOM_SIGNAL_ID_UNKNOWN, "unknown" }, + + { SBG_ECOM_SIGNAL_ID_GPS_L1C_DP, "gpsL1C_DP" }, + { SBG_ECOM_SIGNAL_ID_GPS_L1C_D, "gpsL1C_D" }, + { SBG_ECOM_SIGNAL_ID_GPS_L1C_P, "gpsL1C_P" }, + { SBG_ECOM_SIGNAL_ID_GPS_L1_W, "gpsL1_W" }, + { SBG_ECOM_SIGNAL_ID_GPS_L1_CA, "gpsL1_CA" }, + { SBG_ECOM_SIGNAL_ID_GPS_L1P, "gpsL1P" }, + { SBG_ECOM_SIGNAL_ID_GPS_L1_PY, "gpsL1_PY" }, + { SBG_ECOM_SIGNAL_ID_GPS_L1M, "gpsL1M" }, + { SBG_ECOM_SIGNAL_ID_GPS_L2C_ML, "gpsL2C_ML" }, + { SBG_ECOM_SIGNAL_ID_GPS_L2C_L, "gpsL2C_L" }, + { SBG_ECOM_SIGNAL_ID_GPS_L2_SEMICL, "gpsL2_SEMICL" }, + { SBG_ECOM_SIGNAL_ID_GPS_L2_W, "gpsL2_W" }, + { SBG_ECOM_SIGNAL_ID_GPS_L2_CA, "gpsL2_CA" }, + { SBG_ECOM_SIGNAL_ID_GPS_L2C_M, "gpsL2C_M" }, + { SBG_ECOM_SIGNAL_ID_GPS_L2_PY, "gpsL2_PY" }, + { SBG_ECOM_SIGNAL_ID_GPS_L2M, "gpsL2M" }, + { SBG_ECOM_SIGNAL_ID_GPS_L2P, "gpsL2P" }, + { SBG_ECOM_SIGNAL_ID_GPS_L5_IQ, "gpsL5_IQ" }, + { SBG_ECOM_SIGNAL_ID_GPS_L5_I, "gpsL5_I" }, + { SBG_ECOM_SIGNAL_ID_GPS_L5_Q, "gpsL5_Q" }, + + { SBG_ECOM_SIGNAL_ID_GLONASS_G1_P, "glonassG1_P" }, + { SBG_ECOM_SIGNAL_ID_GLONASS_G1_CA, "glonassG1_CA" }, + { SBG_ECOM_SIGNAL_ID_GLONASS_G2_P, "glonassG2_P" }, + { SBG_ECOM_SIGNAL_ID_GLONASS_G2_CA, "glonassG2_CA" }, + { SBG_ECOM_SIGNAL_ID_GLONASS_G3_I, "glonassG3_I" }, + { SBG_ECOM_SIGNAL_ID_GLONASS_G3_Q, "glonassG3_Q" }, + { SBG_ECOM_SIGNAL_ID_GLONASS_G3_IQ, "glonassG3_IQ" }, + + { SBG_ECOM_SIGNAL_ID_GALILEO_E1_BC, "galileoE1_BC" }, + { SBG_ECOM_SIGNAL_ID_GALILEO_E1_C, "galileoE1_C" }, + { SBG_ECOM_SIGNAL_ID_GALILEO_E1_B, "galileoE1_B" }, + { SBG_ECOM_SIGNAL_ID_GALILEO_E1_A, "galileoE1_A" }, + { SBG_ECOM_SIGNAL_ID_GALILEO_E1_ABC, "galileoE1_ABC" }, + { SBG_ECOM_SIGNAL_ID_GALILEO_E5B_IQ, "galileoE5B_IQ" }, + { SBG_ECOM_SIGNAL_ID_GALILEO_E5B_I, "galileoE5B_I" }, + { SBG_ECOM_SIGNAL_ID_GALILEO_E5B_Q, "galileoE5B_Q" }, + { SBG_ECOM_SIGNAL_ID_GALILEO_E5A_IQ, "galileoE5A_IQ" }, + { SBG_ECOM_SIGNAL_ID_GALILEO_E5A_I, "galileoE5A_I" }, + { SBG_ECOM_SIGNAL_ID_GALILEO_E5A_Q, "galileoE5A_Q" }, + { SBG_ECOM_SIGNAL_ID_GALILEO_E5_IQ, "galileoE5_IQ" }, + { SBG_ECOM_SIGNAL_ID_GALILEO_E5_I, "galileoE5_I" }, + { SBG_ECOM_SIGNAL_ID_GALILEO_E5_Q, "galileoE5_Q" }, + { SBG_ECOM_SIGNAL_ID_GALILEO_E6_BC, "galileoE6_BC" }, + { SBG_ECOM_SIGNAL_ID_GALILEO_E6_C, "galileoE6_C" }, + { SBG_ECOM_SIGNAL_ID_GALILEO_E6_B, "galileoE6_B" }, + { SBG_ECOM_SIGNAL_ID_GALILEO_E6_ABC, "galileoE6_ABC" }, + { SBG_ECOM_SIGNAL_ID_GALILEO_E6_A, "galileoE6_A" }, + + { SBG_ECOM_SIGNAL_ID_BEIDOU_B1IQ, "beidouB1IQ" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B1I, "beidouB1I" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B1Q, "beidouB1Q" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B1C_P, "beidouB1C_P" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B1C_DP, "beidouB1C_DP" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B1C_D, "beidouB1C_D" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B1A_P, "beidouB1A_P" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B1A_DP, "beidouB1A_DP" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B1A_D, "beidouB1A_D" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B2IQ, "beidouB2IQ" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B2I, "beidouB2I" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B2A_P, "beidouB2A_P" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B2A_DP, "beidouB2A_DP" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B2A_D, "beidouB2A_D" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B2Q, "beidouB2Q" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B2B_P, "beidouB2B_P" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B2B_DP, "beidouB2B_DP" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B2B_D, "beidouB2B_D" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B2AB_P, "beidouB2AB_P" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B2AB_DP, "beidouB2AB_DP" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B2AB_D, "beidouB2AB_D" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B3IQ, "beidouB3IQ" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B3I, "beidouB3I" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B3Q, "beidouB3Q" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B3A_D, "beidouB3A_D" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B3A_P, "beidouB3A_P" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B3A_DP, "beidouB3A_DP" }, + + { SBG_ECOM_SIGNAL_ID_QZSS_L1C_DP, "qzssL1C_DP" }, + { SBG_ECOM_SIGNAL_ID_QZSS_L1C_D, "qzssL1C_D" }, + { SBG_ECOM_SIGNAL_ID_QZSS_L1C_P, "qzssL1C_P" }, + { SBG_ECOM_SIGNAL_ID_QZSS_L1_CA, "qzssL1_CA" }, + { SBG_ECOM_SIGNAL_ID_QZSS_L1_SAIF, "qzssL1_SAIF" }, + { SBG_ECOM_SIGNAL_ID_QZSS_L1_SB, "qzssL1_SB" }, + { SBG_ECOM_SIGNAL_ID_QZSS_L2C_ML, "qzssL2C_ML" }, + { SBG_ECOM_SIGNAL_ID_QZSS_L2C_L, "qzssL2C_L" }, + { SBG_ECOM_SIGNAL_ID_QZSS_L2C_M, "qzssL2C_M" }, + { SBG_ECOM_SIGNAL_ID_QZSS_L5_IQ, "qzssL5_IQ" }, + { SBG_ECOM_SIGNAL_ID_QZSS_L5_I, "qzssL5_I" }, + { SBG_ECOM_SIGNAL_ID_QZSS_L5_Q, "qzssL5_Q" }, + { SBG_ECOM_SIGNAL_ID_QZSS_L5S_IQ, "qzssL5S_IQ" }, + { SBG_ECOM_SIGNAL_ID_QZSS_L5S_I, "qzssL5S_I" }, + { SBG_ECOM_SIGNAL_ID_QZSS_L5S_Q, "qzssL5S_Q" }, + { SBG_ECOM_SIGNAL_ID_QZSS_L6_P, "qzssL6_P" }, + { SBG_ECOM_SIGNAL_ID_QZSS_L6_DP, "qzssL6_DP" }, + { SBG_ECOM_SIGNAL_ID_QZSS_L6_D, "qzssL6_D" }, + { SBG_ECOM_SIGNAL_ID_QZSS_L6_E, "qzssL6_E" }, + { SBG_ECOM_SIGNAL_ID_QZSS_L6_DE, "qzssL6_DE" }, + + { SBG_ECOM_SIGNAL_ID_SBAS_L1_CA, "sbasL1_CA" }, + { SBG_ECOM_SIGNAL_ID_SBAS_L5_I, "sbasL5_I" }, + { SBG_ECOM_SIGNAL_ID_SBAS_L5_Q, "sbasL5_Q" }, + { SBG_ECOM_SIGNAL_ID_SBAS_L5_IQ, "sbasL5_IQ" }, + + { SBG_ECOM_SIGNAL_ID_IRNSS_L5_A, "irnssL5_A" }, + { SBG_ECOM_SIGNAL_ID_IRNSS_L5_B, "irnssL5_B" }, + { SBG_ECOM_SIGNAL_ID_IRNSS_L5_C, "irnssL5_C" }, + { SBG_ECOM_SIGNAL_ID_IRNSS_L5_BC, "irnssL5_BC" }, + { SBG_ECOM_SIGNAL_ID_IRNSS_S9_A, "irnssS9_A" }, + { SBG_ECOM_SIGNAL_ID_IRNSS_S9_B, "irnssS9_B" }, + { SBG_ECOM_SIGNAL_ID_IRNSS_S9_C, "irnssS9_C" }, + { SBG_ECOM_SIGNAL_ID_IRNSS_S9_BC, "irnssS9_BC" }, + + { SBG_ECOM_SIGNAL_ID_LBAND, "lband" } + }; + + for (size_t i = 0; i < SBG_ARRAY_SIZE(signalIdDesc); i++) + { + if (signalIdDesc[i].id == signalId) + { + return signalIdDesc[i].pName; + } + } + + // + // Enforce that the first item is the unknown signal ID and return it + // + assert(signalIdDesc[0].id == SBG_ECOM_SIGNAL_ID_UNKNOWN); + return signalIdDesc[0].pName; } bool sbgEComConstellationIdIsValid(uint8_t constellationId) { - bool constellationIdIsValid = false; - - switch (constellationId) - { - case SBG_ECOM_CONSTELLATION_ID_UNKNOWN: - case SBG_ECOM_CONSTELLATION_ID_GPS: - case SBG_ECOM_CONSTELLATION_ID_QZSS: - case SBG_ECOM_CONSTELLATION_ID_GLONASS: - case SBG_ECOM_CONSTELLATION_ID_GALILEO: - case SBG_ECOM_CONSTELLATION_ID_BEIDOU: - case SBG_ECOM_CONSTELLATION_ID_SBAS: - case SBG_ECOM_CONSTELLATION_ID_IRNSS: - case SBG_ECOM_CONSTELLATION_ID_LBAND: - constellationIdIsValid = true; - break; - } - - return constellationIdIsValid; + bool constellationIdIsValid = false; + + switch (constellationId) + { + case SBG_ECOM_CONSTELLATION_ID_UNKNOWN: + case SBG_ECOM_CONSTELLATION_ID_GPS: + case SBG_ECOM_CONSTELLATION_ID_QZSS: + case SBG_ECOM_CONSTELLATION_ID_GLONASS: + case SBG_ECOM_CONSTELLATION_ID_GALILEO: + case SBG_ECOM_CONSTELLATION_ID_BEIDOU: + case SBG_ECOM_CONSTELLATION_ID_SBAS: + case SBG_ECOM_CONSTELLATION_ID_IRNSS: + case SBG_ECOM_CONSTELLATION_ID_LBAND: + constellationIdIsValid = true; + break; + } + + return constellationIdIsValid; } const char *sbgEComConstellationToStr(SbgEComConstellationId constellationId) { - static const char *enumToStrLut[] = - { - [SBG_ECOM_CONSTELLATION_ID_UNKNOWN] = "unknown", - [SBG_ECOM_CONSTELLATION_ID_GPS] = "gps", - [SBG_ECOM_CONSTELLATION_ID_GLONASS] = "glonass", - [SBG_ECOM_CONSTELLATION_ID_GALILEO] = "galileo", - [SBG_ECOM_CONSTELLATION_ID_BEIDOU] = "beidou", - [SBG_ECOM_CONSTELLATION_ID_QZSS] = "qzss", - [SBG_ECOM_CONSTELLATION_ID_SBAS] = "sbas", - [SBG_ECOM_CONSTELLATION_ID_IRNSS] = "irnss", - [SBG_ECOM_CONSTELLATION_ID_LBAND] = "lband", - }; - - if (constellationId < SBG_ARRAY_SIZE(enumToStrLut)) - { - return enumToStrLut[constellationId]; - } - else - { - return enumToStrLut[SBG_ECOM_CONSTELLATION_ID_UNKNOWN]; - } + static const char *enumToStrLut[] = + { + [SBG_ECOM_CONSTELLATION_ID_UNKNOWN] = "unknown", + [SBG_ECOM_CONSTELLATION_ID_GPS] = "gps", + [SBG_ECOM_CONSTELLATION_ID_GLONASS] = "glonass", + [SBG_ECOM_CONSTELLATION_ID_GALILEO] = "galileo", + [SBG_ECOM_CONSTELLATION_ID_BEIDOU] = "beidou", + [SBG_ECOM_CONSTELLATION_ID_QZSS] = "qzss", + [SBG_ECOM_CONSTELLATION_ID_SBAS] = "sbas", + [SBG_ECOM_CONSTELLATION_ID_IRNSS] = "irnss", + [SBG_ECOM_CONSTELLATION_ID_LBAND] = "lband", + }; + + if (constellationId < SBG_ARRAY_SIZE(enumToStrLut)) + { + return enumToStrLut[constellationId]; + } + else + { + return enumToStrLut[SBG_ECOM_CONSTELLATION_ID_UNKNOWN]; + } } diff --git a/src/defs/sbgEComDefsGnss.h b/src/defs/sbgEComDefsGnss.h index f06ef71..8ab2241 100644 --- a/src/defs/sbgEComDefsGnss.h +++ b/src/defs/sbgEComDefsGnss.h @@ -1,13 +1,13 @@ /*! - * \file sbgEComDefsGnss.h - * \ingroup main - * \author SBG Systems - * \date 20 September 2022 + * \file sbgEComDefsGnss.h + * \ingroup main + * \author SBG Systems + * \date 20 September 2022 * - * \brief Common enumeration and definitions for RAW GNSS data + * \brief Common enumeration and definitions for RAW GNSS data * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -51,145 +51,145 @@ extern "C" { */ typedef enum _SbgEComSignalId { - SBG_ECOM_SIGNAL_ID_UNKNOWN = 0, + SBG_ECOM_SIGNAL_ID_UNKNOWN = 0, - // - // GPS constellation (10 to 39) - // - SBG_ECOM_SIGNAL_ID_GPS_L1C_DP = 10, - SBG_ECOM_SIGNAL_ID_GPS_L1C_D = 11, - SBG_ECOM_SIGNAL_ID_GPS_L1C_P = 12, - SBG_ECOM_SIGNAL_ID_GPS_L1_W = 13, - SBG_ECOM_SIGNAL_ID_GPS_L1_CA = 14, - SBG_ECOM_SIGNAL_ID_GPS_L1P = 15, - SBG_ECOM_SIGNAL_ID_GPS_L1_PY = 16, - SBG_ECOM_SIGNAL_ID_GPS_L1M = 17, - SBG_ECOM_SIGNAL_ID_GPS_L2C_ML = 18, - SBG_ECOM_SIGNAL_ID_GPS_L2C_L = 19, - SBG_ECOM_SIGNAL_ID_GPS_L2_SEMICL = 20, - SBG_ECOM_SIGNAL_ID_GPS_L2_W = 21, - SBG_ECOM_SIGNAL_ID_GPS_L2_CA = 22, - SBG_ECOM_SIGNAL_ID_GPS_L2C_M = 23, - SBG_ECOM_SIGNAL_ID_GPS_L2_PY = 24, - SBG_ECOM_SIGNAL_ID_GPS_L2M = 25, - SBG_ECOM_SIGNAL_ID_GPS_L2P = 26, - SBG_ECOM_SIGNAL_ID_GPS_L5_IQ = 27, - SBG_ECOM_SIGNAL_ID_GPS_L5_I = 28, - SBG_ECOM_SIGNAL_ID_GPS_L5_Q = 29, + // + // GPS constellation (10 to 39) + // + SBG_ECOM_SIGNAL_ID_GPS_L1C_DP = 10, + SBG_ECOM_SIGNAL_ID_GPS_L1C_D = 11, + SBG_ECOM_SIGNAL_ID_GPS_L1C_P = 12, + SBG_ECOM_SIGNAL_ID_GPS_L1_W = 13, + SBG_ECOM_SIGNAL_ID_GPS_L1_CA = 14, + SBG_ECOM_SIGNAL_ID_GPS_L1P = 15, + SBG_ECOM_SIGNAL_ID_GPS_L1_PY = 16, + SBG_ECOM_SIGNAL_ID_GPS_L1M = 17, + SBG_ECOM_SIGNAL_ID_GPS_L2C_ML = 18, + SBG_ECOM_SIGNAL_ID_GPS_L2C_L = 19, + SBG_ECOM_SIGNAL_ID_GPS_L2_SEMICL = 20, + SBG_ECOM_SIGNAL_ID_GPS_L2_W = 21, + SBG_ECOM_SIGNAL_ID_GPS_L2_CA = 22, + SBG_ECOM_SIGNAL_ID_GPS_L2C_M = 23, + SBG_ECOM_SIGNAL_ID_GPS_L2_PY = 24, + SBG_ECOM_SIGNAL_ID_GPS_L2M = 25, + SBG_ECOM_SIGNAL_ID_GPS_L2P = 26, + SBG_ECOM_SIGNAL_ID_GPS_L5_IQ = 27, + SBG_ECOM_SIGNAL_ID_GPS_L5_I = 28, + SBG_ECOM_SIGNAL_ID_GPS_L5_Q = 29, - // - // GLONASS constellation (40 to 59) - // - SBG_ECOM_SIGNAL_ID_GLONASS_G1_P = 40, - SBG_ECOM_SIGNAL_ID_GLONASS_G1_CA = 41, - SBG_ECOM_SIGNAL_ID_GLONASS_G2_P = 42, - SBG_ECOM_SIGNAL_ID_GLONASS_G2_CA = 43, - SBG_ECOM_SIGNAL_ID_GLONASS_G3_I = 44, - SBG_ECOM_SIGNAL_ID_GLONASS_G3_Q = 45, - SBG_ECOM_SIGNAL_ID_GLONASS_G3_IQ = 46, + // + // GLONASS constellation (40 to 59) + // + SBG_ECOM_SIGNAL_ID_GLONASS_G1_P = 40, + SBG_ECOM_SIGNAL_ID_GLONASS_G1_CA = 41, + SBG_ECOM_SIGNAL_ID_GLONASS_G2_P = 42, + SBG_ECOM_SIGNAL_ID_GLONASS_G2_CA = 43, + SBG_ECOM_SIGNAL_ID_GLONASS_G3_I = 44, + SBG_ECOM_SIGNAL_ID_GLONASS_G3_Q = 45, + SBG_ECOM_SIGNAL_ID_GLONASS_G3_IQ = 46, - // - // Galileo constellation (60 to 99) - // - SBG_ECOM_SIGNAL_ID_GALILEO_E1_BC = 60, - SBG_ECOM_SIGNAL_ID_GALILEO_E1_C = 61, - SBG_ECOM_SIGNAL_ID_GALILEO_E1_B = 62, - SBG_ECOM_SIGNAL_ID_GALILEO_E1_A = 63, - SBG_ECOM_SIGNAL_ID_GALILEO_E1_ABC = 64, - SBG_ECOM_SIGNAL_ID_GALILEO_E5B_IQ = 65, - SBG_ECOM_SIGNAL_ID_GALILEO_E5B_I = 66, - SBG_ECOM_SIGNAL_ID_GALILEO_E5B_Q = 67, - SBG_ECOM_SIGNAL_ID_GALILEO_E5A_IQ = 68, - SBG_ECOM_SIGNAL_ID_GALILEO_E5A_I = 69, - SBG_ECOM_SIGNAL_ID_GALILEO_E5A_Q = 70, - SBG_ECOM_SIGNAL_ID_GALILEO_E5_IQ = 71, - SBG_ECOM_SIGNAL_ID_GALILEO_E5_I = 72, - SBG_ECOM_SIGNAL_ID_GALILEO_E5_Q = 73, - SBG_ECOM_SIGNAL_ID_GALILEO_E6_BC = 74, - SBG_ECOM_SIGNAL_ID_GALILEO_E6_C = 75, - SBG_ECOM_SIGNAL_ID_GALILEO_E6_B = 76, - SBG_ECOM_SIGNAL_ID_GALILEO_E6_ABC = 77, - SBG_ECOM_SIGNAL_ID_GALILEO_E6_A = 78, + // + // Galileo constellation (60 to 99) + // + SBG_ECOM_SIGNAL_ID_GALILEO_E1_BC = 60, + SBG_ECOM_SIGNAL_ID_GALILEO_E1_C = 61, + SBG_ECOM_SIGNAL_ID_GALILEO_E1_B = 62, + SBG_ECOM_SIGNAL_ID_GALILEO_E1_A = 63, + SBG_ECOM_SIGNAL_ID_GALILEO_E1_ABC = 64, + SBG_ECOM_SIGNAL_ID_GALILEO_E5B_IQ = 65, + SBG_ECOM_SIGNAL_ID_GALILEO_E5B_I = 66, + SBG_ECOM_SIGNAL_ID_GALILEO_E5B_Q = 67, + SBG_ECOM_SIGNAL_ID_GALILEO_E5A_IQ = 68, + SBG_ECOM_SIGNAL_ID_GALILEO_E5A_I = 69, + SBG_ECOM_SIGNAL_ID_GALILEO_E5A_Q = 70, + SBG_ECOM_SIGNAL_ID_GALILEO_E5_IQ = 71, + SBG_ECOM_SIGNAL_ID_GALILEO_E5_I = 72, + SBG_ECOM_SIGNAL_ID_GALILEO_E5_Q = 73, + SBG_ECOM_SIGNAL_ID_GALILEO_E6_BC = 74, + SBG_ECOM_SIGNAL_ID_GALILEO_E6_C = 75, + SBG_ECOM_SIGNAL_ID_GALILEO_E6_B = 76, + SBG_ECOM_SIGNAL_ID_GALILEO_E6_ABC = 77, + SBG_ECOM_SIGNAL_ID_GALILEO_E6_A = 78, - // - // BeiDou constellation (100 to 149) - // - SBG_ECOM_SIGNAL_ID_BEIDOU_B1IQ = 100, - SBG_ECOM_SIGNAL_ID_BEIDOU_B1I = 101, - SBG_ECOM_SIGNAL_ID_BEIDOU_B1Q = 102, - SBG_ECOM_SIGNAL_ID_BEIDOU_B1C_P = 103, - SBG_ECOM_SIGNAL_ID_BEIDOU_B1C_DP = 104, - SBG_ECOM_SIGNAL_ID_BEIDOU_B1C_D = 105, - SBG_ECOM_SIGNAL_ID_BEIDOU_B1A_P = 106, - SBG_ECOM_SIGNAL_ID_BEIDOU_B1A_DP = 107, - SBG_ECOM_SIGNAL_ID_BEIDOU_B1A_D = 108, - SBG_ECOM_SIGNAL_ID_BEIDOU_B2IQ = 109, - SBG_ECOM_SIGNAL_ID_BEIDOU_B2I = 110, - SBG_ECOM_SIGNAL_ID_BEIDOU_B2A_P = 111, - SBG_ECOM_SIGNAL_ID_BEIDOU_B2A_DP = 112, - SBG_ECOM_SIGNAL_ID_BEIDOU_B2A_D = 113, - SBG_ECOM_SIGNAL_ID_BEIDOU_B2Q = 114, - SBG_ECOM_SIGNAL_ID_BEIDOU_B2B_P = 115, - SBG_ECOM_SIGNAL_ID_BEIDOU_B2B_DP = 116, - SBG_ECOM_SIGNAL_ID_BEIDOU_B2B_D = 117, - SBG_ECOM_SIGNAL_ID_BEIDOU_B2AB_P = 118, - SBG_ECOM_SIGNAL_ID_BEIDOU_B2AB_DP = 119, - SBG_ECOM_SIGNAL_ID_BEIDOU_B2AB_D = 120, - SBG_ECOM_SIGNAL_ID_BEIDOU_B3IQ = 121, - SBG_ECOM_SIGNAL_ID_BEIDOU_B3I = 122, - SBG_ECOM_SIGNAL_ID_BEIDOU_B3Q = 123, - SBG_ECOM_SIGNAL_ID_BEIDOU_B3A_D = 124, - SBG_ECOM_SIGNAL_ID_BEIDOU_B3A_P = 125, - SBG_ECOM_SIGNAL_ID_BEIDOU_B3A_DP = 126, + // + // BeiDou constellation (100 to 149) + // + SBG_ECOM_SIGNAL_ID_BEIDOU_B1IQ = 100, + SBG_ECOM_SIGNAL_ID_BEIDOU_B1I = 101, + SBG_ECOM_SIGNAL_ID_BEIDOU_B1Q = 102, + SBG_ECOM_SIGNAL_ID_BEIDOU_B1C_P = 103, + SBG_ECOM_SIGNAL_ID_BEIDOU_B1C_DP = 104, + SBG_ECOM_SIGNAL_ID_BEIDOU_B1C_D = 105, + SBG_ECOM_SIGNAL_ID_BEIDOU_B1A_P = 106, + SBG_ECOM_SIGNAL_ID_BEIDOU_B1A_DP = 107, + SBG_ECOM_SIGNAL_ID_BEIDOU_B1A_D = 108, + SBG_ECOM_SIGNAL_ID_BEIDOU_B2IQ = 109, + SBG_ECOM_SIGNAL_ID_BEIDOU_B2I = 110, + SBG_ECOM_SIGNAL_ID_BEIDOU_B2A_P = 111, + SBG_ECOM_SIGNAL_ID_BEIDOU_B2A_DP = 112, + SBG_ECOM_SIGNAL_ID_BEIDOU_B2A_D = 113, + SBG_ECOM_SIGNAL_ID_BEIDOU_B2Q = 114, + SBG_ECOM_SIGNAL_ID_BEIDOU_B2B_P = 115, + SBG_ECOM_SIGNAL_ID_BEIDOU_B2B_DP = 116, + SBG_ECOM_SIGNAL_ID_BEIDOU_B2B_D = 117, + SBG_ECOM_SIGNAL_ID_BEIDOU_B2AB_P = 118, + SBG_ECOM_SIGNAL_ID_BEIDOU_B2AB_DP = 119, + SBG_ECOM_SIGNAL_ID_BEIDOU_B2AB_D = 120, + SBG_ECOM_SIGNAL_ID_BEIDOU_B3IQ = 121, + SBG_ECOM_SIGNAL_ID_BEIDOU_B3I = 122, + SBG_ECOM_SIGNAL_ID_BEIDOU_B3Q = 123, + SBG_ECOM_SIGNAL_ID_BEIDOU_B3A_D = 124, + SBG_ECOM_SIGNAL_ID_BEIDOU_B3A_P = 125, + SBG_ECOM_SIGNAL_ID_BEIDOU_B3A_DP = 126, - // - // QZSS constellation (150 to 179) - // - SBG_ECOM_SIGNAL_ID_QZSS_L1C_DP = 150, - SBG_ECOM_SIGNAL_ID_QZSS_L1C_D = 151, - SBG_ECOM_SIGNAL_ID_QZSS_L1C_P = 152, - SBG_ECOM_SIGNAL_ID_QZSS_L1_CA = 153, - SBG_ECOM_SIGNAL_ID_QZSS_L1_SAIF = 154, - SBG_ECOM_SIGNAL_ID_QZSS_L1_SB = 155, - SBG_ECOM_SIGNAL_ID_QZSS_L2C_ML = 156, - SBG_ECOM_SIGNAL_ID_QZSS_L2C_L = 157, - SBG_ECOM_SIGNAL_ID_QZSS_L2C_M = 158, - SBG_ECOM_SIGNAL_ID_QZSS_L5_IQ = 159, - SBG_ECOM_SIGNAL_ID_QZSS_L5_I = 160, - SBG_ECOM_SIGNAL_ID_QZSS_L5_Q = 161, - SBG_ECOM_SIGNAL_ID_QZSS_L5S_IQ = 162, - SBG_ECOM_SIGNAL_ID_QZSS_L5S_I = 163, - SBG_ECOM_SIGNAL_ID_QZSS_L5S_Q = 164, - SBG_ECOM_SIGNAL_ID_QZSS_L6_P = 165, - SBG_ECOM_SIGNAL_ID_QZSS_L6_DP = 166, - SBG_ECOM_SIGNAL_ID_QZSS_L6_D = 167, - SBG_ECOM_SIGNAL_ID_QZSS_L6_E = 168, - SBG_ECOM_SIGNAL_ID_QZSS_L6_DE = 169, + // + // QZSS constellation (150 to 179) + // + SBG_ECOM_SIGNAL_ID_QZSS_L1C_DP = 150, + SBG_ECOM_SIGNAL_ID_QZSS_L1C_D = 151, + SBG_ECOM_SIGNAL_ID_QZSS_L1C_P = 152, + SBG_ECOM_SIGNAL_ID_QZSS_L1_CA = 153, + SBG_ECOM_SIGNAL_ID_QZSS_L1_SAIF = 154, + SBG_ECOM_SIGNAL_ID_QZSS_L1_SB = 155, + SBG_ECOM_SIGNAL_ID_QZSS_L2C_ML = 156, + SBG_ECOM_SIGNAL_ID_QZSS_L2C_L = 157, + SBG_ECOM_SIGNAL_ID_QZSS_L2C_M = 158, + SBG_ECOM_SIGNAL_ID_QZSS_L5_IQ = 159, + SBG_ECOM_SIGNAL_ID_QZSS_L5_I = 160, + SBG_ECOM_SIGNAL_ID_QZSS_L5_Q = 161, + SBG_ECOM_SIGNAL_ID_QZSS_L5S_IQ = 162, + SBG_ECOM_SIGNAL_ID_QZSS_L5S_I = 163, + SBG_ECOM_SIGNAL_ID_QZSS_L5S_Q = 164, + SBG_ECOM_SIGNAL_ID_QZSS_L6_P = 165, + SBG_ECOM_SIGNAL_ID_QZSS_L6_DP = 166, + SBG_ECOM_SIGNAL_ID_QZSS_L6_D = 167, + SBG_ECOM_SIGNAL_ID_QZSS_L6_E = 168, + SBG_ECOM_SIGNAL_ID_QZSS_L6_DE = 169, - // - // SBAS system (180 to 199) - // - SBG_ECOM_SIGNAL_ID_SBAS_L1_CA = 180, - SBG_ECOM_SIGNAL_ID_SBAS_L5_I = 181, - SBG_ECOM_SIGNAL_ID_SBAS_L5_Q = 182, - SBG_ECOM_SIGNAL_ID_SBAS_L5_IQ = 183, + // + // SBAS system (180 to 199) + // + SBG_ECOM_SIGNAL_ID_SBAS_L1_CA = 180, + SBG_ECOM_SIGNAL_ID_SBAS_L5_I = 181, + SBG_ECOM_SIGNAL_ID_SBAS_L5_Q = 182, + SBG_ECOM_SIGNAL_ID_SBAS_L5_IQ = 183, - // - // IRNSS / NAVIC constellation (200 to 219) - // - SBG_ECOM_SIGNAL_ID_IRNSS_L5_A = 200, - SBG_ECOM_SIGNAL_ID_IRNSS_L5_B = 201, - SBG_ECOM_SIGNAL_ID_IRNSS_L5_C = 202, - SBG_ECOM_SIGNAL_ID_IRNSS_L5_BC = 203, - SBG_ECOM_SIGNAL_ID_IRNSS_S9_A = 204, - SBG_ECOM_SIGNAL_ID_IRNSS_S9_B = 205, - SBG_ECOM_SIGNAL_ID_IRNSS_S9_C = 206, - SBG_ECOM_SIGNAL_ID_IRNSS_S9_BC = 207, + // + // IRNSS / NAVIC constellation (200 to 219) + // + SBG_ECOM_SIGNAL_ID_IRNSS_L5_A = 200, + SBG_ECOM_SIGNAL_ID_IRNSS_L5_B = 201, + SBG_ECOM_SIGNAL_ID_IRNSS_L5_C = 202, + SBG_ECOM_SIGNAL_ID_IRNSS_L5_BC = 203, + SBG_ECOM_SIGNAL_ID_IRNSS_S9_A = 204, + SBG_ECOM_SIGNAL_ID_IRNSS_S9_B = 205, + SBG_ECOM_SIGNAL_ID_IRNSS_S9_C = 206, + SBG_ECOM_SIGNAL_ID_IRNSS_S9_BC = 207, - // - // L-Band system (220 to 230) - // - SBG_ECOM_SIGNAL_ID_LBAND = 220, + // + // L-Band system (220 to 230) + // + SBG_ECOM_SIGNAL_ID_LBAND = 220, } SbgEComSignalId; /*! @@ -201,15 +201,15 @@ typedef enum _SbgEComSignalId */ typedef enum _SbgEComConstellationId { - SBG_ECOM_CONSTELLATION_ID_UNKNOWN = 0, - SBG_ECOM_CONSTELLATION_ID_GPS = 1, - SBG_ECOM_CONSTELLATION_ID_GLONASS = 2, - SBG_ECOM_CONSTELLATION_ID_GALILEO = 3, - SBG_ECOM_CONSTELLATION_ID_BEIDOU = 4, - SBG_ECOM_CONSTELLATION_ID_QZSS = 5, - SBG_ECOM_CONSTELLATION_ID_SBAS = 6, - SBG_ECOM_CONSTELLATION_ID_IRNSS = 7, - SBG_ECOM_CONSTELLATION_ID_LBAND = 8, + SBG_ECOM_CONSTELLATION_ID_UNKNOWN = 0, + SBG_ECOM_CONSTELLATION_ID_GPS = 1, + SBG_ECOM_CONSTELLATION_ID_GLONASS = 2, + SBG_ECOM_CONSTELLATION_ID_GALILEO = 3, + SBG_ECOM_CONSTELLATION_ID_BEIDOU = 4, + SBG_ECOM_CONSTELLATION_ID_QZSS = 5, + SBG_ECOM_CONSTELLATION_ID_SBAS = 6, + SBG_ECOM_CONSTELLATION_ID_IRNSS = 7, + SBG_ECOM_CONSTELLATION_ID_LBAND = 8, } SbgEComConstellationId; //----------------------------------------------------------------------// @@ -219,42 +219,42 @@ typedef enum _SbgEComConstellationId /*! * Returns a constellation given a signal ID * - * \param[in] signalId Signal ID value. - * \return Constellation this signal belongs to. + * \param[in] signalId Signal ID value. + * \return Constellation this signal belongs to. */ SbgEComConstellationId sbgEComGetConstellationFromSignalId(SbgEComSignalId signalId); /*! * Check if a value belongs to SbgEComSignalId enum. * - * WARNING: SBG_ECOM_SIGNAL_ID_UNKNOWN is considered to be a valid enum value. + * \note: SBG_ECOM_SIGNAL_ID_UNKNOWN is considered to be a valid enum value. * - * \param[in] signalId Signal ID value. - * \return true if the value is valid + * \param[in] signalId Signal ID value. + * \return true if the value is valid */ bool sbgEComSignalIdIsValid(uint8_t signalId); /*! * Get a signal ID as a read only C string. * - * \param[in] signalId Signal ID value. - * \return Signal ID as a read only C string. + * \param[in] signalId Signal ID value. + * \return Signal ID as a read only C string. */ const char *sbgEComSignalToStr(SbgEComSignalId signalId); /*! * Check if a value belongs to SbgEComConstellationId enum. * - * \param[in] constellationId constellation ID value. - * \return true if the value is valid + * \param[in] constellationId constellation ID value. + * \return true if the value is valid */ bool sbgEComConstellationIdIsValid(uint8_t constellationId); /*! * Get a constellation ID as a read only C string. * - * \param[in] constellationId Constellation ID value. - * \return Constellation ID as a read only C string. + * \param[in] constellationId Constellation ID value. + * \return Constellation ID as a read only C string. */ const char *sbgEComConstellationToStr(SbgEComConstellationId constellationId); diff --git a/src/logs/sbgEComLog.c b/src/logs/sbgEComLog.c index 1cdab6b..7bbba00 100644 --- a/src/logs/sbgEComLog.c +++ b/src/logs/sbgEComLog.c @@ -19,9 +19,11 @@ #include "sbgEComLogGnssVel.h" #include "sbgEComLogImu.h" #include "sbgEComLogMag.h" +#include "sbgEComLogMagCalib.h" #include "sbgEComLogOdometer.h" #include "sbgEComLogRawData.h" #include "sbgEComLogSat.h" +#include "sbgEComLogSessionInfo.h" #include "sbgEComLogShipMotion.h" #include "sbgEComLogStatus.h" #include "sbgEComLogUsbl.h" @@ -33,160 +35,166 @@ SbgErrorCode sbgEComLogParse(SbgEComClass msgClass, SbgEComMsgId msgId, const void *pPayload, size_t payloadSize, SbgEComLogUnion *pLogData) { - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgStreamBuffer inputStream; + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgStreamBuffer inputStream; - assert(pPayload); - assert(payloadSize > 0); - assert(pLogData); + assert(pPayload); + assert(payloadSize > 0); + assert(pLogData); - // - // Create an input stream buffer that points to the frame payload so we can easily parse it's content - // - sbgStreamBufferInitForRead(&inputStream, pPayload, payloadSize); + // + // Create an input stream buffer that points to the frame payload so we can easily parse it's content + // + sbgStreamBufferInitForRead(&inputStream, pPayload, payloadSize); - // - // Handle the different classes of messages differently - // - if (msgClass == SBG_ECOM_CLASS_LOG_ECOM_0) - { - // - // Parse the incoming log according to its type - // - switch (msgId) - { - case SBG_ECOM_LOG_STATUS: - errorCode = sbgEComLogStatusReadFromStream(&pLogData->statusData, &inputStream); - break; - case SBG_ECOM_LOG_IMU_DATA: - errorCode = sbgEComLogImuLegacyReadFromStream(&pLogData->imuData, &inputStream); - break; - case SBG_ECOM_LOG_IMU_SHORT: - errorCode = sbgEComLogImuShortReadFromStream(&pLogData->imuShort, &inputStream); - break; - case SBG_ECOM_LOG_EKF_EULER: - errorCode = sbgEComLogEkfEulerReadFromStream(&pLogData->ekfEulerData, &inputStream); - break; - case SBG_ECOM_LOG_EKF_QUAT: - errorCode = sbgEComLogEkfQuatReadFromStream(&pLogData->ekfQuatData, &inputStream); - break; - case SBG_ECOM_LOG_EKF_NAV: - errorCode = sbgEComLogEkfNavReadFromStream(&pLogData->ekfNavData, &inputStream); - break; - case SBG_ECOM_LOG_EKF_VEL_BODY: - errorCode = sbgEComLogEkfVelBodyReadFromStream(&pLogData->ekfVelBody, &inputStream); - break; - case SBG_ECOM_LOG_EKF_ROT_ACCEL_BODY: - case SBG_ECOM_LOG_EKF_ROT_ACCEL_NED: - errorCode = sbgEComLogEkfRotAccelReadFromStream(&pLogData->ekfRotAccel, &inputStream); - break; - case SBG_ECOM_LOG_SHIP_MOTION: - case SBG_ECOM_LOG_SHIP_MOTION_HP: - errorCode = sbgEComLogShipMotionReadFromStream(&pLogData->shipMotionData, &inputStream); - break; - case SBG_ECOM_LOG_ODO_VEL: - errorCode = sbgEComLogOdometerReadFromStream(&pLogData->odometerData, &inputStream); - break; - case SBG_ECOM_LOG_UTC_TIME: - errorCode = sbgEComLogUtcReadFromStream(&pLogData->utcData, &inputStream); - break; - case SBG_ECOM_LOG_GPS1_VEL: - case SBG_ECOM_LOG_GPS2_VEL: - errorCode = sbgEComLogGnssVelReadFromStream(&pLogData->gpsVelData, &inputStream); - break; - case SBG_ECOM_LOG_GPS1_POS: - case SBG_ECOM_LOG_GPS2_POS: - errorCode = sbgEComLogGnssPosReadFromStream(&pLogData->gpsPosData, &inputStream); - break; - case SBG_ECOM_LOG_GPS1_HDT: - case SBG_ECOM_LOG_GPS2_HDT: - errorCode = sbgEComLogGnssHdtReadFromStream(&pLogData->gpsHdtData, &inputStream); - break; - case SBG_ECOM_LOG_GPS1_RAW: - case SBG_ECOM_LOG_GPS2_RAW: - errorCode = sbgEComLogRawDataReadFromStream(&pLogData->gpsRawData, &inputStream); - break; - case SBG_ECOM_LOG_GPS1_SAT: - case SBG_ECOM_LOG_GPS2_SAT: - errorCode = sbgEComLogSatListReadFromStream(&pLogData->satGroupData, &inputStream); - break; - case SBG_ECOM_LOG_RTCM_RAW: - errorCode = sbgEComLogRawDataReadFromStream(&pLogData->rtcmRawData, &inputStream); - break; - case SBG_ECOM_LOG_MAG: - errorCode = sbgEComLogMagReadFromStream(&pLogData->magData, &inputStream); - break; - case SBG_ECOM_LOG_MAG_CALIB: - errorCode = sbgEComLogMagCalibReadFromStream(&pLogData->magCalibData, &inputStream); - break; - case SBG_ECOM_LOG_DVL_BOTTOM_TRACK: - errorCode = sbgEComLogDvlReadFromStream(&pLogData->dvlData, &inputStream); - break; - case SBG_ECOM_LOG_DVL_WATER_TRACK: - errorCode = sbgEComLogDvlReadFromStream(&pLogData->dvlData, &inputStream); - break; - case SBG_ECOM_LOG_AIR_DATA: - errorCode = sbgEComLogAirDataReadFromStream(&pLogData->airData, &inputStream); - break; - case SBG_ECOM_LOG_USBL: - errorCode = sbgEComLogUsblReadFromStream(&pLogData->usblData, &inputStream); - break; - case SBG_ECOM_LOG_DEPTH: - errorCode = sbgEComLogDepthReadFromStream(&pLogData->depthData, &inputStream); - 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 = sbgEComLogEventReadFromStream(&pLogData->eventMarker, &inputStream); - break; - case SBG_ECOM_LOG_DIAG: - errorCode = sbgEComLogDiagReadFromStream(&pLogData->diagData, &inputStream); - break; + // + // Handle the different classes of messages differently + // + if (msgClass == SBG_ECOM_CLASS_LOG_ECOM_0) + { + // + // Parse the incoming log according to its type + // + switch (msgId) + { + case SBG_ECOM_LOG_STATUS: + errorCode = sbgEComLogStatusReadFromStream(&pLogData->statusData, &inputStream); + break; + case SBG_ECOM_LOG_IMU_DATA: + errorCode = sbgEComLogImuLegacyReadFromStream(&pLogData->imuData, &inputStream); + break; + case SBG_ECOM_LOG_IMU_SHORT: + errorCode = sbgEComLogImuShortReadFromStream(&pLogData->imuShort, &inputStream); + break; + case SBG_ECOM_LOG_EKF_EULER: + errorCode = sbgEComLogEkfEulerReadFromStream(&pLogData->ekfEulerData, &inputStream); + break; + case SBG_ECOM_LOG_EKF_QUAT: + errorCode = sbgEComLogEkfQuatReadFromStream(&pLogData->ekfQuatData, &inputStream); + break; + case SBG_ECOM_LOG_EKF_NAV: + errorCode = sbgEComLogEkfNavReadFromStream(&pLogData->ekfNavData, &inputStream); + break; + case SBG_ECOM_LOG_EKF_VEL_BODY: + errorCode = sbgEComLogEkfVelBodyReadFromStream(&pLogData->ekfVelBody, &inputStream); + break; + case SBG_ECOM_LOG_EKF_ROT_ACCEL_BODY: + case SBG_ECOM_LOG_EKF_ROT_ACCEL_NED: + errorCode = sbgEComLogEkfRotAccelReadFromStream(&pLogData->ekfRotAccel, &inputStream); + break; + case SBG_ECOM_LOG_SHIP_MOTION: + case SBG_ECOM_LOG_SHIP_MOTION_HP: + errorCode = sbgEComLogShipMotionReadFromStream(&pLogData->shipMotionData, &inputStream); + break; + case SBG_ECOM_LOG_ODO_VEL: + errorCode = sbgEComLogOdometerReadFromStream(&pLogData->odometerData, &inputStream); + break; + case SBG_ECOM_LOG_UTC_TIME: + errorCode = sbgEComLogUtcReadFromStream(&pLogData->utcData, &inputStream); + break; + case SBG_ECOM_LOG_PTP_STATUS: + errorCode = sbgEComLogPtpReadFromStream(&pLogData->ptpData, &inputStream); + break; + case SBG_ECOM_LOG_GPS1_VEL: + case SBG_ECOM_LOG_GPS2_VEL: + errorCode = sbgEComLogGnssVelReadFromStream(&pLogData->gpsVelData, &inputStream); + break; + case SBG_ECOM_LOG_GPS1_POS: + case SBG_ECOM_LOG_GPS2_POS: + errorCode = sbgEComLogGnssPosReadFromStream(&pLogData->gpsPosData, &inputStream); + break; + case SBG_ECOM_LOG_GPS1_HDT: + case SBG_ECOM_LOG_GPS2_HDT: + errorCode = sbgEComLogGnssHdtReadFromStream(&pLogData->gpsHdtData, &inputStream); + break; + case SBG_ECOM_LOG_GPS1_RAW: + case SBG_ECOM_LOG_GPS2_RAW: + errorCode = sbgEComLogRawDataReadFromStream(&pLogData->gpsRawData, &inputStream); + break; + case SBG_ECOM_LOG_GPS1_SAT: + case SBG_ECOM_LOG_GPS2_SAT: + errorCode = sbgEComLogSatListReadFromStream(&pLogData->satGroupData, &inputStream); + break; + case SBG_ECOM_LOG_RTCM_RAW: + errorCode = sbgEComLogRawDataReadFromStream(&pLogData->rtcmRawData, &inputStream); + break; + case SBG_ECOM_LOG_MAG: + errorCode = sbgEComLogMagReadFromStream(&pLogData->magData, &inputStream); + break; + case SBG_ECOM_LOG_MAG_CALIB: + errorCode = sbgEComLogMagCalibReadFromStream(&pLogData->magCalibData, &inputStream); + break; + case SBG_ECOM_LOG_DVL_BOTTOM_TRACK: + errorCode = sbgEComLogDvlReadFromStream(&pLogData->dvlData, &inputStream); + break; + case SBG_ECOM_LOG_DVL_WATER_TRACK: + errorCode = sbgEComLogDvlReadFromStream(&pLogData->dvlData, &inputStream); + break; + case SBG_ECOM_LOG_AIR_DATA: + errorCode = sbgEComLogAirDataReadFromStream(&pLogData->airData, &inputStream); + break; + case SBG_ECOM_LOG_USBL: + errorCode = sbgEComLogUsblReadFromStream(&pLogData->usblData, &inputStream); + break; + case SBG_ECOM_LOG_DEPTH: + errorCode = sbgEComLogDepthReadFromStream(&pLogData->depthData, &inputStream); + 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 = sbgEComLogEventReadFromStream(&pLogData->eventMarker, &inputStream); + break; + case SBG_ECOM_LOG_DIAG: + errorCode = sbgEComLogDiagReadFromStream(&pLogData->diagData, &inputStream); + break; + case SBG_ECOM_LOG_SESSION_INFO: + errorCode = sbgEComLogSessionInfoReadFromStream(&pLogData->sessionInfoData, &inputStream); + break; - default: - errorCode = SBG_ERROR; - } - } - else if (msgClass == SBG_ECOM_CLASS_LOG_ECOM_1) - { - // - // Parse the message depending on the message ID - // - switch ((SbgEComLog1)msgId) - { - case SBG_ECOM_LOG_FAST_IMU_DATA: - errorCode = sbgEComLogImuFastLegacyReadFromStream(&pLogData->fastImuData, &inputStream); - break; - default: - errorCode = SBG_ERROR; - } - } - else - { - // - // Unhandled message class - // - errorCode = SBG_ERROR; - } + default: + errorCode = SBG_ERROR; + } + } + else if (msgClass == SBG_ECOM_CLASS_LOG_ECOM_1) + { + // + // Parse the message depending on the message ID + // + switch ((SbgEComLog1)msgId) + { + case SBG_ECOM_LOG_FAST_IMU_DATA: + errorCode = sbgEComLogImuFastLegacyReadFromStream(&pLogData->fastImuData, &inputStream); + break; + default: + errorCode = SBG_ERROR; + } + } + else + { + // + // Unhandled message class + // + errorCode = SBG_ERROR; + } - return errorCode; + return errorCode; } void sbgEComLogCleanup(SbgEComLogUnion *pLogData, SbgEComClass msgClass, SbgEComMsgId msgId) { - assert(pLogData); + assert(pLogData); - SBG_UNUSED_PARAMETER(pLogData); - SBG_UNUSED_PARAMETER(msgClass); - SBG_UNUSED_PARAMETER(msgId); + SBG_UNUSED_PARAMETER(pLogData); + SBG_UNUSED_PARAMETER(msgClass); + SBG_UNUSED_PARAMETER(msgId); - // - // Nothing to do for now - // + // + // Nothing to do for now + // } //----------------------------------------------------------------------// @@ -195,5 +203,5 @@ void sbgEComLogCleanup(SbgEComLogUnion *pLogData, SbgEComClass msgClass, SbgECom SbgErrorCode sbgEComBinaryLogParse(SbgEComClass msgClass, SbgEComMsgId msg, const void *pPayload, size_t payloadSize, SbgEComLogUnion *pLogData) { - return sbgEComLogParse(msgClass, msg, pPayload, payloadSize, pLogData); + return sbgEComLogParse(msgClass, msg, pPayload, payloadSize, pLogData); } diff --git a/src/logs/sbgEComLog.h b/src/logs/sbgEComLog.h index 331237f..dc85815 100644 --- a/src/logs/sbgEComLog.h +++ b/src/logs/sbgEComLog.h @@ -1,13 +1,13 @@ /*! - * \file sbgEComLog.h - * \ingroup binaryLogs - * \author SBG Systems - * \date 06 February 2013 + * \file sbgEComLog.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 06 February 2013 * - * \brief Parse incoming sbgECom logs and store result in an union. + * \brief Parse incoming sbgECom logs and store result in an union. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -31,8 +31,8 @@ */ /*! - * \defgroup binaryLogs Binary Logs - * \brief All messages and logs that can be output by the device. + * \defgroup binaryLogs Binary Logs + * \brief All messages and logs that can be output by the device. */ #ifndef SBG_ECOM_LOG_H @@ -57,9 +57,12 @@ #include "sbgEComLogGnssVel.h" #include "sbgEComLogImu.h" #include "sbgEComLogMag.h" +#include "sbgEComLogMagCalib.h" #include "sbgEComLogOdometer.h" +#include "sbgEComLogPtp.h" #include "sbgEComLogRawData.h" #include "sbgEComLogSat.h" +#include "sbgEComLogSessionInfo.h" #include "sbgEComLogShipMotion.h" #include "sbgEComLogStatus.h" #include "sbgEComLogUsbl.h" @@ -74,38 +77,40 @@ extern "C" { //----------------------------------------------------------------------// /*! - * Union used to store received logs data. + * Union used to store received logs data. */ typedef union _SbgEComLogUnion { - SbgEComLogStatus statusData; /*!< Stores data for the SBG_ECOM_LOG_STATUS message. */ - SbgEComLogImuLegacy imuData; /*!< Stores data for the SBG_ECOM_LOG_IMU_DATA message. */ - SbgEComLogImuShort imuShort; /*!< Stores data for the SBG_ECOM_LOG_IMU_SHORT message. */ - SbgEComLogEkfEuler ekfEulerData; /*!< Stores data for the SBG_ECOM_LOG_EKF_EULER message. */ - SbgEComLogEkfQuat ekfQuatData; /*!< Stores data for the SBG_ECOM_LOG_EKF_QUAT message. */ - SbgEComLogEkfNav ekfNavData; /*!< Stores data for the SBG_ECOM_LOG_EKF_NAV message. */ - SbgEComLogEkfVelBody ekfVelBody; /*!< Stores data for the SBG_ECOM_LOG_EKF_VEL_BODY message. */ - SbgEComLogEkfRotAccel ekfRotAccel; /*!< Stores either SBG_ECOM_LOG_EKF_ROT_ACCEL_BODY or SBG_ECOM_LOG_EKF_ROT_ACCEL_NED. */ - SbgEComLogShipMotion shipMotionData; /*!< Stores data for the SBG_ECOM_LOG_SHIP_MOTION or SBG_ECOM_LOG_SHIP_MOTION_HP message. */ - SbgEComLogOdometer odometerData; /*!< Stores data for the SBG_ECOM_LOG_ODO_VEL message. */ - SbgEComLogUtc utcData; /*!< Stores data for the SBG_ECOM_LOG_UTC_TIME message. */ - SbgEComLogGnssPos gpsPosData; /*!< Stores data for the SBG_ECOM_LOG_GPS_POS message. */ - SbgEComLogGnssVel gpsVelData; /*!< Stores data for the SBG_ECOM_LOG_GPS#_VEL message. */ - SbgEComLogGnssHdt gpsHdtData; /*!< Stores data for the SBG_ECOM_LOG_GPS#_HDT message. */ - SbgEComLogRawData gpsRawData; /*!< Stores data for the SBG_ECOM_LOG_GPS#_RAW message. */ - SbgEComLogRawData rtcmRawData; /*!< Stores data for the SBG_ECOM_LOG_RTCM_RAW message. */ - SbgEComLogMag magData; /*!< Stores data for the SBG_ECOM_LOG_MAG message. */ - SbgEComLogMagCalib magCalibData; /*!< Stores data for the SBG_ECOM_LOG_MAG_CALIB message. */ - SbgEComLogDvl dvlData; /*!< Stores data for the SBG_ECOM_LOG_DVL_BOTTOM_TRACK message. */ - SbgEComLogAirData airData; /*!< Stores data for the SBG_ECOM_LOG_AIR_DATA message. */ - SbgEComLogUsbl usblData; /*!< Stores data for the SBG_ECOM_LOG_USBL message. */ - SbgEComLogDepth depthData; /*!< Stores data for the SBG_ECOM_LOG_DEPTH message */ - SbgEComLogEvent eventMarker; /*!< Stores data for the SBG_ECOM_LOG_EVENT_# message. */ - SbgEComLogDiagData diagData; /*!< Stores data for the SBG_ECOM_LOG_DIAG message. */ - SbgEComLogSatList satGroupData; /*!< Stores data for the SBG_ECOM_LOG_SAT message. */ - - /* Fast logs */ - SbgEComLogImuFastLegacy fastImuData; /*!< Stores Fast IMU Data for 1KHz output */ + SbgEComLogStatus statusData; /*!< Stores data for the SBG_ECOM_LOG_STATUS message. */ + SbgEComLogImuLegacy imuData; /*!< Stores data for the SBG_ECOM_LOG_IMU_DATA message. */ + SbgEComLogImuShort imuShort; /*!< Stores data for the SBG_ECOM_LOG_IMU_SHORT message. */ + SbgEComLogEkfEuler ekfEulerData; /*!< Stores data for the SBG_ECOM_LOG_EKF_EULER message. */ + SbgEComLogEkfQuat ekfQuatData; /*!< Stores data for the SBG_ECOM_LOG_EKF_QUAT message. */ + SbgEComLogEkfNav ekfNavData; /*!< Stores data for the SBG_ECOM_LOG_EKF_NAV message. */ + SbgEComLogEkfVelBody ekfVelBody; /*!< Stores data for the SBG_ECOM_LOG_EKF_VEL_BODY message. */ + SbgEComLogEkfRotAccel ekfRotAccel; /*!< Stores either SBG_ECOM_LOG_EKF_ROT_ACCEL_BODY or SBG_ECOM_LOG_EKF_ROT_ACCEL_NED. */ + SbgEComLogShipMotion shipMotionData; /*!< Stores data for the SBG_ECOM_LOG_SHIP_MOTION or SBG_ECOM_LOG_SHIP_MOTION_HP message. */ + SbgEComLogOdometer odometerData; /*!< Stores data for the SBG_ECOM_LOG_ODO_VEL message. */ + SbgEComLogPtp ptpData; /*!< Stores data for the SBG_ECOM_LOG_PTP_STATUS message. */ + SbgEComLogUtc utcData; /*!< Stores data for the SBG_ECOM_LOG_UTC_TIME message. */ + SbgEComLogGnssPos gpsPosData; /*!< Stores data for the SBG_ECOM_LOG_GPS_POS message. */ + SbgEComLogGnssVel gpsVelData; /*!< Stores data for the SBG_ECOM_LOG_GPS#_VEL message. */ + SbgEComLogGnssHdt gpsHdtData; /*!< Stores data for the SBG_ECOM_LOG_GPS#_HDT message. */ + SbgEComLogRawData gpsRawData; /*!< Stores data for the SBG_ECOM_LOG_GPS#_RAW message. */ + SbgEComLogRawData rtcmRawData; /*!< Stores data for the SBG_ECOM_LOG_RTCM_RAW message. */ + SbgEComLogMag magData; /*!< Stores data for the SBG_ECOM_LOG_MAG message. */ + SbgEComLogMagCalib magCalibData; /*!< Stores data for the SBG_ECOM_LOG_MAG_CALIB message. */ + SbgEComLogDvl dvlData; /*!< Stores data for the SBG_ECOM_LOG_DVL_BOTTOM_TRACK message. */ + SbgEComLogAirData airData; /*!< Stores data for the SBG_ECOM_LOG_AIR_DATA message. */ + SbgEComLogUsbl usblData; /*!< Stores data for the SBG_ECOM_LOG_USBL message. */ + SbgEComLogDepth depthData; /*!< Stores data for the SBG_ECOM_LOG_DEPTH message */ + SbgEComLogEvent eventMarker; /*!< Stores data for the SBG_ECOM_LOG_EVENT_# message. */ + SbgEComLogDiagData diagData; /*!< Stores data for the SBG_ECOM_LOG_DIAG message. */ + SbgEComLogSatList satGroupData; /*!< Stores data for the SBG_ECOM_LOG_SAT message. */ + SbgEComLogSessionInfo sessionInfoData; /*!< Stores data for the SBG_ECOM_LOG_SESSION_INFO message. */ + + /* Fast logs */ + SbgEComLogImuFastLegacy fastImuData; /*!< Stores Fast IMU Data for 1KHz output */ } SbgEComLogUnion; @@ -116,20 +121,20 @@ typedef union _SbgEComLogUnion /*! * Parse an incoming log and fill the output union. * - * \param[in] msgClass Received message class - * \param[in] msgId Received message ID - * \param[in] pPayload Read only pointer on the payload buffer. - * \param[in] payloadSize Payload size in bytes. - * \param[out] pLogData Pointer on the output union that stores parsed data. + * \param[in] msgClass Received message class + * \param[in] msgId Received message ID + * \param[in] pPayload Read only pointer on the payload buffer. + * \param[in] payloadSize Payload size in bytes. + * \param[out] pLogData Pointer on the output union that stores parsed data. */ SbgErrorCode sbgEComLogParse(SbgEComClass msgClass, SbgEComMsgId msgId, const void *pPayload, size_t payloadSize, SbgEComLogUnion *pLogData); /*! * Clean up resources allocated during parsing, if any. * - * \param[in] pLogData Log data. - * \param[in] msgClass Message class. - * \param[in] msgId Message ID. + * \param[in] pLogData Log data. + * \param[in] msgClass Message class. + * \param[in] msgId Message ID. */ void sbgEComLogCleanup(SbgEComLogUnion *pLogData, SbgEComClass msgClass, SbgEComMsgId msgId); diff --git a/src/logs/sbgEComLogAirData.c b/src/logs/sbgEComLogAirData.c index ba0d1aa..1d64ec5 100644 --- a/src/logs/sbgEComLogAirData.c +++ b/src/logs/sbgEComLogAirData.c @@ -6,51 +6,51 @@ SbgErrorCode sbgEComLogAirDataReadFromStream(SbgEComLogAirData *pLogData, SbgStreamBuffer *pStreamBuffer) { - assert(pLogData); - assert(pStreamBuffer); + assert(pLogData); + assert(pStreamBuffer); - pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); - pLogData->status = sbgStreamBufferReadUint16LE(pStreamBuffer); + pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); + pLogData->status = sbgStreamBufferReadUint16LE(pStreamBuffer); - pLogData->pressureAbs = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->altitude = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->pressureAbs = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->altitude = sbgStreamBufferReadFloatLE(pStreamBuffer); - // - // The true airspeed fields have been added in version 2.0 - // - if (sbgStreamBufferGetSpace(pStreamBuffer) > 0) - { - pLogData->pressureDiff = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->trueAirspeed = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->airTemperature = sbgStreamBufferReadFloatLE(pStreamBuffer); - } - else - { - pLogData->pressureDiff = 0.0f; - pLogData->trueAirspeed = 0.0f; - pLogData->airTemperature = 0.0f; - } + // + // The true airspeed fields have been added in version 2.0 + // + if (sbgStreamBufferGetSpace(pStreamBuffer) > 0) + { + pLogData->pressureDiff = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->trueAirspeed = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->airTemperature = sbgStreamBufferReadFloatLE(pStreamBuffer); + } + else + { + pLogData->pressureDiff = 0.0f; + pLogData->trueAirspeed = 0.0f; + pLogData->airTemperature = 0.0f; + } - return sbgStreamBufferGetLastError(pStreamBuffer); + return sbgStreamBufferGetLastError(pStreamBuffer); } SbgErrorCode sbgEComLogAirDataWriteToStream(const SbgEComLogAirData *pLogData, SbgStreamBuffer *pStreamBuffer) { - assert(pLogData); - assert(pStreamBuffer); + assert(pLogData); + assert(pStreamBuffer); - sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); - sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->status); + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); + sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->status); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->pressureAbs); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->altitude); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->pressureAbs); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->altitude); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->pressureDiff); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->trueAirspeed); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->pressureDiff); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->trueAirspeed); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->airTemperature); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->airTemperature); - return sbgStreamBufferGetLastError(pStreamBuffer); + return sbgStreamBufferGetLastError(pStreamBuffer); } //----------------------------------------------------------------------// @@ -59,10 +59,10 @@ SbgErrorCode sbgEComLogAirDataWriteToStream(const SbgEComLogAirData *pLogData, S SbgErrorCode sbgEComBinaryLogParseAirData(SbgStreamBuffer *pStreamBuffer, SbgEComLogAirData *pLogData) { - return sbgEComLogAirDataReadFromStream(pLogData, pStreamBuffer); + return sbgEComLogAirDataReadFromStream(pLogData, pStreamBuffer); } SbgErrorCode sbgEComBinaryLogWriteAirData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogAirData *pLogData) { - return sbgEComLogAirDataWriteToStream(pLogData, pStreamBuffer); + return sbgEComLogAirDataWriteToStream(pLogData, pStreamBuffer); } diff --git a/src/logs/sbgEComLogAirData.h b/src/logs/sbgEComLogAirData.h index 2a27901..b5a5147 100644 --- a/src/logs/sbgEComLogAirData.h +++ b/src/logs/sbgEComLogAirData.h @@ -1,16 +1,16 @@ /*! - * \file sbgEComLogAirData.h - * \ingroup binaryLogs - * \author SBG Systems - * \date 20 February 2019 + * \file sbgEComLogAirData.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 20 February 2019 * - * \brief Parse received air data measurement logs such as barometer data. + * \brief Parse received air data measurement logs such as barometer data. * * Air Data logs are used to inject / return barometric altitude * as well as true air speed. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -51,12 +51,12 @@ extern "C" { /*! * 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. */ +#define SBG_ECOM_AIR_DATA_TIME_IS_DELAY (0x0001u << 0) /*!< Set to 1 if the timeStamp field represents a delay instead of an absolute timestamp. */ +#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 -// @@ -67,13 +67,13 @@ extern "C" { */ typedef struct _SbgEComLogAirData { - 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. */ + 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. */ } SbgEComLogAirData; //----------------------------------------------------------------------// @@ -83,18 +83,18 @@ typedef struct _SbgEComLogAirData /*! * Parse data for the SBG_ECOM_LOG_AIR_DATA message and fill the corresponding structure. * - * \param[out] pLogData Log structure instance to fill. - * \param[in] pStreamBuffer Input stream buffer to read the log from. - * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. + * \param[out] pLogData Log structure instance to fill. + * \param[in] pStreamBuffer Input stream buffer to read the log from. + * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. */ SbgErrorCode sbgEComLogAirDataReadFromStream(SbgEComLogAirData *pLogData, SbgStreamBuffer *pStreamBuffer); /*! * Write data for the SBG_ECOM_LOG_AIR_DATA message to the output stream buffer from the provided structure. * - * \param[in] pLogData Log structure instance to write. - * \param[out] pStreamBuffer Output stream buffer to write the log to. - * \return SBG_NO_ERROR if the log has been written to the stream buffer. + * \param[in] pLogData Log structure instance to write. + * \param[out] pStreamBuffer Output stream buffer to write the log to. + * \return SBG_NO_ERROR if the log has been written to the stream buffer. */ SbgErrorCode sbgEComLogAirDataWriteToStream(const SbgEComLogAirData *pLogData, SbgStreamBuffer *pStreamBuffer); diff --git a/src/logs/sbgEComLogAutomotive.h b/src/logs/sbgEComLogAutomotive.h index 589a6a7..d08ad76 100644 --- a/src/logs/sbgEComLogAutomotive.h +++ b/src/logs/sbgEComLogAutomotive.h @@ -1,13 +1,13 @@ /*! - * \file sbgEComLogAutomotive.h - * \ingroup binaryLogs - * \author SBG Systems - * \date 22 April 2020 + * \file sbgEComLogAutomotive.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 22 April 2020 * - * \brief Parse dedicated automotive measurements logs. + * \brief Parse dedicated automotive measurements logs. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -48,9 +48,9 @@ extern "C" { /*! * Automotive data status mask definitions */ -#define SBG_ECOM_AUTO_DATA_TRACK_VALID (0x1u << 0) /*!< Set to 1 if the track angle is valid. */ -#define SBG_ECOM_AUTO_DATA_SLIP_VALID (0x1u << 1) /*!< Set to 1 if the slip angle is valid. */ -#define SBG_ECOM_AUTO_DATA_CURVATURE_VALID (0x1u << 2) /*!< Set to 1 if the curvature radius is valid. */ +#define SBG_ECOM_AUTO_DATA_TRACK_VALID (0x1u << 0) /*!< Set to 1 if the track angle is valid. */ +#define SBG_ECOM_AUTO_DATA_SLIP_VALID (0x1u << 1) /*!< Set to 1 if the slip angle is valid. */ +#define SBG_ECOM_AUTO_DATA_CURVATURE_VALID (0x1u << 2) /*!< Set to 1 if the curvature radius is valid. */ //----------------------------------------------------------------------// //- Log structure definitions -// @@ -61,10 +61,10 @@ extern "C" { */ typedef struct _SbgEComLogAutomotive { - uint8_t status; /*!< Status bit mask. */ - float trackAngle; /*!< Track angle, in rad. */ - float slipAngle; /*!< Slip angle, in rad. */ - float curvatureRadius; /*!< Curvature radius, in m, always positive. */ + uint8_t status; /*!< Status bit mask. */ + float trackAngle; /*!< Track angle, in rad. */ + float slipAngle; /*!< Slip angle, in rad. */ + float curvatureRadius; /*!< Curvature radius, in m, always positive. */ } SbgEComLogAutomotive; //----------------------------------------------------------------------// diff --git a/src/logs/sbgEComLogDepth.c b/src/logs/sbgEComLogDepth.c index 04773e8..dab919d 100644 --- a/src/logs/sbgEComLogDepth.c +++ b/src/logs/sbgEComLogDepth.c @@ -6,30 +6,30 @@ SbgErrorCode sbgEComLogDepthReadFromStream(SbgEComLogDepth *pLogData, SbgStreamBuffer *pStreamBuffer) { - assert(pLogData); - assert(pStreamBuffer); + assert(pLogData); + assert(pStreamBuffer); - pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); - pLogData->status = sbgStreamBufferReadUint16LE(pStreamBuffer); + pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); + pLogData->status = sbgStreamBufferReadUint16LE(pStreamBuffer); - pLogData->pressureAbs = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->altitude = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->pressureAbs = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->altitude = sbgStreamBufferReadFloatLE(pStreamBuffer); - return sbgStreamBufferGetLastError(pStreamBuffer); + return sbgStreamBufferGetLastError(pStreamBuffer); } SbgErrorCode sbgEComLogDepthWriteToStream(const SbgEComLogDepth *pLogData, SbgStreamBuffer *pStreamBuffer) { - assert(pLogData); - assert(pStreamBuffer); + assert(pLogData); + assert(pStreamBuffer); - sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); - sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->status); + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); + sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->status); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->pressureAbs); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->altitude); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->pressureAbs); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->altitude); - return sbgStreamBufferGetLastError(pStreamBuffer); + return sbgStreamBufferGetLastError(pStreamBuffer); } //----------------------------------------------------------------------// @@ -38,10 +38,10 @@ SbgErrorCode sbgEComLogDepthWriteToStream(const SbgEComLogDepth *pLogData, SbgSt SbgErrorCode sbgEComBinaryLogParseDepth(SbgStreamBuffer *pStreamBuffer, SbgEComLogDepth *pLogData) { - return sbgEComLogDepthReadFromStream(pLogData, pStreamBuffer); + return sbgEComLogDepthReadFromStream(pLogData, pStreamBuffer); } SbgErrorCode sbgEComBinaryLogWriteDepth(SbgStreamBuffer *pStreamBuffer, const SbgEComLogDepth *pLogData) { - return sbgEComLogDepthWriteToStream(pLogData, pStreamBuffer); + return sbgEComLogDepthWriteToStream(pLogData, pStreamBuffer); } diff --git a/src/logs/sbgEComLogDepth.h b/src/logs/sbgEComLogDepth.h index 2df87c1..33bfb05 100644 --- a/src/logs/sbgEComLogDepth.h +++ b/src/logs/sbgEComLogDepth.h @@ -1,15 +1,15 @@ /*! - * \file sbgEComLogDepth.h - * \ingroup binaryLogs - * \author SBG Systems - * \date 20 February 2019 + * \file sbgEComLogDepth.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 20 February 2019 * - * \brief Parse received sub-sea depth measurement logs. + * \brief Parse received sub-sea depth measurement logs. * * Depth sensor are used for sub-sea navigation to improve height. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -50,9 +50,9 @@ extern "C" { /*! * 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. */ +#define SBG_ECOM_DEPTH_TIME_IS_DELAY (0x0001u << 0) /*!< Set to 1 if the timeStamp field represents a delay instead of an absolute timestamp. */ +#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 -// @@ -63,10 +63,10 @@ extern "C" { */ typedef struct _SbgEComLogDepth { - 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. */ + 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. */ } SbgEComLogDepth; //----------------------------------------------------------------------// @@ -76,18 +76,18 @@ typedef struct _SbgEComLogDepth /*! * Parse data for the SBG_ECOM_LOG_DEPTH message and fill the corresponding structure. * - * \param[out] pLogData Log structure instance to fill. - * \param[in] pStreamBuffer Input stream buffer to read the log from. - * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. + * \param[out] pLogData Log structure instance to fill. + * \param[in] pStreamBuffer Input stream buffer to read the log from. + * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. */ SbgErrorCode sbgEComLogDepthReadFromStream(SbgEComLogDepth *pLogData, SbgStreamBuffer *pStreamBuffer); /*! * Write data for the SBG_ECOM_LOG_DEPTH message to the output stream buffer from the provided structure. * - * \param[in] pLogData Log structure instance to write. - * \param[out] pStreamBuffer Output stream buffer to write the log to. - * \return SBG_NO_ERROR if the log has been written to the stream buffer. + * \param[in] pLogData Log structure instance to write. + * \param[out] pStreamBuffer Output stream buffer to write the log to. + * \return SBG_NO_ERROR if the log has been written to the stream buffer. */ SbgErrorCode sbgEComLogDepthWriteToStream(const SbgEComLogDepth *pLogData, SbgStreamBuffer *pStreamBuffer); diff --git a/src/logs/sbgEComLogDiag.c b/src/logs/sbgEComLogDiag.c index 469fe41..a5d90e6 100644 --- a/src/logs/sbgEComLogDiag.c +++ b/src/logs/sbgEComLogDiag.c @@ -11,41 +11,41 @@ SbgErrorCode sbgEComLogDiagReadFromStream(SbgEComLogDiagData *pLogData, SbgStreamBuffer *pStreamBuffer) { - assert(pLogData); - assert(pStreamBuffer); + assert(pLogData); + assert(pStreamBuffer); - pLogData->timestamp = sbgStreamBufferReadUint32LE(pStreamBuffer); - pLogData->type = (SbgDebugLogType)sbgStreamBufferReadUint8(pStreamBuffer); - pLogData->errorCode = (SbgErrorCode)sbgStreamBufferReadUint8(pStreamBuffer); + pLogData->timestamp = sbgStreamBufferReadUint32LE(pStreamBuffer); + pLogData->type = (SbgDebugLogType)sbgStreamBufferReadUint8(pStreamBuffer); + pLogData->errorCode = (SbgErrorCode)sbgStreamBufferReadUint8(pStreamBuffer); - sbgStreamBufferReadBuffer(pStreamBuffer, pLogData->string, sbgStreamBufferGetSpace(pStreamBuffer)); - pLogData->string[sizeof(pLogData->string) - 1] = '\0'; + sbgStreamBufferReadBuffer(pStreamBuffer, pLogData->string, sbgStreamBufferGetSpace(pStreamBuffer)); + pLogData->string[sizeof(pLogData->string) - 1] = '\0'; - return sbgStreamBufferGetLastError(pStreamBuffer); + return sbgStreamBufferGetLastError(pStreamBuffer); } SbgErrorCode sbgEComLogDiagWriteToStream(const SbgEComLogDiagData *pLogData, SbgStreamBuffer *pStreamBuffer) { - size_t length; + size_t length; - assert(pLogData); - assert(pStreamBuffer); + assert(pLogData); + assert(pStreamBuffer); - sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timestamp); - sbgStreamBufferWriteUint8(pStreamBuffer, pLogData->type); - sbgStreamBufferWriteUint8(pStreamBuffer, pLogData->errorCode); + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timestamp); + sbgStreamBufferWriteUint8(pStreamBuffer, pLogData->type); + sbgStreamBufferWriteUint8(pStreamBuffer, pLogData->errorCode); - length = strlen(pLogData->string); + length = strlen(pLogData->string); - if (length >= sizeof(pLogData->string)) - { - length = sizeof(pLogData->string) - 1; - } + if (length >= sizeof(pLogData->string)) + { + length = sizeof(pLogData->string) - 1; + } - sbgStreamBufferWriteBuffer(pStreamBuffer, pLogData->string, length); - sbgStreamBufferWriteUint8(pStreamBuffer, 0); + sbgStreamBufferWriteBuffer(pStreamBuffer, pLogData->string, length); + sbgStreamBufferWriteUint8(pStreamBuffer, 0); - return sbgStreamBufferGetLastError(pStreamBuffer); + return sbgStreamBufferGetLastError(pStreamBuffer); } //----------------------------------------------------------------------// @@ -54,10 +54,10 @@ SbgErrorCode sbgEComLogDiagWriteToStream(const SbgEComLogDiagData *pLogData, Sbg SbgErrorCode sbgEComBinaryLogParseDiagData(SbgStreamBuffer *pStreamBuffer, SbgEComLogDiagData *pLogData) { - return sbgEComLogDiagReadFromStream(pLogData, pStreamBuffer); + return sbgEComLogDiagReadFromStream(pLogData, pStreamBuffer); } SbgErrorCode sbgEComBinaryLogWriteDiagData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogDiagData *pLogData) { - return sbgEComLogDiagWriteToStream(pLogData, pStreamBuffer); + return sbgEComLogDiagWriteToStream(pLogData, pStreamBuffer); } diff --git a/src/logs/sbgEComLogDiag.h b/src/logs/sbgEComLogDiag.h index 15cce5b..27bddca 100644 --- a/src/logs/sbgEComLogDiag.h +++ b/src/logs/sbgEComLogDiag.h @@ -1,13 +1,13 @@ /*! - * \file sbgEComLogDiag.h - * \ingroup binaryLogs - * \author SBG Systems - * \date 12 June 2019 + * \file sbgEComLogDiag.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 12 June 2019 * - * \brief Parse diagnostic logs emitted by the device. + * \brief Parse diagnostic logs emitted by the device. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -51,7 +51,7 @@ extern "C" { /*! * Maximum size of the log string, in bytes. */ -#define SBG_ECOM_LOG_DIAG_MAX_STRING_SIZE (SBG_ECOM_MAX_PAYLOAD_SIZE - 6) +#define SBG_ECOM_LOG_DIAG_MAX_STRING_SIZE (SBG_ECOM_MAX_PAYLOAD_SIZE - 6) //----------------------------------------------------------------------// //- Structure definitions -// @@ -62,10 +62,10 @@ extern "C" { */ typedef struct _SbgEComLogDiagData { - 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. */ + 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. */ } SbgEComLogDiagData; //----------------------------------------------------------------------// @@ -75,18 +75,18 @@ typedef struct _SbgEComLogDiagData /*! * Parse data for the SBG_ECOM_LOG_DIAG message and fill the corresponding structure. * - * \param[out] pLogData Log structure instance to fill. - * \param[in] pStreamBuffer Input stream buffer to read the log from. - * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. + * \param[out] pLogData Log structure instance to fill. + * \param[in] pStreamBuffer Input stream buffer to read the log from. + * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. */ SbgErrorCode sbgEComLogDiagReadFromStream(SbgEComLogDiagData *pLogData, SbgStreamBuffer *pStreamBuffer); /*! * Write data for the SBG_ECOM_LOG_DIAG message to the output stream buffer from the provided structure. * - * \param[in] pLogData Log structure instance to write. - * \param[out] pStreamBuffer Output stream buffer to write the log to. - * \return SBG_NO_ERROR if the log has been written to the stream buffer. + * \param[in] pLogData Log structure instance to write. + * \param[out] pStreamBuffer Output stream buffer to write the log to. + * \return SBG_NO_ERROR if the log has been written to the stream buffer. */ SbgErrorCode sbgEComLogDiagWriteToStream(const SbgEComLogDiagData *pLogData, SbgStreamBuffer *pStreamBuffer); diff --git a/src/logs/sbgEComLogDvl.c b/src/logs/sbgEComLogDvl.c index ae8fcfe..746cd94 100644 --- a/src/logs/sbgEComLogDvl.c +++ b/src/logs/sbgEComLogDvl.c @@ -6,40 +6,40 @@ SbgErrorCode sbgEComLogDvlReadFromStream(SbgEComLogDvl *pLogData, SbgStreamBuffer *pStreamBuffer) { - assert(pLogData); - assert(pStreamBuffer); + assert(pLogData); + assert(pStreamBuffer); - pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); - pLogData->status = sbgStreamBufferReadUint16LE(pStreamBuffer); + pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); + pLogData->status = sbgStreamBufferReadUint16LE(pStreamBuffer); - pLogData->velocity[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->velocity[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->velocity[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->velocity[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->velocity[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->velocity[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->velocityQuality[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->velocityQuality[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->velocityQuality[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->velocityQuality[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->velocityQuality[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->velocityQuality[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); - return sbgStreamBufferGetLastError(pStreamBuffer); + return sbgStreamBufferGetLastError(pStreamBuffer); } SbgErrorCode sbgEComLogDvlWriteToStream(const SbgEComLogDvl *pLogData, SbgStreamBuffer *pStreamBuffer) { - assert(pLogData); - assert(pStreamBuffer); + assert(pLogData); + assert(pStreamBuffer); - sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); - sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->status); + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); + sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->status); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocity[0]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocity[1]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocity[2]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocity[0]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocity[1]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocity[2]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocityQuality[0]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocityQuality[1]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocityQuality[2]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocityQuality[0]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocityQuality[1]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocityQuality[2]); - return sbgStreamBufferGetLastError(pStreamBuffer); + return sbgStreamBufferGetLastError(pStreamBuffer); } //----------------------------------------------------------------------// @@ -48,10 +48,10 @@ SbgErrorCode sbgEComLogDvlWriteToStream(const SbgEComLogDvl *pLogData, SbgStream SbgErrorCode sbgEComBinaryLogParseDvlData(SbgStreamBuffer *pStreamBuffer, SbgEComLogDvl *pLogData) { - return sbgEComLogDvlReadFromStream(pLogData, pStreamBuffer); + return sbgEComLogDvlReadFromStream(pLogData, pStreamBuffer); } SbgErrorCode sbgEComBinaryLogWriteDvlData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogDvl *pLogData) { - return sbgEComLogDvlWriteToStream(pLogData, pStreamBuffer); + return sbgEComLogDvlWriteToStream(pLogData, pStreamBuffer); } diff --git a/src/logs/sbgEComLogDvl.h b/src/logs/sbgEComLogDvl.h index ba78db3..b0dda5a 100644 --- a/src/logs/sbgEComLogDvl.h +++ b/src/logs/sbgEComLogDvl.h @@ -1,13 +1,13 @@ /*! - * \file sbgEComLogDvl.h - * \ingroup binaryLogs - * \author SBG Systems - * \date 05 June 2013 + * \file sbgEComLogDvl.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 05 June 2013 * - * \brief Parse received DVL (Doppler Velocity Logger) measurement logs. + * \brief Parse received DVL (Doppler Velocity Logger) measurement logs. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -48,23 +48,28 @@ extern "C" { /*! * 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. */ +#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 3D velocity measurement. + * \brief Log structure for DVL 3D velocity measurement. + * + * This structure contains data related to the DVL 3D velocity measurements, + * including timestamp, status, and velocity information. + * + * \note The DVL quality indicator represents residual error information rather + * than an actual standard deviation measurement. */ typedef struct _SbgEComLogDvl { - 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. */ + uint32_t timeStamp; /*!< Time in microseconds since the sensor power up. */ + uint16_t status; /*!< DVL status bitmask. */ + float velocity[3]; /*!< X, Y, Z velocities in m/s expressed in the DVL instrument frame. */ + float velocityQuality[3]; /*!< Quality indicators for X, Y, Z velocities provided by the DVL sensor, expressed in m/s. */ } SbgEComLogDvl; //----------------------------------------------------------------------// @@ -74,18 +79,18 @@ typedef struct _SbgEComLogDvl /*! * Parse data for the SBG_ECOM_LOG_DVL_BOTTOM_TRACK / SBG_ECOM_LOG_DVL_WATER_TRACK messages and fill the corresponding structure. * - * \param[out] pLogData Log structure instance to fill. - * \param[in] pStreamBuffer Input stream buffer to read the log from. - * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. + * \param[out] pLogData Log structure instance to fill. + * \param[in] pStreamBuffer Input stream buffer to read the log from. + * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. */ SbgErrorCode sbgEComLogDvlReadFromStream(SbgEComLogDvl *pLogData, SbgStreamBuffer *pStreamBuffer); /*! * Write data for the SBG_ECOM_LOG_DVL_BOTTOM_TRACK / SBG_ECOM_LOG_DVL_WATER_TRACK messages to the output stream buffer from the provided structure. * - * \param[in] pLogData Log structure instance to write. - * \param[out] pStreamBuffer Output stream buffer to write the log to. - * \return SBG_NO_ERROR if the log has been written to the stream buffer. + * \param[in] pLogData Log structure instance to write. + * \param[out] pStreamBuffer Output stream buffer to write the log to. + * \return SBG_NO_ERROR if the log has been written to the stream buffer. */ SbgErrorCode sbgEComLogDvlWriteToStream(const SbgEComLogDvl *pLogData, SbgStreamBuffer *pStreamBuffer); diff --git a/src/logs/sbgEComLogEkf.c b/src/logs/sbgEComLogEkf.c index 018fa7f..4de33c2 100644 --- a/src/logs/sbgEComLogEkf.c +++ b/src/logs/sbgEComLogEkf.c @@ -7,51 +7,98 @@ /*! * Solution status mode definitions. */ -#define SBG_ECOM_LOG_EKF_SOLUTION_MODE_SHIFT (0u) /*!< Shift used to extract the clock status part. */ -#define SBG_ECOM_LOG_EKF_SOLUTION_MODE_MASK (0x0000000Fu) /*!< Mask used to keep only the clock status part. */ +#define SBG_ECOM_LOG_EKF_SOLUTION_MODE_SHIFT (0u) /*!< Shift used to extract the clock status part. */ +#define SBG_ECOM_LOG_EKF_SOLUTION_MODE_MASK (0x0000000Fu) /*!< Mask used to keep only the clock status part. */ //----------------------------------------------------------------------// //- Public methods (SbgEComLogEkfEuler) -// //----------------------------------------------------------------------// -SbgErrorCode sbgEComLogEkfEulerReadFromStream(SbgEComLogEkfEuler *pLogData, SbgStreamBuffer *pStreamBuffer) +float sbgEComLogEkfEulerGetMagneticHeading(const SbgEComLogEkfEuler *pEkfEuler) { - assert(pStreamBuffer); - assert(pLogData); - - pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); - - pLogData->euler[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->euler[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->euler[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); - - pLogData->eulerStdDev[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->eulerStdDev[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->eulerStdDev[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); - - pLogData->status = sbgStreamBufferReadUint32LE(pStreamBuffer); + float heading; + + assert(pEkfEuler); + + if (pEkfEuler->magDeclination != NAN) + { + heading = pEkfEuler->euler[2] - pEkfEuler->magDeclination; + + if (heading > SBG_PI_F) + { + heading = heading - 2.0f * SBG_PI_F; + } + else if (heading < -SBG_PI_F) + { + heading = heading + 2.0f * SBG_PI_F; + } + } + else + { + heading = pEkfEuler->euler[2]; + } + + return heading; +} - return sbgStreamBufferGetLastError(pStreamBuffer); +SbgErrorCode sbgEComLogEkfEulerReadFromStream(SbgEComLogEkfEuler *pLogData, SbgStreamBuffer *pStreamBuffer) +{ + assert(pStreamBuffer); + assert(pLogData); + + pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); + + pLogData->euler[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->euler[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->euler[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); + + pLogData->eulerStdDev[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->eulerStdDev[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->eulerStdDev[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); + + pLogData->status = sbgStreamBufferReadUint32LE(pStreamBuffer); + + // + // Added in sbgECom 5.0 + // + if (sbgStreamBufferGetSpace(pStreamBuffer) >= 2*sizeof(float)) + { + pLogData->magDeclination = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->magInclination = sbgStreamBufferReadFloatLE(pStreamBuffer); + } + else + { + pLogData->magDeclination = NAN; + pLogData->magInclination = NAN; + } + + return sbgStreamBufferGetLastError(pStreamBuffer); } SbgErrorCode sbgEComLogEkfEulerWriteToStream(const SbgEComLogEkfEuler *pLogData, SbgStreamBuffer *pStreamBuffer) { - assert(pStreamBuffer); - assert(pLogData); + assert(pStreamBuffer); + assert(pLogData); - sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); - - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->euler[0]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->euler[1]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->euler[2]); + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); + + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->euler[0]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->euler[1]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->euler[2]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->eulerStdDev[0]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->eulerStdDev[1]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->eulerStdDev[2]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->eulerStdDev[0]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->eulerStdDev[1]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->eulerStdDev[2]); - sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->status); + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->status); - return sbgStreamBufferGetLastError(pStreamBuffer); + // + // Added in sbgECom 5.0 + // + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->magDeclination); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->magInclination); + + return sbgStreamBufferGetLastError(pStreamBuffer); } //----------------------------------------------------------------------// @@ -60,44 +107,64 @@ SbgErrorCode sbgEComLogEkfEulerWriteToStream(const SbgEComLogEkfEuler *pLogData, SbgErrorCode sbgEComLogEkfQuatReadFromStream(SbgEComLogEkfQuat *pLogData, SbgStreamBuffer *pStreamBuffer) { - assert(pStreamBuffer); - assert(pLogData); - - pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); - - pLogData->quaternion[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->quaternion[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->quaternion[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->quaternion[3] = sbgStreamBufferReadFloatLE(pStreamBuffer); - - pLogData->eulerStdDev[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->eulerStdDev[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->eulerStdDev[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); - - pLogData->status = sbgStreamBufferReadUint32LE(pStreamBuffer); - - return sbgStreamBufferGetLastError(pStreamBuffer); + assert(pStreamBuffer); + assert(pLogData); + + pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); + + pLogData->quaternion[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->quaternion[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->quaternion[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->quaternion[3] = sbgStreamBufferReadFloatLE(pStreamBuffer); + + pLogData->eulerStdDev[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->eulerStdDev[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->eulerStdDev[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); + + pLogData->status = sbgStreamBufferReadUint32LE(pStreamBuffer); + + // + // Added in sbgECom 5.0 + // + if (sbgStreamBufferGetSpace(pStreamBuffer) >= 2*sizeof(float)) + { + pLogData->magDeclination = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->magInclination = sbgStreamBufferReadFloatLE(pStreamBuffer); + } + else + { + pLogData->magDeclination = NAN; + pLogData->magInclination = NAN; + } + + return sbgStreamBufferGetLastError(pStreamBuffer); } SbgErrorCode sbgEComLogEkfQuatWriteToStream(const SbgEComLogEkfQuat *pLogData, SbgStreamBuffer *pStreamBuffer) { - assert(pStreamBuffer); - assert(pLogData); + assert(pStreamBuffer); + assert(pLogData); + + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); - sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->quaternion[0]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->quaternion[1]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->quaternion[2]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->quaternion[3]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->quaternion[0]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->quaternion[1]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->quaternion[2]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->quaternion[3]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->eulerStdDev[0]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->eulerStdDev[1]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->eulerStdDev[2]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->eulerStdDev[0]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->eulerStdDev[1]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->eulerStdDev[2]); + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->status); - sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->status); + // + // Added in sbgECom 5.0 + // + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->magDeclination); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->magInclination); - return sbgStreamBufferGetLastError(pStreamBuffer); + return sbgStreamBufferGetLastError(pStreamBuffer); } //----------------------------------------------------------------------// @@ -106,62 +173,62 @@ SbgErrorCode sbgEComLogEkfQuatWriteToStream(const SbgEComLogEkfQuat *pLogData, S SbgErrorCode sbgEComLogEkfNavReadFromStream(SbgEComLogEkfNav *pLogData, SbgStreamBuffer *pStreamBuffer) { - assert(pStreamBuffer); - assert(pLogData); + assert(pStreamBuffer); + assert(pLogData); - pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); + pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); - pLogData->velocity[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->velocity[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->velocity[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->velocity[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->velocity[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->velocity[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->velocityStdDev[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->velocityStdDev[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->velocityStdDev[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->velocityStdDev[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->velocityStdDev[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->velocityStdDev[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->position[0] = sbgStreamBufferReadDoubleLE(pStreamBuffer); - pLogData->position[1] = sbgStreamBufferReadDoubleLE(pStreamBuffer); - pLogData->position[2] = sbgStreamBufferReadDoubleLE(pStreamBuffer); + pLogData->position[0] = sbgStreamBufferReadDoubleLE(pStreamBuffer); + pLogData->position[1] = sbgStreamBufferReadDoubleLE(pStreamBuffer); + pLogData->position[2] = sbgStreamBufferReadDoubleLE(pStreamBuffer); - pLogData->undulation = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->undulation = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->positionStdDev[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->positionStdDev[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->positionStdDev[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->positionStdDev[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->positionStdDev[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->positionStdDev[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->status = sbgStreamBufferReadUint32LE(pStreamBuffer); + pLogData->status = sbgStreamBufferReadUint32LE(pStreamBuffer); - return sbgStreamBufferGetLastError(pStreamBuffer); + return sbgStreamBufferGetLastError(pStreamBuffer); } SbgErrorCode sbgEComLogEkfNavWriteToStream(const SbgEComLogEkfNav *pLogData, SbgStreamBuffer *pStreamBuffer) { - assert(pStreamBuffer); - assert(pLogData); + assert(pStreamBuffer); + assert(pLogData); - sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocity[0]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocity[1]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocity[2]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocity[0]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocity[1]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocity[2]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocityStdDev[0]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocityStdDev[1]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocityStdDev[2]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocityStdDev[0]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocityStdDev[1]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocityStdDev[2]); - sbgStreamBufferWriteDoubleLE(pStreamBuffer, pLogData->position[0]); - sbgStreamBufferWriteDoubleLE(pStreamBuffer, pLogData->position[1]); - sbgStreamBufferWriteDoubleLE(pStreamBuffer, pLogData->position[2]); + sbgStreamBufferWriteDoubleLE(pStreamBuffer, pLogData->position[0]); + sbgStreamBufferWriteDoubleLE(pStreamBuffer, pLogData->position[1]); + sbgStreamBufferWriteDoubleLE(pStreamBuffer, pLogData->position[2]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->undulation); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->undulation); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->positionStdDev[0]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->positionStdDev[1]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->positionStdDev[2]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->positionStdDev[0]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->positionStdDev[1]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->positionStdDev[2]); - sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->status); + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->status); - return sbgStreamBufferGetLastError(pStreamBuffer); + return sbgStreamBufferGetLastError(pStreamBuffer); } //----------------------------------------------------------------------// @@ -170,47 +237,47 @@ SbgErrorCode sbgEComLogEkfNavWriteToStream(const SbgEComLogEkfNav *pLogData, Sbg void sbgEComLogEkfVelBodyConstruct(SbgEComLogEkfVelBody *pLogData) { - assert(pLogData); + assert(pLogData); - memset(pLogData, 0x00, sizeof(*pLogData)); + memset(pLogData, 0x00, sizeof(*pLogData)); } SbgErrorCode sbgEComLogEkfVelBodyReadFromStream(SbgEComLogEkfVelBody *pLogData, SbgStreamBuffer *pStreamBuffer) { - assert(pStreamBuffer); - assert(pLogData); + assert(pStreamBuffer); + assert(pLogData); - pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); - pLogData->status = sbgStreamBufferReadUint32LE(pStreamBuffer); + pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); + pLogData->status = sbgStreamBufferReadUint32LE(pStreamBuffer); - pLogData->velocity[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->velocity[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->velocity[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->velocity[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->velocity[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->velocity[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->velocityStdDev[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->velocityStdDev[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->velocityStdDev[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); - - return sbgStreamBufferGetLastError(pStreamBuffer); + pLogData->velocityStdDev[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->velocityStdDev[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->velocityStdDev[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); + + return sbgStreamBufferGetLastError(pStreamBuffer); } SbgErrorCode sbgEComLogEkfVelBodyWriteToStream(const SbgEComLogEkfVelBody *pLogData, SbgStreamBuffer *pStreamBuffer) { - assert(pStreamBuffer); - assert(pLogData); + assert(pStreamBuffer); + assert(pLogData); - sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); - sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->status); + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->status); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocity[0]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocity[1]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocity[2]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocity[0]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocity[1]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocity[2]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocityStdDev[0]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocityStdDev[1]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocityStdDev[2]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocityStdDev[0]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocityStdDev[1]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocityStdDev[2]); - return sbgStreamBufferGetLastError(pStreamBuffer); + return sbgStreamBufferGetLastError(pStreamBuffer); } //----------------------------------------------------------------------// @@ -219,12 +286,12 @@ SbgErrorCode sbgEComLogEkfVelBodyWriteToStream(const SbgEComLogEkfVelBody *pLogD SbgEComSolutionMode sbgEComLogEkfGetSolutionMode(uint32_t status) { - return (SbgEComSolutionMode)((status >> SBG_ECOM_LOG_EKF_SOLUTION_MODE_SHIFT) & SBG_ECOM_LOG_EKF_SOLUTION_MODE_MASK); + return (SbgEComSolutionMode)((status >> SBG_ECOM_LOG_EKF_SOLUTION_MODE_SHIFT) & SBG_ECOM_LOG_EKF_SOLUTION_MODE_MASK); } uint32_t sbgEComLogEkfBuildSolutionStatus(SbgEComSolutionMode solutionMode, uint32_t masks) { - return ((((uint32_t)solutionMode)&SBG_ECOM_LOG_EKF_SOLUTION_MODE_MASK) << SBG_ECOM_LOG_EKF_SOLUTION_MODE_SHIFT) | masks; + return ((((uint32_t)solutionMode)&SBG_ECOM_LOG_EKF_SOLUTION_MODE_MASK) << SBG_ECOM_LOG_EKF_SOLUTION_MODE_SHIFT) | masks; } //----------------------------------------------------------------------// @@ -233,30 +300,30 @@ uint32_t sbgEComLogEkfBuildSolutionStatus(SbgEComSolutionMode solutionMode, uint SbgErrorCode sbgEComBinaryLogParseEkfEulerData(SbgStreamBuffer *pStreamBuffer, SbgEComLogEkfEuler *pLogData) { - return sbgEComLogEkfEulerReadFromStream(pLogData, pStreamBuffer); + return sbgEComLogEkfEulerReadFromStream(pLogData, pStreamBuffer); } SbgErrorCode sbgEComBinaryLogWriteEkfEulerData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogEkfEuler *pLogData) { - return sbgEComLogEkfEulerWriteToStream(pLogData, pStreamBuffer); + return sbgEComLogEkfEulerWriteToStream(pLogData, pStreamBuffer); } SbgErrorCode sbgEComBinaryLogParseEkfQuatData(SbgStreamBuffer *pStreamBuffer, SbgEComLogEkfQuat *pLogData) { - return sbgEComLogEkfQuatReadFromStream(pLogData, pStreamBuffer); + return sbgEComLogEkfQuatReadFromStream(pLogData, pStreamBuffer); } SbgErrorCode sbgEComBinaryLogWriteEkfQuatData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogEkfQuat *pLogData) { - return sbgEComLogEkfQuatWriteToStream(pLogData, pStreamBuffer); + return sbgEComLogEkfQuatWriteToStream(pLogData, pStreamBuffer); } SbgErrorCode sbgEComBinaryLogParseEkfNavData(SbgStreamBuffer *pStreamBuffer, SbgEComLogEkfNav *pLogData) { - return sbgEComLogEkfNavReadFromStream(pLogData, pStreamBuffer); + return sbgEComLogEkfNavReadFromStream(pLogData, pStreamBuffer); } SbgErrorCode sbgEComBinaryLogWriteEkfNavData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogEkfNav *pLogData) { - return sbgEComLogEkfNavWriteToStream(pLogData, pStreamBuffer); + return sbgEComLogEkfNavWriteToStream(pLogData, pStreamBuffer); } diff --git a/src/logs/sbgEComLogEkf.h b/src/logs/sbgEComLogEkf.h index 6c24205..5cd93a7 100644 --- a/src/logs/sbgEComLogEkf.h +++ b/src/logs/sbgEComLogEkf.h @@ -1,13 +1,13 @@ /*! - * \file sbgEComLogEkf.h - * \ingroup binaryLogs - * \author SBG Systems - * \date 25 February 2013 + * \file sbgEComLogEkf.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 25 February 2013 * - * \brief Parse EKF measurements such as attitude, position and velocity logs. + * \brief Parse EKF measurements such as attitude, position and velocity logs. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -48,40 +48,40 @@ extern "C" { /*! * 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 heading 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 sub-sea navigation) is used in solution (data used and valid since 3s). */ - +#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 heading 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 Zero Velocity Update (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 sub-sea navigation) is used in solution (data used and valid since 3s). */ +#define SBG_ECOM_SOL_ZARU_USED (0x00000001u << 29) /*!< Set to 1 if a Zero Angular Rate Update (ZARU) 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. */ + 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; //----------------------------------------------------------------------// @@ -90,24 +90,40 @@ typedef enum _SbgEComSolutionMode /*! * EKF computed orientation using euler angles. + * + * Note: In AHRS (Attitude and Heading Reference System) or INS (Inertial Navigation System) modes, + * the yaw angle represents the geographic (true) heading of the device. + * + * To convert a geographic heading to a magnetic heading, use the following formula: + * Magnetic Heading = Geographic Heading - Magnetic Declination */ typedef struct _SbgEComLogEkfEuler { - 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. */ + uint32_t timeStamp; /*!< Time in us since the sensor power up. */ + float euler[3]; /*!< Roll [-pi;+pi], pitch [-pi/2;+pi/2], yaw [-pi;+pi] in radians. */ + float eulerStdDev[3]; /*!< Roll std. [0;+pi], pitch std. [0;+pi/2] and yaw std. [0;+pi] estimated 1-sigma accuracy in radians */ + uint32_t status; /*!< EKF solution status bitmask and enum. */ + float magDeclination; /*!< Local magnetic declination in radians (positive east - set to NAN if not available). */ + float magInclination; /*!< Local magnetic inclination in radians (positive down - set to NAN if not available). */ } SbgEComLogEkfEuler; /*! * EFK computed orientation using quaternion. + * + * Note: In AHRS (Attitude and Heading Reference System) or INS (Inertial Navigation System) modes, + * the yaw angle represents the geographic (true) heading of the device. + * + * To convert a geographic heading to a magnetic heading, use the following formula: + * Magnetic Heading = Geographic Heading - Magnetic Declination */ typedef struct _SbgEComLogEkfQuat { - 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. */ + 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 std. [0;+pi], pitch std. [0;+pi/2] and yaw std. [0;+pi] estimated 1-sigma accuracy in radians */ + uint32_t status; /*!< EKF solution status bitmask and enum. */ + float magDeclination; /*!< Local magnetic declination in radians (positive east - set to NAN if not available). */ + float magInclination; /*!< Local magnetic inclination in radians (positive down - set to NAN if not available). */ } SbgEComLogEkfQuat; /*! @@ -115,14 +131,14 @@ typedef struct _SbgEComLogEkfQuat */ typedef struct _SbgEComLogEkfNav { - 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. */ + 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. */ } SbgEComLogEkfNav; /*! @@ -130,31 +146,39 @@ typedef struct _SbgEComLogEkfNav */ typedef struct _SbgEComLogEkfVelBody { - uint32_t timeStamp; /*!< Time in us since the sensor power up. */ - uint32_t status; /*!< EKF solution status bitmask and enum. */ - float velocity[3]; /*!< X,Y,Z body velocity in m.s^-1. */ - float velocityStdDev[3]; /*!< X,Y,Z body velocity 1 sigma standard deviation in m.s^-1. */ + uint32_t timeStamp; /*!< Time in us since the sensor power up. */ + uint32_t status; /*!< EKF solution status bitmask and enum. */ + float velocity[3]; /*!< X,Y,Z body velocity in m.s^-1. */ + float velocityStdDev[3]; /*!< X,Y,Z body velocity 1 sigma standard deviation in m.s^-1. */ } SbgEComLogEkfVelBody; //----------------------------------------------------------------------// //- Public methods (SbgEComLogEkfEuler) -// //----------------------------------------------------------------------// +/*! + * Returns the magnetic heading from the true heading by applying the local declination. + * + * \param[in] pEkfEuler Log instance. + * \return Magnetic heading [-pi;+pi] in radians. + */ +float sbgEComLogEkfEulerGetMagneticHeading(const SbgEComLogEkfEuler *pEkfEuler); + /*! * Parse data for the SBG_ECOM_LOG_EKF_EULER message and fill the corresponding structure. * - * \param[out] pLogData Log structure instance to fill. - * \param[in] pStreamBuffer Input stream buffer to read the log from. - * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. + * \param[out] pLogData Log structure instance to fill. + * \param[in] pStreamBuffer Input stream buffer to read the log from. + * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. */ SbgErrorCode sbgEComLogEkfEulerReadFromStream(SbgEComLogEkfEuler *pLogData, SbgStreamBuffer *pStreamBuffer); /*! * Write data for the SBG_ECOM_LOG_EKF_EULER message to the output stream buffer from the provided structure. * - * \param[in] pLogData Log structure instance to write. - * \param[out] pStreamBuffer Output stream buffer to write the log to. - * \return SBG_NO_ERROR if the log has been written to the stream buffer. + * \param[in] pLogData Log structure instance to write. + * \param[out] pStreamBuffer Output stream buffer to write the log to. + * \return SBG_NO_ERROR if the log has been written to the stream buffer. */ SbgErrorCode sbgEComLogEkfEulerWriteToStream(const SbgEComLogEkfEuler *pLogData, SbgStreamBuffer *pStreamBuffer); @@ -165,18 +189,18 @@ SbgErrorCode sbgEComLogEkfEulerWriteToStream(const SbgEComLogEkfEuler *pLogData, /*! * Parse data for the SBG_ECOM_LOG_EKF_QUAT message and fill the corresponding structure. * - * \param[out] pLogData Log structure instance to fill. - * \param[in] pStreamBuffer Input stream buffer to read the log from. - * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. + * \param[out] pLogData Log structure instance to fill. + * \param[in] pStreamBuffer Input stream buffer to read the log from. + * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. */ SbgErrorCode sbgEComLogEkfQuatReadFromStream(SbgEComLogEkfQuat *pLogData, SbgStreamBuffer *pStreamBuffer); /*! * Write data for the SBG_ECOM_LOG_EKF_QUAT message to the output stream buffer from the provided structure. * - * \param[in] pLogData Log structure instance to write. - * \param[out] pStreamBuffer Output stream buffer to write the log to. - * \return SBG_NO_ERROR if the log has been written to the stream buffer. + * \param[in] pLogData Log structure instance to write. + * \param[out] pStreamBuffer Output stream buffer to write the log to. + * \return SBG_NO_ERROR if the log has been written to the stream buffer. */ SbgErrorCode sbgEComLogEkfQuatWriteToStream(const SbgEComLogEkfQuat *pLogData, SbgStreamBuffer *pStreamBuffer); @@ -187,18 +211,18 @@ SbgErrorCode sbgEComLogEkfQuatWriteToStream(const SbgEComLogEkfQuat *pLogData, S /*! * Parse data for the SBG_ECOM_LOG_EKF_NAV message and fill the corresponding structure. * - * \param[out] pLogData Log structure instance to fill. - * \param[in] pStreamBuffer Input stream buffer to read the log from. - * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. + * \param[out] pLogData Log structure instance to fill. + * \param[in] pStreamBuffer Input stream buffer to read the log from. + * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. */ SbgErrorCode sbgEComLogEkfNavReadFromStream(SbgEComLogEkfNav *pLogData, SbgStreamBuffer *pStreamBuffer); /*! * Write data for the SBG_ECOM_LOG_EKF_NAV message to the output stream buffer from the provided structure. * - * \param[in] pLogData Log structure instance to write. - * \param[out] pStreamBuffer Output stream buffer to write the log to. - * \return SBG_NO_ERROR if the log has been written to the stream buffer. + * \param[in] pLogData Log structure instance to write. + * \param[out] pStreamBuffer Output stream buffer to write the log to. + * \return SBG_NO_ERROR if the log has been written to the stream buffer. */ SbgErrorCode sbgEComLogEkfNavWriteToStream(const SbgEComLogEkfNav *pLogData, SbgStreamBuffer *pStreamBuffer); @@ -209,25 +233,25 @@ SbgErrorCode sbgEComLogEkfNavWriteToStream(const SbgEComLogEkfNav *pLogData, Sbg /*! * Construct an empty / zero initialized instance. * - * \param[in] pLogData Log instance. + * \param[in] pLogData Log instance. */ void sbgEComLogEkfVelBodyConstruct(SbgEComLogEkfVelBody *pLogData); /*! * Read SBG_ECOM_LOG_EKF_VEL_BODY message from a stream buffer. * - * \param[out] pLogData Log instance. - * \param[in] pStreamBuffer Input stream buffer to read the log from. - * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. + * \param[out] pLogData Log instance. + * \param[in] pStreamBuffer Input stream buffer to read the log from. + * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. */ SbgErrorCode sbgEComLogEkfVelBodyReadFromStream(SbgEComLogEkfVelBody *pLogData, SbgStreamBuffer *pStreamBuffer); /*! * Write SBG_ECOM_LOG_EKF_VEL_BODY message to a stream buffer. * - * \param[in] pLogData Log instance. - * \param[out] pStreamBuffer Output stream buffer to write the log to. - * \return SBG_NO_ERROR if the log has been written to the stream buffer. + * \param[in] pLogData Log instance. + * \param[out] pStreamBuffer Output stream buffer to write the log to. + * \return SBG_NO_ERROR if the log has been written to the stream buffer. */ SbgErrorCode sbgEComLogEkfVelBodyWriteToStream(const SbgEComLogEkfVelBody *pLogData, SbgStreamBuffer *pStreamBuffer); @@ -238,17 +262,17 @@ SbgErrorCode sbgEComLogEkfVelBodyWriteToStream(const SbgEComLogEkfVelBody *pLogD /*! * 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. + * \param[in] status Status uint32_t value to extract the solution mode from it. + * \return The extracted solution mode. */ SbgEComSolutionMode sbgEComLogEkfGetSolutionMode(uint32_t status); /*! * 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. + * \param[in] solutionMode The solution mode to set. + * \param[in] masks Bit mask to set. + * \return The build solution status field. */ uint32_t sbgEComLogEkfBuildSolutionStatus(SbgEComSolutionMode solutionMode, uint32_t masks); @@ -256,9 +280,9 @@ uint32_t sbgEComLogEkfBuildSolutionStatus(SbgEComSolutionMode solutionMode, uint //- DEPRECATED - Used for backward compatibility -// //----------------------------------------------------------------------// -SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogEkfEuler SbgLogEkfEulerData); -SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogEkfQuat SbgLogEkfQuatData); -SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogEkfNav SbgLogEkfNavData); +SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogEkfEuler SbgLogEkfEulerData); +SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogEkfQuat SbgLogEkfQuatData); +SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogEkfNav SbgLogEkfNavData); SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogParseEkfEulerData(SbgStreamBuffer *pStreamBuffer, SbgEComLogEkfEuler *pLogData)); SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogWriteEkfEulerData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogEkfEuler *pLogData)); diff --git a/src/logs/sbgEComLogEkfRotAccel.c b/src/logs/sbgEComLogEkfRotAccel.c index 169ebda..369546f 100644 --- a/src/logs/sbgEComLogEkfRotAccel.c +++ b/src/logs/sbgEComLogEkfRotAccel.c @@ -11,45 +11,45 @@ void sbgEComLogEkfRotAccelConstruct(SbgEComLogEkfRotAccel *pLogData) { - assert(pLogData); + assert(pLogData); - memset(pLogData, 0x00, sizeof(*pLogData)); + memset(pLogData, 0x00, sizeof(*pLogData)); } SbgErrorCode sbgEComLogEkfRotAccelReadFromStream(SbgEComLogEkfRotAccel *pLogData, SbgStreamBuffer *pStreamBuffer) { - assert(pStreamBuffer); - assert(pLogData); + assert(pStreamBuffer); + assert(pLogData); - pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); - pLogData->status = sbgStreamBufferReadUint32LE(pStreamBuffer); + pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); + pLogData->status = sbgStreamBufferReadUint32LE(pStreamBuffer); - pLogData->rate[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->rate[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->rate[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->rate[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->rate[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->rate[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->acceleration[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->acceleration[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->acceleration[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->acceleration[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->acceleration[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->acceleration[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); - return sbgStreamBufferGetLastError(pStreamBuffer); + return sbgStreamBufferGetLastError(pStreamBuffer); } SbgErrorCode sbgEComLogEkfRotAccelWriteToStream(const SbgEComLogEkfRotAccel *pLogData, SbgStreamBuffer *pStreamBuffer) { - assert(pStreamBuffer); - assert(pLogData); + assert(pStreamBuffer); + assert(pLogData); - sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); - sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->status); + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->status); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->rate[0]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->rate[1]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->rate[2]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->rate[0]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->rate[1]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->rate[2]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->acceleration[0]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->acceleration[1]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->acceleration[2]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->acceleration[0]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->acceleration[1]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->acceleration[2]); - return sbgStreamBufferGetLastError(pStreamBuffer); + return sbgStreamBufferGetLastError(pStreamBuffer); } diff --git a/src/logs/sbgEComLogEkfRotAccel.h b/src/logs/sbgEComLogEkfRotAccel.h index 2944962..4420e6e 100644 --- a/src/logs/sbgEComLogEkfRotAccel.h +++ b/src/logs/sbgEComLogEkfRotAccel.h @@ -1,10 +1,10 @@ /*! - * \file sbgEComLogEkfRotAccel.h - * \ingroup binaryLogs - * \author SBG Systems - * \date 12 June 2023 + * \file sbgEComLogEkfRotAccel.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 12 June 2023 * - * \brief Write and parse SBG_ECOM_LOG_EKF_ROT_ACCEL_XXXX messages. + * \brief Write and parse SBG_ECOM_LOG_EKF_ROT_ACCEL_XXXX messages. * * The SBG_ECOM_LOG_EKF_ROT_ACCEL_BODY and SBG_ECOM_LOG_EKF_ROT_ACCEL_NED logs * returns compensated INS rotation rate and lateral accelerations. @@ -26,8 +26,8 @@ * local earth gravity has been removed. It should returns zero if the INS is not * moving at all. * - * \copyright Copyright (C) 2023, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -70,10 +70,10 @@ extern "C" { */ typedef struct _SbgEComLogEkfRotAccel { - uint32_t timeStamp; /*!< Time in us since the sensor power up. */ - uint32_t status; /*!< Status - reserved for future use. */ - float rate[3]; /*!< X,Y,Z or North/East/Down INS rotation rate free from earth rotation and sensor bias/scale errors (rad.s^-1). */ - float acceleration[3]; /*!< X,Y,Z or North/East/Down INS accelerations and free from gravity and sensor bias/scale errors (m.s^-2). */ + uint32_t timeStamp; /*!< Time in us since the sensor power up. */ + uint32_t status; /*!< Status - reserved for future use. */ + float rate[3]; /*!< X,Y,Z or North/East/Down INS rotation rate free from earth rotation and sensor bias/scale errors (rad.s^-1). */ + float acceleration[3]; /*!< X,Y,Z or North/East/Down INS accelerations and free from gravity and sensor bias/scale errors (m.s^-2). */ } SbgEComLogEkfRotAccel; //----------------------------------------------------------------------// @@ -83,25 +83,25 @@ typedef struct _SbgEComLogEkfRotAccel /*! * Construct an empty / zero initialized instance. * - * \param[in] pLogData Log instance. + * \param[in] pLogData Log instance. */ void sbgEComLogEkfRotAccelConstruct(SbgEComLogEkfRotAccel *pLogData); /*! * Read SBG_ECOM_LOG_EKF_ROT_ACCEL_BODY or SBG_ECOM_LOG_EKF_ROT_ACCEL_NED messages from a stream buffer. * - * \param[out] pLogData Log instance. - * \param[in] pStreamBuffer Input stream buffer to read the log from. - * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. + * \param[out] pLogData Log instance. + * \param[in] pStreamBuffer Input stream buffer to read the log from. + * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. */ SbgErrorCode sbgEComLogEkfRotAccelReadFromStream(SbgEComLogEkfRotAccel *pLogData, SbgStreamBuffer *pStreamBuffer); /*! * Write SBG_ECOM_LOG_EKF_ROT_ACCEL_BODY or SBG_ECOM_LOG_EKF_ROT_ACCEL_NED messages to a stream buffer. * - * \param[in] pLogData Log instance. - * \param[out] pStreamBuffer Output stream buffer to write the log to. - * \return SBG_NO_ERROR if the log has been written to the stream buffer. + * \param[in] pLogData Log instance. + * \param[out] pStreamBuffer Output stream buffer to write the log to. + * \return SBG_NO_ERROR if the log has been written to the stream buffer. */ SbgErrorCode sbgEComLogEkfRotAccelWriteToStream(const SbgEComLogEkfRotAccel *pLogData, SbgStreamBuffer *pStreamBuffer); diff --git a/src/logs/sbgEComLogEvent.c b/src/logs/sbgEComLogEvent.c index 38fe8b6..f4de42a 100644 --- a/src/logs/sbgEComLogEvent.c +++ b/src/logs/sbgEComLogEvent.c @@ -6,32 +6,32 @@ SbgErrorCode sbgEComLogEventReadFromStream(SbgEComLogEvent *pLogData, SbgStreamBuffer *pStreamBuffer) { - assert(pStreamBuffer); - assert(pLogData); + assert(pStreamBuffer); + assert(pLogData); - pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); - pLogData->status = sbgStreamBufferReadUint16LE(pStreamBuffer); - pLogData->timeOffset0 = sbgStreamBufferReadUint16LE(pStreamBuffer); - pLogData->timeOffset1 = sbgStreamBufferReadUint16LE(pStreamBuffer); - pLogData->timeOffset2 = sbgStreamBufferReadUint16LE(pStreamBuffer); - pLogData->timeOffset3 = sbgStreamBufferReadUint16LE(pStreamBuffer); + pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); + pLogData->status = sbgStreamBufferReadUint16LE(pStreamBuffer); + pLogData->timeOffset0 = sbgStreamBufferReadUint16LE(pStreamBuffer); + pLogData->timeOffset1 = sbgStreamBufferReadUint16LE(pStreamBuffer); + pLogData->timeOffset2 = sbgStreamBufferReadUint16LE(pStreamBuffer); + pLogData->timeOffset3 = sbgStreamBufferReadUint16LE(pStreamBuffer); - return sbgStreamBufferGetLastError(pStreamBuffer); + return sbgStreamBufferGetLastError(pStreamBuffer); } SbgErrorCode sbgEComLogEventWriteToStream(const SbgEComLogEvent *pLogData, SbgStreamBuffer *pStreamBuffer) { - assert(pStreamBuffer); - assert(pLogData); - - sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); - sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->status); - sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->timeOffset0); - sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->timeOffset1); - sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->timeOffset2); - sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->timeOffset3); - - return sbgStreamBufferGetLastError(pStreamBuffer); + assert(pStreamBuffer); + assert(pLogData); + + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); + sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->status); + sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->timeOffset0); + sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->timeOffset1); + sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->timeOffset2); + sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->timeOffset3); + + return sbgStreamBufferGetLastError(pStreamBuffer); } //----------------------------------------------------------------------// @@ -40,10 +40,10 @@ SbgErrorCode sbgEComLogEventWriteToStream(const SbgEComLogEvent *pLogData, SbgSt SbgErrorCode sbgEComBinaryLogParseEvent(SbgStreamBuffer *pStreamBuffer, SbgEComLogEvent *pLogData) { - return sbgEComLogEventReadFromStream(pLogData, pStreamBuffer); + return sbgEComLogEventReadFromStream(pLogData, pStreamBuffer); } SbgErrorCode sbgEComBinaryLogWriteEvent(SbgStreamBuffer *pStreamBuffer, const SbgEComLogEvent *pLogData) { - return sbgEComLogEventWriteToStream(pLogData, pStreamBuffer); + return sbgEComLogEventWriteToStream(pLogData, pStreamBuffer); } diff --git a/src/logs/sbgEComLogEvent.h b/src/logs/sbgEComLogEvent.h index 2e18707..f3d29e4 100644 --- a/src/logs/sbgEComLogEvent.h +++ b/src/logs/sbgEComLogEvent.h @@ -1,13 +1,13 @@ /*! - * \file sbgEComLogEvent.h - * \ingroup binaryLogs - * \author SBG Systems - * \date 28 October 2013 + * \file sbgEComLogEvent.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 28 October 2013 * - * \brief Parse event markers logs used to timestamp external signals. + * \brief Parse event markers logs used to timestamp external signals. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -48,11 +48,11 @@ extern "C" { /*! * 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. */ +#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 -// @@ -63,12 +63,12 @@ extern "C" { */ typedef struct _SbgEComLogEvent { - 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. */ + 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. */ } SbgEComLogEvent; //----------------------------------------------------------------------// @@ -78,18 +78,18 @@ typedef struct _SbgEComLogEvent /*! * Parse data for the SBG_ECOM_LOG_EVENT_# message and fill the corresponding structure. * - * \param[out] pLogData Log structure instance to fill. - * \param[in] pStreamBuffer Input stream buffer to read the log from. - * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. + * \param[out] pLogData Log structure instance to fill. + * \param[in] pStreamBuffer Input stream buffer to read the log from. + * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. */ SbgErrorCode sbgEComLogEventReadFromStream(SbgEComLogEvent *pLogData, SbgStreamBuffer *pStreamBuffer); /*! * Write data for the SBG_ECOM_LOG_EVENT_# message to the output stream buffer from the provided structure. * - * \param[in] pLogData Log structure instance to write. - * \param[out] pStreamBuffer Output stream buffer to write the log to. - * \return SBG_NO_ERROR if the log has been written to the stream buffer. + * \param[in] pLogData Log structure instance to write. + * \param[out] pStreamBuffer Output stream buffer to write the log to. + * \return SBG_NO_ERROR if the log has been written to the stream buffer. */ SbgErrorCode sbgEComLogEventWriteToStream(const SbgEComLogEvent *pLogData, SbgStreamBuffer *pStreamBuffer); diff --git a/src/logs/sbgEComLogGnssHdt.c b/src/logs/sbgEComLogGnssHdt.c index 7912e16..8dbf350 100644 --- a/src/logs/sbgEComLogGnssHdt.c +++ b/src/logs/sbgEComLogGnssHdt.c @@ -8,10 +8,10 @@ //----------------------------------------------------------------------// //- Private definitions -// //----------------------------------------------------------------------// -#define SBG_ECOM_LOG_GNSS_HDT_STATUS_SHIFT (0u) /*!< Shift used to extract the GNSS HDT status part. */ -#define SBG_ECOM_LOG_GNSS_HDT_STATUS_MASK (0x003Fu) /*!< Mask used to keep only the GNSS HDT status part. */ +#define SBG_ECOM_LOG_GNSS_HDT_STATUS_SHIFT (0u) /*!< Shift used to extract the GNSS HDT status part. */ +#define SBG_ECOM_LOG_GNSS_HDT_STATUS_MASK (0x003Fu) /*!< Mask used to keep only the GNSS HDT status part. */ -#define SBG_ECOM_LOG_GNSS_HDT_BASELINE_VALID (0x0001 << 6) /*!< Set to 1 if the baseline length field is filled and valid. */ +#define SBG_ECOM_LOG_GNSS_HDT_BASELINE_VALID (0x0001 << 6) /*!< Set to 1 if the baseline length field is filled and valid. */ //----------------------------------------------------------------------// //- Public methods -// @@ -19,84 +19,84 @@ void sbgEComLogGnssHdtZeroInit(SbgEComLogGnssHdt *pLogData) { - assert(pLogData); + assert(pLogData); - memset(pLogData, 0x00, sizeof(*pLogData)); + memset(pLogData, 0x00, sizeof(*pLogData)); - pLogData->headingAccuracy = 180.0f; - pLogData->pitchAccuracy = 90.0f; - pLogData->numSvTracked = UINT8_MAX; - pLogData->numSvUsed = UINT8_MAX; + pLogData->headingAccuracy = 180.0f; + pLogData->pitchAccuracy = 90.0f; + pLogData->numSvTracked = UINT8_MAX; + pLogData->numSvUsed = UINT8_MAX; - sbgEComLogGnssHdtSetStatus(pLogData, SBG_ECOM_GNSS_HDT_STATUS_INSUFFICIENT_OBS); + sbgEComLogGnssHdtSetStatus(pLogData, SBG_ECOM_GNSS_HDT_STATUS_INSUFFICIENT_OBS); } SbgErrorCode sbgEComLogGnssHdtReadFromStream(SbgEComLogGnssHdt *pLogData, SbgStreamBuffer *pStreamBuffer) { - assert(pLogData); - assert(pStreamBuffer); - - pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); - pLogData->status = sbgStreamBufferReadUint16LE(pStreamBuffer); - pLogData->timeOfWeek = sbgStreamBufferReadUint32LE(pStreamBuffer); - pLogData->heading = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->headingAccuracy = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->pitch = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->pitchAccuracy = sbgStreamBufferReadFloatLE(pStreamBuffer); - - // - // The baseline field have been added in version 2.0 - // - if (sbgStreamBufferGetSpace(pStreamBuffer) > 0) - { - pLogData->baseline = sbgStreamBufferReadFloatLE(pStreamBuffer); - } - else - { - pLogData->baseline = 0.0f; - } - - // - // Read numSvTracked and numSvUsed added in version 4.0 - // - if (sbgStreamBufferGetSpace(pStreamBuffer) > 0) - { - pLogData->numSvTracked = sbgStreamBufferReadUint8LE(pStreamBuffer); - } - else - { - pLogData->numSvTracked = UINT8_MAX; - } - - if (sbgStreamBufferGetSpace(pStreamBuffer) > 0) - { - pLogData->numSvUsed = sbgStreamBufferReadUint8LE(pStreamBuffer); - } - else - { - pLogData->numSvUsed = UINT8_MAX; - } - - return sbgStreamBufferGetLastError(pStreamBuffer); + assert(pLogData); + assert(pStreamBuffer); + + pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); + pLogData->status = sbgStreamBufferReadUint16LE(pStreamBuffer); + pLogData->timeOfWeek = sbgStreamBufferReadUint32LE(pStreamBuffer); + pLogData->heading = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->headingAccuracy = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->pitch = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->pitchAccuracy = sbgStreamBufferReadFloatLE(pStreamBuffer); + + // + // The baseline field have been added in version 2.0 + // + if (sbgStreamBufferGetSpace(pStreamBuffer) > 0) + { + pLogData->baseline = sbgStreamBufferReadFloatLE(pStreamBuffer); + } + else + { + pLogData->baseline = 0.0f; + } + + // + // Read numSvTracked and numSvUsed added in version 4.0 + // + if (sbgStreamBufferGetSpace(pStreamBuffer) > 0) + { + pLogData->numSvTracked = sbgStreamBufferReadUint8LE(pStreamBuffer); + } + else + { + pLogData->numSvTracked = UINT8_MAX; + } + + if (sbgStreamBufferGetSpace(pStreamBuffer) > 0) + { + pLogData->numSvUsed = sbgStreamBufferReadUint8LE(pStreamBuffer); + } + else + { + pLogData->numSvUsed = UINT8_MAX; + } + + return sbgStreamBufferGetLastError(pStreamBuffer); } SbgErrorCode sbgEComLogGnssHdtWriteToStream(const SbgEComLogGnssHdt *pLogData, SbgStreamBuffer *pStreamBuffer) { - assert(pLogData); - assert(pStreamBuffer); - - sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); - sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->status); - sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeOfWeek); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->heading); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->headingAccuracy); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->pitch); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->pitchAccuracy); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->baseline); - sbgStreamBufferWriteUint8LE(pStreamBuffer, pLogData->numSvTracked); - sbgStreamBufferWriteUint8LE(pStreamBuffer, pLogData->numSvUsed); - - return sbgStreamBufferGetLastError(pStreamBuffer); + assert(pLogData); + assert(pStreamBuffer); + + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); + sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->status); + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeOfWeek); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->heading); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->headingAccuracy); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->pitch); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->pitchAccuracy); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->baseline); + sbgStreamBufferWriteUint8LE(pStreamBuffer, pLogData->numSvTracked); + sbgStreamBufferWriteUint8LE(pStreamBuffer, pLogData->numSvUsed); + + return sbgStreamBufferGetLastError(pStreamBuffer); } //----------------------------------------------------------------------// @@ -105,102 +105,102 @@ SbgErrorCode sbgEComLogGnssHdtWriteToStream(const SbgEComLogGnssHdt *pLogData, S void sbgEComLogGnssHdtSetStatus(SbgEComLogGnssHdt *pLogData, SbgEComGnssHdtStatus status) { - assert(pLogData); - assert(status <= SBG_ECOM_LOG_GNSS_HDT_STATUS_MASK); + assert(pLogData); + assert(status <= SBG_ECOM_LOG_GNSS_HDT_STATUS_MASK); - pLogData->status &= ~(SBG_ECOM_LOG_GNSS_HDT_STATUS_MASK << SBG_ECOM_LOG_GNSS_HDT_STATUS_SHIFT); - pLogData->status |= ((uint16_t)status&SBG_ECOM_LOG_GNSS_HDT_STATUS_MASK) << SBG_ECOM_LOG_GNSS_HDT_STATUS_SHIFT; + pLogData->status &= ~(SBG_ECOM_LOG_GNSS_HDT_STATUS_MASK << SBG_ECOM_LOG_GNSS_HDT_STATUS_SHIFT); + pLogData->status |= ((uint16_t)status&SBG_ECOM_LOG_GNSS_HDT_STATUS_MASK) << SBG_ECOM_LOG_GNSS_HDT_STATUS_SHIFT; } SbgEComGnssHdtStatus sbgEComLogGnssHdtGetStatus(const SbgEComLogGnssHdt *pLogData) { - assert(pLogData); + assert(pLogData); - return (SbgEComGnssHdtStatus)((pLogData->status >> SBG_ECOM_LOG_GNSS_HDT_STATUS_SHIFT)&SBG_ECOM_LOG_GNSS_HDT_STATUS_MASK); + return (SbgEComGnssHdtStatus)((pLogData->status >> SBG_ECOM_LOG_GNSS_HDT_STATUS_SHIFT)&SBG_ECOM_LOG_GNSS_HDT_STATUS_MASK); } bool sbgEComLogGnssHdtHeadingIsValid(const SbgEComLogGnssHdt *pLogData) { - assert(pLogData); - - if ( (pLogData->headingAccuracy > 0.0f) && (pLogData->headingAccuracy < 180.0f) ) - { - return true; - } - else - { - return false; - } + assert(pLogData); + + if ( (pLogData->headingAccuracy > 0.0f) && (pLogData->headingAccuracy < 180.0f) ) + { + return true; + } + else + { + return false; + } } bool sbgEComLogGnssHdtPitchIsValid(const SbgEComLogGnssHdt *pLogData) { - assert(pLogData); - - if ( (pLogData->pitchAccuracy > 0.0f) && (pLogData->pitchAccuracy < 90.0f) ) - { - return true; - } - else - { - return false; - } + assert(pLogData); + + if ( (pLogData->pitchAccuracy > 0.0f) && (pLogData->pitchAccuracy < 90.0f) ) + { + return true; + } + else + { + return false; + } } void sbgEComLogGnssHdtSetBaseLineValid(SbgEComLogGnssHdt *pLogData, bool isValid) { - assert(pLogData); - - if (isValid) - { - pLogData->status |= SBG_ECOM_LOG_GNSS_HDT_BASELINE_VALID; - } - else - { - pLogData->status &= ~SBG_ECOM_LOG_GNSS_HDT_BASELINE_VALID; - } + assert(pLogData); + + if (isValid) + { + pLogData->status |= SBG_ECOM_LOG_GNSS_HDT_BASELINE_VALID; + } + else + { + pLogData->status &= ~SBG_ECOM_LOG_GNSS_HDT_BASELINE_VALID; + } } bool sbgEComLogGnssHdtBaseLineIsValid(const SbgEComLogGnssHdt *pLogData) { - assert(pLogData); - - if ( (pLogData->status & SBG_ECOM_LOG_GNSS_HDT_BASELINE_VALID) && (pLogData->baseline > 0.0f) ) - { - return true; - } - else - { - return false; - } + assert(pLogData); + + if ( (pLogData->status & SBG_ECOM_LOG_GNSS_HDT_BASELINE_VALID) && (pLogData->baseline > 0.0f) ) + { + return true; + } + else + { + return false; + } } bool sbgEComLogGnssHdtNumSvTrackedIsValid(const SbgEComLogGnssHdt *pLogData) { - assert(pLogData); - - if (pLogData->numSvTracked == UINT8_MAX) - { - return false; - } - else - { - return true; - } + assert(pLogData); + + if (pLogData->numSvTracked == UINT8_MAX) + { + return false; + } + else + { + return true; + } } bool sbgEComLogGnssHdtNumSvUsedIsValid(const SbgEComLogGnssHdt *pLogData) { - assert(pLogData); - - if (pLogData->numSvUsed == UINT8_MAX) - { - return false; - } - else - { - return true; - } + assert(pLogData); + + if (pLogData->numSvUsed == UINT8_MAX) + { + return false; + } + else + { + return true; + } } //----------------------------------------------------------------------// @@ -209,20 +209,20 @@ bool sbgEComLogGnssHdtNumSvUsedIsValid(const SbgEComLogGnssHdt *pLogData) uint32_t sbgEComLogGpsHdtBuildStatus(SbgEComGnssHdtStatus status, uint32_t masks) { - return (((uint32_t)status&SBG_ECOM_LOG_GNSS_HDT_STATUS_MASK) << SBG_ECOM_LOG_GNSS_HDT_STATUS_SHIFT) | masks; + return (((uint32_t)status&SBG_ECOM_LOG_GNSS_HDT_STATUS_MASK) << SBG_ECOM_LOG_GNSS_HDT_STATUS_SHIFT) | masks; } SbgEComGnssHdtStatus sbgEComLogGpsHdtGetStatus(uint32_t status) { - return (SbgEComGnssHdtStatus)((status >> SBG_ECOM_LOG_GNSS_HDT_STATUS_SHIFT) & SBG_ECOM_LOG_GNSS_HDT_STATUS_MASK); + return (SbgEComGnssHdtStatus)((status >> SBG_ECOM_LOG_GNSS_HDT_STATUS_SHIFT) & SBG_ECOM_LOG_GNSS_HDT_STATUS_MASK); } SbgErrorCode sbgEComBinaryLogParseGpsHdtData(SbgStreamBuffer *pStreamBuffer, SbgEComLogGnssHdt *pLogData) { - return sbgEComLogGnssHdtReadFromStream(pLogData, pStreamBuffer); + return sbgEComLogGnssHdtReadFromStream(pLogData, pStreamBuffer); } SbgErrorCode sbgEComBinaryLogWriteGpsHdtData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogGnssHdt *pLogData) { - return sbgEComLogGnssHdtWriteToStream(pLogData, pStreamBuffer); + return sbgEComLogGnssHdtWriteToStream(pLogData, pStreamBuffer); } diff --git a/src/logs/sbgEComLogGnssHdt.h b/src/logs/sbgEComLogGnssHdt.h index 8f552bd..673ae19 100644 --- a/src/logs/sbgEComLogGnssHdt.h +++ b/src/logs/sbgEComLogGnssHdt.h @@ -1,13 +1,13 @@ /*! - * \file sbgEComLogGnssHdt.h - * \ingroup binaryLogs - * \author SBG Systems - * \date 05 May 2023 + * \file sbgEComLogGnssHdt.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 05 May 2023 * - * \brief GNSS True Heading related logs. + * \brief GNSS True Heading related logs. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -50,10 +50,10 @@ extern "C" { */ typedef enum _SbgEComGnssHdtStatus { - SBG_ECOM_GNSS_HDT_STATUS_SOL_COMPUTED = 0, /*!< A valid solution has been computed. */ - SBG_ECOM_GNSS_HDT_STATUS_INSUFFICIENT_OBS = 1, /*!< Not enough valid SV to compute a solution. */ - SBG_ECOM_GNSS_HDT_STATUS_INTERNAL_ERROR = 2, /*!< An internal error has occurred. */ - SBG_ECOM_GNSS_HDT_STATUS_HEIGHT_LIMIT = 3 /*!< The height limit has been exceeded. */ + SBG_ECOM_GNSS_HDT_STATUS_SOL_COMPUTED = 0, /*!< A valid solution has been computed. */ + SBG_ECOM_GNSS_HDT_STATUS_INSUFFICIENT_OBS = 1, /*!< Not enough valid SV to compute a solution. */ + SBG_ECOM_GNSS_HDT_STATUS_INTERNAL_ERROR = 2, /*!< An internal error has occurred. */ + SBG_ECOM_GNSS_HDT_STATUS_HEIGHT_LIMIT = 3 /*!< The height limit has been exceeded. */ } SbgEComGnssHdtStatus; //----------------------------------------------------------------------// @@ -65,16 +65,16 @@ typedef enum _SbgEComGnssHdtStatus */ typedef struct _SbgEComLogGnssHdt { - 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 (0 to 360). */ - float headingAccuracy; /*!< 1 sigma GPS true heading accuracy in degrees (0 to 180). */ - float pitch; /*!< GPS pitch angle measured from the master to the rover in degrees (-90 to +90). */ - float pitchAccuracy; /*!< 1 sigma GPS pitch angle accuracy in degrees (0 to 90). */ - float baseline; /*!< The distance between the main and aux antenna in meters (added in 2.0) */ - uint8_t numSvTracked; /*!< Number of space vehicles tracked by the secondary GNSS antenna - set to 0xFF if not available. (added in 4.0) */ - uint8_t numSvUsed; /*!< Number of space vehicles used to compute the dual antenna solution - set to 0xFF if not available. (added in 4.0) */ + 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 (0 to 360). */ + float headingAccuracy; /*!< 1 sigma GPS true heading accuracy in degrees (0 to 180). */ + float pitch; /*!< GPS pitch angle measured from the master to the rover in degrees (-90 to +90). */ + float pitchAccuracy; /*!< 1 sigma GPS pitch angle accuracy in degrees (0 to 90). */ + float baseline; /*!< The distance between the main and aux antenna in meters (added in 2.0) */ + uint8_t numSvTracked; /*!< Number of space vehicles tracked by the secondary GNSS antenna - set to 0xFF if not available. (added in 4.0) */ + uint8_t numSvUsed; /*!< Number of space vehicles used to compute the dual antenna solution - set to 0xFF if not available. (added in 4.0) */ } SbgEComLogGnssHdt; //----------------------------------------------------------------------// @@ -84,25 +84,25 @@ typedef struct _SbgEComLogGnssHdt /*! * Zero initialize the message struct. * - * \param[out] pLogData Structure instance to zero init. + * \param[out] pLogData Structure instance to zero init. */ void sbgEComLogGnssHdtZeroInit(SbgEComLogGnssHdt *pLogData); /*! * Parse data for the SBG_ECOM_LOG_GPS#_HDT message and fill the corresponding structure. * - * \param[out] pLogData Log structure instance to fill. - * \param[in] pStreamBuffer Input stream buffer to read the log from. - * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. + * \param[out] pLogData Log structure instance to fill. + * \param[in] pStreamBuffer Input stream buffer to read the log from. + * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. */ SbgErrorCode sbgEComLogGnssHdtReadFromStream(SbgEComLogGnssHdt *pLogData, SbgStreamBuffer *pStreamBuffer); /*! * Write data for the SBG_ECOM_LOG_GPS#_HDT message to the output stream buffer from the provided structure. * - * \param[in] pLogData Log structure instance to write. - * \param[out] pStreamBuffer Output stream buffer to write the log to. - * \return SBG_NO_ERROR if the log has been written to the stream buffer. + * \param[in] pLogData Log structure instance to write. + * \param[out] pStreamBuffer Output stream buffer to write the log to. + * \return SBG_NO_ERROR if the log has been written to the stream buffer. */ SbgErrorCode sbgEComLogGnssHdtWriteToStream(const SbgEComLogGnssHdt *pLogData, SbgStreamBuffer *pStreamBuffer); @@ -113,24 +113,24 @@ SbgErrorCode sbgEComLogGnssHdtWriteToStream(const SbgEComLogGnssHdt *pLogData, S /*! * Set the true heading solution status. * - * \param[in] pLogData Log instance. - * \param[in] status The solution status to set. + * \param[in] pLogData Log instance. + * \param[in] status The solution status to set. */ void sbgEComLogGnssHdtSetStatus(SbgEComLogGnssHdt *pLogData, SbgEComGnssHdtStatus status); /*! * Returns the true heading solution status. * - * \param[in] pLogData Log instance. - * \return The true heading solution status. + * \param[in] pLogData Log instance. + * \return The true heading solution status. */ SbgEComGnssHdtStatus sbgEComLogGnssHdtGetStatus(const SbgEComLogGnssHdt *pLogData); /*! * Returns true if the heading information is valid. * - * \param[in] pLogData Log instance. - * \return true if the heading information is valid or false otherwise. + * \param[in] pLogData Log instance. + * \return true if the heading information is valid or false otherwise. */ bool sbgEComLogGnssHdtHeadingIsValid(const SbgEComLogGnssHdt *pLogData); @@ -141,8 +141,8 @@ bool sbgEComLogGnssHdtHeadingIsValid(const SbgEComLogGnssHdt *pLogData); * This methods checks if a valid pitch measurement is available with * an associated standard deviation. * - * \param[in] pLogData Log instance. - * \return true if the pitch information is valid or false otherwise. + * \param[in] pLogData Log instance. + * \return true if the pitch information is valid or false otherwise. */ bool sbgEComLogGnssHdtPitchIsValid(const SbgEComLogGnssHdt *pLogData); @@ -152,32 +152,32 @@ bool sbgEComLogGnssHdtPitchIsValid(const SbgEComLogGnssHdt *pLogData); * Some GNSS receivers don't provide the baseline information between antenna 1 and 2. * Set this flag to false to report this. * - * \param[in] pLogData Log instance. - * \param[in] isValid true if the baseline is valid. + * \param[in] pLogData Log instance. + * \param[in] isValid true if the baseline is valid. */ void sbgEComLogGnssHdtSetBaseLineValid(SbgEComLogGnssHdt *pLogData, bool isValid); /*! * Returns true if the baseline field is valid. * - * \param[in] pLogData Log instance. - * \return true if the baseline field is valid or false otherwise. + * \param[in] pLogData Log instance. + * \return true if the baseline field is valid or false otherwise. */ bool sbgEComLogGnssHdtBaseLineIsValid(const SbgEComLogGnssHdt *pLogData); /*! * Returns true if the numSvTracked field is valid. * - * \param[in] pLogData Log instance. - * \return true if the numSvTracked field is valid or false otherwise. + * \param[in] pLogData Log instance. + * \return true if the numSvTracked field is valid or false otherwise. */ bool sbgEComLogGnssHdtNumSvTrackedIsValid(const SbgEComLogGnssHdt *pLogData); /*! * Returns true if the numSvUsed field is valid. * - * \param[in] pLogData Log instance. - * \return true if the numSvUsed field is valid or false otherwise. + * \param[in] pLogData Log instance. + * \return true if the numSvUsed field is valid or false otherwise. */ bool sbgEComLogGnssHdtNumSvUsedIsValid(const SbgEComLogGnssHdt *pLogData); @@ -186,16 +186,16 @@ bool sbgEComLogGnssHdtNumSvUsedIsValid(const SbgEComLogGnssHdt *pLogData); //----------------------------------------------------------------------// #ifdef SBG_ECOM_USE_DEPRECATED_MACROS - #define SBG_ECOM_GPS_HDT_BASELINE_VALID (0x0001 << 6) + #define SBG_ECOM_GPS_HDT_BASELINE_VALID (0x0001 << 6) - #define SBG_ECOM_HDT_SOL_COMPUTED (SBG_ECOM_GNSS_HDT_STATUS_SOL_COMPUTED) - #define SBG_ECOM_HDT_INSUFFICIENT_OBS (SBG_ECOM_GNSS_HDT_STATUS_INSUFFICIENT_OBS) - #define SBG_ECOM_HDT_INTERNAL_ERROR (SBG_ECOM_GNSS_HDT_STATUS_INTERNAL_ERROR) - #define SBG_ECOM_HDT_HEIGHT_LIMIT (SBG_ECOM_GNSS_HDT_STATUS_HEIGHT_LIMIT) + #define SBG_ECOM_HDT_SOL_COMPUTED (SBG_ECOM_GNSS_HDT_STATUS_SOL_COMPUTED) + #define SBG_ECOM_HDT_INSUFFICIENT_OBS (SBG_ECOM_GNSS_HDT_STATUS_INSUFFICIENT_OBS) + #define SBG_ECOM_HDT_INTERNAL_ERROR (SBG_ECOM_GNSS_HDT_STATUS_INTERNAL_ERROR) + #define SBG_ECOM_HDT_HEIGHT_LIMIT (SBG_ECOM_GNSS_HDT_STATUS_HEIGHT_LIMIT) #endif -SBG_DEPRECATED_TYPEDEF(typedef enum _SbgEComGnssHdtStatus SbgEComGpsHdtStatus); -SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogGnssHdt SbgLogGpsHdt); +SBG_DEPRECATED_TYPEDEF(typedef enum _SbgEComGnssHdtStatus SbgEComGpsHdtStatus); +SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogGnssHdt SbgLogGpsHdt); SBG_DEPRECATED(uint32_t sbgEComLogGpsHdtBuildStatus(SbgEComGnssHdtStatus status, uint32_t masks)); SBG_DEPRECATED(SbgEComGnssHdtStatus sbgEComLogGpsHdtGetStatus(uint32_t status)); diff --git a/src/logs/sbgEComLogGnssPos.c b/src/logs/sbgEComLogGnssPos.c index 91addc0..eb75c69 100644 --- a/src/logs/sbgEComLogGnssPos.c +++ b/src/logs/sbgEComLogGnssPos.c @@ -8,25 +8,25 @@ //----------------------------------------------------------------------// //- Private definitions for status field -// //----------------------------------------------------------------------// -#define SBG_ECOM_LOG_GNSS_POS_STATUS_SHIFT (0u) /*!< Shift used to extract the GNSS position status part. */ -#define SBG_ECOM_LOG_GNSS_POS_STATUS_MASK (0x0000003Fu) /*!< Mask used to keep only the GNSS position status part. */ +#define SBG_ECOM_LOG_GNSS_POS_STATUS_SHIFT (0u) /*!< Shift used to extract the GNSS position status part. */ +#define SBG_ECOM_LOG_GNSS_POS_STATUS_MASK (0x0000003Fu) /*!< Mask used to keep only the GNSS position status part. */ -#define SBG_ECOM_LOG_GNSS_POS_TYPE_SHIFT (6u) /*!< Shift used to extract the GNSS position type part. */ -#define SBG_ECOM_LOG_GNSS_POS_TYPE_MASK (0x0000003Fu) /*!< Mask used to keep only the GNSS position type part. */ +#define SBG_ECOM_LOG_GNSS_POS_TYPE_SHIFT (6u) /*!< Shift used to extract the GNSS position type part. */ +#define SBG_ECOM_LOG_GNSS_POS_TYPE_MASK (0x0000003Fu) /*!< Mask used to keep only the GNSS position type part. */ -#define SBG_ECOM_LOG_GNSS_POS_SIGNALS_MASK (0xFFFFF000u) /*!< Mask used to keep only the signals used in solution part. */ +#define SBG_ECOM_LOG_GNSS_POS_SIGNALS_MASK (0xFFFFF000u) /*!< Mask used to keep only the signals used in solution part. */ //----------------------------------------------------------------------// //- Private definitions for statusExt field -// //----------------------------------------------------------------------// -#define SBG_ECOM_LOG_GNSS_POS_IFM_SHIFT (0u) /*!< Shift used to extract the GNSS interference monitoring and mitigation indicator. */ -#define SBG_ECOM_LOG_GNSS_POS_IFM_MASK (0x0000000Fu) /*!< Mask used to keep only the GNSS interference monitoring and mitigation indicator. */ +#define SBG_ECOM_LOG_GNSS_POS_IFM_SHIFT (0u) /*!< Shift used to extract the GNSS interference monitoring and mitigation indicator. */ +#define SBG_ECOM_LOG_GNSS_POS_IFM_MASK (0x0000000Fu) /*!< Mask used to keep only the GNSS interference monitoring and mitigation indicator. */ -#define SBG_ECOM_LOG_GNSS_POS_SPOOFING_SHIFT (4u) /*!< Shift used to extract the GNSS spoofing indicator. */ -#define SBG_ECOM_LOG_GNSS_POS_SPOOFING_MASK (0x0000000Fu) /*!< Mask used to keep only the GNSS spoofing indicator. */ +#define SBG_ECOM_LOG_GNSS_POS_SPOOFING_SHIFT (4u) /*!< Shift used to extract the GNSS spoofing indicator. */ +#define SBG_ECOM_LOG_GNSS_POS_SPOOFING_MASK (0x0000000Fu) /*!< Mask used to keep only the GNSS spoofing indicator. */ -#define SBG_ECOM_LOG_GNSS_POS_OSNMA_SHIFT (8u) /*!< Shift used to extract the Galileo OSNMA status. */ -#define SBG_ECOM_LOG_GNSS_POS_OSNMA_MASK (0x0000000Fu) /*!< Mask used to keep only the Galileo OSNMA status. */ +#define SBG_ECOM_LOG_GNSS_POS_OSNMA_SHIFT (8u) /*!< Shift used to extract the Galileo OSNMA status. */ +#define SBG_ECOM_LOG_GNSS_POS_OSNMA_MASK (0x0000000Fu) /*!< Mask used to keep only the Galileo OSNMA status. */ //----------------------------------------------------------------------// //- Public methods -// @@ -34,110 +34,110 @@ void sbgEComLogGnssPosZeroInit(SbgEComLogGnssPos *pLogData) { - assert(pLogData); + assert(pLogData); - memset(pLogData, 0x00, sizeof(*pLogData)); + memset(pLogData, 0x00, sizeof(*pLogData)); - pLogData->latitudeAccuracy = 9999.0f; - pLogData->longitudeAccuracy = 9999.0f; - pLogData->altitudeAccuracy = 9999.0f; - pLogData->numSvTracked = UINT8_MAX; - pLogData->numSvUsed = UINT8_MAX; - pLogData->baseStationId = UINT16_MAX; - pLogData->differentialAge = UINT16_MAX; + pLogData->latitudeAccuracy = 9999.0f; + pLogData->longitudeAccuracy = 9999.0f; + pLogData->altitudeAccuracy = 9999.0f; + pLogData->numSvTracked = UINT8_MAX; + pLogData->numSvUsed = UINT8_MAX; + pLogData->baseStationId = UINT16_MAX; + pLogData->differentialAge = UINT16_MAX; - sbgEComLogGnssPosSetStatus(pLogData, SBG_ECOM_GNSS_POS_STATUS_INSUFFICIENT_OBS); - sbgEComLogGnssPosSetType(pLogData, SBG_ECOM_GNSS_POS_TYPE_NO_SOLUTION); + sbgEComLogGnssPosSetStatus(pLogData, SBG_ECOM_GNSS_POS_STATUS_INSUFFICIENT_OBS); + sbgEComLogGnssPosSetType(pLogData, SBG_ECOM_GNSS_POS_TYPE_NO_SOLUTION); - sbgEComLogGnssPosSetIfmStatus(pLogData, SBG_ECOM_GNSS_IFM_STATUS_UNKNOWN); - sbgEComLogGnssPosSetSpoofingStatus(pLogData, SBG_ECOM_GNSS_SPOOFING_STATUS_UNKNOWN); - sbgEComLogGnssPosSetOsnmaStatus(pLogData, SBG_ECOM_GNSS_OSNMA_STATUS_DISABLED); + sbgEComLogGnssPosSetIfmStatus(pLogData, SBG_ECOM_GNSS_IFM_STATUS_UNKNOWN); + sbgEComLogGnssPosSetSpoofingStatus(pLogData, SBG_ECOM_GNSS_SPOOFING_STATUS_UNKNOWN); + sbgEComLogGnssPosSetOsnmaStatus(pLogData, SBG_ECOM_GNSS_OSNMA_STATUS_DISABLED); } SbgErrorCode sbgEComLogGnssPosReadFromStream(SbgEComLogGnssPos *pLogData, SbgStreamBuffer *pStreamBuffer) { - bool shouldContinue; - - assert(pLogData); - assert(pStreamBuffer); - - pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); - pLogData->status = sbgStreamBufferReadUint32LE(pStreamBuffer); - pLogData->timeOfWeek = sbgStreamBufferReadUint32LE(pStreamBuffer); - pLogData->latitude = sbgStreamBufferReadDoubleLE(pStreamBuffer); - pLogData->longitude = sbgStreamBufferReadDoubleLE(pStreamBuffer); - pLogData->altitude = sbgStreamBufferReadDoubleLE(pStreamBuffer); - pLogData->undulation = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->latitudeAccuracy = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->longitudeAccuracy = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->altitudeAccuracy = sbgStreamBufferReadFloatLE(pStreamBuffer); - - // - // Read additional fields added in version 1.4 - // - if (sbgStreamBufferGetSpace(pStreamBuffer) >= 5) - { - pLogData->numSvUsed = sbgStreamBufferReadUint8LE(pStreamBuffer); - pLogData->baseStationId = sbgStreamBufferReadUint16LE(pStreamBuffer); - pLogData->differentialAge = sbgStreamBufferReadUint16LE(pStreamBuffer); - shouldContinue = true; - } - else - { - pLogData->numSvUsed = UINT8_MAX; - pLogData->baseStationId = UINT16_MAX; - pLogData->differentialAge = UINT16_MAX; - shouldContinue = false; - } - - // - // Read additional status added in version 4.0 - // - if ( shouldContinue && (sbgStreamBufferGetSpace(pStreamBuffer) >= 5) ) - { - pLogData->numSvTracked = sbgStreamBufferReadUint8LE(pStreamBuffer); - pLogData->statusExt = sbgStreamBufferReadUint32LE(pStreamBuffer); - } - else - { - pLogData->numSvTracked = UINT8_MAX; - pLogData->statusExt = 0; - - sbgEComLogGnssPosSetIfmStatus(pLogData, SBG_ECOM_GNSS_IFM_STATUS_UNKNOWN); - sbgEComLogGnssPosSetSpoofingStatus(pLogData, SBG_ECOM_GNSS_SPOOFING_STATUS_UNKNOWN); - sbgEComLogGnssPosSetOsnmaStatus(pLogData, SBG_ECOM_GNSS_OSNMA_STATUS_DISABLED); - } - - return sbgStreamBufferGetLastError(pStreamBuffer); + bool shouldContinue; + + assert(pLogData); + assert(pStreamBuffer); + + pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); + pLogData->status = sbgStreamBufferReadUint32LE(pStreamBuffer); + pLogData->timeOfWeek = sbgStreamBufferReadUint32LE(pStreamBuffer); + pLogData->latitude = sbgStreamBufferReadDoubleLE(pStreamBuffer); + pLogData->longitude = sbgStreamBufferReadDoubleLE(pStreamBuffer); + pLogData->altitude = sbgStreamBufferReadDoubleLE(pStreamBuffer); + pLogData->undulation = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->latitudeAccuracy = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->longitudeAccuracy = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->altitudeAccuracy = sbgStreamBufferReadFloatLE(pStreamBuffer); + + // + // Read additional fields added in version 1.4 + // + if (sbgStreamBufferGetSpace(pStreamBuffer) >= 5) + { + pLogData->numSvUsed = sbgStreamBufferReadUint8LE(pStreamBuffer); + pLogData->baseStationId = sbgStreamBufferReadUint16LE(pStreamBuffer); + pLogData->differentialAge = sbgStreamBufferReadUint16LE(pStreamBuffer); + shouldContinue = true; + } + else + { + pLogData->numSvUsed = UINT8_MAX; + pLogData->baseStationId = UINT16_MAX; + pLogData->differentialAge = UINT16_MAX; + shouldContinue = false; + } + + // + // Read additional status added in version 4.0 + // + if ( shouldContinue && (sbgStreamBufferGetSpace(pStreamBuffer) >= 5) ) + { + pLogData->numSvTracked = sbgStreamBufferReadUint8LE(pStreamBuffer); + pLogData->statusExt = sbgStreamBufferReadUint32LE(pStreamBuffer); + } + else + { + pLogData->numSvTracked = UINT8_MAX; + pLogData->statusExt = 0; + + sbgEComLogGnssPosSetIfmStatus(pLogData, SBG_ECOM_GNSS_IFM_STATUS_UNKNOWN); + sbgEComLogGnssPosSetSpoofingStatus(pLogData, SBG_ECOM_GNSS_SPOOFING_STATUS_UNKNOWN); + sbgEComLogGnssPosSetOsnmaStatus(pLogData, SBG_ECOM_GNSS_OSNMA_STATUS_DISABLED); + } + + return sbgStreamBufferGetLastError(pStreamBuffer); } SbgErrorCode sbgEComLogGnssPosWriteToStream(const SbgEComLogGnssPos *pLogData, SbgStreamBuffer *pStreamBuffer) { - assert(pLogData); - assert(pStreamBuffer); + assert(pLogData); + assert(pStreamBuffer); - sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); - sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->status); - sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeOfWeek); + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->status); + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeOfWeek); - sbgStreamBufferWriteDoubleLE(pStreamBuffer, pLogData->latitude); - sbgStreamBufferWriteDoubleLE(pStreamBuffer, pLogData->longitude); - sbgStreamBufferWriteDoubleLE(pStreamBuffer, pLogData->altitude); + sbgStreamBufferWriteDoubleLE(pStreamBuffer, pLogData->latitude); + sbgStreamBufferWriteDoubleLE(pStreamBuffer, pLogData->longitude); + sbgStreamBufferWriteDoubleLE(pStreamBuffer, pLogData->altitude); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->undulation); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->undulation); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->latitudeAccuracy); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->longitudeAccuracy); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->altitudeAccuracy); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->latitudeAccuracy); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->longitudeAccuracy); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->altitudeAccuracy); - sbgStreamBufferWriteUint8LE(pStreamBuffer, pLogData->numSvUsed); - sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->baseStationId); - sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->differentialAge); + sbgStreamBufferWriteUint8LE(pStreamBuffer, pLogData->numSvUsed); + sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->baseStationId); + sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->differentialAge); - sbgStreamBufferWriteUint8LE(pStreamBuffer, pLogData->numSvTracked); - sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->statusExt); + sbgStreamBufferWriteUint8LE(pStreamBuffer, pLogData->numSvTracked); + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->statusExt); - return sbgStreamBufferGetLastError(pStreamBuffer); + return sbgStreamBufferGetLastError(pStreamBuffer); } //----------------------------------------------------------------------// @@ -146,200 +146,200 @@ SbgErrorCode sbgEComLogGnssPosWriteToStream(const SbgEComLogGnssPos *pLogData, S void sbgEComLogGnssPosSetStatus(SbgEComLogGnssPos *pLogData, SbgEComGnssPosStatus status) { - assert(pLogData); - assert(status <= SBG_ECOM_LOG_GNSS_POS_STATUS_MASK); + assert(pLogData); + assert(status <= SBG_ECOM_LOG_GNSS_POS_STATUS_MASK); - pLogData->status &= ~(SBG_ECOM_LOG_GNSS_POS_STATUS_MASK << SBG_ECOM_LOG_GNSS_POS_STATUS_SHIFT); - pLogData->status |= ((uint32_t)status&SBG_ECOM_LOG_GNSS_POS_STATUS_MASK) << SBG_ECOM_LOG_GNSS_POS_STATUS_SHIFT; + pLogData->status &= ~(SBG_ECOM_LOG_GNSS_POS_STATUS_MASK << SBG_ECOM_LOG_GNSS_POS_STATUS_SHIFT); + pLogData->status |= ((uint32_t)status&SBG_ECOM_LOG_GNSS_POS_STATUS_MASK) << SBG_ECOM_LOG_GNSS_POS_STATUS_SHIFT; } SbgEComGnssPosStatus sbgEComLogGnssPosGetStatus(const SbgEComLogGnssPos *pLogData) { - assert(pLogData); + assert(pLogData); - return (SbgEComGnssPosStatus)((pLogData->status >> SBG_ECOM_LOG_GNSS_POS_STATUS_SHIFT)&SBG_ECOM_LOG_GNSS_POS_STATUS_MASK); + return (SbgEComGnssPosStatus)((pLogData->status >> SBG_ECOM_LOG_GNSS_POS_STATUS_SHIFT)&SBG_ECOM_LOG_GNSS_POS_STATUS_MASK); } void sbgEComLogGnssPosSetType(SbgEComLogGnssPos *pLogData, SbgEComGnssPosType posType) { - assert(pLogData); - assert(posType <= SBG_ECOM_LOG_GNSS_POS_TYPE_MASK); + assert(pLogData); + assert(posType <= SBG_ECOM_LOG_GNSS_POS_TYPE_MASK); - pLogData->status &= ~(SBG_ECOM_LOG_GNSS_POS_TYPE_MASK << SBG_ECOM_LOG_GNSS_POS_TYPE_SHIFT); - pLogData->status |= ((uint32_t)posType&SBG_ECOM_LOG_GNSS_POS_TYPE_MASK) << SBG_ECOM_LOG_GNSS_POS_TYPE_SHIFT; + pLogData->status &= ~(SBG_ECOM_LOG_GNSS_POS_TYPE_MASK << SBG_ECOM_LOG_GNSS_POS_TYPE_SHIFT); + pLogData->status |= ((uint32_t)posType&SBG_ECOM_LOG_GNSS_POS_TYPE_MASK) << SBG_ECOM_LOG_GNSS_POS_TYPE_SHIFT; } SbgEComGnssPosType sbgEComLogGnssPosGetType(const SbgEComLogGnssPos *pLogData) { - assert(pLogData); + assert(pLogData); - return (SbgEComGnssPosType)((pLogData->status >> SBG_ECOM_LOG_GNSS_POS_TYPE_SHIFT)&SBG_ECOM_LOG_GNSS_POS_TYPE_MASK); + return (SbgEComGnssPosType)((pLogData->status >> SBG_ECOM_LOG_GNSS_POS_TYPE_SHIFT)&SBG_ECOM_LOG_GNSS_POS_TYPE_MASK); } void sbgEComLogGnssPosSetSignalsUsed(SbgEComLogGnssPos *pLogData, uint32_t signalMask) { - assert(pLogData); - assert((signalMask&SBG_ECOM_LOG_GNSS_POS_SIGNALS_MASK) == signalMask); + assert(pLogData); + assert((signalMask&SBG_ECOM_LOG_GNSS_POS_SIGNALS_MASK) == signalMask); - pLogData->status &= ~SBG_ECOM_LOG_GNSS_POS_SIGNALS_MASK; - pLogData->status |= signalMask&SBG_ECOM_LOG_GNSS_POS_SIGNALS_MASK; + pLogData->status &= ~SBG_ECOM_LOG_GNSS_POS_SIGNALS_MASK; + pLogData->status |= signalMask&SBG_ECOM_LOG_GNSS_POS_SIGNALS_MASK; } bool sbgEComLogGnssPosSignalsAreUsed(const SbgEComLogGnssPos *pLogData, uint32_t signalsMask) { - assert(pLogData); - - if ((pLogData->status & signalsMask) == signalsMask) - { - return true; - } - else - { - return false; - } + assert(pLogData); + + if ((pLogData->status & signalsMask) == signalsMask) + { + return true; + } + else + { + return false; + } } bool sbgEComLogGnssPosNumSvTrackedIsValid(const SbgEComLogGnssPos *pLogData) { - assert(pLogData); - - if (pLogData->numSvTracked == UINT8_MAX) - { - return false; - } - else - { - return true; - } + assert(pLogData); + + if (pLogData->numSvTracked == UINT8_MAX) + { + return false; + } + else + { + return true; + } } bool sbgEComLogGnssPosNumSvUsedIsValid(const SbgEComLogGnssPos *pLogData) { - assert(pLogData); - - if (pLogData->numSvUsed == UINT8_MAX) - { - return false; - } - else - { - return true; - } + assert(pLogData); + + if (pLogData->numSvUsed == UINT8_MAX) + { + return false; + } + else + { + return true; + } } bool sbgEComLogGnssPosBaseStationIdIsValid(const SbgEComLogGnssPos *pLogData) { - assert(pLogData); - - if (pLogData->baseStationId == UINT16_MAX) - { - return false; - } - else - { - return true; - } + assert(pLogData); + + if (pLogData->baseStationId == UINT16_MAX) + { + return false; + } + else + { + return true; + } } bool sbgEComLogGnssPosDifferentialAgeIsValid(const SbgEComLogGnssPos *pLogData) { - assert(pLogData); - - if (pLogData->differentialAge == UINT16_MAX) - { - return false; - } - else - { - return true; - } + assert(pLogData); + + if (pLogData->differentialAge == UINT16_MAX) + { + return false; + } + else + { + return true; + } } void sbgEComLogGnssPosSetDifferentialAge(SbgEComLogGnssPos *pLogData, float differentialAge) { - assert(pLogData); - assert((isnan(differentialAge) != 0) || (differentialAge >= 0.0f)); - - if (isnan(differentialAge) == 0) - { - float diffAgeScaled; - - diffAgeScaled = differentialAge / 100.0f; - - if (diffAgeScaled >= (float)UINT16_MAX) - { - pLogData->differentialAge = UINT16_MAX; - } - else - { - pLogData->differentialAge = (uint16_t)diffAgeScaled; - } - } - else - { - pLogData->differentialAge = UINT16_MAX; - } + assert(pLogData); + assert((isnan(differentialAge) != 0) || (differentialAge >= 0.0f)); + + if (isnan(differentialAge) == 0) + { + float diffAgeScaled; + + diffAgeScaled = differentialAge / 100.0f; + + if (diffAgeScaled >= (float)UINT16_MAX) + { + pLogData->differentialAge = UINT16_MAX; + } + else + { + pLogData->differentialAge = (uint16_t)diffAgeScaled; + } + } + else + { + pLogData->differentialAge = UINT16_MAX; + } } float sbgEComLogGnssPosGetDifferentialAge(const SbgEComLogGnssPos *pLogData) { - float diffAgeSeconds; + float diffAgeSeconds; - assert(pLogData); + assert(pLogData); - if (pLogData->differentialAge == UINT16_MAX) - { - diffAgeSeconds = NAN; - } - else - { - diffAgeSeconds = pLogData->differentialAge / 100.0f; - } + if (pLogData->differentialAge == UINT16_MAX) + { + diffAgeSeconds = NAN; + } + else + { + diffAgeSeconds = pLogData->differentialAge / 100.0f; + } - return diffAgeSeconds; + return diffAgeSeconds; } void sbgEComLogGnssPosSetIfmStatus(SbgEComLogGnssPos *pLogData, SbgEComGnssIfmStatus status) { - assert(pLogData); - assert(status <= SBG_ECOM_LOG_GNSS_POS_IFM_MASK); + assert(pLogData); + assert(status <= SBG_ECOM_LOG_GNSS_POS_IFM_MASK); - pLogData->statusExt &= ~(SBG_ECOM_LOG_GNSS_POS_IFM_MASK << SBG_ECOM_LOG_GNSS_POS_IFM_SHIFT); - pLogData->statusExt |= ((uint32_t)status&SBG_ECOM_LOG_GNSS_POS_IFM_MASK) << SBG_ECOM_LOG_GNSS_POS_IFM_SHIFT; + pLogData->statusExt &= ~(SBG_ECOM_LOG_GNSS_POS_IFM_MASK << SBG_ECOM_LOG_GNSS_POS_IFM_SHIFT); + pLogData->statusExt |= ((uint32_t)status&SBG_ECOM_LOG_GNSS_POS_IFM_MASK) << SBG_ECOM_LOG_GNSS_POS_IFM_SHIFT; } SbgEComGnssIfmStatus sbgEComLogGnssPosGetIfmStatus(const SbgEComLogGnssPos *pLogData) { - assert(pLogData); + assert(pLogData); - return (SbgEComGnssIfmStatus)((pLogData->statusExt >> SBG_ECOM_LOG_GNSS_POS_IFM_SHIFT)&SBG_ECOM_LOG_GNSS_POS_IFM_MASK); + return (SbgEComGnssIfmStatus)((pLogData->statusExt >> SBG_ECOM_LOG_GNSS_POS_IFM_SHIFT)&SBG_ECOM_LOG_GNSS_POS_IFM_MASK); } void sbgEComLogGnssPosSetSpoofingStatus(SbgEComLogGnssPos *pLogData, SbgEComGnssSpoofingStatus status) { - assert(pLogData); - assert(status <= SBG_ECOM_LOG_GNSS_POS_SPOOFING_MASK); + assert(pLogData); + assert(status <= SBG_ECOM_LOG_GNSS_POS_SPOOFING_MASK); - pLogData->statusExt &= ~(SBG_ECOM_LOG_GNSS_POS_SPOOFING_MASK << SBG_ECOM_LOG_GNSS_POS_SPOOFING_SHIFT); - pLogData->statusExt |= ((uint32_t)status&SBG_ECOM_LOG_GNSS_POS_SPOOFING_MASK) << SBG_ECOM_LOG_GNSS_POS_SPOOFING_SHIFT; + pLogData->statusExt &= ~(SBG_ECOM_LOG_GNSS_POS_SPOOFING_MASK << SBG_ECOM_LOG_GNSS_POS_SPOOFING_SHIFT); + pLogData->statusExt |= ((uint32_t)status&SBG_ECOM_LOG_GNSS_POS_SPOOFING_MASK) << SBG_ECOM_LOG_GNSS_POS_SPOOFING_SHIFT; } SbgEComGnssSpoofingStatus sbgEComLogGnssPosGetSpoofingStatus(const SbgEComLogGnssPos *pLogData) { - return (SbgEComGnssSpoofingStatus)((pLogData->statusExt >> SBG_ECOM_LOG_GNSS_POS_SPOOFING_SHIFT)&SBG_ECOM_LOG_GNSS_POS_SPOOFING_MASK); + return (SbgEComGnssSpoofingStatus)((pLogData->statusExt >> SBG_ECOM_LOG_GNSS_POS_SPOOFING_SHIFT)&SBG_ECOM_LOG_GNSS_POS_SPOOFING_MASK); } void sbgEComLogGnssPosSetOsnmaStatus(SbgEComLogGnssPos *pLogData, SbgEComGnssOsnmaStatus status) { - assert(pLogData); - assert(status <= SBG_ECOM_LOG_GNSS_POS_OSNMA_MASK); + assert(pLogData); + assert(status <= SBG_ECOM_LOG_GNSS_POS_OSNMA_MASK); - pLogData->statusExt &= ~(SBG_ECOM_LOG_GNSS_POS_OSNMA_MASK << SBG_ECOM_LOG_GNSS_POS_OSNMA_SHIFT); - pLogData->statusExt |= ((uint32_t)status&SBG_ECOM_LOG_GNSS_POS_OSNMA_MASK) << SBG_ECOM_LOG_GNSS_POS_OSNMA_SHIFT; + pLogData->statusExt &= ~(SBG_ECOM_LOG_GNSS_POS_OSNMA_MASK << SBG_ECOM_LOG_GNSS_POS_OSNMA_SHIFT); + pLogData->statusExt |= ((uint32_t)status&SBG_ECOM_LOG_GNSS_POS_OSNMA_MASK) << SBG_ECOM_LOG_GNSS_POS_OSNMA_SHIFT; } SbgEComGnssOsnmaStatus sbgEComLogGnssPosGetOsnmaStatus(const SbgEComLogGnssPos *pLogData) { - return (SbgEComGnssOsnmaStatus)((pLogData->statusExt >> SBG_ECOM_LOG_GNSS_POS_OSNMA_SHIFT)&SBG_ECOM_LOG_GNSS_POS_OSNMA_MASK); + return (SbgEComGnssOsnmaStatus)((pLogData->statusExt >> SBG_ECOM_LOG_GNSS_POS_OSNMA_SHIFT)&SBG_ECOM_LOG_GNSS_POS_OSNMA_MASK); } //----------------------------------------------------------------------// //- DEPRECATED - Used for backward compatibility -// @@ -347,26 +347,26 @@ SbgEComGnssOsnmaStatus sbgEComLogGnssPosGetOsnmaStatus(const SbgEComLogGnssPos * uint32_t sbgEComLogGpsPosBuildStatus(SbgEComGnssPosStatus status, SbgEComGnssPosType type, uint32_t masks) { - return ((((uint32_t)status)&SBG_ECOM_LOG_GNSS_POS_STATUS_MASK) << SBG_ECOM_LOG_GNSS_POS_STATUS_SHIFT) | - ((((uint32_t)type)&SBG_ECOM_LOG_GNSS_POS_TYPE_MASK) << SBG_ECOM_LOG_GNSS_POS_TYPE_SHIFT) | masks; + return ((((uint32_t)status)&SBG_ECOM_LOG_GNSS_POS_STATUS_MASK) << SBG_ECOM_LOG_GNSS_POS_STATUS_SHIFT) | + ((((uint32_t)type)&SBG_ECOM_LOG_GNSS_POS_TYPE_MASK) << SBG_ECOM_LOG_GNSS_POS_TYPE_SHIFT) | masks; } SbgEComGnssPosStatus sbgEComLogGpsPosGetStatus(uint32_t status) { - return (SbgEComGnssPosStatus)((status >> SBG_ECOM_LOG_GNSS_POS_STATUS_SHIFT) & SBG_ECOM_LOG_GNSS_POS_STATUS_MASK); + return (SbgEComGnssPosStatus)((status >> SBG_ECOM_LOG_GNSS_POS_STATUS_SHIFT) & SBG_ECOM_LOG_GNSS_POS_STATUS_MASK); } SbgEComGnssPosType sbgEComLogGpsPosGetType(uint32_t status) { - return (SbgEComGnssPosType)((status >> SBG_ECOM_LOG_GNSS_POS_TYPE_SHIFT) & SBG_ECOM_LOG_GNSS_POS_TYPE_MASK); + return (SbgEComGnssPosType)((status >> SBG_ECOM_LOG_GNSS_POS_TYPE_SHIFT) & SBG_ECOM_LOG_GNSS_POS_TYPE_MASK); } SbgErrorCode sbgEComBinaryLogParseGpsPosData(SbgStreamBuffer *pStreamBuffer, SbgEComLogGnssPos *pLogData) { - return sbgEComLogGnssPosReadFromStream(pLogData, pStreamBuffer); + return sbgEComLogGnssPosReadFromStream(pLogData, pStreamBuffer); } SbgErrorCode sbgEComBinaryLogWriteGpsPosData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogGnssPos *pLogData) { - return sbgEComLogGnssPosWriteToStream(pLogData, pStreamBuffer); + return sbgEComLogGnssPosWriteToStream(pLogData, pStreamBuffer); } diff --git a/src/logs/sbgEComLogGnssPos.h b/src/logs/sbgEComLogGnssPos.h index 2149367..36782e8 100644 --- a/src/logs/sbgEComLogGnssPos.h +++ b/src/logs/sbgEComLogGnssPos.h @@ -1,13 +1,13 @@ /*! - * \file sbgEComLogGnssPos.h - * \ingroup binaryLogs - * \author SBG Systems - * \date 09 May 2023 + * \file sbgEComLogGnssPos.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 09 May 2023 * - * \brief GNSS position related logs. + * \brief GNSS position related logs. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -48,37 +48,37 @@ extern "C" { /*! * GNSS signals definitions */ -#define SBG_ECOM_GNSS_POS_GPS_L1_USED (0x00000001u << 12) /*!< Set to 1 if GPS L1CA/L1P is used in solution. */ -#define SBG_ECOM_GNSS_POS_GPS_L2_USED (0x00000001u << 13) /*!< Set to 1 if GPS L2P/L2C is used in solution. */ -#define SBG_ECOM_GNSS_POS_GPS_L5_USED (0x00000001u << 14) /*!< Set to 1 if GPS L5 is used in solution. */ +#define SBG_ECOM_GNSS_POS_GPS_L1_USED (0x00000001u << 12) /*!< Set to 1 if GPS L1CA/L1P is used in solution. */ +#define SBG_ECOM_GNSS_POS_GPS_L2_USED (0x00000001u << 13) /*!< Set to 1 if GPS L2P/L2C is used in solution. */ +#define SBG_ECOM_GNSS_POS_GPS_L5_USED (0x00000001u << 14) /*!< Set to 1 if GPS L5 is used in solution. */ -#define SBG_ECOM_GNSS_POS_GLO_L1_USED (0x00000001u << 15) /*!< Set to 1 if GLONASS L1CA is used in solution. */ -#define SBG_ECOM_GNSS_POS_GLO_L2_USED (0x00000001u << 16) /*!< Set to 1 if GLONASS L2C/L2P is used in solution. */ -#define SBG_ECOM_GNSS_POS_GLO_L3_USED (0x00000001u << 17) /*!< Set to 1 if GLONASS L3 is used in solution. */ +#define SBG_ECOM_GNSS_POS_GLO_L1_USED (0x00000001u << 15) /*!< Set to 1 if GLONASS L1CA is used in solution. */ +#define SBG_ECOM_GNSS_POS_GLO_L2_USED (0x00000001u << 16) /*!< Set to 1 if GLONASS L2C/L2P is used in solution. */ +#define SBG_ECOM_GNSS_POS_GLO_L3_USED (0x00000001u << 17) /*!< Set to 1 if GLONASS L3 is used in solution. */ -#define SBG_ECOM_GNSS_POS_GAL_E1_USED (0x00000001u << 18) /*!< Set to 1 if Galileo E1 is used in solution. */ -#define SBG_ECOM_GNSS_POS_GAL_E5A_USED (0x00000001u << 19) /*!< Set to 1 if Galileo E5a is used in solution. */ -#define SBG_ECOM_GNSS_POS_GAL_E5B_USED (0x00000001u << 20) /*!< Set to 1 if Galileo E5b is used in solution. */ -#define SBG_ECOM_GNSS_POS_GAL_E5ALT_USED (0x00000001u << 21) /*!< Set to 1 if Galileo E5 AltBoc is used in solution. */ -#define SBG_ECOM_GNSS_POS_GAL_E6_USED (0x00000001u << 22) /*!< Set to 1 if Galileo E6 is used in solution. */ +#define SBG_ECOM_GNSS_POS_GAL_E1_USED (0x00000001u << 18) /*!< Set to 1 if Galileo E1 is used in solution. */ +#define SBG_ECOM_GNSS_POS_GAL_E5A_USED (0x00000001u << 19) /*!< Set to 1 if Galileo E5a is used in solution. */ +#define SBG_ECOM_GNSS_POS_GAL_E5B_USED (0x00000001u << 20) /*!< Set to 1 if Galileo E5b is used in solution. */ +#define SBG_ECOM_GNSS_POS_GAL_E5ALT_USED (0x00000001u << 21) /*!< Set to 1 if Galileo E5 AltBoc is used in solution. */ +#define SBG_ECOM_GNSS_POS_GAL_E6_USED (0x00000001u << 22) /*!< Set to 1 if Galileo E6 is used in solution. */ -#define SBG_ECOM_GNSS_POS_BDS_B1_USED (0x00000001u << 23) /*!< Set to 1 if BeiDou B1 is used in solution. */ -#define SBG_ECOM_GNSS_POS_BDS_B2_USED (0x00000001u << 24) /*!< Set to 1 if BeiDou B2 is used in solution. */ -#define SBG_ECOM_GNSS_POS_BDS_B3_USED (0x00000001u << 25) /*!< Set to 1 if BeiDou B3 is used in solution. */ +#define SBG_ECOM_GNSS_POS_BDS_B1_USED (0x00000001u << 23) /*!< Set to 1 if BeiDou B1 is used in solution. */ +#define SBG_ECOM_GNSS_POS_BDS_B2_USED (0x00000001u << 24) /*!< Set to 1 if BeiDou B2 is used in solution. */ +#define SBG_ECOM_GNSS_POS_BDS_B3_USED (0x00000001u << 25) /*!< Set to 1 if BeiDou B3 is used in solution. */ -#define SBG_ECOM_GNSS_POS_QZSS_L1_USED (0x00000001u << 26) /*!< Set to 1 if QZSS L1CA is used in solution. */ -#define SBG_ECOM_GNSS_POS_QZSS_L2_USED (0x00000001u << 27) /*!< Set to 1 if QZSS L2C is used in solution. */ -#define SBG_ECOM_GNSS_POS_QZSS_L5_USED (0x00000001u << 28) /*!< Set to 1 if QZSS L5 is used in solution. */ +#define SBG_ECOM_GNSS_POS_QZSS_L1_USED (0x00000001u << 26) /*!< Set to 1 if QZSS L1CA is used in solution. */ +#define SBG_ECOM_GNSS_POS_QZSS_L2_USED (0x00000001u << 27) /*!< Set to 1 if QZSS L2C is used in solution. */ +#define SBG_ECOM_GNSS_POS_QZSS_L5_USED (0x00000001u << 28) /*!< Set to 1 if QZSS L5 is used in solution. */ /*! * GNSS position status definitions. */ typedef enum _SbgEComGnssPosStatus { - SBG_ECOM_GNSS_POS_STATUS_SOL_COMPUTED = 0, /*!< A valid solution has been computed. */ - SBG_ECOM_GNSS_POS_STATUS_INSUFFICIENT_OBS = 1, /*!< Not enough valid SV to compute a solution. */ - SBG_ECOM_GNSS_POS_STATUS_INTERNAL_ERROR = 2, /*!< An internal error has occurred. */ - SBG_ECOM_GNSS_POS_STATUS_HEIGHT_LIMIT = 3 /*!< The height limit has been exceeded. */ + SBG_ECOM_GNSS_POS_STATUS_SOL_COMPUTED = 0, /*!< A valid solution has been computed. */ + SBG_ECOM_GNSS_POS_STATUS_INSUFFICIENT_OBS = 1, /*!< Not enough valid SV to compute a solution. */ + SBG_ECOM_GNSS_POS_STATUS_INTERNAL_ERROR = 2, /*!< An internal error has occurred. */ + SBG_ECOM_GNSS_POS_STATUS_HEIGHT_LIMIT = 3 /*!< The height limit has been exceeded. */ } SbgEComGnssPosStatus; /*! @@ -86,17 +86,17 @@ typedef enum _SbgEComGnssPosStatus */ typedef enum _SbgEComGnssPosType { - SBG_ECOM_GNSS_POS_TYPE_NO_SOLUTION = 0, /*!< No valid solution available. */ - SBG_ECOM_GNSS_POS_TYPE_UNKNOWN = 1, /*!< An unknown solution type has been computed. */ - SBG_ECOM_GNSS_POS_TYPE_SINGLE = 2, /*!< Single point solution position. */ - SBG_ECOM_GNSS_POS_TYPE_PSRDIFF = 3, /*!< Standard Pseudorange Differential Solution (DGPS). */ - SBG_ECOM_GNSS_POS_TYPE_SBAS = 4, /*!< SBAS satellite used for differential corrections. */ - SBG_ECOM_GNSS_POS_TYPE_OMNISTAR = 5, /*!< Omnistar VBS Position (L1 sub-meter). */ - SBG_ECOM_GNSS_POS_TYPE_RTK_FLOAT = 6, /*!< Floating RTK ambiguity solution (20 cms RTK). */ - SBG_ECOM_GNSS_POS_TYPE_RTK_INT = 7, /*!< Integer RTK ambiguity solution (2 cms RTK). */ - SBG_ECOM_GNSS_POS_TYPE_PPP_FLOAT = 8, /*!< Precise Point Positioning with float ambiguities. */ - SBG_ECOM_GNSS_POS_TYPE_PPP_INT = 9, /*!< Precise Point Positioning with fixed ambiguities. */ - SBG_ECOM_GNSS_POS_TYPE_FIXED = 10 /*!< Fixed location solution position. */ + SBG_ECOM_GNSS_POS_TYPE_NO_SOLUTION = 0, /*!< No valid solution available. */ + SBG_ECOM_GNSS_POS_TYPE_UNKNOWN = 1, /*!< An unknown solution type has been computed. */ + SBG_ECOM_GNSS_POS_TYPE_SINGLE = 2, /*!< Single point solution position. */ + SBG_ECOM_GNSS_POS_TYPE_PSRDIFF = 3, /*!< Standard Pseudorange Differential Solution (DGPS). */ + SBG_ECOM_GNSS_POS_TYPE_SBAS = 4, /*!< SBAS satellite used for differential corrections. */ + SBG_ECOM_GNSS_POS_TYPE_OMNISTAR = 5, /*!< Omnistar VBS Position (L1 sub-meter). */ + SBG_ECOM_GNSS_POS_TYPE_RTK_FLOAT = 6, /*!< Floating RTK ambiguity solution (20 cms RTK). */ + SBG_ECOM_GNSS_POS_TYPE_RTK_INT = 7, /*!< Integer RTK ambiguity solution (2 cms RTK). */ + SBG_ECOM_GNSS_POS_TYPE_PPP_FLOAT = 8, /*!< Precise Point Positioning with float ambiguities. */ + SBG_ECOM_GNSS_POS_TYPE_PPP_INT = 9, /*!< Precise Point Positioning with fixed ambiguities. */ + SBG_ECOM_GNSS_POS_TYPE_FIXED = 10 /*!< Fixed location solution position. */ } SbgEComGnssPosType; /*! @@ -104,11 +104,11 @@ typedef enum _SbgEComGnssPosType */ typedef enum _SbgEComGnssIfmStatus { - SBG_ECOM_GNSS_IFM_STATUS_ERROR = 0, /*!< Interference monitoring system is in error and doesn't work. */ - SBG_ECOM_GNSS_IFM_STATUS_UNKNOWN = 1, /*!< Interference monitoring is either disabled or not available. */ - SBG_ECOM_GNSS_IFM_STATUS_CLEAN = 2, /*!< Interference are monitored and the environment is clean. */ - SBG_ECOM_GNSS_IFM_STATUS_MITIGATED = 3, /*!< Interference are detected and mitigated, the PVT is OK. */ - SBG_ECOM_GNSS_IFM_STATUS_CRITICAL = 4 /*!< Interference are detected and couldn't be mitigated, the PVT is invalid. */ + SBG_ECOM_GNSS_IFM_STATUS_ERROR = 0, /*!< Interference monitoring system is in error and doesn't work. */ + SBG_ECOM_GNSS_IFM_STATUS_UNKNOWN = 1, /*!< Interference monitoring is either disabled or not available. */ + SBG_ECOM_GNSS_IFM_STATUS_CLEAN = 2, /*!< Interference are monitored and the environment is clean. */ + SBG_ECOM_GNSS_IFM_STATUS_MITIGATED = 3, /*!< Interference are detected and mitigated, the PVT is OK. */ + SBG_ECOM_GNSS_IFM_STATUS_CRITICAL = 4 /*!< Interference are detected and couldn't be mitigated, the PVT is invalid. */ } SbgEComGnssIfmStatus; /*! @@ -116,11 +116,11 @@ typedef enum _SbgEComGnssIfmStatus */ typedef enum _SbgEComGnssSpoofingStatus { - SBG_ECOM_GNSS_SPOOFING_STATUS_ERROR = 0, /*!< Spoofing detection system is in error and doesn't work. */ - SBG_ECOM_GNSS_SPOOFING_STATUS_UNKNOWN = 1, /*!< Spoofing detection is either disabled or not available. */ - SBG_ECOM_GNSS_SPOOFING_STATUS_CLEAN = 2, /*!< Spoofing detection is enabled and not spoofer is detected. */ - SBG_ECOM_GNSS_SPOOFING_STATUS_SINGLE = 3, /*!< Probable spoofing identified by one method only. */ - SBG_ECOM_GNSS_SPOOFING_STATUS_MULTIPLE = 4 /*!< Confirmed spoofing identified by several methods such as Galileo OSNMA + Built In. */ + SBG_ECOM_GNSS_SPOOFING_STATUS_ERROR = 0, /*!< Spoofing detection system is in error and doesn't work. */ + SBG_ECOM_GNSS_SPOOFING_STATUS_UNKNOWN = 1, /*!< Spoofing detection is either disabled or not available. */ + SBG_ECOM_GNSS_SPOOFING_STATUS_CLEAN = 2, /*!< Spoofing detection is enabled and not spoofer is detected. */ + SBG_ECOM_GNSS_SPOOFING_STATUS_SINGLE = 3, /*!< Probable spoofing identified by one method only. */ + SBG_ECOM_GNSS_SPOOFING_STATUS_MULTIPLE = 4 /*!< Confirmed spoofing identified by several methods such as Galileo OSNMA + Built In. */ } SbgEComGnssSpoofingStatus; /*! @@ -128,12 +128,12 @@ typedef enum _SbgEComGnssSpoofingStatus */ typedef enum _SbgEComGnssOsnmaStatus { - SBG_ECOM_GNSS_OSNMA_STATUS_ERROR = 0, /*!< OSNMA is in error state and doesn't work. */ - SBG_ECOM_GNSS_OSNMA_STATUS_DISABLED = 1, /*!< OSNMA is either disabled or not available. */ - SBG_ECOM_GNSS_OSNMA_STATUS_INITIALIZING = 2, /*!< OSNMA initialization in progress. */ - SBG_ECOM_GNSS_OSNMA_STATUS_WAITING_NTP = 3, /*!< OSNMA strict is used and is waiting to receive valid time from NTP server. */ - SBG_ECOM_GNSS_OSNMA_STATUS_VALID = 4, /*!< OSNMA is actively authenticating Galileo signals and not spoofing is detected. */ - SBG_ECOM_GNSS_OSNMA_STATUS_SPOOFED = 5 /*!< OSNMA has detected spoofing on Galileo signals. */ + SBG_ECOM_GNSS_OSNMA_STATUS_ERROR = 0, /*!< OSNMA is in error state and doesn't work. */ + SBG_ECOM_GNSS_OSNMA_STATUS_DISABLED = 1, /*!< OSNMA is either disabled or not available. */ + SBG_ECOM_GNSS_OSNMA_STATUS_INITIALIZING = 2, /*!< OSNMA initialization in progress. */ + SBG_ECOM_GNSS_OSNMA_STATUS_WAITING_NTP = 3, /*!< OSNMA strict is used and is waiting to receive valid time from NTP server. */ + SBG_ECOM_GNSS_OSNMA_STATUS_VALID = 4, /*!< OSNMA is actively authenticating Galileo signals and not spoofing is detected. */ + SBG_ECOM_GNSS_OSNMA_STATUS_SPOOFED = 5 /*!< OSNMA has detected spoofing on Galileo signals. */ } SbgEComGnssOsnmaStatus; //----------------------------------------------------------------------// @@ -145,21 +145,21 @@ typedef enum _SbgEComGnssOsnmaStatus */ typedef struct _SbgEComLogGnssPos { - 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 (0 to 9999). */ - float longitudeAccuracy; /*!< 1 sigma longitude accuracy in meters (0 to 9999). */ - float altitudeAccuracy; /*!< 1 sigma altitude accuracy in meters (0 to 9999). */ - uint8_t numSvUsed; /*!< Number of space vehicles used to compute the solution - set to 0xFF if not available. (added in 1.4) */ - uint16_t baseStationId; /*!< Base station id for differential corrections (0-4095) - set to 0xFFFF if differential are not used or not available. (added in 1.4). */ - uint16_t differentialAge; /*!< Differential correction age in 0.01 seconds - set to 0xFFFF if differential are not used or not available. (added in 1.4). */ - uint8_t numSvTracked; /*!< Number of space vehicles tracked by the GNSS - set to 0xFF if not available. (added in 4.0) */ - uint32_t statusExt; /*!< Additional status for interference, spoofing and OSNMA (added in 4.0). */ + 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 (0 to 9999). */ + float longitudeAccuracy; /*!< 1 sigma longitude accuracy in meters (0 to 9999). */ + float altitudeAccuracy; /*!< 1 sigma altitude accuracy in meters (0 to 9999). */ + uint8_t numSvUsed; /*!< Number of space vehicles used to compute the solution - set to 0xFF if not available. (added in 1.4) */ + uint16_t baseStationId; /*!< Base station id for differential corrections (0-4095) - set to 0xFFFF if differential are not used or not available. (added in 1.4). */ + uint16_t differentialAge; /*!< Differential correction age in 0.01 seconds - set to 0xFFFF if differential are not used or not available. (added in 1.4). */ + uint8_t numSvTracked; /*!< Number of space vehicles tracked by the GNSS - set to 0xFF if not available. (added in 4.0) */ + uint32_t statusExt; /*!< Additional status for interference, spoofing and OSNMA (added in 4.0). */ } SbgEComLogGnssPos; //----------------------------------------------------------------------// @@ -169,25 +169,25 @@ typedef struct _SbgEComLogGnssPos /*! * Zero initialize the message struct. * - * \param[out] pLogData Structure instance to zero init. + * \param[out] pLogData Structure instance to zero init. */ void sbgEComLogGnssPosZeroInit(SbgEComLogGnssPos *pLogData); /*! * Parse data for the SBG_ECOM_LOG_GPS#_POS message and fill the corresponding structure. * - * \param[out] pLogData Log structure instance to fill. - * \param[in] pStreamBuffer Input stream buffer to read the log from. - * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. + * \param[out] pLogData Log structure instance to fill. + * \param[in] pStreamBuffer Input stream buffer to read the log from. + * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. */ SbgErrorCode sbgEComLogGnssPosReadFromStream(SbgEComLogGnssPos *pLogData, SbgStreamBuffer *pStreamBuffer); /*! * Write data for the SBG_ECOM_LOG_GPS#_POS message to the output stream buffer from the provided structure. * - * \param[in] pLogData Log structure instance to write. - * \param[out] pStreamBuffer Output stream buffer to write the log to. - * \return SBG_NO_ERROR if the log has been written to the stream buffer. + * \param[in] pLogData Log structure instance to write. + * \param[out] pStreamBuffer Output stream buffer to write the log to. + * \return SBG_NO_ERROR if the log has been written to the stream buffer. */ SbgErrorCode sbgEComLogGnssPosWriteToStream(const SbgEComLogGnssPos *pLogData, SbgStreamBuffer *pStreamBuffer); @@ -198,8 +198,8 @@ SbgErrorCode sbgEComLogGnssPosWriteToStream(const SbgEComLogGnssPos *pLogData, S /*! * Set the GNSS position solution status. * - * \param[in] pLogData Log instance. - * \param[in] status The solution status to set. + * \param[in] pLogData Log instance. + * \param[in] status The solution status to set. */ void sbgEComLogGnssPosSetStatus(SbgEComLogGnssPos *pLogData, SbgEComGnssPosStatus status); @@ -208,16 +208,16 @@ void sbgEComLogGnssPosSetStatus(SbgEComLogGnssPos *pLogData, SbgEComGnssPosStatu * * Note: Method doesn't follow standard naming conventions because of legacy sbgEComLogGnssPosGetStatus method. * - * \param[in] pLogData Log instance. - * \return The solution status. + * \param[in] pLogData Log instance. + * \return The solution status. */ SbgEComGnssPosStatus sbgEComLogGnssPosGetStatus(const SbgEComLogGnssPos *pLogData); /*! * Set the GNSS position solution type. * - * \param[in] pLogData Log instance. - * \param[in] posType The solution type to set. + * \param[in] pLogData Log instance. + * \param[in] posType The solution type to set. */ void sbgEComLogGnssPosSetType(SbgEComLogGnssPos *pLogData, SbgEComGnssPosType posType); @@ -226,16 +226,16 @@ void sbgEComLogGnssPosSetType(SbgEComLogGnssPos *pLogData, SbgEComGnssPosType po * * Note: Method doesn't follow standard naming conventions because of legacy sbgEComLogGnssPosGetStatus method. * - * \param[in] pLogData Log instance. - * \return The solution type. + * \param[in] pLogData Log instance. + * \return The solution type. */ SbgEComGnssPosType sbgEComLogGnssPosGetType(const SbgEComLogGnssPos *pLogData); /*! * Set the GNSS signals used in solution bitmask. * - * \param[in] pLogData Log instance. - * \param[in] status Bitmak of signals used in solution to set. + * \param[in] pLogData Log instance. + * \param[in] signalMask Bitmak of signals used in solution to set. */ void sbgEComLogGnssPosSetSignalsUsed(SbgEComLogGnssPos *pLogData, uint32_t signalMask); @@ -244,41 +244,41 @@ void sbgEComLogGnssPosSetSignalsUsed(SbgEComLogGnssPos *pLogData, uint32_t signa * * Only returns true if all signals set in signalsMask are used in solution. * - * \param[in] pLogData Log instance. - * \param[in) signalsMask Bitmask of signals to check if there are used in solution. - * \return true if the signal(s) are used in solution. + * \param[in] pLogData Log instance. + * \param[in] signalsMask Bitmask of signals to check if there are used in solution. + * \return true if the signal(s) are used in solution. */ bool sbgEComLogGnssPosSignalsAreUsed(const SbgEComLogGnssPos *pLogData, uint32_t signalsMask); /*! * Returns true if the numSvTracked field is valid. * - * \param[in] pLogData Log instance. - * \return true if the numSvTracked field is valid or false otherwise. + * \param[in] pLogData Log instance. + * \return true if the numSvTracked field is valid or false otherwise. */ bool sbgEComLogGnssPosNumSvTrackedIsValid(const SbgEComLogGnssPos *pLogData); /*! * Returns true if the numSvUsed field is valid. * - * \param[in] pLogData Log instance. - * \return true if the numSvUsed field is valid or false otherwise. + * \param[in] pLogData Log instance. + * \return true if the numSvUsed field is valid or false otherwise. */ bool sbgEComLogGnssPosNumSvUsedIsValid(const SbgEComLogGnssPos *pLogData); /*! * Returns true if the baseStationId field is valid. * - * \param[in] pLogData Log instance. - * \return true if the baseStationId field is valid or false otherwise. + * \param[in] pLogData Log instance. + * \return true if the baseStationId field is valid or false otherwise. */ bool sbgEComLogGnssPosBaseStationIdIsValid(const SbgEComLogGnssPos *pLogData); /*! * Returns true if the differentialAge field is valid. * - * \param[in] pLogData Log instance. - * \return true if the differentialAge field is valid or false otherwise. + * \param[in] pLogData Log instance. + * \return true if the differentialAge field is valid or false otherwise. */ bool sbgEComLogGnssPosDifferentialAgeIsValid(const SbgEComLogGnssPos *pLogData); @@ -288,8 +288,8 @@ bool sbgEComLogGnssPosDifferentialAgeIsValid(const SbgEComLogGnssPos *pLogData); * If differential correction age is greater than 655.340 seconds or NaN * the field is set to invalid/not available. * - * \param[in] pLogData Log instance. - * \param[in] differentialAge Differential correction age in seconds (>= 0). + * \param[in] pLogData Log instance. + * \param[in] differentialAge Differential correction age in seconds (>= 0). */ void sbgEComLogGnssPosSetDifferentialAge(SbgEComLogGnssPos *pLogData, float differentialAge); @@ -298,56 +298,56 @@ void sbgEComLogGnssPosSetDifferentialAge(SbgEComLogGnssPos *pLogData, float diff * * If there is no valid differential age, returns NaN. * - * \param[in] pLogData Log instance. - * \return Differential correction age in seconds. + * \param[in] pLogData Log instance. + * \return Differential correction age in seconds. */ float sbgEComLogGnssPosGetDifferentialAge(const SbgEComLogGnssPos *pLogData); /*! * Set the GNSS interference monitoring and mitigation status. * - * \param[in] pLogData Log instance. - * \param[in] status The status to set. + * \param[in] pLogData Log instance. + * \param[in] status The status to set. */ void sbgEComLogGnssPosSetIfmStatus(SbgEComLogGnssPos *pLogData, SbgEComGnssIfmStatus status); /*! * Returns the GNSS position solution status. * - * \param[in] pLogData Log instance. - * \return The interference monitoring and mitigation status. + * \param[in] pLogData Log instance. + * \return The interference monitoring and mitigation status. */ SbgEComGnssIfmStatus sbgEComLogGnssPosGetIfmStatus(const SbgEComLogGnssPos *pLogData); /*! * Set the GNSS spoofing status. * - * \param[in] pLogData Log instance. - * \param[in] status The status to set. + * \param[in] pLogData Log instance. + * \param[in] status The status to set. */ void sbgEComLogGnssPosSetSpoofingStatus(SbgEComLogGnssPos *pLogData, SbgEComGnssSpoofingStatus status); /*! * Returns the GNSS spoofing status. * - * \param[in] pLogData Log instance. - * \return The spoofing status. + * \param[in] pLogData Log instance. + * \return The spoofing status. */ SbgEComGnssSpoofingStatus sbgEComLogGnssPosGetSpoofingStatus(const SbgEComLogGnssPos *pLogData); /*! * Set the GNSS Galileo OSNMA status. * - * \param[in] pLogData Log instance. - * \param[in] status The status to set. + * \param[in] pLogData Log instance. + * \param[in] status The status to set. */ void sbgEComLogGnssPosSetOsnmaStatus(SbgEComLogGnssPos *pLogData, SbgEComGnssOsnmaStatus status); /*! * Returns the GNSS Galileo OSNMA status. * - * \param[in] pLogData Log instance. - * \return The Galileo OSNMA status. + * \param[in] pLogData Log instance. + * \return The Galileo OSNMA status. */ SbgEComGnssOsnmaStatus sbgEComLogGnssPosGetOsnmaStatus(const SbgEComLogGnssPos *pLogData); @@ -356,49 +356,49 @@ SbgEComGnssOsnmaStatus sbgEComLogGnssPosGetOsnmaStatus(const SbgEComLogGnssPos * //----------------------------------------------------------------------// #ifdef SBG_ECOM_USE_DEPRECATED_MACROS - #define SBG_ECOM_GPS_POS_GPS_L1_USED (0x00000001u << 12) - #define SBG_ECOM_GPS_POS_GPS_L2_USED (0x00000001u << 13) - #define SBG_ECOM_GPS_POS_GPS_L5_USED (0x00000001u << 14) - - #define SBG_ECOM_GPS_POS_GLO_L1_USED (0x00000001u << 15) - #define SBG_ECOM_GPS_POS_GLO_L2_USED (0x00000001u << 16) - #define SBG_ECOM_GPS_POS_GLO_L3_USED (0x00000001u << 17) - - #define SBG_ECOM_GPS_POS_GAL_E1_USED (0x00000001u << 18) - #define SBG_ECOM_GPS_POS_GAL_E5A_USED (0x00000001u << 19) - #define SBG_ECOM_GPS_POS_GAL_E5B_USED (0x00000001u << 20) - #define SBG_ECOM_GPS_POS_GAL_E5ALT_USED (0x00000001u << 21) - #define SBG_ECOM_GPS_POS_GAL_E6_USED (0x00000001u << 22) - - #define SBG_ECOM_GPS_POS_BDS_B1_USED (0x00000001u << 23) - #define SBG_ECOM_GPS_POS_BDS_B2_USED (0x00000001u << 24) - #define SBG_ECOM_GPS_POS_BDS_B3_USED (0x00000001u << 25) - - #define SBG_ECOM_GPS_POS_QZSS_L1_USED (0x00000001u << 26) - #define SBG_ECOM_GPS_POS_QZSS_L2_USED (0x00000001u << 27) - #define SBG_ECOM_GPS_POS_QZSS_L5_USED (0x00000001u << 28) - - #define SBG_ECOM_POS_SOL_COMPUTED (SBG_ECOM_GNSS_POS_STATUS_SOL_COMPUTED) - #define SBG_ECOM_POS_INSUFFICIENT_OBS (SBG_ECOM_GNSS_POS_STATUS_INSUFFICIENT_OBS) - #define SBG_ECOM_POS_INTERNAL_ERROR (SBG_ECOM_GNSS_POS_STATUS_INTERNAL_ERROR) - #define SBG_ECOM_POS_HEIGHT_LIMIT (SBG_ECOM_GNSS_POS_STATUS_HEIGHT_LIMIT) - - #define SBG_ECOM_POS_NO_SOLUTION (SBG_ECOM_GNSS_POS_TYPE_NO_SOLUTION) - #define SBG_ECOM_POS_UNKNOWN_TYPE (SBG_ECOM_GNSS_POS_TYPE_UNKNOWN) - #define SBG_ECOM_POS_SINGLE (SBG_ECOM_GNSS_POS_TYPE_SINGLE) - #define SBG_ECOM_POS_PSRDIFF (SBG_ECOM_GNSS_POS_TYPE_PSRDIFF) - #define SBG_ECOM_POS_SBAS (SBG_ECOM_GNSS_POS_TYPE_SBAS) - #define SBG_ECOM_POS_OMNISTAR (SBG_ECOM_GNSS_POS_TYPE_OMNISTAR) - #define SBG_ECOM_POS_RTK_FLOAT (SBG_ECOM_GNSS_POS_TYPE_RTK_FLOAT) - #define SBG_ECOM_POS_RTK_INT (SBG_ECOM_GNSS_POS_TYPE_RTK_INT) - #define SBG_ECOM_POS_PPP_FLOAT (SBG_ECOM_GNSS_POS_TYPE_PPP_FLOAT) - #define SBG_ECOM_POS_PPP_INT (SBG_ECOM_GNSS_POS_TYPE_PPP_INT) - #define SBG_ECOM_POS_FIXED (SBG_ECOM_GNSS_POS_TYPE_FIXED) + #define SBG_ECOM_GPS_POS_GPS_L1_USED (0x00000001u << 12) + #define SBG_ECOM_GPS_POS_GPS_L2_USED (0x00000001u << 13) + #define SBG_ECOM_GPS_POS_GPS_L5_USED (0x00000001u << 14) + + #define SBG_ECOM_GPS_POS_GLO_L1_USED (0x00000001u << 15) + #define SBG_ECOM_GPS_POS_GLO_L2_USED (0x00000001u << 16) + #define SBG_ECOM_GPS_POS_GLO_L3_USED (0x00000001u << 17) + + #define SBG_ECOM_GPS_POS_GAL_E1_USED (0x00000001u << 18) + #define SBG_ECOM_GPS_POS_GAL_E5A_USED (0x00000001u << 19) + #define SBG_ECOM_GPS_POS_GAL_E5B_USED (0x00000001u << 20) + #define SBG_ECOM_GPS_POS_GAL_E5ALT_USED (0x00000001u << 21) + #define SBG_ECOM_GPS_POS_GAL_E6_USED (0x00000001u << 22) + + #define SBG_ECOM_GPS_POS_BDS_B1_USED (0x00000001u << 23) + #define SBG_ECOM_GPS_POS_BDS_B2_USED (0x00000001u << 24) + #define SBG_ECOM_GPS_POS_BDS_B3_USED (0x00000001u << 25) + + #define SBG_ECOM_GPS_POS_QZSS_L1_USED (0x00000001u << 26) + #define SBG_ECOM_GPS_POS_QZSS_L2_USED (0x00000001u << 27) + #define SBG_ECOM_GPS_POS_QZSS_L5_USED (0x00000001u << 28) + + #define SBG_ECOM_POS_SOL_COMPUTED (SBG_ECOM_GNSS_POS_STATUS_SOL_COMPUTED) + #define SBG_ECOM_POS_INSUFFICIENT_OBS (SBG_ECOM_GNSS_POS_STATUS_INSUFFICIENT_OBS) + #define SBG_ECOM_POS_INTERNAL_ERROR (SBG_ECOM_GNSS_POS_STATUS_INTERNAL_ERROR) + #define SBG_ECOM_POS_HEIGHT_LIMIT (SBG_ECOM_GNSS_POS_STATUS_HEIGHT_LIMIT) + + #define SBG_ECOM_POS_NO_SOLUTION (SBG_ECOM_GNSS_POS_TYPE_NO_SOLUTION) + #define SBG_ECOM_POS_UNKNOWN_TYPE (SBG_ECOM_GNSS_POS_TYPE_UNKNOWN) + #define SBG_ECOM_POS_SINGLE (SBG_ECOM_GNSS_POS_TYPE_SINGLE) + #define SBG_ECOM_POS_PSRDIFF (SBG_ECOM_GNSS_POS_TYPE_PSRDIFF) + #define SBG_ECOM_POS_SBAS (SBG_ECOM_GNSS_POS_TYPE_SBAS) + #define SBG_ECOM_POS_OMNISTAR (SBG_ECOM_GNSS_POS_TYPE_OMNISTAR) + #define SBG_ECOM_POS_RTK_FLOAT (SBG_ECOM_GNSS_POS_TYPE_RTK_FLOAT) + #define SBG_ECOM_POS_RTK_INT (SBG_ECOM_GNSS_POS_TYPE_RTK_INT) + #define SBG_ECOM_POS_PPP_FLOAT (SBG_ECOM_GNSS_POS_TYPE_PPP_FLOAT) + #define SBG_ECOM_POS_PPP_INT (SBG_ECOM_GNSS_POS_TYPE_PPP_INT) + #define SBG_ECOM_POS_FIXED (SBG_ECOM_GNSS_POS_TYPE_FIXED) #endif -SBG_DEPRECATED_TYPEDEF(typedef enum _SbgEComGnssPosStatus SbgEComGpsPosStatus); -SBG_DEPRECATED_TYPEDEF(typedef enum _SbgEComGnssPosType SbgEComGpsPosType); -SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogGnssPos SbgLogGpsPos); +SBG_DEPRECATED_TYPEDEF(typedef enum _SbgEComGnssPosStatus SbgEComGpsPosStatus); +SBG_DEPRECATED_TYPEDEF(typedef enum _SbgEComGnssPosType SbgEComGpsPosType); +SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogGnssPos SbgLogGpsPos); SBG_DEPRECATED(uint32_t sbgEComLogGpsPosBuildStatus(SbgEComGnssPosStatus status, SbgEComGnssPosType type, uint32_t masks)); SBG_DEPRECATED(SbgEComGnssPosStatus sbgEComLogGpsPosGetStatus(uint32_t status)); diff --git a/src/logs/sbgEComLogGnssVel.c b/src/logs/sbgEComLogGnssVel.c index 3dd6c48..8e07d26 100644 --- a/src/logs/sbgEComLogGnssVel.c +++ b/src/logs/sbgEComLogGnssVel.c @@ -9,11 +9,11 @@ //- Private definitions -// //----------------------------------------------------------------------// -#define SBG_ECOM_LOG_GNSS_VEL_STATUS_SHIFT (0u) /*!< Shift used to extract the GPS velocity status part. */ -#define SBG_ECOM_LOG_GNSS_VEL_STATUS_MASK (0x0000003Fu) /*!< Mask used to keep only the GPS velocity status part. */ +#define SBG_ECOM_LOG_GNSS_VEL_STATUS_SHIFT (0u) /*!< Shift used to extract the GPS velocity status part. */ +#define SBG_ECOM_LOG_GNSS_VEL_STATUS_MASK (0x0000003Fu) /*!< Mask used to keep only the GPS velocity status part. */ -#define SBG_ECOM_LOG_GNSS_VEL_TYPE_SHIFT (6u) /*!< Shift used to extract the GPS velocity type part. */ -#define SBG_ECOM_LOG_GNSS_VEL_TYPE_MASK (0x0000003Fu) /*!< Mask used to keep only the GPS velocity type part. */ +#define SBG_ECOM_LOG_GNSS_VEL_TYPE_SHIFT (6u) /*!< Shift used to extract the GPS velocity type part. */ +#define SBG_ECOM_LOG_GNSS_VEL_TYPE_MASK (0x0000003Fu) /*!< Mask used to keep only the GPS velocity type part. */ //----------------------------------------------------------------------// //- Public methods -// @@ -21,64 +21,64 @@ void sbgEComLogGnssVelZeroInit(SbgEComLogGnssVel *pLogData) { - assert(pLogData); + assert(pLogData); - memset(pLogData, 0x00, sizeof(*pLogData)); + memset(pLogData, 0x00, sizeof(*pLogData)); - pLogData->velocityAcc[0] = 9999.0f; - pLogData->velocityAcc[1] = 9999.0f; - pLogData->velocityAcc[2] = 9999.0f; + pLogData->velocityAcc[0] = 9999.0f; + pLogData->velocityAcc[1] = 9999.0f; + pLogData->velocityAcc[2] = 9999.0f; - pLogData->courseAcc = 180.0f; - - sbgEComLogGnssVelSetStatus(pLogData, SBG_ECOM_GNSS_VEL_STATUS_INSUFFICIENT_OBS); - sbgEComLogGnssVelSetType(pLogData, SBG_ECOM_GNSS_VEL_TYPE_NO_SOLUTION); + pLogData->courseAcc = 180.0f; + + sbgEComLogGnssVelSetStatus(pLogData, SBG_ECOM_GNSS_VEL_STATUS_INSUFFICIENT_OBS); + sbgEComLogGnssVelSetType(pLogData, SBG_ECOM_GNSS_VEL_TYPE_NO_SOLUTION); } SbgErrorCode sbgEComLogGnssVelReadFromStream(SbgEComLogGnssVel *pLogData, SbgStreamBuffer *pStreamBuffer) { - assert(pLogData); - assert(pStreamBuffer); - - pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); - pLogData->status = sbgStreamBufferReadUint32LE(pStreamBuffer); - pLogData->timeOfWeek = sbgStreamBufferReadUint32LE(pStreamBuffer); - - pLogData->velocity[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->velocity[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->velocity[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); - - pLogData->velocityAcc[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->velocityAcc[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->velocityAcc[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); - - pLogData->course = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->courseAcc = sbgStreamBufferReadFloatLE(pStreamBuffer); - - return sbgStreamBufferGetLastError(pStreamBuffer); + assert(pLogData); + assert(pStreamBuffer); + + pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); + pLogData->status = sbgStreamBufferReadUint32LE(pStreamBuffer); + pLogData->timeOfWeek = sbgStreamBufferReadUint32LE(pStreamBuffer); + + pLogData->velocity[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->velocity[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->velocity[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); + + pLogData->velocityAcc[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->velocityAcc[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->velocityAcc[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); + + pLogData->course = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->courseAcc = sbgStreamBufferReadFloatLE(pStreamBuffer); + + return sbgStreamBufferGetLastError(pStreamBuffer); } SbgErrorCode sbgEComLogGnssVelWriteToStream(const SbgEComLogGnssVel *pLogData, SbgStreamBuffer *pStreamBuffer) { - assert(pLogData); - assert(pStreamBuffer); + assert(pLogData); + assert(pStreamBuffer); - sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); - sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->status); - sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeOfWeek); + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->status); + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeOfWeek); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocity[0]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocity[1]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocity[2]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocity[0]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocity[1]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocity[2]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocityAcc[0]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocityAcc[1]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocityAcc[2]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocityAcc[0]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocityAcc[1]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocityAcc[2]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->course); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->courseAcc); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->course); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->courseAcc); - return sbgStreamBufferGetLastError(pStreamBuffer); + return sbgStreamBufferGetLastError(pStreamBuffer); } //----------------------------------------------------------------------// @@ -87,48 +87,48 @@ SbgErrorCode sbgEComLogGnssVelWriteToStream(const SbgEComLogGnssVel *pLogData, S void sbgEComLogGnssVelSetStatus(SbgEComLogGnssVel *pLogData, SbgEComGnssVelStatus status) { - assert(pLogData); - assert(status <= SBG_ECOM_LOG_GNSS_VEL_STATUS_MASK); + assert(pLogData); + assert(status <= SBG_ECOM_LOG_GNSS_VEL_STATUS_MASK); - pLogData->status &= ~(SBG_ECOM_LOG_GNSS_VEL_STATUS_MASK << SBG_ECOM_LOG_GNSS_VEL_STATUS_SHIFT); - pLogData->status |= ((uint32_t)status&SBG_ECOM_LOG_GNSS_VEL_STATUS_MASK) << SBG_ECOM_LOG_GNSS_VEL_STATUS_SHIFT; + pLogData->status &= ~(SBG_ECOM_LOG_GNSS_VEL_STATUS_MASK << SBG_ECOM_LOG_GNSS_VEL_STATUS_SHIFT); + pLogData->status |= ((uint32_t)status&SBG_ECOM_LOG_GNSS_VEL_STATUS_MASK) << SBG_ECOM_LOG_GNSS_VEL_STATUS_SHIFT; } SbgEComGnssVelStatus sbgEComLogGnssVelGetStatus(const SbgEComLogGnssVel *pLogData) { - assert(pLogData); + assert(pLogData); - return (SbgEComGnssVelStatus)((pLogData->status >> SBG_ECOM_LOG_GNSS_VEL_STATUS_SHIFT)&SBG_ECOM_LOG_GNSS_VEL_STATUS_MASK); + return (SbgEComGnssVelStatus)((pLogData->status >> SBG_ECOM_LOG_GNSS_VEL_STATUS_SHIFT)&SBG_ECOM_LOG_GNSS_VEL_STATUS_MASK); } void sbgEComLogGnssVelSetType(SbgEComLogGnssVel *pLogData, SbgEComGnssVelType posType) { - assert(pLogData); - assert(posType <= SBG_ECOM_LOG_GNSS_VEL_TYPE_MASK); + assert(pLogData); + assert(posType <= SBG_ECOM_LOG_GNSS_VEL_TYPE_MASK); - pLogData->status &= ~(SBG_ECOM_LOG_GNSS_VEL_TYPE_MASK << SBG_ECOM_LOG_GNSS_VEL_TYPE_SHIFT); - pLogData->status |= ((uint32_t)posType&SBG_ECOM_LOG_GNSS_VEL_TYPE_MASK) << SBG_ECOM_LOG_GNSS_VEL_TYPE_SHIFT; + pLogData->status &= ~(SBG_ECOM_LOG_GNSS_VEL_TYPE_MASK << SBG_ECOM_LOG_GNSS_VEL_TYPE_SHIFT); + pLogData->status |= ((uint32_t)posType&SBG_ECOM_LOG_GNSS_VEL_TYPE_MASK) << SBG_ECOM_LOG_GNSS_VEL_TYPE_SHIFT; } SbgEComGnssVelType sbgEComLogGnssVelGetType(const SbgEComLogGnssVel *pLogData) { - assert(pLogData); + assert(pLogData); - return (SbgEComGnssVelType)((pLogData->status >> SBG_ECOM_LOG_GNSS_VEL_TYPE_SHIFT)&SBG_ECOM_LOG_GNSS_VEL_TYPE_MASK); + return (SbgEComGnssVelType)((pLogData->status >> SBG_ECOM_LOG_GNSS_VEL_TYPE_SHIFT)&SBG_ECOM_LOG_GNSS_VEL_TYPE_MASK); } bool sbgEComLogGnssVelDownVelocityIsValid(const SbgEComLogGnssVel *pLogData) { - assert(pLogData); - - if (pLogData->velocityAcc[2] < (9999.0f - FLT_EPSILON)) - { - return true; - } - else - { - return false; - } + assert(pLogData); + + if (pLogData->velocityAcc[2] < (9999.0f - FLT_EPSILON)) + { + return true; + } + else + { + return false; + } } //----------------------------------------------------------------------// @@ -137,26 +137,26 @@ bool sbgEComLogGnssVelDownVelocityIsValid(const SbgEComLogGnssVel *pLogData) uint32_t sbgEComLogGpsVelBuildStatus(SbgEComGnssVelStatus status, SbgEComGnssVelType type) { - return ((((uint32_t)status)&SBG_ECOM_LOG_GNSS_VEL_STATUS_MASK) << SBG_ECOM_LOG_GNSS_VEL_STATUS_SHIFT) | - ((((uint32_t)type)&SBG_ECOM_LOG_GNSS_VEL_TYPE_MASK) << SBG_ECOM_LOG_GNSS_VEL_TYPE_SHIFT); + return ((((uint32_t)status)&SBG_ECOM_LOG_GNSS_VEL_STATUS_MASK) << SBG_ECOM_LOG_GNSS_VEL_STATUS_SHIFT) | + ((((uint32_t)type)&SBG_ECOM_LOG_GNSS_VEL_TYPE_MASK) << SBG_ECOM_LOG_GNSS_VEL_TYPE_SHIFT); } SbgEComGnssVelStatus sbgEComLogGpsVelGetStatus(uint32_t status) { - return (SbgEComGnssVelStatus)((status >> SBG_ECOM_LOG_GNSS_VEL_STATUS_SHIFT) & SBG_ECOM_LOG_GNSS_VEL_STATUS_MASK); + return (SbgEComGnssVelStatus)((status >> SBG_ECOM_LOG_GNSS_VEL_STATUS_SHIFT) & SBG_ECOM_LOG_GNSS_VEL_STATUS_MASK); } SbgEComGnssVelType sbgEComLogGpsVelGetType(uint32_t status) { - return (SbgEComGnssVelType)((status >> SBG_ECOM_LOG_GNSS_VEL_TYPE_SHIFT) & SBG_ECOM_LOG_GNSS_VEL_TYPE_MASK); + return (SbgEComGnssVelType)((status >> SBG_ECOM_LOG_GNSS_VEL_TYPE_SHIFT) & SBG_ECOM_LOG_GNSS_VEL_TYPE_MASK); } SbgErrorCode sbgEComBinaryLogParseGpsVelData(SbgStreamBuffer *pStreamBuffer, SbgEComLogGnssVel *pLogData) { - return sbgEComLogGnssVelReadFromStream(pLogData, pStreamBuffer); + return sbgEComLogGnssVelReadFromStream(pLogData, pStreamBuffer); } SbgErrorCode sbgEComBinaryLogWriteGpsVelData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogGnssVel *pLogData) { - return sbgEComLogGnssVelWriteToStream(pLogData, pStreamBuffer); + return sbgEComLogGnssVelWriteToStream(pLogData, pStreamBuffer); } diff --git a/src/logs/sbgEComLogGnssVel.h b/src/logs/sbgEComLogGnssVel.h index 6d9c38c..7424a5b 100644 --- a/src/logs/sbgEComLogGnssVel.h +++ b/src/logs/sbgEComLogGnssVel.h @@ -1,13 +1,13 @@ /*! - * \file sbgEComLogGnssVel.h - * \ingroup binaryLogs - * \author SBG Systems - * \date 09 May 2023 + * \file sbgEComLogGnssVel.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 09 May 2023 * - * \brief GNSS velocity logs. + * \brief GNSS velocity logs. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -50,10 +50,10 @@ extern "C" { */ typedef enum _SbgEComGnssVelStatus { - SBG_ECOM_GNSS_VEL_STATUS_SOL_COMPUTED = 0, /*!< A valid solution has been computed. */ - SBG_ECOM_GNSS_VEL_STATUS_INSUFFICIENT_OBS = 1, /*!< Not enough valid SV to compute a solution. */ - SBG_ECOM_GNSS_VEL_STATUS_INTERNAL_ERROR = 2, /*!< An internal error has occurred. */ - SBG_ECOM_GNSS_VEL_STATUS_LIMIT = 3 /*!< Velocity limit exceeded. */ + SBG_ECOM_GNSS_VEL_STATUS_SOL_COMPUTED = 0, /*!< A valid solution has been computed. */ + SBG_ECOM_GNSS_VEL_STATUS_INSUFFICIENT_OBS = 1, /*!< Not enough valid SV to compute a solution. */ + SBG_ECOM_GNSS_VEL_STATUS_INTERNAL_ERROR = 2, /*!< An internal error has occurred. */ + SBG_ECOM_GNSS_VEL_STATUS_LIMIT = 3 /*!< Velocity limit exceeded. */ } SbgEComGnssVelStatus; /*! @@ -61,10 +61,10 @@ typedef enum _SbgEComGnssVelStatus */ typedef enum _SbgEComGnssVelType { - SBG_ECOM_GNSS_VEL_TYPE_NO_SOLUTION = 0, /*!< No valid velocity solution available. */ - SBG_ECOM_GNSS_VEL_TYPE_UNKNOWN = 1, /*!< An unknown solution type has been computed. */ - SBG_ECOM_GNSS_VEL_TYPE_DOPPLER = 2, /*!< A Doppler velocity has been computed. */ - SBG_ECOM_GNSS_VEL_TYPE_DIFFERENTIAL = 3 /*!< A differential velocity has been computed between two positions. */ + SBG_ECOM_GNSS_VEL_TYPE_NO_SOLUTION = 0, /*!< No valid velocity solution available. */ + SBG_ECOM_GNSS_VEL_TYPE_UNKNOWN = 1, /*!< An unknown solution type has been computed. */ + SBG_ECOM_GNSS_VEL_TYPE_DOPPLER = 2, /*!< A Doppler velocity has been computed. */ + SBG_ECOM_GNSS_VEL_TYPE_DIFFERENTIAL = 3 /*!< A differential velocity has been computed between two positions. */ } SbgEComGnssVelType; //----------------------------------------------------------------------// @@ -72,20 +72,24 @@ typedef enum _SbgEComGnssVelType //----------------------------------------------------------------------// /*! - * Structure that stores data for the SBG_ECOM_LOG_GPS#_VEL message. + * \brief Structure that stores data for the SBG_ECOM_LOG_GPS#_VEL message. * - * WARNING: Some GNSS protocols such as NMEA are not able to return a full 3D velocity. - * The maximum standard deviation value of 9999 m.s^-1 should be used to indicate an invalid down velocity component. + * This structure holds information about GPS velocity, including timestamp, + * status, time of week, velocity components, and their accuracies. + * + * \note Some GNSS protocols, such as NMEA, cannot provide a full 3D velocity. + * An invalid down velocity component should be indicated by using the maximum + * standard deviation value of 9999 m/s. */ typedef struct _SbgEComLogGnssVel { - 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 (0 to 9999). */ - float course; /*!< Track ground course in degrees (0 to 360). */ - float courseAcc; /*!< Course accuracy in degrees (0 to 180). */ + uint32_t timeStamp; /*!< Time in microseconds since the sensor power up. */ + uint32_t status; /*!< GPS velocity status, type and bitmask. */ + uint32_t timeOfWeek; /*!< GPS time of week in milliseconds. */ + float velocity[3]; /*!< GPS North, East, Down velocity in m/s. */ + float velocityAcc[3]; /*!< GPS North, East, Down velocity 1 sigma accuracy in m/s (0 to 9999). */ + float course; /*!< Track ground course in degrees (0 to 360). */ + float courseAcc; /*!< Course accuracy in degrees (0 to 180). */ } SbgEComLogGnssVel; //----------------------------------------------------------------------// @@ -95,25 +99,25 @@ typedef struct _SbgEComLogGnssVel /*! * Zero initialize the message struct. * - * \param[out] pLogData Structure instance to zero init. + * \param[out] pLogData Structure instance to zero init. */ void sbgEComLogGnssVelZeroInit(SbgEComLogGnssVel *pLogData); /*! * Parse data for the SBG_ECOM_LOG_GPS#_VEL message and fill the corresponding structure. * - * \param[out] pLogData Log structure instance to fill. - * \param[in] pStreamBuffer Input stream buffer to read the log from. - * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. + * \param[out] pLogData Log structure instance to fill. + * \param[in] pStreamBuffer Input stream buffer to read the log from. + * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. */ SbgErrorCode sbgEComLogGnssVelReadFromStream(SbgEComLogGnssVel *pLogData, SbgStreamBuffer *pStreamBuffer); /*! * Write data for the SBG_ECOM_LOG_GPS#_VEL message to the output stream buffer from the provided structure. * - * \param[in] pLogData Log structure instance to write. - * \param[out] pStreamBuffer Output stream buffer to write the log to. - * \return SBG_NO_ERROR if the log has been written to the stream buffer. + * \param[in] pLogData Log structure instance to write. + * \param[out] pStreamBuffer Output stream buffer to write the log to. + * \return SBG_NO_ERROR if the log has been written to the stream buffer. */ SbgErrorCode sbgEComLogGnssVelWriteToStream(const SbgEComLogGnssVel *pLogData, SbgStreamBuffer *pStreamBuffer); @@ -124,32 +128,32 @@ SbgErrorCode sbgEComLogGnssVelWriteToStream(const SbgEComLogGnssVel *pLogData, S /*! * Set the GNSS position solution status. * - * \param[in] pLogData Log instance. - * \param[in] status The solution status to set. + * \param[in] pLogData Log instance. + * \param[in] status The solution status to set. */ void sbgEComLogGnssVelSetStatus(SbgEComLogGnssVel *pLogData, SbgEComGnssVelStatus status); /*! * Returns the GNSS position solution status. * - * \param[in] pLogData Log instance. - * \return The solution status. + * \param[in] pLogData Log instance. + * \return The solution status. */ SbgEComGnssVelStatus sbgEComLogGnssVelGetStatus(const SbgEComLogGnssVel *pLogData); /*! * Set the GNSS position solution type. * - * \param[in] pLogData Log instance. - * \param[in] posType The solution type to set. + * \param[in] pLogData Log instance. + * \param[in] posType The solution type to set. */ void sbgEComLogGnssVelSetType(SbgEComLogGnssVel *pLogData, SbgEComGnssVelType posType); /*! * Returns the GNSS position solution type. * - * \param[in] pLogData Log instance. - * \return The solution type. + * \param[in] pLogData Log instance. + * \return The solution type. */ SbgEComGnssVelType sbgEComLogGnssVelGetType(const SbgEComLogGnssVel *pLogData); @@ -159,8 +163,8 @@ SbgEComGnssVelType sbgEComLogGnssVelGetType(const SbgEComLogGnssVel *pLogData); * Some GNSS receivers such as the ones using NMEA protocols can't provide down velocity. * This methods checks the velocity standard deviation down component. * - * \param[in] pLogData Log instance. - * \return true if the velocity down component is available/valid. + * \param[in] pLogData Log instance. + * \return true if the velocity down component is available/valid. */ bool sbgEComLogGnssVelDownVelocityIsValid(const SbgEComLogGnssVel *pLogData); @@ -169,20 +173,20 @@ bool sbgEComLogGnssVelDownVelocityIsValid(const SbgEComLogGnssVel *pLogData); //----------------------------------------------------------------------// #ifdef SBG_ECOM_USE_DEPRECATED_MACROS - #define SBG_ECOM_VEL_SOL_COMPUTED (SBG_ECOM_GNSS_VEL_STATUS_SOL_COMPUTED) - #define SBG_ECOM_VEL_INSUFFICIENT_OBS (SBG_ECOM_GNSS_VEL_STATUS_INSUFFICIENT_OBS) - #define SBG_ECOM_VEL_INTERNAL_ERROR (SBG_ECOM_GNSS_VEL_STATUS_INTERNAL_ERROR) - #define SBG_ECOM_VEL_LIMIT (SBG_ECOM_GNSS_VEL_STATUS_LIMIT) - - #define SBG_ECOM_VEL_NO_SOLUTION (SBG_ECOM_GNSS_VEL_TYPE_NO_SOLUTION) - #define SBG_ECOM_VEL_UNKNOWN_TYPE (SBG_ECOM_GNSS_VEL_TYPE_UNKNOWN) - #define SBG_ECOM_VEL_DOPPLER (SBG_ECOM_GNSS_VEL_TYPE_DOPPLER) - #define SBG_ECOM_VEL_DIFFERENTIAL (SBG_ECOM_GNSS_VEL_TYPE_DIFFERENTIAL) + #define SBG_ECOM_VEL_SOL_COMPUTED (SBG_ECOM_GNSS_VEL_STATUS_SOL_COMPUTED) + #define SBG_ECOM_VEL_INSUFFICIENT_OBS (SBG_ECOM_GNSS_VEL_STATUS_INSUFFICIENT_OBS) + #define SBG_ECOM_VEL_INTERNAL_ERROR (SBG_ECOM_GNSS_VEL_STATUS_INTERNAL_ERROR) + #define SBG_ECOM_VEL_LIMIT (SBG_ECOM_GNSS_VEL_STATUS_LIMIT) + + #define SBG_ECOM_VEL_NO_SOLUTION (SBG_ECOM_GNSS_VEL_TYPE_NO_SOLUTION) + #define SBG_ECOM_VEL_UNKNOWN_TYPE (SBG_ECOM_GNSS_VEL_TYPE_UNKNOWN) + #define SBG_ECOM_VEL_DOPPLER (SBG_ECOM_GNSS_VEL_TYPE_DOPPLER) + #define SBG_ECOM_VEL_DIFFERENTIAL (SBG_ECOM_GNSS_VEL_TYPE_DIFFERENTIAL) #endif -SBG_DEPRECATED_TYPEDEF(typedef enum _SbgEComGnssVelStatus SbgEComGpsVelStatus); -SBG_DEPRECATED_TYPEDEF(typedef enum _SbgEComGnssVelType SbgEComGpsVelType); -SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogGnssVel SbgLogGpsVel); +SBG_DEPRECATED_TYPEDEF(typedef enum _SbgEComGnssVelStatus SbgEComGpsVelStatus); +SBG_DEPRECATED_TYPEDEF(typedef enum _SbgEComGnssVelType SbgEComGpsVelType); +SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogGnssVel SbgLogGpsVel); SBG_DEPRECATED(uint32_t sbgEComLogGpsVelBuildStatus(SbgEComGnssVelStatus status, SbgEComGnssVelType type)); SBG_DEPRECATED(SbgEComGnssVelStatus sbgEComLogGpsVelGetStatus(uint32_t status)); diff --git a/src/logs/sbgEComLogImu.c b/src/logs/sbgEComLogImu.c index e156b0f..4cb5e4a 100644 --- a/src/logs/sbgEComLogImu.c +++ b/src/logs/sbgEComLogImu.c @@ -1,145 +1,178 @@ #include "sbgEComLogImu.h" +//----------------------------------------------------------------------// +//- Private constant definitions -// +//----------------------------------------------------------------------// + +/*! + * Standard gyroscope scale factor. + */ +#define SBG_ECOM_LOG_IMU_GYRO_SCALE_STD (67108864.0f) + +/*! + * High range gyroscope scale factor derived from 10000 degrees per second maximum range. + * + * Calculation: (2^31 - 1) / (10000 * π / 180) + */ +#define SBG_ECOM_LOG_IMU_GYRO_SCALE_HIGH (12304174.0f) + +/*! + * Maximum value for the standard scale factor, in radians per second. + * + * Approximately equivalent to 1833 °/s. + */ +#define SBG_ECOM_LOG_IMU_GYRO_SCALE_STD_MAX_RAD ((float)INT32_MAX / SBG_ECOM_LOG_IMU_GYRO_SCALE_STD) + +/*! + * Standard accelerometer scale factor. + */ +#define SBG_ECOM_LOG_IMU_ACCEL_SCALE_STD (1048576.0f) + +/*! + * Standard temperature scale factor. + */ +#define SBG_ECOM_LOG_IMU_TEMP_SCALE_STD (256.0f) + //----------------------------------------------------------------------// //- Public methods -// //----------------------------------------------------------------------// SbgErrorCode sbgEComLogImuLegacyReadFromStream(SbgEComLogImuLegacy *pLogData, SbgStreamBuffer *pStreamBuffer) { - assert(pStreamBuffer); - assert(pLogData); + assert(pStreamBuffer); + assert(pLogData); + + pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); + pLogData->status = sbgStreamBufferReadUint16LE(pStreamBuffer); + + pLogData->accelerometers[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->accelerometers[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->accelerometers[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); - pLogData->status = sbgStreamBufferReadUint16LE(pStreamBuffer); - - pLogData->accelerometers[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->accelerometers[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->accelerometers[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->gyroscopes[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->gyroscopes[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->gyroscopes[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->gyroscopes[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->gyroscopes[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->gyroscopes[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->temperature = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->temperature = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->deltaVelocity[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->deltaVelocity[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->deltaVelocity[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->deltaVelocity[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->deltaVelocity[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->deltaVelocity[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); - - pLogData->deltaAngle[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->deltaAngle[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->deltaAngle[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->deltaAngle[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->deltaAngle[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->deltaAngle[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); - return sbgStreamBufferGetLastError(pStreamBuffer); + return sbgStreamBufferGetLastError(pStreamBuffer); } SbgErrorCode sbgEComLogImuLegacyWriteToStream(const SbgEComLogImuLegacy *pLogData, SbgStreamBuffer *pStreamBuffer) { - assert(pStreamBuffer); - assert(pLogData); + assert(pStreamBuffer); + assert(pLogData); - sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); - sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->status); - - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->accelerometers[0]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->accelerometers[1]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->accelerometers[2]); + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); + sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->status); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->gyroscopes[0]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->gyroscopes[1]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->gyroscopes[2]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->accelerometers[0]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->accelerometers[1]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->accelerometers[2]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->temperature); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->gyroscopes[0]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->gyroscopes[1]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->gyroscopes[2]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->deltaVelocity[0]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->deltaVelocity[1]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->deltaVelocity[2]); - - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->deltaAngle[0]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->deltaAngle[1]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->deltaAngle[2]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->temperature); - return sbgStreamBufferGetLastError(pStreamBuffer); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->deltaVelocity[0]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->deltaVelocity[1]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->deltaVelocity[2]); + + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->deltaAngle[0]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->deltaAngle[1]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->deltaAngle[2]); + + return sbgStreamBufferGetLastError(pStreamBuffer); } SbgErrorCode sbgEComLogImuShortReadFromStream(SbgEComLogImuShort *pLogData, SbgStreamBuffer *pStreamBuffer) { - assert(pStreamBuffer); - assert(pLogData); + assert(pStreamBuffer); + assert(pLogData); - pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); - pLogData->status = sbgStreamBufferReadUint16LE(pStreamBuffer); + pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); + pLogData->status = sbgStreamBufferReadUint16LE(pStreamBuffer); - pLogData->deltaVelocity[0] = sbgStreamBufferReadInt32LE(pStreamBuffer); - pLogData->deltaVelocity[1] = sbgStreamBufferReadInt32LE(pStreamBuffer); - pLogData->deltaVelocity[2] = sbgStreamBufferReadInt32LE(pStreamBuffer); + pLogData->deltaVelocity[0] = sbgStreamBufferReadInt32LE(pStreamBuffer); + pLogData->deltaVelocity[1] = sbgStreamBufferReadInt32LE(pStreamBuffer); + pLogData->deltaVelocity[2] = sbgStreamBufferReadInt32LE(pStreamBuffer); - pLogData->deltaAngle[0] = sbgStreamBufferReadInt32LE(pStreamBuffer); - pLogData->deltaAngle[1] = sbgStreamBufferReadInt32LE(pStreamBuffer); - pLogData->deltaAngle[2] = sbgStreamBufferReadInt32LE(pStreamBuffer); + pLogData->deltaAngle[0] = sbgStreamBufferReadInt32LE(pStreamBuffer); + pLogData->deltaAngle[1] = sbgStreamBufferReadInt32LE(pStreamBuffer); + pLogData->deltaAngle[2] = sbgStreamBufferReadInt32LE(pStreamBuffer); - pLogData->temperature = sbgStreamBufferReadInt16LE(pStreamBuffer); + pLogData->temperature = sbgStreamBufferReadInt16LE(pStreamBuffer); - return sbgStreamBufferGetLastError(pStreamBuffer); + return sbgStreamBufferGetLastError(pStreamBuffer); } SbgErrorCode sbgEComLogImuShortWriteToStream(const SbgEComLogImuShort *pLogData, SbgStreamBuffer *pStreamBuffer) { - assert(pStreamBuffer); - assert(pLogData); + assert(pStreamBuffer); + assert(pLogData); - sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); - sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->status); + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); + sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->status); - sbgStreamBufferWriteInt32LE(pStreamBuffer, pLogData->deltaVelocity[0]); - sbgStreamBufferWriteInt32LE(pStreamBuffer, pLogData->deltaVelocity[1]); - sbgStreamBufferWriteInt32LE(pStreamBuffer, pLogData->deltaVelocity[2]); + sbgStreamBufferWriteInt32LE(pStreamBuffer, pLogData->deltaVelocity[0]); + sbgStreamBufferWriteInt32LE(pStreamBuffer, pLogData->deltaVelocity[1]); + sbgStreamBufferWriteInt32LE(pStreamBuffer, pLogData->deltaVelocity[2]); - sbgStreamBufferWriteInt32LE(pStreamBuffer, pLogData->deltaAngle[0]); - sbgStreamBufferWriteInt32LE(pStreamBuffer, pLogData->deltaAngle[1]); - sbgStreamBufferWriteInt32LE(pStreamBuffer, pLogData->deltaAngle[2]); + sbgStreamBufferWriteInt32LE(pStreamBuffer, pLogData->deltaAngle[0]); + sbgStreamBufferWriteInt32LE(pStreamBuffer, pLogData->deltaAngle[1]); + sbgStreamBufferWriteInt32LE(pStreamBuffer, pLogData->deltaAngle[2]); - sbgStreamBufferWriteInt16LE(pStreamBuffer, pLogData->temperature); + sbgStreamBufferWriteInt16LE(pStreamBuffer, pLogData->temperature); - return sbgStreamBufferGetLastError(pStreamBuffer); + return sbgStreamBufferGetLastError(pStreamBuffer); } SbgErrorCode sbgEComLogImuFastLegacyReadFromStream(SbgEComLogImuFastLegacy *pLogData, SbgStreamBuffer *pStreamBuffer) { - assert(pStreamBuffer); - assert(pLogData); + assert(pStreamBuffer); + assert(pLogData); + + pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); + pLogData->status = sbgStreamBufferReadUint16LE(pStreamBuffer); - pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); - pLogData->status = sbgStreamBufferReadUint16LE(pStreamBuffer); - - pLogData->accelerometers[0] = (float)sbgStreamBufferReadInt16LE(pStreamBuffer) * 0.01f; - pLogData->accelerometers[1] = (float)sbgStreamBufferReadInt16LE(pStreamBuffer) * 0.01f; - pLogData->accelerometers[2] = (float)sbgStreamBufferReadInt16LE(pStreamBuffer) * 0.01f; + pLogData->accelerometers[0] = (float)sbgStreamBufferReadInt16LE(pStreamBuffer) * 0.01f; + pLogData->accelerometers[1] = (float)sbgStreamBufferReadInt16LE(pStreamBuffer) * 0.01f; + pLogData->accelerometers[2] = (float)sbgStreamBufferReadInt16LE(pStreamBuffer) * 0.01f; - pLogData->gyroscopes[0] = (float)sbgStreamBufferReadInt16LE(pStreamBuffer) * 0.001f; - pLogData->gyroscopes[1] = (float)sbgStreamBufferReadInt16LE(pStreamBuffer) * 0.001f; - pLogData->gyroscopes[2] = (float)sbgStreamBufferReadInt16LE(pStreamBuffer) * 0.001f; + pLogData->gyroscopes[0] = (float)sbgStreamBufferReadInt16LE(pStreamBuffer) * 0.001f; + pLogData->gyroscopes[1] = (float)sbgStreamBufferReadInt16LE(pStreamBuffer) * 0.001f; + pLogData->gyroscopes[2] = (float)sbgStreamBufferReadInt16LE(pStreamBuffer) * 0.001f; - return sbgStreamBufferGetLastError(pStreamBuffer); + return sbgStreamBufferGetLastError(pStreamBuffer); } SbgErrorCode sbgEComLogImuFastLegacyWriteToStream(const SbgEComLogImuFastLegacy *pLogData, SbgStreamBuffer *pStreamBuffer) { - assert(pStreamBuffer); - assert(pLogData); + assert(pStreamBuffer); + assert(pLogData); - sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); - sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->status); - - sbgStreamBufferWriteInt16LE(pStreamBuffer, (int16_t)(pLogData->accelerometers[0] * 100.0f)); - sbgStreamBufferWriteInt16LE(pStreamBuffer, (int16_t)(pLogData->accelerometers[1] * 100.0f)); - sbgStreamBufferWriteInt16LE(pStreamBuffer, (int16_t)(pLogData->accelerometers[2] * 100.0f)); - - sbgStreamBufferWriteInt16LE(pStreamBuffer, (int16_t)(pLogData->gyroscopes[0] * 1000.0f)); - sbgStreamBufferWriteInt16LE(pStreamBuffer, (int16_t)(pLogData->gyroscopes[1] * 1000.0f)); - sbgStreamBufferWriteInt16LE(pStreamBuffer, (int16_t)(pLogData->gyroscopes[2] * 1000.0f)); + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); + sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->status); - return sbgStreamBufferGetLastError(pStreamBuffer); + sbgStreamBufferWriteInt16LE(pStreamBuffer, (int16_t)(pLogData->accelerometers[0] * 100.0f)); + sbgStreamBufferWriteInt16LE(pStreamBuffer, (int16_t)(pLogData->accelerometers[1] * 100.0f)); + sbgStreamBufferWriteInt16LE(pStreamBuffer, (int16_t)(pLogData->accelerometers[2] * 100.0f)); + + sbgStreamBufferWriteInt16LE(pStreamBuffer, (int16_t)(pLogData->gyroscopes[0] * 1000.0f)); + sbgStreamBufferWriteInt16LE(pStreamBuffer, (int16_t)(pLogData->gyroscopes[1] * 1000.0f)); + sbgStreamBufferWriteInt16LE(pStreamBuffer, (int16_t)(pLogData->gyroscopes[2] * 1000.0f)); + + return sbgStreamBufferGetLastError(pStreamBuffer); } //----------------------------------------------------------------------// @@ -148,25 +181,77 @@ SbgErrorCode sbgEComLogImuFastLegacyWriteToStream(const SbgEComLogImuFastLegacy float sbgEComLogImuShortGetDeltaAngle(const SbgEComLogImuShort *pImuShort, size_t idx) { - assert(pImuShort); - assert(idx < 3); + float scaleFactor = SBG_ECOM_LOG_IMU_GYRO_SCALE_STD; + + assert(pImuShort); + assert(idx < 3); - return pImuShort->deltaAngle[idx] / 67108864.0f; + if (pImuShort->status & SBG_ECOM_IMU_GYROS_USE_HIGH_SCALE) + { + scaleFactor = SBG_ECOM_LOG_IMU_GYRO_SCALE_HIGH; + } + + return pImuShort->deltaAngle[idx] / scaleFactor; +} + +void sbgEComLogImuShortSetDeltaAngle(SbgEComLogImuShort *pImuShort, const float *pArray, size_t arraySize) +{ + float scaleFactor = SBG_ECOM_LOG_IMU_GYRO_SCALE_STD; + + assert(pImuShort); + assert(pArray); + assert(arraySize == 3); + + pImuShort->status &= ~SBG_ECOM_IMU_GYROS_USE_HIGH_SCALE; + + for (size_t i = 0; i < arraySize; i++) + { + if (fabsf(pArray[i]) > SBG_ECOM_LOG_IMU_GYRO_SCALE_STD_MAX_RAD) + { + scaleFactor = SBG_ECOM_LOG_IMU_GYRO_SCALE_HIGH; + pImuShort->status |= SBG_ECOM_IMU_GYROS_USE_HIGH_SCALE; + break; + } + } + + for (size_t i = 0; i < arraySize; i++) + { + pImuShort->deltaAngle[i] = (int32_t)(pArray[i] * scaleFactor); + } } float sbgEComLogImuShortGetDeltaVelocity(const SbgEComLogImuShort *pImuShort, size_t idx) { - assert(pImuShort); - assert(idx < 3); + assert(pImuShort); + assert(idx < 3); - return pImuShort->deltaVelocity[idx] / 1048576.0f; + return pImuShort->deltaVelocity[idx] / SBG_ECOM_LOG_IMU_ACCEL_SCALE_STD; +} + +void sbgEComLogImuShortSetDeltaVelocity(SbgEComLogImuShort *pImuShort, const float *pArray, size_t arraySize) +{ + assert(pImuShort); + assert(pArray); + assert(arraySize == 3); + + for (size_t i = 0; i < arraySize; i++) + { + pImuShort->deltaVelocity[i] = (int32_t)(pArray[i] * SBG_ECOM_LOG_IMU_ACCEL_SCALE_STD); + } } float sbgEComLogImuShortGetTemperature(const SbgEComLogImuShort *pImuShort) { - assert(pImuShort); + assert(pImuShort); + + return pImuShort->temperature / SBG_ECOM_LOG_IMU_TEMP_SCALE_STD; +} + +void sbgEComLogImuShortSetTemperature(SbgEComLogImuShort *pImuShort, float temperature) +{ + assert(pImuShort); - return pImuShort->temperature / 256.0f; + pImuShort->temperature = (int16_t)(temperature * SBG_ECOM_LOG_IMU_TEMP_SCALE_STD); } //----------------------------------------------------------------------// @@ -175,45 +260,45 @@ float sbgEComLogImuShortGetTemperature(const SbgEComLogImuShort *pImuShort) SbgErrorCode sbgEComBinaryLogParseImuData(SbgStreamBuffer *pStreamBuffer, SbgEComLogImuLegacy *pLogData) { - return sbgEComLogImuLegacyReadFromStream(pLogData, pStreamBuffer); + return sbgEComLogImuLegacyReadFromStream(pLogData, pStreamBuffer); } SbgErrorCode sbgEComBinaryLogWriteImuData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogImuLegacy *pLogData) { - return sbgEComLogImuLegacyWriteToStream(pLogData, pStreamBuffer); + return sbgEComLogImuLegacyWriteToStream(pLogData, pStreamBuffer); } SbgErrorCode sbgEComBinaryLogParseImuShort(SbgStreamBuffer *pStreamBuffer, SbgEComLogImuShort *pLogData) { - return sbgEComLogImuShortReadFromStream(pLogData, pStreamBuffer); + return sbgEComLogImuShortReadFromStream(pLogData, pStreamBuffer); } SbgErrorCode sbgEComBinaryLogWriteImuShort(SbgStreamBuffer *pStreamBuffer, const SbgEComLogImuShort *pLogData) { - return sbgEComLogImuShortWriteToStream(pLogData, pStreamBuffer); + return sbgEComLogImuShortWriteToStream(pLogData, pStreamBuffer); } SbgErrorCode sbgEComBinaryLogParseFastImuData(SbgStreamBuffer *pStreamBuffer, SbgEComLogImuFastLegacy *pLogData) { - return sbgEComLogImuFastLegacyReadFromStream(pLogData, pStreamBuffer); + return sbgEComLogImuFastLegacyReadFromStream(pLogData, pStreamBuffer); } SbgErrorCode sbgEComBinaryLogWriteFastImuData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogImuFastLegacy *pLogData) { - return sbgEComLogImuFastLegacyWriteToStream(pLogData, pStreamBuffer); + return sbgEComLogImuFastLegacyWriteToStream(pLogData, pStreamBuffer); } float sbgLogImuShortGetDeltaAngle(const SbgEComLogImuShort *pImuShort, size_t idx) { - return sbgEComLogImuShortGetDeltaAngle(pImuShort, idx); + return sbgEComLogImuShortGetDeltaAngle(pImuShort, idx); } float sbgLogImuShortGetDeltaVelocity(const SbgEComLogImuShort *pImuShort, size_t idx) { - return sbgEComLogImuShortGetDeltaVelocity(pImuShort, idx); + return sbgEComLogImuShortGetDeltaVelocity(pImuShort, idx); } float sbgLogImuShortGetTemperature(const SbgEComLogImuShort *pImuShort) { - return sbgEComLogImuShortGetTemperature(pImuShort); + return sbgEComLogImuShortGetTemperature(pImuShort); } diff --git a/src/logs/sbgEComLogImu.h b/src/logs/sbgEComLogImu.h index 2d78337..f043459 100644 --- a/src/logs/sbgEComLogImu.h +++ b/src/logs/sbgEComLogImu.h @@ -1,14 +1,14 @@ /*! - * \file sbgEComLogImu.h - * \ingroup binaryLogs - * \author SBG Systems - * \date 25 February 2013 + * \file sbgEComLogImu.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 25 February 2013 * - * \brief Parse IMU (Inertial Measurement Unit) measurement logs. + * \brief Parse IMU (Inertial Measurement Unit) measurement logs. + * + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license - * * 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 @@ -26,7 +26,7 @@ * 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. - * + * * \endlicense */ @@ -48,78 +48,78 @@ extern "C" { /*! * 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_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_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_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. */ +#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. */ +#define SBG_ECOM_IMU_GYROS_USE_HIGH_SCALE (0x00000001u << 10) /*!< Set if the gyroscope scale factor range is high. Applicable only for SBG_ECOM_LOG_IMU_SHORT logs. */ //----------------------------------------------------------------------// //- Log structure definitions -// //----------------------------------------------------------------------// /*! - * Structure that stores data for the SBG_ECOM_LOG_IMU_DATA message. + * \brief Structure that stores data for the SBG_ECOM_LOG_IMU_DATA message. * - * This message returns synchronous IMU measurements that are aligned to UTC time. - * - * All SBG Systems INS, except ELLIPSE products, use an IMU that provides asynchronous measurements. - * The INS has to extrapolate the IMU measurements to the current EKF processing loop. + * This message provides synchronous IMU measurements aligned to UTC time. + * Most SBG Systems INS, except ELLIPSE products, use IMUs with asynchronous measurements. * - * For a 200Hz processing loop, the extrapolation can be as high as 5ms. - * The extrapolation process introduces artificial noise in the measurements. - * - * This message is thus not suitable for post processing for example. - * - * If you would like IMU measurements correctly time stamped to UTC/GPS time - * but as output by the IMU (asynchronously), please use SBG_ECOM_LOG_IMU_SHORT - * - * WARNING: This message is not recommended for new designs and will be deprecated in sbgECom 5.x + * These measurements are extrapolated to match the EKF processing loop, which can introduce + * artificial noise, particularly in high-frequency processing loops (e.g., 200Hz with up to 5ms extrapolation). + * + * \note This message is not suitable for post-processing due to the introduced noise. + * For accurate UTC/GPS-timestamped asynchronous IMU measurements, use SBG_ECOM_LOG_IMU_SHORT. + * + * \warning This message is not recommended for new designs and will be deprecated in sbgECom 5.x. */ typedef struct _SbgEComLogImuLegacy { - 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. (Exact same value as accelerometers field) */ - float deltaAngle[3]; /*!< X, Y, Z delta angle in rad.s^-1. (Exact same value as gyroscopes field) */ + uint32_t timeStamp; /*!< Time in microseconds since the sensor power up. */ + uint16_t status; /*!< IMU status bitmask. */ + float accelerometers[3]; /*!< X, Y, Z accelerometer readings in m/s². */ + float gyroscopes[3]; /*!< X, Y, Z gyroscope readings in rad/s. */ + float temperature; /*!< Internal temperature in °C. */ + float deltaVelocity[3]; /*!< X, Y, Z delta velocity in m/s² (same as accelerometers field). */ + float deltaAngle[3]; /*!< X, Y, Z delta angle in rad/s (same as gyroscopes field). */ } SbgEComLogImuLegacy; /*! * Structure that stores data for the SBG_ECOM_LOG_IMU_SHORT message. - * + * * This message is sent asynchronously and must be used for post processing. + * + * The delta angle values are scaled based on the gyroscopes output. If any output exceeds + * a predefined limit, the scale factor switches from standard to high range to prevent saturation. */ typedef struct _SbgEComLogImuShort { - 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. */ + 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 either 67108864 LSB for 1 rad.s^-1 (standard) or 12304174 LSB for 1 rad.s^-1 (high range), managed automatically. */ + int16_t temperature; /*!< IMU average temperature. Unit is 256 LSB for 1°C. */ } SbgEComLogImuShort; /*! * Structure that stores the data for SBG_ECOM_LOG_FAST_IMU_DATA message * - * WARNING: This message is not recommended for new designs and will be deprecated in sbgECom 5.x + * \note This message is not recommended for new designs and will be deprecated in sbgECom 5.x */ typedef struct _SbgEComLogImuFastLegacy { - 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. */ + 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. */ } SbgEComLogImuFastLegacy; //----------------------------------------------------------------------// @@ -128,55 +128,55 @@ typedef struct _SbgEComLogImuFastLegacy /*! * Parse data for the SBG_ECOM_LOG_IMU_DATA message and fill the corresponding structure. - * - * \param[out] pLogData Log structure instance to fill. - * \param[in] pStreamBuffer Input stream buffer to read the log from. - * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. + * + * \param[out] pLogData Log structure instance to fill. + * \param[in] pStreamBuffer Input stream buffer to read the log from. + * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. */ SbgErrorCode sbgEComLogImuLegacyReadFromStream(SbgEComLogImuLegacy *pLogData, SbgStreamBuffer *pStreamBuffer); /*! * Write data for the SBG_ECOM_LOG_IMU_DATA message to the output stream buffer from the provided structure. * - * \param[in] pLogData Log structure instance to write. - * \param[out] pStreamBuffer Output stream buffer to write the log to. - * \return SBG_NO_ERROR if the log has been written to the stream buffer. + * \param[in] pLogData Log structure instance to write. + * \param[out] pStreamBuffer Output stream buffer to write the log to. + * \return SBG_NO_ERROR if the log has been written to the stream buffer. */ SbgErrorCode sbgEComLogImuLegacyWriteToStream(const SbgEComLogImuLegacy *pLogData, SbgStreamBuffer *pStreamBuffer); /*! * Parse data for the SBG_ECOM_LOG_IMU_SHORT message and fill the corresponding structure. - * - * \param[out] pLogData Log structure instance to fill. - * \param[in] pStreamBuffer Input stream buffer to read the log from. - * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. + * + * \param[out] pLogData Log structure instance to fill. + * \param[in] pStreamBuffer Input stream buffer to read the log from. + * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. */ SbgErrorCode sbgEComLogImuShortReadFromStream(SbgEComLogImuShort *pLogData, SbgStreamBuffer *pStreamBuffer); /*! * Write data for the SBG_ECOM_LOG_IMU_SHORT message to the output stream buffer from the provided structure. * - * \param[in] pLogData Log structure instance to write. - * \param[out] pStreamBuffer Output stream buffer to write the log to. - * \return SBG_NO_ERROR if the log has been written to the stream buffer. + * \param[in] pLogData Log structure instance to write. + * \param[out] pStreamBuffer Output stream buffer to write the log to. + * \return SBG_NO_ERROR if the log has been written to the stream buffer. */ SbgErrorCode sbgEComLogImuShortWriteToStream(const SbgEComLogImuShort *pLogData, SbgStreamBuffer *pStreamBuffer); /*! * Parse data for the SBG_ECOM_LOG_FAST_IMU_DATA message and fill the corresponding structure. - * - * \param[out] pLogData Log structure instance to fill. - * \param[in] pStreamBuffer Input stream buffer to read the log from. - * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. + * + * \param[out] pLogData Log structure instance to fill. + * \param[in] pStreamBuffer Input stream buffer to read the log from. + * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. */ SbgErrorCode sbgEComLogImuFastLegacyReadFromStream(SbgEComLogImuFastLegacy *pLogData, SbgStreamBuffer *pStreamBuffer); /*! * Write data for the SBG_ECOM_LOG_FAST_IMU_DATA message to the output stream buffer from the provided structure. * - * \param[in] pLogData Log structure instance to write. - * \param[out] pStreamBuffer Output stream buffer to write the log to. - * \return SBG_NO_ERROR if the log has been written to the stream buffer. + * \param[in] pLogData Log structure instance to write. + * \param[out] pStreamBuffer Output stream buffer to write the log to. + * \return SBG_NO_ERROR if the log has been written to the stream buffer. */ SbgErrorCode sbgEComLogImuFastLegacyWriteToStream(const SbgEComLogImuFastLegacy *pLogData, SbgStreamBuffer *pStreamBuffer); @@ -186,37 +186,63 @@ SbgErrorCode sbgEComLogImuFastLegacyWriteToStream(const SbgEComLogImuFastLegacy /*! * 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. + * + * \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 sbgEComLogImuShortGetDeltaAngle(const SbgEComLogImuShort *pImuShort, size_t idx); +/*! + * Set the delta angle values. + * + * \param[in] pImuShort Input IMU short message instance. + * \param[in] pArray Array of delta angle values, in rad.s^-1. + * \param[in] arraySize Size of the array (should be 3). + */ +void sbgEComLogImuShortSetDeltaAngle(SbgEComLogImuShort *pImuShort, const float *pArray, size_t arraySize); + /*! * 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. + * + * \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 sbgEComLogImuShortGetDeltaVelocity(const SbgEComLogImuShort *pImuShort, size_t idx); +/*! + * Set the delta velocity values. + * + * \param[in] pImuShort Input IMU short message instance. + * \param[in] pArray Array of delta velocity values, in m.s^-2. + * \param[in] arraySize Size of the array (should be 3). + */ +void sbgEComLogImuShortSetDeltaVelocity(SbgEComLogImuShort *pImuShort, const float *pArray, size_t arraySize); + /*! * Return from an IMU Short log, the temperature in °C - * - * \param[in] pImuShort Input IMU short message instance. - * \return The converted temperature in °C + * + * \param[in] pImuShort Input IMU short message instance. + * \return The converted temperature in °C */ float sbgEComLogImuShortGetTemperature(const SbgEComLogImuShort *pImuShort); +/*! + * Set the temperature. + * + * \param[in] pImuShort Input IMU short message instance. + * \param[in] temperature Temperature value to set, in °C. + */ +void sbgEComLogImuShortSetTemperature(SbgEComLogImuShort *pImuShort, float temperature); + //----------------------------------------------------------------------// //- DEPRECATED - Used for backward compatibility -// //----------------------------------------------------------------------// -SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogImuLegacy SbgLogImuData); -SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogImuShort SbgLogImuShort); -SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogImuFastLegacy SbgLogFastImuData); +SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogImuLegacy SbgLogImuData); +SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogImuShort SbgLogImuShort); +SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogImuFastLegacy SbgLogFastImuData); SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogParseImuData(SbgStreamBuffer *pStreamBuffer, SbgEComLogImuLegacy *pLogData)); SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogWriteImuData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogImuLegacy *pLogData)); diff --git a/src/logs/sbgEComLogMag.c b/src/logs/sbgEComLogMag.c index 29da35f..21031b9 100644 --- a/src/logs/sbgEComLogMag.c +++ b/src/logs/sbgEComLogMag.c @@ -1,4 +1,9 @@ -#include "sbgEComLogMag.h" +// sbgCommonLib headers +#include +#include + +// Local headers +#include "sbgEComLogMag.h" //----------------------------------------------------------------------// //- Public methods -// @@ -6,87 +11,54 @@ SbgErrorCode sbgEComLogMagReadFromStream(SbgEComLogMag *pLogData, SbgStreamBuffer *pStreamBuffer) { - assert(pStreamBuffer); - assert(pLogData); + assert(pStreamBuffer); + assert(pLogData); - pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); - pLogData->status = sbgStreamBufferReadUint16LE(pStreamBuffer); + pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); + pLogData->status = sbgStreamBufferReadUint16LE(pStreamBuffer); - pLogData->magnetometers[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->magnetometers[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->magnetometers[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); - - pLogData->accelerometers[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->accelerometers[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->accelerometers[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->magnetometers[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->magnetometers[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->magnetometers[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); + + pLogData->accelerometers[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->accelerometers[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->accelerometers[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); - return sbgStreamBufferGetLastError(pStreamBuffer); + return sbgStreamBufferGetLastError(pStreamBuffer); } SbgErrorCode sbgEComLogMagWriteToStream(const SbgEComLogMag *pLogData, SbgStreamBuffer *pStreamBuffer) { - assert(pStreamBuffer); - assert(pLogData); - - sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); - sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->status); - - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->magnetometers[0]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->magnetometers[1]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->magnetometers[2]); - - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->accelerometers[0]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->accelerometers[1]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->accelerometers[2]); - - return sbgStreamBufferGetLastError(pStreamBuffer); -} - -SbgErrorCode sbgEComLogMagCalibReadFromStream(SbgEComLogMagCalib *pLogData, SbgStreamBuffer *pStreamBuffer) -{ - assert(pStreamBuffer); - assert(pLogData); - - pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); - pLogData->reserved = sbgStreamBufferReadUint16LE(pStreamBuffer); - - return sbgStreamBufferReadBuffer(pStreamBuffer, pLogData->magData, sizeof(pLogData->magData)); -} - -SbgErrorCode sbgEComLogMagCalibWriteToStream(const SbgEComLogMagCalib *pLogData, SbgStreamBuffer *pStreamBuffer) -{ - assert(pStreamBuffer); - assert(pLogData); - - sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); - sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->reserved); - - return sbgStreamBufferWriteBuffer(pStreamBuffer, pLogData->magData, sizeof(pLogData->magData)); + assert(pStreamBuffer); + assert(pLogData); + + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); + sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->status); + + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->magnetometers[0]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->magnetometers[1]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->magnetometers[2]); + + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->accelerometers[0]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->accelerometers[1]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->accelerometers[2]); + + return sbgStreamBufferGetLastError(pStreamBuffer); } //----------------------------------------------------------------------// //- DEPRECATED - Used for backward compatibility -// //----------------------------------------------------------------------// -SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogMag SbgLogMag); -SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogMagCalib SbgLogMagCalib); +SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogMag SbgLogMag); SbgErrorCode sbgEComBinaryLogParseMagData(SbgStreamBuffer *pStreamBuffer, SbgEComLogMag *pLogData) { - return sbgEComLogMagReadFromStream(pLogData, pStreamBuffer); + return sbgEComLogMagReadFromStream(pLogData, pStreamBuffer); } SbgErrorCode sbgEComBinaryLogWriteMagData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogMag *pLogData) { - return sbgEComLogMagWriteToStream(pLogData, pStreamBuffer); -} - -SbgErrorCode sbgEComBinaryLogParseMagCalibData(SbgStreamBuffer *pStreamBuffer, SbgEComLogMagCalib *pLogData) -{ - return sbgEComLogMagCalibReadFromStream(pLogData, pStreamBuffer); -} - -SbgErrorCode sbgEComBinaryLogWriteMagCalibData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogMagCalib *pLogData) -{ - return sbgEComLogMagCalibWriteToStream(pLogData, pStreamBuffer); + return sbgEComLogMagWriteToStream(pLogData, pStreamBuffer); } diff --git a/src/logs/sbgEComLogMag.h b/src/logs/sbgEComLogMag.h index cf78260..836afd4 100644 --- a/src/logs/sbgEComLogMag.h +++ b/src/logs/sbgEComLogMag.h @@ -1,13 +1,13 @@ /*! - * \file sbgEComLogMag.h - * \ingroup binaryLogs - * \author SBG Systems - * \date 12 March 2013 + * \file sbgEComLogMag.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 12 March 2013 * - * \brief Parse magnetic field measurements logs. + * \brief Parse magnetic field measurements logs. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -48,44 +48,37 @@ extern "C" { /*! * 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_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_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_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. */ +#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. + * Represents data from the SBG_ECOM_LOG_MAG message. + * + * This structure encapsulates the magnetic field data, which is calibrated to correct for soft and hard iron effects. It provides + * 3D magnetic field readings intended for use by the INS filter. */ typedef struct _SbgEComLogMag { - 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. */ + 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 arbitrary units (A.U.). */ + float accelerometers[3]; /*!< X, Y, Z accelerometer data in (m/s^2). */ } SbgEComLogMag; -/*! - * Structure that stores data for the SBG_ECOM_LOG_MAG_CALIB message. - */ -typedef struct _SbgEComLogMagCalib -{ - 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. */ -} SbgEComLogMagCalib; - //----------------------------------------------------------------------// //- Public methods -// //----------------------------------------------------------------------// @@ -93,52 +86,30 @@ typedef struct _SbgEComLogMagCalib /*! * Parse data for the SBG_ECOM_LOG_MAG message and fill the corresponding structure. * - * \param[out] pLogData Log structure instance to fill. - * \param[in] pStreamBuffer Input stream buffer to read the log from. - * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. + * \param[out] pLogData Log structure instance to fill. + * \param[in] pStreamBuffer Input stream buffer to read the log from. + * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. */ SbgErrorCode sbgEComLogMagReadFromStream(SbgEComLogMag *pLogData, SbgStreamBuffer *pStreamBuffer); /*! * Write data for the SBG_ECOM_LOG_MAG message to the output stream buffer from the provided structure. * - * \param[in] pLogData Log structure instance to write. - * \param[out] pStreamBuffer Output stream buffer to write the log to. - * \return SBG_NO_ERROR if the log has been written to the stream buffer. + * \param[in] pLogData Log structure instance to write. + * \param[out] pStreamBuffer Output stream buffer to write the log to. + * \return SBG_NO_ERROR if the log has been written to the stream buffer. */ SbgErrorCode sbgEComLogMagWriteToStream(const SbgEComLogMag *pLogData, SbgStreamBuffer *pStreamBuffer); -/*! - * Parse data for the SBG_ECOM_LOG_MAG_CALIB message and fill the corresponding structure. - * - * \param[out] pLogData Log structure instance to fill. - * \param[in] pStreamBuffer Input stream buffer to read the log from. - * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. - */ -SbgErrorCode sbgEComLogMagCalibReadFromStream(SbgEComLogMagCalib *pLogData, SbgStreamBuffer *pStreamBuffer); - -/*! - * Write data for the SBG_ECOM_LOG_MAG_CALIB message to the output stream buffer from the provided structure. - * - * \param[in] pLogData Log structure instance to write. - * \param[out] pStreamBuffer Output stream buffer to write the log to. - * \return SBG_NO_ERROR if the log has been written to the stream buffer. - */ -SbgErrorCode sbgEComLogMagCalibWriteToStream(const SbgEComLogMagCalib *pLogData, SbgStreamBuffer *pStreamBuffer); - //----------------------------------------------------------------------// //- DEPRECATED - Used for backward compatibility -// //----------------------------------------------------------------------// -SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogMag SbgLogMag); -SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogMagCalib SbgLogMagCalib); +SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogMag SbgLogMag); SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogParseMagData(SbgStreamBuffer *pStreamBuffer, SbgEComLogMag *pLogData)); SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogWriteMagData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogMag *pLogData)); -SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogParseMagCalibData(SbgStreamBuffer *pStreamBuffer, SbgEComLogMagCalib *pLogData)); -SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogWriteMagCalibData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogMagCalib *pLogData)); - #ifdef __cplusplus } #endif diff --git a/src/logs/sbgEComLogMagCalib.c b/src/logs/sbgEComLogMagCalib.c new file mode 100644 index 0000000..39c7096 --- /dev/null +++ b/src/logs/sbgEComLogMagCalib.c @@ -0,0 +1,48 @@ +// sbgCommonLib headers +#include +#include + +// Local headers +#include "sbgEComLogMagCalib.h" + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComLogMagCalibReadFromStream(SbgEComLogMagCalib *pLogData, SbgStreamBuffer *pStreamBuffer) +{ + assert(pStreamBuffer); + assert(pLogData); + + pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); + pLogData->reserved = sbgStreamBufferReadUint16LE(pStreamBuffer); + + return sbgStreamBufferReadBuffer(pStreamBuffer, pLogData->magData, sizeof(pLogData->magData)); +} + +SbgErrorCode sbgEComLogMagCalibWriteToStream(const SbgEComLogMagCalib *pLogData, SbgStreamBuffer *pStreamBuffer) +{ + assert(pStreamBuffer); + assert(pLogData); + + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); + sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->reserved); + + return sbgStreamBufferWriteBuffer(pStreamBuffer, pLogData->magData, sizeof(pLogData->magData)); +} + +//----------------------------------------------------------------------// +//- DEPRECATED - Used for backward compatibility -// +//----------------------------------------------------------------------// + +SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogMagCalib SbgLogMagCalib); + +SbgErrorCode sbgEComBinaryLogParseMagCalibData(SbgStreamBuffer *pStreamBuffer, SbgEComLogMagCalib *pLogData) +{ + return sbgEComLogMagCalibReadFromStream(pLogData, pStreamBuffer); +} + +SbgErrorCode sbgEComBinaryLogWriteMagCalibData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogMagCalib *pLogData) +{ + return sbgEComLogMagCalibWriteToStream(pLogData, pStreamBuffer); +} diff --git a/src/logs/sbgEComLogMagCalib.h b/src/logs/sbgEComLogMagCalib.h new file mode 100644 index 0000000..d086c89 --- /dev/null +++ b/src/logs/sbgEComLogMagCalib.h @@ -0,0 +1,100 @@ +/*! + * \file sbgEComLogMagCalib.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 15 Mai 2024 + * + * \brief Parse magnetic calibration logs. + * + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * 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. + * + * \endlicense + */ + +#ifndef SBG_ECOM_LOG_MAG_CALIB_H +#define SBG_ECOM_LOG_MAG_CALIB_H + +// sbgCommonLib headers +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Log structure definitions -// +//----------------------------------------------------------------------// + +/*! + * Represents data from the SBG_ECOM_LOG_MAG_CALIB message. + * + * This structure contains raw magnetic field data that has not been compensated for soft and hard iron effects. It is intended + * to be used with SBG Systems' magnetic calibration algorithms, which calculate the necessary compensation parameters to correct + * for these distortions. + * + * Note: An effective strategy for utilizing this data involves storing the raw data stream that includes this message and later + * processing it with the sbgCenter software to perform offline soft and hard iron magnetic compensation. + */ +typedef struct _SbgEComLogMagCalib +{ + uint32_t timeStamp; /*!< Time in us since the sensor power up. */ + uint16_t reserved; /*!< Reserved for future use, not currently utilized. */ + uint8_t magData[16]; /*!< Raw magnetometer calibration data array, essential for compensation calculations. */ +} SbgEComLogMagCalib; + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * Parse data for the SBG_ECOM_LOG_MAG_CALIB message and fill the corresponding structure. + * + * \param[out] pLogData Log structure instance to fill. + * \param[in] pStreamBuffer Input stream buffer to read the log from. + * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. + */ +SbgErrorCode sbgEComLogMagCalibReadFromStream(SbgEComLogMagCalib *pLogData, SbgStreamBuffer *pStreamBuffer); + +/*! + * Write data for the SBG_ECOM_LOG_MAG_CALIB message to the output stream buffer from the provided structure. + * + * \param[in] pLogData Log structure instance to write. + * \param[out] pStreamBuffer Output stream buffer to write the log to. + * \return SBG_NO_ERROR if the log has been written to the stream buffer. + */ +SbgErrorCode sbgEComLogMagCalibWriteToStream(const SbgEComLogMagCalib *pLogData, SbgStreamBuffer *pStreamBuffer); + +//----------------------------------------------------------------------// +//- DEPRECATED - Used for backward compatibility -// +//----------------------------------------------------------------------// + +SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogMagCalib SbgLogMagCalib); + +SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogParseMagCalibData(SbgStreamBuffer *pStreamBuffer, SbgEComLogMagCalib *pLogData)); +SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogWriteMagCalibData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogMagCalib *pLogData)); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_LOG_MAG_CALIB_H diff --git a/src/logs/sbgEComLogOdometer.c b/src/logs/sbgEComLogOdometer.c index d9c4894..23e5f3e 100644 --- a/src/logs/sbgEComLogOdometer.c +++ b/src/logs/sbgEComLogOdometer.c @@ -6,28 +6,28 @@ SbgErrorCode sbgEComLogOdometerReadFromStream(SbgEComLogOdometer *pLogData, SbgStreamBuffer *pStreamBuffer) { - assert(pStreamBuffer); - assert(pLogData); + assert(pStreamBuffer); + assert(pLogData); - pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); - pLogData->status = sbgStreamBufferReadUint16LE(pStreamBuffer); + pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); + pLogData->status = sbgStreamBufferReadUint16LE(pStreamBuffer); - pLogData->velocity = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->velocity = sbgStreamBufferReadFloatLE(pStreamBuffer); - return sbgStreamBufferGetLastError(pStreamBuffer); + return sbgStreamBufferGetLastError(pStreamBuffer); } SbgErrorCode sbgEComLogOdometerWriteToStream(const SbgEComLogOdometer *pLogData, SbgStreamBuffer *pStreamBuffer) { - assert(pStreamBuffer); - assert(pLogData); + assert(pStreamBuffer); + assert(pLogData); - sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); - sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->status); + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); + sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->status); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocity); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocity); - return sbgStreamBufferGetLastError(pStreamBuffer); + return sbgStreamBufferGetLastError(pStreamBuffer); } //----------------------------------------------------------------------// @@ -36,10 +36,10 @@ SbgErrorCode sbgEComLogOdometerWriteToStream(const SbgEComLogOdometer *pLogData, SbgErrorCode sbgEComBinaryLogParseOdometerData(SbgStreamBuffer *pStreamBuffer, SbgEComLogOdometer *pLogData) { - return sbgEComLogOdometerReadFromStream(pLogData, pStreamBuffer); + return sbgEComLogOdometerReadFromStream(pLogData, pStreamBuffer); } SbgErrorCode sbgEComBinaryLogWriteOdometerData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogOdometer *pLogData) { - return sbgEComLogOdometerWriteToStream(pLogData, pStreamBuffer); + return sbgEComLogOdometerWriteToStream(pLogData, pStreamBuffer); } diff --git a/src/logs/sbgEComLogOdometer.h b/src/logs/sbgEComLogOdometer.h index ec8041e..ba4df38 100644 --- a/src/logs/sbgEComLogOdometer.h +++ b/src/logs/sbgEComLogOdometer.h @@ -1,13 +1,13 @@ /*! - * \file sbgEComLogOdometer.h - * \ingroup binaryLogs - * \author SBG Systems - * \date 25 February 2013 + * \file sbgEComLogOdometer.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 25 February 2013 * - * \brief Parse received odometer/DMI velocity measurement logs. + * \brief Parse received odometer/DMI velocity measurement logs. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -48,8 +48,8 @@ extern "C" { /*! * 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. */ +#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 -// @@ -60,9 +60,9 @@ extern "C" { */ typedef struct _SbgEComLogOdometer { - 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. */ + 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. */ } SbgEComLogOdometer; //----------------------------------------------------------------------// @@ -72,18 +72,18 @@ typedef struct _SbgEComLogOdometer /*! * Parse data for the SBG_ECOM_LOG_ODO_VEL message and fill the corresponding structure. * - * \param[out] pLogData Log structure instance to fill. - * \param[in] pStreamBuffer Input stream buffer to read the log from. - * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. + * \param[out] pLogData Log structure instance to fill. + * \param[in] pStreamBuffer Input stream buffer to read the log from. + * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. */ SbgErrorCode sbgEComLogOdometerReadFromStream(SbgEComLogOdometer *pLogData, SbgStreamBuffer *pStreamBuffer); /*! * Write data for the SBG_ECOM_LOG_ODO_VEL message to the output stream buffer from the provided structure. * - * \param[in] pLogData Log structure instance to write. - * \param[out] pStreamBuffer Output stream buffer to write the log to. - * \return SBG_NO_ERROR if the log has been written to the stream buffer. + * \param[in] pLogData Log structure instance to write. + * \param[out] pStreamBuffer Output stream buffer to write the log to. + * \return SBG_NO_ERROR if the log has been written to the stream buffer. */ SbgErrorCode sbgEComLogOdometerWriteToStream(const SbgEComLogOdometer *pLogData, SbgStreamBuffer *pStreamBuffer); diff --git a/src/logs/sbgEComLogPtp.c b/src/logs/sbgEComLogPtp.c new file mode 100644 index 0000000..76224de --- /dev/null +++ b/src/logs/sbgEComLogPtp.c @@ -0,0 +1,187 @@ +// sbgCommonLib headers +#include +#include + +// Local headers +#include "sbgEComLogPtp.h" + +//----------------------------------------------------------------------// +//- Private functions -// +//----------------------------------------------------------------------// + +/*! + * Convert a value into a state. + * + * \param[in] value Value. + * \param[out] pState State. + * \return SBG_NO_ERROR if successful. + */ +static SbgErrorCode sbgEComLogPtpConvertState(uint8_t value, SbgEComLogPtpState *pState) +{ + SbgErrorCode errorCode = SBG_INVALID_PARAMETER; + + assert(pState); + + switch ((SbgEComLogPtpState)value) + { + case SBG_ECOM_LOG_PTP_STATE_DISABLED: + case SBG_ECOM_LOG_PTP_STATE_FAULTY: + case SBG_ECOM_LOG_PTP_STATE_MASTER: + case SBG_ECOM_LOG_PTP_STATE_PASSIVE: + *pState = (SbgEComLogPtpState)value; + errorCode = SBG_NO_ERROR; + break; + } + + if (errorCode == SBG_INVALID_PARAMETER) + { + SBG_LOG_ERROR(errorCode, "unable to convert state: invalid value %" PRIu8, value); + } + + return errorCode; +} + +/*! + * Convert a value into a time scale. + * + * \param[in] value Value. + * \param[out] pTimeScale Time scale. + * \return SBG_NO_ERROR if successful. + */ +static SbgErrorCode sbgEComLogPtpConvertTimeScale(uint8_t value, SbgEComLogPtpTimeScale *pTimeScale) +{ + SbgErrorCode errorCode = SBG_INVALID_PARAMETER; + + assert(pTimeScale); + + switch ((SbgEComLogPtpTimeScale)value) + { + case SBG_ECOM_LOG_PTP_TIME_SCALE_TAI: + case SBG_ECOM_LOG_PTP_TIME_SCALE_UTC: + case SBG_ECOM_LOG_PTP_TIME_SCALE_GPS: + *pTimeScale = (SbgEComLogPtpTimeScale)value; + errorCode = SBG_NO_ERROR; + break; + } + + if (errorCode == SBG_INVALID_PARAMETER) + { + SBG_LOG_ERROR(errorCode, "unable to convert time scale: invalid value %" PRIu8, value); + } + + return errorCode; +} + +//----------------------------------------------------------------------// +//- Public functions -// +//----------------------------------------------------------------------// + +void sbgEComLogPtpZeroInit(SbgEComLogPtp *pLogData) +{ + assert(pLogData); + + memset(pLogData, 0, sizeof(*pLogData)); + + pLogData->state = SBG_ECOM_LOG_PTP_STATE_FAULTY; + pLogData->timeScale = SBG_ECOM_LOG_PTP_TIME_SCALE_TAI; + pLogData->timeScaleOffset = 0.0; + pLogData->localClockIdentity = UINT64_MAX; + pLogData->masterClockIdentity = UINT64_MAX; + pLogData->masterIpAddress = UINT32_MAX; + pLogData->meanPathDelay = NAN; + pLogData->meanPathDelayStdDev = NAN; + pLogData->clockOffset = NAN; + pLogData->clockOffsetStdDev = NAN; + pLogData->clockFreqOffset = NAN; + pLogData->clockFreqOffsetStdDev = NAN; +} + +SbgErrorCode sbgEComLogPtpReadFromStream(SbgEComLogPtp *pLogData, SbgStreamBuffer *pStreamBuffer) +{ + SbgErrorCode errorCode; + uint8_t state; + uint8_t timeScale; + + assert(pStreamBuffer); + assert(pLogData); + + pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); + state = sbgStreamBufferReadUint8(pStreamBuffer); + timeScale = sbgStreamBufferReadUint8(pStreamBuffer); + pLogData->timeScaleOffset = sbgStreamBufferReadDoubleLE(pStreamBuffer); + + pLogData->localClockIdentity = sbgStreamBufferReadUint64LE(pStreamBuffer); + pLogData->localClockPriority1 = sbgStreamBufferReadUint8(pStreamBuffer); + pLogData->localClockPriority2 = sbgStreamBufferReadUint8(pStreamBuffer); + pLogData->localClockClass = sbgStreamBufferReadUint8(pStreamBuffer); + pLogData->localClockAccuracy = sbgStreamBufferReadUint8(pStreamBuffer); + pLogData->localClockLog2Variance = sbgStreamBufferReadUint16LE(pStreamBuffer); + pLogData->localClockTimeSource = sbgStreamBufferReadUint8(pStreamBuffer); + + pLogData->masterClockIdentity = sbgStreamBufferReadUint64LE(pStreamBuffer); + pLogData->masterClockPriority1 = sbgStreamBufferReadUint8(pStreamBuffer); + pLogData->masterClockPriority2 = sbgStreamBufferReadUint8(pStreamBuffer); + pLogData->masterClockClass = sbgStreamBufferReadUint8(pStreamBuffer); + pLogData->masterClockAccuracy = sbgStreamBufferReadUint8(pStreamBuffer); + pLogData->masterClockLog2Variance = sbgStreamBufferReadUint16LE(pStreamBuffer); + pLogData->masterClockTimeSource = sbgStreamBufferReadUint8(pStreamBuffer); + pLogData->masterIpAddress = sbgStreamBufferReadUint32LE(pStreamBuffer); + + pLogData->meanPathDelay = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->meanPathDelayStdDev = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->clockOffset = sbgStreamBufferReadDoubleLE(pStreamBuffer); + pLogData->clockOffsetStdDev = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->clockFreqOffset = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->clockFreqOffsetStdDev = sbgStreamBufferReadFloatLE(pStreamBuffer); + + errorCode = sbgStreamBufferGetLastError(pStreamBuffer); + + if (errorCode == SBG_NO_ERROR) + { + errorCode = sbgEComLogPtpConvertState(state, &pLogData->state); + } + + if (errorCode == SBG_NO_ERROR) + { + errorCode = sbgEComLogPtpConvertTimeScale(timeScale, &pLogData->timeScale); + } + + return errorCode; +} + +SbgErrorCode sbgEComLogPtpWriteToStream(const SbgEComLogPtp *pLogData, SbgStreamBuffer *pStreamBuffer) +{ + assert(pStreamBuffer); + assert(pLogData); + + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); + sbgStreamBufferWriteUint8(pStreamBuffer, pLogData->state); + sbgStreamBufferWriteUint8(pStreamBuffer, pLogData->timeScale); + sbgStreamBufferWriteDoubleLE(pStreamBuffer, pLogData->timeScaleOffset); + + sbgStreamBufferWriteUint64LE(pStreamBuffer, pLogData->localClockIdentity); + sbgStreamBufferWriteUint8(pStreamBuffer, pLogData->localClockPriority1); + sbgStreamBufferWriteUint8(pStreamBuffer, pLogData->localClockPriority2); + sbgStreamBufferWriteUint8(pStreamBuffer, pLogData->localClockClass); + sbgStreamBufferWriteUint8(pStreamBuffer, pLogData->localClockAccuracy); + sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->localClockLog2Variance); + sbgStreamBufferWriteUint8(pStreamBuffer, pLogData->localClockTimeSource); + + sbgStreamBufferWriteUint64LE(pStreamBuffer, pLogData->masterClockIdentity); + sbgStreamBufferWriteUint8(pStreamBuffer, pLogData->masterClockPriority1); + sbgStreamBufferWriteUint8(pStreamBuffer, pLogData->masterClockPriority2); + sbgStreamBufferWriteUint8(pStreamBuffer, pLogData->masterClockClass); + sbgStreamBufferWriteUint8(pStreamBuffer, pLogData->masterClockAccuracy); + sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->masterClockLog2Variance); + sbgStreamBufferWriteUint8(pStreamBuffer, pLogData->masterClockTimeSource); + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->masterIpAddress); + + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->meanPathDelay); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->meanPathDelayStdDev); + sbgStreamBufferWriteDoubleLE(pStreamBuffer, pLogData->clockOffset); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->clockOffsetStdDev); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->clockFreqOffset); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->clockFreqOffsetStdDev); + + return sbgStreamBufferGetLastError(pStreamBuffer); +} diff --git a/src/logs/sbgEComLogPtp.h b/src/logs/sbgEComLogPtp.h new file mode 100644 index 0000000..1bd4f71 --- /dev/null +++ b/src/logs/sbgEComLogPtp.h @@ -0,0 +1,151 @@ +/*! + * \file sbgEComLogPtp.h + * \ingroup binaryLogs + * \author SBG Systems + * \date Monday 17, 2024 + * + * \brief Parse logs used to report device PTP status. + * + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * 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. + * + * \endlicense + */ + +#ifndef SBG_ECOM_LOG_PTP_H +#define SBG_ECOM_LOG_PTP_H + +// sbgCommonLib headers +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Enumeration definitions -// +//----------------------------------------------------------------------// + +/*! + * PTP states. + * + * These are on-the-wire values. + */ +typedef enum _SbgEComLogPtpState +{ + SBG_ECOM_LOG_PTP_STATE_DISABLED = 0, /*!< PTP is disabled. */ + SBG_ECOM_LOG_PTP_STATE_FAULTY = 1, /*!< The device is in the faulty state. */ + SBG_ECOM_LOG_PTP_STATE_MASTER = 2, /*!< The device is the domain master. */ + SBG_ECOM_LOG_PTP_STATE_PASSIVE = 3, /*!< The device is passive. */ +} SbgEComLogPtpState; + +/*! + * Time scales. + * + * These are on-the-wire values. + */ +typedef enum _SbgEComLogPtpTimeScale +{ + SBG_ECOM_LOG_PTP_TIME_SCALE_TAI = 0, /*!< TAI. */ + SBG_ECOM_LOG_PTP_TIME_SCALE_UTC = 1, /*!< UTC. */ + SBG_ECOM_LOG_PTP_TIME_SCALE_GPS = 2, /*!< GPS. */ +} SbgEComLogPtpTimeScale; + +//----------------------------------------------------------------------// +//- Log structure definitions -// +//----------------------------------------------------------------------// + +/*! + * Structure that stores data for the SBG_ECOM_LOG_PTP_STATUS message. + * + * The local clock members are valid only if the local clock identity is valid. + * Similarly, the master clock members are valid only if the master clock identity is valid. + * + * The mean path delay, clock offset, and clock frequency offset, as well as their standard + * deviations, are set to NAN if invalid or unknown. + */ +typedef struct _SbgEComLogPtp +{ + uint32_t timeStamp; /*!< Timestamp, in us . */ + SbgEComLogPtpState state; /*!< State. */ + SbgEComLogPtpTimeScale timeScale; /*!< Internal time scale. */ + double timeScaleOffset; /*!< Internal time scale offset, in seconds. */ + + uint64_t localClockIdentity; /*!< Local clock identity, UINT64_MAX if invalid. */ + uint8_t localClockPriority1; /*!< Local clock priority1 attribute. */ + uint8_t localClockPriority2; /*!< Local clock priority2 attribute. */ + uint8_t localClockClass; /*!< Local clock class attribute. */ + uint8_t localClockAccuracy; /*!< Local clock accuracy. */ + uint16_t localClockLog2Variance; /*!< Local clock PTP variance. */ + uint8_t localClockTimeSource; /*!< Local clock time source. */ + + uint64_t masterClockIdentity; /*!< Master clock identity, UINT64_MAX if invalid. */ + uint8_t masterClockPriority1; /*!< Master clock priority1 attribute. */ + uint8_t masterClockPriority2; /*!< Master clock priority2 attribute. */ + uint8_t masterClockClass; /*!< Master clock class attribute. */ + uint8_t masterClockAccuracy; /*!< Master clock accuracy. */ + uint16_t masterClockLog2Variance; /*!< Master clock PTP variance. */ + uint8_t masterClockTimeSource; /*!< Master clock time source. */ + uint32_t masterIpAddress; /*!< Master clock IP address, UINT32_MAX if invalid. */ + + float meanPathDelay; /*!< Mean path delay to/from the master clock, in seconds. */ + float meanPathDelayStdDev; /*!< Mean path delay standard deviation, in seconds. */ + double clockOffset; /*!< Offset between the local and master clocks, in seconds. */ + float clockOffsetStdDev; /*!< Master clock offset standard deviation, in seconds. */ + float clockFreqOffset; /*!< Offset between the frequency of the local and master clocks, in Hz. */ + float clockFreqOffsetStdDev; /*!< Frequency offset standard deviation, in Hz. */ +} SbgEComLogPtp; + +//----------------------------------------------------------------------// +//- Public functions -// +//----------------------------------------------------------------------// + +/*! + * Zero initialize the message struct. + * + * \param[out] pLogData Structure instance to zero init. + */ +void sbgEComLogPtpZeroInit(SbgEComLogPtp *pLogData); + +/*! + * Parse data for the SBG_ECOM_LOG_PTP_STATUS message and fill the corresponding structure. + * + * \param[out] pLogData Log structure instance to fill. + * \param[in] pStreamBuffer Input stream buffer to read the log from. + * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. + */ +SbgErrorCode sbgEComLogPtpReadFromStream(SbgEComLogPtp *pLogData, SbgStreamBuffer *pStreamBuffer); + +/*! + * Write data for the SBG_ECOM_LOG_PTP_STATUS message to the output stream buffer from the provided structure. + * + * \param[in] pLogData Log structure instance to write. + * \param[out] pStreamBuffer Output stream buffer to write the log to. + * \return SBG_NO_ERROR if the log has been written to the stream buffer. + */ +SbgErrorCode sbgEComLogPtpWriteToStream(const SbgEComLogPtp *pLogData, SbgStreamBuffer *pStreamBuffer); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_LOG_PTP_H diff --git a/src/logs/sbgEComLogRawData.c b/src/logs/sbgEComLogRawData.c index 1c6365f..eb1e4fa 100644 --- a/src/logs/sbgEComLogRawData.c +++ b/src/logs/sbgEComLogRawData.c @@ -10,33 +10,33 @@ SbgErrorCode sbgEComLogRawDataReadFromStream(SbgEComLogRawData *pLogData, SbgStreamBuffer *pStreamBuffer) { - SbgErrorCode errorCode = SBG_NO_ERROR; - size_t payloadSize; + SbgErrorCode errorCode = SBG_NO_ERROR; + size_t payloadSize; - assert(pStreamBuffer); - assert(pLogData); + assert(pStreamBuffer); + assert(pLogData); - payloadSize = sbgStreamBufferGetSize(pStreamBuffer); + payloadSize = sbgStreamBufferGetSize(pStreamBuffer); - if (payloadSize <= SBG_ECOM_RAW_DATA_MAX_BUFFER_SIZE) - { - errorCode = sbgStreamBufferReadBuffer(pStreamBuffer, pLogData->rawBuffer, payloadSize); - pLogData->bufferSize = payloadSize; - } - else - { - errorCode = SBG_BUFFER_OVERFLOW; - } + if (payloadSize <= SBG_ECOM_RAW_DATA_MAX_BUFFER_SIZE) + { + errorCode = sbgStreamBufferReadBuffer(pStreamBuffer, pLogData->rawBuffer, payloadSize); + pLogData->bufferSize = payloadSize; + } + else + { + errorCode = SBG_BUFFER_OVERFLOW; + } - return errorCode; + return errorCode; } SbgErrorCode sbgEComLogRawDataWriteToStream(const SbgEComLogRawData *pLogData, SbgStreamBuffer *pStreamBuffer) { - assert(pStreamBuffer); - assert(pLogData); + assert(pStreamBuffer); + assert(pLogData); - return sbgStreamBufferWriteBuffer(pStreamBuffer, pLogData->rawBuffer, pLogData->bufferSize); + return sbgStreamBufferWriteBuffer(pStreamBuffer, pLogData->rawBuffer, pLogData->bufferSize); } //----------------------------------------------------------------------// @@ -45,10 +45,10 @@ SbgErrorCode sbgEComLogRawDataWriteToStream(const SbgEComLogRawData *pLogData, S SbgErrorCode sbgEComBinaryLogParseRawData(SbgStreamBuffer *pStreamBuffer, SbgEComLogRawData *pLogData) { - return sbgEComLogRawDataReadFromStream(pLogData, pStreamBuffer); + return sbgEComLogRawDataReadFromStream(pLogData, pStreamBuffer); } SbgErrorCode sbgEComBinaryLogWriteRawData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogRawData *pLogData) { - return sbgEComLogRawDataWriteToStream(pLogData, pStreamBuffer); + return sbgEComLogRawDataWriteToStream(pLogData, pStreamBuffer); } diff --git a/src/logs/sbgEComLogRawData.h b/src/logs/sbgEComLogRawData.h index 0769d35..8fd0199 100644 --- a/src/logs/sbgEComLogRawData.h +++ b/src/logs/sbgEComLogRawData.h @@ -1,13 +1,13 @@ /*! - * \file sbgEComLogRawData.h - * \ingroup binaryLogs - * \author SBG Systems - * \date 16 November 2020 + * \file sbgEComLogRawData.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 16 November 2020 * - * \brief Parse logs used to store a binary stream such as RAW GNSS or RTCM stream. + * \brief Parse logs used to store a binary stream such as RAW GNSS or RTCM stream. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -45,7 +45,7 @@ extern "C" { //- Log raw Data const definitions -// //----------------------------------------------------------------------// -#define SBG_ECOM_RAW_DATA_MAX_BUFFER_SIZE (4086u) /*!< Maximum buffer size in bytes that can be stored in the raw data log. */ +#define SBG_ECOM_RAW_DATA_MAX_BUFFER_SIZE (4086u) /*!< Maximum buffer size in bytes that can be stored in the raw data log. */ //----------------------------------------------------------------------// //- Log structure definitions -// @@ -56,8 +56,8 @@ extern "C" { */ typedef struct _SbgEComLogRawData { - uint8_t rawBuffer[SBG_ECOM_RAW_DATA_MAX_BUFFER_SIZE]; /*!< Buffer that contains raw data. */ - size_t bufferSize; /*!< Raw buffer size in bytes. */ + uint8_t rawBuffer[SBG_ECOM_RAW_DATA_MAX_BUFFER_SIZE]; /*!< Buffer that contains raw data. */ + size_t bufferSize; /*!< Raw buffer size in bytes. */ } SbgEComLogRawData; //----------------------------------------------------------------------// @@ -67,18 +67,18 @@ typedef struct _SbgEComLogRawData /*! * Parse raw data message and fill the corresponding structure. * - * \param[out] pLogData Log structure instance to fill. - * \param[in] pStreamBuffer Input stream buffer to read the log from. - * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. + * \param[out] pLogData Log structure instance to fill. + * \param[in] pStreamBuffer Input stream buffer to read the log from. + * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. */ SbgErrorCode sbgEComLogRawDataReadFromStream(SbgEComLogRawData *pLogData, SbgStreamBuffer *pStreamBuffer); /*! * Write raw data message to the output stream buffer from the provided structure. * - * \param[in] pLogData Log structure instance to write. - * \param[out] pStreamBuffer Output stream buffer to write the log to. - * \return SBG_NO_ERROR if the log has been written to the stream buffer. + * \param[in] pLogData Log structure instance to write. + * \param[out] pStreamBuffer Output stream buffer to write the log to. + * \return SBG_NO_ERROR if the log has been written to the stream buffer. */ SbgErrorCode sbgEComLogRawDataWriteToStream(const SbgEComLogRawData *pLogData, SbgStreamBuffer *pStreamBuffer); diff --git a/src/logs/sbgEComLogSat.c b/src/logs/sbgEComLogSat.c index 58ee8f3..892fe15 100644 --- a/src/logs/sbgEComLogSat.c +++ b/src/logs/sbgEComLogSat.c @@ -1,12 +1,12 @@ /*! - * \file sbgEComLogSat.c - * \author SBG Systems - * \date 1 March 2022 + * \file sbgEComLogSat.c + * \author SBG Systems + * \date 1 March 2022 * - * \brief Handle binary satellite logs. + * \brief Handle binary satellite logs. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -43,23 +43,23 @@ //- Constant definitions -// //----------------------------------------------------------------------// -#define SBG_ECOM_LOG_SAT_TRACKING_STATUS_OFFSET (0) /*!< Offset of the tracking status field, in bits. */ -#define SBG_ECOM_LOG_SAT_TRACKING_STATUS_WIDTH (3) /*!< Width of the tracking status field, in bits. */ -#define SBG_ECOM_LOG_SAT_TRACKING_STATUS_MASK ((1u << SBG_ECOM_LOG_SAT_TRACKING_STATUS_WIDTH) - 1) /*!< Tracking status field mask. */ +#define SBG_ECOM_LOG_SAT_TRACKING_STATUS_OFFSET (0) /*!< Offset of the tracking status field, in bits. */ +#define SBG_ECOM_LOG_SAT_TRACKING_STATUS_WIDTH (3) /*!< Width of the tracking status field, in bits. */ +#define SBG_ECOM_LOG_SAT_TRACKING_STATUS_MASK ((1u << SBG_ECOM_LOG_SAT_TRACKING_STATUS_WIDTH) - 1) /*!< Tracking status field mask. */ -#define SBG_ECOM_LOG_SAT_HEALTH_STATUS_OFFSET (3) /*!< Offset of the health status field, in bits. */ -#define SBG_ECOM_LOG_SAT_HEALTH_STATUS_WIDTH (2) /*!< Width of the health status field, in bits. */ -#define SBG_ECOM_LOG_SAT_HEALTH_STATUS_MASK ((1u << SBG_ECOM_LOG_SAT_HEALTH_STATUS_WIDTH) - 1) /*!< Health status field mask. */ +#define SBG_ECOM_LOG_SAT_HEALTH_STATUS_OFFSET (3) /*!< Offset of the health status field, in bits. */ +#define SBG_ECOM_LOG_SAT_HEALTH_STATUS_WIDTH (2) /*!< Width of the health status field, in bits. */ +#define SBG_ECOM_LOG_SAT_HEALTH_STATUS_MASK ((1u << SBG_ECOM_LOG_SAT_HEALTH_STATUS_WIDTH) - 1) /*!< Health status field mask. */ -#define SBG_ECOM_LOG_SAT_ELEVATION_STATUS_OFFSET (5) /*!< Offset of the elevation status field, in bits. */ -#define SBG_ECOM_LOG_SAT_ELEVATION_STATUS_WIDTH (2) /*!< Width of the elevation status field, in bits. */ -#define SBG_ECOM_LOG_SAT_ELEVATION_STATUS_MASK ((1u << SBG_ECOM_LOG_SAT_ELEVATION_STATUS_WIDTH) - 1) /*!< Elevation status field mask. */ +#define SBG_ECOM_LOG_SAT_ELEVATION_STATUS_OFFSET (5) /*!< Offset of the elevation status field, in bits. */ +#define SBG_ECOM_LOG_SAT_ELEVATION_STATUS_WIDTH (2) /*!< Width of the elevation status field, in bits. */ +#define SBG_ECOM_LOG_SAT_ELEVATION_STATUS_MASK ((1u << SBG_ECOM_LOG_SAT_ELEVATION_STATUS_WIDTH) - 1) /*!< Elevation status field mask. */ -#define SBG_ECOM_LOG_SAT_CONSTELLATION_ID_OFFSET (7) /*!< Offset of the constellation ID field, in bits. */ -#define SBG_ECOM_LOG_SAT_CONSTELLATION_ID_WIDTH (4) /*!< Width of the constellation ID field, in bits. */ -#define SBG_ECOM_LOG_SAT_CONSTELLATION_ID_MASK ((1u << SBG_ECOM_LOG_SAT_CONSTELLATION_ID_WIDTH) - 1) /*!< Constellation ID field mask. */ +#define SBG_ECOM_LOG_SAT_CONSTELLATION_ID_OFFSET (7) /*!< Offset of the constellation ID field, in bits. */ +#define SBG_ECOM_LOG_SAT_CONSTELLATION_ID_WIDTH (4) /*!< Width of the constellation ID field, in bits. */ +#define SBG_ECOM_LOG_SAT_CONSTELLATION_ID_MASK ((1u << SBG_ECOM_LOG_SAT_CONSTELLATION_ID_WIDTH) - 1) /*!< Constellation ID field mask. */ -#define SBG_ECOM_LOG_SAT_SIGNAL_SNR_VALID (1u << 5) /*!< Set if the SNR value is valid. */ +#define SBG_ECOM_LOG_SAT_SIGNAL_SNR_VALID (1u << 5) /*!< Set if the SNR value is valid. */ //----------------------------------------------------------------------// //- Private functions -// @@ -68,143 +68,143 @@ /*! * Get a bit field from a set of flags. * - * \param[in] flags Flags. - * \param[in] offset Field offset, in bits. - * \param[in] mask Field mask. - * \return Field value. + * \param[in] flags Flags. + * \param[in] offset Field offset, in bits. + * \param[in] mask Field mask. + * \return Field value. */ -#define sbgEComLogSatGetField(flags, offset, mask) (((flags) >> (offset)) & (mask)) +#define sbgEComLogSatGetField(flags, offset, mask) (((flags) >> (offset)) & (mask)) /*! * Set a bit field from a set of flags. * - * \param[in/out] flags Flags. - * \param[in] value Field value. - * \param[in] offset Field offset, in bits. - * \param[in] mask Field mask. + * \param[in/out] flags Flags. + * \param[in] value Field value. + * \param[in] offset Field offset, in bits. + * \param[in] mask Field mask. */ -#define sbgEComLogSatSetField(flags, value, offset, mask) (flags) &= ~((mask) << (offset)); (flags) |= ((value) & (mask)) << (offset) +#define sbgEComLogSatSetField(flags, value, offset, mask) (flags) &= ~((mask) << (offset)); (flags) |= ((value) & (mask)) << (offset) /*! * Returns tracking status string from enum value * - * \param[in] trackingStatus Tracking status enum to convert - * \return Tracking status as a read only C string. + * \param[in] trackingStatus Tracking status enum to convert + * \return Tracking status as a read only C string. */ static const char *sbgEComLogSatTrackingStatusToStr(SbgEComSatTrackingStatus trackingStatus) { - static const char *enumToStrLut[] = - { - [SBG_ECOM_SAT_TRACKING_STATUS_UNKNOWN] = "unknown", - [SBG_ECOM_SAT_TRACKING_STATUS_SEARCHING] = "searching", - [SBG_ECOM_SAT_TRACKING_STATUS_TRACKING_UNKNOWN] = "tracking", - [SBG_ECOM_SAT_TRACKING_STATUS_TRACKING_NOT_USED] = "unused", - [SBG_ECOM_SAT_TRACKING_STATUS_TRACKING_REJECTED] = "rejected", - [SBG_ECOM_SAT_TRACKING_STATUS_TRACKING_USED] = "used", - }; - - if (trackingStatus < SBG_ARRAY_SIZE(enumToStrLut)) - { - return enumToStrLut[trackingStatus]; - } - else - { - return enumToStrLut[SBG_ECOM_SAT_TRACKING_STATUS_UNKNOWN]; - } + static const char *enumToStrLut[] = + { + [SBG_ECOM_SAT_TRACKING_STATUS_UNKNOWN] = "unknown", + [SBG_ECOM_SAT_TRACKING_STATUS_SEARCHING] = "searching", + [SBG_ECOM_SAT_TRACKING_STATUS_TRACKING_UNKNOWN] = "tracking", + [SBG_ECOM_SAT_TRACKING_STATUS_TRACKING_NOT_USED] = "unused", + [SBG_ECOM_SAT_TRACKING_STATUS_TRACKING_REJECTED] = "rejected", + [SBG_ECOM_SAT_TRACKING_STATUS_TRACKING_USED] = "used", + }; + + if (trackingStatus < SBG_ARRAY_SIZE(enumToStrLut)) + { + return enumToStrLut[trackingStatus]; + } + else + { + return enumToStrLut[SBG_ECOM_SAT_TRACKING_STATUS_UNKNOWN]; + } } /*! * Returns the best tracking status between the two inputs. * - * \param[in] trackingStatus1 First tracking status. - * \param[in] trackingStatus2 Second tracking status. + * \param[in] trackingStatus1 First tracking status. + * \param[in] trackingStatus2 Second tracking status. */ static SbgEComSatTrackingStatus sbgEComLogSatSelectTrackingStatus(SbgEComSatTrackingStatus trackingStatus1, SbgEComSatTrackingStatus trackingStatus2) { - SbgEComSatTrackingStatus trackingStatus; + SbgEComSatTrackingStatus trackingStatus; - if (trackingStatus2 > trackingStatus1) - { - trackingStatus = trackingStatus2; - } - else - { - trackingStatus = trackingStatus1; - } + if (trackingStatus2 > trackingStatus1) + { + trackingStatus = trackingStatus2; + } + else + { + trackingStatus = trackingStatus1; + } - return trackingStatus; + return trackingStatus; } /*! * Returns health status string from enum value * - * \param[in] trackingStatus Tracking status enum to convert - * \return Tracking status as a read only C string. + * \param[in] trackingStatus Tracking status enum to convert + * \return Tracking status as a read only C string. */ static const char *sbgEComLogSatHealthStatusToStr(SbgEComSatHealthStatus healthStatus) { - static const char *enumToStrLut[] = - { - [SBG_ECOM_SAT_HEALTH_STATUS_UNKNOWN] = "unknown", - [SBG_ECOM_SAT_HEALTH_STATUS_HEALTHY] = "healthy", - [SBG_ECOM_SAT_HEALTH_STATUS_UNHEALTHY] = "unhealthy", - }; + static const char *enumToStrLut[] = + { + [SBG_ECOM_SAT_HEALTH_STATUS_UNKNOWN] = "unknown", + [SBG_ECOM_SAT_HEALTH_STATUS_HEALTHY] = "healthy", + [SBG_ECOM_SAT_HEALTH_STATUS_UNHEALTHY] = "unhealthy", + }; - if (healthStatus < SBG_ARRAY_SIZE(enumToStrLut)) - { - return enumToStrLut[healthStatus]; - } - else - { - return enumToStrLut[SBG_ECOM_SAT_HEALTH_STATUS_UNKNOWN]; - } + if (healthStatus < SBG_ARRAY_SIZE(enumToStrLut)) + { + return enumToStrLut[healthStatus]; + } + else + { + return enumToStrLut[SBG_ECOM_SAT_HEALTH_STATUS_UNKNOWN]; + } } /*! * Returns the worst health status between the two inputs. * - * \param[in] healthStatus1 First health status. - * \param[in] healthStatus2 Second health status. + * \param[in] healthStatus1 First health status. + * \param[in] healthStatus2 Second health status. */ static SbgEComSatHealthStatus sbgEComLogSatSelectHealthStatus(SbgEComSatHealthStatus healthStatus1, SbgEComSatHealthStatus healthStatus2) { - SbgEComSatHealthStatus healthStatus; + SbgEComSatHealthStatus healthStatus; - if (healthStatus2 > healthStatus1) - { - healthStatus = healthStatus2; - } - else - { - healthStatus = healthStatus1; - } + if (healthStatus2 > healthStatus1) + { + healthStatus = healthStatus2; + } + else + { + healthStatus = healthStatus1; + } - return healthStatus; + return healthStatus; } /*! * Returns the elevation status string from enum value * - * \param[in] elevationStatus Elevation status enum to convert - * \return Elevation status as a read only C string. + * \param[in] elevationStatus Elevation status enum to convert + * \return Elevation status as a read only C string. */ static const char *sbgEComLogSatElevationStatusToStr(SbgEComSatElevationStatus elevationStatus) { - static const char *enumToStrLut[] = - { - [SBG_ECOM_SAT_ELEVATION_STATUS_UNKNOWN] = "unknown", - [SBG_ECOM_SAT_ELEVATION_STATUS_SETTING] = "setting", - [SBG_ECOM_SAT_ELEVATION_STATUS_RISING] = "rising", - }; + static const char *enumToStrLut[] = + { + [SBG_ECOM_SAT_ELEVATION_STATUS_UNKNOWN] = "unknown", + [SBG_ECOM_SAT_ELEVATION_STATUS_SETTING] = "setting", + [SBG_ECOM_SAT_ELEVATION_STATUS_RISING] = "rising", + }; - if (elevationStatus < SBG_ARRAY_SIZE(enumToStrLut)) - { - return enumToStrLut[elevationStatus]; - } - else - { - return enumToStrLut[SBG_ECOM_SAT_ELEVATION_STATUS_UNKNOWN]; - } + if (elevationStatus < SBG_ARRAY_SIZE(enumToStrLut)) + { + return enumToStrLut[elevationStatus]; + } + else + { + return enumToStrLut[SBG_ECOM_SAT_ELEVATION_STATUS_UNKNOWN]; + } } //----------------------------------------------------------------------// @@ -214,39 +214,39 @@ static const char *sbgEComLogSatElevationStatusToStr(SbgEComSatElevationStatus e /*! * Parse a satellite signal instance from a stream buffer. * - * \param[out] pSatSignal Satellite signal instance. - * \param[in] pStreamBuffer Stream buffer. - * \return SBG_NO_ERROR if successful. + * \param[out] pSatSignal Satellite signal instance. + * \param[in] pStreamBuffer Stream buffer. + * \return SBG_NO_ERROR if successful. */ static SbgErrorCode sbgEComLogSatSignalReadFromStream(SbgEComLogSatSignal *pSatSignal, SbgStreamBuffer *pStreamBuffer) { - assert(pSatSignal); - assert(pStreamBuffer); + assert(pSatSignal); + assert(pStreamBuffer); - pSatSignal->id = sbgStreamBufferReadUint8LE(pStreamBuffer); - pSatSignal->flags = sbgStreamBufferReadUint8LE(pStreamBuffer); - pSatSignal->snr = sbgStreamBufferReadUint8LE(pStreamBuffer); + pSatSignal->id = sbgStreamBufferReadUint8LE(pStreamBuffer); + pSatSignal->flags = sbgStreamBufferReadUint8LE(pStreamBuffer); + pSatSignal->snr = sbgStreamBufferReadUint8LE(pStreamBuffer); - return sbgStreamBufferGetLastError(pStreamBuffer); + return sbgStreamBufferGetLastError(pStreamBuffer); } /*! * Write a satellite signal instance to a stream buffer. * - * \param[in] pSatSignal Satellite signal instance. - * \param[out] pStreamBuffer Stream buffer. - * \return SBG_NO_ERROR if successful. + * \param[in] pSatSignal Satellite signal instance. + * \param[out] pStreamBuffer Stream buffer. + * \return SBG_NO_ERROR if successful. */ static SbgErrorCode sbgEComLogSatSignalWriteToStream(const SbgEComLogSatSignal *pSatSignal, SbgStreamBuffer *pStreamBuffer) { - assert(pSatSignal); - assert(pStreamBuffer); + assert(pSatSignal); + assert(pStreamBuffer); - sbgStreamBufferWriteUint8LE(pStreamBuffer, pSatSignal->id); - sbgStreamBufferWriteUint8LE(pStreamBuffer, pSatSignal->flags); - sbgStreamBufferWriteUint8LE(pStreamBuffer, pSatSignal->snr); + sbgStreamBufferWriteUint8LE(pStreamBuffer, pSatSignal->id); + sbgStreamBufferWriteUint8LE(pStreamBuffer, pSatSignal->flags); + sbgStreamBufferWriteUint8LE(pStreamBuffer, pSatSignal->snr); - return sbgStreamBufferGetLastError(pStreamBuffer); + return sbgStreamBufferGetLastError(pStreamBuffer); } //----------------------------------------------------------------------// @@ -256,31 +256,31 @@ static SbgErrorCode sbgEComLogSatSignalWriteToStream(const SbgEComLogSatSignal * /*! * Satellite entry constructor. * - * \param[in] pSatData Satellite entry. - * \param[in] id Satellite ID. - * \param[in] elevation Elevation, in degrees. - * \param[in] azimuth Azimuth, in degrees. - * \param[in] constellationId Constellation ID. - * \param[in] elevationStatus Elevation status. - * \param[in] healthStatus Health status. - * \param[in] trackingStatus Tracking status. + * \param[in] pSatData Satellite entry. + * \param[in] id Satellite ID. + * \param[in] elevation Elevation, in degrees. + * \param[in] azimuth Azimuth, in degrees. + * \param[in] constellationId Constellation ID. + * \param[in] elevationStatus Elevation status. + * \param[in] healthStatus Health status. + * \param[in] trackingStatus Tracking status. */ static void sbgEComLogSatEntryConstruct(SbgEComLogSatEntry *pSatData, uint8_t id, int8_t elevation, uint16_t azimuth, SbgEComConstellationId constellationId, SbgEComSatElevationStatus elevationStatus, SbgEComSatHealthStatus healthStatus, SbgEComSatTrackingStatus trackingStatus) { - uint16_t flags = 0; + uint16_t flags = 0; - assert(pSatData); + assert(pSatData); - sbgEComLogSatSetField(flags, constellationId, SBG_ECOM_LOG_SAT_CONSTELLATION_ID_OFFSET, SBG_ECOM_LOG_SAT_CONSTELLATION_ID_MASK); - sbgEComLogSatSetField(flags, elevationStatus, SBG_ECOM_LOG_SAT_ELEVATION_STATUS_OFFSET, SBG_ECOM_LOG_SAT_ELEVATION_STATUS_MASK); - sbgEComLogSatSetField(flags, healthStatus, SBG_ECOM_LOG_SAT_HEALTH_STATUS_OFFSET, SBG_ECOM_LOG_SAT_HEALTH_STATUS_MASK); - sbgEComLogSatSetField(flags, trackingStatus, SBG_ECOM_LOG_SAT_TRACKING_STATUS_OFFSET, SBG_ECOM_LOG_SAT_TRACKING_STATUS_MASK); + sbgEComLogSatSetField(flags, constellationId, SBG_ECOM_LOG_SAT_CONSTELLATION_ID_OFFSET, SBG_ECOM_LOG_SAT_CONSTELLATION_ID_MASK); + sbgEComLogSatSetField(flags, elevationStatus, SBG_ECOM_LOG_SAT_ELEVATION_STATUS_OFFSET, SBG_ECOM_LOG_SAT_ELEVATION_STATUS_MASK); + sbgEComLogSatSetField(flags, healthStatus, SBG_ECOM_LOG_SAT_HEALTH_STATUS_OFFSET, SBG_ECOM_LOG_SAT_HEALTH_STATUS_MASK); + sbgEComLogSatSetField(flags, trackingStatus, SBG_ECOM_LOG_SAT_TRACKING_STATUS_OFFSET, SBG_ECOM_LOG_SAT_TRACKING_STATUS_MASK); - pSatData->id = id; - pSatData->elevation = elevation; - pSatData->azimuth = azimuth; - pSatData->flags = flags; - pSatData->nrSignals = 0; + pSatData->id = id; + pSatData->elevation = elevation; + pSatData->azimuth = azimuth; + pSatData->flags = flags; + pSatData->nrSignals = 0; } /*! @@ -293,110 +293,110 @@ static void sbgEComLogSatEntryConstruct(SbgEComLogSatEntry *pSatData, uint8_t id * - if at least one signal reports an unhealthy signal, the whole SV is flagged unhealthy. * - if at least one signal is tracked and used in the solution, the whole SV is flagged as tracked+used. * - * \param[in] pSatData Satellite data. - * \param[in] healthStatus Health status. - * \param[in] trackingStatus Tracking status. + * \param[in] pSatData Satellite data. + * \param[in] healthStatus Health status. + * \param[in] trackingStatus Tracking status. */ static void sbgEComLogSatEntryUpdateStatus(SbgEComLogSatEntry *pSatData, SbgEComSatHealthStatus healthStatus, SbgEComSatTrackingStatus trackingStatus) { - uint16_t flags; + uint16_t flags; - assert(pSatData); + assert(pSatData); - flags = pSatData->flags; + flags = pSatData->flags; - healthStatus = sbgEComLogSatSelectHealthStatus(sbgEComLogSatEntryGetHealthStatus(pSatData), healthStatus); - trackingStatus = sbgEComLogSatSelectTrackingStatus(sbgEComLogSatEntryGetTrackingStatus(pSatData), trackingStatus); + healthStatus = sbgEComLogSatSelectHealthStatus(sbgEComLogSatEntryGetHealthStatus(pSatData), healthStatus); + trackingStatus = sbgEComLogSatSelectTrackingStatus(sbgEComLogSatEntryGetTrackingStatus(pSatData), trackingStatus); - sbgEComLogSatSetField(flags, healthStatus, SBG_ECOM_LOG_SAT_HEALTH_STATUS_OFFSET, SBG_ECOM_LOG_SAT_HEALTH_STATUS_MASK); - sbgEComLogSatSetField(flags, trackingStatus, SBG_ECOM_LOG_SAT_TRACKING_STATUS_OFFSET, SBG_ECOM_LOG_SAT_TRACKING_STATUS_MASK); + sbgEComLogSatSetField(flags, healthStatus, SBG_ECOM_LOG_SAT_HEALTH_STATUS_OFFSET, SBG_ECOM_LOG_SAT_HEALTH_STATUS_MASK); + sbgEComLogSatSetField(flags, trackingStatus, SBG_ECOM_LOG_SAT_TRACKING_STATUS_OFFSET, SBG_ECOM_LOG_SAT_TRACKING_STATUS_MASK); - pSatData->flags = flags; + pSatData->flags = flags; } /*! * Read a satellite entry instance from a stream buffer. * - * \param[out] pSatData Satellite entry instance. - * \param[in] pStreamBuffer Stream buffer. - * \return SBG_NO_ERROR if successful. + * \param[out] pSatData Satellite entry instance. + * \param[in] pStreamBuffer Stream buffer. + * \return SBG_NO_ERROR if successful. */ static SbgErrorCode sbgEComLogSatEntryReadFromStream(SbgEComLogSatEntry *pSatData, SbgStreamBuffer *pStreamBuffer) { - SbgErrorCode errorCode; - - assert(pSatData); - assert(pStreamBuffer); - - pSatData->id = sbgStreamBufferReadUint8LE(pStreamBuffer); - pSatData->elevation = sbgStreamBufferReadInt8LE(pStreamBuffer); - pSatData->azimuth = sbgStreamBufferReadUint16LE(pStreamBuffer); - pSatData->flags = sbgStreamBufferReadUint16LE(pStreamBuffer); - pSatData->nrSignals = sbgStreamBufferReadUint8LE(pStreamBuffer); - - errorCode = sbgStreamBufferGetLastError(pStreamBuffer); - - if (errorCode == SBG_NO_ERROR) - { - if (pSatData->nrSignals <= SBG_ARRAY_SIZE(pSatData->signalData)) - { - for (size_t i = 0; i < pSatData->nrSignals; i++) - { - errorCode = sbgEComLogSatSignalReadFromStream(&pSatData->signalData[i], pStreamBuffer); - - if (errorCode != SBG_NO_ERROR) - { - break; - } - } - } - else - { - errorCode = SBG_INVALID_FRAME; - SBG_LOG_ERROR(errorCode, "invalid number of signals: %zu", pSatData->nrSignals); - } - } - - return errorCode; + SbgErrorCode errorCode; + + assert(pSatData); + assert(pStreamBuffer); + + pSatData->id = sbgStreamBufferReadUint8LE(pStreamBuffer); + pSatData->elevation = sbgStreamBufferReadInt8LE(pStreamBuffer); + pSatData->azimuth = sbgStreamBufferReadUint16LE(pStreamBuffer); + pSatData->flags = sbgStreamBufferReadUint16LE(pStreamBuffer); + pSatData->nrSignals = sbgStreamBufferReadUint8LE(pStreamBuffer); + + errorCode = sbgStreamBufferGetLastError(pStreamBuffer); + + if (errorCode == SBG_NO_ERROR) + { + if (pSatData->nrSignals <= SBG_ARRAY_SIZE(pSatData->signalData)) + { + for (size_t i = 0; i < pSatData->nrSignals; i++) + { + errorCode = sbgEComLogSatSignalReadFromStream(&pSatData->signalData[i], pStreamBuffer); + + if (errorCode != SBG_NO_ERROR) + { + break; + } + } + } + else + { + errorCode = SBG_INVALID_FRAME; + SBG_LOG_ERROR(errorCode, "invalid number of signals: %zu", pSatData->nrSignals); + } + } + + return errorCode; } /*! * Write a satellite entry instance to a stream buffer. * - * \param[in] pSatData Satellite entry instance. - * \param[out] pStreamBuffer Stream buffer. - * \return SBG_NO_ERROR if successful. + * \param[in] pSatData Satellite entry instance. + * \param[out] pStreamBuffer Stream buffer. + * \return SBG_NO_ERROR if successful. */ static SbgErrorCode sbgEComLogSatEntryWriteToStream(const SbgEComLogSatEntry *pSatData, SbgStreamBuffer *pStreamBuffer) { - SbgErrorCode errorCode; + SbgErrorCode errorCode; - assert(pSatData); - assert(pStreamBuffer); - assert(pSatData->nrSignals <= UINT8_MAX); + assert(pSatData); + assert(pStreamBuffer); + assert(pSatData->nrSignals <= UINT8_MAX); - sbgStreamBufferWriteUint8LE(pStreamBuffer, pSatData->id); - sbgStreamBufferWriteInt8LE(pStreamBuffer, pSatData->elevation); - sbgStreamBufferWriteUint16LE(pStreamBuffer, pSatData->azimuth); - sbgStreamBufferWriteUint16LE(pStreamBuffer, pSatData->flags); - sbgStreamBufferWriteUint8LE(pStreamBuffer, (uint8_t)pSatData->nrSignals); + sbgStreamBufferWriteUint8LE(pStreamBuffer, pSatData->id); + sbgStreamBufferWriteInt8LE(pStreamBuffer, pSatData->elevation); + sbgStreamBufferWriteUint16LE(pStreamBuffer, pSatData->azimuth); + sbgStreamBufferWriteUint16LE(pStreamBuffer, pSatData->flags); + sbgStreamBufferWriteUint8LE(pStreamBuffer, (uint8_t)pSatData->nrSignals); - errorCode = sbgStreamBufferGetLastError(pStreamBuffer); + errorCode = sbgStreamBufferGetLastError(pStreamBuffer); - if (errorCode == SBG_NO_ERROR) - { - for (size_t i = 0; i < pSatData->nrSignals; i++) - { - errorCode = sbgEComLogSatSignalWriteToStream(&pSatData->signalData[i], pStreamBuffer); + if (errorCode == SBG_NO_ERROR) + { + for (size_t i = 0; i < pSatData->nrSignals; i++) + { + errorCode = sbgEComLogSatSignalWriteToStream(&pSatData->signalData[i], pStreamBuffer); - if (errorCode != SBG_NO_ERROR) - { - break; - } - } - } + if (errorCode != SBG_NO_ERROR) + { + break; + } + } + } - return errorCode; + return errorCode; } //----------------------------------------------------------------------// @@ -405,44 +405,44 @@ static SbgErrorCode sbgEComLogSatEntryWriteToStream(const SbgEComLogSatEntry *pS const char *sbgEComLogSatSignalGetSignalIdAsStr(const SbgEComLogSatSignal *pSignalData) { - assert(pSignalData); + assert(pSignalData); - return sbgEComSignalToStr(pSignalData->id); + return sbgEComSignalToStr(pSignalData->id); } bool sbgEComLogSatSignalSnrIsValid(const SbgEComLogSatSignal *pSignalData) { - assert(pSignalData); + assert(pSignalData); - return (pSignalData->flags&SBG_ECOM_LOG_SAT_SIGNAL_SNR_VALID?true:false); + return (pSignalData->flags&SBG_ECOM_LOG_SAT_SIGNAL_SNR_VALID?true:false); } SbgEComSatHealthStatus sbgEComLogSatSignalGetHealthStatus(const SbgEComLogSatSignal *pSignalData) { - assert(pSignalData); + assert(pSignalData); - return (SbgEComSatHealthStatus)sbgEComLogSatGetField(pSignalData->flags, SBG_ECOM_LOG_SAT_HEALTH_STATUS_OFFSET, SBG_ECOM_LOG_SAT_HEALTH_STATUS_MASK); + return (SbgEComSatHealthStatus)sbgEComLogSatGetField(pSignalData->flags, SBG_ECOM_LOG_SAT_HEALTH_STATUS_OFFSET, SBG_ECOM_LOG_SAT_HEALTH_STATUS_MASK); } const char *sbgEComLogSatSignalGetHealthStatusAsStr(const SbgEComLogSatSignal *pSignalData) { - assert(pSignalData); + assert(pSignalData); - return sbgEComLogSatHealthStatusToStr(sbgEComLogSatSignalGetHealthStatus(pSignalData)); + return sbgEComLogSatHealthStatusToStr(sbgEComLogSatSignalGetHealthStatus(pSignalData)); } SbgEComSatTrackingStatus sbgEComLogSatSignalGetTrackingStatus(const SbgEComLogSatSignal *pSignalData) { - assert(pSignalData); + assert(pSignalData); - return (SbgEComSatTrackingStatus)sbgEComLogSatGetField(pSignalData->flags, SBG_ECOM_LOG_SAT_TRACKING_STATUS_OFFSET, SBG_ECOM_LOG_SAT_TRACKING_STATUS_MASK); + return (SbgEComSatTrackingStatus)sbgEComLogSatGetField(pSignalData->flags, SBG_ECOM_LOG_SAT_TRACKING_STATUS_OFFSET, SBG_ECOM_LOG_SAT_TRACKING_STATUS_MASK); } const char *sbgEComLogSatSignalGetTrackingStatusAsStr(const SbgEComLogSatSignal *pSignalData) { - assert(pSignalData); + assert(pSignalData); - return sbgEComLogSatTrackingStatusToStr(sbgEComLogSatSignalGetTrackingStatus(pSignalData)); + return sbgEComLogSatTrackingStatusToStr(sbgEComLogSatSignalGetTrackingStatus(pSignalData)); } //----------------------------------------------------------------------// @@ -451,37 +451,37 @@ const char *sbgEComLogSatSignalGetTrackingStatusAsStr(const SbgEComLogSatSignal SbgEComLogSatSignal *sbgEComLogSatEntryAdd(SbgEComLogSatEntry *pSatData, SbgEComSignalId id, SbgEComSatHealthStatus healthStatus, SbgEComSatTrackingStatus trackingStatus, bool snrValid, uint8_t snr) { - SbgEComLogSatSignal *pSignalData = NULL; + SbgEComLogSatSignal *pSignalData = NULL; - assert(pSatData); + assert(pSatData); - if (pSatData->nrSignals < SBG_ARRAY_SIZE(pSatData->signalData)) - { - uint8_t flags = 0; + if (pSatData->nrSignals < SBG_ARRAY_SIZE(pSatData->signalData)) + { + uint8_t flags = 0; - sbgEComLogSatSetField(flags, healthStatus, SBG_ECOM_LOG_SAT_HEALTH_STATUS_OFFSET, SBG_ECOM_LOG_SAT_HEALTH_STATUS_MASK); - sbgEComLogSatSetField(flags, trackingStatus, SBG_ECOM_LOG_SAT_TRACKING_STATUS_OFFSET, SBG_ECOM_LOG_SAT_TRACKING_STATUS_MASK); + sbgEComLogSatSetField(flags, healthStatus, SBG_ECOM_LOG_SAT_HEALTH_STATUS_OFFSET, SBG_ECOM_LOG_SAT_HEALTH_STATUS_MASK); + sbgEComLogSatSetField(flags, trackingStatus, SBG_ECOM_LOG_SAT_TRACKING_STATUS_OFFSET, SBG_ECOM_LOG_SAT_TRACKING_STATUS_MASK); - if (snrValid) - { - flags |= SBG_ECOM_LOG_SAT_SIGNAL_SNR_VALID; - } + if (snrValid) + { + flags |= SBG_ECOM_LOG_SAT_SIGNAL_SNR_VALID; + } - pSignalData = &pSatData->signalData[pSatData->nrSignals]; - pSatData->nrSignals++; + pSignalData = &pSatData->signalData[pSatData->nrSignals]; + pSatData->nrSignals++; - pSignalData->id = id; - pSignalData->flags = flags; - pSignalData->snr = snr; + pSignalData->id = id; + pSignalData->flags = flags; + pSignalData->snr = snr; - sbgEComLogSatEntryUpdateStatus(pSatData, healthStatus, trackingStatus); - } - else - { - SBG_LOG_ERROR(SBG_BUFFER_OVERFLOW, "not free slot to add a new signal"); - } + sbgEComLogSatEntryUpdateStatus(pSatData, healthStatus, trackingStatus); + } + else + { + SBG_LOG_ERROR(SBG_BUFFER_OVERFLOW, "not free slot to add a new signal"); + } - return pSignalData; + return pSignalData; } //----------------------------------------------------------------------// @@ -490,74 +490,74 @@ SbgEComLogSatSignal *sbgEComLogSatEntryAdd(SbgEComLogSatEntry *pSatData, SbgECom SbgEComLogSatSignal *sbgEComLogSatEntryGet(SbgEComLogSatEntry *pSatData, SbgEComSignalId id) { - SbgEComLogSatSignal *pSignalData = NULL; + SbgEComLogSatSignal *pSignalData = NULL; - assert(pSatData); + assert(pSatData); - for (size_t i = 0; i < pSatData->nrSignals; i++) - { - if (pSatData->signalData[i].id == id) - { - pSignalData = &pSatData->signalData[i]; - break; - } - } + for (size_t i = 0; i < pSatData->nrSignals; i++) + { + if (pSatData->signalData[i].id == id) + { + pSignalData = &pSatData->signalData[i]; + break; + } + } - return pSignalData; + return pSignalData; } SbgEComConstellationId sbgEComLogSatEntryGetConstellationId(const SbgEComLogSatEntry *pSatData) { - assert(pSatData); + assert(pSatData); - return (SbgEComConstellationId)sbgEComLogSatGetField(pSatData->flags, SBG_ECOM_LOG_SAT_CONSTELLATION_ID_OFFSET, SBG_ECOM_LOG_SAT_CONSTELLATION_ID_MASK); + return (SbgEComConstellationId)sbgEComLogSatGetField(pSatData->flags, SBG_ECOM_LOG_SAT_CONSTELLATION_ID_OFFSET, SBG_ECOM_LOG_SAT_CONSTELLATION_ID_MASK); } const char *sbgEComLogSatEntryGetConstellationIdAsStr(const SbgEComLogSatEntry *pSatData) { - return sbgEComConstellationToStr(sbgEComLogSatEntryGetConstellationId(pSatData)); + return sbgEComConstellationToStr(sbgEComLogSatEntryGetConstellationId(pSatData)); } SbgEComSatElevationStatus sbgEComLogSatEntryGetElevationStatus(const SbgEComLogSatEntry *pSatData) { - assert(pSatData); + assert(pSatData); - return (SbgEComSatElevationStatus)sbgEComLogSatGetField(pSatData->flags, SBG_ECOM_LOG_SAT_ELEVATION_STATUS_OFFSET, SBG_ECOM_LOG_SAT_ELEVATION_STATUS_MASK); + return (SbgEComSatElevationStatus)sbgEComLogSatGetField(pSatData->flags, SBG_ECOM_LOG_SAT_ELEVATION_STATUS_OFFSET, SBG_ECOM_LOG_SAT_ELEVATION_STATUS_MASK); } const char *sbgEComLogSatEntryGetElevationStatusAsStr(const SbgEComLogSatEntry *pSatData) { - assert(pSatData); - - return sbgEComLogSatElevationStatusToStr(sbgEComLogSatEntryGetElevationStatus(pSatData)); + assert(pSatData); + + return sbgEComLogSatElevationStatusToStr(sbgEComLogSatEntryGetElevationStatus(pSatData)); } SbgEComSatHealthStatus sbgEComLogSatEntryGetHealthStatus(const SbgEComLogSatEntry *pSatData) { - assert(pSatData); + assert(pSatData); - return (SbgEComSatHealthStatus) sbgEComLogSatGetField(pSatData->flags, SBG_ECOM_LOG_SAT_HEALTH_STATUS_OFFSET, SBG_ECOM_LOG_SAT_HEALTH_STATUS_MASK); + return (SbgEComSatHealthStatus) sbgEComLogSatGetField(pSatData->flags, SBG_ECOM_LOG_SAT_HEALTH_STATUS_OFFSET, SBG_ECOM_LOG_SAT_HEALTH_STATUS_MASK); } const char *sbgEComLogSatEntryGetHealthStatusAsStr(const SbgEComLogSatEntry *pSatData) { - assert(pSatData); + assert(pSatData); - return sbgEComLogSatHealthStatusToStr(sbgEComLogSatEntryGetHealthStatus(pSatData)); + return sbgEComLogSatHealthStatusToStr(sbgEComLogSatEntryGetHealthStatus(pSatData)); } SbgEComSatTrackingStatus sbgEComLogSatEntryGetTrackingStatus(const SbgEComLogSatEntry *pSatData) { - assert(pSatData); + assert(pSatData); - return (SbgEComSatTrackingStatus)sbgEComLogSatGetField(pSatData->flags, SBG_ECOM_LOG_SAT_TRACKING_STATUS_OFFSET, SBG_ECOM_LOG_SAT_TRACKING_STATUS_MASK); + return (SbgEComSatTrackingStatus)sbgEComLogSatGetField(pSatData->flags, SBG_ECOM_LOG_SAT_TRACKING_STATUS_OFFSET, SBG_ECOM_LOG_SAT_TRACKING_STATUS_MASK); } const char *sbgEComLogSatEntryGetTrackingStatusAsStr(const SbgEComLogSatEntry *pSatData) { - assert(pSatData); + assert(pSatData); - return sbgEComLogSatTrackingStatusToStr(sbgEComLogSatEntryGetTrackingStatus(pSatData)); + return sbgEComLogSatTrackingStatusToStr(sbgEComLogSatEntryGetTrackingStatus(pSatData)); } //----------------------------------------------------------------------// @@ -566,99 +566,99 @@ const char *sbgEComLogSatEntryGetTrackingStatusAsStr(const SbgEComLogSatEntry *p void sbgEComLogSatListConstruct(SbgEComLogSatList *pSatList, uint32_t timeStamp) { - assert(pSatList); - - pSatList->timeStamp = timeStamp; - pSatList->reserved = 0; - pSatList->nrSatellites = 0; + assert(pSatList); + + pSatList->timeStamp = timeStamp; + pSatList->reserved = 0; + pSatList->nrSatellites = 0; } SbgEComLogSatEntry *sbgEComLogSatListAdd(SbgEComLogSatList *pSatList, uint8_t id, int8_t elevation, uint16_t azimuth, SbgEComConstellationId constellationId, SbgEComSatElevationStatus elevationStatus, SbgEComSatHealthStatus healthStatus, SbgEComSatTrackingStatus trackingStatus) { - SbgEComLogSatEntry *pSatData = NULL; + SbgEComLogSatEntry *pSatData = NULL; - assert(pSatList); + assert(pSatList); - if (pSatList->nrSatellites < SBG_ARRAY_SIZE(pSatList->satData)) - { - pSatData = &pSatList->satData[pSatList->nrSatellites]; - pSatList->nrSatellites++; + if (pSatList->nrSatellites < SBG_ARRAY_SIZE(pSatList->satData)) + { + pSatData = &pSatList->satData[pSatList->nrSatellites]; + pSatList->nrSatellites++; - sbgEComLogSatEntryConstruct(pSatData, id, elevation, azimuth, constellationId, elevationStatus, healthStatus, trackingStatus); - } - else - { - SBG_LOG_ERROR(SBG_BUFFER_OVERFLOW, "no free slot to add a new satellite."); - } + sbgEComLogSatEntryConstruct(pSatData, id, elevation, azimuth, constellationId, elevationStatus, healthStatus, trackingStatus); + } + else + { + SBG_LOG_ERROR(SBG_BUFFER_OVERFLOW, "no free slot to add a new satellite."); + } - return pSatData; + return pSatData; } SbgErrorCode sbgEComLogSatListReadFromStream(SbgEComLogSatList *pSatList, SbgStreamBuffer *pStreamBuffer) { - SbgErrorCode errorCode; - - assert(pSatList); - assert(pStreamBuffer); - - pSatList->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); - pSatList->reserved = sbgStreamBufferReadUint32LE(pStreamBuffer); - pSatList->nrSatellites = sbgStreamBufferReadUint8LE(pStreamBuffer); - - errorCode = sbgStreamBufferGetLastError(pStreamBuffer); - - if (errorCode == SBG_NO_ERROR) - { - if (pSatList->nrSatellites <= SBG_ARRAY_SIZE(pSatList->satData)) - { - for (size_t i = 0; i < pSatList->nrSatellites; i++) - { - errorCode = sbgEComLogSatEntryReadFromStream(&pSatList->satData[i], pStreamBuffer); - - if (errorCode != SBG_NO_ERROR) - { - break; - } - } - } - else - { - errorCode = SBG_INVALID_FRAME; - SBG_LOG_ERROR(errorCode, "invalid number of satellites: %zu", pSatList->nrSatellites); - } - } - - return errorCode; + SbgErrorCode errorCode; + + assert(pSatList); + assert(pStreamBuffer); + + pSatList->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); + pSatList->reserved = sbgStreamBufferReadUint32LE(pStreamBuffer); + pSatList->nrSatellites = sbgStreamBufferReadUint8LE(pStreamBuffer); + + errorCode = sbgStreamBufferGetLastError(pStreamBuffer); + + if (errorCode == SBG_NO_ERROR) + { + if (pSatList->nrSatellites <= SBG_ARRAY_SIZE(pSatList->satData)) + { + for (size_t i = 0; i < pSatList->nrSatellites; i++) + { + errorCode = sbgEComLogSatEntryReadFromStream(&pSatList->satData[i], pStreamBuffer); + + if (errorCode != SBG_NO_ERROR) + { + break; + } + } + } + else + { + errorCode = SBG_INVALID_FRAME; + SBG_LOG_ERROR(errorCode, "invalid number of satellites: %zu", pSatList->nrSatellites); + } + } + + return errorCode; } SbgErrorCode sbgEComLogSatListWriteToStream(const SbgEComLogSatList *pSatList, SbgStreamBuffer *pStreamBuffer) { - SbgErrorCode errorCode; + SbgErrorCode errorCode; - assert(pSatList); - assert(pSatList->nrSatellites <= UINT8_MAX); - assert(pStreamBuffer); + assert(pSatList); + assert(pSatList->nrSatellites <= UINT8_MAX); + assert(pStreamBuffer); - sbgStreamBufferWriteUint32LE(pStreamBuffer, pSatList->timeStamp); - sbgStreamBufferWriteUint32LE(pStreamBuffer, pSatList->reserved); - sbgStreamBufferWriteUint8LE(pStreamBuffer, (uint8_t)pSatList->nrSatellites); + sbgStreamBufferWriteUint32LE(pStreamBuffer, pSatList->timeStamp); + sbgStreamBufferWriteUint32LE(pStreamBuffer, pSatList->reserved); + sbgStreamBufferWriteUint8LE(pStreamBuffer, (uint8_t)pSatList->nrSatellites); - errorCode = sbgStreamBufferGetLastError(pStreamBuffer); + errorCode = sbgStreamBufferGetLastError(pStreamBuffer); - if (errorCode == SBG_NO_ERROR) - { - for (size_t i = 0; i < pSatList->nrSatellites; i++) - { - errorCode = sbgEComLogSatEntryWriteToStream(&pSatList->satData[i], pStreamBuffer); + if (errorCode == SBG_NO_ERROR) + { + for (size_t i = 0; i < pSatList->nrSatellites; i++) + { + errorCode = sbgEComLogSatEntryWriteToStream(&pSatList->satData[i], pStreamBuffer); - if (errorCode != SBG_NO_ERROR) - { - break; - } - } - } + if (errorCode != SBG_NO_ERROR) + { + break; + } + } + } - return errorCode; + return errorCode; } //----------------------------------------------------------------------// @@ -667,20 +667,20 @@ SbgErrorCode sbgEComLogSatListWriteToStream(const SbgEComLogSatList *pSatList, S SbgEComLogSatEntry *sbgEComLogSatListGet(SbgEComLogSatList *pSatList, uint8_t id) { - SbgEComLogSatEntry *pSatData = NULL; + SbgEComLogSatEntry *pSatData = NULL; - assert(pSatList); + assert(pSatList); - for (size_t i = 0; i < pSatList->nrSatellites; i++) - { - if (pSatList->satData[i].id == id) - { - pSatData = &pSatList->satData[i]; - break; - } - } + for (size_t i = 0; i < pSatList->nrSatellites; i++) + { + if (pSatList->satData[i].id == id) + { + pSatData = &pSatList->satData[i]; + break; + } + } - return pSatData; + return pSatData; } //----------------------------------------------------------------------// @@ -689,10 +689,10 @@ SbgEComLogSatEntry *sbgEComLogSatListGet(SbgEComLogSatList *pSatList, uint8_t id SbgErrorCode sbgEComBinaryLogParseSatGroupData(SbgStreamBuffer *pStreamBuffer, SbgEComLogSatList *pSatList) { - return sbgEComLogSatListReadFromStream(pSatList, pStreamBuffer); + return sbgEComLogSatListReadFromStream(pSatList, pStreamBuffer); } SbgErrorCode sbgEComBinaryLogWriteSatGroupData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogSatList *pSatList) { - return sbgEComLogSatListWriteToStream(pSatList, pStreamBuffer); + return sbgEComLogSatListWriteToStream(pSatList, pStreamBuffer); } diff --git a/src/logs/sbgEComLogSat.h b/src/logs/sbgEComLogSat.h index c35c38c..df0368c 100644 --- a/src/logs/sbgEComLogSat.h +++ b/src/logs/sbgEComLogSat.h @@ -1,13 +1,13 @@ /*! - * \file sbgEComLogSat.h - * \ingroup binaryLogs - * \author SBG Systems - * \date 1 March 2022 + * \file sbgEComLogSat.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 1 March 2022 * - * \brief Parse space vehicles in view information log. + * \brief Parse space vehicles in view information log. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -51,12 +51,12 @@ extern "C" { /*! * Maximum number of satellites in a satellite group. */ -#define SBG_ECOM_SAT_MAX_NR_SATELLITES (64) +#define SBG_ECOM_SAT_MAX_NR_SATELLITES (64) /*! * Maximum number of signals per satellite. */ -#define SBG_ECOM_SAT_MAX_NR_SIGNALS (8) +#define SBG_ECOM_SAT_MAX_NR_SIGNALS (8) //----------------------------------------------------------------------// //- Enumeration definitions -// @@ -73,12 +73,12 @@ extern "C" { */ typedef enum _SbgEComSatTrackingStatus { - SBG_ECOM_SAT_TRACKING_STATUS_UNKNOWN = 0, /*!< Unknown tracking status such as no signal / idle. */ - SBG_ECOM_SAT_TRACKING_STATUS_SEARCHING = 1, /*!< Signal is being searched and can't be used yet. */ - SBG_ECOM_SAT_TRACKING_STATUS_TRACKING_UNKNOWN = 2, /*!< Signal is tracked but don't know if used or not in the solution. */ - SBG_ECOM_SAT_TRACKING_STATUS_TRACKING_NOT_USED = 3, /*!< Signal is tracked and is not used in the solution. */ - SBG_ECOM_SAT_TRACKING_STATUS_TRACKING_REJECTED = 4, /*!< Signal is tracked and is rejected from the solution. */ - SBG_ECOM_SAT_TRACKING_STATUS_TRACKING_USED = 5, /*!< Signal is tracked and used in the solution. */ + SBG_ECOM_SAT_TRACKING_STATUS_UNKNOWN = 0, /*!< Unknown tracking status such as no signal / idle. */ + SBG_ECOM_SAT_TRACKING_STATUS_SEARCHING = 1, /*!< Signal is being searched and can't be used yet. */ + SBG_ECOM_SAT_TRACKING_STATUS_TRACKING_UNKNOWN = 2, /*!< Signal is tracked but don't know if used or not in the solution. */ + SBG_ECOM_SAT_TRACKING_STATUS_TRACKING_NOT_USED = 3, /*!< Signal is tracked and is not used in the solution. */ + SBG_ECOM_SAT_TRACKING_STATUS_TRACKING_REJECTED = 4, /*!< Signal is tracked and is rejected from the solution. */ + SBG_ECOM_SAT_TRACKING_STATUS_TRACKING_USED = 5, /*!< Signal is tracked and used in the solution. */ } SbgEComSatTrackingStatus; /*! @@ -90,9 +90,9 @@ typedef enum _SbgEComSatTrackingStatus */ typedef enum _SbgEComSatHealthStatus { - SBG_ECOM_SAT_HEALTH_STATUS_UNKNOWN = 0, /*!< Don't know the satellite or the signal health status. */ - SBG_ECOM_SAT_HEALTH_STATUS_HEALTHY = 1, /*!< The satellite or the signal is healthy and can be used. */ - SBG_ECOM_SAT_HEALTH_STATUS_UNHEALTHY = 2, /*!< The satellite or the signal is not healthy and can't be used. */ + SBG_ECOM_SAT_HEALTH_STATUS_UNKNOWN = 0, /*!< Don't know the satellite or the signal health status. */ + SBG_ECOM_SAT_HEALTH_STATUS_HEALTHY = 1, /*!< The satellite or the signal is healthy and can be used. */ + SBG_ECOM_SAT_HEALTH_STATUS_UNHEALTHY = 2, /*!< The satellite or the signal is not healthy and can't be used. */ } SbgEComSatHealthStatus; /*! @@ -104,9 +104,9 @@ typedef enum _SbgEComSatHealthStatus */ typedef enum _SbgEComSatElevationStatus { - SBG_ECOM_SAT_ELEVATION_STATUS_UNKNOWN = 0, /*!< Don't know if the satellite elevation is setting or rising. */ - SBG_ECOM_SAT_ELEVATION_STATUS_SETTING = 1, /*!< The satellite elevation is setting. */ - SBG_ECOM_SAT_ELEVATION_STATUS_RISING = 2, /*!< The satellite elevation is rising */ + SBG_ECOM_SAT_ELEVATION_STATUS_UNKNOWN = 0, /*!< Don't know if the satellite elevation is setting or rising. */ + SBG_ECOM_SAT_ELEVATION_STATUS_SETTING = 1, /*!< The satellite elevation is setting. */ + SBG_ECOM_SAT_ELEVATION_STATUS_RISING = 2, /*!< The satellite elevation is rising */ } SbgEComSatElevationStatus; //----------------------------------------------------------------------// @@ -120,9 +120,9 @@ typedef enum _SbgEComSatElevationStatus */ typedef struct _SbgEComLogSatSignal { - SbgEComSignalId id; /*!< Signal ID. */ - uint8_t flags; /*!< Flags. */ - uint8_t snr; /*!< Signal-to-noise ratio, in dB. */ + SbgEComSignalId id; /*!< Signal ID. */ + uint8_t flags; /*!< Flags. */ + uint8_t snr; /*!< Signal-to-noise ratio, in dB. */ } SbgEComLogSatSignal; /*! @@ -146,12 +146,12 @@ typedef struct _SbgEComLogSatSignal */ typedef struct _SbgEComLogSatEntry { - uint8_t id; /*!< Satellite ID. */ - int8_t elevation; /*!< Elevation, in degrees [-90; +90], valid if and only if the elevation is known. */ - uint16_t azimuth; /*!< Azimuth, in degrees [0; 359], valid if and only if the elevation is known. */ - uint16_t flags; /*!< Flags. */ - size_t nrSignals; /*!< Number of signals. */ - SbgEComLogSatSignal signalData[SBG_ECOM_SAT_MAX_NR_SIGNALS]; /*!< Signal data array. */ + uint8_t id; /*!< Satellite ID. */ + int8_t elevation; /*!< Elevation, in degrees [-90; +90], valid if and only if the elevation is known. */ + uint16_t azimuth; /*!< Azimuth, in degrees [0; 359], valid if and only if the elevation is known. */ + uint16_t flags; /*!< Flags. */ + size_t nrSignals; /*!< Number of signals. */ + SbgEComLogSatSignal signalData[SBG_ECOM_SAT_MAX_NR_SIGNALS]; /*!< Signal data array. */ } SbgEComLogSatEntry; /*! @@ -159,10 +159,10 @@ typedef struct _SbgEComLogSatEntry */ typedef struct _SbgEComLogSatList { - uint32_t timeStamp; /*!< Time since the sensor power up, in us. */ - uint32_t reserved; /*!< Reserved for future use. */ - size_t nrSatellites; /*!< Number of satellites. */ - SbgEComLogSatEntry satData[SBG_ECOM_SAT_MAX_NR_SATELLITES]; /*!< Satellite data array. */ + uint32_t timeStamp; /*!< Time since the sensor power up, in us. */ + uint32_t reserved; /*!< Reserved for future use. */ + size_t nrSatellites; /*!< Number of satellites. */ + SbgEComLogSatEntry satData[SBG_ECOM_SAT_MAX_NR_SATELLITES]; /*!< Satellite data array. */ } SbgEComLogSatList; //----------------------------------------------------------------------// @@ -172,48 +172,48 @@ typedef struct _SbgEComLogSatList /*! * Get a signal id as a read only C string. * - * \param[in] pSignalData Signal data. - * \return Signal id as a read only C string. + * \param[in] pSignalData Signal data. + * \return Signal id as a read only C string. */ const char *sbgEComLogSatSignalGetSignalIdAsStr(const SbgEComLogSatSignal *pSignalData); /*! * Returns true if the SNR value is valid. * - * \param[in] pSignalData Signal data. - * \return true if the SNR value is valid. + * \param[in] pSignalData Signal data. + * \return true if the SNR value is valid. */ bool sbgEComLogSatSignalSnrIsValid(const SbgEComLogSatSignal *pSignalData); /*! * Get the health status of signal data. * - * \param[in] pSignalData Signal data. - * \return Health status. + * \param[in] pSignalData Signal data. + * \return Health status. */ SbgEComSatHealthStatus sbgEComLogSatSignalGetHealthStatus(const SbgEComLogSatSignal *pSignalData); /*! * Get the health status of signal data as a read only C string. * - * \param[in] pSignalData Signal data. - * \return Health status as a read only C string. + * \param[in] pSignalData Signal data. + * \return Health status as a read only C string. */ const char *sbgEComLogSatSignalGetHealthStatusAsStr(const SbgEComLogSatSignal *pSignalData); /*! * Get the tracking status of signal data. * - * \param[in] pSignalData Signal data. - * \return Tracking status. + * \param[in] pSignalData Signal data. + * \return Tracking status. */ SbgEComSatTrackingStatus sbgEComLogSatSignalGetTrackingStatus(const SbgEComLogSatSignal *pSignalData); /*! * Get the tracking status of signal data as a read only C string. * - * \param[in] pSignalData Signal data. - * \return Tracking status as a read only C string. + * \param[in] pSignalData Signal data. + * \return Tracking status as a read only C string. */ const char *sbgEComLogSatSignalGetTrackingStatusAsStr(const SbgEComLogSatSignal *pSignalData); @@ -227,13 +227,13 @@ const char *sbgEComLogSatSignalGetTrackingStatusAsStr(const SbgEComLogSatSignal * The health and tracking statuses of the satellite data are updated according to their respective * priority rules. * - * \param[in] pSatData Satellite data. - * \param[in] id Signal ID. - * \param[in] healthStatus Health status. - * \param[in] trackingStatus Tracking status. - * \param[in] snrValid Set to true if the SNR value is valid. - * \param[in] snr Signal-to-noise ratio, in dB. - * \return Signal data, NULL if an error occurs. + * \param[in] pSatData Satellite data. + * \param[in] id Signal ID. + * \param[in] healthStatus Health status. + * \param[in] trackingStatus Tracking status. + * \param[in] snrValid Set to true if the SNR value is valid. + * \param[in] snr Signal-to-noise ratio, in dB. + * \return Signal data, NULL if an error occurs. */ SbgEComLogSatSignal *sbgEComLogSatEntryAdd(SbgEComLogSatEntry *pSatData, SbgEComSignalId id, SbgEComSatHealthStatus healthStatus, SbgEComSatTrackingStatus trackingStatus, bool snrValid, uint8_t snr); @@ -244,73 +244,73 @@ SbgEComLogSatSignal *sbgEComLogSatEntryAdd(SbgEComLogSatEntry *pSatData, SbgECom /*! * Get signal data from satellite data. * - * \param[in] pSatData Satellite data. - * \param[in] id Signal ID. - * \return Signal data, NULL if not found. + * \param[in] pSatData Satellite data. + * \param[in] id Signal ID. + * \return Signal data, NULL if not found. */ SbgEComLogSatSignal *sbgEComLogSatEntryGet(SbgEComLogSatEntry *pSatData, SbgEComSignalId id); /*! * Get the constellation ID of satellite data. * - * \param[in] pSatData Satellite data. - * \return Constellation ID. + * \param[in] pSatData Satellite data. + * \return Constellation ID. */ SbgEComConstellationId sbgEComLogSatEntryGetConstellationId(const SbgEComLogSatEntry *pSatData); /*! * Get the constellation ID of satellite data as a read only C string. * - * \param[in] pSatData Satellite data. - * \return Constellation ID as a read only C string. + * \param[in] pSatData Satellite data. + * \return Constellation ID as a read only C string. */ const char *sbgEComLogSatEntryGetConstellationIdAsStr(const SbgEComLogSatEntry *pSatData); /*! * Get the elevation status of satellite data. * - * \param[in] pSatData Satellite data. - * \return Elevation status. + * \param[in] pSatData Satellite data. + * \return Elevation status. */ SbgEComSatElevationStatus sbgEComLogSatEntryGetElevationStatus(const SbgEComLogSatEntry *pSatData); /*! * Get the elevation status of satellite data as a read only C string. * - * \param[in] pSatData Satellite data. - * \return Elevation status as a read only C string. + * \param[in] pSatData Satellite data. + * \return Elevation status as a read only C string. */ const char *sbgEComLogSatEntryGetElevationStatusAsStr(const SbgEComLogSatEntry *pSatData); /*! * Get the health status of satellite data. * - * \param[in] pSatData Satellite data. - * \return Health status. + * \param[in] pSatData Satellite data. + * \return Health status. */ SbgEComSatHealthStatus sbgEComLogSatEntryGetHealthStatus(const SbgEComLogSatEntry *pSatData); /*! * Get the health status of satellite data as a read only C string. * - * \param[in] pSatData Satellite data. - * \return Health status as a read only C string. + * \param[in] pSatData Satellite data. + * \return Health status as a read only C string. */ const char *sbgEComLogSatEntryGetHealthStatusAsStr(const SbgEComLogSatEntry *pSatData); /*! * Get the tracking status of satellite data. * - * \param[in] pSatData Satellite data. - * \return Tracking status. + * \param[in] pSatData Satellite data. + * \return Tracking status. */ SbgEComSatTrackingStatus sbgEComLogSatEntryGetTrackingStatus(const SbgEComLogSatEntry *pSatData); /*! * Get the tracking status of satellite data as a read only C string. * - * \param[in] pSatData Satellite data. - * \return Tracking status as a read only C string. + * \param[in] pSatData Satellite data. + * \return Tracking status as a read only C string. */ const char *sbgEComLogSatEntryGetTrackingStatusAsStr(const SbgEComLogSatEntry *pSatData); @@ -321,41 +321,41 @@ const char *sbgEComLogSatEntryGetTrackingStatusAsStr(const SbgEComLogSatEntry *p /*! * Satellite list constructor. * - * \param[in] pSatList Satellite list instance. - * \param[in] timeStamp Time stamp, in us. + * \param[in] pSatList Satellite list instance. + * \param[in] timeStamp Timestamp, in us. */ void sbgEComLogSatListConstruct(SbgEComLogSatList *pSatList, uint32_t timeStamp); /*! * Add a satellite entry to the list of satellites. * - * \param[in] pSatList Satellite list instance. - * \param[in] id Satellite ID. - * \param[in] elevation Elevation, in degrees. - * \param[in] azimuth Azimuth, in degrees. - * \param[in] constellationId Constellation ID. - * \param[in] elevationStatus Elevation status. - * \param[in] healthStatus Health status. - * \param[in] trackingStatus Tracking status. - * \return Satellite data, NULL if an error occurs. + * \param[in] pSatList Satellite list instance. + * \param[in] id Satellite ID. + * \param[in] elevation Elevation, in degrees. + * \param[in] azimuth Azimuth, in degrees. + * \param[in] constellationId Constellation ID. + * \param[in] elevationStatus Elevation status. + * \param[in] healthStatus Health status. + * \param[in] trackingStatus Tracking status. + * \return Satellite data, NULL if an error occurs. */ SbgEComLogSatEntry *sbgEComLogSatListAdd(SbgEComLogSatList *pSatList, uint8_t id, int8_t elevation, uint16_t azimuth, SbgEComConstellationId constellationId, SbgEComSatElevationStatus elevationStatus, SbgEComSatHealthStatus healthStatus, SbgEComSatTrackingStatus trackingStatus); /*! * Parse data for the SBG_ECOM_LOG_GPS#_SAT message and fill the corresponding structure. * - * \param[out] pSatList Satellite list instance. - * \param[in] pStreamBuffer Input stream buffer to read the log from. - * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. + * \param[out] pSatList Satellite list instance. + * \param[in] pStreamBuffer Input stream buffer to read the log from. + * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. */ SbgErrorCode sbgEComLogSatListReadFromStream(SbgEComLogSatList *pSatList, SbgStreamBuffer *pStreamBuffer); /*! * Write data for the SBG_ECOM_LOG_GPS#_SAT message to the output stream buffer from the provided structure. * - * \param[in] pSatList Satellite list instance. - * \param[out] pStreamBuffer Output stream buffer to write the log to. - * \return SBG_NO_ERROR if the log has been written to the stream buffer. + * \param[in] pSatList Satellite list instance. + * \param[out] pStreamBuffer Output stream buffer to write the log to. + * \return SBG_NO_ERROR if the log has been written to the stream buffer. */ SbgErrorCode sbgEComLogSatListWriteToStream(const SbgEComLogSatList *pSatList, SbgStreamBuffer *pStreamBuffer); @@ -366,9 +366,9 @@ SbgErrorCode sbgEComLogSatListWriteToStream(const SbgEComLogSatList *pSatList, S /*! * Get a satellite entry from its ID. * - * \param[in] pSatList Satellite list instance. - * \param[in] id Satellite ID. - * \return Satellite data, NULL if not found. + * \param[in] pSatList Satellite list instance. + * \param[in] id Satellite ID. + * \return Satellite data, NULL if not found. */ SbgEComLogSatEntry *sbgEComLogSatListGet(SbgEComLogSatList *pSatList, uint8_t id); @@ -376,9 +376,9 @@ SbgEComLogSatEntry *sbgEComLogSatListGet(SbgEComLogSatList *pSatList, uint8_t id //- DEPRECATED - Used for backward compatibility -// //----------------------------------------------------------------------// -SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogSatSignal SbgLogSatSignalData); -SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogSatEntry SbgLogSatData); -SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogSatList SbgLogSatGroupData); +SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogSatSignal SbgLogSatSignalData); +SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogSatEntry SbgLogSatData); +SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogSatList SbgLogSatGroupData); SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogParseSatGroupData(SbgStreamBuffer *pStreamBuffer, SbgEComLogSatList *pSatList)); SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogWriteSatGroupData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogSatList *pSatList)); diff --git a/src/logs/sbgEComLogSessionInfo.c b/src/logs/sbgEComLogSessionInfo.c new file mode 100644 index 0000000..83aaa1d --- /dev/null +++ b/src/logs/sbgEComLogSessionInfo.c @@ -0,0 +1,68 @@ +// sbgCommonLib headers +#include +#include + +// Local headers +#include "sbgEComLogSessionInfo.h" + +//----------------------------------------------------------------------// +//- Public functions -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComLogSessionInfoReadFromStream(SbgEComLogSessionInfo *pLogData, SbgStreamBuffer *pStreamBuffer) +{ + SbgErrorCode errorCode; + + assert(pStreamBuffer); + assert(pLogData); + + pLogData->pageIndex = sbgStreamBufferReadUint16LE(pStreamBuffer); + pLogData->nrPages = sbgStreamBufferReadUint16LE(pStreamBuffer); + pLogData->size = sbgStreamBufferReadUint16LE(pStreamBuffer); + + errorCode = sbgStreamBufferGetLastError(pStreamBuffer); + + if (errorCode == SBG_NO_ERROR) + { + if (pLogData->pageIndex >= pLogData->nrPages) + { + errorCode = SBG_INVALID_FRAME; + SBG_LOG_ERROR(errorCode, "invalid session information page index %" PRIu16 "/%" PRIu16, pLogData->pageIndex, pLogData->nrPages); + } + } + + if (errorCode == SBG_NO_ERROR) + { + if (pLogData->size <= sizeof(pLogData->buffer)) + { + errorCode = sbgStreamBufferReadBuffer(pStreamBuffer, pLogData->buffer, pLogData->size); + + if (errorCode != SBG_NO_ERROR) + { + SBG_LOG_ERROR(errorCode, "invalid session information size %zu, actual:%zu", pLogData->size, sbgStreamBufferGetSpace(pStreamBuffer)); + } + } + else + { + errorCode = SBG_INVALID_FRAME; + SBG_LOG_ERROR(errorCode, "invalid session information size %zu", pLogData->size); + } + } + + return errorCode; +} + +SbgErrorCode sbgEComLogSessionInfoWriteToStream(const SbgEComLogSessionInfo *pLogData, SbgStreamBuffer *pStreamBuffer) +{ + assert(pLogData); + assert(pLogData->size <= UINT16_MAX); + assert(pStreamBuffer); + + sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->pageIndex); + sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->nrPages); + sbgStreamBufferWriteUint16LE(pStreamBuffer, (uint16_t)pLogData->size); + + sbgStreamBufferWriteBuffer(pStreamBuffer, pLogData->buffer, pLogData->size); + + return sbgStreamBufferGetLastError(pStreamBuffer); +} diff --git a/src/logs/sbgEComLogSessionInfo.h b/src/logs/sbgEComLogSessionInfo.h new file mode 100644 index 0000000..169334c --- /dev/null +++ b/src/logs/sbgEComLogSessionInfo.h @@ -0,0 +1,87 @@ +/*! + * \file sbgEComLogSessionInfo.h + * \ingroup binaryLogs + * \author SBG Systems + * \date December 19, 2023 + * + * \brief Parse logs used to report session information. + * + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * 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. + * + * \endlicense + */ + +#ifndef SBG_ECOM_LOG_SESSION_INFO_H +#define SBG_ECOM_LOG_SESSION_INFO_H + +// sbgCommonLib headers +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Structure definitions -// +//----------------------------------------------------------------------// + +/*! + * Session information log. + * + * This is a public structure. + */ +typedef struct _SbgEComLogSessionInfo +{ + uint16_t pageIndex; /*!< Page index. */ + uint16_t nrPages; /*!< Total number of pages. */ + size_t size; /*!< Buffer size, in bytes. */ + char buffer[256]; /*!< Buffer. */ +} SbgEComLogSessionInfo; + +//----------------------------------------------------------------------// +//- Public functions -// +//----------------------------------------------------------------------// + +/*! + * Parse data for the SBG_ECOM_LOG_SESSION_INFO message and fill the corresponding structure. + * + * \param[out] pLogData Log structure instance to fill. + * \param[in] pStreamBuffer Input stream buffer to read the log from. + * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. + */ +SbgErrorCode sbgEComLogSessionInfoReadFromStream(SbgEComLogSessionInfo *pLogData, SbgStreamBuffer *pStreamBuffer); + +/*! + * Write data for the SBG_ECOM_LOG_SESSION_INFO message to the output stream buffer from the provided structure. + * + * \param[in] pLogData Log structure instance to write. + * \param[out] pStreamBuffer Output stream buffer to write the log to. + * \return SBG_NO_ERROR if the log has been written to the stream buffer. + */ +SbgErrorCode sbgEComLogSessionInfoWriteToStream(const SbgEComLogSessionInfo *pLogData, SbgStreamBuffer *pStreamBuffer); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_LOG_SESSION_INFO_H diff --git a/src/logs/sbgEComLogShipMotion.c b/src/logs/sbgEComLogShipMotion.c index 2db2d1b..2891801 100644 --- a/src/logs/sbgEComLogShipMotion.c +++ b/src/logs/sbgEComLogShipMotion.c @@ -6,75 +6,75 @@ SbgErrorCode sbgEComLogShipMotionReadFromStream(SbgEComLogShipMotion *pLogData, SbgStreamBuffer *pStreamBuffer) { - assert(pStreamBuffer); - assert(pLogData); - - pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); - pLogData->mainHeavePeriod = sbgStreamBufferReadFloatLE(pStreamBuffer); - - pLogData->shipMotion[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->shipMotion[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->shipMotion[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); - - pLogData->shipAccel[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->shipAccel[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->shipAccel[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); - - // - // Test if we have a additional information such as ship velocity and status (since version 1.4) - // - if (sbgStreamBufferGetSpace(pStreamBuffer) >= 14) - { - // - // Read new outputs - // - pLogData->shipVel[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->shipVel[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->shipVel[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); - - pLogData->status = sbgStreamBufferReadUint16LE(pStreamBuffer); - } - else - { - // - // Those outputs are not available in previous versions - // - pLogData->shipVel[0] = 0.0f; - pLogData->shipVel[1] = 0.0f; - pLogData->shipVel[2] = 0.0f; - - pLogData->status = 0; - } - - return sbgStreamBufferGetLastError(pStreamBuffer); + assert(pStreamBuffer); + assert(pLogData); + + pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); + pLogData->mainHeavePeriod = sbgStreamBufferReadFloatLE(pStreamBuffer); + + pLogData->shipMotion[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->shipMotion[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->shipMotion[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); + + pLogData->shipAccel[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->shipAccel[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->shipAccel[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); + + // + // Test if we have a additional information such as ship velocity and status (since version 1.4) + // + if (sbgStreamBufferGetSpace(pStreamBuffer) >= 14) + { + // + // Read new outputs + // + pLogData->shipVel[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->shipVel[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->shipVel[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); + + pLogData->status = sbgStreamBufferReadUint16LE(pStreamBuffer); + } + else + { + // + // Those outputs are not available in previous versions + // + pLogData->shipVel[0] = 0.0f; + pLogData->shipVel[1] = 0.0f; + pLogData->shipVel[2] = 0.0f; + + pLogData->status = 0; + } + + return sbgStreamBufferGetLastError(pStreamBuffer); } SbgErrorCode sbgEComLogShipMotionWriteToStream(const SbgEComLogShipMotion *pLogData, SbgStreamBuffer *pStreamBuffer) { - assert(pStreamBuffer); - assert(pLogData); + assert(pStreamBuffer); + assert(pLogData); - sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->mainHeavePeriod); + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->mainHeavePeriod); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->shipMotion[0]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->shipMotion[1]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->shipMotion[2]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->shipMotion[0]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->shipMotion[1]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->shipMotion[2]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->shipAccel[0]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->shipAccel[1]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->shipAccel[2]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->shipAccel[0]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->shipAccel[1]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->shipAccel[2]); - // - // Write additional fields added in version 1.4 - // - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->shipVel[0]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->shipVel[1]); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->shipVel[2]); + // + // Write additional fields added in version 1.4 + // + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->shipVel[0]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->shipVel[1]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->shipVel[2]); - sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->status); + sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->status); - return sbgStreamBufferGetLastError(pStreamBuffer); + return sbgStreamBufferGetLastError(pStreamBuffer); } //----------------------------------------------------------------------// @@ -83,10 +83,10 @@ SbgErrorCode sbgEComLogShipMotionWriteToStream(const SbgEComLogShipMotion *pLogD SbgErrorCode sbgEComBinaryLogParseShipMotionData(SbgStreamBuffer *pStreamBuffer, SbgEComLogShipMotion *pLogData) { - return sbgEComLogShipMotionReadFromStream(pLogData, pStreamBuffer); + return sbgEComLogShipMotionReadFromStream(pLogData, pStreamBuffer); } SbgErrorCode sbgEComBinaryLogWriteShipMotionData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogShipMotion *pLogData) { - return sbgEComLogShipMotionWriteToStream(pLogData, pStreamBuffer); + return sbgEComLogShipMotionWriteToStream(pLogData, pStreamBuffer); } diff --git a/src/logs/sbgEComLogShipMotion.h b/src/logs/sbgEComLogShipMotion.h index 7671c76..a81de18 100644 --- a/src/logs/sbgEComLogShipMotion.h +++ b/src/logs/sbgEComLogShipMotion.h @@ -1,13 +1,13 @@ /*! - * \file sbgEComLogShipMotion.h - * \ingroup binaryLogs - * \author SBG Systems - * \date 30 March 2013 + * \file sbgEComLogShipMotion.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 30 March 2013 * - * \brief Parse logs that returns ship motion values such as heave. + * \brief Parse logs that returns ship motion values such as heave. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -45,12 +45,12 @@ extern "C" { //- 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. */ +#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 -// @@ -67,12 +67,12 @@ extern "C" { */ typedef struct _SbgEComLogShipMotion { - 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 */ + 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 */ } SbgEComLogShipMotion; //----------------------------------------------------------------------// @@ -82,18 +82,18 @@ typedef struct _SbgEComLogShipMotion /*! * Parse data for the SBG_ECOM_LOG_SHIP_MOTION or SBG_ECOM_LOG_SHIP_MOTION_HP message and fill the corresponding structure. * - * \param[out] pLogData Log structure instance to fill. - * \param[in] pStreamBuffer Input stream buffer to read the log from. - * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. + * \param[out] pLogData Log structure instance to fill. + * \param[in] pStreamBuffer Input stream buffer to read the log from. + * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. */ SbgErrorCode sbgEComLogShipMotionReadFromStream(SbgEComLogShipMotion *pLogData, SbgStreamBuffer *pStreamBuffer); /*! * 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[in] pLogData Log structure instance to write. - * \param[out] pStreamBuffer Output stream buffer to write the log to. - * \return SBG_NO_ERROR if the log has been written to the stream buffer. + * \param[in] pLogData Log structure instance to write. + * \param[out] pStreamBuffer Output stream buffer to write the log to. + * \return SBG_NO_ERROR if the log has been written to the stream buffer. */ SbgErrorCode sbgEComLogShipMotionWriteToStream(const SbgEComLogShipMotion *pLogData, SbgStreamBuffer *pStreamBuffer); diff --git a/src/logs/sbgEComLogStatus.c b/src/logs/sbgEComLogStatus.c index 0286535..ac5c717 100644 --- a/src/logs/sbgEComLogStatus.c +++ b/src/logs/sbgEComLogStatus.c @@ -7,8 +7,8 @@ /*! * CAN bus status definitions for comStatus field. */ -#define SBG_ECOM_LOG_STATUS_CAN_BUS_STATUS_SHIFT (28u) /*!< Shift used to access the CAN status part. */ -#define SBG_ECOM_LOG_STATUS_CAN_BUS_STATUS_MASK (0x00000007u) /*!< Mask used to keep only the CAN status part. */ +#define SBG_ECOM_LOG_STATUS_CAN_BUS_STATUS_SHIFT (28u) /*!< Shift used to access the CAN status part. */ +#define SBG_ECOM_LOG_STATUS_CAN_BUS_STATUS_MASK (0x00000007u) /*!< Mask used to keep only the CAN status part. */ //----------------------------------------------------------------------// //- Public methods -// @@ -16,51 +16,60 @@ SbgErrorCode sbgEComLogStatusReadFromStream(SbgEComLogStatus *pLogData, SbgStreamBuffer *pStreamBuffer) { - assert(pStreamBuffer); - assert(pLogData); - - pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); - pLogData->generalStatus = sbgStreamBufferReadUint16LE(pStreamBuffer); - pLogData->comStatus2 = sbgStreamBufferReadUint16LE(pStreamBuffer); - pLogData->comStatus = sbgStreamBufferReadUint32LE(pStreamBuffer); - pLogData->aidingStatus = sbgStreamBufferReadUint32LE(pStreamBuffer); - pLogData->reserved2 = sbgStreamBufferReadUint32LE(pStreamBuffer); - pLogData->reserved3 = sbgStreamBufferReadUint16LE(pStreamBuffer); - - // - // Test if we have a additional information such as uptime (since version 1.7) - // - if (sbgStreamBufferGetSpace(pStreamBuffer) >= sizeof(uint32_t)) - { - pLogData->uptime = sbgStreamBufferReadUint32LE(pStreamBuffer); - } - else - { - pLogData->uptime = 0; - } - - return sbgStreamBufferGetLastError(pStreamBuffer); + assert(pStreamBuffer); + assert(pLogData); + + pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); + pLogData->generalStatus = sbgStreamBufferReadUint16LE(pStreamBuffer); + pLogData->comStatus2 = sbgStreamBufferReadUint16LE(pStreamBuffer); + pLogData->comStatus = sbgStreamBufferReadUint32LE(pStreamBuffer); + pLogData->aidingStatus = sbgStreamBufferReadUint32LE(pStreamBuffer); + pLogData->reserved2 = sbgStreamBufferReadUint32LE(pStreamBuffer); + pLogData->reserved3 = sbgStreamBufferReadUint16LE(pStreamBuffer); + + // + // Device up time added in version 1.7 + // + if (sbgStreamBufferGetSpace(pStreamBuffer) >= sizeof(uint32_t)) + { + pLogData->uptime = sbgStreamBufferReadUint32LE(pStreamBuffer); + } + else + { + pLogData->uptime = 0; + } + + // + // CPU usage added in version 5.0 + // + if (sbgStreamBufferGetSpace(pStreamBuffer) >= sizeof(uint8_t)) + { + pLogData->cpuUsage = sbgStreamBufferReadUint8LE(pStreamBuffer); + } + else + { + pLogData->cpuUsage = UINT8_MAX; + } + + return sbgStreamBufferGetLastError(pStreamBuffer); } SbgErrorCode sbgEComLogStatusWriteToStream(const SbgEComLogStatus *pLogData, SbgStreamBuffer *pStreamBuffer) { - assert(pStreamBuffer); - assert(pLogData); - - sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); - sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->generalStatus); - sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->comStatus2); - sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->comStatus); - sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->aidingStatus); - sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->reserved2); - sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->reserved3); - - // - // Write the additional information added in version 1.7 - // - sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->uptime); - - return sbgStreamBufferGetLastError(pStreamBuffer); + assert(pStreamBuffer); + assert(pLogData); + + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); + sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->generalStatus); + sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->comStatus2); + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->comStatus); + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->aidingStatus); + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->reserved2); + sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->reserved3); + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->uptime); + sbgStreamBufferWriteUint8LE(pStreamBuffer, pLogData->cpuUsage); + + return sbgStreamBufferGetLastError(pStreamBuffer); } //----------------------------------------------------------------------// @@ -69,18 +78,32 @@ SbgErrorCode sbgEComLogStatusWriteToStream(const SbgEComLogStatus *pLogData, Sbg void sbgEComLogStatusSetCanBusStatus(SbgEComLogStatus *pLogData, SbgEComCanBusStatus status) { - assert(pLogData); - assert(status <= SBG_ECOM_LOG_STATUS_CAN_BUS_STATUS_MASK); + assert(pLogData); + assert(status <= SBG_ECOM_LOG_STATUS_CAN_BUS_STATUS_MASK); - pLogData->comStatus &= ~(SBG_ECOM_LOG_STATUS_CAN_BUS_STATUS_MASK << SBG_ECOM_LOG_STATUS_CAN_BUS_STATUS_SHIFT); - pLogData->comStatus |= ((uint32_t)status&SBG_ECOM_LOG_STATUS_CAN_BUS_STATUS_MASK) << SBG_ECOM_LOG_STATUS_CAN_BUS_STATUS_SHIFT; + pLogData->comStatus &= ~(SBG_ECOM_LOG_STATUS_CAN_BUS_STATUS_MASK << SBG_ECOM_LOG_STATUS_CAN_BUS_STATUS_SHIFT); + pLogData->comStatus |= ((uint32_t)status&SBG_ECOM_LOG_STATUS_CAN_BUS_STATUS_MASK) << SBG_ECOM_LOG_STATUS_CAN_BUS_STATUS_SHIFT; } SbgEComCanBusStatus sbgEComLogStatusGetCanBusStatus(const SbgEComLogStatus *pLogData) { - assert(pLogData); + assert(pLogData); - return (SbgEComCanBusStatus)((pLogData->comStatus >> SBG_ECOM_LOG_STATUS_CAN_BUS_STATUS_SHIFT)&SBG_ECOM_LOG_STATUS_CAN_BUS_STATUS_MASK); + return (SbgEComCanBusStatus)((pLogData->comStatus >> SBG_ECOM_LOG_STATUS_CAN_BUS_STATUS_SHIFT)&SBG_ECOM_LOG_STATUS_CAN_BUS_STATUS_MASK); +} + +bool sbgEComLogStatusIsCpuUsageAvailable(const SbgEComLogStatus *pLogData) +{ + assert(pLogData); + + if (pLogData->cpuUsage != UINT8_MAX) + { + return true; + } + else + { + return false; + } } //----------------------------------------------------------------------// @@ -89,20 +112,20 @@ SbgEComCanBusStatus sbgEComLogStatusGetCanBusStatus(const SbgEComLogStatus *pLog SbgErrorCode sbgEComBinaryLogParseStatusData(SbgStreamBuffer *pStreamBuffer, SbgEComLogStatus *pLogData) { - return sbgEComLogStatusReadFromStream(pLogData, pStreamBuffer); + return sbgEComLogStatusReadFromStream(pLogData, pStreamBuffer); } SbgErrorCode sbgEComBinaryLogWriteStatusData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogStatus *pLogData) { - return sbgEComLogStatusWriteToStream(pLogData, pStreamBuffer); + return sbgEComLogStatusWriteToStream(pLogData, pStreamBuffer); } SbgEComCanBusStatus sbgEComLogStatusGetCanStatus(uint32_t status) { - return (SbgEComCanBusStatus)((status >> SBG_ECOM_LOG_STATUS_CAN_BUS_STATUS_SHIFT) & SBG_ECOM_LOG_STATUS_CAN_BUS_STATUS_MASK); + return (SbgEComCanBusStatus)((status >> SBG_ECOM_LOG_STATUS_CAN_BUS_STATUS_SHIFT) & SBG_ECOM_LOG_STATUS_CAN_BUS_STATUS_MASK); } uint32_t sbgEComLogStatusBuildCommunicationStatus(SbgEComCanBusStatus canStatus, uint32_t masks) { - return ((((uint32_t)canStatus)&SBG_ECOM_LOG_STATUS_CAN_BUS_STATUS_MASK) << SBG_ECOM_LOG_STATUS_CAN_BUS_STATUS_SHIFT) | masks; + return ((((uint32_t)canStatus)&SBG_ECOM_LOG_STATUS_CAN_BUS_STATUS_MASK) << SBG_ECOM_LOG_STATUS_CAN_BUS_STATUS_SHIFT) | masks; } diff --git a/src/logs/sbgEComLogStatus.h b/src/logs/sbgEComLogStatus.h index e117410..105e4c4 100644 --- a/src/logs/sbgEComLogStatus.h +++ b/src/logs/sbgEComLogStatus.h @@ -1,13 +1,13 @@ /*! - * \file sbgEComLogStatus.h - * \ingroup binaryLogs - * \author SBG Systems - * \date 03 April 2013 + * \file sbgEComLogStatus.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 03 April 2013 * - * \brief Parse logs used to report device status. + * \brief Parse logs used to report device status. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -44,13 +44,13 @@ extern "C" { //----------------------------------------------------------------------// //- 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.*/ +#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 -// @@ -59,78 +59,78 @@ extern "C" { /*! * Communication status bit mask definitions (comStatus). */ -#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. */ +#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, /*!< CAN bus is OFF (either not enabled or disabled due to too much errors). */ - SBG_ECOM_CAN_BUS_TX_RX_ERR = 1, /*!< CAN bus transmitter or receiver has error. */ - SBG_ECOM_CAN_BUS_OK = 2, /*!< CAN Bus is OK. */ - SBG_ECOM_CAN_BUS_ERROR = 3 /*!< CAN Bus has a general error such as busy bus. */ + SBG_ECOM_CAN_BUS_OFF = 0, /*!< CAN bus is OFF (either not enabled or disabled due to too much errors). */ + SBG_ECOM_CAN_BUS_TX_RX_ERR = 1, /*!< CAN bus transmitter or receiver has error. */ + SBG_ECOM_CAN_BUS_OK = 2, /*!< CAN Bus is OK. */ + SBG_ECOM_CAN_BUS_ERROR = 3 /*!< CAN Bus has a general error such as busy bus. */ } SbgEComCanBusStatus; /*! * Second communication status bit mask definitions (comStatus2). */ -#define SBG_ECOM_COM2_ETH0_RX_OK (0x0001u << 0) /*!< Set to 0 in case of error on ETH0 input. */ -#define SBG_ECOM_COM2_ETH0_TX_OK (0x0001u << 1) /*!< Set to 0 in case of error on ETH0 output. */ -#define SBG_ECOM_COM2_ETH1_RX_OK (0x0001u << 2) /*!< Set to 0 in case of error on ETH1 input. */ -#define SBG_ECOM_COM2_ETH1_TX_OK (0x0001u << 3) /*!< Set to 0 in case of error on ETH1 output. */ -#define SBG_ECOM_COM2_ETH2_RX_OK (0x0001u << 4) /*!< Set to 0 in case of error on ETH2 input. */ -#define SBG_ECOM_COM2_ETH2_TX_OK (0x0001u << 5) /*!< Set to 0 in case of error on ETH2 output. */ -#define SBG_ECOM_COM2_ETH3_RX_OK (0x0001u << 6) /*!< Set to 0 in case of error on ETH3 input. */ -#define SBG_ECOM_COM2_ETH3_TX_OK (0x0001u << 7) /*!< Set to 0 in case of error on ETH3 output. */ -#define SBG_ECOM_COM2_ETH4_RX_OK (0x0001u << 8) /*!< Set to 0 in case of error on ETH4 input. */ -#define SBG_ECOM_COM2_ETH4_TX_OK (0x0001u << 9) /*!< Set to 0 in case of error on ETH4 output. */ +#define SBG_ECOM_COM2_ETH0_RX_OK (0x0001u << 0) /*!< Set to 0 in case of error on ETH0 input. */ +#define SBG_ECOM_COM2_ETH0_TX_OK (0x0001u << 1) /*!< Set to 0 in case of error on ETH0 output. */ +#define SBG_ECOM_COM2_ETH1_RX_OK (0x0001u << 2) /*!< Set to 0 in case of error on ETH1 input. */ +#define SBG_ECOM_COM2_ETH1_TX_OK (0x0001u << 3) /*!< Set to 0 in case of error on ETH1 output. */ +#define SBG_ECOM_COM2_ETH2_RX_OK (0x0001u << 4) /*!< Set to 0 in case of error on ETH2 input. */ +#define SBG_ECOM_COM2_ETH2_TX_OK (0x0001u << 5) /*!< Set to 0 in case of error on ETH2 output. */ +#define SBG_ECOM_COM2_ETH3_RX_OK (0x0001u << 6) /*!< Set to 0 in case of error on ETH3 input. */ +#define SBG_ECOM_COM2_ETH3_TX_OK (0x0001u << 7) /*!< Set to 0 in case of error on ETH3 output. */ +#define SBG_ECOM_COM2_ETH4_RX_OK (0x0001u << 8) /*!< Set to 0 in case of error on ETH4 input. */ +#define SBG_ECOM_COM2_ETH4_TX_OK (0x0001u << 9) /*!< Set to 0 in case of error on ETH4 output. */ //----------------------------------------------------------------------// //- 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. */ +#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 -// @@ -141,14 +141,15 @@ typedef enum _SbgEComCanBusStatus */ typedef struct _SbgEComLogStatus { - uint32_t timeStamp; /*!< Time in us since the sensor power up. */ - uint16_t generalStatus; /*!< General status bitmask and enum. */ - uint32_t comStatus; /*!< Communication status bitmask and enum. */ - uint16_t comStatus2; /*!< Second communication status bitmask and enum. */ - uint32_t aidingStatus; /*!< Aiding equipments status bitmask and enum. */ - 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. */ + uint32_t timeStamp; /*!< Time in us since the sensor power up. */ + uint16_t generalStatus; /*!< General status bitmask and enum. */ + uint32_t comStatus; /*!< Communication status bitmask and enum. */ + uint16_t comStatus2; /*!< Second communication status bitmask and enum (used since v4.0). */ + uint32_t aidingStatus; /*!< Aiding equipments status bitmask and enum. */ + 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 - 0 if N/A (added v1.7). */ + uint8_t cpuUsage; /*!< Main CPU usage in percent - 0xFF if N/A (added v5.0). */ } SbgEComLogStatus; //----------------------------------------------------------------------// @@ -158,18 +159,18 @@ typedef struct _SbgEComLogStatus /*! * Parse data for the SBG_ECOM_LOG_STATUS message and fill the corresponding structure. * - * \param[out] pLogData Log structure instance to fill. - * \param[in] pStreamBuffer Input stream buffer to read the log from. - * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. + * \param[out] pLogData Log structure instance to fill. + * \param[in] pStreamBuffer Input stream buffer to read the log from. + * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. */ SbgErrorCode sbgEComLogStatusReadFromStream(SbgEComLogStatus *pLogData, SbgStreamBuffer *pStreamBuffer); /*! * Write data for the SBG_ECOM_LOG_STATUS message to the output stream buffer from the provided structure. * - * \param[in] pLogData Log structure instance to write. - * \param[out] pStreamBuffer Output stream buffer to write the log to. - * \return SBG_NO_ERROR if the log has been written to the stream buffer. + * \param[in] pLogData Log structure instance to write. + * \param[out] pStreamBuffer Output stream buffer to write the log to. + * \return SBG_NO_ERROR if the log has been written to the stream buffer. */ SbgErrorCode sbgEComLogStatusWriteToStream(const SbgEComLogStatus *pLogData, SbgStreamBuffer *pStreamBuffer); @@ -180,19 +181,27 @@ SbgErrorCode sbgEComLogStatusWriteToStream(const SbgEComLogStatus *pLogData, Sbg /*! * Defines the CAN bus status. * - * \param[out] pLogData Log status instance. - * \param[in] status CAN bus status to set. + * \param[out] pLogData Log status instance. + * \param[in] status CAN bus status to set. */ void sbgEComLogStatusSetCanBusStatus(SbgEComLogStatus *pLogData, SbgEComCanBusStatus status); /*! * Returns the CAN bus status. * - * \param[in] pLogData Log status instance. - * \return CAN bus status. + * \param[in] pLogData Log status instance. + * \return CAN bus status. */ SbgEComCanBusStatus sbgEComLogStatusGetCanBusStatus(const SbgEComLogStatus *pLogData); +/*! + * Returns true if the CPU usage in percent is available. + * + * \param[in] pLogData Log status instance. + * \return true if available. + */ +bool sbgEComLogStatusIsCpuUsageAvailable(const SbgEComLogStatus *pLogData); + //----------------------------------------------------------------------// //- DEPRECATED - Used for backward compatibility -// //----------------------------------------------------------------------// diff --git a/src/logs/sbgEComLogUsbl.c b/src/logs/sbgEComLogUsbl.c index d31a9c9..ddf2510 100644 --- a/src/logs/sbgEComLogUsbl.c +++ b/src/logs/sbgEComLogUsbl.c @@ -6,42 +6,42 @@ SbgErrorCode sbgEComLogUsblReadFromStream(SbgEComLogUsbl *pLogData, SbgStreamBuffer *pStreamBuffer) { - assert(pStreamBuffer); - assert(pLogData); + assert(pStreamBuffer); + assert(pLogData); - pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); - pLogData->status = sbgStreamBufferReadUint16LE(pStreamBuffer); + pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); + pLogData->status = sbgStreamBufferReadUint16LE(pStreamBuffer); - pLogData->latitude = sbgStreamBufferReadDoubleLE(pStreamBuffer); - pLogData->longitude = sbgStreamBufferReadDoubleLE(pStreamBuffer); + pLogData->latitude = sbgStreamBufferReadDoubleLE(pStreamBuffer); + pLogData->longitude = sbgStreamBufferReadDoubleLE(pStreamBuffer); - pLogData->depth = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->depth = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->latitudeAccuracy = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->longitudeAccuracy = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->depthAccuracy = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->latitudeAccuracy = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->longitudeAccuracy = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->depthAccuracy = sbgStreamBufferReadFloatLE(pStreamBuffer); - return sbgStreamBufferGetLastError(pStreamBuffer); + return sbgStreamBufferGetLastError(pStreamBuffer); } SbgErrorCode sbgEComLogUsblWriteToStream(const SbgEComLogUsbl *pLogData, SbgStreamBuffer *pStreamBuffer) { - assert(pStreamBuffer); - assert(pLogData); + assert(pStreamBuffer); + assert(pLogData); - sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); - sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->status); + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); + sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->status); - sbgStreamBufferWriteDoubleLE(pStreamBuffer, pLogData->latitude); - sbgStreamBufferWriteDoubleLE(pStreamBuffer, pLogData->longitude); + sbgStreamBufferWriteDoubleLE(pStreamBuffer, pLogData->latitude); + sbgStreamBufferWriteDoubleLE(pStreamBuffer, pLogData->longitude); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->depth); - - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->latitudeAccuracy); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->longitudeAccuracy); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->depthAccuracy); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->depth); + + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->latitudeAccuracy); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->longitudeAccuracy); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->depthAccuracy); - return sbgStreamBufferGetLastError(pStreamBuffer); + return sbgStreamBufferGetLastError(pStreamBuffer); } //----------------------------------------------------------------------// @@ -50,10 +50,10 @@ SbgErrorCode sbgEComLogUsblWriteToStream(const SbgEComLogUsbl *pLogData, SbgStre SbgErrorCode sbgEComBinaryLogParseUsblData(SbgStreamBuffer *pStreamBuffer, SbgEComLogUsbl *pLogData) { - return sbgEComLogUsblReadFromStream(pLogData, pStreamBuffer); + return sbgEComLogUsblReadFromStream(pLogData, pStreamBuffer); } SbgErrorCode sbgEComBinaryLogWriteUsblData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogUsbl *pLogData) { - return sbgEComLogUsblWriteToStream(pLogData, pStreamBuffer); + return sbgEComLogUsblWriteToStream(pLogData, pStreamBuffer); } diff --git a/src/logs/sbgEComLogUsbl.h b/src/logs/sbgEComLogUsbl.h index a630e43..551a4dd 100644 --- a/src/logs/sbgEComLogUsbl.h +++ b/src/logs/sbgEComLogUsbl.h @@ -1,15 +1,15 @@ /*! - * \file sbgEComLogUsbl.h - * \ingroup binaryLogs - * \author SBG Systems - * \date 02 June 2014 + * \file sbgEComLogUsbl.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 02 June 2014 * - * \brief Parse received USBL position mesurements logs. + * \brief Parse received USBL position mesurements logs. * * USBL binary logs contains underwater positioning data of a USBL beacon. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -50,9 +50,9 @@ extern "C" { /*! * 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. */ +#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 -// @@ -63,17 +63,17 @@ extern "C" { */ typedef struct _SbgEComLogUsbl { - uint32_t timeStamp; /*!< Time in us since the sensor power up. */ - uint16_t status; /*!< USBL system status bitmask. */ + 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. */ + 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 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. */ + float latitudeAccuracy; /*!< 1 sigma latitude accuracy in meters. */ + float longitudeAccuracy; /*!< 1 sigma longitude accuracy in meters. */ + float depthAccuracy; /*!< 1 sigma depth accuracy in meters. */ } SbgEComLogUsbl; //----------------------------------------------------------------------// @@ -83,18 +83,18 @@ typedef struct _SbgEComLogUsbl /*! * Parse data for the SBG_ECOM_LOG_USBL message and fill the corresponding structure. * - * \param[out] pLogData Log structure instance to fill. - * \param[in] pStreamBuffer Input stream buffer to read the log from. - * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. + * \param[out] pLogData Log structure instance to fill. + * \param[in] pStreamBuffer Input stream buffer to read the log from. + * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. */ SbgErrorCode sbgEComLogUsblReadFromStream(SbgEComLogUsbl *pLogData, SbgStreamBuffer *pStreamBuffer); /*! * Write data for the SBG_ECOM_LOG_USBL message to the output stream buffer from the provided structure. * - * \param[in] pLogData Log structure instance to write. - * \param[out] pStreamBuffer Output stream buffer to write the log to. - * \return SBG_NO_ERROR if the log has been written to the stream buffer. + * \param[in] pLogData Log structure instance to write. + * \param[out] pStreamBuffer Output stream buffer to write the log to. + * \return SBG_NO_ERROR if the log has been written to the stream buffer. */ SbgErrorCode sbgEComLogUsblWriteToStream(const SbgEComLogUsbl *pLogData, SbgStreamBuffer *pStreamBuffer); diff --git a/src/logs/sbgEComLogUtc.c b/src/logs/sbgEComLogUtc.c index 8e4394b..b990b09 100644 --- a/src/logs/sbgEComLogUtc.c +++ b/src/logs/sbgEComLogUtc.c @@ -12,13 +12,13 @@ /*! * Clock status and UTC time status definitions. */ -#define SBG_ECOM_LOG_UTC_CLOCK_STATE_SHIFT (1u) /*!< Shift used to extract the clock state part. */ -#define SBG_ECOM_LOG_UTC_CLOCK_STATE_MASK (0x000Fu) /*!< Mask used to keep only the clock state part. */ -#define SBG_ECOM_LOG_UTC_TIME_STATUS_SHIFT (6u) /*!< Shift used to extract the UTC status part. */ -#define SBG_ECOM_LOG_UTC_TIME_STATUS_MASK (0x000Fu) /*!< Mask used to keep only the UTC status part. */ +#define SBG_ECOM_LOG_UTC_CLOCK_STATE_SHIFT (1u) /*!< Shift used to extract the clock state part. */ +#define SBG_ECOM_LOG_UTC_CLOCK_STATE_MASK (0x000Fu) /*!< Mask used to keep only the clock state part. */ +#define SBG_ECOM_LOG_UTC_TIME_STATUS_SHIFT (6u) /*!< Shift used to extract the UTC status part. */ +#define SBG_ECOM_LOG_UTC_TIME_STATUS_MASK (0x000Fu) /*!< Mask used to keep only the UTC status part. */ -#define SBG_ECOM_LOG_UTC_STATUS_HAS_CLOCK_INPUT (0x0001u << 0) /*!< Set to 1 if a stable input clock could be used to synchronized the internal clock. */ -#define SBG_ECOM_LOG_UTC_STATUS_UTC_IS_ACCURATE (0x0001u << 5) /*!< The UTC time information is accurate and correctly timestamped with a PPS. */ +#define SBG_ECOM_LOG_UTC_STATUS_HAS_CLOCK_INPUT (0x0001u << 0) /*!< Set to 1 if a stable input clock could be used to synchronized the internal clock. */ +#define SBG_ECOM_LOG_UTC_STATUS_UTC_IS_ACCURATE (0x0001u << 5) /*!< The UTC time information is accurate and correctly timestamped with a PPS. */ //----------------------------------------------------------------------// //- Public methods -// @@ -26,68 +26,68 @@ void sbgEComLogUtcZeroInit(SbgEComLogUtc *pLogData) { - assert(pLogData); + assert(pLogData); - memset(pLogData, 0x00, sizeof(*pLogData)); + memset(pLogData, 0x00, sizeof(*pLogData)); - pLogData->clkBiasStd = NAN; - pLogData->clkSfErrorStd = NAN; - pLogData->clkResidualError = NAN; + pLogData->clkBiasStd = NAN; + pLogData->clkSfErrorStd = NAN; + pLogData->clkResidualError = NAN; } SbgErrorCode sbgEComLogUtcReadFromStream(SbgEComLogUtc *pLogData, SbgStreamBuffer *pStreamBuffer) { - assert(pStreamBuffer); - assert(pLogData); - - pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); - pLogData->status = sbgStreamBufferReadUint16LE(pStreamBuffer); - pLogData->year = sbgStreamBufferReadUint16LE(pStreamBuffer); - pLogData->month = sbgStreamBufferReadInt8LE(pStreamBuffer); - pLogData->day = sbgStreamBufferReadInt8LE(pStreamBuffer); - pLogData->hour = sbgStreamBufferReadInt8LE(pStreamBuffer); - pLogData->minute = sbgStreamBufferReadInt8LE(pStreamBuffer); - pLogData->second = sbgStreamBufferReadInt8LE(pStreamBuffer); - pLogData->nanoSecond = sbgStreamBufferReadInt32LE(pStreamBuffer); - pLogData->gpsTimeOfWeek = sbgStreamBufferReadUint32LE(pStreamBuffer); - - if (sbgStreamBufferGetSpace(pStreamBuffer) >= 3*sizeof(float)) - { - pLogData->clkBiasStd = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->clkSfErrorStd = sbgStreamBufferReadFloatLE(pStreamBuffer); - pLogData->clkResidualError = sbgStreamBufferReadFloatLE(pStreamBuffer); - } - else - { - pLogData->clkBiasStd = NAN; - pLogData->clkSfErrorStd = NAN; - pLogData->clkResidualError = NAN; - } - - return sbgStreamBufferGetLastError(pStreamBuffer); + assert(pStreamBuffer); + assert(pLogData); + + pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); + pLogData->status = sbgStreamBufferReadUint16LE(pStreamBuffer); + pLogData->year = sbgStreamBufferReadUint16LE(pStreamBuffer); + pLogData->month = sbgStreamBufferReadInt8LE(pStreamBuffer); + pLogData->day = sbgStreamBufferReadInt8LE(pStreamBuffer); + pLogData->hour = sbgStreamBufferReadInt8LE(pStreamBuffer); + pLogData->minute = sbgStreamBufferReadInt8LE(pStreamBuffer); + pLogData->second = sbgStreamBufferReadInt8LE(pStreamBuffer); + pLogData->nanoSecond = sbgStreamBufferReadInt32LE(pStreamBuffer); + pLogData->gpsTimeOfWeek = sbgStreamBufferReadUint32LE(pStreamBuffer); + + if (sbgStreamBufferGetSpace(pStreamBuffer) >= 3*sizeof(float)) + { + pLogData->clkBiasStd = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->clkSfErrorStd = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->clkResidualError = sbgStreamBufferReadFloatLE(pStreamBuffer); + } + else + { + pLogData->clkBiasStd = NAN; + pLogData->clkSfErrorStd = NAN; + pLogData->clkResidualError = NAN; + } + + return sbgStreamBufferGetLastError(pStreamBuffer); } SbgErrorCode sbgEComLogUtcWriteToStream(const SbgEComLogUtc *pLogData, SbgStreamBuffer *pStreamBuffer) { - assert(pStreamBuffer); - assert(pLogData); - - sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); - sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->status); - sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->year); - sbgStreamBufferWriteInt8LE(pStreamBuffer, pLogData->month); - sbgStreamBufferWriteInt8LE(pStreamBuffer, pLogData->day); - sbgStreamBufferWriteInt8LE(pStreamBuffer, pLogData->hour); - sbgStreamBufferWriteInt8LE(pStreamBuffer, pLogData->minute); - sbgStreamBufferWriteInt8LE(pStreamBuffer, pLogData->second); - sbgStreamBufferWriteInt32LE(pStreamBuffer, pLogData->nanoSecond); - sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->gpsTimeOfWeek); - - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->clkBiasStd); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->clkSfErrorStd); - sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->clkResidualError); - - return sbgStreamBufferGetLastError(pStreamBuffer); + assert(pStreamBuffer); + assert(pLogData); + + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); + sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->status); + sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->year); + sbgStreamBufferWriteInt8LE(pStreamBuffer, pLogData->month); + sbgStreamBufferWriteInt8LE(pStreamBuffer, pLogData->day); + sbgStreamBufferWriteInt8LE(pStreamBuffer, pLogData->hour); + sbgStreamBufferWriteInt8LE(pStreamBuffer, pLogData->minute); + sbgStreamBufferWriteInt8LE(pStreamBuffer, pLogData->second); + sbgStreamBufferWriteInt32LE(pStreamBuffer, pLogData->nanoSecond); + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->gpsTimeOfWeek); + + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->clkBiasStd); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->clkSfErrorStd); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->clkResidualError); + + return sbgStreamBufferGetLastError(pStreamBuffer); } //----------------------------------------------------------------------// @@ -96,148 +96,148 @@ SbgErrorCode sbgEComLogUtcWriteToStream(const SbgEComLogUtc *pLogData, SbgStream void sbgEComLogUtcSetClockState(SbgEComLogUtc *pLogData, SbgEComClockState status) { - assert(pLogData); - assert(status <= SBG_ECOM_LOG_UTC_CLOCK_STATE_MASK); + assert(pLogData); + assert(status <= SBG_ECOM_LOG_UTC_CLOCK_STATE_MASK); - pLogData->status &= ~(SBG_ECOM_LOG_UTC_CLOCK_STATE_MASK << SBG_ECOM_LOG_UTC_CLOCK_STATE_SHIFT); - pLogData->status |= ((uint16_t)status&SBG_ECOM_LOG_UTC_CLOCK_STATE_MASK) << SBG_ECOM_LOG_UTC_CLOCK_STATE_SHIFT; + pLogData->status &= ~(SBG_ECOM_LOG_UTC_CLOCK_STATE_MASK << SBG_ECOM_LOG_UTC_CLOCK_STATE_SHIFT); + pLogData->status |= ((uint16_t)status&SBG_ECOM_LOG_UTC_CLOCK_STATE_MASK) << SBG_ECOM_LOG_UTC_CLOCK_STATE_SHIFT; } SbgEComClockState sbgEComLogUtcGetClockState(const SbgEComLogUtc *pLogData) { - assert(pLogData); + assert(pLogData); - return (SbgEComClockState)((pLogData->status >> SBG_ECOM_LOG_UTC_CLOCK_STATE_SHIFT)&SBG_ECOM_LOG_UTC_CLOCK_STATE_MASK); + return (SbgEComClockState)((pLogData->status >> SBG_ECOM_LOG_UTC_CLOCK_STATE_SHIFT)&SBG_ECOM_LOG_UTC_CLOCK_STATE_MASK); } const char *sbgEComLogUtcGetClockStateAsString(const SbgEComLogUtc *pLogData) { - static const char *clockStateStr[] = - { - [SBG_ECOM_CLOCK_STATE_ERROR] = "error", - [SBG_ECOM_CLOCK_STATE_FREE_RUNNING] = "free", - [SBG_ECOM_CLOCK_STATE_STEERING] = "steering", - [SBG_ECOM_CLOCK_STATE_VALID] = "valid" - }; - - SbgEComClockState clockState; - - assert(pLogData); - - clockState = sbgEComLogUtcGetClockState(pLogData); - - if (clockState < SBG_ARRAY_SIZE(clockStateStr)) - { - return clockStateStr[clockState]; - } - else - { - return "undefined"; - } + static const char *clockStateStr[] = + { + [SBG_ECOM_CLOCK_STATE_ERROR] = "error", + [SBG_ECOM_CLOCK_STATE_FREE_RUNNING] = "free", + [SBG_ECOM_CLOCK_STATE_STEERING] = "steering", + [SBG_ECOM_CLOCK_STATE_VALID] = "valid" + }; + + SbgEComClockState clockState; + + assert(pLogData); + + clockState = sbgEComLogUtcGetClockState(pLogData); + + if (clockState < SBG_ARRAY_SIZE(clockStateStr)) + { + return clockStateStr[clockState]; + } + else + { + return "undefined"; + } } void sbgEComLogUtcSetUtcStatus(SbgEComLogUtc *pLogData, SbgEComUtcStatus status) { - assert(pLogData); - assert(status <= SBG_ECOM_LOG_UTC_TIME_STATUS_MASK); + assert(pLogData); + assert(status <= SBG_ECOM_LOG_UTC_TIME_STATUS_MASK); - pLogData->status &= ~(SBG_ECOM_LOG_UTC_TIME_STATUS_MASK << SBG_ECOM_LOG_UTC_TIME_STATUS_SHIFT); - pLogData->status |= ((uint16_t)status&SBG_ECOM_LOG_UTC_TIME_STATUS_MASK) << SBG_ECOM_LOG_UTC_TIME_STATUS_SHIFT; + pLogData->status &= ~(SBG_ECOM_LOG_UTC_TIME_STATUS_MASK << SBG_ECOM_LOG_UTC_TIME_STATUS_SHIFT); + pLogData->status |= ((uint16_t)status&SBG_ECOM_LOG_UTC_TIME_STATUS_MASK) << SBG_ECOM_LOG_UTC_TIME_STATUS_SHIFT; } SbgEComUtcStatus sbgEComLogUtcGetUtcStatus(const SbgEComLogUtc *pLogData) { - assert(pLogData); + assert(pLogData); - return (SbgEComUtcStatus)((pLogData->status >> SBG_ECOM_LOG_UTC_TIME_STATUS_SHIFT)&SBG_ECOM_LOG_UTC_TIME_STATUS_MASK); + return (SbgEComUtcStatus)((pLogData->status >> SBG_ECOM_LOG_UTC_TIME_STATUS_SHIFT)&SBG_ECOM_LOG_UTC_TIME_STATUS_MASK); } const char *sbgEComLogUtcGetUtcStatusAsString(const SbgEComLogUtc *pLogData) { - static const char *utcStatusStr[] = - { - [SBG_ECOM_UTC_STATUS_INVALID] = "invalid", - [SBG_ECOM_UTC_STATUS_NO_LEAP_SEC] = "noLeapSec", - [SBG_ECOM_UTC_STATUS_INITIALIZED] = "initialized", - }; - - SbgEComUtcStatus utcStatus; - - assert(pLogData); - - utcStatus = sbgEComLogUtcGetUtcStatus(pLogData); - - if (utcStatus < SBG_ARRAY_SIZE(utcStatusStr)) - { - return utcStatusStr[utcStatus]; - } - else - { - return "undefined"; - } + static const char *utcStatusStr[] = + { + [SBG_ECOM_UTC_STATUS_INVALID] = "invalid", + [SBG_ECOM_UTC_STATUS_NO_LEAP_SEC] = "noLeapSec", + [SBG_ECOM_UTC_STATUS_INITIALIZED] = "initialized", + }; + + SbgEComUtcStatus utcStatus; + + assert(pLogData); + + utcStatus = sbgEComLogUtcGetUtcStatus(pLogData); + + if (utcStatus < SBG_ARRAY_SIZE(utcStatusStr)) + { + return utcStatusStr[utcStatus]; + } + else + { + return "undefined"; + } } void sbgEComLogUtcSetHasClockInput(SbgEComLogUtc *pLogData, bool hasClockInput) { - assert(pLogData); - - if (hasClockInput) - { - pLogData->status |= SBG_ECOM_LOG_UTC_STATUS_HAS_CLOCK_INPUT; - } - else - { - pLogData->status &= ~SBG_ECOM_LOG_UTC_STATUS_HAS_CLOCK_INPUT; - } + assert(pLogData); + + if (hasClockInput) + { + pLogData->status |= SBG_ECOM_LOG_UTC_STATUS_HAS_CLOCK_INPUT; + } + else + { + pLogData->status &= ~SBG_ECOM_LOG_UTC_STATUS_HAS_CLOCK_INPUT; + } } bool sbgEComLogUtcHasClockInput(const SbgEComLogUtc *pLogData) { - assert(pLogData); + assert(pLogData); - return (pLogData->status&SBG_ECOM_LOG_UTC_STATUS_HAS_CLOCK_INPUT?true:false); + return (pLogData->status&SBG_ECOM_LOG_UTC_STATUS_HAS_CLOCK_INPUT?true:false); } void sbgEComLogUtcTimeSetIsAccurate(SbgEComLogUtc *pLogData, bool utcIsAccurate) { - assert(pLogData); - - if (utcIsAccurate) - { - pLogData->status |= SBG_ECOM_LOG_UTC_STATUS_UTC_IS_ACCURATE; - } - else - { - pLogData->status &= ~SBG_ECOM_LOG_UTC_STATUS_UTC_IS_ACCURATE; - } + assert(pLogData); + + if (utcIsAccurate) + { + pLogData->status |= SBG_ECOM_LOG_UTC_STATUS_UTC_IS_ACCURATE; + } + else + { + pLogData->status &= ~SBG_ECOM_LOG_UTC_STATUS_UTC_IS_ACCURATE; + } } bool sbgEComLogUtcTimeIsAccurate(const SbgEComLogUtc *pLogData) { - assert(pLogData); + assert(pLogData); - return (pLogData->status&SBG_ECOM_LOG_UTC_STATUS_UTC_IS_ACCURATE?true:false); + return (pLogData->status&SBG_ECOM_LOG_UTC_STATUS_UTC_IS_ACCURATE?true:false); } bool sbgEComLogUtcTimeClkBiasStdIsValid(const SbgEComLogUtc *pLogData) { - assert(pLogData); + assert(pLogData); - return (isnan(pLogData->clkBiasStd)?false:true); + return (isnan(pLogData->clkBiasStd)?false:true); } bool sbgEComLogUtcTimeClkSfErrorStdIsValid(const SbgEComLogUtc *pLogData) { - assert(pLogData); + assert(pLogData); - return (isnan(pLogData->clkSfErrorStd)?false:true); + return (isnan(pLogData->clkSfErrorStd)?false:true); } bool sbgEComLogUtcTimeClkResidualErrorIsValid(const SbgEComLogUtc *pLogData) { - assert(pLogData); + assert(pLogData); - return (isnan(pLogData->clkResidualError)?false:true); + return (isnan(pLogData->clkResidualError)?false:true); } //----------------------------------------------------------------------// @@ -246,36 +246,36 @@ bool sbgEComLogUtcTimeClkResidualErrorIsValid(const SbgEComLogUtc *pLogData) SbgErrorCode sbgEComBinaryLogParseUtcData(SbgStreamBuffer *pStreamBuffer, SbgEComLogUtc *pLogData) { - return sbgEComLogUtcReadFromStream(pLogData, pStreamBuffer); + return sbgEComLogUtcReadFromStream(pLogData, pStreamBuffer); } SbgErrorCode sbgEComBinaryLogWriteUtcData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogUtc *pLogData) { - return sbgEComLogUtcWriteToStream(pLogData, pStreamBuffer); + return sbgEComLogUtcWriteToStream(pLogData, pStreamBuffer); } SbgEComClockState sbgEComLogUtcGetClockStatus(uint16_t status) { - return (SbgEComClockState)((status >> SBG_ECOM_LOG_UTC_CLOCK_STATE_SHIFT) & SBG_ECOM_LOG_UTC_CLOCK_STATE_MASK); + return (SbgEComClockState)((status >> SBG_ECOM_LOG_UTC_CLOCK_STATE_SHIFT) & SBG_ECOM_LOG_UTC_CLOCK_STATE_MASK); } const char *sbgEcomLogUtcGetClockStatusAsString(const SbgEComLogUtc *pLogUtc) { - return sbgEComLogUtcGetClockStateAsString(pLogUtc); + return sbgEComLogUtcGetClockStateAsString(pLogUtc); } SbgEComUtcStatus sbgEComLogUtcGetClockUtcStatus(uint16_t status) { - return (SbgEComUtcStatus)((status >> SBG_ECOM_LOG_UTC_TIME_STATUS_SHIFT) & SBG_ECOM_LOG_UTC_TIME_STATUS_MASK); + return (SbgEComUtcStatus)((status >> SBG_ECOM_LOG_UTC_TIME_STATUS_SHIFT) & SBG_ECOM_LOG_UTC_TIME_STATUS_MASK); } const char *sbgEcomLogUtcGetUtcStatusAsString(const SbgEComLogUtc *pLogUtc) { - return sbgEComLogUtcGetUtcStatusAsString(pLogUtc); + return sbgEComLogUtcGetUtcStatusAsString(pLogUtc); } uint16_t sbgEComLogUtcBuildClockStatus(SbgEComClockState clockState, SbgEComUtcStatus utcStatus, uint16_t masks) { - return ((((uint16_t)clockState)&SBG_ECOM_LOG_UTC_CLOCK_STATE_MASK) << SBG_ECOM_LOG_UTC_CLOCK_STATE_SHIFT) | - ((((uint16_t)utcStatus)&SBG_ECOM_LOG_UTC_TIME_STATUS_MASK) << SBG_ECOM_LOG_UTC_TIME_STATUS_SHIFT) | masks; + return ((((uint16_t)clockState)&SBG_ECOM_LOG_UTC_CLOCK_STATE_MASK) << SBG_ECOM_LOG_UTC_CLOCK_STATE_SHIFT) | + ((((uint16_t)utcStatus)&SBG_ECOM_LOG_UTC_TIME_STATUS_MASK) << SBG_ECOM_LOG_UTC_TIME_STATUS_SHIFT) | masks; } diff --git a/src/logs/sbgEComLogUtc.h b/src/logs/sbgEComLogUtc.h index 3c5d200..dbafca1 100644 --- a/src/logs/sbgEComLogUtc.h +++ b/src/logs/sbgEComLogUtc.h @@ -1,13 +1,13 @@ /*! - * \file sbgEComLogUtc.h - * \ingroup binaryLogs - * \author SBG Systems - * \date 20 February 2013 + * \file sbgEComLogUtc.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 20 February 2013 * - * \brief Parse logs used to report device UTC time. + * \brief Parse logs used to report device UTC time. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -50,10 +50,10 @@ extern "C" { */ typedef enum _SbgEComClockState { - SBG_ECOM_CLOCK_STATE_ERROR = 0, /*!< An error has occurred on the clock estimation. */ - SBG_ECOM_CLOCK_STATE_FREE_RUNNING = 1, /*!< The clock is only based on the internal crystal using latest known clock bias and scale factor. */ - SBG_ECOM_CLOCK_STATE_STEERING = 2, /*!< A PPS has been detected and the clock is converging to it. */ - SBG_ECOM_CLOCK_STATE_VALID = 3 /*!< The internal clock is converged to the PPS signal or is still considered to be accurate. */ + SBG_ECOM_CLOCK_STATE_ERROR = 0, /*!< An error has occurred on the clock estimation. */ + SBG_ECOM_CLOCK_STATE_FREE_RUNNING = 1, /*!< The clock is only based on the internal crystal using latest known clock bias and scale factor. */ + SBG_ECOM_CLOCK_STATE_STEERING = 2, /*!< A PPS has been detected and the clock is converging to it. */ + SBG_ECOM_CLOCK_STATE_VALID = 3 /*!< The internal clock is converged to the PPS signal or is still considered to be accurate. */ } SbgEComClockState; /*! @@ -61,9 +61,9 @@ typedef enum _SbgEComClockState */ typedef enum _SbgEComUtcStatus { - SBG_ECOM_UTC_STATUS_INVALID = 0, /*!< UTC time is not known and invalid. The INS uses the firmware default initial date/time. */ - SBG_ECOM_UTC_STATUS_NO_LEAP_SEC = 1, /*!< UTC time is initialized but the leap second is not known - the default firmware value is used. */ - SBG_ECOM_UTC_STATUS_INITIALIZED = 2 /*!< UTC time is initialized with a fully resolved leap second information. */ + SBG_ECOM_UTC_STATUS_INVALID = 0, /*!< UTC time is not known and invalid. The INS uses the firmware default initial date/time. */ + SBG_ECOM_UTC_STATUS_NO_LEAP_SEC = 1, /*!< UTC time is initialized but the leap second is not known - the default firmware value is used. */ + SBG_ECOM_UTC_STATUS_INITIALIZED = 2 /*!< UTC time is initialized with a fully resolved leap second information. */ } SbgEComUtcStatus; //----------------------------------------------------------------------// @@ -75,19 +75,19 @@ typedef enum _SbgEComUtcStatus */ typedef struct _SbgEComLogUtc { - uint32_t timeStamp; /*!< Time in us since the sensor power up. */ - uint16_t status; /*!< Clock state and UTC time status. */ - 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. */ - float clkBiasStd; /*!< Estimated internal clock bias standard deviation in seconds - set to NaN if not available. (added in 4.0) */ - float clkSfErrorStd; /*!< Estimated internal clock scale factor error standard deviation - set to NaN if not available. (added in 4.0) */ - float clkResidualError; /*!< Latest residual clock error from the GNSS PPS signal in seconds - set to NaN if not available. (added in 4.0) */ + uint32_t timeStamp; /*!< Time in us since the sensor power up. */ + uint16_t status; /*!< Clock state and UTC time status. */ + 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. */ + float clkBiasStd; /*!< Estimated internal clock bias standard deviation in seconds - set to NaN if not available. (added in 4.0) */ + float clkSfErrorStd; /*!< Estimated internal clock scale factor error standard deviation - set to NaN if not available. (added in 4.0) */ + float clkResidualError; /*!< Latest residual clock error from the GNSS PPS signal in seconds - set to NaN if not available. (added in 4.0) */ } SbgEComLogUtc; //----------------------------------------------------------------------// @@ -97,25 +97,25 @@ typedef struct _SbgEComLogUtc /*! * Zero initialize the message struct. * - * \param[out] pLogData Structure instance to zero init. + * \param[out] pLogData Structure instance to zero init. */ void sbgEComLogUtcZeroInit(SbgEComLogUtc *pLogData); /*! * Parse data for the SBG_ECOM_LOG_UTC_DATA message and fill the corresponding structure. * - * \param[out] pLogData Log structure instance to fill. - * \param[in] pStreamBuffer Input stream buffer to read the log from. - * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. + * \param[out] pLogData Log structure instance to fill. + * \param[in] pStreamBuffer Input stream buffer to read the log from. + * \return SBG_NO_ERROR if a valid log has been read from the stream buffer. */ SbgErrorCode sbgEComLogUtcReadFromStream(SbgEComLogUtc *pLogData, SbgStreamBuffer *pStreamBuffer); /*! * Write data for the SBG_ECOM_LOG_UTC_DATA message to the output stream buffer from the provided structure. * - * \param[in] pLogData Log structure instance to write. - * \param[out] pStreamBuffer Output stream buffer to write the log to. - * \return SBG_NO_ERROR if the log has been written to the stream buffer. + * \param[in] pLogData Log structure instance to write. + * \param[out] pStreamBuffer Output stream buffer to write the log to. + * \return SBG_NO_ERROR if the log has been written to the stream buffer. */ SbgErrorCode sbgEComLogUtcWriteToStream(const SbgEComLogUtc *pLogData, SbgStreamBuffer *pStreamBuffer); @@ -126,56 +126,56 @@ SbgErrorCode sbgEComLogUtcWriteToStream(const SbgEComLogUtc *pLogData, SbgStream /*! * Set the clock alignment state. * - * \param[in] pLogData Log instance. - * \param[in] state The status to set. + * \param[in] pLogData Log instance. + * \param[in] state The status to set. */ void sbgEComLogUtcSetClockState(SbgEComLogUtc *pLogData, SbgEComClockState state); /*! * Returns the clock alignment state. * - * \param[in] pLogData Log instance. - * \return The clock status. + * \param[in] pLogData Log instance. + * \return The clock status. */ SbgEComClockState sbgEComLogUtcGetClockState(const SbgEComLogUtc *pLogData); /*! * Returns the clock alignment state as a NULL terminated C string. * - * \param[in] pLogData Log instance. - * \return The clock state as a C string. + * \param[in] pLogData Log instance. + * \return The clock state as a C string. */ const char *sbgEComLogUtcGetClockStateAsString(const SbgEComLogUtc *pLogData); /*! * Set the UTC time information status. * - * \param[in] pLogData Log instance. - * \param[in] status The status to set. + * \param[in] pLogData Log instance. + * \param[in] status The status to set. */ void sbgEComLogUtcSetUtcStatus(SbgEComLogUtc *pLogData, SbgEComUtcStatus status); /*! * Returns the GNSS UTC time information status. * - * \param[in] pLogData Log instance. - * \return The UTC time information status. + * \param[in] pLogData Log instance. + * \return The UTC time information status. */ SbgEComUtcStatus sbgEComLogUtcGetUtcStatus(const SbgEComLogUtc *pLogData); /*! * Returns the GNSS UTC time information status as a NULL terminated C string. * - * \param[in] pLogData Log instance. - * \return The UTC time information status as a C string. + * \param[in] pLogData Log instance. + * \return The UTC time information status as a C string. */ const char *sbgEComLogUtcGetUtcStatusAsString(const SbgEComLogUtc *pLogData); /*! * Set if a valid PPS signal is received and can be used to align the internal clock. * - * \param[in] pLogData Log instance. - * \param[in] hasClockInput true if a valid PPS signal is received. + * \param[in] pLogData Log instance. + * \param[in] hasClockInput true if a valid PPS signal is received. */ void sbgEComLogUtcSetHasClockInput(SbgEComLogUtc *pLogData, bool hasClockInput); @@ -188,16 +188,16 @@ void sbgEComLogUtcSetHasClockInput(SbgEComLogUtc *pLogData, bool hasClockInput); * During nominal INS operations with good GNSS reception, this method should return true. * During GNSS outages, no PPS signal is received and this method should return false. * - * \param[in] pLogData Log instance. - * \return true if a valid PPS signal is received and false if no PPS signal is received. + * \param[in] pLogData Log instance. + * \return true if a valid PPS signal is received and false if no PPS signal is received. */ bool sbgEComLogUtcHasClockInput(const SbgEComLogUtc *pLogData); /*! * Set if the reported UTC time is accurate or not (ie timestamped with a PPS). * - * \param[in] pLogData Log instance. - * \param[in] utcIsAccurate true if the UTC time is accurate or false otherwise. + * \param[in] pLogData Log instance. + * \param[in] utcIsAccurate true if the UTC time is accurate or false otherwise. */ void sbgEComLogUtcTimeSetIsAccurate(SbgEComLogUtc *pLogData, bool utcIsAccurate); @@ -212,32 +212,32 @@ void sbgEComLogUtcTimeSetIsAccurate(SbgEComLogUtc *pLogData, bool utcIsAccurate) * If the PPS signal is lost for too much time, this method should return false indicating * a degraded UTC time information accuracy. * - * \param[in] pLogData Log instance. - * \return true if the UTC time information is accurate. + * \param[in] pLogData Log instance. + * \return true if the UTC time information is accurate. */ bool sbgEComLogUtcTimeIsAccurate(const SbgEComLogUtc *pLogData); /*! * Returns true if the clkBiasStd field is filled and valid. * - * \param[in] pLogData Log instance. - * \return true if the field is valid. + * \param[in] pLogData Log instance. + * \return true if the field is valid. */ bool sbgEComLogUtcTimeClkBiasStdIsValid(const SbgEComLogUtc *pLogData); /*! * Returns true if the clkSfErrorStd field is filled and valid. * - * \param[in] pLogData Log instance. - * \return true if the field is valid. + * \param[in] pLogData Log instance. + * \return true if the field is valid. */ bool sbgEComLogUtcTimeClkSfErrorStdIsValid(const SbgEComLogUtc *pLogData); /*! * Returns true if the clkResidualError field is filled and valid. * - * \param[in] pLogData Log instance. - * \return true if the field is valid. + * \param[in] pLogData Log instance. + * \return true if the field is valid. */ bool sbgEComLogUtcTimeClkResidualErrorIsValid(const SbgEComLogUtc *pLogData); @@ -246,22 +246,22 @@ bool sbgEComLogUtcTimeClkResidualErrorIsValid(const SbgEComLogUtc *pLogData); //----------------------------------------------------------------------// #ifdef SBG_ECOM_USE_DEPRECATED_MACROS - #define SBG_ECOM_CLOCK_STABLE_INPUT (0x0001u << 0) - #define SBG_ECOM_CLOCK_UTC_SYNC (0x0001u << 5) - - #define SBG_ECOM_CLOCK_ERROR (SBG_ECOM_CLOCK_STATE_ERROR) - #define SBG_ECOM_CLOCK_FREE_RUNNING (SBG_ECOM_CLOCK_STATE_FREE_RUNNING) - #define SBG_ECOM_CLOCK_STEERING (SBG_ECOM_CLOCK_STATE_STEERING) - #define SBG_ECOM_CLOCK_VALID (SBG_ECOM_CLOCK_STATE_VALID) - - #define SBG_ECOM_UTC_INVALID (SBG_ECOM_UTC_STATUS_INVALID) - #define SBG_ECOM_UTC_NO_LEAP_SEC (SBG_ECOM_UTC_STATUS_NO_LEAP_SEC) - #define SBG_ECOM_UTC_VALID (SBG_ECOM_UTC_STATUS_INITIALIZED) + #define SBG_ECOM_CLOCK_STABLE_INPUT (0x0001u << 0) + #define SBG_ECOM_CLOCK_UTC_SYNC (0x0001u << 5) + + #define SBG_ECOM_CLOCK_ERROR (SBG_ECOM_CLOCK_STATE_ERROR) + #define SBG_ECOM_CLOCK_FREE_RUNNING (SBG_ECOM_CLOCK_STATE_FREE_RUNNING) + #define SBG_ECOM_CLOCK_STEERING (SBG_ECOM_CLOCK_STATE_STEERING) + #define SBG_ECOM_CLOCK_VALID (SBG_ECOM_CLOCK_STATE_VALID) + + #define SBG_ECOM_UTC_INVALID (SBG_ECOM_UTC_STATUS_INVALID) + #define SBG_ECOM_UTC_NO_LEAP_SEC (SBG_ECOM_UTC_STATUS_NO_LEAP_SEC) + #define SBG_ECOM_UTC_VALID (SBG_ECOM_UTC_STATUS_INITIALIZED) #endif -SBG_DEPRECATED_TYPEDEF(typedef enum _SbgEComClockState SbgEComClockStatus); -SBG_DEPRECATED_TYPEDEF(typedef enum _SbgEComUtcStatus SbgEComClockUtcStatus); -SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogUtc SbgLogUtcData); +SBG_DEPRECATED_TYPEDEF(typedef enum _SbgEComClockState SbgEComClockStatus); +SBG_DEPRECATED_TYPEDEF(typedef enum _SbgEComUtcStatus SbgEComClockUtcStatus); +SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogUtc SbgLogUtcData); SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogParseUtcData(SbgStreamBuffer *pStreamBuffer, SbgEComLogUtc *pLogData)); SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogWriteUtcData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogUtc *pLogData)); diff --git a/src/protocol/sbgEComProtocol.c b/src/protocol/sbgEComProtocol.c index 0c6ca93..f2a2471 100644 --- a/src/protocol/sbgEComProtocol.c +++ b/src/protocol/sbgEComProtocol.c @@ -14,7 +14,7 @@ /*! * Delay before another send attempt when sending a large payload, in milliseconds. */ -#define SBG_ECOM_PROTOCOL_EXT_SEND_DELAY (50) +#define SBG_ECOM_PROTOCOL_EXT_SEND_DELAY (50) //----------------------------------------------------------------------// //- Private functions -// @@ -25,83 +25,83 @@ * * Any allocated resource is released, and the payload returns to its constructed state. * - * \param[in] pPayload Payload. + * \param[in] pPayload Payload. */ static void sbgEComProtocolPayloadClear(SbgEComProtocolPayload *pPayload) { - assert(pPayload); + assert(pPayload); - if (pPayload->allocated) - { - free(pPayload->pBuffer); + if (pPayload->allocated) + { + free(pPayload->pBuffer); - pPayload->allocated = false; - } + pPayload->allocated = false; + } - pPayload->pBuffer = NULL; - pPayload->size = 0; + pPayload->pBuffer = NULL; + pPayload->size = 0; } /*! * Set the properties of a payload. * - * \param[in] pPayload Payload. - * \param[in] allocated True if the given buffer is allocated with malloc(). - * \param[in] pBuffer Buffer. - * \param[in] size Buffer size, in bytes. + * \param[in] pPayload Payload. + * \param[in] allocated True if the given buffer is allocated with malloc(). + * \param[in] pBuffer Buffer. + * \param[in] size Buffer size, in bytes. */ static void sbgEComProtocolPayloadSet(SbgEComProtocolPayload *pPayload, bool allocated, void *pBuffer, size_t size) { - assert(pPayload); - assert(pBuffer); + assert(pPayload); + assert(pBuffer); - pPayload->allocated = allocated; - pPayload->pBuffer = pBuffer; - pPayload->size = size; + pPayload->allocated = allocated; + pPayload->pBuffer = pBuffer; + pPayload->size = size; } /*! * Discard unused bytes from the work buffer of a protocol. * - * \param[in] pProtocol Protocol. + * \param[in] pProtocol Protocol. */ static void sbgEComProtocolDiscardUnusedBytes(SbgEComProtocol *pProtocol) { - assert(pProtocol); + assert(pProtocol); - if (pProtocol->discardSize != 0) - { - assert(pProtocol->discardSize <= pProtocol->rxBufferSize); + if (pProtocol->discardSize != 0) + { + assert(pProtocol->discardSize <= pProtocol->rxBufferSize); - memmove(pProtocol->rxBuffer, &pProtocol->rxBuffer[pProtocol->discardSize], pProtocol->rxBufferSize - pProtocol->discardSize); + memmove(pProtocol->rxBuffer, &pProtocol->rxBuffer[pProtocol->discardSize], pProtocol->rxBufferSize - pProtocol->discardSize); - pProtocol->rxBufferSize -= pProtocol->discardSize; - pProtocol->discardSize = 0; - } + pProtocol->rxBufferSize -= pProtocol->discardSize; + pProtocol->discardSize = 0; + } } /*! * Read data from the underlying interface into the work buffer of a protocol. * - * \param[in] pProtocol Protocol. + * \param[in] pProtocol Protocol. */ static void sbgEComProtocolRead(SbgEComProtocol *pProtocol) { - SbgErrorCode errorCode; + SbgErrorCode errorCode; - assert(pProtocol); + assert(pProtocol); - if (pProtocol->rxBufferSize < sizeof(pProtocol->rxBuffer)) - { - size_t nrBytesRead; + if (pProtocol->rxBufferSize < sizeof(pProtocol->rxBuffer)) + { + size_t nrBytesRead; - errorCode = sbgInterfaceRead(pProtocol->pLinkedInterface, &pProtocol->rxBuffer[pProtocol->rxBufferSize], &nrBytesRead, sizeof(pProtocol->rxBuffer) - pProtocol->rxBufferSize); + errorCode = sbgInterfaceRead(pProtocol->pLinkedInterface, &pProtocol->rxBuffer[pProtocol->rxBufferSize], &nrBytesRead, sizeof(pProtocol->rxBuffer) - pProtocol->rxBufferSize); - if (errorCode == SBG_NO_ERROR) - { - pProtocol->rxBufferSize += nrBytesRead; - } - } + if (errorCode == SBG_NO_ERROR) + { + pProtocol->rxBufferSize += nrBytesRead; + } + } } /*! @@ -109,44 +109,44 @@ static void sbgEComProtocolRead(SbgEComProtocol *pProtocol) * * The output offset is set if either SBG_NO_ERROR or SBG_NOT_CONTINUOUS_FRAME is returned. * - * \param[in] pProtocol Protocol. - * \param[in] startOffset Start offset, in bytes. - * \param[out] pOffset Offset, in bytes. - * \return SBG_NO_ERROR if successful, - * SBG_NOT_CONTINUOUS_FRAME if only the first SYNC byte was found, - * SBG_NOT_READY otherwise. + * \param[in] pProtocol Protocol. + * \param[in] startOffset Start offset, in bytes. + * \param[out] pOffset Offset, in bytes. + * \return SBG_NO_ERROR if successful, + * SBG_NOT_CONTINUOUS_FRAME if only the first SYNC byte was found, + * SBG_NOT_READY otherwise. */ static SbgErrorCode sbgEComProtocolFindSyncBytes(SbgEComProtocol *pProtocol, size_t startOffset, size_t *pOffset) { - SbgErrorCode errorCode; - - assert(pProtocol); - assert(pOffset); - assert(pProtocol->rxBufferSize > 0); - - errorCode = SBG_NOT_READY; - - for (size_t i = startOffset; i < (pProtocol->rxBufferSize - 1); i++) - { - if ((pProtocol->rxBuffer[i] == SBG_ECOM_SYNC_1) && (pProtocol->rxBuffer[i + 1] == SBG_ECOM_SYNC_2)) - { - *pOffset = i; - errorCode = SBG_NO_ERROR; - break; - } - } - - // - // The SYNC bytes were not found, but check if the last byte in the work buffer is the first SYNC byte, - // as it could result from receiving a partial frame. - // - if ((errorCode != SBG_NO_ERROR) && (pProtocol->rxBuffer[pProtocol->rxBufferSize - 1] == SBG_ECOM_SYNC_1)) - { - *pOffset = pProtocol->rxBufferSize - 1; - errorCode = SBG_NOT_CONTINUOUS_FRAME; - } - - return errorCode; + SbgErrorCode errorCode; + + assert(pProtocol); + assert(pOffset); + assert(pProtocol->rxBufferSize > 0); + + errorCode = SBG_NOT_READY; + + for (size_t i = startOffset; i < (pProtocol->rxBufferSize - 1); i++) + { + if ((pProtocol->rxBuffer[i] == SBG_ECOM_SYNC_1) && (pProtocol->rxBuffer[i + 1] == SBG_ECOM_SYNC_2)) + { + *pOffset = i; + errorCode = SBG_NO_ERROR; + break; + } + } + + // + // The SYNC bytes were not found, but check if the last byte in the work buffer is the first SYNC byte, + // as it could result from receiving a partial frame. + // + if ((errorCode != SBG_NO_ERROR) && (pProtocol->rxBuffer[pProtocol->rxBufferSize - 1] == SBG_ECOM_SYNC_1)) + { + *pOffset = pProtocol->rxBufferSize - 1; + errorCode = SBG_NOT_CONTINUOUS_FRAME; + } + + return errorCode; } /*! @@ -154,172 +154,172 @@ static SbgErrorCode sbgEComProtocolFindSyncBytes(SbgEComProtocol *pProtocol, siz * * A non-zero number of pages indicates the reception of an extended frame. * - * \param[in] pProtocol Protocol. - * \param[in] offset Frame offset in the protocol work buffer. - * \param[out] pEndOffset Frame end offset in the protocol work buffer. - * \param[out] pMsgClass Message class. - * \param[out] pMsgId Message ID. - * \param[out] pTransferId Transfer ID. - * \param[out] pPageIndex Page index. - * \param[out] pNrPages Number of pages. - * \param[out] pBuffer Payload buffer. - * \param[out] pSize Payload buffer size, in bytes. - * \return SBG_NO_ERROR if successful, - * SBG_NOT_READY if the frame is incomplete, - * SBG_INVALID_FRAME if the frame is invalid, - * SBG_INVALID_CRC if the frame CRC is invalid. + * \param[in] pProtocol Protocol. + * \param[in] offset Frame offset in the protocol work buffer. + * \param[out] pEndOffset Frame end offset in the protocol work buffer. + * \param[out] pMsgClass Message class. + * \param[out] pMsgId Message ID. + * \param[out] pTransferId Transfer ID. + * \param[out] pPageIndex Page index. + * \param[out] pNrPages Number of pages. + * \param[out] pBuffer Payload buffer. + * \param[out] pSize Payload buffer size, in bytes. + * \return SBG_NO_ERROR if successful, + * SBG_NOT_READY if the frame is incomplete, + * SBG_INVALID_FRAME if the frame is invalid, + * SBG_INVALID_CRC if the frame CRC is invalid. */ static SbgErrorCode sbgEComProtocolParseFrame(SbgEComProtocol *pProtocol, size_t offset, size_t *pEndOffset, uint8_t *pMsgClass, uint8_t *pMsgId, uint8_t *pTransferId, uint16_t *pPageIndex, uint16_t *pNrPages, void **pBuffer, size_t *pSize) { - SbgErrorCode errorCode; - SbgStreamBuffer streamBuffer; - uint8_t msgId; - uint8_t msgClass; - size_t standardPayloadSize; - - assert(pProtocol); - assert(offset < pProtocol->rxBufferSize); - assert(pEndOffset); - assert(pMsgClass); - assert(pMsgId); - assert(pTransferId); - assert(pPageIndex); - assert(pNrPages); - assert(pBuffer); - assert(pSize); - - sbgStreamBufferInitForRead(&streamBuffer, &pProtocol->rxBuffer[offset], pProtocol->rxBufferSize - offset); - - // - // Skip SYNC bytes. - // - sbgStreamBufferSeek(&streamBuffer, 2, SB_SEEK_CUR_INC); - - msgId = sbgStreamBufferReadUint8(&streamBuffer); - msgClass = sbgStreamBufferReadUint8(&streamBuffer); - standardPayloadSize = sbgStreamBufferReadUint16LE(&streamBuffer); - - errorCode = sbgStreamBufferGetLastError(&streamBuffer); - - if (errorCode == SBG_NO_ERROR) - { - if (standardPayloadSize <= SBG_ECOM_MAX_PAYLOAD_SIZE) - { - if (sbgStreamBufferGetSize(&streamBuffer) >= (standardPayloadSize + 9)) - { - size_t payloadSize; - uint8_t transferId; - uint16_t pageIndex; - uint16_t nrPages; - - if ((msgClass & 0x80) == 0) - { - payloadSize = standardPayloadSize; - - transferId = 0; - pageIndex = 0; - nrPages = 0; - - errorCode = SBG_NO_ERROR; - } - else - { - msgClass &= 0x7f; - - // - // In extended frames, the payload size includes the extended headers. - // - payloadSize = standardPayloadSize - 5; - - transferId = sbgStreamBufferReadUint8(&streamBuffer); - pageIndex = sbgStreamBufferReadUint16LE(&streamBuffer); - nrPages = sbgStreamBufferReadUint16LE(&streamBuffer); - - if ((transferId & 0xf0) != 0) - { - SBG_LOG_WARNING(SBG_INVALID_FRAME, "reserved bits set in extended headers"); - transferId &= 0xf; - } - - if (pageIndex < nrPages) - { - errorCode = SBG_NO_ERROR; - } - else - { - errorCode = SBG_INVALID_FRAME; - SBG_LOG_ERROR(errorCode, "invalid page information : %" PRIu16 "/%" PRIu16, pageIndex, nrPages); - } - } - - if (errorCode == SBG_NO_ERROR) - { - void *pPayloadAddr; - uint16_t frameCrc; - uint8_t lastByte; - - pPayloadAddr = sbgStreamBufferGetCursor(&streamBuffer); - - sbgStreamBufferSeek(&streamBuffer, payloadSize, SB_SEEK_CUR_INC); - - frameCrc = sbgStreamBufferReadUint16LE(&streamBuffer); - lastByte = sbgStreamBufferReadUint8(&streamBuffer); - - assert(sbgStreamBufferGetLastError(&streamBuffer) == SBG_NO_ERROR); - - if (lastByte == SBG_ECOM_ETX) - { - uint16_t computedCrc; - - // - // The CRC spans from the header (excluding the SYNC bytes) up to the CRC bytes. - // - sbgStreamBufferSeek(&streamBuffer, 2, SB_SEEK_SET); - computedCrc = sbgCrc16Compute(sbgStreamBufferGetCursor(&streamBuffer), standardPayloadSize + 4); - - if (frameCrc == computedCrc) - { - *pEndOffset = offset + standardPayloadSize + 9; - *pMsgClass = msgClass; - *pMsgId = msgId; - *pTransferId = transferId; - *pPageIndex = pageIndex; - *pNrPages = nrPages; - *pBuffer = pPayloadAddr; - *pSize = payloadSize; - - errorCode = SBG_NO_ERROR; - } - else - { - errorCode = SBG_INVALID_CRC; - SBG_LOG_ERROR(errorCode, "invalid CRC, frame:%#" PRIx16 " computed:%#" PRIx16, frameCrc, computedCrc); - } - } - else - { - errorCode = SBG_INVALID_FRAME; - SBG_LOG_ERROR(errorCode, "invalid end-of-frame: byte:%#" PRIx8, lastByte); - } - } - } - else - { - errorCode = SBG_NOT_READY; - } - } - else - { - errorCode = SBG_INVALID_FRAME; - SBG_LOG_ERROR(errorCode, "invalid payload size %zu", standardPayloadSize); - } - } - else - { - errorCode = SBG_NOT_READY; - } - - return errorCode; + SbgErrorCode errorCode; + SbgStreamBuffer streamBuffer; + uint8_t msgId; + uint8_t msgClass; + size_t standardPayloadSize; + + assert(pProtocol); + assert(offset < pProtocol->rxBufferSize); + assert(pEndOffset); + assert(pMsgClass); + assert(pMsgId); + assert(pTransferId); + assert(pPageIndex); + assert(pNrPages); + assert(pBuffer); + assert(pSize); + + sbgStreamBufferInitForRead(&streamBuffer, &pProtocol->rxBuffer[offset], pProtocol->rxBufferSize - offset); + + // + // Skip SYNC bytes. + // + sbgStreamBufferSeek(&streamBuffer, 2, SB_SEEK_CUR_INC); + + msgId = sbgStreamBufferReadUint8(&streamBuffer); + msgClass = sbgStreamBufferReadUint8(&streamBuffer); + standardPayloadSize = sbgStreamBufferReadUint16LE(&streamBuffer); + + errorCode = sbgStreamBufferGetLastError(&streamBuffer); + + if (errorCode == SBG_NO_ERROR) + { + if (standardPayloadSize <= SBG_ECOM_MAX_PAYLOAD_SIZE) + { + if (sbgStreamBufferGetSize(&streamBuffer) >= (standardPayloadSize + 9)) + { + size_t payloadSize; + uint8_t transferId; + uint16_t pageIndex; + uint16_t nrPages; + + if ((msgClass & 0x80) == 0) + { + payloadSize = standardPayloadSize; + + transferId = 0; + pageIndex = 0; + nrPages = 0; + + errorCode = SBG_NO_ERROR; + } + else + { + msgClass &= 0x7f; + + // + // In extended frames, the payload size includes the extended headers. + // + payloadSize = standardPayloadSize - 5; + + transferId = sbgStreamBufferReadUint8(&streamBuffer); + pageIndex = sbgStreamBufferReadUint16LE(&streamBuffer); + nrPages = sbgStreamBufferReadUint16LE(&streamBuffer); + + if ((transferId & 0xf0) != 0) + { + SBG_LOG_WARNING(SBG_INVALID_FRAME, "reserved bits set in extended headers"); + transferId &= 0xf; + } + + if (pageIndex < nrPages) + { + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_INVALID_FRAME; + SBG_LOG_ERROR(errorCode, "invalid page information : %" PRIu16 "/%" PRIu16, pageIndex, nrPages); + } + } + + if (errorCode == SBG_NO_ERROR) + { + void *pPayloadAddr; + uint16_t frameCrc; + uint8_t lastByte; + + pPayloadAddr = sbgStreamBufferGetCursor(&streamBuffer); + + sbgStreamBufferSeek(&streamBuffer, payloadSize, SB_SEEK_CUR_INC); + + frameCrc = sbgStreamBufferReadUint16LE(&streamBuffer); + lastByte = sbgStreamBufferReadUint8(&streamBuffer); + + assert(sbgStreamBufferGetLastError(&streamBuffer) == SBG_NO_ERROR); + + if (lastByte == SBG_ECOM_ETX) + { + uint16_t computedCrc; + + // + // The CRC spans from the header (excluding the SYNC bytes) up to the CRC bytes. + // + sbgStreamBufferSeek(&streamBuffer, 2, SB_SEEK_SET); + computedCrc = sbgCrc16Compute(sbgStreamBufferGetCursor(&streamBuffer), standardPayloadSize + 4); + + if (frameCrc == computedCrc) + { + *pEndOffset = offset + standardPayloadSize + 9; + *pMsgClass = msgClass; + *pMsgId = msgId; + *pTransferId = transferId; + *pPageIndex = pageIndex; + *pNrPages = nrPages; + *pBuffer = pPayloadAddr; + *pSize = payloadSize; + + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_INVALID_CRC; + SBG_LOG_ERROR(errorCode, "invalid CRC, frame:%#" PRIx16 " computed:%#" PRIx16, frameCrc, computedCrc); + } + } + else + { + errorCode = SBG_INVALID_FRAME; + SBG_LOG_ERROR(errorCode, "invalid end-of-frame: byte:%#" PRIx8, lastByte); + } + } + } + else + { + errorCode = SBG_NOT_READY; + } + } + else + { + errorCode = SBG_INVALID_FRAME; + SBG_LOG_ERROR(errorCode, "invalid payload size %zu", standardPayloadSize); + } + } + else + { + errorCode = SBG_NOT_READY; + } + + return errorCode; } /*! @@ -327,230 +327,230 @@ static SbgErrorCode sbgEComProtocolParseFrame(SbgEComProtocol *pProtocol, size_t * * If an extended frame is received, the number of pages is set to a non-zero value. * - * \param[in] pProtocol Protocol. - * \param[out] pMsgClass Message class. - * \param[out] pMsgId Message ID. - * \param[out] pTransferId Transfer ID. - * \param[out] pPageIndex Page index. - * \param[out] pNrPages Number of pages. - * \param[out] pBuffer Payload buffer. - * \param[out] pSize Payload buffer size, in bytes. - * \return SBG_NO_ERROR if successful, - * SBG_NOT_READY if no frame was found. + * \param[in] pProtocol Protocol. + * \param[out] pMsgClass Message class. + * \param[out] pMsgId Message ID. + * \param[out] pTransferId Transfer ID. + * \param[out] pPageIndex Page index. + * \param[out] pNrPages Number of pages. + * \param[out] pBuffer Payload buffer. + * \param[out] pSize Payload buffer size, in bytes. + * \return SBG_NO_ERROR if successful, + * SBG_NOT_READY if no frame was found. */ static SbgErrorCode sbgEComProtocolFindFrame(SbgEComProtocol *pProtocol, uint8_t *pMsgClass, uint8_t *pMsgId, uint8_t *pTransferId, uint16_t *pPageIndex, uint16_t *pNrPages, void **pBuffer, size_t *pSize) { - SbgErrorCode errorCode; - size_t startOffset; - - assert(pProtocol); - - errorCode = SBG_NOT_READY; - startOffset = 0; - - while (startOffset < pProtocol->rxBufferSize) - { - size_t offset; - - errorCode = sbgEComProtocolFindSyncBytes(pProtocol, startOffset, &offset); - - if (errorCode == SBG_NO_ERROR) - { - size_t endOffset; - - errorCode = sbgEComProtocolParseFrame(pProtocol, offset, &endOffset, pMsgClass, pMsgId, pTransferId, pPageIndex, pNrPages, pBuffer, pSize); - - if (errorCode == SBG_NO_ERROR) - { - // - // Valid frame found, discard all data up to and including that frame - // on the next read. - // - pProtocol->discardSize = endOffset; - - // - // If installed, call the method used to intercept received sbgECom frames - // - if (pProtocol->pReceiveFrameCb) - { - SbgStreamBuffer fullFrameStream; - - sbgStreamBufferInitForRead(&fullFrameStream, &pProtocol->rxBuffer[offset], endOffset-offset); - pProtocol->pReceiveFrameCb(pProtocol, *pMsgClass, *pMsgId, &fullFrameStream, pProtocol->pUserArg); - } - - break; - } - else if (errorCode == SBG_NOT_READY) - { - // - // There may be a valid frame at the parse offset, but it's not complete. - // Have all preceding bytes discarded on the next read. - // - pProtocol->discardSize = offset; - break; - } - else - { - // - // Not a valid frame, skip SYNC bytes and try again. - // - startOffset = offset + 2; - errorCode = SBG_NOT_READY; - } - } - else if (errorCode == SBG_NOT_CONTINUOUS_FRAME) - { - // - // The first SYNC byte was found, but not the second. It may be a valid - // frame, so keep the SYNC byte but have all preceding bytes discarded - // on the next read. - // - pProtocol->discardSize = offset; - errorCode = SBG_NOT_READY; - break; - } - else - { - // - // No SYNC byte found, discard all data. - // - pProtocol->rxBufferSize = 0; - errorCode = SBG_NOT_READY; - break; - } - } - - assert(pProtocol->discardSize <= pProtocol->rxBufferSize); - - return errorCode; + SbgErrorCode errorCode; + size_t startOffset; + + assert(pProtocol); + + errorCode = SBG_NOT_READY; + startOffset = 0; + + while (startOffset < pProtocol->rxBufferSize) + { + size_t offset; + + errorCode = sbgEComProtocolFindSyncBytes(pProtocol, startOffset, &offset); + + if (errorCode == SBG_NO_ERROR) + { + size_t endOffset; + + errorCode = sbgEComProtocolParseFrame(pProtocol, offset, &endOffset, pMsgClass, pMsgId, pTransferId, pPageIndex, pNrPages, pBuffer, pSize); + + if (errorCode == SBG_NO_ERROR) + { + // + // Valid frame found, discard all data up to and including that frame + // on the next read. + // + pProtocol->discardSize = endOffset; + + // + // If installed, call the method used to intercept received sbgECom frames + // + if (pProtocol->pReceiveFrameCb) + { + SbgStreamBuffer fullFrameStream; + + sbgStreamBufferInitForRead(&fullFrameStream, &pProtocol->rxBuffer[offset], endOffset-offset); + pProtocol->pReceiveFrameCb(pProtocol, *pMsgClass, *pMsgId, &fullFrameStream, pProtocol->pUserArg); + } + + break; + } + else if (errorCode == SBG_NOT_READY) + { + // + // There may be a valid frame at the parse offset, but it's not complete. + // Have all preceding bytes discarded on the next read. + // + pProtocol->discardSize = offset; + break; + } + else + { + // + // Not a valid frame, skip SYNC bytes and try again. + // + startOffset = offset + 2; + errorCode = SBG_NOT_READY; + } + } + else if (errorCode == SBG_NOT_CONTINUOUS_FRAME) + { + // + // The first SYNC byte was found, but not the second. It may be a valid + // frame, so keep the SYNC byte but have all preceding bytes discarded + // on the next read. + // + pProtocol->discardSize = offset; + errorCode = SBG_NOT_READY; + break; + } + else + { + // + // No SYNC byte found, discard all data. + // + pProtocol->rxBufferSize = 0; + errorCode = SBG_NOT_READY; + break; + } + } + + assert(pProtocol->discardSize <= pProtocol->rxBufferSize); + + return errorCode; } /*! * Send a standard frame. * - * \param[in] pProtocol Protocol. - * \param[in] msgClass Message class. - * \param[in] msgId Message ID. - * \param[in] pData Data buffer. - * \param[in] size Data buffer size, in bytes. - * \return SBG_NO_ERROR if successful. + * \param[in] pProtocol Protocol. + * \param[in] msgClass Message class. + * \param[in] msgId Message ID. + * \param[in] pData Data buffer. + * \param[in] size Data buffer size, in bytes. + * \return SBG_NO_ERROR if successful. */ static SbgErrorCode sbgEComProtocolSendStandardFrame(SbgEComProtocol *pProtocol, uint8_t msgClass, uint8_t msgId, const void *pData, size_t size) { - uint8_t buffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer streamBuffer; - const uint8_t *crcDataStart; - const uint8_t *crcDataEnd; - uint16_t crc; + uint8_t buffer[SBG_ECOM_MAX_BUFFER_SIZE]; + SbgStreamBuffer streamBuffer; + const uint8_t *crcDataStart; + const uint8_t *crcDataEnd; + uint16_t crc; - assert(pProtocol); - assert((msgClass & 0x80) == 0); - assert(size <= SBG_ECOM_MAX_PAYLOAD_SIZE); - assert(pData || (size == 0)); + assert(pProtocol); + assert((msgClass & 0x80) == 0); + assert(size <= SBG_ECOM_MAX_PAYLOAD_SIZE); + assert(pData || (size == 0)); - sbgStreamBufferInitForWrite(&streamBuffer, buffer, sizeof(buffer)); + sbgStreamBufferInitForWrite(&streamBuffer, buffer, sizeof(buffer)); - sbgStreamBufferWriteUint8(&streamBuffer, SBG_ECOM_SYNC_1); - sbgStreamBufferWriteUint8(&streamBuffer, SBG_ECOM_SYNC_2); + sbgStreamBufferWriteUint8(&streamBuffer, SBG_ECOM_SYNC_1); + sbgStreamBufferWriteUint8(&streamBuffer, SBG_ECOM_SYNC_2); - crcDataStart = sbgStreamBufferGetCursor(&streamBuffer); + crcDataStart = sbgStreamBufferGetCursor(&streamBuffer); - sbgStreamBufferWriteUint8(&streamBuffer, msgId); - sbgStreamBufferWriteUint8(&streamBuffer, msgClass); + sbgStreamBufferWriteUint8(&streamBuffer, msgId); + sbgStreamBufferWriteUint8(&streamBuffer, msgClass); - sbgStreamBufferWriteUint16LE(&streamBuffer, (uint16_t)size); + sbgStreamBufferWriteUint16LE(&streamBuffer, (uint16_t)size); - sbgStreamBufferWriteBuffer(&streamBuffer, pData, size); + sbgStreamBufferWriteBuffer(&streamBuffer, pData, size); - crcDataEnd = sbgStreamBufferGetCursor(&streamBuffer); + crcDataEnd = sbgStreamBufferGetCursor(&streamBuffer); - crc = sbgCrc16Compute(crcDataStart, crcDataEnd - crcDataStart); + crc = sbgCrc16Compute(crcDataStart, crcDataEnd - crcDataStart); - sbgStreamBufferWriteUint16LE(&streamBuffer, crc); + sbgStreamBufferWriteUint16LE(&streamBuffer, crc); - sbgStreamBufferWriteUint8(&streamBuffer, SBG_ECOM_ETX); + sbgStreamBufferWriteUint8(&streamBuffer, SBG_ECOM_ETX); - assert(sbgStreamBufferGetLastError(&streamBuffer) == SBG_NO_ERROR); + assert(sbgStreamBufferGetLastError(&streamBuffer) == SBG_NO_ERROR); - return sbgInterfaceWrite(pProtocol->pLinkedInterface, sbgStreamBufferGetLinkedBuffer(&streamBuffer), sbgStreamBufferGetLength(&streamBuffer)); + return sbgInterfaceWrite(pProtocol->pLinkedInterface, sbgStreamBufferGetLinkedBuffer(&streamBuffer), sbgStreamBufferGetLength(&streamBuffer)); } /*! * Send an extended frame. * - * \param[in] pProtocol Protocol. - * \param[in] msgClass Message class. - * \param[in] msgId Message ID. - * \param[in] transferId Transfer ID. - * \param[in] pageIndex Page index (0 to 65534) - * \param[in] nrPages Total number of pages (1 to 65535) - * \param[in] pData Data buffer. - * \param[in] size Data buffer size, in bytes. - * \return SBG_NO_ERROR if successful. + * \param[in] pProtocol Protocol. + * \param[in] msgClass Message class. + * \param[in] msgId Message ID. + * \param[in] transferId Transfer ID. + * \param[in] pageIndex Page index (0 to 65534) + * \param[in] nrPages Total number of pages (1 to 65535) + * \param[in] pData Data buffer. + * \param[in] size Data buffer size, in bytes. + * \return SBG_NO_ERROR if successful. */ static SbgErrorCode sbgEComProtocolSendExtendedFrame(SbgEComProtocol *pProtocol, uint8_t msgClass, uint8_t msgId, uint8_t transferId, size_t pageIndex, size_t nrPages, const void *pData, size_t size) { - SbgErrorCode errorCode; - uint8_t buffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer streamBuffer; - const uint8_t *crcDataStart; - const uint8_t *crcDataEnd; - uint16_t crc; + SbgErrorCode errorCode; + uint8_t buffer[SBG_ECOM_MAX_BUFFER_SIZE]; + SbgStreamBuffer streamBuffer; + const uint8_t *crcDataStart; + const uint8_t *crcDataEnd; + uint16_t crc; - assert(pProtocol); - assert((msgClass & 0x80) == 0); - assert((transferId & 0xf0) == 0); - assert(pageIndex < UINT16_MAX); - assert((nrPages > 0) && (nrPages <= UINT16_MAX)); - assert(pageIndex < nrPages); - assert(size <= SBG_ECOM_MAX_EXTENDED_PAYLOAD_SIZE); - assert(pData || (size == 0)); + assert(pProtocol); + assert((msgClass & 0x80) == 0); + assert((transferId & 0xf0) == 0); + assert(pageIndex < UINT16_MAX); + assert((nrPages > 0) && (nrPages <= UINT16_MAX)); + assert(pageIndex < nrPages); + assert(size <= SBG_ECOM_MAX_EXTENDED_PAYLOAD_SIZE); + assert(pData || (size == 0)); - sbgStreamBufferInitForWrite(&streamBuffer, buffer, sizeof(buffer)); + sbgStreamBufferInitForWrite(&streamBuffer, buffer, sizeof(buffer)); - sbgStreamBufferWriteUint8(&streamBuffer, SBG_ECOM_SYNC_1); - sbgStreamBufferWriteUint8(&streamBuffer, SBG_ECOM_SYNC_2); + sbgStreamBufferWriteUint8(&streamBuffer, SBG_ECOM_SYNC_1); + sbgStreamBufferWriteUint8(&streamBuffer, SBG_ECOM_SYNC_2); - crcDataStart = sbgStreamBufferGetCursor(&streamBuffer); + crcDataStart = sbgStreamBufferGetCursor(&streamBuffer); - sbgStreamBufferWriteUint8(&streamBuffer, msgId); - sbgStreamBufferWriteUint8(&streamBuffer, 0x80 | msgClass); + sbgStreamBufferWriteUint8(&streamBuffer, msgId); + sbgStreamBufferWriteUint8(&streamBuffer, 0x80 | msgClass); - // - // For compatibility reasons, the size must span over the extended headers. - // - sbgStreamBufferWriteUint16LE(&streamBuffer, (uint16_t)size + 5); + // + // For compatibility reasons, the size must span over the extended headers. + // + sbgStreamBufferWriteUint16LE(&streamBuffer, (uint16_t)size + 5); - sbgStreamBufferWriteUint8(&streamBuffer, transferId); - sbgStreamBufferWriteUint16LE(&streamBuffer, (uint16_t)pageIndex); - sbgStreamBufferWriteUint16LE(&streamBuffer, (uint16_t)nrPages); + sbgStreamBufferWriteUint8(&streamBuffer, transferId); + sbgStreamBufferWriteUint16LE(&streamBuffer, (uint16_t)pageIndex); + sbgStreamBufferWriteUint16LE(&streamBuffer, (uint16_t)nrPages); - sbgStreamBufferWriteBuffer(&streamBuffer, pData, size); + sbgStreamBufferWriteBuffer(&streamBuffer, pData, size); - crcDataEnd = sbgStreamBufferGetCursor(&streamBuffer); + crcDataEnd = sbgStreamBufferGetCursor(&streamBuffer); - crc = sbgCrc16Compute(crcDataStart, crcDataEnd - crcDataStart); + crc = sbgCrc16Compute(crcDataStart, crcDataEnd - crcDataStart); - sbgStreamBufferWriteUint16LE(&streamBuffer, crc); + sbgStreamBufferWriteUint16LE(&streamBuffer, crc); - sbgStreamBufferWriteUint8(&streamBuffer, SBG_ECOM_ETX); + sbgStreamBufferWriteUint8(&streamBuffer, SBG_ECOM_ETX); - assert(sbgStreamBufferGetLastError(&streamBuffer) == SBG_NO_ERROR); + assert(sbgStreamBufferGetLastError(&streamBuffer) == SBG_NO_ERROR); - for (;;) - { - errorCode = sbgInterfaceWrite(pProtocol->pLinkedInterface, sbgStreamBufferGetLinkedBuffer(&streamBuffer), sbgStreamBufferGetLength(&streamBuffer)); + for (;;) + { + errorCode = sbgInterfaceWrite(pProtocol->pLinkedInterface, sbgStreamBufferGetLinkedBuffer(&streamBuffer), sbgStreamBufferGetLength(&streamBuffer)); - if (errorCode != SBG_BUFFER_OVERFLOW) - { - break; - } + if (errorCode != SBG_BUFFER_OVERFLOW) + { + break; + } - sbgSleep(SBG_ECOM_PROTOCOL_EXT_SEND_DELAY); - } + sbgSleep(SBG_ECOM_PROTOCOL_EXT_SEND_DELAY); + } - return errorCode; + return errorCode; } /*! @@ -558,225 +558,225 @@ static SbgErrorCode sbgEComProtocolSendExtendedFrame(SbgEComProtocol *pProtocol, * * This function returns a different ID every time it's called. * - * \param[in] pProtocol Protocol. - * \return Transfer ID of the next large send. + * \param[in] pProtocol Protocol. + * \return Transfer ID of the next large send. */ static SbgErrorCode sbgEComProtocolGetTxId(SbgEComProtocol *pProtocol) { - uint8_t transferId; + uint8_t transferId; - assert(pProtocol); - assert((pProtocol->nextLargeTxId & 0xf0) == 0); + assert(pProtocol); + assert((pProtocol->nextLargeTxId & 0xf0) == 0); - transferId = pProtocol->nextLargeTxId; - pProtocol->nextLargeTxId = (pProtocol->nextLargeTxId + 1) & 0xf0; + transferId = pProtocol->nextLargeTxId; + pProtocol->nextLargeTxId = (pProtocol->nextLargeTxId + 1) & 0xf0; - return transferId; + return transferId; } /*! * Check if a large transfer is in progress. * - * \param[in] pProtocol Protocol. - * \return True if a large transfer is in progress. + * \param[in] pProtocol Protocol. + * \return True if a large transfer is in progress. */ static bool sbgEComProtocolLargeTransferInProgress(const SbgEComProtocol *pProtocol) { - assert(pProtocol); + assert(pProtocol); - return pProtocol->pLargeBuffer; + return pProtocol->pLargeBuffer; } /*! * Reset The large transfer member variables of a protocol. * - * \param[in] pProtocol Protocol. + * \param[in] pProtocol Protocol. */ static void sbgEComProtocolResetLargeTransfer(SbgEComProtocol *pProtocol) { - assert(pProtocol); + assert(pProtocol); - pProtocol->pLargeBuffer = NULL; - pProtocol->largeBufferSize = 0; - pProtocol->transferId = 0; - pProtocol->pageIndex = 0; - pProtocol->nrPages = 0; + pProtocol->pLargeBuffer = NULL; + pProtocol->largeBufferSize = 0; + pProtocol->transferId = 0; + pProtocol->pageIndex = 0; + pProtocol->nrPages = 0; } /*! * Clear any large transfer in progress. * - * \param[in] pProtocol Protocol. + * \param[in] pProtocol Protocol. */ static void sbgEComProtocolClearLargeTransfer(SbgEComProtocol *pProtocol) { - assert(pProtocol); + assert(pProtocol); - free(pProtocol->pLargeBuffer); + free(pProtocol->pLargeBuffer); - sbgEComProtocolResetLargeTransfer(pProtocol); + sbgEComProtocolResetLargeTransfer(pProtocol); } /*! * Process an extended frame. * - * \param[in] pProtocol Protocol. - * \param[in] msgClass Message class. - * \param[in] msgId Message ID. - * \param[in] transferId Transfer ID. - * \param[in] pageIndex Page index. - * \param[in] nrPages Number of pages. - * \param[in] pBuffer Buffer. - * \param[in] size Buffer size, in bytes. - * \return SBG_NO_ERROR if a large transfer is complete, - * SBG_NOT_READY otherwise. + * \param[in] pProtocol Protocol. + * \param[in] msgClass Message class. + * \param[in] msgId Message ID. + * \param[in] transferId Transfer ID. + * \param[in] pageIndex Page index. + * \param[in] nrPages Number of pages. + * \param[in] pBuffer Buffer. + * \param[in] size Buffer size, in bytes. + * \return SBG_NO_ERROR if a large transfer is complete, + * SBG_NOT_READY otherwise. */ static SbgErrorCode sbgEComProtocolProcessExtendedFrame(SbgEComProtocol *pProtocol, uint8_t msgClass, uint8_t msgId, uint8_t transferId, uint16_t pageIndex, uint16_t nrPages, const void *pBuffer, size_t size) { - SbgErrorCode errorCode; - - assert(pProtocol); - assert((transferId & 0xf0) == 0); - assert(pageIndex < nrPages); - assert(pBuffer || (size == 0)); - assert(size <= SBG_ECOM_MAX_EXTENDED_PAYLOAD_SIZE); - - if (pageIndex == 0) - { - size_t capacity; - - if (sbgEComProtocolLargeTransferInProgress(pProtocol)) - { - SBG_LOG_ERROR(SBG_ERROR, "large transfer started while a large transfer is in progress"); - SBG_LOG_ERROR(SBG_ERROR, "terminating large transfer"); - - sbgEComProtocolClearLargeTransfer(pProtocol); - } - - capacity = nrPages * SBG_ECOM_MAX_EXTENDED_PAYLOAD_SIZE; - - pProtocol->pLargeBuffer = malloc(capacity); - - if (pProtocol->pLargeBuffer) - { - pProtocol->largeBufferSize = 0; - pProtocol->msgClass = msgClass; - pProtocol->msgId = msgId; - pProtocol->transferId = transferId; - pProtocol->pageIndex = 0; - pProtocol->nrPages = nrPages; - - errorCode = SBG_NO_ERROR; - } - else - { - SBG_LOG_ERROR(SBG_MALLOC_FAILED, "unable to allocate buffer"); - - sbgEComProtocolResetLargeTransfer(pProtocol); - - errorCode = SBG_NOT_READY; - } - } - else - { - if (sbgEComProtocolLargeTransferInProgress(pProtocol)) - { - errorCode = SBG_NO_ERROR; - } - else - { - SBG_LOG_ERROR(SBG_ERROR, "extended frame received while no large transfer in progress"); - - errorCode = SBG_NOT_READY; - } - } - - if (errorCode == SBG_NO_ERROR) - { - if (msgClass == pProtocol->msgClass) - { - errorCode = SBG_NO_ERROR; - } - else - { - SBG_LOG_ERROR(SBG_ERROR, "message class mismatch in extended frame"); - SBG_LOG_ERROR(SBG_ERROR, "terminating large transfer"); - - sbgEComProtocolClearLargeTransfer(pProtocol); - - errorCode = SBG_NOT_READY; - } - - if (msgId == pProtocol->msgId) - { - errorCode = SBG_NO_ERROR; - } - else - { - SBG_LOG_ERROR(SBG_ERROR, "message ID mismatch in extended frame"); - SBG_LOG_ERROR(SBG_ERROR, "terminating large transfer"); - - sbgEComProtocolClearLargeTransfer(pProtocol); - - errorCode = SBG_NOT_READY; - } - - if (transferId == pProtocol->transferId) - { - errorCode = SBG_NO_ERROR; - } - else - { - SBG_LOG_ERROR(SBG_ERROR, "transfer ID mismatch in extended frame"); - SBG_LOG_ERROR(SBG_ERROR, "terminating large transfer"); - - sbgEComProtocolClearLargeTransfer(pProtocol); - - errorCode = SBG_NOT_READY; - } - - if (errorCode == SBG_NO_ERROR) - { - if (nrPages == pProtocol->nrPages) - { - if (pageIndex == pProtocol->pageIndex) - { - size_t offset; - - offset = pageIndex * SBG_ECOM_MAX_EXTENDED_PAYLOAD_SIZE; - memcpy(&pProtocol->pLargeBuffer[offset], pBuffer, size); - - pProtocol->largeBufferSize += size; - pProtocol->pageIndex++; - - if (pProtocol->pageIndex != pProtocol->nrPages) - { - errorCode = SBG_NOT_READY; - } - } - else - { - SBG_LOG_ERROR(SBG_ERROR, "extended frame received out of sequence"); - SBG_LOG_ERROR(SBG_ERROR, "terminating large transfer"); - - sbgEComProtocolClearLargeTransfer(pProtocol); - - errorCode = SBG_NOT_READY; - } - } - else - { - SBG_LOG_ERROR(SBG_ERROR, "page count mismatch in extended frame"); - SBG_LOG_ERROR(SBG_ERROR, "terminating large transfer"); - - sbgEComProtocolClearLargeTransfer(pProtocol); - - errorCode = SBG_NOT_READY; - } - } - } - - return errorCode; + SbgErrorCode errorCode; + + assert(pProtocol); + assert((transferId & 0xf0) == 0); + assert(pageIndex < nrPages); + assert(pBuffer || (size == 0)); + assert(size <= SBG_ECOM_MAX_EXTENDED_PAYLOAD_SIZE); + + if (pageIndex == 0) + { + size_t capacity; + + if (sbgEComProtocolLargeTransferInProgress(pProtocol)) + { + SBG_LOG_ERROR(SBG_ERROR, "large transfer started while a large transfer is in progress"); + SBG_LOG_ERROR(SBG_ERROR, "terminating large transfer"); + + sbgEComProtocolClearLargeTransfer(pProtocol); + } + + capacity = nrPages * SBG_ECOM_MAX_EXTENDED_PAYLOAD_SIZE; + + pProtocol->pLargeBuffer = malloc(capacity); + + if (pProtocol->pLargeBuffer) + { + pProtocol->largeBufferSize = 0; + pProtocol->msgClass = msgClass; + pProtocol->msgId = msgId; + pProtocol->transferId = transferId; + pProtocol->pageIndex = 0; + pProtocol->nrPages = nrPages; + + errorCode = SBG_NO_ERROR; + } + else + { + SBG_LOG_ERROR(SBG_MALLOC_FAILED, "unable to allocate buffer"); + + sbgEComProtocolResetLargeTransfer(pProtocol); + + errorCode = SBG_NOT_READY; + } + } + else + { + if (sbgEComProtocolLargeTransferInProgress(pProtocol)) + { + errorCode = SBG_NO_ERROR; + } + else + { + SBG_LOG_ERROR(SBG_ERROR, "extended frame received while no large transfer in progress"); + + errorCode = SBG_NOT_READY; + } + } + + if (errorCode == SBG_NO_ERROR) + { + if (msgClass == pProtocol->msgClass) + { + errorCode = SBG_NO_ERROR; + } + else + { + SBG_LOG_ERROR(SBG_ERROR, "message class mismatch in extended frame"); + SBG_LOG_ERROR(SBG_ERROR, "terminating large transfer"); + + sbgEComProtocolClearLargeTransfer(pProtocol); + + errorCode = SBG_NOT_READY; + } + + if (msgId == pProtocol->msgId) + { + errorCode = SBG_NO_ERROR; + } + else + { + SBG_LOG_ERROR(SBG_ERROR, "message ID mismatch in extended frame"); + SBG_LOG_ERROR(SBG_ERROR, "terminating large transfer"); + + sbgEComProtocolClearLargeTransfer(pProtocol); + + errorCode = SBG_NOT_READY; + } + + if (transferId == pProtocol->transferId) + { + errorCode = SBG_NO_ERROR; + } + else + { + SBG_LOG_ERROR(SBG_ERROR, "transfer ID mismatch in extended frame"); + SBG_LOG_ERROR(SBG_ERROR, "terminating large transfer"); + + sbgEComProtocolClearLargeTransfer(pProtocol); + + errorCode = SBG_NOT_READY; + } + + if (errorCode == SBG_NO_ERROR) + { + if (nrPages == pProtocol->nrPages) + { + if (pageIndex == pProtocol->pageIndex) + { + size_t offset; + + offset = pageIndex * SBG_ECOM_MAX_EXTENDED_PAYLOAD_SIZE; + memcpy(&pProtocol->pLargeBuffer[offset], pBuffer, size); + + pProtocol->largeBufferSize += size; + pProtocol->pageIndex++; + + if (pProtocol->pageIndex != pProtocol->nrPages) + { + errorCode = SBG_NOT_READY; + } + } + else + { + SBG_LOG_ERROR(SBG_ERROR, "extended frame received out of sequence"); + SBG_LOG_ERROR(SBG_ERROR, "terminating large transfer"); + + sbgEComProtocolClearLargeTransfer(pProtocol); + + errorCode = SBG_NOT_READY; + } + } + else + { + SBG_LOG_ERROR(SBG_ERROR, "page count mismatch in extended frame"); + SBG_LOG_ERROR(SBG_ERROR, "terminating large transfer"); + + sbgEComProtocolClearLargeTransfer(pProtocol); + + errorCode = SBG_NOT_READY; + } + } + } + + return errorCode; } //----------------------------------------------------------------------// @@ -785,73 +785,73 @@ static SbgErrorCode sbgEComProtocolProcessExtendedFrame(SbgEComProtocol *pProtoc void sbgEComProtocolPayloadConstruct(SbgEComProtocolPayload *pPayload) { - assert(pPayload); + assert(pPayload); - pPayload->allocated = false; - pPayload->pBuffer = NULL; - pPayload->size = 0; + pPayload->allocated = false; + pPayload->pBuffer = NULL; + pPayload->size = 0; } void sbgEComProtocolPayloadDestroy(SbgEComProtocolPayload *pPayload) { - assert(pPayload); + assert(pPayload); - if (pPayload->allocated) - { - free(pPayload->pBuffer); - } + if (pPayload->allocated) + { + free(pPayload->pBuffer); + } } const void *sbgEComProtocolPayloadGetBuffer(const SbgEComProtocolPayload *pPayload) { - assert(pPayload); + assert(pPayload); - return pPayload->pBuffer; + return pPayload->pBuffer; } size_t sbgEComProtocolPayloadGetSize(const SbgEComProtocolPayload *pPayload) { - assert(pPayload); + assert(pPayload); - return pPayload->size; + return pPayload->size; } void *sbgEComProtocolPayloadMoveBuffer(SbgEComProtocolPayload *pPayload) { - void *pBuffer; - - assert(pPayload); - - if (pPayload->pBuffer) - { - if (pPayload->allocated) - { - pBuffer = pPayload->pBuffer; - - sbgEComProtocolPayloadConstruct(pPayload); - } - else - { - pBuffer = malloc(pPayload->size); - - if (pBuffer) - { - memcpy(pBuffer, pPayload->pBuffer, pPayload->size); - - sbgEComProtocolPayloadConstruct(pPayload); - } - else - { - SBG_LOG_ERROR(SBG_MALLOC_FAILED, "unable to allocate buffer"); - } - } - } - else - { - pBuffer = NULL; - } - - return pBuffer; + void *pBuffer; + + assert(pPayload); + + if (pPayload->pBuffer) + { + if (pPayload->allocated) + { + pBuffer = pPayload->pBuffer; + + sbgEComProtocolPayloadConstruct(pPayload); + } + else + { + pBuffer = malloc(pPayload->size); + + if (pBuffer) + { + memcpy(pBuffer, pPayload->pBuffer, pPayload->size); + + sbgEComProtocolPayloadConstruct(pPayload); + } + else + { + SBG_LOG_ERROR(SBG_MALLOC_FAILED, "unable to allocate buffer"); + } + } + } + else + { + pBuffer = NULL; + } + + return pBuffer; } //----------------------------------------------------------------------// @@ -860,363 +860,363 @@ void *sbgEComProtocolPayloadMoveBuffer(SbgEComProtocolPayload *pPayload) SbgErrorCode sbgEComProtocolInit(SbgEComProtocol *pProtocol, SbgInterface *pInterface) { - assert(pProtocol); - assert(pInterface); + assert(pProtocol); + assert(pInterface); - memset(pProtocol, 0x00, sizeof(*pProtocol)); + memset(pProtocol, 0x00, sizeof(*pProtocol)); - pProtocol->pLinkedInterface = pInterface; + pProtocol->pLinkedInterface = pInterface; - sbgEComProtocolResetLargeTransfer(pProtocol); + sbgEComProtocolResetLargeTransfer(pProtocol); - return SBG_NO_ERROR; + return SBG_NO_ERROR; } SbgErrorCode sbgEComProtocolClose(SbgEComProtocol *pProtocol) { - assert(pProtocol); + assert(pProtocol); - pProtocol->pLinkedInterface = NULL; - pProtocol->rxBufferSize = 0; - pProtocol->discardSize = 0; - pProtocol->nextLargeTxId = 0; + pProtocol->pLinkedInterface = NULL; + pProtocol->rxBufferSize = 0; + pProtocol->discardSize = 0; + pProtocol->nextLargeTxId = 0; - sbgEComProtocolClearLargeTransfer(pProtocol); + sbgEComProtocolClearLargeTransfer(pProtocol); - return SBG_NO_ERROR; + return SBG_NO_ERROR; } SbgErrorCode sbgEComProtocolPurgeIncoming(SbgEComProtocol *pProtocol) { - SbgErrorCode errorCode = SBG_NO_ERROR; - size_t numBytesRead; - uint32_t timeStamp; - - // - // Reset the work buffer - // - pProtocol->rxBufferSize = 0; - pProtocol->discardSize = 0; - pProtocol->nextLargeTxId = 0; - - sbgEComProtocolClearLargeTransfer(pProtocol); - - // - // Try to read all incoming data for at least 100 ms and trash them - /// - timeStamp = sbgGetTime(); - - do - { - errorCode = sbgInterfaceRead(pProtocol->pLinkedInterface, pProtocol->rxBuffer, &numBytesRead, sizeof(pProtocol->rxBuffer)); - - if (errorCode != SBG_NO_ERROR) - { - SBG_LOG_ERROR(errorCode, "Unable to read data from interface"); - break; - } - } while ((sbgGetTime() - timeStamp) < 100); - - // - // If we still have read some bytes it means we were not able to purge successfully the rx buffer - // - if ( (errorCode == SBG_NO_ERROR) && (numBytesRead > 0) ) - { - errorCode = SBG_ERROR; - SBG_LOG_ERROR(errorCode, "Unable to purge the rx buffer, %zu bytes remaining", numBytesRead); - } - - return errorCode; + SbgErrorCode errorCode = SBG_NO_ERROR; + size_t numBytesRead; + uint32_t timeStamp; + + // + // Reset the work buffer + // + pProtocol->rxBufferSize = 0; + pProtocol->discardSize = 0; + pProtocol->nextLargeTxId = 0; + + sbgEComProtocolClearLargeTransfer(pProtocol); + + // + // Try to read all incoming data for at least 100 ms and trash them + /// + timeStamp = sbgGetTime(); + + do + { + errorCode = sbgInterfaceRead(pProtocol->pLinkedInterface, pProtocol->rxBuffer, &numBytesRead, sizeof(pProtocol->rxBuffer)); + + if (errorCode != SBG_NO_ERROR) + { + SBG_LOG_ERROR(errorCode, "Unable to read data from interface"); + break; + } + } while ((sbgGetTime() - timeStamp) < 100); + + // + // If we still have read some bytes it means we were not able to purge successfully the rx buffer + // + if ( (errorCode == SBG_NO_ERROR) && (numBytesRead > 0) ) + { + errorCode = SBG_ERROR; + SBG_LOG_ERROR(errorCode, "Unable to purge the rx buffer, %zu bytes remaining", numBytesRead); + } + + return errorCode; } SbgErrorCode sbgEComProtocolSend(SbgEComProtocol *pProtocol, uint8_t msgClass, uint8_t msgId, const void *pData, size_t size) { - SbgErrorCode errorCode; - - if (size <= SBG_ECOM_MAX_PAYLOAD_SIZE) - { - errorCode = sbgEComProtocolSendStandardFrame(pProtocol, msgClass, msgId, pData, size); - } - else - { - size_t nrPages; - - nrPages = sbgDivCeil(size, SBG_ECOM_MAX_EXTENDED_PAYLOAD_SIZE); - - if (nrPages <= UINT16_MAX) - { - const uint8_t *pBuffer; - size_t offset; - uint8_t transferId; - - pBuffer = pData; - offset = 0; - transferId = sbgEComProtocolGetTxId(pProtocol); - errorCode = SBG_INVALID_PARAMETER; - - for (uint16_t pageIndex = 0; pageIndex < nrPages; pageIndex++) - { - size_t transferSize; - - if ((size - offset) < SBG_ECOM_MAX_EXTENDED_PAYLOAD_SIZE) - { - transferSize = size - offset; - } - else - { - transferSize = SBG_ECOM_MAX_EXTENDED_PAYLOAD_SIZE; - } - - errorCode = sbgEComProtocolSendExtendedFrame(pProtocol, msgClass, msgId, transferId, pageIndex, nrPages, &pBuffer[offset], transferSize); - - if (errorCode != SBG_NO_ERROR) - { - break; - } - - offset += transferSize; - } - } - else - { - errorCode = SBG_INVALID_PARAMETER; - SBG_LOG_ERROR(errorCode, "payload size too large: %zu", size); - } - } - - return errorCode; + SbgErrorCode errorCode; + + if (size <= SBG_ECOM_MAX_PAYLOAD_SIZE) + { + errorCode = sbgEComProtocolSendStandardFrame(pProtocol, msgClass, msgId, pData, size); + } + else + { + size_t nrPages; + + nrPages = sbgDivCeil(size, SBG_ECOM_MAX_EXTENDED_PAYLOAD_SIZE); + + if (nrPages <= UINT16_MAX) + { + const uint8_t *pBuffer; + size_t offset; + uint8_t transferId; + + pBuffer = pData; + offset = 0; + transferId = sbgEComProtocolGetTxId(pProtocol); + errorCode = SBG_INVALID_PARAMETER; + + for (uint16_t pageIndex = 0; pageIndex < nrPages; pageIndex++) + { + size_t transferSize; + + if ((size - offset) < SBG_ECOM_MAX_EXTENDED_PAYLOAD_SIZE) + { + transferSize = size - offset; + } + else + { + transferSize = SBG_ECOM_MAX_EXTENDED_PAYLOAD_SIZE; + } + + errorCode = sbgEComProtocolSendExtendedFrame(pProtocol, msgClass, msgId, transferId, pageIndex, nrPages, &pBuffer[offset], transferSize); + + if (errorCode != SBG_NO_ERROR) + { + break; + } + + offset += transferSize; + } + } + else + { + errorCode = SBG_INVALID_PARAMETER; + SBG_LOG_ERROR(errorCode, "payload size too large: %zu", size); + } + } + + return errorCode; } SbgErrorCode sbgEComProtocolReceive(SbgEComProtocol *pProtocol, uint8_t *pMsgClass, uint8_t *pMsgId, void *pData, size_t *pSize, size_t maxSize) { - SbgErrorCode errorCode; - SbgEComProtocolPayload payload; + SbgErrorCode errorCode; + SbgEComProtocolPayload payload; - sbgEComProtocolPayloadConstruct(&payload); + sbgEComProtocolPayloadConstruct(&payload); - errorCode = sbgEComProtocolReceive2(pProtocol, pMsgClass, pMsgId, &payload); + errorCode = sbgEComProtocolReceive2(pProtocol, pMsgClass, pMsgId, &payload); - if (errorCode == SBG_NO_ERROR) - { - size_t size; + if (errorCode == SBG_NO_ERROR) + { + size_t size; - size = sbgEComProtocolPayloadGetSize(&payload); + size = sbgEComProtocolPayloadGetSize(&payload); - if (size <= maxSize) - { - if (pData) - { - const void *pBuffer; + if (size <= maxSize) + { + if (pData) + { + const void *pBuffer; - pBuffer = sbgEComProtocolPayloadGetBuffer(&payload); + pBuffer = sbgEComProtocolPayloadGetBuffer(&payload); - memcpy(pData, pBuffer, size); - } + memcpy(pData, pBuffer, size); + } - if (pSize) - { - *pSize = size; - } - } - else - { - errorCode = SBG_BUFFER_OVERFLOW; - } - } + if (pSize) + { + *pSize = size; + } + } + else + { + errorCode = SBG_BUFFER_OVERFLOW; + } + } - sbgEComProtocolPayloadDestroy(&payload); + sbgEComProtocolPayloadDestroy(&payload); - return errorCode; + return errorCode; } SbgErrorCode sbgEComProtocolReceive2(SbgEComProtocol *pProtocol, uint8_t *pMsgClass, uint8_t *pMsgId, SbgEComProtocolPayload *pPayload) { - SbgErrorCode errorCode; - uint8_t msgClass; - uint8_t msgId; - uint8_t transferId; - uint16_t pageIndex; - uint16_t nrPages; - void *pBuffer; - size_t size; - - assert(pProtocol); - assert(pProtocol->discardSize <= pProtocol->rxBufferSize); - - sbgEComProtocolPayloadClear(pPayload); - - sbgEComProtocolDiscardUnusedBytes(pProtocol); - - sbgEComProtocolRead(pProtocol); - - errorCode = sbgEComProtocolFindFrame(pProtocol, &msgClass, &msgId, &transferId, &pageIndex, &nrPages, &pBuffer, &size); - - if (errorCode == SBG_NO_ERROR) - { - if (nrPages == 0) - { - if (sbgEComProtocolLargeTransferInProgress(pProtocol)) - { - SBG_LOG_ERROR(SBG_ERROR, "standard frame received while a large transfer is in progress"); - SBG_LOG_ERROR(SBG_ERROR, "terminating large transfer"); - - sbgEComProtocolClearLargeTransfer(pProtocol); - } - - if (pMsgClass) - { - *pMsgClass = msgClass; - } - - if (pMsgId) - { - *pMsgId = msgId; - } - - sbgEComProtocolPayloadSet(pPayload, false, pBuffer, size); - } - else - { - errorCode = sbgEComProtocolProcessExtendedFrame(pProtocol, msgClass, msgId, transferId, pageIndex, nrPages, pBuffer, size); - - if (errorCode == SBG_NO_ERROR) - { - if (pMsgClass) - { - *pMsgClass = msgClass; - } - - if (pMsgId) - { - *pMsgId = msgId; - } - - sbgEComProtocolPayloadSet(pPayload, true, pProtocol->pLargeBuffer, pProtocol->largeBufferSize); - sbgEComProtocolResetLargeTransfer(pProtocol); - } - } - } - - return errorCode; + SbgErrorCode errorCode; + uint8_t msgClass; + uint8_t msgId; + uint8_t transferId; + uint16_t pageIndex; + uint16_t nrPages; + void *pBuffer; + size_t size; + + assert(pProtocol); + assert(pProtocol->discardSize <= pProtocol->rxBufferSize); + + sbgEComProtocolPayloadClear(pPayload); + + sbgEComProtocolDiscardUnusedBytes(pProtocol); + + sbgEComProtocolRead(pProtocol); + + errorCode = sbgEComProtocolFindFrame(pProtocol, &msgClass, &msgId, &transferId, &pageIndex, &nrPages, &pBuffer, &size); + + if (errorCode == SBG_NO_ERROR) + { + if (nrPages == 0) + { + if (sbgEComProtocolLargeTransferInProgress(pProtocol)) + { + SBG_LOG_ERROR(SBG_ERROR, "standard frame received while a large transfer is in progress"); + SBG_LOG_ERROR(SBG_ERROR, "terminating large transfer"); + + sbgEComProtocolClearLargeTransfer(pProtocol); + } + + if (pMsgClass) + { + *pMsgClass = msgClass; + } + + if (pMsgId) + { + *pMsgId = msgId; + } + + sbgEComProtocolPayloadSet(pPayload, false, pBuffer, size); + } + else + { + errorCode = sbgEComProtocolProcessExtendedFrame(pProtocol, msgClass, msgId, transferId, pageIndex, nrPages, pBuffer, size); + + if (errorCode == SBG_NO_ERROR) + { + if (pMsgClass) + { + *pMsgClass = msgClass; + } + + if (pMsgId) + { + *pMsgId = msgId; + } + + sbgEComProtocolPayloadSet(pPayload, true, pProtocol->pLargeBuffer, pProtocol->largeBufferSize); + sbgEComProtocolResetLargeTransfer(pProtocol); + } + } + } + + return errorCode; } void sbgEComProtocolSetOnFrameReceivedCb(SbgEComProtocol *pProtocol, SbgEComProtocolFrameCb pOnFrameReceivedCb, void *pUserArg) { - assert(pProtocol); - - pProtocol->pReceiveFrameCb = pOnFrameReceivedCb; - pProtocol->pUserArg = pUserArg; + assert(pProtocol); + + pProtocol->pReceiveFrameCb = pOnFrameReceivedCb; + pProtocol->pUserArg = pUserArg; } SbgErrorCode sbgEComStartFrameGeneration(SbgStreamBuffer *pOutputStream, uint8_t msgClass, uint8_t msg, size_t *pStreamCursor) { - assert(pOutputStream); - assert((msgClass & 0x80) == 0); - 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); + assert(pOutputStream); + assert((msgClass & 0x80) == 0); + 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); } 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 %zu 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; + 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 %zu 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/src/protocol/sbgEComProtocol.h b/src/protocol/sbgEComProtocol.h index 9e5620e..a5126f9 100644 --- a/src/protocol/sbgEComProtocol.h +++ b/src/protocol/sbgEComProtocol.h @@ -1,28 +1,13 @@ /*! - * \file sbgEComProtocol.h - * \ingroup protocol - * \author SBG Systems - * \date 06 February 2013 + * \file sbgEComProtocol.h + * \ingroup protocol + * \author SBG Systems + * \date 06 February 2013 * - * \brief Implementation of the sbgECom binary communication protocol. - * - * 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.
+ * \brief Implementation of the sbgECom binary communication protocol. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -46,8 +31,8 @@ */ /*! - * \defgroup protocol Low Level Protocol - * \brief Defines the low level protocol method to receive and send frames. + * \defgroup protocol Low Level Protocol + * \brief Defines the low level protocol method to receive and send frames. */ #ifndef SBG_ECOM_PROTOCOL_H @@ -66,14 +51,14 @@ extern "C" { //- Constant 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_MAX_EXTENDED_PAYLOAD_SIZE (4081) /*!< Maximum payload size in an extended frame, 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_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_MAX_EXTENDED_PAYLOAD_SIZE (4081) /*!< Maximum payload size in an extended frame, 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. */ +#define SBG_ECOM_RX_TIME_OUT (450) /*!< Default time out for new frame reception. */ //----------------------------------------------------------------------// //- Callbacks definitions -// @@ -90,11 +75,11 @@ typedef struct _SbgEComProtocol SbgEComProtocol; * This callback is used to intercept a valid and full sbgECom frame to easily * intercept and store raw stream. * - * \param[in] pProtocol sbgECom protocol handle instance. - * \param[in] msgClass Received frame message class. - * \param[in] msgId Received frame message id. - * \param[out] pReceivedFrame Stream buffer initialized for read operations on the whole frame data. - * \param[in] pUserArg Optional user supplied argument. + * \param[in] pProtocol sbgECom protocol handle instance. + * \param[in] msgClass Received frame message class. + * \param[in] msgId Received frame message id. + * \param[out] pReceivedFrame Stream buffer initialized for read operations on the whole frame data. + * \param[in] pUserArg Optional user supplied argument. */ typedef void (*SbgEComProtocolFrameCb)(SbgEComProtocol *pProtocol, uint8_t msgClass, uint8_t msgId, SbgStreamBuffer *pReceivedFrame, void *pUserArg); @@ -110,9 +95,9 @@ typedef void (*SbgEComProtocolFrameCb)(SbgEComProtocol *pProtocol, uint8_t msgCl */ typedef struct _SbgEComProtocolPayload { - bool allocated; /*!< True if the buffer is allocated with malloc(). */ - void *pBuffer; /*!< Buffer. */ - size_t size; /*!< Buffer size, in bytes. */ + bool allocated; /*!< True if the buffer is allocated with malloc(). */ + void *pBuffer; /*!< Buffer. */ + size_t size; /*!< Buffer size, in bytes. */ } SbgEComProtocolPayload; /*! @@ -122,28 +107,28 @@ typedef struct _SbgEComProtocolPayload */ 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. */ - size_t discardSize; /*!< Number of bytes to discard on the next receive attempt. */ - uint8_t nextLargeTxId; /*!< Transfer ID of the next large send. */ - - // - // Raw stream sbgECom frame reception callback - // - SbgEComProtocolFrameCb pReceiveFrameCb; /*!< Optional callback used to intercept any received sbgECom frame. */ - void *pUserArg; /*!< Optional user supplied argument for the callback. */ - - // - // Member variables related to large transfer reception. - // - uint8_t *pLargeBuffer; /*!< Buffer for large transfers, allocated with malloc() if valid. */ - size_t largeBufferSize; /*!< Size of the large transfer buffer, in bytes. */ - uint8_t msgClass; /*!< Message class for the current large transfer. */ - uint8_t msgId; /*!< Message ID for the current large transfer. */ - uint8_t transferId; /*!< ID of the current large transfer. */ - uint16_t pageIndex; /*!< Expected page index of the next frame. */ - uint16_t nrPages; /*!< Number of pages in the current transfer. */ + 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. */ + size_t discardSize; /*!< Number of bytes to discard on the next receive attempt. */ + uint8_t nextLargeTxId; /*!< Transfer ID of the next large send. */ + + // + // Raw stream sbgECom frame reception callback + // + SbgEComProtocolFrameCb pReceiveFrameCb; /*!< Optional callback used to intercept any received sbgECom frame. */ + void *pUserArg; /*!< Optional user supplied argument for the callback. */ + + // + // Member variables related to large transfer reception. + // + uint8_t *pLargeBuffer; /*!< Buffer for large transfers, allocated with malloc() if valid. */ + size_t largeBufferSize; /*!< Size of the large transfer buffer, in bytes. */ + uint8_t msgClass; /*!< Message class for the current large transfer. */ + uint8_t msgId; /*!< Message ID for the current large transfer. */ + uint8_t transferId; /*!< ID of the current large transfer. */ + uint16_t pageIndex; /*!< Expected page index of the next frame. */ + uint16_t nrPages; /*!< Number of pages in the current transfer. */ }; //----------------------------------------------------------------------// @@ -153,30 +138,30 @@ struct _SbgEComProtocol /*! * Payload constructor. * - * \param[in] pPayload Payload. + * \param[in] pPayload Payload. */ void sbgEComProtocolPayloadConstruct(SbgEComProtocolPayload *pPayload); /*! * Payload destructor. * - * \param[in] pPayload Payload. + * \param[in] pPayload Payload. */ void sbgEComProtocolPayloadDestroy(SbgEComProtocolPayload *pPayload); /*! * Get the buffer of a payload. * - * \param[in] pPayload Payload. - * \return Payload buffer. + * \param[in] pPayload Payload. + * \return Payload buffer. */ const void *sbgEComProtocolPayloadGetBuffer(const SbgEComProtocolPayload *pPayload); /*! * Get the size of a payload buffer. * - * \param[in] pPayload Payload. - * \return Size of the payload buffer, in bytes. + * \param[in] pPayload Payload. + * \return Size of the payload buffer, in bytes. */ size_t sbgEComProtocolPayloadGetSize(const SbgEComProtocolPayload *pPayload); @@ -188,8 +173,8 @@ size_t sbgEComProtocolPayloadGetSize(const SbgEComProtocolPayload *pPayload); * * The buffer must be released with free() once unused. * - * \param[in] pPayload Payload. - * \return Payload buffer if successful, NULL otherwise. + * \param[in] pPayload Payload. + * \return Payload buffer if successful, NULL otherwise. */ void *sbgEComProtocolPayloadMoveBuffer(SbgEComProtocolPayload *pPayload); @@ -200,17 +185,17 @@ void *sbgEComProtocolPayloadMoveBuffer(SbgEComProtocolPayload *pPayload); /*! * Initialize the protocol system used to communicate with the product and return the created handle. * - * \param[in] pProtocol Protocol instance to construct. - * \param[in] pInterface Interface to use for read/write operations. - * \return SBG_NO_ERROR if we have initialized the protocol system. + * \param[in] pProtocol Protocol instance to construct. + * \param[in] pInterface Interface to use for read/write operations. + * \return SBG_NO_ERROR if we have initialized the protocol system. */ SbgErrorCode sbgEComProtocolInit(SbgEComProtocol *pProtocol, SbgInterface *pInterface); /*! * Close the protocol system. * - * \param[in] pProtocol A valid protocol instance. - * \return SBG_NO_ERROR if we have closed and released the protocol system. + * \param[in] pProtocol A valid protocol instance. + * \return SBG_NO_ERROR if we have closed and released the protocol system. */ SbgErrorCode sbgEComProtocolClose(SbgEComProtocol *pProtocol); @@ -219,10 +204,10 @@ SbgErrorCode sbgEComProtocolClose(SbgEComProtocol *pProtocol); * * For example, if the program flow has been interrupted, this method can be helpful to discard all trash received data. * - * WARNING: This method is blocking for 100ms and actively tries to read incoming data. + * \note This method is blocking for 100ms and actively tries to read incoming data. * - * \param[in] pProtocol A valid protocol instance. - * \return SBG_NO_ERROR if the incoming data has been purged successfully. + * \param[in] pProtocol A valid protocol instance. + * \return SBG_NO_ERROR if the incoming data has been purged successfully. */ SbgErrorCode sbgEComProtocolPurgeIncoming(SbgEComProtocol *pProtocol); @@ -232,27 +217,27 @@ SbgErrorCode sbgEComProtocolPurgeIncoming(SbgEComProtocol *pProtocol); * If the size is SBG_ECOM_MAX_PAYLOAD_SIZE or less, the data is sent in a single frame. Otherwise, * is it fragmented into multiple extended frames, each sent in order, which may block. * - * \param[in] pProtocol A valid protocol instance. - * \param[in] msgClass Message class. - * \param[in] msg Message ID. - * \param[in] pData Data buffer. - * \param[in] size Data buffer size, in bytes. - * \return SBG_NO_ERROR if the frame has been sent. + * \param[in] pProtocol A valid protocol instance. + * \param[in] msgClass Message class. + * \param[in] msg Message ID. + * \param[in] pData Data buffer. + * \param[in] size Data buffer size, in bytes. + * \return SBG_NO_ERROR if the frame has been sent. */ SbgErrorCode sbgEComProtocolSend(SbgEComProtocol *pProtocol, uint8_t msgClass, uint8_t msg, const void *pData, size_t size); /*! * Receive a frame. * - * \param[in] pProtocol A valid protocol instance. - * \param[out] pMsgClass Message class, may be NULL. - * \param[out] pMsgId Message ID, may be NULL. - * \param[out] pData Data buffer. - * \param[out] pSize Number of bytes received. - * \param[in] maxSize Data buffer size, in bytes. - * \return SBG_NO_ERROR if successful, - * SBG_NOT_READY if no complete frame has been received, - * SBG_BUFFER_OVERFLOW if the payload of the received frame couldn't fit into the data buffer. + * \param[in] pProtocol A valid protocol instance. + * \param[out] pMsgClass Message class, may be NULL. + * \param[out] pMsgId Message ID, may be NULL. + * \param[out] pData Data buffer. + * \param[out] pSize Number of bytes received. + * \param[in] maxSize Data buffer size, in bytes. + * \return SBG_NO_ERROR if successful, + * SBG_NOT_READY if no complete frame has been received, + * SBG_BUFFER_OVERFLOW if the payload of the received frame couldn't fit into the data buffer. */ SbgErrorCode sbgEComProtocolReceive(SbgEComProtocol *pProtocol, uint8_t *pMsgClass, uint8_t *pMsgId, void *pData, size_t *pSize, size_t maxSize); @@ -268,12 +253,12 @@ SbgErrorCode sbgEComProtocolReceive(SbgEComProtocol *pProtocol, uint8_t *pMsgCla * Because the payload buffer may directly refer to the protocol work buffer on return, it is only valid until * the next attempt to receive a frame, with any of the receive functions. * - * \param[in] pProtocol A valid protocol instance. - * \param[out] pMsgClass Message class, may be NULL. - * \param[out] pMsgId Message ID, may be NULL. - * \param[out] pPayload Payload. - * \return SBG_NO_ERROR if successful, - * SBG_NOT_READY if no complete frame has been received. + * \param[in] pProtocol A valid protocol instance. + * \param[out] pMsgClass Message class, may be NULL. + * \param[out] pMsgId Message ID, may be NULL. + * \param[out] pPayload Payload. + * \return SBG_NO_ERROR if successful, + * SBG_NOT_READY if no complete frame has been received. */ SbgErrorCode sbgEComProtocolReceive2(SbgEComProtocol *pProtocol, uint8_t *pMsgClass, uint8_t *pMsgId, SbgEComProtocolPayload *pPayload); @@ -285,9 +270,9 @@ SbgErrorCode sbgEComProtocolReceive2(SbgEComProtocol *pProtocol, uint8_t *pMsgCl * * EXPERIMENTAL: This handler is still experimental and API may change in next sbgECom versions. * - * \param[in] pHandle A valid protocol instance. - * \param[in] pOnFrameReceivedCb Function to call each time a valid sbgECom frame is received. - * \param[in] pUserArg Optional user argument that will be passed to the callback method. + * \param[in] pProtocol A valid protocol instance. + * \param[in] pOnFrameReceivedCb Function to call each time a valid sbgECom frame is received. + * \param[in] pUserArg Optional user argument that will be passed to the callback method. */ void sbgEComProtocolSetOnFrameReceivedCb(SbgEComProtocol *pProtocol, SbgEComProtocolFrameCb pOnFrameReceivedCb, void *pUserArg); @@ -298,12 +283,12 @@ void sbgEComProtocolSetOnFrameReceivedCb(SbgEComProtocol *pProtocol, SbgEComProt * * Only standard frames may be sent with this function. * - * \param[in] pOutputStream Pointer to an allocated and initialized output stream. - * \param[in] msgClass Message class. - * \param[in] msg Message ID. - * \param[out] pStreamCursor The initial output stream cursor that thus points to the beginning of the generated message. - * This value should be passed to sbgEComFinalizeFrameGeneration for correct operations. - * \return SBG_NO_ERROR in case of good operation. + * \param[in] pOutputStream Pointer to an allocated and initialized output stream. + * \param[in] msgClass Message class. + * \param[in] msg Message ID. + * \param[out] pStreamCursor The initial output stream cursor that thus points to the beginning 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); @@ -313,10 +298,10 @@ SbgErrorCode sbgEComStartFrameGeneration(SbgStreamBuffer *pOutputStream, uint8_t * 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. + * \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); diff --git a/src/sbgECanId.h b/src/sbgECanId.h index e4790a3..7a6e3ca 100644 --- a/src/sbgECanId.h +++ b/src/sbgECanId.h @@ -1,13 +1,13 @@ /*! - * \file sbgECanId.h - * \ingroup main - * \author SBG Systems - * \date 10 October 2014 + * \file sbgECanId.h + * \ingroup main + * \author SBG Systems + * \date 10 October 2014 * - * \brief Defines all sbgECom commands identifiers. + * \brief Defines all sbgECom commands identifiers. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -46,117 +46,117 @@ extern "C" { */ 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_AUTO_TRACK_SLIP_CURV = 0x220, + // + // 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_AUTO_TRACK_SLIP_CURV = 0x220, } SbgECanMessageId; #ifdef __cplusplus } #endif -#endif // SBG_ECAN_ID_H +#endif // SBG_ECAN_ID_H diff --git a/src/sbgECom.c b/src/sbgECom.c index d3c6f9d..a4a2411 100644 --- a/src/sbgECom.c +++ b/src/sbgECom.c @@ -13,249 +13,249 @@ SbgErrorCode sbgEComInit(SbgEComHandle *pHandle, SbgInterface *pInterface) { - SbgErrorCode errorCode = SBG_NO_ERROR; - - assert(pHandle); - assert(pInterface); - - // - // Initialize the sbgECom handle - // - pHandle->pReceiveLogCallback = NULL; - pHandle->pUserArg = NULL; + SbgErrorCode errorCode = SBG_NO_ERROR; + + assert(pHandle); + assert(pInterface); + + // + // 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 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); - - return errorCode; + // + // Initialize the protocol + // + errorCode = sbgEComProtocolInit(&pHandle->protocolHandle, pInterface); + + return errorCode; } SbgErrorCode sbgEComClose(SbgEComHandle *pHandle) { - SbgErrorCode errorCode = SBG_NO_ERROR; + SbgErrorCode errorCode = SBG_NO_ERROR; - assert(pHandle); + assert(pHandle); - // - // Close the protocol - // - errorCode = sbgEComProtocolClose(&pHandle->protocolHandle); - - return errorCode; + // + // Close the protocol + // + errorCode = sbgEComProtocolClose(&pHandle->protocolHandle); + + return errorCode; } SbgErrorCode sbgEComHandleOneLog(SbgEComHandle *pHandle) { - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgEComLogUnion logData; - uint8_t receivedMsg; - uint8_t receivedMsgClass; - size_t payloadSize; - uint8_t payloadData[SBG_ECOM_MAX_PAYLOAD_SIZE]; + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComLogUnion logData; + uint8_t receivedMsg; + uint8_t receivedMsgClass; + size_t payloadSize; + uint8_t payloadData[SBG_ECOM_MAX_PAYLOAD_SIZE]; - assert(pHandle); + assert(pHandle); - // - // Try to read a received frame - // - errorCode = sbgEComProtocolReceive(&pHandle->protocolHandle, &receivedMsgClass, &receivedMsg, payloadData, &payloadSize, sizeof(payloadData)); + // + // 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 = sbgEComLogParse((SbgEComClass)receivedMsgClass, (SbgEComMsgId)receivedMsg, payloadData, payloadSize, &logData); + // + // 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 = sbgEComLogParse((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); - } + // + // 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); + } - // - // Clean up resources allocated during parsing, if any. - // - sbgEComLogCleanup(&logData, (SbgEComClass)receivedMsgClass, (SbgEComMsgId)receivedMsg); - } - 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; + // + // Clean up resources allocated during parsing, if any. + // + sbgEComLogCleanup(&logData, (SbgEComClass)receivedMsgClass, (SbgEComMsgId)receivedMsg); + } + 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; } SbgErrorCode sbgEComHandle(SbgEComHandle *pHandle) { - SbgErrorCode errorCode = SBG_NO_ERROR; + SbgErrorCode errorCode = SBG_NO_ERROR; - assert(pHandle); + 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; + // + // 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; } SbgErrorCode sbgEComPurgeIncoming(SbgEComHandle *pHandle) { - SbgErrorCode errorCode = SBG_NO_ERROR; + SbgErrorCode errorCode = SBG_NO_ERROR; - assert(pHandle); + assert(pHandle); - errorCode = sbgEComProtocolPurgeIncoming(&pHandle->protocolHandle); + errorCode = sbgEComProtocolPurgeIncoming(&pHandle->protocolHandle); - return errorCode; + return errorCode; } void sbgEComSetReceiveLogCallback(SbgEComHandle *pHandle, SbgEComReceiveLogFunc pReceiveLogCallback, void *pUserArg) { - assert(pHandle); + assert(pHandle); - // - // Define the callback and the user argument - // - pHandle->pReceiveLogCallback = pReceiveLogCallback; - pHandle->pUserArg = pUserArg; + // + // Define the callback and the user argument + // + pHandle->pReceiveLogCallback = pReceiveLogCallback; + pHandle->pUserArg = pUserArg; } void sbgEComSetCmdTrialsAndTimeOut(SbgEComHandle *pHandle, uint32_t numTrials, uint32_t cmdDefaultTimeOut) { - assert(pHandle); - assert(numTrials > 0); - assert(cmdDefaultTimeOut > 0); + assert(pHandle); + assert(numTrials > 0); + assert(cmdDefaultTimeOut > 0); - // - // Define the new settings - // - pHandle->numTrials = numTrials; - pHandle->cmdDefaultTimeOut = cmdDefaultTimeOut; + // + // Define the new settings + // + pHandle->numTrials = numTrials; + pHandle->cmdDefaultTimeOut = cmdDefaultTimeOut; } 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 canceled 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; - } - } + 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 SBG_CALIB_MAG_NOT_ENOUGH_POINTS: + strcpy(errorMsg, "SBG_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 canceled 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/src/sbgECom.h b/src/sbgECom.h index e70815b..6424537 100644 --- a/src/sbgECom.h +++ b/src/sbgECom.h @@ -1,13 +1,13 @@ /*! - * \file sbgECom.h - * \ingroup main - * \author SBG Systems - * \date 05 February 2013 + * \file sbgECom.h + * \ingroup main + * \author SBG Systems + * \date 05 February 2013 * - * \brief Contains main sbgECom methods. + * \brief Contains main sbgECom methods. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -31,8 +31,8 @@ */ /*! - * \defgroup main Main Library - * \brief Main sbgECom library initialization and parsing methods. + * \defgroup main Main Library + * \brief Main sbgECom library initialization and parsing methods. */ #ifndef SBG_ECOM_H @@ -68,12 +68,12 @@ typedef struct _SbgEComHandle SbgEComHandle; /*! * 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. + * \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 SbgEComLogUnion *pLogData, void *pUserArg); @@ -86,13 +86,13 @@ typedef SbgErrorCode (*SbgEComReceiveLogFunc)(SbgEComHandle *pHandle, SbgEComCla */ struct _SbgEComHandle { - SbgEComProtocol protocolHandle; /*!< Handle on the protocol system. */ + 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. */ + 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). */ + 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). */ }; //----------------------------------------------------------------------// @@ -102,33 +102,33 @@ struct _SbgEComHandle /*! * 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. + * \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. + * \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. + * \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. + * \param[in] pHandle A valid sbgECom handle. + * \return SBG_NO_ERROR if no error occurs during incoming logs parsing. */ SbgErrorCode sbgEComHandle(SbgEComHandle *pHandle); @@ -137,36 +137,36 @@ SbgErrorCode sbgEComHandle(SbgEComHandle *pHandle); * * For example, if the program flow has been interrupted, this method can be helpful to discard all trash received data. * - * WARNING: This method is blocking for 100ms and actively tries to read incoming data. + * \note This method is blocking for 100ms and actively tries to read incoming data. * - * \param[in] pHandle A valid sbgECom handle. - * \return SBG_NO_ERROR if the incoming data has been purged successfully. + * \param[in] pHandle A valid sbgECom handle. + * \return SBG_NO_ERROR if the incoming data has been purged successfully. */ SbgErrorCode sbgEComPurgeIncoming(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. + * \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. */ void 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. - * \param[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. + * \param[in] pHandle A valid sbgECom handle. + * \param[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. + * 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. + * \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]); @@ -174,5 +174,5 @@ void sbgEComErrorToString(SbgErrorCode errorCode, char errorMsg[256]); } #endif -#endif // SBG_ECOM_H +#endif // SBG_ECOM_H diff --git a/src/sbgEComGetVersion.c b/src/sbgEComGetVersion.c index 8c2f207..77e3d62 100644 --- a/src/sbgEComGetVersion.c +++ b/src/sbgEComGetVersion.c @@ -14,10 +14,10 @@ uint32_t sbgEComGetVersion(void) { - return SBG_E_COM_VERSION; + return SBG_E_COM_VERSION; } const char *sbgEComGetVersionAsString(void) { - return SBG_E_COM_VERSION_STR; + return SBG_E_COM_VERSION_STR; } diff --git a/src/sbgEComGetVersion.h b/src/sbgEComGetVersion.h index 6f628b7..77e584f 100644 --- a/src/sbgEComGetVersion.h +++ b/src/sbgEComGetVersion.h @@ -1,12 +1,12 @@ /*! - * \file sbgEComGetVersion.h - * \ingroup main - * \author SBG Systems - * \date 05 February 2013 - * \brief Version information. + * \file sbgEComGetVersion.h + * \ingroup main + * \author SBG Systems + * \date 05 February 2013 + * \brief Version information. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -46,14 +46,14 @@ extern "C" { /*! * Returns an integer representing the version of the sbgECom library. * - * \return An integer representing the version of the sbgECom library.
+ * \return An integer representing the version of the sbgECom library. */ uint32_t sbgEComGetVersion(void); /*! * Retrieve the sbgECom library version as a string (1.0.443-stable). * - * \return Null terminated string that contains the sbgECom library version. + * \return Null terminated string that contains the sbgECom library version. */ const char *sbgEComGetVersionAsString(void); diff --git a/src/sbgEComIds.h b/src/sbgEComIds.h index 389ef67..d5851e2 100644 --- a/src/sbgEComIds.h +++ b/src/sbgEComIds.h @@ -1,13 +1,13 @@ /*! - * \file sbgEComIds.h - * \ingroup main - * \author SBG Systems - * \date 25 February 2013 + * \file sbgEComIds.h + * \ingroup main + * \author SBG Systems + * \date 25 February 2013 * - * \brief Defines all sbgECom commands identifiers. + * \brief Defines all sbgECom commands identifiers. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -48,19 +48,20 @@ extern "C" { */ typedef enum _SbgEComClass { - SBG_ECOM_CLASS_LOG_ECOM_0 = 0x00, /*!< Class that contains sbgECom protocol input/output log messages. */ + 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 (deprecated in ELLIPSE firmware v3.x and above) . */ - 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 standard NMEA messages such as GGA/RMC/ZDA produced from the INS filer. */ + SBG_ECOM_CLASS_LOG_NMEA_1 = 0x03, /*!< Class that contains proprietary NMEA (and NMEA like) output logs. */ + + SBG_ECOM_CLASS_LOG_THIRD_PARTY_0 = 0x04, /*!< Class that contains third party output logs using mostly binary protocols. */ + + SBG_ECOM_CLASS_LOG_NMEA_GNSS = 0x05, /*!< Class that contains standard NMEA messages such as GGA/RMC/ZDA but produced from the internal GNSS data. */ - 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. */ + SBG_ECOM_CLASS_LOG_CMD_0 = 0x10, /*!< Class that contains sbgECom protocol commands. */ + + SBG_ECOM_CLASS_LOG_ALL = 0xFF /*!< Special value to represents all message classes. */ } SbgEComClass; //----------------------------------------------------------------------// @@ -72,69 +73,75 @@ typedef enum _SbgEComClass */ typedef enum _SbgEComLog { - SBG_ECOM_LOG_STATUS = 1, /*!< Status general, clock, com aiding, solution, heave */ + 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, /*!< DEPRECATED: Synchronous IMU measurements (time aligned to UTC - NEVER use for Post Processing). */ + + 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_UTC_TIME = 2, /*!< Provides UTC time reference */ + 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_IMU_DATA = 3, /*!< DEPRECATED: Synchronous IMU measurements (time aligned to UTC - NEVER use for Post Processing). */ + SBG_ECOM_LOG_SHIP_MOTION = 9, /*!< Heave, surge and sway and accelerations on each axis. */ - 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_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_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_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_SHIP_MOTION = 9, /*!< Heave, surge and sway and accelerations on each axis. */ + SBG_ECOM_LOG_ODO_VEL = 19, /*!< Provides odometer velocity */ - 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_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_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_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_ODO_VEL = 19, /*!< Provides odometer velocity */ + SBG_ECOM_LOG_GPS1_RAW = 31, /*!< GPS 1 raw data for post processing. */ - 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_SHIP_MOTION_HP = 32, /*!< Return delayed ship motion such as surge, sway, heave. */ - 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_AIR_DATA = 36, /*!< Air Data aiding such as barometric altimeter and true air speed. */ - SBG_ECOM_LOG_GPS1_RAW = 31, /*!< GPS 1 raw data for post processing. */ + SBG_ECOM_LOG_USBL = 37, /*!< Raw USBL position data for sub-sea navigation. */ - SBG_ECOM_LOG_SHIP_MOTION_HP = 32, /*!< Return delayed ship motion such as surge, sway, heave. */ + SBG_ECOM_LOG_GPS2_RAW = 38, /*!< GPS 2 raw data for post processing. */ - 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 sub-sea navigation. */ + SBG_ECOM_LOG_IMU_SHORT = 44, /*!< Asynchronous IMU measurements output at the IMU rate and to use for Post Processing with Qinertia. */ - SBG_ECOM_LOG_GPS2_RAW = 38, /*!< GPS 2 raw data for post processing. */ + SBG_ECOM_LOG_EVENT_OUT_A = 45, /*!< Event marker used to timestamp each generated Sync Out A signal. */ + SBG_ECOM_LOG_EVENT_OUT_B = 46, /*!< Event marker used to timestamp each generated Sync Out B signal. */ + SBG_ECOM_LOG_DEPTH = 47, /*!< Depth sensor measurement log used for sub-sea navigation. */ + SBG_ECOM_LOG_DIAG = 48, /*!< Diagnostic log. */ - SBG_ECOM_LOG_IMU_SHORT = 44, /*!< Asynchronous IMU measurements output at the IMU rate and to use for Post Processing with Qinertia. */ + SBG_ECOM_LOG_RTCM_RAW = 49, /*!< RTCM raw data. */ - 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_GPS1_SAT = 50, /*!< GPS 1 Satellite data. */ + SBG_ECOM_LOG_GPS2_SAT = 51, /*!< GNSS2 Satellite data. */ - SBG_ECOM_LOG_DEPTH = 47, /*!< Depth sensor measurement log used for sub-sea navigation. */ - SBG_ECOM_LOG_DIAG = 48, /*!< Diagnostic log. */ + SBG_ECOM_LOG_EKF_ROT_ACCEL_BODY = 52, /*!< INS body rotation rate and lateral acceleration (bias, earth rotation and gravity free compensated). */ + SBG_ECOM_LOG_EKF_ROT_ACCEL_NED = 53, /*!< INS North/East/Down rotation rate and lateral acceleration (bias, earth rotation and gravity free compensated). */ + SBG_ECOM_LOG_EKF_VEL_BODY = 54, /*!< INS X,Y,Z body velocity and standard deviation. */ - SBG_ECOM_LOG_RTCM_RAW = 49, /*!< RTCM raw data. */ + SBG_ECOM_LOG_SESSION_INFO = 55, /*!< Session information, including device information and current settings. */ - SBG_ECOM_LOG_GPS1_SAT = 50, /*!< GPS 1 Satellite data. */ - SBG_ECOM_LOG_GPS2_SAT = 51, /*!< GNSS2 Satellite data. */ + SBG_ECOM_LOG_EKF_DEBUG = 56, /*!< EKF debug data. */ - SBG_ECOM_LOG_EKF_ROT_ACCEL_BODY = 52, /*!< INS body rotation rate and lateral acceleration (bias, earth rotation and gravity free compensated). */ - SBG_ECOM_LOG_EKF_ROT_ACCEL_NED = 53, /*!< INS North/East/Down rotation rate and lateral acceleration (bias, earth rotation and gravity free compensated). */ - SBG_ECOM_LOG_EKF_VEL_BODY = 54, /*!< INS X,Y,Z body velocity and standard deviation. */ + SBG_ECOM_LOG_PTP_STATUS = 57, /*!< PTP status. */ - SBG_ECOM_LOG_ECOM_NUM_MESSAGES /*!< Helper definition to know the number of ECom messages */ + SBG_ECOM_LOG_ECOM_NUM_MESSAGES /*!< Helper definition to know the number of ECom messages */ } SbgEComLog; /*! @@ -142,26 +149,26 @@ typedef enum _SbgEComLog */ 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 */ + 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. + * Defines standard NMEA output logs based on the INS filter data. */ 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_RTO = 9, /*!< Rate and direction of turn. */ - SBG_ECOM_LOG_NMEA_GSV = 10, /*!< GNSS Satellites in View with azimuth, elevation and SNR information */ - SBG_ECOM_LOG_NMEA_NUM_MESSAGES /*!< Helper definition to know the number of NMEA messages */ + 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_RTO = 9, /*!< Rate and direction of turn. */ + SBG_ECOM_LOG_NMEA_GSV = 10, /*!< GNSS Satellites in View with azimuth, elevation and SNR information */ + SBG_ECOM_LOG_NMEA_NUM_MESSAGES /*!< Helper definition to know the number of NMEA messages */ } SbgEComNmeaLog; /*! @@ -169,25 +176,25 @@ typedef enum _SbgEComNmeaLog */ 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_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_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_GGK = 10, /*!< Trimble NMEA like log with Time, Latitude, Longitude, Ellipsoidal height */ - SBG_ECOM_LOG_NMEA_1_PPS = 11, /*!< Trimble (Applanix) NMEA like log with UTC and PPS information. */ + SBG_ECOM_LOG_NMEA_1_GGK = 10, /*!< Trimble NMEA like log with Time, Latitude, Longitude, Ellipsoidal height */ + SBG_ECOM_LOG_NMEA_1_PPS = 11, /*!< Trimble (Applanix) NMEA like log with UTC and PPS information. */ - SBG_ECOM_LOG_NMEA_1_WASSP = 12, /*!< WASSP NMEA like log similar to PASHR one. */ + SBG_ECOM_LOG_NMEA_1_WASSP = 12, /*!< WASSP NMEA like log similar to PASHR one. */ - SBG_ECOM_LOG_NMEA_1_PSBGA = 13, /*!< SBG Systems proprietary sentence that reports EKF attitude and status. */ + SBG_ECOM_LOG_NMEA_1_PSBGA = 13, /*!< SBG Systems proprietary sentence that reports EKF attitude and status. */ - SBG_ECOM_LOG_NMEA_1_NUM_MESSAGES /*!< Helper definition to know the number of NMEA messages */ + SBG_ECOM_LOG_NMEA_1_NUM_MESSAGES /*!< Helper definition to know the number of NMEA messages */ } SbgEComIdNmea1Log; /*! @@ -195,127 +202,138 @@ typedef enum _SbgEComIdNmea1Log */ 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_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, /*!< Kongsberg SimRad 1000 proprietary frame that outputs Roll, Pitch and Heading. */ - SBG_ECOM_THIRD_PARTY_SIMRAD_3000 = 4, /*!< Kongsberg SimRad 3000 proprietary frame that outputs Roll, Pitch and Heading. */ + SBG_ECOM_THIRD_PARTY_PD0 = 2, /*!< Teledyne PD0 DVL proprietary frame. */ + SBG_ECOM_THIRD_PARTY_SIMRAD_1000 = 3, /*!< Kongsberg SimRad 1000 proprietary frame that outputs Roll, Pitch and Heading. */ + SBG_ECOM_THIRD_PARTY_SIMRAD_3000 = 4, /*!< Kongsberg SimRad 3000 proprietary frame that outputs Roll, Pitch and Heading. */ - SBG_ECOM_THIRD_PARTY_SEAPATH_B26 = 5, /*!< Kongsberg 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_THIRD_PARTY_ADA_01 = 8, /*!< ADA specific Data Packet with IMU/INS/Status data */ + SBG_ECOM_THIRD_PARTY_SEAPATH_B26 = 5, /*!< Kongsberg 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_THIRD_PARTY_ADA_01 = 8, /*!< ADA specific Data Packet with IMU/INS/Status data */ - SBG_ECOM_THIRD_PARTY_AT_ITINS = 9, /*!< Cobham Aviator UAV 200 navigation (orientation & position) data */ + SBG_ECOM_THIRD_PARTY_AT_ITINS = 9, /*!< Cobham Aviator UAV 200 navigation (orientation & position) data */ - SBG_ECOM_THIRD_PARTY_KONGSBERG_MB = 10, /*!< Kongsberg multi-beam binary log. */ + SBG_ECOM_THIRD_PARTY_KONGSBERG_MB = 10, /*!< Kongsberg multi-beam binary log. */ - SBG_ECOM_LOG_THIRD_PARTY_NUM_MESSAGES /*!< Helper definition to know the number of third party messages */ + SBG_ECOM_LOG_THIRD_PARTY_NUM_MESSAGES /*!< Helper definition to know the number of third party messages */ } SbgEComIdThirdParty; +/*! + * Defines standard NMEA output logs based on the GNSS data. + */ +typedef enum _SbgEComNmeaGnssLog +{ + SBG_ECOM_LOG_NMEA_GNSS_GGA = 0, /*!< Latitude, Longitude, Altitude, Quality indicator. */ + SBG_ECOM_LOG_NMEA_GNSS_RMC = 1, /*!< Latitude, Longitude, velocity, course over ground. */ + SBG_ECOM_LOG_NMEA_GNSS_HDT = 3, /*!< Heading (True). */ + SBG_ECOM_LOG_NMEA_GNSS_VTG = 8, /*!< Track an Speed over the ground. */ +} SbgEComNmeaGnssLog; + /*! * Enum that defines all the available commands for the sbgECom library. */ typedef enum _SbgEComCmd { - /* Acknowledge */ - SBG_ECOM_CMD_ACK = 0, /*!< Acknowledge */ + /* 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 */ + /* 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 */ + /* 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 */ + /* 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 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. */ + /* 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_REJECT_MODES = 19, /*!< GNSS aiding rejection modes configuration. */ + /* 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_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. */ + /* 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 */ + /* 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 */ + /* 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 */ + /* Output configuration */ + SBG_ECOM_CMD_NMEA_TALKER_ID = 29, /*!< NMEA talker ID configuration */ + SBG_ECOM_CMD_OUTPUT_CONF = 30, /*!< Output configuration */ - /* Advanced configuration */ - SBG_ECOM_CMD_ADVANCED_CONF = 32, /*!< Advanced settings configuration */ + /* Advanced configuration */ + SBG_ECOM_CMD_ADVANCED_CONF = 32, /*!< Advanced settings configuration */ - /* Features related commands */ - SBG_ECOM_CMD_FEATURES = 33, /*!< Retrieve device features */ + /* 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 */ + /* 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 */ + /* 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. */ + /* 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 */ + /* 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. */ + /* 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. */ + /* 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) */ + /* Odometer configuration (using CAN) */ + SBG_ECOM_CMD_ODO_CAN_CONF = 45, /*!< Configuration for CAN based odometer (CAN ID & DBC) */ - SBG_ECOM_CMD_GNSS_1_INSTALLATION = 46, /*!< Define or retrieve the GNSS 1 main and secondary lever arms configuration. */ + SBG_ECOM_CMD_GNSS_1_INSTALLATION = 46, /*!< Define or retrieve the GNSS 1 main and secondary lever arms configuration. */ - /* REST API related commands */ - SBG_ECOM_CMD_API_POST = 47, /*!< Command equivalent to the HTTP POST method for a REST API. */ - SBG_ECOM_CMD_API_GET = 48, /*!< Command equivalent to the HTTP GET method for a REST API. */ + /* REST API related commands */ + SBG_ECOM_CMD_API_POST = 47, /*!< Command equivalent to the HTTP POST method for a REST API. */ + SBG_ECOM_CMD_API_GET = 48, /*!< Command equivalent to the HTTP GET method for a REST API. */ - /* Misc. */ - SBG_ECOM_LOG_ECOM_NUM_CMDS /*!< Helper definition to know the number of commands */ + /* 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. + * 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; +typedef uint8_t SbgEComMsgId; //----------------------------------------------------------------------// //- Inline helpers for log IDs -// @@ -324,26 +342,26 @@ typedef uint8_t SbgEComMsgId; /*! * 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. + * \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; - } + // + // 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; + } } #ifdef __cplusplus } #endif -#endif // SBG_ECOM_IDS_H +#endif // SBG_ECOM_IDS_H diff --git a/src/sbgEComLib.h b/src/sbgEComLib.h index 7a2beb8..99c0722 100644 --- a/src/sbgEComLib.h +++ b/src/sbgEComLib.h @@ -1,15 +1,15 @@ /*! - * \file sbgEComLib.h - * \ingroup main - * \author SBG Systems - * \date 05 February 2013 + * \file sbgEComLib.h + * \ingroup main + * \author SBG Systems + * \date 05 February 2013 * - * \brief Main header file for the SBG Systems Enhanced Communication Library. + * \brief Main header file for the SBG Systems Enhanced Communication Library. * * Only this main header file should be included to use the library. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -57,6 +57,7 @@ extern "C" { #include "commands/sbgEComCmd.h" #include "logs/sbgEComLog.h" #include "protocol/sbgEComProtocol.h" +#include "sessionInfo/sbgEComSessionInfo.h" #include "sbgEComVersion.h" #include "sbgEComGetVersion.h" @@ -67,4 +68,4 @@ extern "C" { } #endif -#endif // SBG_ECOM_LIB_H +#endif // SBG_ECOM_LIB_H diff --git a/src/sbgEComVersion.h b/src/sbgEComVersion.h index 2ed547a..1c970ce 100644 --- a/src/sbgEComVersion.h +++ b/src/sbgEComVersion.h @@ -1,13 +1,13 @@ /*! - * \file sbgEComVersion.h - * \ingroup main - * \author SBG Systems - * \date 05 February 2013 + * \file sbgEComVersion.h + * \ingroup main + * \author SBG Systems + * \date 05 February 2013 * - * \brief Header file that contains all versions related information such as change log. + * \brief Header file that contains all versions related information such as change log. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -45,23 +45,23 @@ extern "C" { //- Version definitions -// //----------------------------------------------------------------------// -#define SBG_E_COM_VERSION_MAJOR 4 -#define SBG_E_COM_VERSION_MINOR 0 -#define SBG_E_COM_VERSION_REV 1987 -#define SBG_E_COM_VERSION_BUILD SBG_VERSION_QUALIFIER_STABLE +#define SBG_E_COM_VERSION_MAJOR 5 +#define SBG_E_COM_VERSION_MINOR 1 +#define SBG_E_COM_VERSION_REV 708 +#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) +#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 + #define SBG_STR(X) #X #endif #ifndef SBG_ASSTR - #define SBG_ASSTR(X) SBG_STR(X) + #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" +#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" #ifdef __cplusplus } diff --git a/src/sessionInfo/sbgEComSessionInfo.c b/src/sessionInfo/sbgEComSessionInfo.c new file mode 100644 index 0000000..163256f --- /dev/null +++ b/src/sessionInfo/sbgEComSessionInfo.c @@ -0,0 +1,110 @@ +// sbgCommonLib headers +#include + +// Local headers +#include "sbgEComSessionInfo.h" + +//----------------------------------------------------------------------// +//- Private functions -// +//----------------------------------------------------------------------// + +/*! + * Reset a session information context. + * + * \param[in] pCtx Context. + */ +static void sbgEComSessionInfoCtxReset(SbgEComSessionInfoCtx *pCtx) +{ + assert(pCtx); + + pCtx->string[0] = '\0'; + + pCtx->length = 0; + pCtx->pageIndex = 0; + pCtx->nrPages = 0; +} + +//----------------------------------------------------------------------// +//- Public functions -// +//----------------------------------------------------------------------// + +void sbgEComSessionInfoCtxConstruct(SbgEComSessionInfoCtx *pCtx) +{ + assert(pCtx); + + memset(pCtx->string, 0, sizeof(pCtx->string)); + + pCtx->length = 0; + pCtx->pageIndex = 0; + pCtx->nrPages = 0; +} + +SbgErrorCode sbgEComSessionInfoCtxProcess(SbgEComSessionInfoCtx *pCtx, uint16_t pageIndex, uint16_t nrPages, const void *pBuffer, size_t size) +{ + SbgErrorCode errorCode = SBG_NOT_READY; + + assert(pCtx); + assert(pageIndex < nrPages); + assert(pBuffer || (size == 0)); + + if (pCtx->pageIndex != pageIndex) + { + if ((pageIndex != 0) || (pCtx->pageIndex != pCtx->nrPages)) + { + SBG_LOG_WARNING(SBG_ERROR, "unexpected page index received, session information context reset"); + } + + sbgEComSessionInfoCtxReset(pCtx); + } + + if (pageIndex == 0) + { + pCtx->nrPages = nrPages; + } + + if (pCtx->pageIndex == pageIndex) + { + size_t newSize; + + newSize = pCtx->length + size; + + // + // Make sure there's at least one available slot for the null-terminating byte. + // + if (newSize < sizeof(pCtx->string)) + { + memcpy(&pCtx->string[pCtx->length], pBuffer, size); + pCtx->string[newSize] = '\0'; + + pCtx->length = newSize; + pCtx->pageIndex++; + + if (pCtx->pageIndex == pCtx->nrPages) + { + errorCode = SBG_NO_ERROR; + } + } + else + { + SBG_LOG_ERROR(SBG_BUFFER_OVERFLOW, "session information too large"); + + sbgEComSessionInfoCtxReset(pCtx); + } + } + + return errorCode; +} + +const char *sbgEComSessionInfoCtxGetString(const SbgEComSessionInfoCtx *pCtx) +{ + const char *pString = NULL; + + assert(pCtx); + + if ((pCtx->nrPages != 0) && (pCtx->pageIndex == pCtx->nrPages)) + { + pString = pCtx->string; + } + + return pString; +} diff --git a/src/sessionInfo/sbgEComSessionInfo.h b/src/sessionInfo/sbgEComSessionInfo.h new file mode 100644 index 0000000..d093af0 --- /dev/null +++ b/src/sessionInfo/sbgEComSessionInfo.h @@ -0,0 +1,120 @@ +/*! + * \file sbgEComSessionInfo.h + * \ingroup sessionInfo + * \author SBG Systems + * \date December 20, 2023 + * + * \brief Session information management. + * + * Session information, including device information and current settings, may + * be sent regularly by a device. This module handles the reassembly of that + * information for the convenience of the user. + * + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * 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. + * + * \endlicense + */ + +/*! + * \defgroup sessionInfo Session information management + * \brief Provides a convenient way to reassembly and work with session information. + */ + +#ifndef SBG_ECOM_SESSION_INFO_H +#define SBG_ECOM_SESSION_INFO_H + +// sbgCommonLib headers +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Constant definitions -// +//----------------------------------------------------------------------// + +/*! + * Session information buffer size, in bytes. + */ +#define SBG_ECOM_SESSION_INFO_BUFFER_SIZE (32768) + +//----------------------------------------------------------------------// +//- Structure definitions -// +//----------------------------------------------------------------------// + +/*! + * Session information context. + * + * String lengths do not include the null-terminating byte. + */ +typedef struct _SbgEComSessionInfoCtx +{ + char string[SBG_ECOM_SESSION_INFO_BUFFER_SIZE]; /*!< Session information string. */ + size_t length; /*!< Session information string length, in bytes. */ + uint16_t pageIndex; /*!< Page index. */ + uint16_t nrPages; /*!< Total number of pages. */ +} SbgEComSessionInfoCtx; + +//----------------------------------------------------------------------// +//- Public functions -// +//----------------------------------------------------------------------// + +/*! + * Session information context constructor. + * + * On return, the session information string of the context is empty. + * + * \param[in] pCtx Session information context. + */ +void sbgEComSessionInfoCtxConstruct(SbgEComSessionInfoCtx *pCtx); + +/*! + * Process the content of a session information log. + * + * If successful, the session information string remains valid until the next process operation. + * + * \param[in] pCtx Session information context. + * \param[in] pageIndex Page index. + * \param[in] nrPages Total number of pages. + * \param[in] pBuffer Buffer. + * \param[in] size Buffer size, in bytes. + * \return SBG_NO_ERROR if successful, i.e. the session information string has been reassembled, + * SBG_NOT_READY if the session information string is not complete. + */ +SbgErrorCode sbgEComSessionInfoCtxProcess(SbgEComSessionInfoCtx *pCtx, uint16_t pageIndex, uint16_t nrPages, const void *pBuffer, size_t size); + +/*! + * Get the string of a session information context. + * + * If valid, the session information string remains so until the next process operation. + * + * \param[in] pCtx Session information context. + * \return Session information string, NULL if not complete. + */ +const char *sbgEComSessionInfoCtxGetString(const SbgEComSessionInfoCtx *pCtx); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_SESSION_INFO_H diff --git a/src/transfer/sbgEComTransfer.c b/src/transfer/sbgEComTransfer.c index 8212ac5..139a368 100644 --- a/src/transfer/sbgEComTransfer.c +++ b/src/transfer/sbgEComTransfer.c @@ -16,441 +16,441 @@ /*! * 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. + * \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[6]; - uint32_t trial; - - 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 (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // 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; + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgStreamBuffer streamBuffer; + uint8_t outputBuffer[6]; + uint32_t trial; + + 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 (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // 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. + * \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_TRANSFER_PACKET_SIZE+6]; - uint32_t trial; - - 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 (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // 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; + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgStreamBuffer streamBuffer; + uint8_t outputBuffer[SBG_ECOM_TRANSFER_PACKET_SIZE+6]; + uint32_t trial; + + 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 (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // 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. + * \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 outputStream; - uint8_t outputBuffer[2]; - uint32_t trial; - - assert(pHandle); - - // - // Build payload, only a SBG_ECOM_TRANSFER_END cmd - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - sbgStreamBufferWriteUint16LE(&outputStream, SBG_ECOM_TRANSFER_END); - - // - // Send command (multiple times in case of failures) - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send upload end payload encapsulated in a ECom protocol frame - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, msgClass, msg, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); - - 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; + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgStreamBuffer outputStream; + uint8_t outputBuffer[2]; + uint32_t trial; + + assert(pHandle); + + // + // Build payload, only a SBG_ECOM_TRANSFER_END cmd + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + sbgStreamBufferWriteUint16LE(&outputStream, SBG_ECOM_TRANSFER_END); + + // + // Send command (multiple times in case of failures) + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send upload end payload encapsulated in a ECom protocol frame + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, msgClass, msg, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + 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. + * \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_ERROR; - SbgEComProtocolPayload receivedPayload; - SbgStreamBuffer outputStream; - uint8_t outputBuffer[2]; - uint16_t transferCmd; - size_t transferSize; - uint32_t trial; - - assert(pHandle); - - sbgEComProtocolPayloadConstruct(&receivedPayload); - - // - // Build payload, only a SBG_ECOM_TRANSFER_START cmd - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - sbgStreamBufferWriteUint16LE(&outputStream, SBG_ECOM_TRANSFER_START); - - // - // Send command (multiple times in case of failures) - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send transfer payload encapsulated in an ECom protocol frame - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, msgClass, msg, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); - - if (errorCode == SBG_NO_ERROR) - { - // - // Wait for response, 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 = sbgEComReceiveCmd2(pHandle, msgClass, msg, &receivedPayload, pHandle->cmdDefaultTimeOut); - - if (errorCode == SBG_NO_ERROR) - { - SbgStreamBuffer inputStream; - - // - // Init stream buffer on received payload to process it - // - sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); - - // - // Retrieve parameters, the first one is the transfer command - // The second one is the total transfer size - // - transferCmd = sbgStreamBufferReadUint16LE(&inputStream); - transferSize = sbgStreamBufferReadSizeT32LE(&inputStream); - - // - // 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 if (errorCode != SBG_TIME_OUT) - { - SBG_LOG_ERROR(errorCode, "Invalid answer received"); - } - else - { - SBG_LOG_ERROR(errorCode, "No response received"); - } - } - else - { - SBG_LOG_ERROR(errorCode, "Unable to send the command"); - } - } - - sbgEComProtocolPayloadDestroy(&receivedPayload); - - return errorCode; + SbgErrorCode errorCode = SBG_ERROR; + SbgEComProtocolPayload receivedPayload; + SbgStreamBuffer outputStream; + uint8_t outputBuffer[2]; + uint16_t transferCmd; + size_t transferSize; + uint32_t trial; + + assert(pHandle); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Build payload, only a SBG_ECOM_TRANSFER_START cmd + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + sbgStreamBufferWriteUint16LE(&outputStream, SBG_ECOM_TRANSFER_START); + + // + // Send command (multiple times in case of failures) + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send transfer payload encapsulated in an ECom protocol frame + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, msgClass, msg, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + if (errorCode == SBG_NO_ERROR) + { + // + // Wait for response, 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 = sbgEComReceiveCmd2(pHandle, msgClass, msg, &receivedPayload, pHandle->cmdDefaultTimeOut); + + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Init stream buffer on received payload to process it + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // Retrieve parameters, the first one is the transfer command + // The second one is the total transfer size + // + transferCmd = sbgStreamBufferReadUint16LE(&inputStream); + transferSize = sbgStreamBufferReadSizeT32LE(&inputStream); + + // + // 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 if (errorCode != SBG_TIME_OUT) + { + SBG_LOG_ERROR(errorCode, "Invalid answer received"); + } + else + { + SBG_LOG_ERROR(errorCode, "No response received"); + } + } + else + { + SBG_LOG_ERROR(errorCode, "Unable to send the command"); + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + 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. + * \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; - SbgEComProtocolPayload receivedPayload; - SbgStreamBuffer outputStream; - uint8_t outputBuffer[10]; - uint32_t trial; - - assert(pHandle); - assert(pBuffer); - assert(packetSize > 0); - - sbgEComProtocolPayloadConstruct(&receivedPayload); - - // - // 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 - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - sbgStreamBufferWriteUint16LE(&outputStream, SBG_ECOM_TRANSFER_DATA); - sbgStreamBufferWriteSizeT32LE(&outputStream, offset); - sbgStreamBufferWriteSizeT32LE(&outputStream, packetSize); - - // - // Send command (multiple times in case of failures) - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send transfer payload encapsulated in an ECom protocol frame - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, msgClass, msg, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); - - if (errorCode == SBG_NO_ERROR) - { - // - // Wait for response, 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 = sbgEComReceiveCmd2(pHandle, msgClass, msg, &receivedPayload, pHandle->cmdDefaultTimeOut); - - if (errorCode == SBG_NO_ERROR) - { - SbgStreamBuffer inputStream; - uint16_t transferCmd; - size_t rcvdOffset; - - // - // Initialize stream buffer for read on input buffer - // - sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); - - // - // Read response fields, first is the transfer command, second is the offset - // - transferCmd = sbgStreamBufferReadUint16LE(&inputStream); - rcvdOffset = sbgStreamBufferReadSizeT32LE(&inputStream); - - // - // 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 == (sbgEComProtocolPayloadGetSize(&receivedPayload) - (sizeof(uint16_t) + sizeof(uint32_t)))) ) - { - // - // Read all the buffer - // - sbgStreamBufferReadBuffer(&inputStream, pBuffer, sbgEComProtocolPayloadGetSize(&receivedPayload) - (sizeof(uint16_t) + sizeof(uint32_t))); - - // - // No need for other trials, exit loop - // - break; - } - } - } - } - - sbgEComProtocolPayloadDestroy(&receivedPayload); - - return errorCode; + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + SbgStreamBuffer outputStream; + uint8_t outputBuffer[10]; + uint32_t trial; + + assert(pHandle); + assert(pBuffer); + assert(packetSize > 0); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // 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 + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + sbgStreamBufferWriteUint16LE(&outputStream, SBG_ECOM_TRANSFER_DATA); + sbgStreamBufferWriteSizeT32LE(&outputStream, offset); + sbgStreamBufferWriteSizeT32LE(&outputStream, packetSize); + + // + // Send command (multiple times in case of failures) + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send transfer payload encapsulated in an ECom protocol frame + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, msgClass, msg, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + if (errorCode == SBG_NO_ERROR) + { + // + // Wait for response, 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 = sbgEComReceiveCmd2(pHandle, msgClass, msg, &receivedPayload, pHandle->cmdDefaultTimeOut); + + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + uint16_t transferCmd; + size_t rcvdOffset; + + // + // Initialize stream buffer for read on input buffer + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // Read response fields, first is the transfer command, second is the offset + // + transferCmd = sbgStreamBufferReadUint16LE(&inputStream); + rcvdOffset = sbgStreamBufferReadSizeT32LE(&inputStream); + + // + // 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 == (sbgEComProtocolPayloadGetSize(&receivedPayload) - (sizeof(uint16_t) + sizeof(uint32_t)))) ) + { + // + // Read all the buffer + // + sbgStreamBufferReadBuffer(&inputStream, pBuffer, sbgEComProtocolPayloadGetSize(&receivedPayload) - (sizeof(uint16_t) + sizeof(uint32_t))); + + // + // No need for other trials, exit loop + // + break; + } + } + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + 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. + * \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 outputStream; - uint8_t outputBuffer[2]; - uint32_t trial; - - assert(pHandle); - - // - // Build payload, only a SBG_ECOM_TRANSFER_END cmd - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - sbgStreamBufferWriteUint16LE(&outputStream, SBG_ECOM_TRANSFER_END); - - // - // Send command (multiple times in case of failures) - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send upload end payload encapsulated in a ECom protocol frame - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, msgClass, msg, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); - - 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; + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgStreamBuffer outputStream; + uint8_t outputBuffer[2]; + uint32_t trial; + + assert(pHandle); + + // + // Build payload, only a SBG_ECOM_TRANSFER_END cmd + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + sbgStreamBufferWriteUint16LE(&outputStream, SBG_ECOM_TRANSFER_END); + + // + // Send command (multiple times in case of failures) + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send upload end payload encapsulated in a ECom protocol frame + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, msgClass, msg, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + 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; } //----------------------------------------------------------------------// @@ -459,163 +459,163 @@ static SbgErrorCode sbgEComTransferReceiveEnd(SbgEComHandle *pHandle, uint8_t ms 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; + 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; } 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; + 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/src/transfer/sbgEComTransfer.h b/src/transfer/sbgEComTransfer.h index 76184d4..a8b91d8 100644 --- a/src/transfer/sbgEComTransfer.h +++ b/src/transfer/sbgEComTransfer.h @@ -1,13 +1,13 @@ /*! - * \file sbgEComTransfer.h - * \ingroup protocol - * \author SBG Systems - * \date 19 November 2013 + * \file sbgEComTransfer.h + * \ingroup protocol + * \author SBG Systems + * \date 19 November 2013 * - * \brief Handle large send/receive transfer for specific ECom Protocol commands. + * \brief Handle large send/receive transfer for specific ECom Protocol commands. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense The MIT license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -47,8 +47,8 @@ extern "C" { //- 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 (512u) /*!< Max packet size transmitted in a single frame */ +#define SBG_ECOM_TRANSFER_MAX_SIZE (8192u) /*!< Maximum buffer that can be transmitted using the sbgECom transfer protocol. */ +#define SBG_ECOM_TRANSFER_PACKET_SIZE (512u) /*!< Max packet size transmitted in a single frame */ //----------------------------------------------------------------------// //- Communication protocol struct and definitions -// @@ -59,9 +59,9 @@ extern "C" { */ 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. */ + 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; //----------------------------------------------------------------------// @@ -71,25 +71,25 @@ typedef enum _SbgEComTransferCmd /*! * 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. + * \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] bufferSize The size of the buffer. - * \return SBG_NO_ERROR in case of a successful download. + * \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] bufferSize 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); diff --git a/tools/sbgBasicLogger/README.md b/tools/sbgBasicLogger/README.md index 1b15887..01af85f 100644 --- a/tools/sbgBasicLogger/README.md +++ b/tools/sbgBasicLogger/README.md @@ -33,14 +33,15 @@ The sbgBasicLogger can be used to parse incoming data from a serial or an Ethern You can also select an binary file containing raw sbgECom dump and easily export sbgECom data to CSV like text files. ## UTC Time & Timestamp -All sbgECom logs output the internal IMU/AHRS/INS time stamp in microseconds. +All sbgECom logs output the internal IMU/AHRS/INS timestamp in microseconds. This is the default time format used by the sbgBasicLogger. -However, if the INS receives valid GNSS time information, the sbgBasicLogger is able to output data referenced to UTC time. -You can select this mode with the `--time-mode=utcIso8601` option. +However, if the INS receives valid GNSS time information, the sbgBasicLogger is able to output data referenced to UTC time. +To enable this, use the `--time-mode=utcIso8601` option, which produces timestamps in the format: `YYYY-MM-DDTHH:MM:SS.SSSSSSZ`. -In this mode, before the INS has a valid UTC time, the internal timestamp in microseconds is output then an ISO 8601 is used. -You can skip logs before a valid UTC time is available with the option `--discard-invalid-time `. +In this mode, before the INS acquires a valid UTC time, the logger will output the internal timestamp in microseconds. +Once the UTC time is available and synchronized, it switches to the ISO 8601 format. +To exclude logs recorded before a valid UTC time is available, use the option `--discard-invalid-time `. # Usage diff --git a/tools/sbgBasicLogger/src/loggerApp.cpp b/tools/sbgBasicLogger/src/loggerApp.cpp index 45ed226..01b1e70 100644 --- a/tools/sbgBasicLogger/src/loggerApp.cpp +++ b/tools/sbgBasicLogger/src/loggerApp.cpp @@ -90,6 +90,7 @@ void CLoggerApp::createLogger(const CLoggerSettings &settings) m_manager->registerLog(); m_manager->registerLog(); m_manager->registerLog(); + m_manager->registerLog(); m_manager->registerLog(); // diff --git a/tools/sbgBasicLogger/src/loggerApp.h b/tools/sbgBasicLogger/src/loggerApp.h index 0ba2e41..ff23fef 100644 --- a/tools/sbgBasicLogger/src/loggerApp.h +++ b/tools/sbgBasicLogger/src/loggerApp.h @@ -1,12 +1,12 @@ /*! - * \file loggerApp.h - * \author SBG Systems - * \date March 10, 2023 + * \file loggerApp.h + * \author SBG Systems + * \date March 10, 2023 * - * \brief Basic Logger main application entry point. + * \brief Basic Logger main application entry point. * - * \copyright Copyright (C) 2023, SBG Systems SAS. All rights reserved. - * \beginlicense Proprietary license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense Proprietary license * * This source code is intended for use only by SBG Systems SAS and * those that have explicit written permission to use it from @@ -41,79 +41,79 @@ namespace sbg { - /*! - * Application entry point. - */ - class CLoggerApp - { - private: - std::unique_ptr m_manager; /*!< Logger manager instance. */ - std::function m_continueCb; /*!< Optional callback used to either continue or stop the process. */ + /*! + * Application entry point. + */ + class CLoggerApp + { + private: + std::unique_ptr m_manager; /*!< Logger manager instance. */ + std::function m_continueCb; /*!< Optional callback used to either continue or stop the process. */ - //----------------------------------------------------------------------// - //- Private methods -// - //----------------------------------------------------------------------// - - /*! - * Returns the application name. - * - * \return application name as a read only string. - */ - std::string getApplicationName() const; + //----------------------------------------------------------------------// + //- Private methods -// + //----------------------------------------------------------------------// + + /*! + * Returns the application name. + * + * \return application name as a read only string. + */ + std::string getApplicationName() const; - /*! - * Instantiate the logger and register all log handlers. - * - * \param[in] settings Logger settings to use. - */ - void createLogger(const CLoggerSettings &settings); + /*! + * Instantiate the logger and register all log handlers. + * + * \param[in] settings Logger settings to use. + */ + void createLogger(const CLoggerSettings &settings); - /*! - * Run the logger until interface EOF is reached or user request to exit. - */ - void process(); + /*! + * Run the logger until interface EOF is reached or user request to exit. + */ + void process(); - /*! - * Parse command line arguments and returns a filled settings instance. - * - * \param[in] argc Number of input arguments. - * \param[in] argv Input arguments as an array of strings. - * \throw std::invalid_argument if input command line argument are invalid. - * std::bad_alloc if arg3 malloc failed. - */ - CLoggerSettings processArgs(int argc, char **argv); + /*! + * Parse command line arguments and returns a filled settings instance. + * + * \param[in] argc Number of input arguments. + * \param[in] argv Input arguments as an array of strings. + * \throw std::invalid_argument if input command line argument are invalid. + * std::bad_alloc if arg3 malloc failed. + */ + CLoggerSettings processArgs(int argc, char **argv); - public: - //----------------------------------------------------------------------// - //- Public methods -// - //----------------------------------------------------------------------// + public: + //----------------------------------------------------------------------// + //- Public methods -// + //----------------------------------------------------------------------// - /*! - * Define an optional callback used to continue or stop the processing. - * - * The function is called periodically from the application processing loop. - * - * If the callback returns true, the processing continues normally. - * If the callback returns false, the processing stops and program flow returns from exec(). - * - * Please keep in mind this callback belongs to the same thread as exec(). - * - * \param[in] continueCb optional and user provided continue function to call. - */ - void setContinueCallback(std::function continueCb) noexcept; + /*! + * Define an optional callback used to continue or stop the processing. + * + * The function is called periodically from the application processing loop. + * + * If the callback returns true, the processing continues normally. + * If the callback returns false, the processing stops and program flow returns from exec(). + * + * Please keep in mind this callback belongs to the same thread as exec(). + * + * \param[in] continueCb optional and user provided continue function to call. + */ + void setContinueCallback(std::function continueCb) noexcept; - /*! - * Execute the console application and returns once processing is done or stoped. - * - * The processing can be cancelled at anytime by registering a function with setContinueCallback - * - * \param[in] argc Number of input arguments. - * \param[in] argv Input arguments as an array of strings. - * \return EXIT_SUCCESS if successful. - * EXIT_FAILURE if an error has occurred. - */ - int exec(int argc, char **argv) noexcept; - }; + /*! + * Execute the console application and returns once processing is done or stoped. + * + * The processing can be cancelled at anytime by registering a function with setContinueCallback + * + * \param[in] argc Number of input arguments. + * \param[in] argv Input arguments as an array of strings. + * \return EXIT_SUCCESS if successful. + * EXIT_FAILURE if an error has occurred. + */ + int exec(int argc, char **argv) noexcept; + }; } #endif // SBG_LOGGER_APP_H diff --git a/tools/sbgBasicLogger/src/loggerEntry/helpers/imuDataMean.h b/tools/sbgBasicLogger/src/loggerEntry/helpers/imuDataMean.h index 34387ab..2b2e162 100644 --- a/tools/sbgBasicLogger/src/loggerEntry/helpers/imuDataMean.h +++ b/tools/sbgBasicLogger/src/loggerEntry/helpers/imuDataMean.h @@ -1,12 +1,12 @@ /*! - * \file imuDataMean.h - * \author SBG Systems - * \date March 14, 2023 + * \file imuDataMean.h + * \author SBG Systems + * \date March 14, 2023 * - * \brief Implemented simple moving average filter for IMU data entries. + * \brief Implemented simple moving average filter for IMU data entries. * - * \copyright Copyright (C) 2023, SBG Systems SAS. All rights reserved. - * \beginlicense Proprietary license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense Proprietary license * * This source code is intended for use only by SBG Systems SAS and * those that have explicit written permission to use it from @@ -34,106 +34,106 @@ namespace sbg { - //----------------------------------------------------------------------// - //- CImuDataMean -// - //----------------------------------------------------------------------// + //----------------------------------------------------------------------// + //- CImuDataMean -// + //----------------------------------------------------------------------// - /*! - * Helper used to accumulate and decimate IMU data. - * The timestamp represents the latest added measurement. - * The status represents the OR combinaison of all accumulated status. - */ - class CImuDataMean - { - public: + /*! + * Helper used to accumulate and decimate IMU data. + * The timestamp represents the latest added measurement. + * The status represents the OR combinaison of all accumulated status. + */ + class CImuDataMean + { + public: - /*! - * Default constructor. - */ - CImuDataMean(); - - /*! - * Accumulate a IMU measurement and return the number of accumulated values. - * - * \param[in] imuData IMU data to accumulate. - * \return Number of accumulated data so far. - */ - int32_t add(const SbgEComLogImuLegacy &imuData); - - /*! - * Accumulate a IMU measurement and return the number of accumulated values. - * - * \param[in] imuData IMU data to accumulate. - * \return Number of accumulated data so far. - */ - int32_t add(const SbgEComLogImuShort &imuData); - - /*! - * Accumulate a IMU measurement and return the number of accumulated values. - * - * \param[in] imuData IMU data to accumulate. - * \return Number of accumulated data so far. - */ - int32_t add(const SbgEComLogImuFastLegacy &imuData); + /*! + * Default constructor. + */ + CImuDataMean(); + + /*! + * Accumulate a IMU measurement and return the number of accumulated values. + * + * \param[in] imuData IMU data to accumulate. + * \return Number of accumulated data so far. + */ + int32_t add(const SbgEComLogImuLegacy &imuData); + + /*! + * Accumulate a IMU measurement and return the number of accumulated values. + * + * \param[in] imuData IMU data to accumulate. + * \return Number of accumulated data so far. + */ + int32_t add(const SbgEComLogImuShort &imuData); + + /*! + * Accumulate a IMU measurement and return the number of accumulated values. + * + * \param[in] imuData IMU data to accumulate. + * \return Number of accumulated data so far. + */ + int32_t add(const SbgEComLogImuFastLegacy &imuData); - /*! - * Reset the accumulators. - */ - void reset(); - - /*! - * Returns the latest time stamp value. - * - * \return timestamp in us. - */ - uint32_t getTimeStamp() const; - - /*! - * Returns the accumulated status. - * - * \return status bitmask. - */ - uint32_t getStatus() const; - - /*! - * Returns the mean accelerometer value. - * - * \param[in] idx Axis index from 0 to 2. - * \return Mean accelerometer value in m.s^-2. - */ - double getAccelerometer(size_t idx) const; - - /*! - * Returns the mean rotation rate value. - * - * \param[in] idx Axis index from 0 to 2. - * \return Mean gyroscope value in rad.s^-1. - */ - double getGyroscope(size_t idx) const; + /*! + * Reset the accumulators. + */ + void reset(); + + /*! + * Returns the latest timestamp value. + * + * \return timestamp in us. + */ + uint32_t getTimeStamp() const; + + /*! + * Returns the accumulated status. + * + * \return status bitmask. + */ + uint32_t getStatus() const; + + /*! + * Returns the mean accelerometer value. + * + * \param[in] idx Axis index from 0 to 2. + * \return Mean accelerometer value in m.s^-2. + */ + double getAccelerometer(size_t idx) const; + + /*! + * Returns the mean rotation rate value. + * + * \param[in] idx Axis index from 0 to 2. + * \return Mean gyroscope value in rad.s^-1. + */ + double getGyroscope(size_t idx) const; - /*! - * Returns the mean rotation rate value. - * - * \param[in] idx Axis index from 0 to 2. - * \return Mean gyroscope value in deg.s^-1. - */ - double getGyroscopeDeg(size_t idx) const; + /*! + * Returns the mean rotation rate value. + * + * \param[in] idx Axis index from 0 to 2. + * \return Mean gyroscope value in deg.s^-1. + */ + double getGyroscopeDeg(size_t idx) const; - /*! - * Returns the mean temperature value. - * - * \return Mean temperature in degC. - */ - double getTemperature() const; + /*! + * Returns the mean temperature value. + * + * \return Mean temperature in degC. + */ + double getTemperature() const; - private: - uint32_t m_timeStamp; /*!< Latest added measurement timestamp in us. */ - uint32_t m_status; /*!< Status bitmask. */ - std::array m_accelerometer; /*!< X,Y,Z accelerometer accumulator in m.s^-2 */ - std::array m_gyroscope; /*!< X,y,Z rotation rate accumulator in rad.s^-1 */ - double m_temperature; /*!< Temperature accumulator in degC. */ - int32_t m_numAccValues; /*!< Number of accumulated values. */ - }; + private: + uint32_t m_timeStamp; /*!< Latest added measurement timestamp in us. */ + uint32_t m_status; /*!< Status bitmask. */ + std::array m_accelerometer; /*!< X,Y,Z accelerometer accumulator in m.s^-2 */ + std::array m_gyroscope; /*!< X,y,Z rotation rate accumulator in rad.s^-1 */ + double m_temperature; /*!< Temperature accumulator in degC. */ + int32_t m_numAccValues; /*!< Number of accumulated values. */ + }; }; #endif // SBG_IMU_DATA_MEAN_H diff --git a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryAidings.h b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryAidings.h index 0a8eee6..e628442 100644 --- a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryAidings.h +++ b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryAidings.h @@ -1,12 +1,12 @@ /*! - * \file loggerEntryAidings.h - * \author SBG Systems - * \date March 14, 2023 + * \file loggerEntryAidings.h + * \author SBG Systems + * \date March 14, 2023 * - * \brief Implement odometer velocity (DMI) aiding + * \brief Implement odometer velocity (DMI) aiding * - * \copyright Copyright (C) 2023, SBG Systems SAS. All rights reserved. - * \beginlicense Proprietary license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense Proprietary license * * This source code is intended for use only by SBG Systems SAS and * those that have explicit written permission to use it from @@ -35,197 +35,197 @@ namespace sbg { - /*! - * Handle SBG_ECOM_LOG_ODO_VEL - */ - class CLoggerEntryOdometer : public ILoggerEntry, public ILoggerEntryKey - { - public: - - //----------------------------------------------------------------------// - //- Public getters -// - //----------------------------------------------------------------------// - - /*! - * Returns the log name. - * - * \return log name. - */ - std::string getName() const override; - - private: - - //----------------------------------------------------------------------// - //- Private methods -// - //----------------------------------------------------------------------// - - /*! - * Write the header to the file and/or console. - * - * \param[in] context Logger context and settings. - */ - void writeHeaderToFile(const CLoggerContext &context) override; - - /*! - * Write the data log to the file - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - - /*! - * Write the data log to the console - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - }; - - /*! - * Handle SBG_ECOM_LOG_AIR_DATA - */ - class CLoggerEntryAirData : public ILoggerEntry, public ILoggerEntryKey - { - public: - - //----------------------------------------------------------------------// - //- Public getters -// - //----------------------------------------------------------------------// - - /*! - * Returns the log name. - * - * \return log name. - */ - std::string getName() const override; - - private: - - //----------------------------------------------------------------------// - //- Private methods -// - //----------------------------------------------------------------------// - - /*! - * Write the header to the file and/or console. - * - * \param[in] context Logger context and settings. - */ - void writeHeaderToFile(const CLoggerContext &context) override; - - /*! - * Write the data log to the file - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - - /*! - * Write the data log to the console - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - }; - - /*! - * Handle SBG_ECOM_LOG_DEPTH - */ - class CLoggerEntryDepth : public ILoggerEntry, public ILoggerEntryKey - { - public: - - //----------------------------------------------------------------------// - //- Public getters -// - //----------------------------------------------------------------------// - - /*! - * Returns the log name. - * - * \return log name. - */ - std::string getName() const override; - - private: - - //----------------------------------------------------------------------// - //- Private methods -// - //----------------------------------------------------------------------// - - /*! - * Write the header to the file and/or console. - * - * \param[in] context Logger context and settings. - */ - void writeHeaderToFile(const CLoggerContext &context) override; - - /*! - * Write the data log to the file - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - - /*! - * Write the data log to the console - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - }; - - /*! - * Handle SBG_ECOM_LOG_USBL - */ - class CLoggerEntryUsbl : public ILoggerEntry, public ILoggerEntryKey - { - public: - - //----------------------------------------------------------------------// - //- Public getters -// - //----------------------------------------------------------------------// - - /*! - * Returns the log name. - * - * \return log name. - */ - std::string getName() const override; - - private: - - //----------------------------------------------------------------------// - //- Private methods -// - //----------------------------------------------------------------------// - - /*! - * Write the header to the file and/or console. - * - * \param[in] context Logger context and settings. - */ - void writeHeaderToFile(const CLoggerContext &context) override; - - /*! - * Write the data log to the file - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - - /*! - * Write the data log to the console - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - }; + /*! + * Handle SBG_ECOM_LOG_ODO_VEL + */ + class CLoggerEntryOdometer : public ILoggerEntry, public ILoggerEntryKey + { + public: + + //----------------------------------------------------------------------// + //- Public getters -// + //----------------------------------------------------------------------// + + /*! + * Returns the log name. + * + * \return log name. + */ + std::string getName() const override; + + private: + + //----------------------------------------------------------------------// + //- Private methods -// + //----------------------------------------------------------------------// + + /*! + * Write the header to the file and/or console. + * + * \param[in] context Logger context and settings. + */ + void writeHeaderToFile(const CLoggerContext &context) override; + + /*! + * Write the data log to the file + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + + /*! + * Write the data log to the console + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + }; + + /*! + * Handle SBG_ECOM_LOG_AIR_DATA + */ + class CLoggerEntryAirData : public ILoggerEntry, public ILoggerEntryKey + { + public: + + //----------------------------------------------------------------------// + //- Public getters -// + //----------------------------------------------------------------------// + + /*! + * Returns the log name. + * + * \return log name. + */ + std::string getName() const override; + + private: + + //----------------------------------------------------------------------// + //- Private methods -// + //----------------------------------------------------------------------// + + /*! + * Write the header to the file and/or console. + * + * \param[in] context Logger context and settings. + */ + void writeHeaderToFile(const CLoggerContext &context) override; + + /*! + * Write the data log to the file + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + + /*! + * Write the data log to the console + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + }; + + /*! + * Handle SBG_ECOM_LOG_DEPTH + */ + class CLoggerEntryDepth : public ILoggerEntry, public ILoggerEntryKey + { + public: + + //----------------------------------------------------------------------// + //- Public getters -// + //----------------------------------------------------------------------// + + /*! + * Returns the log name. + * + * \return log name. + */ + std::string getName() const override; + + private: + + //----------------------------------------------------------------------// + //- Private methods -// + //----------------------------------------------------------------------// + + /*! + * Write the header to the file and/or console. + * + * \param[in] context Logger context and settings. + */ + void writeHeaderToFile(const CLoggerContext &context) override; + + /*! + * Write the data log to the file + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + + /*! + * Write the data log to the console + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + }; + + /*! + * Handle SBG_ECOM_LOG_USBL + */ + class CLoggerEntryUsbl : public ILoggerEntry, public ILoggerEntryKey + { + public: + + //----------------------------------------------------------------------// + //- Public getters -// + //----------------------------------------------------------------------// + + /*! + * Returns the log name. + * + * \return log name. + */ + std::string getName() const override; + + private: + + //----------------------------------------------------------------------// + //- Private methods -// + //----------------------------------------------------------------------// + + /*! + * Write the header to the file and/or console. + * + * \param[in] context Logger context and settings. + */ + void writeHeaderToFile(const CLoggerContext &context) override; + + /*! + * Write the data log to the file + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + + /*! + * Write the data log to the console + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + }; }; #endif // SBG_LOGGER_ENTRY_AIDINGS_H diff --git a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryDvl.h b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryDvl.h index 1fd57a5..13b3ee9 100644 --- a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryDvl.h +++ b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryDvl.h @@ -1,12 +1,12 @@ /*! - * \file loggerEntryDvl.h - * \author SBG Systems - * \date March 14, 2023 + * \file loggerEntryDvl.h + * \author SBG Systems + * \date March 14, 2023 * - * \brief Implement DVL velocity aidings log handlers + * \brief Implement DVL velocity aidings log handlers * - * \copyright Copyright (C) 2023, SBG Systems SAS. All rights reserved. - * \beginlicense Proprietary license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense Proprietary license * * This source code is intended for use only by SBG Systems SAS and * those that have explicit written permission to use it from @@ -35,78 +35,78 @@ namespace sbg { - //----------------------------------------------------------------------// - //- DVL Generic Implementation -// - //----------------------------------------------------------------------// + //----------------------------------------------------------------------// + //- DVL Generic Implementation -// + //----------------------------------------------------------------------// - /*! - * Handle SBG_ECOM_LOG_DVL_# - */ - class CLoggerEntryDvl : public ILoggerEntry - { - private: - - //----------------------------------------------------------------------// - //- Private methods -// - //----------------------------------------------------------------------// + /*! + * Handle SBG_ECOM_LOG_DVL_# + */ + class CLoggerEntryDvl : public ILoggerEntry + { + private: + + //----------------------------------------------------------------------// + //- Private methods -// + //----------------------------------------------------------------------// - /*! - * Write the header to the file and/or console. - * - * \param[in] context Logger context and settings. - */ - void writeHeaderToFile(const CLoggerContext &context) override; + /*! + * Write the header to the file and/or console. + * + * \param[in] context Logger context and settings. + */ + void writeHeaderToFile(const CLoggerContext &context) override; - /*! - * Write the data log to the file - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + /*! + * Write the data log to the file + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - /*! - * Write the data log to the console - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - }; + /*! + * Write the data log to the console + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + }; - //----------------------------------------------------------------------// - //- DVL Specializations -// - //----------------------------------------------------------------------// + //----------------------------------------------------------------------// + //- DVL Specializations -// + //----------------------------------------------------------------------// - /*! - * Handle SBG_ECOM_LOG_DVL_BOTTOM_TRACK - */ - class CLoggerEntryDvlBottom : public CLoggerEntryDvl, public ILoggerEntryKey - { - public: + /*! + * Handle SBG_ECOM_LOG_DVL_BOTTOM_TRACK + */ + class CLoggerEntryDvlBottom : public CLoggerEntryDvl, public ILoggerEntryKey + { + public: - /*! - * Returns the log name. - * - * \return log name. - */ - std::string getName() const override; - }; + /*! + * Returns the log name. + * + * \return log name. + */ + std::string getName() const override; + }; - /*! - * Handle SBG_ECOM_LOG_DVL_WATER_TRACK - */ - class CLoggerEntryDvlWater : public CLoggerEntryDvl, public ILoggerEntryKey - { - public: + /*! + * Handle SBG_ECOM_LOG_DVL_WATER_TRACK + */ + class CLoggerEntryDvlWater : public CLoggerEntryDvl, public ILoggerEntryKey + { + public: - /*! - * Returns the log name. - * - * \return log name. - */ - std::string getName() const override; - }; + /*! + * Returns the log name. + * + * \return log name. + */ + std::string getName() const override; + }; }; #endif // SBG_LOGGER_ENTRY_DVL_H diff --git a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryEkf.cpp b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryEkf.cpp index e338491..709c5a7 100644 --- a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryEkf.cpp +++ b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryEkf.cpp @@ -29,8 +29,8 @@ std::string CLoggerEntryEkfEuler::getName() const void CLoggerEntryEkfEuler::writeHeaderToFile(const CLoggerContext &context) { - m_outFile << context.getTimeColTitle() << "\tstatus\troll\tpitch\tyaw\trollStd\tpitchStd\tyawStd\n"; - m_outFile << context.getTimeUnit() << "\t(na)\t(rad)\t(rad)\t(rad)\t(rad)\t(rad)\t(rad)\n"; + m_outFile << context.getTimeColTitle() << "\tstatus\troll\tpitch\tyaw\trollStd\tpitchStd\tyawStd\tmagHeading\tmagDecl\tmagIncl\n"; + m_outFile << context.getTimeUnit() << "\t(na)\t(deg)\t(deg)\t(deg)\t(deg)\t(deg)\t(deg)\t(deg)\t(deg)\t(deg)\n"; } void CLoggerEntryEkfEuler::writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) @@ -39,12 +39,15 @@ void CLoggerEntryEkfEuler::writeDataToFile(const CLoggerContext &context, const m_outFile << context.fmtTime(data.timeStamp) << "\t" << context.fmtStatus(data.status) << "\t" - << data.euler[0] << "\t" - << data.euler[1] << "\t" - << data.euler[2] << "\t" - << data.eulerStdDev[0] << "\t" - << data.eulerStdDev[1] << "\t" - << data.eulerStdDev[2] << "\n"; + << sbgRadToDegf(data.euler[0]) << "\t" + << sbgRadToDegf(data.euler[1]) << "\t" + << sbgRadToDegf(data.euler[2]) << "\t" + << sbgRadToDegf(data.eulerStdDev[0]) << "\t" + << sbgRadToDegf(data.eulerStdDev[1]) << "\t" + << sbgRadToDegf(data.eulerStdDev[2]) << "\t" + << sbgRadToDegf(sbgEComLogEkfEulerGetMagneticHeading(&data)) << "\t" + << sbgRadToDegf(data.magDeclination) << "\t" + << sbgRadToDegf(data.magInclination) << "\n"; } void CLoggerEntryEkfEuler::writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) @@ -53,15 +56,17 @@ void CLoggerEntryEkfEuler::writeDataToConsole(const CLoggerContext &context, con std::cout << std::setw(12) << getName() << ": " << std::setw(12) << context.fmtStatus(data.status) - << std::setw(12) << data.euler[0] - << std::setw(12) << data.euler[1] - << std::setw(12) << data.euler[2] - << std::setw(12) << data.eulerStdDev[0] - << std::setw(12) << data.eulerStdDev[1] - << std::setw(12) << data.eulerStdDev[2] << "\n"; + << std::setw(12) << sbgRadToDegf(data.euler[0]) + << std::setw(12) << sbgRadToDegf(data.euler[1]) + << std::setw(12) << sbgRadToDegf(data.euler[2]) + << std::setw(12) << sbgRadToDegf(data.eulerStdDev[0]) + << std::setw(12) << sbgRadToDegf(data.eulerStdDev[1]) + << std::setw(12) << sbgRadToDegf(data.eulerStdDev[2]) + << std::setw(12) << sbgRadToDegf(sbgEComLogEkfEulerGetMagneticHeading(&data)) + << std::setw(12) << sbgRadToDegf(data.magDeclination) + << std::setw(12) << sbgRadToDegf(data.magInclination) << "\n"; } - //----------------------------------------------------------------------// //- CLoggerEntryEkfQuat -// //----------------------------------------------------------------------// @@ -73,8 +78,8 @@ std::string CLoggerEntryEkfQuat::getName() const void CLoggerEntryEkfQuat::writeHeaderToFile(const CLoggerContext &context) { - m_outFile << context.getTimeColTitle() << "\tstatus\tqW\tqX\tqY\tqZ\trollStd\tpitchStd\tyawStd\n"; - m_outFile << context.getTimeUnit() << "\t(na)\t(au)\t(au)\t(au)\t(au)\t(rad)\t(rad)\t(rad)\n"; + m_outFile << context.getTimeColTitle() << "\tstatus\tqW\tqX\tqY\tqZ\trollStd\tpitchStd\tyawStd\tmagDecl\tmagIncl\n"; + m_outFile << context.getTimeUnit() << "\t(na)\t(au)\t(au)\t(au)\t(au)\t(deg)\t(deg)\t(deg)\t(deg)\t(deg)\n"; } void CLoggerEntryEkfQuat::writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) @@ -87,9 +92,11 @@ void CLoggerEntryEkfQuat::writeDataToFile(const CLoggerContext &context, const S << data.quaternion[1] << "\t" << data.quaternion[2] << "\t" << data.quaternion[3] << "\t" - << data.eulerStdDev[0] << "\t" - << data.eulerStdDev[1] << "\t" - << data.eulerStdDev[2] << "\n"; + << sbgRadToDegf(data.eulerStdDev[0]) << "\t" + << sbgRadToDegf(data.eulerStdDev[1]) << "\t" + << sbgRadToDegf(data.eulerStdDev[2]) << "\t" + << sbgRadToDegf(data.magDeclination) << "\t" + << sbgRadToDegf(data.magInclination) << "\n"; } void CLoggerEntryEkfQuat::writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) @@ -102,9 +109,11 @@ void CLoggerEntryEkfQuat::writeDataToConsole(const CLoggerContext &context, cons << std::setw(12) << data.quaternion[1] << std::setw(12) << data.quaternion[2] << std::setw(12) << data.quaternion[3] - << std::setw(12) << data.eulerStdDev[0] - << std::setw(12) << data.eulerStdDev[1] - << std::setw(12) << data.eulerStdDev[2] << "\n"; + << std::setw(12) << sbgRadToDegf(data.eulerStdDev[0]) + << std::setw(12) << sbgRadToDegf(data.eulerStdDev[1]) + << std::setw(12) << sbgRadToDegf(data.eulerStdDev[2]) + << std::setw(12) << sbgRadToDegf(data.magDeclination) + << std::setw(12) << sbgRadToDegf(data.magInclination) << "\n"; } //----------------------------------------------------------------------// diff --git a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryEkf.h b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryEkf.h index f3ae648..bb588f7 100644 --- a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryEkf.h +++ b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryEkf.h @@ -1,12 +1,12 @@ /*! - * \file loggerEntryEkf.h - * \author SBG Systems - * \date March 14, 2023 + * \file loggerEntryEkf.h + * \author SBG Systems + * \date March 14, 2023 * - * \brief Implemented INS output such as attitude & navigation logs. + * \brief Implemented INS output such as attitude & navigation logs. * - * \copyright Copyright (C) 2023, SBG Systems SAS. All rights reserved. - * \beginlicense Proprietary license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense Proprietary license * * This source code is intended for use only by SBG Systems SAS and * those that have explicit written permission to use it from @@ -35,198 +35,198 @@ namespace sbg { - /*! - * Handle SBG_ECOM_LOG_EKF_EULER - */ - class CLoggerEntryEkfEuler : public ILoggerEntry, public ILoggerEntryKey - { - public: - - //----------------------------------------------------------------------// - //- Public getters -// - //----------------------------------------------------------------------// - - /*! - * Returns the log name. - * - * \return log name. - */ - std::string getName() const override; - - private: - - //----------------------------------------------------------------------// - //- Private methods -// - //----------------------------------------------------------------------// - - /*! - * Write the header to the file and/or console. - * - * \param[in] context Logger context and settings. - */ - void writeHeaderToFile(const CLoggerContext &context) override; - - /*! - * Write the data log to the file - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - - /*! - * Write the data log to the console - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - }; - - /*! - * Handle SBG_ECOM_LOG_EKF_QUAT - */ - class CLoggerEntryEkfQuat : public ILoggerEntry, public ILoggerEntryKey - { - public: - - //----------------------------------------------------------------------// - //- Public getters -// - //----------------------------------------------------------------------// - - /*! - * Returns the log name. - * - * \return log name. - */ - std::string getName() const override; - - private: - - //----------------------------------------------------------------------// - //- Private methods -// - //----------------------------------------------------------------------// - - /*! - * Write the header to the file and/or console. - * - * \param[in] context Logger context and settings. - */ - void writeHeaderToFile(const CLoggerContext &context) override; - - /*! - * Write the data log to the file - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - - /*! - * Write the data log to the console - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - }; - - /*! - * Handle SBG_ECOM_LOG_EKF_NAV - */ - class CLoggerEntryEkfNav : public ILoggerEntry, public ILoggerEntryKey - { - public: - - //----------------------------------------------------------------------// - //- Public getters -// - //----------------------------------------------------------------------// - - /*! - * Returns the log name. - * - * \return log name. - */ - std::string getName() const override; - - private: - - //----------------------------------------------------------------------// - //- Private methods -// - //----------------------------------------------------------------------// - - /*! - * Write the header to the file and/or console. - * - * \param[in] context Logger context and settings. - */ - void writeHeaderToFile(const CLoggerContext &context) override; - - /*! - * Write the data log to the file - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - - /*! - * Write the data log to the console - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - }; - - /*! - * Handle SBG_ECOM_LOG_EKF_VEL_BODY - */ - class CLoggerEntryEkfVelBody : public ILoggerEntry, public ILoggerEntryKey - { - public: - - //----------------------------------------------------------------------// - //- Public getters -// - //----------------------------------------------------------------------// - - /*! - * Returns the log name. - * - * \return log name. - */ - std::string getName() const override; - - private: - - //----------------------------------------------------------------------// - //- Private methods -// - //----------------------------------------------------------------------// - - /*! - * Write the header to the file and/or console. - * - * \param[in] context Logger context and settings. - */ - void writeHeaderToFile(const CLoggerContext &context) override; - - /*! - * Write the data log to the file - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - - /*! - * Write the data log to the console - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - - }; + /*! + * Handle SBG_ECOM_LOG_EKF_EULER + */ + class CLoggerEntryEkfEuler : public ILoggerEntry, public ILoggerEntryKey + { + public: + + //----------------------------------------------------------------------// + //- Public getters -// + //----------------------------------------------------------------------// + + /*! + * Returns the log name. + * + * \return log name. + */ + std::string getName() const override; + + private: + + //----------------------------------------------------------------------// + //- Private methods -// + //----------------------------------------------------------------------// + + /*! + * Write the header to the file and/or console. + * + * \param[in] context Logger context and settings. + */ + void writeHeaderToFile(const CLoggerContext &context) override; + + /*! + * Write the data log to the file + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + + /*! + * Write the data log to the console + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + }; + + /*! + * Handle SBG_ECOM_LOG_EKF_QUAT + */ + class CLoggerEntryEkfQuat : public ILoggerEntry, public ILoggerEntryKey + { + public: + + //----------------------------------------------------------------------// + //- Public getters -// + //----------------------------------------------------------------------// + + /*! + * Returns the log name. + * + * \return log name. + */ + std::string getName() const override; + + private: + + //----------------------------------------------------------------------// + //- Private methods -// + //----------------------------------------------------------------------// + + /*! + * Write the header to the file and/or console. + * + * \param[in] context Logger context and settings. + */ + void writeHeaderToFile(const CLoggerContext &context) override; + + /*! + * Write the data log to the file + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + + /*! + * Write the data log to the console + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + }; + + /*! + * Handle SBG_ECOM_LOG_EKF_NAV + */ + class CLoggerEntryEkfNav : public ILoggerEntry, public ILoggerEntryKey + { + public: + + //----------------------------------------------------------------------// + //- Public getters -// + //----------------------------------------------------------------------// + + /*! + * Returns the log name. + * + * \return log name. + */ + std::string getName() const override; + + private: + + //----------------------------------------------------------------------// + //- Private methods -// + //----------------------------------------------------------------------// + + /*! + * Write the header to the file and/or console. + * + * \param[in] context Logger context and settings. + */ + void writeHeaderToFile(const CLoggerContext &context) override; + + /*! + * Write the data log to the file + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + + /*! + * Write the data log to the console + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + }; + + /*! + * Handle SBG_ECOM_LOG_EKF_VEL_BODY + */ + class CLoggerEntryEkfVelBody : public ILoggerEntry, public ILoggerEntryKey + { + public: + + //----------------------------------------------------------------------// + //- Public getters -// + //----------------------------------------------------------------------// + + /*! + * Returns the log name. + * + * \return log name. + */ + std::string getName() const override; + + private: + + //----------------------------------------------------------------------// + //- Private methods -// + //----------------------------------------------------------------------// + + /*! + * Write the header to the file and/or console. + * + * \param[in] context Logger context and settings. + */ + void writeHeaderToFile(const CLoggerContext &context) override; + + /*! + * Write the data log to the file + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + + /*! + * Write the data log to the console + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + + }; }; #endif // SBG_LOGGER_ENTRY_EKF_H diff --git a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryEkfRotAccel.h b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryEkfRotAccel.h index f05a5d0..20cd21b 100644 --- a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryEkfRotAccel.h +++ b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryEkfRotAccel.h @@ -1,12 +1,12 @@ /*! - * \file loggerEntryEkfRotAccel.h - * \author SBG Systems - * \date June 12, 2023 + * \file loggerEntryEkfRotAccel.h + * \author SBG Systems + * \date June 12, 2023 * - * \brief Implements body/NED rotation rate and accelerations. + * \brief Implements body/NED rotation rate and accelerations. * - * \copyright Copyright (C) 2023, SBG Systems SAS. All rights reserved. - * \beginlicense Proprietary license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense Proprietary license * * This source code is intended for use only by SBG Systems SAS and * those that have explicit written permission to use it from @@ -35,101 +35,101 @@ namespace sbg { - /*! - * Handle SBG_ECOM_LOG_EKF_ROT_ACCEL_BODY - */ - class CLoggerEntryEkfRotAccelBody : public ILoggerEntry, public ILoggerEntryKey - { - public: - - //----------------------------------------------------------------------// - //- Public getters -// - //----------------------------------------------------------------------// - - /*! - * Returns the log name. - * - * \return log name. - */ - std::string getName() const override; - - private: - - //----------------------------------------------------------------------// - //- Private methods -// - //----------------------------------------------------------------------// - - /*! - * Write the header to the file and/or console. - * - * \param[in] context Logger context and settings. - */ - void writeHeaderToFile(const CLoggerContext &context) override; - - /*! - * Write the data log to the file - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - - /*! - * Write the data log to the console - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - }; - - /*! - * Handle SBG_ECOM_LOG_EKF_ROT_ACCEL_NED - */ - class CLoggerEntryEkfRotAccelNed : public ILoggerEntry, public ILoggerEntryKey - { - public: - - //----------------------------------------------------------------------// - //- Public getters -// - //----------------------------------------------------------------------// - - /*! - * Returns the log name. - * - * \return log name. - */ - std::string getName() const override; - - private: - - //----------------------------------------------------------------------// - //- Private methods -// - //----------------------------------------------------------------------// - - /*! - * Write the header to the file and/or console. - * - * \param[in] context Logger context and settings. - */ - void writeHeaderToFile(const CLoggerContext &context) override; - - /*! - * Write the data log to the file - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - - /*! - * Write the data log to the console - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - }; + /*! + * Handle SBG_ECOM_LOG_EKF_ROT_ACCEL_BODY + */ + class CLoggerEntryEkfRotAccelBody : public ILoggerEntry, public ILoggerEntryKey + { + public: + + //----------------------------------------------------------------------// + //- Public getters -// + //----------------------------------------------------------------------// + + /*! + * Returns the log name. + * + * \return log name. + */ + std::string getName() const override; + + private: + + //----------------------------------------------------------------------// + //- Private methods -// + //----------------------------------------------------------------------// + + /*! + * Write the header to the file and/or console. + * + * \param[in] context Logger context and settings. + */ + void writeHeaderToFile(const CLoggerContext &context) override; + + /*! + * Write the data log to the file + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + + /*! + * Write the data log to the console + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + }; + + /*! + * Handle SBG_ECOM_LOG_EKF_ROT_ACCEL_NED + */ + class CLoggerEntryEkfRotAccelNed : public ILoggerEntry, public ILoggerEntryKey + { + public: + + //----------------------------------------------------------------------// + //- Public getters -// + //----------------------------------------------------------------------// + + /*! + * Returns the log name. + * + * \return log name. + */ + std::string getName() const override; + + private: + + //----------------------------------------------------------------------// + //- Private methods -// + //----------------------------------------------------------------------// + + /*! + * Write the header to the file and/or console. + * + * \param[in] context Logger context and settings. + */ + void writeHeaderToFile(const CLoggerContext &context) override; + + /*! + * Write the data log to the file + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + + /*! + * Write the data log to the console + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + }; }; #endif // SBG_LOGGER_ENTRY_EKF_ROT_ACCEL_H diff --git a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryEvent.h b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryEvent.h index eac8e8c..f619a9c 100644 --- a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryEvent.h +++ b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryEvent.h @@ -1,12 +1,12 @@ /*! - * \file loggerEntryEvent.h - * \author SBG Systems - * \date March 14, 2023 + * \file loggerEntryEvent.h + * \author SBG Systems + * \date March 14, 2023 * - * \brief Implement Sync In/Out event handlers. + * \brief Implement Sync In/Out event handlers. * - * \copyright Copyright (C) 2023, SBG Systems SAS. All rights reserved. - * \beginlicense Proprietary license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense Proprietary license * * This source code is intended for use only by SBG Systems SAS and * those that have explicit written permission to use it from @@ -35,153 +35,153 @@ namespace sbg { - //----------------------------------------------------------------------// - //- Generic Event implementations -// - //----------------------------------------------------------------------// - - /*! - * Handle SBG_ECOM_LOG_EVENT_# & SBG_ECOM_LOG_EVENT_OUT_# - */ - class CLoggerEntryEvent : public ILoggerEntry - { - private: - - //----------------------------------------------------------------------// - //- Private methods -// - //----------------------------------------------------------------------// - - /*! - * Write the header to the file and/or console. - * - * \param[in] context Logger context and settings. - */ - void writeHeaderToFile(const CLoggerContext &context) override; - - /*! - * Write the data log to the file - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - - /*! - * Write the data log to the console - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - }; - - //----------------------------------------------------------------------// - //- Event specialization/implementations -// - //----------------------------------------------------------------------// - - /*! - * Handle SBG_ECOM_LOG_EVENT_A - */ - class CLoggerEntryEventInA : public CLoggerEntryEvent, public ILoggerEntryKey - { - public: - - /*! - * Returns the log name. - * - * \return log name. - */ - std::string getName() const override; - }; - - /*! - * Handle SBG_ECOM_LOG_EVENT_B - */ - class CLoggerEntryEventInB : public CLoggerEntryEvent, public ILoggerEntryKey - { - public: - - /*! - * Returns the log name. - * - * \return log name. - */ - std::string getName() const override; - }; - - /*! - * Handle SBG_ECOM_LOG_EVENT_C - */ - class CLoggerEntryEventInC : public CLoggerEntryEvent, public ILoggerEntryKey - { - public: - - /*! - * Returns the log name. - * - * \return log name. - */ - std::string getName() const override; - }; - - /*! - * Handle SBG_ECOM_LOG_EVENT_D - */ - class CLoggerEntryEventInD : public CLoggerEntryEvent, public ILoggerEntryKey - { - public: - - /*! - * Returns the log name. - * - * \return log name. - */ - std::string getName() const override; - }; - - /*! - * Handle SBG_ECOM_LOG_EVENT_E - */ - class CLoggerEntryEventInE : public CLoggerEntryEvent, public ILoggerEntryKey - { - public: - - /*! - * Returns the log name. - * - * \return log name. - */ - std::string getName() const override; - }; - - /*! - * Handle SBG_ECOM_LOG_EVENT_OUT_A - */ - class CLoggerEntryEventOutA : public CLoggerEntryEvent, public ILoggerEntryKey - { - public: - - /*! - * Returns the log name. - * - * \return log name. - */ - std::string getName() const override; - }; - - /*! - * Handle SBG_ECOM_LOG_EVENT_OUT_B - */ - class CLoggerEntryEventOutB : public CLoggerEntryEvent, public ILoggerEntryKey - { - public: - - /*! - * Returns the log name. - * - * \return log name. - */ - std::string getName() const override; - }; + //----------------------------------------------------------------------// + //- Generic Event implementations -// + //----------------------------------------------------------------------// + + /*! + * Handle SBG_ECOM_LOG_EVENT_# & SBG_ECOM_LOG_EVENT_OUT_# + */ + class CLoggerEntryEvent : public ILoggerEntry + { + private: + + //----------------------------------------------------------------------// + //- Private methods -// + //----------------------------------------------------------------------// + + /*! + * Write the header to the file and/or console. + * + * \param[in] context Logger context and settings. + */ + void writeHeaderToFile(const CLoggerContext &context) override; + + /*! + * Write the data log to the file + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + + /*! + * Write the data log to the console + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + }; + + //----------------------------------------------------------------------// + //- Event specialization/implementations -// + //----------------------------------------------------------------------// + + /*! + * Handle SBG_ECOM_LOG_EVENT_A + */ + class CLoggerEntryEventInA : public CLoggerEntryEvent, public ILoggerEntryKey + { + public: + + /*! + * Returns the log name. + * + * \return log name. + */ + std::string getName() const override; + }; + + /*! + * Handle SBG_ECOM_LOG_EVENT_B + */ + class CLoggerEntryEventInB : public CLoggerEntryEvent, public ILoggerEntryKey + { + public: + + /*! + * Returns the log name. + * + * \return log name. + */ + std::string getName() const override; + }; + + /*! + * Handle SBG_ECOM_LOG_EVENT_C + */ + class CLoggerEntryEventInC : public CLoggerEntryEvent, public ILoggerEntryKey + { + public: + + /*! + * Returns the log name. + * + * \return log name. + */ + std::string getName() const override; + }; + + /*! + * Handle SBG_ECOM_LOG_EVENT_D + */ + class CLoggerEntryEventInD : public CLoggerEntryEvent, public ILoggerEntryKey + { + public: + + /*! + * Returns the log name. + * + * \return log name. + */ + std::string getName() const override; + }; + + /*! + * Handle SBG_ECOM_LOG_EVENT_E + */ + class CLoggerEntryEventInE : public CLoggerEntryEvent, public ILoggerEntryKey + { + public: + + /*! + * Returns the log name. + * + * \return log name. + */ + std::string getName() const override; + }; + + /*! + * Handle SBG_ECOM_LOG_EVENT_OUT_A + */ + class CLoggerEntryEventOutA : public CLoggerEntryEvent, public ILoggerEntryKey + { + public: + + /*! + * Returns the log name. + * + * \return log name. + */ + std::string getName() const override; + }; + + /*! + * Handle SBG_ECOM_LOG_EVENT_OUT_B + */ + class CLoggerEntryEventOutB : public CLoggerEntryEvent, public ILoggerEntryKey + { + public: + + /*! + * Returns the log name. + * + * \return log name. + */ + std::string getName() const override; + }; }; diff --git a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryGeneral.cpp b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryGeneral.cpp index ba6b10f..e7cd5b6 100644 --- a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryGeneral.cpp +++ b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryGeneral.cpp @@ -4,6 +4,7 @@ // sbgCommonLib headers #include +#include // sbgECom headers #include @@ -89,8 +90,8 @@ std::string CLoggerEntryStatus::getName() const void CLoggerEntryStatus::writeHeaderToFile(const CLoggerContext &context) { - m_outFile << context.getTimeColTitle() << "\tgeneral\tcom\tcom2\taiding\n"; - m_outFile << context.getTimeUnit() << "\t(na)\t(na)\t(na)\t(na)\t(na)\n"; + m_outFile << context.getTimeColTitle() << "\tgeneral\tcom\tcom2\taiding\tcpuUsage\n"; + m_outFile << context.getTimeUnit() << "\t(na)\t(na)\t(na)\t(na)\t(na)\t(%%)\n"; } void CLoggerEntryStatus::writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) @@ -101,7 +102,8 @@ void CLoggerEntryStatus::writeDataToFile(const CLoggerContext &context, const Sb << context.fmtStatus(data.generalStatus) << "\t" << context.fmtStatus(data.comStatus) << "\t" << context.fmtStatus(data.comStatus2) << "\t" - << context.fmtStatus(data.aidingStatus) << "\n"; + << context.fmtStatus(data.aidingStatus) << "\t" + << (uint32_t)data.cpuUsage << "\n"; } void CLoggerEntryStatus::writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) @@ -112,7 +114,8 @@ void CLoggerEntryStatus::writeDataToConsole(const CLoggerContext &context, const << std::setw(12) << context.fmtStatus(data.generalStatus) << std::setw(12) << context.fmtStatus(data.comStatus) << std::setw(12) << context.fmtStatus(data.comStatus2) - << std::setw(12) << context.fmtStatus(data.aidingStatus) << "\n"; + << std::setw(12) << context.fmtStatus(data.aidingStatus) + << std::setw(12) << (uint32_t)data.cpuUsage << "\n"; } //----------------------------------------------------------------------// @@ -196,6 +199,127 @@ void CLoggerEntryDiag::writeDataToConsole(const CLoggerContext &context, const S } } +//----------------------------------------------------------------------// +//- CLoggerEntryPtpStatus -// +//----------------------------------------------------------------------// + +std::string CLoggerEntryPtpStatus::convertState(SbgEComLogPtpState state) +{ + std::string stateStr = "unknown"; + + switch (state) + { + case SBG_ECOM_LOG_PTP_STATE_DISABLED: + stateStr = "disabled"; + break; + case SBG_ECOM_LOG_PTP_STATE_FAULTY: + stateStr = "faulty"; + break; + case SBG_ECOM_LOG_PTP_STATE_MASTER: + stateStr = "master"; + break; + case SBG_ECOM_LOG_PTP_STATE_PASSIVE: + stateStr = "passive"; + break; + } + + return stateStr; +} + +std::string CLoggerEntryPtpStatus::convertTimeScale(SbgEComLogPtpTimeScale timeScale) +{ + std::string timeScaleStr = "unknown"; + + switch (timeScale) + { + case SBG_ECOM_LOG_PTP_TIME_SCALE_TAI: + timeScaleStr = "tai"; + break; + case SBG_ECOM_LOG_PTP_TIME_SCALE_UTC: + timeScaleStr = "utc"; + break; + case SBG_ECOM_LOG_PTP_TIME_SCALE_GPS: + timeScaleStr = "gps"; + break; + } + + return timeScaleStr; +} + +std::string CLoggerEntryPtpStatus::getName() const +{ + return "ptpStatus"; +} + +void CLoggerEntryPtpStatus::writeHeaderToFile(const CLoggerContext &context) +{ + m_outFile << context.getTimeColTitle() << "\tstate\ttimeScale\ttimeScaleOffset\tlocalClockIdentity\tlocalClockPriority1\tlocalClockPriority2\tlocalClockClass\tlocalClockAccuracy\tlocalClockLog2Variance\tlocalClockTimeSource\tmasterClockIdentity\tmasterClockPriority1\tmasterClockPriority2\tmasterClockClass\tmasterClockAccuracy\tmasterClockLog2Variance\tmasterClockTimeSource\tmasterIpAddress\tmeanPathDelay\tmeanPathDelayStdDev\tclockOffset\tclockOffsetStdDev\tclockFreqOffset\tclockFreqOffsetStdDev\n"; + m_outFile << context.getTimeUnit() << "\t(na)\t(na)\t(s)\t(na)\t(na)\t(na)\t(na)\t(na)\t(na)\t(na)\t(na)\t(na)\t(na)\t(na)\t(na)\t(na)\t(na)\t(na)\t(s)\t(s)\t(s)\t(s)\t(s)\t(s)\n"; +} + +void CLoggerEntryPtpStatus::writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogPtp &data = logData.ptpData; + char masterIpAddressString[SBG_NETWORK_IPV4_STRING_SIZE]; + + sbgNetworkIpToString(data.masterIpAddress, masterIpAddressString, sizeof(masterIpAddressString)); + + m_outFile << context.fmtTime(data.timeStamp) << "\t" + << convertState(data.state) << "\t" + << convertTimeScale(data.timeScale) << "\t" + << data.timeScaleOffset << "\t" + + << std::hex << data.localClockIdentity << std::dec << "\t" + << (uint32_t)data.localClockPriority1 << "\t" + << (uint32_t)data.localClockPriority2 << "\t" + << (uint32_t)data.localClockClass << "\t" + << (uint32_t)data.localClockAccuracy << "\t" + << data.localClockLog2Variance << "\t" + << (uint32_t)data.localClockTimeSource << "\t" + + << std::hex << data.masterClockIdentity << std::dec << "\t" + << (uint32_t)data.masterClockPriority1 << "\t" + << (uint32_t)data.masterClockPriority2 << "\t" + << (uint32_t)data.masterClockClass << "\t" + << (uint32_t)data.masterClockAccuracy << "\t" + << data.masterClockLog2Variance << "\t" + << (uint32_t)data.masterClockTimeSource << "\t" + << masterIpAddressString << "\t" + + << data.meanPathDelay << "\t" + << data.meanPathDelayStdDev << "\t" + << data.clockOffset << "\t" + << data.clockOffsetStdDev << "\t" + << data.clockFreqOffset << "\t" + << data.clockFreqOffsetStdDev << "\n"; +} + +void CLoggerEntryPtpStatus::writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogPtp &data = logData.ptpData; + char masterIpAddressString[SBG_NETWORK_IPV4_STRING_SIZE]; + + SBG_UNUSED_PARAMETER(context); + + sbgNetworkIpToString(data.masterIpAddress, masterIpAddressString, sizeof(masterIpAddressString)); + + std::cout << std::setw(12) << getName() << ": " + << std::setw(12) << convertState(data.state) + << std::setw(12) << convertTimeScale(data.timeScale) + << std::setw(12) << data.timeScaleOffset + + << std::setw(20) << std::hex << data.localClockIdentity << std::dec + << std::setw(20) << std::hex << data.masterClockIdentity << std::dec + << std::setw(20) << masterIpAddressString + + << std::setw(12) << data.meanPathDelay + << std::setw(12) << data.meanPathDelayStdDev + << std::setw(12) << data.clockOffset + << std::setw(12) << data.clockOffsetStdDev + << std::setw(12) << data.clockFreqOffset + << std::setw(12) << data.clockFreqOffsetStdDev << "\n"; +} + //----------------------------------------------------------------------// //- CLoggerEntryRtcmRaw -// //----------------------------------------------------------------------// diff --git a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryGeneral.h b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryGeneral.h index 63e5dbd..03de55d 100644 --- a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryGeneral.h +++ b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryGeneral.h @@ -1,12 +1,12 @@ /*! - * \file loggerEntryGeneral.h - * \author SBG Systems - * \date March 07, 2023 + * \file loggerEntryGeneral.h + * \author SBG Systems + * \date March 07, 2023 * - * \brief Implement general sbgECom log such as status/utc/etc + * \brief Implement general sbgECom log such as status/utc/etc * - * \copyright Copyright (C) 2023, SBG Systems SAS. All rights reserved. - * \beginlicense Proprietary license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense Proprietary license * * This source code is intended for use only by SBG Systems SAS and * those that have explicit written permission to use it from @@ -35,209 +35,261 @@ namespace sbg { - /*! - * Handle SBG_ECOM_LOG_UTC_TIME - */ - class CLoggerEntryUtcTime : public ILoggerEntry, public ILoggerEntryKey - { - public: - - //----------------------------------------------------------------------// - //- Public getters -// - //----------------------------------------------------------------------// - - /*! - * Returns the log name. - * - * \return log name. - */ - std::string getName() const override; - - private: - - //----------------------------------------------------------------------// - //- Private methods -// - //----------------------------------------------------------------------// - - /*! - * Process the incoming log to update the context with latest UTC time information. - * - * \param[in/out] context Logger context and settings. - * \param[in] logData Received sbgECom log data to process. - * \return true to continue processing the log or false to skip/discard it. - */ - bool preProcess(CLoggerContext &context, const SbgEComLogUnion &logData) override; - - /*! - * Write the header to the file and/or console. - * - * \param[in] context Logger context and settings. - */ - void writeHeaderToFile(const CLoggerContext &context) override; - - /*! - * Write the data log to the file - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - - /*! - * Write the data log to the console - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - - }; - - /*! - * Handle SBG_ECOM_LOG_STATUS - */ - class CLoggerEntryStatus : public ILoggerEntry, public ILoggerEntryKey - { - public: - - //----------------------------------------------------------------------// - //- Public getters -// - //----------------------------------------------------------------------// - - /*! - * Returns the log name. - * - * \return log name. - */ - std::string getName() const override; - - private: - - //----------------------------------------------------------------------// - //- Private methods -// - //----------------------------------------------------------------------// - - /*! - * Write the header to the file and/or console. - * - * \param[in] context Logger context and settings. - */ - void writeHeaderToFile(const CLoggerContext &context) override; - - /*! - * Write the data log to the file - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - - /*! - * Write the data log to the console - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - }; - - /*! - * Handle SBG_ECOM_LOG_DIAG - */ - class CLoggerEntryDiag : public ILoggerEntry, public ILoggerEntryKey - { - public: - - //----------------------------------------------------------------------// - //- Public getters -// - //----------------------------------------------------------------------// - - /*! - * Returns the log name. - * - * \return log name. - */ - std::string getName() const override; - - private: - //----------------------------------------------------------------------// - //- Private methods -// - //----------------------------------------------------------------------// - - /*! - * For diagnostic messages, never discard data when time is invalid. - * - * \param[in] context Logger context and settings. - * \return always returns false. - */ - bool getDiscardData(const CLoggerContext &context) const override; - - /*! - * Write the data log to the file - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - - /*! - * Write the data log to the console - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - }; - - /*! - * Handle SBG_ECOM_LOG_RTCM_RAW - */ - class CLoggerEntryRtcmRaw : public ILoggerEntry, public ILoggerEntryKey - { - public: - //----------------------------------------------------------------------// - //- Settings/getters -// - //----------------------------------------------------------------------// - - /*! - * Returns the log name. - * - * \return log name. - */ - std::string getName() const override; - - /*! - * Returns the file name to use. - * - * The default implementation just appends a .txt to the log name. - * - * \return file name for this log. - */ - std::string getFileName() const override; - - /*! - * Returns false if the output file is text or true for binary. - * - * The default base implementation consider text file (false). - * - * \return false for text file and true for binary. - */ - bool isBinary() const override; - - private: - - //----------------------------------------------------------------------// - //- Private methods -// - //----------------------------------------------------------------------// - - /*! - * Write the data log to the file - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - }; + /*! + * Handle SBG_ECOM_LOG_UTC_TIME + */ + class CLoggerEntryUtcTime : public ILoggerEntry, public ILoggerEntryKey + { + public: + + //----------------------------------------------------------------------// + //- Public getters -// + //----------------------------------------------------------------------// + + /*! + * Returns the log name. + * + * \return log name. + */ + std::string getName() const override; + + private: + + //----------------------------------------------------------------------// + //- Private methods -// + //----------------------------------------------------------------------// + + /*! + * Process the incoming log to update the context with latest UTC time information. + * + * \param[in/out] context Logger context and settings. + * \param[in] logData Received sbgECom log data to process. + * \return true to continue processing the log or false to skip/discard it. + */ + bool preProcess(CLoggerContext &context, const SbgEComLogUnion &logData) override; + + /*! + * Write the header to the file and/or console. + * + * \param[in] context Logger context and settings. + */ + void writeHeaderToFile(const CLoggerContext &context) override; + + /*! + * Write the data log to the file + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + + /*! + * Write the data log to the console + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + + }; + + /*! + * Handle SBG_ECOM_LOG_STATUS + */ + class CLoggerEntryStatus : public ILoggerEntry, public ILoggerEntryKey + { + public: + + //----------------------------------------------------------------------// + //- Public getters -// + //----------------------------------------------------------------------// + + /*! + * Returns the log name. + * + * \return log name. + */ + std::string getName() const override; + + private: + + //----------------------------------------------------------------------// + //- Private methods -// + //----------------------------------------------------------------------// + + /*! + * Write the header to the file and/or console. + * + * \param[in] context Logger context and settings. + */ + void writeHeaderToFile(const CLoggerContext &context) override; + + /*! + * Write the data log to the file + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + + /*! + * Write the data log to the console + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + }; + + /*! + * Handle SBG_ECOM_LOG_DIAG + */ + class CLoggerEntryDiag : public ILoggerEntry, public ILoggerEntryKey + { + public: + + //----------------------------------------------------------------------// + //- Public getters -// + //----------------------------------------------------------------------// + + /*! + * Returns the log name. + * + * \return log name. + */ + std::string getName() const override; + + private: + //----------------------------------------------------------------------// + //- Private methods -// + //----------------------------------------------------------------------// + + /*! + * For diagnostic messages, never discard data when time is invalid. + * + * \param[in] context Logger context and settings. + * \return always returns false. + */ + bool getDiscardData(const CLoggerContext &context) const override; + + /*! + * Write the data log to the file + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + + /*! + * Write the data log to the console + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + }; + + /*! + * Handle SBG_ECOM_LOG_PTP_STATUS + */ + class CLoggerEntryPtpStatus : public ILoggerEntry, public ILoggerEntryKey + { + public: + + //----------------------------------------------------------------------// + //- Public getters -// + //----------------------------------------------------------------------// + + /*! + * Returns the log name. + * + * \return log name. + */ + std::string getName() const override; + + private: + + //----------------------------------------------------------------------// + //- Private methods -// + //----------------------------------------------------------------------// + + static std::string convertState(SbgEComLogPtpState state); + + static std::string convertTimeScale(SbgEComLogPtpTimeScale timeScale); + + /*! + * Write the header to the file and/or console. + * + * \param[in] context Logger context and settings. + */ + void writeHeaderToFile(const CLoggerContext &context) override; + + /*! + * Write the data log to the file + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + + /*! + * Write the data log to the console + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + }; + + /*! + * Handle SBG_ECOM_LOG_RTCM_RAW + */ + class CLoggerEntryRtcmRaw : public ILoggerEntry, public ILoggerEntryKey + { + public: + //----------------------------------------------------------------------// + //- Settings/getters -// + //----------------------------------------------------------------------// + + /*! + * Returns the log name. + * + * \return log name. + */ + std::string getName() const override; + + /*! + * Returns the file name to use. + * + * The default implementation just appends a .txt to the log name. + * + * \return file name for this log. + */ + std::string getFileName() const override; + + /*! + * Returns false if the output file is text or true for binary. + * + * The default base implementation consider text file (false). + * + * \return false for text file and true for binary. + */ + bool isBinary() const override; + + private: + + //----------------------------------------------------------------------// + //- Private methods -// + //----------------------------------------------------------------------// + + /*! + * Write the data log to the file + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + }; }; #endif // SBG_LOGGER_ENTRY_GENERAL_H diff --git a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryGnss.cpp b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryGnss.cpp index 97ac830..1101f7d 100644 --- a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryGnss.cpp +++ b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryGnss.cpp @@ -135,8 +135,8 @@ void CLoggerEntryGnssHdt::writeDataToFile(const CLoggerContext &context, const S << data.pitch << "\t" << data.pitchAccuracy << "\t" << data.baseline << "\t" - << data.numSvTracked << "\t" - << data.numSvUsed << "\n"; + << (uint32_t)data.numSvTracked << "\t" + << (uint32_t)data.numSvUsed << "\n"; } void CLoggerEntryGnssHdt::writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) @@ -151,8 +151,8 @@ void CLoggerEntryGnssHdt::writeDataToConsole(const CLoggerContext &context, cons << std::setw(12) << data.pitch << std::setw(12) << data.pitchAccuracy << std::setw(12) << data.baseline - << std::setw(12) << data.numSvTracked - << std::setw(12) << data.numSvUsed << "\n"; + << std::setw(12) << (uint32_t)data.numSvTracked + << std::setw(12) << (uint32_t)data.numSvUsed << "\n"; } //----------------------------------------------------------------------// diff --git a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryGnss.h b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryGnss.h index 6a3d568..ff862c7 100644 --- a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryGnss.h +++ b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryGnss.h @@ -1,12 +1,12 @@ /*! - * \file loggerEntryGnss.h - * \author SBG Systems - * \date March 06, 2023 + * \file loggerEntryGnss.h + * \author SBG Systems + * \date March 06, 2023 * - * \brief Implement GNSS log handlers + * \brief Implement GNSS log handlers * - * \copyright Copyright (C) 2023, SBG Systems SAS. All rights reserved. - * \beginlicense Proprietary license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense Proprietary license * * This source code is intended for use only by SBG Systems SAS and * those that have explicit written permission to use it from @@ -35,343 +35,343 @@ namespace sbg { - //----------------------------------------------------------------------// - //- Generic GNSS implementations -// - //----------------------------------------------------------------------// - - /*! - * Handle SBG_ECOM_LOG_GNSS#_VEL - */ - class CLoggerEntryGnssVel : public ILoggerEntry - { - private: - - //----------------------------------------------------------------------// - //- Private methods -// - //----------------------------------------------------------------------// - - /*! - * Write the header to the file and/or console. - * - * \param[in] context Logger context and settings. - */ - void writeHeaderToFile(const CLoggerContext &context) override; - - /*! - * Write the data log to the file - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - - /*! - * Write the data log to the console - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - }; - - /*! - * Handle SBG_ECOM_LOG_GNSS#_POS - */ - class CLoggerEntryGnssPos : public ILoggerEntry - { - private: - - //----------------------------------------------------------------------// - //- Private methods -// - //----------------------------------------------------------------------// - - /*! - * Write the header to the file and/or console. - * - * \param[in] context Logger context and settings. - */ - void writeHeaderToFile(const CLoggerContext &context) override; - - /*! - * Write the data log to the file - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - - /*! - * Write the data log to the console - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - }; - - /*! - * Handle SBG_ECOM_LOG_GNSS#_HDT - */ - class CLoggerEntryGnssHdt : public ILoggerEntry - { - private: - - //----------------------------------------------------------------------// - //- Private methods -// - //----------------------------------------------------------------------// - - /*! - * Write the header to the file and/or console. - * - * \param[in] context Logger context and settings. - */ - void writeHeaderToFile(const CLoggerContext &context) override; - - /*! - * Write the data log to the file - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - - /*! - * Write the data log to the console - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - }; - - /*! - * Handle SBG_ECOM_LOG_GNSS#_RAW - */ - class CLoggerEntryGnssRaw : public ILoggerEntry - { - public: - //----------------------------------------------------------------------// - //- Settings/getters -// - //----------------------------------------------------------------------// - - /*! - * Returns the file name to use. - * - * The default implementation just appends a .txt to the log name. - * - * \return file name for this log. - */ - std::string getFileName() const override; - - /*! - * Returns false if the output file is text or true for binary. - * - * The default base implementation consider text file (false). - * - * \return false for text file and true for binary. - */ - bool isBinary() const override; - - private: - - //----------------------------------------------------------------------// - //- Private methods -// - //----------------------------------------------------------------------// - - /*! - * Write the data log to the file - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - }; - - /*! - * Handle SBG_ECOM_LOG_GNSS#_SAT - */ - class CLoggerEntryGnssSat : public ILoggerEntry - { - private: - - //----------------------------------------------------------------------// - //- Private methods -// - //----------------------------------------------------------------------// - - /*! - * Write the data log to the file - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - - /*! - * Write the data log to the console - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - }; - - //----------------------------------------------------------------------// - //- GNSS1 implementations -// - //----------------------------------------------------------------------// - - /*! - * Handle SBG_ECOM_LOG_GNSS1_VEL - */ - class CLoggerEntryGnss1Vel : public CLoggerEntryGnssVel, public ILoggerEntryKey - { - public: - - /*! - * Returns the log name. - * - * \return log name. - */ - std::string getName() const override; - }; - - /*! - * Handle SBG_ECOM_LOG_GNSS1_POS - */ - class CLoggerEntryGnss1Pos : public CLoggerEntryGnssPos, public ILoggerEntryKey - { - public: - - /*! - * Returns the log name. - * - * \return log name. - */ - std::string getName() const override; - }; - - /*! - * Handle SBG_ECOM_LOG_GNSS1_HDT - */ - class CLoggerEntryGnss1Hdt : public CLoggerEntryGnssHdt, public ILoggerEntryKey - { - public: - - /*! - * Returns the log name. - * - * \return log name. - */ - std::string getName() const override; - }; - - /*! - * Handle SBG_ECOM_LOG_GNSS1_RAW - */ - class CLoggerEntryGnss1Raw : public CLoggerEntryGnssRaw, public ILoggerEntryKey - { - public: - - /*! - * Returns the log name. - * - * \return log name. - */ - std::string getName() const override; - }; - - /*! - * Handle SBG_ECOM_LOG_GNSS1_SAT - */ - class CLoggerEntryGnss1Sat : public CLoggerEntryGnssSat, public ILoggerEntryKey - { - public: - - /*! - * Returns the log name. - * - * \return log name. - */ - std::string getName() const override; - }; - - //----------------------------------------------------------------------// - //- GNSS2 implementations -// - //----------------------------------------------------------------------// - - /*! - * Handle SBG_ECOM_LOG_GNSS2_VEL - */ - class CLoggerEntryGnss2Vel : public CLoggerEntryGnssVel, public ILoggerEntryKey - { - public: - - /*! - * Returns the log name. - * - * \return log name. - */ - std::string getName() const override; - }; - - /*! - * Handle SBG_ECOM_LOG_GNSS2_POS - */ - class CLoggerEntryGnss2Pos : public CLoggerEntryGnssPos, public ILoggerEntryKey - { - public: - - /*! - * Returns the log name. - * - * \return log name. - */ - std::string getName() const override; - }; - - /*! - * Handle SBG_ECOM_LOG_GNSS2_HDT - */ - class CLoggerEntryGnss2Hdt : public CLoggerEntryGnssHdt, public ILoggerEntryKey - { - public: - - /*! - * Returns the log name. - * - * \return log name. - */ - std::string getName() const override; - }; - - /*! - * Handle SBG_ECOM_LOG_GNSS2_RAW - */ - class CLoggerEntryGnss2Raw : public CLoggerEntryGnssRaw, public ILoggerEntryKey - { - public: - - /*! - * Returns the log name. - * - * \return log name. - */ - std::string getName() const override; - }; - - /*! - * Handle SBG_ECOM_LOG_GNSS2_SAT - */ - class CLoggerEntryGnss2Sat : public CLoggerEntryGnssSat, public ILoggerEntryKey - { - public: - - /*! - * Returns the log name. - * - * \return log name. - */ - std::string getName() const override; - }; + //----------------------------------------------------------------------// + //- Generic GNSS implementations -// + //----------------------------------------------------------------------// + + /*! + * Handle SBG_ECOM_LOG_GNSS#_VEL + */ + class CLoggerEntryGnssVel : public ILoggerEntry + { + private: + + //----------------------------------------------------------------------// + //- Private methods -// + //----------------------------------------------------------------------// + + /*! + * Write the header to the file and/or console. + * + * \param[in] context Logger context and settings. + */ + void writeHeaderToFile(const CLoggerContext &context) override; + + /*! + * Write the data log to the file + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + + /*! + * Write the data log to the console + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + }; + + /*! + * Handle SBG_ECOM_LOG_GNSS#_POS + */ + class CLoggerEntryGnssPos : public ILoggerEntry + { + private: + + //----------------------------------------------------------------------// + //- Private methods -// + //----------------------------------------------------------------------// + + /*! + * Write the header to the file and/or console. + * + * \param[in] context Logger context and settings. + */ + void writeHeaderToFile(const CLoggerContext &context) override; + + /*! + * Write the data log to the file + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + + /*! + * Write the data log to the console + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + }; + + /*! + * Handle SBG_ECOM_LOG_GNSS#_HDT + */ + class CLoggerEntryGnssHdt : public ILoggerEntry + { + private: + + //----------------------------------------------------------------------// + //- Private methods -// + //----------------------------------------------------------------------// + + /*! + * Write the header to the file and/or console. + * + * \param[in] context Logger context and settings. + */ + void writeHeaderToFile(const CLoggerContext &context) override; + + /*! + * Write the data log to the file + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + + /*! + * Write the data log to the console + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + }; + + /*! + * Handle SBG_ECOM_LOG_GNSS#_RAW + */ + class CLoggerEntryGnssRaw : public ILoggerEntry + { + public: + //----------------------------------------------------------------------// + //- Settings/getters -// + //----------------------------------------------------------------------// + + /*! + * Returns the file name to use. + * + * The default implementation just appends a .txt to the log name. + * + * \return file name for this log. + */ + std::string getFileName() const override; + + /*! + * Returns false if the output file is text or true for binary. + * + * The default base implementation consider text file (false). + * + * \return false for text file and true for binary. + */ + bool isBinary() const override; + + private: + + //----------------------------------------------------------------------// + //- Private methods -// + //----------------------------------------------------------------------// + + /*! + * Write the data log to the file + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + }; + + /*! + * Handle SBG_ECOM_LOG_GNSS#_SAT + */ + class CLoggerEntryGnssSat : public ILoggerEntry + { + private: + + //----------------------------------------------------------------------// + //- Private methods -// + //----------------------------------------------------------------------// + + /*! + * Write the data log to the file + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + + /*! + * Write the data log to the console + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + }; + + //----------------------------------------------------------------------// + //- GNSS1 implementations -// + //----------------------------------------------------------------------// + + /*! + * Handle SBG_ECOM_LOG_GNSS1_VEL + */ + class CLoggerEntryGnss1Vel : public CLoggerEntryGnssVel, public ILoggerEntryKey + { + public: + + /*! + * Returns the log name. + * + * \return log name. + */ + std::string getName() const override; + }; + + /*! + * Handle SBG_ECOM_LOG_GNSS1_POS + */ + class CLoggerEntryGnss1Pos : public CLoggerEntryGnssPos, public ILoggerEntryKey + { + public: + + /*! + * Returns the log name. + * + * \return log name. + */ + std::string getName() const override; + }; + + /*! + * Handle SBG_ECOM_LOG_GNSS1_HDT + */ + class CLoggerEntryGnss1Hdt : public CLoggerEntryGnssHdt, public ILoggerEntryKey + { + public: + + /*! + * Returns the log name. + * + * \return log name. + */ + std::string getName() const override; + }; + + /*! + * Handle SBG_ECOM_LOG_GNSS1_RAW + */ + class CLoggerEntryGnss1Raw : public CLoggerEntryGnssRaw, public ILoggerEntryKey + { + public: + + /*! + * Returns the log name. + * + * \return log name. + */ + std::string getName() const override; + }; + + /*! + * Handle SBG_ECOM_LOG_GNSS1_SAT + */ + class CLoggerEntryGnss1Sat : public CLoggerEntryGnssSat, public ILoggerEntryKey + { + public: + + /*! + * Returns the log name. + * + * \return log name. + */ + std::string getName() const override; + }; + + //----------------------------------------------------------------------// + //- GNSS2 implementations -// + //----------------------------------------------------------------------// + + /*! + * Handle SBG_ECOM_LOG_GNSS2_VEL + */ + class CLoggerEntryGnss2Vel : public CLoggerEntryGnssVel, public ILoggerEntryKey + { + public: + + /*! + * Returns the log name. + * + * \return log name. + */ + std::string getName() const override; + }; + + /*! + * Handle SBG_ECOM_LOG_GNSS2_POS + */ + class CLoggerEntryGnss2Pos : public CLoggerEntryGnssPos, public ILoggerEntryKey + { + public: + + /*! + * Returns the log name. + * + * \return log name. + */ + std::string getName() const override; + }; + + /*! + * Handle SBG_ECOM_LOG_GNSS2_HDT + */ + class CLoggerEntryGnss2Hdt : public CLoggerEntryGnssHdt, public ILoggerEntryKey + { + public: + + /*! + * Returns the log name. + * + * \return log name. + */ + std::string getName() const override; + }; + + /*! + * Handle SBG_ECOM_LOG_GNSS2_RAW + */ + class CLoggerEntryGnss2Raw : public CLoggerEntryGnssRaw, public ILoggerEntryKey + { + public: + + /*! + * Returns the log name. + * + * \return log name. + */ + std::string getName() const override; + }; + + /*! + * Handle SBG_ECOM_LOG_GNSS2_SAT + */ + class CLoggerEntryGnss2Sat : public CLoggerEntryGnssSat, public ILoggerEntryKey + { + public: + + /*! + * Returns the log name. + * + * \return log name. + */ + std::string getName() const override; + }; }; #endif // SBG_LOGGER_ENTRY_GNSS_H diff --git a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryImu.h b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryImu.h index c4befef..07bee8e 100644 --- a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryImu.h +++ b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryImu.h @@ -1,12 +1,12 @@ /*! - * \file loggerEntryImu.h - * \author SBG Systems - * \date March 14, 2023 + * \file loggerEntryImu.h + * \author SBG Systems + * \date March 14, 2023 * - * \brief Implemented IMU with optional decimation. + * \brief Implemented IMU with optional decimation. * - * \copyright Copyright (C) 2023, SBG Systems SAS. All rights reserved. - * \beginlicense Proprietary license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense Proprietary license * * This source code is intended for use only by SBG Systems SAS and * those that have explicit written permission to use it from @@ -38,167 +38,167 @@ namespace sbg { - /*! - * Generic handler for IMU data Handle - */ - class CLoggerEntryImu : public ILoggerEntry - { - protected: - - /*! - * Write the IMU data or short log to the file - * - * \param[in] context Logger context and settings. - * \param[in] imuData Input sbgECom IMU data to write. - */ - template - void writeImuDataToFile(const CLoggerContext &context, const T &imuData); - - /*! - * Write the IMU data or short log to the file - * - * \param[in] context Logger context and settings. - * \param[in] imuData Input sbgECom IMU data to write. - */ - template - void writeImuDataToConsole(const CLoggerContext &context, const T &imuData); - - private: - //----------------------------------------------------------------------// - //- Private methods -// - //----------------------------------------------------------------------// - - /*! - * Write the header to the file and/or console. - * - * \param[in] context Logger context and settings. - */ - void writeHeaderToFile(const CLoggerContext &context) override; - - //----------------------------------------------------------------------// - //- Private members -// - //----------------------------------------------------------------------// - - CImuDataMean m_fileValues; /*!< decimated IMU values for file output. */ - CImuDataMean m_consoleValues; /*!< decimated IMU values for console output. */ - }; - - /*! - * Handle SBG_ECOM_LOG_IMU_DATA - */ - class CLoggerEntryImuData : public CLoggerEntryImu, public ILoggerEntryKey - { - public: - //----------------------------------------------------------------------// - //- Public getters -// - //----------------------------------------------------------------------// - - /*! - * Returns the log name. - * - * \return log name. - */ - std::string getName() const override; - - private: - //----------------------------------------------------------------------// - //- Private methods -// - //----------------------------------------------------------------------// - - /*! - * Write the data log to the file - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - - /*! - * Write the data log to the console - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - }; - - /*! - * Handle SBG_ECOM_LOG_IMU_SHORT - */ - class CLoggerEntryImuShort : public CLoggerEntryImu, public ILoggerEntryKey - { - public: - //----------------------------------------------------------------------// - //- Public getters -// - //----------------------------------------------------------------------// - - /*! - * Returns the log name. - * - * \return log name. - */ - std::string getName() const override; - - private: - //----------------------------------------------------------------------// - //- Private methods -// - //----------------------------------------------------------------------// - - /*! - * Write the data log to the file - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - - /*! - * Write the data log to the console - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - }; - - /*! - * Handle SBG_ECOM_LOG_FAST_IMU_DATA - */ - class CLoggerEntryImuFast : public CLoggerEntryImu, public ILoggerEntryKey - { - public: - //----------------------------------------------------------------------// - //- Public getters -// - //----------------------------------------------------------------------// - - /*! - * Returns the log name. - * - * \return log name. - */ - std::string getName() const override; - - private: - //----------------------------------------------------------------------// - //- Private methods -// - //----------------------------------------------------------------------// - - /*! - * Write the data log to the file - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - - /*! - * Write the data log to the console - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - }; + /*! + * Generic handler for IMU data Handle + */ + class CLoggerEntryImu : public ILoggerEntry + { + protected: + + /*! + * Write the IMU data or short log to the file + * + * \param[in] context Logger context and settings. + * \param[in] imuData Input sbgECom IMU data to write. + */ + template + void writeImuDataToFile(const CLoggerContext &context, const T &imuData); + + /*! + * Write the IMU data or short log to the file + * + * \param[in] context Logger context and settings. + * \param[in] imuData Input sbgECom IMU data to write. + */ + template + void writeImuDataToConsole(const CLoggerContext &context, const T &imuData); + + private: + //----------------------------------------------------------------------// + //- Private methods -// + //----------------------------------------------------------------------// + + /*! + * Write the header to the file and/or console. + * + * \param[in] context Logger context and settings. + */ + void writeHeaderToFile(const CLoggerContext &context) override; + + //----------------------------------------------------------------------// + //- Private members -// + //----------------------------------------------------------------------// + + CImuDataMean m_fileValues; /*!< decimated IMU values for file output. */ + CImuDataMean m_consoleValues; /*!< decimated IMU values for console output. */ + }; + + /*! + * Handle SBG_ECOM_LOG_IMU_DATA + */ + class CLoggerEntryImuData : public CLoggerEntryImu, public ILoggerEntryKey + { + public: + //----------------------------------------------------------------------// + //- Public getters -// + //----------------------------------------------------------------------// + + /*! + * Returns the log name. + * + * \return log name. + */ + std::string getName() const override; + + private: + //----------------------------------------------------------------------// + //- Private methods -// + //----------------------------------------------------------------------// + + /*! + * Write the data log to the file + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + + /*! + * Write the data log to the console + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + }; + + /*! + * Handle SBG_ECOM_LOG_IMU_SHORT + */ + class CLoggerEntryImuShort : public CLoggerEntryImu, public ILoggerEntryKey + { + public: + //----------------------------------------------------------------------// + //- Public getters -// + //----------------------------------------------------------------------// + + /*! + * Returns the log name. + * + * \return log name. + */ + std::string getName() const override; + + private: + //----------------------------------------------------------------------// + //- Private methods -// + //----------------------------------------------------------------------// + + /*! + * Write the data log to the file + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + + /*! + * Write the data log to the console + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + }; + + /*! + * Handle SBG_ECOM_LOG_FAST_IMU_DATA + */ + class CLoggerEntryImuFast : public CLoggerEntryImu, public ILoggerEntryKey + { + public: + //----------------------------------------------------------------------// + //- Public getters -// + //----------------------------------------------------------------------// + + /*! + * Returns the log name. + * + * \return log name. + */ + std::string getName() const override; + + private: + //----------------------------------------------------------------------// + //- Private methods -// + //----------------------------------------------------------------------// + + /*! + * Write the data log to the file + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + + /*! + * Write the data log to the console + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + }; }; #endif // SBG_LOGGER_ENTRY_IMU_H diff --git a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryMag.h b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryMag.h index acb0653..e872b30 100644 --- a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryMag.h +++ b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryMag.h @@ -1,12 +1,12 @@ /*! - * \file loggerEntryMag.h - * \author SBG Systems - * \date March 13, 2023 + * \file loggerEntryMag.h + * \author SBG Systems + * \date March 13, 2023 * - * \brief Implemented magnetometers releated logs. + * \brief Implemented magnetometers releated logs. * - * \copyright Copyright (C) 2023, SBG Systems SAS. All rights reserved. - * \beginlicense Proprietary license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense Proprietary license * * This source code is intended for use only by SBG Systems SAS and * those that have explicit written permission to use it from @@ -35,103 +35,103 @@ namespace sbg { - /*! - * Handle SBG_ECOM_LOG_MAG - */ - class CLoggerEntryMag : public ILoggerEntry, public ILoggerEntryKey - { - public: - - //----------------------------------------------------------------------// - //- Public getters -// - //----------------------------------------------------------------------// - - /*! - * Returns the log name. - * - * \return log name. - */ - std::string getName() const override; - - private: - - //----------------------------------------------------------------------// - //- Private methods -// - //----------------------------------------------------------------------// - - /*! - * Write the header to the file and/or console. - * - * \param[in] context Logger context and settings. - */ - void writeHeaderToFile(const CLoggerContext &context) override; - - /*! - * Write the data log to the file - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - - /*! - * Write the data log to the console - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - }; - - /*! - * Handle SBG_ECOM_LOG_MAG_CALIB - */ - class CLoggerEntryMagCalib : public ILoggerEntry, public ILoggerEntryKey - { - public: - //----------------------------------------------------------------------// - //- Settings/getters -// - //----------------------------------------------------------------------// - - /*! - * Returns the log name. - * - * \return log name. - */ - std::string getName() const override; - - /*! - * Returns the file name to use. - * - * The default implementation just appends a .txt to the log name. - * - * \return file name for this log. - */ - std::string getFileName() const override; - - /*! - * Returns false if the output file is text or true for binary. - * - * The default base implementation consider text file (false). - * - * \return false for text file and true for binary. - */ - bool isBinary() const override; - - private: - - //----------------------------------------------------------------------// - //- Private methods -// - //----------------------------------------------------------------------// - - /*! - * Write the data log to the file - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - }; + /*! + * Handle SBG_ECOM_LOG_MAG + */ + class CLoggerEntryMag : public ILoggerEntry, public ILoggerEntryKey + { + public: + + //----------------------------------------------------------------------// + //- Public getters -// + //----------------------------------------------------------------------// + + /*! + * Returns the log name. + * + * \return log name. + */ + std::string getName() const override; + + private: + + //----------------------------------------------------------------------// + //- Private methods -// + //----------------------------------------------------------------------// + + /*! + * Write the header to the file and/or console. + * + * \param[in] context Logger context and settings. + */ + void writeHeaderToFile(const CLoggerContext &context) override; + + /*! + * Write the data log to the file + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + + /*! + * Write the data log to the console + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + }; + + /*! + * Handle SBG_ECOM_LOG_MAG_CALIB + */ + class CLoggerEntryMagCalib : public ILoggerEntry, public ILoggerEntryKey + { + public: + //----------------------------------------------------------------------// + //- Settings/getters -// + //----------------------------------------------------------------------// + + /*! + * Returns the log name. + * + * \return log name. + */ + std::string getName() const override; + + /*! + * Returns the file name to use. + * + * The default implementation just appends a .txt to the log name. + * + * \return file name for this log. + */ + std::string getFileName() const override; + + /*! + * Returns false if the output file is text or true for binary. + * + * The default base implementation consider text file (false). + * + * \return false for text file and true for binary. + */ + bool isBinary() const override; + + private: + + //----------------------------------------------------------------------// + //- Private methods -// + //----------------------------------------------------------------------// + + /*! + * Write the data log to the file + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + }; }; #endif // SBG_LOGGER_ENTRY_MAG_H diff --git a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryShipMotion.h b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryShipMotion.h index 0f36ae2..3fc356a 100644 --- a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryShipMotion.h +++ b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryShipMotion.h @@ -1,12 +1,12 @@ /*! - * \file loggerEntryShipMotion.h - * \author SBG Systems - * \date March 14, 2023 + * \file loggerEntryShipMotion.h + * \author SBG Systems + * \date March 14, 2023 * - * \brief Implemented Ship Motion logs (heave/surge/sway). + * \brief Implemented Ship Motion logs (heave/surge/sway). * - * \copyright Copyright (C) 2023, SBG Systems SAS. All rights reserved. - * \beginlicense Proprietary license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense Proprietary license * * This source code is intended for use only by SBG Systems SAS and * those that have explicit written permission to use it from @@ -35,78 +35,78 @@ namespace sbg { - //----------------------------------------------------------------------// - //- Ship Motion Generic Implementation -// - //----------------------------------------------------------------------// + //----------------------------------------------------------------------// + //- Ship Motion Generic Implementation -// + //----------------------------------------------------------------------// - /*! - * Generic Handle SBG_ECOM_LOG_SHIP_MOTION_## - */ - class CLoggerEntryShipMotion : public ILoggerEntry - { - private: - - //----------------------------------------------------------------------// - //- Private methods -// - //----------------------------------------------------------------------// + /*! + * Generic Handle SBG_ECOM_LOG_SHIP_MOTION_## + */ + class CLoggerEntryShipMotion : public ILoggerEntry + { + private: + + //----------------------------------------------------------------------// + //- Private methods -// + //----------------------------------------------------------------------// - /*! - * Write the header to the file and/or console. - * - * \param[in] context Logger context and settings. - */ - void writeHeaderToFile(const CLoggerContext &context) override; + /*! + * Write the header to the file and/or console. + * + * \param[in] context Logger context and settings. + */ + void writeHeaderToFile(const CLoggerContext &context) override; - /*! - * Write the data log to the file - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + /*! + * Write the data log to the file + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - /*! - * Write the data log to the console - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; - }; + /*! + * Write the data log to the console + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) override; + }; - //----------------------------------------------------------------------// - //- Ship Motion Specializations -// - //----------------------------------------------------------------------// + //----------------------------------------------------------------------// + //- Ship Motion Specializations -// + //----------------------------------------------------------------------// - /*! - * Handle SBG_ECOM_LOG_SHIP_MOTION (real time) - */ - class CLoggerEntryShipMotionRt : public CLoggerEntryShipMotion, public ILoggerEntryKey - { - public: + /*! + * Handle SBG_ECOM_LOG_SHIP_MOTION (real time) + */ + class CLoggerEntryShipMotionRt : public CLoggerEntryShipMotion, public ILoggerEntryKey + { + public: - /*! - * Returns the log name. - * - * \return log name. - */ - std::string getName() const override; - }; + /*! + * Returns the log name. + * + * \return log name. + */ + std::string getName() const override; + }; - /*! - * Handle SBG_ECOM_LOG_SHIP_MOTION_HP (delayed heave) - */ - class CLoggerEntryShipMotionHp : public CLoggerEntryShipMotion, public ILoggerEntryKey - { - public: + /*! + * Handle SBG_ECOM_LOG_SHIP_MOTION_HP (delayed heave) + */ + class CLoggerEntryShipMotionHp : public CLoggerEntryShipMotion, public ILoggerEntryKey + { + public: - /*! - * Returns the log name. - * - * \return log name. - */ - std::string getName() const override; - }; + /*! + * Returns the log name. + * + * \return log name. + */ + std::string getName() const override; + }; }; #endif // SBG_LOGGER_ENTRY_SHIP_MOTION_H diff --git a/tools/sbgBasicLogger/src/loggerManager/loggerContext.cpp b/tools/sbgBasicLogger/src/loggerManager/loggerContext.cpp index ae3116a..c93afdb 100644 --- a/tools/sbgBasicLogger/src/loggerManager/loggerContext.cpp +++ b/tools/sbgBasicLogger/src/loggerManager/loggerContext.cpp @@ -1,9 +1,11 @@ // STL headers #include +#include +#include +#include #include #include -#include -#include +#include // sbgCommonLib headers #include @@ -67,7 +69,7 @@ std::string CLoggerContext::getTimeUnit() const { if (getSettings().getTimeMode() == CLoggerSettings::TimeMode::UtcIso8601) { - return "(yyyy-mm-ddThh:mm:ss.sssZ)"; + return "(yyyy-mm-ddThh:mm:ss.ssssssZ)"; } else { @@ -77,43 +79,63 @@ std::string CLoggerContext::getTimeUnit() const std::string CLoggerContext::fmtTime(uint32_t timeStampUs) const { - char fullTimeStr[sizeof("yyyy-mm-ddThh:mm:ss.sssZ")]; + char fullTimeStr[sizeof("yyyy-mm-ddThh:mm:ss.ssssssZ")]; + + // + // We have to do arithemtics on time_t so make sure it is defined as an integer type + // + static_assert(std::is_integral::value); if (getSettings().getTimeMode() == CLoggerSettings::TimeMode::UtcIso8601) { if (isUtcTimeValid()) { - char timeSecStr[sizeof("yyyy-mm-ddThh:mm:ss")]; - std::tm utcTime{}; - - utcTime.tm_year = m_lastUtcTime.year-1900; - utcTime.tm_mon = m_lastUtcTime.month-1; - utcTime.tm_mday = m_lastUtcTime.day; + char timeSecStr[sizeof("yyyy-mm-ddThh:mm:ss")]; + std::tm utcTimeTm{}; + uint32_t remainingUs; - utcTime.tm_hour = m_lastUtcTime.hour; - utcTime.tm_min = m_lastUtcTime.minute; - utcTime.tm_sec = m_lastUtcTime.second; + // + // Convert the latest UTC date time to a time_t to ease date/time arithmetics + // + utcTimeTm.tm_year = m_lastUtcTime.year-1900; + utcTimeTm.tm_mon = m_lastUtcTime.month-1; + utcTimeTm.tm_mday = m_lastUtcTime.day; + + utcTimeTm.tm_hour = m_lastUtcTime.hour; + utcTimeTm.tm_min = m_lastUtcTime.minute; + utcTimeTm.tm_sec = m_lastUtcTime.second; + + utcTimeTm.tm_isdst = 0; // - // Output standard UTC time only + // As the utcTime is only able to represent plain seconds, correct for any residual nanoSecond value // - utcTime.tm_isdst = 0; - + std::time_t utcTime = std::mktime(&utcTimeTm); + int64_t utcTimeUs = utcTime * 1000000; + uint32_t utcTimeStampUs = (m_lastUtcTime.timeStamp - m_lastUtcTime.nanoSecond/1000); + int32_t elapsedTimeUs = timeStampUs - utcTimeStampUs; + // - // Apply the offset between the latest UTC time and the current time stamp + // Apply the offset between the latest UTC time and the current timestamp // - std::time_t utcTimeSecs = std::mktime(&utcTime); - int32_t elapsedTimeUs = timeStampUs - m_lastUtcTime.timeStamp; - int32_t elapsedTime = elapsedTimeUs/1000; - int32_t remainingUs = elapsedTimeUs - elapsedTime*1000; + utcTimeUs = utcTimeUs + elapsedTimeUs; + utcTime = (time_t)(utcTimeUs/1000000); + remainingUs = (uint32_t)(utcTimeUs - (int64_t)utcTime*1000000); - utcTimeSecs = utcTimeSecs + elapsedTime; - // // Write ISO 8601 time: yyyy-mm-ddThh:mm:ss.sssZ // - std::strftime(timeSecStr, sizeof(timeSecStr), "%FT%T", &utcTime); - std::snprintf(fullTimeStr, sizeof(fullTimeStr), "%s.%03" PRId32 "Z", timeSecStr, remainingUs); + if (std::strftime(timeSecStr, sizeof(timeSecStr), "%FT%T", std::gmtime(&utcTime)) == 0) + { + throw std::overflow_error("timeSecStr is too small"); + } + + int ret = std::snprintf(fullTimeStr, sizeof(fullTimeStr), "%s.%06" PRIu32 "Z", timeSecStr, remainingUs); + + if (ret >= sizeof(fullTimeStr)) + { + throw std::overflow_error("fullTimeStr is too small"); + } } else { diff --git a/tools/sbgBasicLogger/src/loggerManager/loggerContext.h b/tools/sbgBasicLogger/src/loggerManager/loggerContext.h index 2269dcf..67e3867 100644 --- a/tools/sbgBasicLogger/src/loggerManager/loggerContext.h +++ b/tools/sbgBasicLogger/src/loggerManager/loggerContext.h @@ -1,12 +1,12 @@ /*! - * \file loggerContext.h - * \author SBG Systems - * \date March 06, 2023 + * \file loggerContext.h + * \author SBG Systems + * \date March 06, 2023 * - * \brief Define the logger context such as current UTC time. + * \brief Define the logger context such as current UTC time. * - * \copyright Copyright (C) 2023, SBG Systems SAS. All rights reserved. - * \beginlicense Proprietary license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense Proprietary license * * This source code is intended for use only by SBG Systems SAS and * those that have explicit written permission to use it from @@ -39,103 +39,103 @@ namespace sbg { - /*! - * Logger shared context between all log handlers. - * Used to share logger settings and convert time stamp to absolute time. - */ - class CLoggerContext - { - public: - //----------------------------------------------------------------------// - //- Constructor / destructor -// - //----------------------------------------------------------------------// - - /*! - * Default constructor. - * - * \param[in] settings Logger settings to set. - */ - CLoggerContext(const CLoggerSettings &settings); - - //----------------------------------------------------------------------// - //- Getters/settings -// - //----------------------------------------------------------------------// - - /*! - * Returns the logger settings instance (read only). - * - * \return logger settings reference. - */ - const CLoggerSettings &getSettings() const; - - /*! - * Update the UTC time with a newly received information. - * - * \param[in] utcTime New UTC time information. - */ - void setUtcTime(const SbgEComLogUtc &utcTime); - - /*! - * Returns true if at least one valid UTC time information has been set. - * - * \return true if a valid UTC time information is available. - */ - bool isUtcTimeValid() const; - - /*! - * Returns for the selected time mode a header string. - * - * \return time header. - */ - std::string getTimeColTitle() const; - - /*! - * Returns for the selected time mode a header string. - * - * \return time header. - */ - std::string getTimeUnit() const; - - /*! - * Convert the INS time stamp in us to a ISO 8601 time. - * - * \param[in] timeStampUs Time stamp in us to convert. - * \return Corresponding time using ISO 8601 format. - */ - std::string fmtTime(uint32_t timeStampUs) const; - - /*! - * Convert a status unsigned integer to a string according to selected output format/ - * - * Status can be either output using decimal or hexadecimal format. - * - * \param[in] value The status unsigned value to convert. - * \param[in] width Optional number of digits to use for hexadecimal format. - * \return Status formatted as a string. - */ - template - std::string fmtStatus(T value, size_t width = sizeof(T)*2) const - { - std::stringstream outputStr; - - if (getSettings().getStatusFormat() == CLoggerSettings::StatusFormat::Hexadecimal) - { - outputStr.fill('0'); - outputStr << "0x"<< std::noshowbase << std::hex << std::setw(width) << value; - } - else - { - outputStr << static_cast(value); - } - - return outputStr.str(); - } - - private: - CLoggerSettings m_settings; /*!< Logger settings. */ - SbgEComLogUtc m_lastUtcTime; /*!< Last received UTC time if any. */ - bool m_utcTimeIsValid {false}; /*!< Set to true as soon as one valid UTC time has been received. It never goes back to false. */ - }; + /*! + * Logger shared context between all log handlers. + * Used to share logger settings and convert timestamp to absolute time. + */ + class CLoggerContext + { + public: + //----------------------------------------------------------------------// + //- Constructor / destructor -// + //----------------------------------------------------------------------// + + /*! + * Default constructor. + * + * \param[in] settings Logger settings to set. + */ + CLoggerContext(const CLoggerSettings &settings); + + //----------------------------------------------------------------------// + //- Getters/settings -// + //----------------------------------------------------------------------// + + /*! + * Returns the logger settings instance (read only). + * + * \return logger settings reference. + */ + const CLoggerSettings &getSettings() const; + + /*! + * Update the UTC time with a newly received information. + * + * \param[in] utcTime New UTC time information. + */ + void setUtcTime(const SbgEComLogUtc &utcTime); + + /*! + * Returns true if at least one valid UTC time information has been set. + * + * \return true if a valid UTC time information is available. + */ + bool isUtcTimeValid() const; + + /*! + * Returns for the selected time mode a header string. + * + * \return time header. + */ + std::string getTimeColTitle() const; + + /*! + * Returns for the selected time mode a header string. + * + * \return time header. + */ + std::string getTimeUnit() const; + + /*! + * Convert the INS timestamp in us to a ISO 8601 time. + * + * \param[in] timeStampUs Timestamp in us to convert. + * \return Corresponding time using ISO 8601 format. + */ + std::string fmtTime(uint32_t timeStampUs) const; + + /*! + * Convert a status unsigned integer to a string according to selected output format/ + * + * Status can be either output using decimal or hexadecimal format. + * + * \param[in] value The status unsigned value to convert. + * \param[in] width Optional number of digits to use for hexadecimal format. + * \return Status formatted as a string. + */ + template + std::string fmtStatus(T value, size_t width = sizeof(T)*2) const + { + std::stringstream outputStr; + + if (getSettings().getStatusFormat() == CLoggerSettings::StatusFormat::Hexadecimal) + { + outputStr.fill('0'); + outputStr << "0x"<< std::noshowbase << std::hex << std::setw(width) << value; + } + else + { + outputStr << static_cast(value); + } + + return outputStr.str(); + } + + private: + CLoggerSettings m_settings; /*!< Logger settings. */ + SbgEComLogUtc m_lastUtcTime; /*!< Last received UTC time if any. */ + bool m_utcTimeIsValid {false}; /*!< Set to true as soon as one valid UTC time has been received. It never goes back to false. */ + }; }; #endif // SBG_LOGGER_SETTINGS_H diff --git a/tools/sbgBasicLogger/src/loggerManager/loggerEntry.h b/tools/sbgBasicLogger/src/loggerManager/loggerEntry.h index 7c37187..63dc717 100644 --- a/tools/sbgBasicLogger/src/loggerManager/loggerEntry.h +++ b/tools/sbgBasicLogger/src/loggerManager/loggerEntry.h @@ -1,12 +1,12 @@ /*! - * \file loggerEntry.h - * \author SBG Systems - * \date July 30, 2021 + * \file loggerEntry.h + * \author SBG Systems + * \date July 30, 2021 * - * \brief All log handlers should implement ILoggerEntryKey. + * \brief All log handlers should implement ILoggerEntryKey. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense Proprietary license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense Proprietary license * * This source code is intended for use only by SBG Systems SAS and * those that have explicit written permission to use it from @@ -39,183 +39,183 @@ namespace sbg { - /*! - * Small helper macro used to compute a unique identifier based on a sbgECom message class and id. - * - * \param[in] msgClass sbgECom message class. - * \param[in] msgId sbgECom message id. - * \return unique identifier for the sbgECom class/id pair. - */ - constexpr uint32_t sbgEComComputeKey(SbgEComClass msgClass, SbgEComMsgId msgId) - { - return ((uint32_t)msgClass << 8) | (uint32_t)msgId; - } - - /*! - * Base interface to implement a log handler. - */ - class ILoggerEntry - { - public: - //----------------------------------------------------------------------// - //- Constructor/destructor -// - //----------------------------------------------------------------------// - - /*! - * Default destructor. - */ - virtual ~ILoggerEntry(); - - //----------------------------------------------------------------------// - //- Public getters -// - //----------------------------------------------------------------------// - - /*! - * Returns the log name. - * - * \return log name. - */ - virtual std::string getName() const = 0; - - /*! - * Returns the file name to use. - * - * The default implementation just appends a .txt to the log name. - * - * \return file name for this log. - */ - virtual std::string getFileName() const; - - /*! - * Returns false if the output file is text or true for binary. - * - * The default base implementation consider text file (false). - * - * \return false for text file and true for binary. - */ - virtual bool isBinary() const; - - //----------------------------------------------------------------------// - //- Public methods -// - //----------------------------------------------------------------------// - - /*! - * Process a new incoming log. - * - * \param[in/out] context Logger context and settings. - * \param[in] logData Received sbgECom log data to process. - */ - void process(CLoggerContext &context, const SbgEComLogUnion &logData); - - private: - //----------------------------------------------------------------------// - //- Private methods -// - //----------------------------------------------------------------------// - - /*! - * Returns true if the data should be discarded because the time is invalid for example. - * - * \param[in] context Logger context and settings. - * \return true if data should be discarded because time is invalid or false otherwise. - */ - virtual bool getDiscardData(const CLoggerContext &context) const; - - /*! - * Open file fir written if needed using either binary of text format. - * - * \param[in] context Logger context and settings. - */ - void createFile(const CLoggerContext &context); - - /*! - * Called right before processing the incoming log. - * - * This method doesn't nothing and should be overriden if specific pre-processing is required. - * - * \param[in/out] context Logger context and settings. - * \param[in] logData Received sbgECom log data to process. - * \return true to continue processing the log or false to skip/discard it. - */ - virtual bool preProcess(CLoggerContext &context, const SbgEComLogUnion &logData); - - /*! - * Write the header to the file. - * - * \param[in] context Logger context and settings. - */ - virtual void writeHeaderToFile(const CLoggerContext &context); - - /*! - * Write the data log to the file - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - virtual void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) = 0; - - /*! - * Write the data log to the console - * - * \param[in] context Logger context and settings. - * \param[in] logData Input sbgECom log data to write. - */ - virtual void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData); - - protected: - //----------------------------------------------------------------------// - //- Protected members -// - //----------------------------------------------------------------------// - - std::ofstream m_outFile; /*!< Output file to write data to. */ - - private: - //----------------------------------------------------------------------// - //- Private members -// - //----------------------------------------------------------------------// - - bool m_headerWritten {false}; /*!< true if the header has already been written. */ - }; - - /*! - * Interface that adds a unique key identifier based on sbgECom class/id. - * - * All sbgECom log handlers should implement this interface. - * Note: C++ (11) doesn't support string litterals for template non-type arguments. - */ - template - class ILoggerEntryKey - { - public: - /*! - * Returns the sbgECom message class managed by this handler. - * - * \return The managed sbgECom message class. - */ - static constexpr SbgEComClass getClass() - { - return msgClass; - } - - /*! - * Returns the sbgECom message id managed by this handler. - * - * \return The managed sbgECom message id. - */ - static constexpr SbgEComMsgId getId() - { - return msgId; - } - - /*! - * Returns a unique key based on the sbgECom message class & id. - * - * \return Unique identifier that manages this message. - */ - static constexpr uint32_t getKey() - { - return sbgEComComputeKey(msgClass, msgId); - } - }; + /*! + * Small helper macro used to compute a unique identifier based on a sbgECom message class and id. + * + * \param[in] msgClass sbgECom message class. + * \param[in] msgId sbgECom message id. + * \return unique identifier for the sbgECom class/id pair. + */ + constexpr uint32_t sbgEComComputeKey(SbgEComClass msgClass, SbgEComMsgId msgId) + { + return ((uint32_t)msgClass << 8) | (uint32_t)msgId; + } + + /*! + * Base interface to implement a log handler. + */ + class ILoggerEntry + { + public: + //----------------------------------------------------------------------// + //- Constructor/destructor -// + //----------------------------------------------------------------------// + + /*! + * Default destructor. + */ + virtual ~ILoggerEntry(); + + //----------------------------------------------------------------------// + //- Public getters -// + //----------------------------------------------------------------------// + + /*! + * Returns the log name. + * + * \return log name. + */ + virtual std::string getName() const = 0; + + /*! + * Returns the file name to use. + * + * The default implementation just appends a .txt to the log name. + * + * \return file name for this log. + */ + virtual std::string getFileName() const; + + /*! + * Returns false if the output file is text or true for binary. + * + * The default base implementation consider text file (false). + * + * \return false for text file and true for binary. + */ + virtual bool isBinary() const; + + //----------------------------------------------------------------------// + //- Public methods -// + //----------------------------------------------------------------------// + + /*! + * Process a new incoming log. + * + * \param[in/out] context Logger context and settings. + * \param[in] logData Received sbgECom log data to process. + */ + void process(CLoggerContext &context, const SbgEComLogUnion &logData); + + private: + //----------------------------------------------------------------------// + //- Private methods -// + //----------------------------------------------------------------------// + + /*! + * Returns true if the data should be discarded because the time is invalid for example. + * + * \param[in] context Logger context and settings. + * \return true if data should be discarded because time is invalid or false otherwise. + */ + virtual bool getDiscardData(const CLoggerContext &context) const; + + /*! + * Open file fir written if needed using either binary of text format. + * + * \param[in] context Logger context and settings. + */ + void createFile(const CLoggerContext &context); + + /*! + * Called right before processing the incoming log. + * + * This method doesn't nothing and should be overriden if specific pre-processing is required. + * + * \param[in/out] context Logger context and settings. + * \param[in] logData Received sbgECom log data to process. + * \return true to continue processing the log or false to skip/discard it. + */ + virtual bool preProcess(CLoggerContext &context, const SbgEComLogUnion &logData); + + /*! + * Write the header to the file. + * + * \param[in] context Logger context and settings. + */ + virtual void writeHeaderToFile(const CLoggerContext &context); + + /*! + * Write the data log to the file + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + virtual void writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) = 0; + + /*! + * Write the data log to the console + * + * \param[in] context Logger context and settings. + * \param[in] logData Input sbgECom log data to write. + */ + virtual void writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData); + + protected: + //----------------------------------------------------------------------// + //- Protected members -// + //----------------------------------------------------------------------// + + std::ofstream m_outFile; /*!< Output file to write data to. */ + + private: + //----------------------------------------------------------------------// + //- Private members -// + //----------------------------------------------------------------------// + + bool m_headerWritten {false}; /*!< true if the header has already been written. */ + }; + + /*! + * Interface that adds a unique key identifier based on sbgECom class/id. + * + * All sbgECom log handlers should implement this interface. + * Note: C++ (11) doesn't support string litterals for template non-type arguments. + */ + template + class ILoggerEntryKey + { + public: + /*! + * Returns the sbgECom message class managed by this handler. + * + * \return The managed sbgECom message class. + */ + static constexpr SbgEComClass getClass() + { + return msgClass; + } + + /*! + * Returns the sbgECom message id managed by this handler. + * + * \return The managed sbgECom message id. + */ + static constexpr SbgEComMsgId getId() + { + return msgId; + } + + /*! + * Returns a unique key based on the sbgECom message class & id. + * + * \return Unique identifier that manages this message. + */ + static constexpr uint32_t getKey() + { + return sbgEComComputeKey(msgClass, msgId); + } + }; }; #endif // SBG_LOGGER_ENTRY_H diff --git a/tools/sbgBasicLogger/src/loggerManager/loggerManager.cpp b/tools/sbgBasicLogger/src/loggerManager/loggerManager.cpp index 4c969df..6efc6dd 100644 --- a/tools/sbgBasicLogger/src/loggerManager/loggerManager.cpp +++ b/tools/sbgBasicLogger/src/loggerManager/loggerManager.cpp @@ -1,4 +1,6 @@ // STL headers +#include +#include #include #include @@ -34,8 +36,12 @@ m_context(settings) { throw std::runtime_error("unable to init sbgECom library"); } - + sbgEComSetReceiveLogCallback(&m_ecomHandle, onSbgEComLogReceived, this); + + sbgEComSessionInfoCtxConstruct(&m_ecomSessionInfoCtx); + + m_sessionInfoFileId = 0; } CLoggerManager::~CLoggerManager() @@ -127,7 +133,6 @@ void CLoggerManager::openInterface() SbgErrorCode CLoggerManager::onSbgEComLogReceived(SbgEComHandle *pHandle, SbgEComClass msgClass, SbgEComMsgId msgId, const SbgEComLogUnion *pLogData, void *pUserArg) { - SbgErrorCode errorCode = SBG_NO_ERROR; CLoggerManager *pLogManager; assert(pHandle); @@ -135,9 +140,17 @@ SbgErrorCode CLoggerManager::onSbgEComLogReceived(SbgEComHandle *pHandle, SbgECo assert(pUserArg); pLogManager = static_cast(pUserArg); - pLogManager->processLog(msgClass, msgId, *pLogData); + + if ((msgClass == SBG_ECOM_CLASS_LOG_ECOM_0) && (msgId == SBG_ECOM_LOG_SESSION_INFO)) + { + pLogManager->processSessionInformation(&pLogData->sessionInfoData); + } + else + { + pLogManager->processLog(msgClass, msgId, *pLogData); + } - return errorCode; + return SBG_NO_ERROR; } void CLoggerManager::processLog(SbgEComClass msgClass, SbgEComMsgId msgId, const SbgEComLogUnion &logData) @@ -152,4 +165,41 @@ void CLoggerManager::processLog(SbgEComClass msgClass, SbgEComMsgId msgId, const } } +void CLoggerManager::processSessionInformation(const SbgEComLogSessionInfo *pSessionInfoData) +{ + SbgErrorCode errorCode; + + assert(pSessionInfoData); + + errorCode = sbgEComSessionInfoCtxProcess(&m_ecomSessionInfoCtx, pSessionInfoData->pageIndex, pSessionInfoData->nrPages, pSessionInfoData->buffer, pSessionInfoData->size); + + if (errorCode == SBG_NO_ERROR) + { + std::string string; + + string = sbgEComSessionInfoCtxGetString(&m_ecomSessionInfoCtx); + + if (string != m_ecomSessionInfoString) + { + m_ecomSessionInfoString = string; + + if (m_context.getSettings().getWriteToConsole()) + { + std::cout << "session information received, size:" << m_ecomSessionInfoString.length() << "\n"; + } + + if (m_context.getSettings().getWriteToFile()) + { + std::ofstream outFile; + + outFile.open(m_context.getSettings().getBasePath() + "sessionInfo_" + std::to_string(m_sessionInfoFileId) + ".json", std::ofstream::out | std::ofstream::trunc); + outFile << m_ecomSessionInfoString; + outFile.close(); + + m_sessionInfoFileId++; + } + } + } +} + }; // namespace sbg diff --git a/tools/sbgBasicLogger/src/loggerManager/loggerManager.h b/tools/sbgBasicLogger/src/loggerManager/loggerManager.h index 635e9e8..cabc885 100644 --- a/tools/sbgBasicLogger/src/loggerManager/loggerManager.h +++ b/tools/sbgBasicLogger/src/loggerManager/loggerManager.h @@ -1,12 +1,12 @@ /*! - * \file loggerManager.h - * \author SBG Systems - * \date March 03, 2023 + * \file loggerManager.h + * \author SBG Systems + * \date March 03, 2023 * - * \brief Basic logger implementation. + * \brief Basic logger implementation. * - * \copyright Copyright (C) 2023, SBG Systems SAS. All rights reserved. - * \beginlicense Proprietary license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense Proprietary license * * This source code is intended for use only by SBG Systems SAS and * those that have explicit written permission to use it from @@ -46,121 +46,131 @@ namespace sbg { - /*! - * Basic logger manager implementation. - * - * This class parse the incoming sbgECom log and call the - * appropriate handler to write the data to file/console. - */ - class CLoggerManager - { - public: - //----------------------------------------------------------------------// - //- Public definitions -// - //----------------------------------------------------------------------// - - /*! - * Returns status about the input interface - */ - enum class StreamStatus - { - HasMoreData, /*!< The input stream has pending data. */ - NoMoreData, /*!< There is not more pending data but more may arrive later. */ - EndOfStream, /*!< The end of stream has been reached and the processing should stop. */ - }; - - //----------------------------------------------------------------------// - //- Constructor/destructor -// - //----------------------------------------------------------------------// - - /*! - * Default constructor. - * - * \param[in] settings Logger settings to use. - * \throw std::runtime_error if - */ - CLoggerManager(const CLoggerSettings &settings); - - /*! - * Default destructor. - */ - ~CLoggerManager(); - - //----------------------------------------------------------------------// - //- Public methods -// - //----------------------------------------------------------------------// - - /*! - * Register a new log handler. - * - * Example: registerLog(); - */ - template - void registerLog() - { - m_logList.emplace(T::getKey(), std::make_unique()); - } - - /*! - * Process one incoming log from the interface. - * - * \return the stream status to check if we should stop the processing. - */ - StreamStatus processOneLog(); - - private: - //----------------------------------------------------------------------// - //- Private methods -// - //----------------------------------------------------------------------// - - /*! - * Open the input interface according to configuration. - * - * \throw std::invalid_argument if the interface can't be opened. - * std::logic_error if an other interface is already opened. - */ - void openInterface(); - - /*! - * Method called by the sbgECom each time a new log is received. - * - * \param[in] pHandle Valid handle on the sbgECom instance that has called this callback. - * \param[in] msgClass Received log message class. - * \param[in] msgId Received log message id. - * \param[in] logData Union storing the received data. - * \param[in] pUserArg User argument that should reference the log manager. - * \return SBG_NO_ERROR if the received log has been used successfully. - */ - static SbgErrorCode onSbgEComLogReceived(SbgEComHandle *pHandle, SbgEComClass msgClass, SbgEComMsgId msgId, const SbgEComLogUnion *pLogData, void *pUserArg); - - /*! - * Process an incoming log. - * - * \param[in] msgClass Received log message class. - * \param[in] msgId Received log message id. - * \param[in] logData Union storing the received data. - */ - void processLog(SbgEComClass msgClass, SbgEComMsgId msgId, const SbgEComLogUnion &logData); - - private: - //----------------------------------------------------------------------// - //- Private definitions -// - //----------------------------------------------------------------------// - - /*! - * Typedef to store per sbgECom message ID the associated log handler. - */ - typedef std::unordered_map> LogHandlers; - - //----------------------------------------------------------------------// - //- Private members -// - //----------------------------------------------------------------------// - SbgInterface m_interface; /*!< interface to read log from. */ - SbgEComHandle m_ecomHandle; /*!< sbgECom handler. */ - - LogHandlers m_logList; /*!< List of managed logs. */ - CLoggerContext m_context; /*!< Logger context & settings. */ - }; + /*! + * Basic logger manager implementation. + * + * This class parse the incoming sbgECom log and call the + * appropriate handler to write the data to file/console. + */ + class CLoggerManager + { + public: + //----------------------------------------------------------------------// + //- Public definitions -// + //----------------------------------------------------------------------// + + /*! + * Returns status about the input interface + */ + enum class StreamStatus + { + HasMoreData, /*!< The input stream has pending data. */ + NoMoreData, /*!< There is not more pending data but more may arrive later. */ + EndOfStream, /*!< The end of stream has been reached and the processing should stop. */ + }; + + //----------------------------------------------------------------------// + //- Constructor/destructor -// + //----------------------------------------------------------------------// + + /*! + * Default constructor. + * + * \param[in] settings Logger settings to use. + * \throw std::runtime_error if + */ + CLoggerManager(const CLoggerSettings &settings); + + /*! + * Default destructor. + */ + ~CLoggerManager(); + + //----------------------------------------------------------------------// + //- Public methods -// + //----------------------------------------------------------------------// + + /*! + * Register a new log handler. + * + * Example: registerLog(); + */ + template + void registerLog() + { + m_logList.emplace(T::getKey(), std::make_unique()); + } + + /*! + * Process one incoming log from the interface. + * + * \return the stream status to check if we should stop the processing. + */ + StreamStatus processOneLog(); + + private: + //----------------------------------------------------------------------// + //- Private methods -// + //----------------------------------------------------------------------// + + /*! + * Open the input interface according to configuration. + * + * \throw std::invalid_argument if the interface can't be opened. + * std::logic_error if an other interface is already opened. + */ + void openInterface(); + + /*! + * Method called by the sbgECom each time a new log is received. + * + * \param[in] pHandle Valid handle on the sbgECom instance that has called this callback. + * \param[in] msgClass Received log message class. + * \param[in] msgId Received log message id. + * \param[in] logData Union storing the received data. + * \param[in] pUserArg User argument that should reference the log manager. + * \return SBG_NO_ERROR if the received log has been used successfully. + */ + static SbgErrorCode onSbgEComLogReceived(SbgEComHandle *pHandle, SbgEComClass msgClass, SbgEComMsgId msgId, const SbgEComLogUnion *pLogData, void *pUserArg); + + /*! + * Process an incoming log. + * + * \param[in] msgClass Received log message class. + * \param[in] msgId Received log message id. + * \param[in] logData Union storing the received data. + */ + void processLog(SbgEComClass msgClass, SbgEComMsgId msgId, const SbgEComLogUnion &logData); + + /*! + * Process session information data. + * + * \param[in] pSessionInfoData Session information data. + */ + void processSessionInformation(const SbgEComLogSessionInfo *pSessionInfoData); + + private: + //----------------------------------------------------------------------// + //- Private definitions -// + //----------------------------------------------------------------------// + + /*! + * Typedef to store per sbgECom message ID the associated log handler. + */ + typedef std::unordered_map> LogHandlers; + + //----------------------------------------------------------------------// + //- Private members -// + //----------------------------------------------------------------------// + SbgInterface m_interface; /*!< interface to read log from. */ + SbgEComHandle m_ecomHandle; /*!< sbgECom handler. */ + std::string m_ecomSessionInfoString; /*!< Session information string. */ + SbgEComSessionInfoCtx m_ecomSessionInfoCtx; /*!< sbgECom session information context. */ + uint32_t m_sessionInfoFileId; /*!< Identifier of the file used to log the session information. */ + + LogHandlers m_logList; /*!< List of managed logs. */ + CLoggerContext m_context; /*!< Logger context & settings. */ + }; } #endif // SBG_BASIC_LOGGER_HANDLER_H diff --git a/tools/sbgBasicLogger/src/loggerManager/loggerSettings.h b/tools/sbgBasicLogger/src/loggerManager/loggerSettings.h index 2f1f3a0..084f800 100644 --- a/tools/sbgBasicLogger/src/loggerManager/loggerSettings.h +++ b/tools/sbgBasicLogger/src/loggerManager/loggerSettings.h @@ -1,12 +1,12 @@ /*! - * \file loggerSettings.h - * \author SBG Systems - * \date March 06, 2023 + * \file loggerSettings.h + * \author SBG Systems + * \date March 06, 2023 * - * \brief Define the logger settings. + * \brief Define the logger settings. * - * \copyright Copyright (C) 2023, SBG Systems SAS. All rights reserved. - * \beginlicense Proprietary license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense Proprietary license * * This source code is intended for use only by SBG Systems SAS and * those that have explicit written permission to use it from @@ -34,298 +34,298 @@ namespace sbg { - /*! - * General logger settings as set by the CLI - */ - class CLoggerSettings - { - public: - //----------------------------------------------------------------------// - //- Public definitions -// - //----------------------------------------------------------------------// - - /*! - * Defines time field output format. - */ - enum class TimeMode - { - TimeStamp, /*!< Use the internal time stamp value in us. */ - UtcIso8601 /*!< Use an absolute UTC time using ISO 8601 format. */ - }; - - /*! - * Defines status field output format. - */ - enum class StatusFormat - { - Decimal, /*!< Output status using a decimal format. */ - Hexadecimal /*!< Output status using an hexadecimal format (ie 0x0000AABB). */ - }; - - /*! - * Defines the interface mode to use. - */ - enum class InterfaceMode - { - Unknown, /*!< Unknown / undefined logger interface type. */ - Serial, /*!< Logger is setup to read data from a serial interface. */ - Udp, /*!< Logger is setup to read data from an Ethernet UDP interface. */ - File /*!< Logger is setup to read data from a file. */ - }; - - /*! - * Settings for a serial interface. - */ - struct Serial - { - std::string location {}; /*!< Serial port location. */ - uint32_t baudRate {0}; /*!< Serial baud rate. */ - }; - - /*! - * Settings for an UDP interface - */ - struct Udp - { - std::string remoteIp {}; /*!< device IPv4 address to connect to ("a.b.c.d"). */ - uint32_t remotePort {0}; /*!< device port number to send data to. */ - uint32_t listenPort {0}; /*!< computer listen port on which we should receive data. */ - }; - - //----------------------------------------------------------------------// - //- Public getters & setters -// - //----------------------------------------------------------------------// - - /*! - * Define the base path to store all logs to - * - * \param[in] basePath The base path to store logs to. - */ - void setBasePath(const std::string &basePath); - - /*! - * Returns the base path. - * - * \return The base path ending with '\' - */ - const std::string &getBasePath() const; - - /*! - * Set if logs should be written to the console. - * - * \param[in] writeToConsole true ti write logs to console. - */ - void setWriteToConsole(bool writeToConsole); - - /*! - * Returns if the log should be written to the console. - * - * \return true if logs should be written to the console. - */ - bool getWriteToConsole() const; - - /*! - * Set if logs should be written to files. - * - * \param[in] writeToFile true ti write logs to file. - */ - void setWriteToFile(bool writeToFile); - - /*! - * Returns if the log should be written to files. - * - * \return true if logs should be written to file. - */ - bool getWriteToFile() const; - - /*! - * Set if the headers should be written to file. - * - * \param[in] writeHeaderToFile true to write headers to file or false otherwise. - */ - void setWriteHeaderToFile(bool writeHeaderToFile); - - /*! - * Returns if headers should be written to file. - * - * \return true if headers should be written to file or false otherwise. - */ - bool getWriteHeaderToFile() const; - - /*! - * Set the decimation factor for the file output. - * - * \param[in] consoleDecimation Decimation factor from 1 to 10 000. - * \throw std::invalid_argument if the decimation is invalid. - */ - void setFileDecimation(int32_t fileDecimation); - - /*! - * Returns the decimation factor for the file output. - * - * \return The decimation factor. - */ - int32_t getFileDecimation() const; - - /*! - * Set the decimation factor for the console output. - * - * \param[in] consoleDecimation Decimation factor from 1 to 10 000. - * \throw std::invalid_argument if the decimation is invalid. - */ - void setConsoleDecimation(int32_t consoleDecimation); - - /*! - * Returns the decimation factor for the console output. - * - * \return The decimation factor. - */ - int32_t getConsoleDecimation() const; - - /*! - * Set if data with invalid time information should be discarded. - * - * \param[in] discardInvalidTime true to discard data with an invalid time information. - */ - void setDiscardInvalidTime(bool discardInvalidTime); - - /*! - * Returns true if data with invalid time should be discarded or false otherwise. - * - * \return true if data with invalid time should be discarded. - */ - bool getDiscardInvalidTime() const; - - /*! - * Set the time mode/format to use. - * - * \param[in] timeMode the time mode to use. - */ - void setTimeMode(TimeMode timeMode); - - /*! - * Returns the time mode to use. - * - * \return the time mode to use. - */ - TimeMode getTimeMode() const; - - /*! - * Set the status output format. - * - * \param[in] statusFormat the status output format. - */ - void setStatusFormat(StatusFormat statusFormat); - - /*! - * Returns the status output format. - * - * \return the status output format. - */ - StatusFormat getStatusFormat() const; - - /*! - * Returns true if a valid output configuration is set. - * - * A valid output configuration is either the console or the file or both. - * - * \return true if there is a valid output configuration. - */ - bool isOutputConfValid() const; - - /*! - * Returns the configured interface mode. - * - * \return the interface mode that is setup. - */ - InterfaceMode getInterfaceMode() const; - - /*! - * Returns true if a valid serial or UDP or file interface is configured. - * - * \return true if there is a valid interface configuration. - */ - bool hasInterfaceConf() const; - - /*! - * Define the configuration for a serial interface. - * - * \param[in] serialConf The serial configuration to set. - * \throw std::logic_error if this configuration overrides a previous one. - */ - void setSerialConf(const Serial &serialConf); - - /*! - * Returns the serial configuration if any. - * - * \return The serial configuration if any. - * \throw std::logic_error if no valid serial configuration has been set. - */ - const Serial &getSerialConf() const; - - /*! - * Define the configuration for an UDP interface. - * - * \param[in] udpConf The UDP configuration to set. - * \throw std::logic_error if this configuration overrides a previous one. - */ - void setUdpConf(const Udp &udpConf); - - /*! - * Returns the UDP configuration if any. - * - * \return The UDP configuration if any. - * \throw std::logic_error if no valid UDP configuration has been set. - */ - const Udp &getUdpConf() const; - - /*! - * Define the configuration for a file interface. - * - * \param[in] fileConf The file configuration to set (ie file path to open). - * \throw std::logic_error if this configuration overrides a previous one. - */ - void setFileConf(const std::string &fileConf); - - /*! - * Returns the file configuration if any. - * - * \return The file configuration if any. - * \throw std::logic_error if no valid file configuration has been set. - */ - const std::string &getFileConf() const; - - /*! - * Returns true if the settings are valid and can be used. - * - * \return true if settings are valid. - */ - bool isValid() const; - - private: - //----------------------------------------------------------------------// - //- Private members -// - //----------------------------------------------------------------------// - - std::string m_basePath {}; /*!< Base path to output all files to. */ - bool m_writeToConsole {false}; /*!< Set to true to output the received logs to the console. */ - bool m_writeToFile {false}; /*!< Set to true to output the receive logs to the file. */ - bool m_writeHeaderToFile {false}; /*!< Set to true to write the header to the file. */ - int32_t m_fileDecimation {1}; /*!< File decimation factor >= 1. */ - int32_t m_consoleDecimation {1}; /*!< Console decimation factor >= 1. */ - bool m_discardInvalidTime {false}; /*!< If set to true, don't output data with an invalid time. */ - TimeMode m_timeMode {TimeMode::TimeStamp}; /*!< Define how to output time information in files. */ - StatusFormat m_statusFormat {StatusFormat::Hexadecimal}; /*!< Define the status output format. */ - - // - // Interface configuration are exclusive but in C++ 14 we don't have variant - // TODO: Might be reworked with a proper IInterfaceSettings base class and so on - // - InterfaceMode m_interfaceMode {InterfaceMode::Unknown}; /*!< Define the interface mode to use. */ - std::string m_fileConf {}; /*!< File interface configuration (ie file name). */ - Serial m_serialConf {}; /*!< Serial interface configuration. */ - Udp m_udpConf {}; /*!< UDP interface configuration. */ - }; + /*! + * General logger settings as set by the CLI + */ + class CLoggerSettings + { + public: + //----------------------------------------------------------------------// + //- Public definitions -// + //----------------------------------------------------------------------// + + /*! + * Defines time field output format. + */ + enum class TimeMode + { + TimeStamp, /*!< Use the internal timestamp value in us. */ + UtcIso8601 /*!< Use an absolute UTC time using ISO 8601 format. */ + }; + + /*! + * Defines status field output format. + */ + enum class StatusFormat + { + Decimal, /*!< Output status using a decimal format. */ + Hexadecimal /*!< Output status using an hexadecimal format (ie 0x0000AABB). */ + }; + + /*! + * Defines the interface mode to use. + */ + enum class InterfaceMode + { + Unknown, /*!< Unknown / undefined logger interface type. */ + Serial, /*!< Logger is setup to read data from a serial interface. */ + Udp, /*!< Logger is setup to read data from an Ethernet UDP interface. */ + File /*!< Logger is setup to read data from a file. */ + }; + + /*! + * Settings for a serial interface. + */ + struct Serial + { + std::string location {}; /*!< Serial port location. */ + uint32_t baudRate {0}; /*!< Serial baud rate. */ + }; + + /*! + * Settings for an UDP interface + */ + struct Udp + { + std::string remoteIp {}; /*!< device IPv4 address to connect to ("a.b.c.d"). */ + uint32_t remotePort {0}; /*!< device port number to send data to. */ + uint32_t listenPort {0}; /*!< computer listen port on which we should receive data. */ + }; + + //----------------------------------------------------------------------// + //- Public getters & setters -// + //----------------------------------------------------------------------// + + /*! + * Define the base path to store all logs to + * + * \param[in] basePath The base path to store logs to. + */ + void setBasePath(const std::string &basePath); + + /*! + * Returns the base path. + * + * \return The base path ending with '\' + */ + const std::string &getBasePath() const; + + /*! + * Set if logs should be written to the console. + * + * \param[in] writeToConsole true ti write logs to console. + */ + void setWriteToConsole(bool writeToConsole); + + /*! + * Returns if the log should be written to the console. + * + * \return true if logs should be written to the console. + */ + bool getWriteToConsole() const; + + /*! + * Set if logs should be written to files. + * + * \param[in] writeToFile true ti write logs to file. + */ + void setWriteToFile(bool writeToFile); + + /*! + * Returns if the log should be written to files. + * + * \return true if logs should be written to file. + */ + bool getWriteToFile() const; + + /*! + * Set if the headers should be written to file. + * + * \param[in] writeHeaderToFile true to write headers to file or false otherwise. + */ + void setWriteHeaderToFile(bool writeHeaderToFile); + + /*! + * Returns if headers should be written to file. + * + * \return true if headers should be written to file or false otherwise. + */ + bool getWriteHeaderToFile() const; + + /*! + * Set the decimation factor for the file output. + * + * \param[in] consoleDecimation Decimation factor from 1 to 10 000. + * \throw std::invalid_argument if the decimation is invalid. + */ + void setFileDecimation(int32_t fileDecimation); + + /*! + * Returns the decimation factor for the file output. + * + * \return The decimation factor. + */ + int32_t getFileDecimation() const; + + /*! + * Set the decimation factor for the console output. + * + * \param[in] consoleDecimation Decimation factor from 1 to 10 000. + * \throw std::invalid_argument if the decimation is invalid. + */ + void setConsoleDecimation(int32_t consoleDecimation); + + /*! + * Returns the decimation factor for the console output. + * + * \return The decimation factor. + */ + int32_t getConsoleDecimation() const; + + /*! + * Set if data with invalid time information should be discarded. + * + * \param[in] discardInvalidTime true to discard data with an invalid time information. + */ + void setDiscardInvalidTime(bool discardInvalidTime); + + /*! + * Returns true if data with invalid time should be discarded or false otherwise. + * + * \return true if data with invalid time should be discarded. + */ + bool getDiscardInvalidTime() const; + + /*! + * Set the time mode/format to use. + * + * \param[in] timeMode the time mode to use. + */ + void setTimeMode(TimeMode timeMode); + + /*! + * Returns the time mode to use. + * + * \return the time mode to use. + */ + TimeMode getTimeMode() const; + + /*! + * Set the status output format. + * + * \param[in] statusFormat the status output format. + */ + void setStatusFormat(StatusFormat statusFormat); + + /*! + * Returns the status output format. + * + * \return the status output format. + */ + StatusFormat getStatusFormat() const; + + /*! + * Returns true if a valid output configuration is set. + * + * A valid output configuration is either the console or the file or both. + * + * \return true if there is a valid output configuration. + */ + bool isOutputConfValid() const; + + /*! + * Returns the configured interface mode. + * + * \return the interface mode that is setup. + */ + InterfaceMode getInterfaceMode() const; + + /*! + * Returns true if a valid serial or UDP or file interface is configured. + * + * \return true if there is a valid interface configuration. + */ + bool hasInterfaceConf() const; + + /*! + * Define the configuration for a serial interface. + * + * \param[in] serialConf The serial configuration to set. + * \throw std::logic_error if this configuration overrides a previous one. + */ + void setSerialConf(const Serial &serialConf); + + /*! + * Returns the serial configuration if any. + * + * \return The serial configuration if any. + * \throw std::logic_error if no valid serial configuration has been set. + */ + const Serial &getSerialConf() const; + + /*! + * Define the configuration for an UDP interface. + * + * \param[in] udpConf The UDP configuration to set. + * \throw std::logic_error if this configuration overrides a previous one. + */ + void setUdpConf(const Udp &udpConf); + + /*! + * Returns the UDP configuration if any. + * + * \return The UDP configuration if any. + * \throw std::logic_error if no valid UDP configuration has been set. + */ + const Udp &getUdpConf() const; + + /*! + * Define the configuration for a file interface. + * + * \param[in] fileConf The file configuration to set (ie file path to open). + * \throw std::logic_error if this configuration overrides a previous one. + */ + void setFileConf(const std::string &fileConf); + + /*! + * Returns the file configuration if any. + * + * \return The file configuration if any. + * \throw std::logic_error if no valid file configuration has been set. + */ + const std::string &getFileConf() const; + + /*! + * Returns true if the settings are valid and can be used. + * + * \return true if settings are valid. + */ + bool isValid() const; + + private: + //----------------------------------------------------------------------// + //- Private members -// + //----------------------------------------------------------------------// + + std::string m_basePath {}; /*!< Base path to output all files to. */ + bool m_writeToConsole {false}; /*!< Set to true to output the received logs to the console. */ + bool m_writeToFile {false}; /*!< Set to true to output the receive logs to the file. */ + bool m_writeHeaderToFile {false}; /*!< Set to true to write the header to the file. */ + int32_t m_fileDecimation {1}; /*!< File decimation factor >= 1. */ + int32_t m_consoleDecimation {1}; /*!< Console decimation factor >= 1. */ + bool m_discardInvalidTime {false}; /*!< If set to true, don't output data with an invalid time. */ + TimeMode m_timeMode {TimeMode::TimeStamp}; /*!< Define how to output time information in files. */ + StatusFormat m_statusFormat {StatusFormat::Hexadecimal}; /*!< Define the status output format. */ + + // + // Interface configuration are exclusive but in C++ 14 we don't have variant + // TODO: Might be reworked with a proper IInterfaceSettings base class and so on + // + InterfaceMode m_interfaceMode {InterfaceMode::Unknown}; /*!< Define the interface mode to use. */ + std::string m_fileConf {}; /*!< File interface configuration (ie file name). */ + Serial m_serialConf {}; /*!< Serial interface configuration. */ + Udp m_udpConf {}; /*!< UDP interface configuration. */ + }; }; #endif // SBG_LOGGER_SETTINGS_H diff --git a/tools/sbgBasicLogger/src/main.cpp b/tools/sbgBasicLogger/src/main.cpp index e3ded51..90f4d33 100644 --- a/tools/sbgBasicLogger/src/main.cpp +++ b/tools/sbgBasicLogger/src/main.cpp @@ -5,7 +5,7 @@ * * \brief Simple console application that parse and output sbgECom logs to CSV like files. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. * \beginlicense Proprietary license * * This source code is intended for use only by SBG Systems SAS and diff --git a/tools/sbgEComApi/src/main.c b/tools/sbgEComApi/src/main.c index 140b0a3..05fbcb0 100644 --- a/tools/sbgEComApi/src/main.c +++ b/tools/sbgEComApi/src/main.c @@ -1,12 +1,12 @@ /*! - * \file main.c - * \author SBG Systems - * \date October 27, 2020 + * \file main.c + * \author SBG Systems + * \date October 27, 2020 * - * \brief Tool to perform REST GET/POST queries through sbgECom commands. + * \brief Tool to perform REST GET/POST queries through sbgECom commands. * - * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. - * \beginlicense Proprietary license + * \copyright Copyright (C) 2007-2024, SBG Systems SAS. All rights reserved. + * \beginlicense Proprietary license * * This source code is intended for use only by SBG Systems SAS and * those that have explicit written permission to use it from @@ -43,17 +43,17 @@ /*! * Program name. */ -#define PROGRAM_NAME "sbgEComApi" +#define PROGRAM_NAME "sbgEComApi" /*! * Default number of attempts per command execution. */ -#define DEFAULT_CMD_NR_ATTEMPTS (3) +#define DEFAULT_CMD_NR_ATTEMPTS (3) /*! * Default time-out per command execution attempt, in seconds. */ -#define DEFAULT_CMD_TIMEOUT (5) +#define DEFAULT_CMD_TIMEOUT (5) //----------------------------------------------------------------------// //- Structure definitions -// @@ -64,9 +64,9 @@ */ typedef struct _ExitCodeDesc { - int exitCode; /*!< Exit code, on 7 bits. */ - uint16_t statusCode; /*!< Status code. */ - const char *pMessage; /*!< Message. */ + int exitCode; /*!< Exit code, on 7 bits. */ + uint16_t statusCode; /*!< Status code. */ + const char *pMessage; /*!< Message. */ } ExitCodeDesc; //----------------------------------------------------------------------// @@ -76,16 +76,16 @@ typedef struct _ExitCodeDesc /*! * Table of exit code descriptors. */ -static const ExitCodeDesc gExitCodeDescs[] = +static const ExitCodeDesc gExitCodeDescs[] = { - { .exitCode = 0, .statusCode = 200, .pMessage = "OK" }, - { .exitCode = 60, .statusCode = 400, .pMessage = "BAD REQUEST" }, - { .exitCode = 61, .statusCode = 401, .pMessage = "UNAUTHORIZED" }, - { .exitCode = 63, .statusCode = 403, .pMessage = "FORBIDDEN" }, - { .exitCode = 64, .statusCode = 404, .pMessage = "NOT FOUND" }, - { .exitCode = 69, .statusCode = 409, .pMessage = "CONFLICT" }, - { .exitCode = 82, .statusCode = 422, .pMessage = "UNPROCESSABLE ENTITY" }, - { .exitCode = 100, .statusCode = 200, .pMessage = "INTERNAL SERVER ERROR" }, + { .exitCode = 0, .statusCode = 200, .pMessage = "OK" }, + { .exitCode = 60, .statusCode = 400, .pMessage = "BAD REQUEST" }, + { .exitCode = 61, .statusCode = 401, .pMessage = "UNAUTHORIZED" }, + { .exitCode = 63, .statusCode = 403, .pMessage = "FORBIDDEN" }, + { .exitCode = 64, .statusCode = 404, .pMessage = "NOT FOUND" }, + { .exitCode = 69, .statusCode = 409, .pMessage = "CONFLICT" }, + { .exitCode = 82, .statusCode = 422, .pMessage = "UNPROCESSABLE ENTITY" }, + { .exitCode = 100, .statusCode = 200, .pMessage = "INTERNAL SERVER ERROR" }, }; //----------------------------------------------------------------------// @@ -95,62 +95,62 @@ static const ExitCodeDesc gExitCodeDescs[] = /*! * Look up the exit code descriptor matching the given status code. * - * \param[in] statusCode Status code. - * \return Exit code descriptor. + * \param[in] statusCode Status code. + * \return Exit code descriptor. */ static const ExitCodeDesc *lookupExitCodeDesc(uint16_t statusCode) { - const ExitCodeDesc *pDesc = NULL; + const ExitCodeDesc *pDesc = NULL; - for (size_t i = 0; i < SBG_ARRAY_SIZE(gExitCodeDescs); i++) - { - const ExitCodeDesc *pTmpDesc; + for (size_t i = 0; i < SBG_ARRAY_SIZE(gExitCodeDescs); i++) + { + const ExitCodeDesc *pTmpDesc; - pTmpDesc = &gExitCodeDescs[i]; + pTmpDesc = &gExitCodeDescs[i]; - if (pTmpDesc->statusCode == statusCode) - { - pDesc = pTmpDesc; - break; - } - } + if (pTmpDesc->statusCode == statusCode) + { + pDesc = pTmpDesc; + break; + } + } - return pDesc; + return pDesc; } /*! * Convert a status code into an exit code. * - * \param[in] statusCode Status code. - * \return Exit code. + * \param[in] statusCode Status code. + * \return Exit code. */ static int convertStatusCodeToExitCode(uint16_t statusCode) { - const ExitCodeDesc *pDesc; - int exitCode; + const ExitCodeDesc *pDesc; + int exitCode; - pDesc = lookupExitCodeDesc(statusCode); + pDesc = lookupExitCodeDesc(statusCode); - if (pDesc) - { - exitCode = pDesc->exitCode; - } - else - { - exitCode = EXIT_FAILURE; - } + if (pDesc) + { + exitCode = pDesc->exitCode; + } + else + { + exitCode = EXIT_FAILURE; + } - return exitCode; + return exitCode; } /*! * Print help about a single exit code / status code mapping. * - * \param[in] pDesc Exit code descriptor. + * \param[in] pDesc Exit code descriptor. */ static void printExitCodeMapping(const ExitCodeDesc *pDesc) { - printf(" %3u: %3u %s\n", pDesc->exitCode, pDesc->statusCode, pDesc->pMessage); + printf(" %3u: %3u %s\n", pDesc->exitCode, pDesc->statusCode, pDesc->pMessage); } /*! @@ -158,261 +158,261 @@ static void printExitCodeMapping(const ExitCodeDesc *pDesc) */ static void printExitCodeHelp(void) { - printf("Exit codes :\n"); + printf("Exit codes :\n"); - for (size_t i = 0; i < SBG_ARRAY_SIZE(gExitCodeDescs); i++) - { - printExitCodeMapping(&gExitCodeDescs[i]); - } + for (size_t i = 0; i < SBG_ARRAY_SIZE(gExitCodeDescs); i++) + { + printExitCodeMapping(&gExitCodeDescs[i]); + } - puts(""); - printf("EXIT_FAILURE for a general error unrelated to the status code or if the status code is unknown.\n\n"); + puts(""); + printf("EXIT_FAILURE for a general error unrelated to the status code or if the status code is unknown.\n\n"); } /*! * Callback definition used to route log error messages. * - * \param[in] pFileName The file in which the log was triggered. - * \param[in] pFunctionName The function where the log was triggered. - * \param[in] line The line in the file where the log was triggered. - * \param[in] pCategory Category for this log or "None" if no category has been specified. - * \param[in] logType Associated log message level. - * \param[in] errorCode Associated error code or SBG_NO_ERROR for INFO & DEBUG level logs. - * \param[in] pMessage The message to log. + * \param[in] pFileName The file in which the log was triggered. + * \param[in] pFunctionName The function where the log was triggered. + * \param[in] line The line in the file where the log was triggered. + * \param[in] pCategory Category for this log or "None" if no category has been specified. + * \param[in] logType Associated log message level. + * \param[in] errorCode Associated error code or SBG_NO_ERROR for INFO & DEBUG level logs. + * \param[in] pMessage The message to log. */ static void onLogCallback(const char *pFileName, const char *pFunctionName, uint32_t line, const char *pCategory, SbgDebugLogType type, SbgErrorCode errorCode, const char *pMessage) { - const char *pTypeStr; - const char *pBaseName; - - assert(pFileName); - assert(pFunctionName); - assert(pCategory); - assert(pMessage); - - pTypeStr = sbgDebugLogTypeToStr(type); - pBaseName = strrchr(pFileName, '/'); - - if (!pBaseName) - { - pBaseName = pFileName; - } - else - { - // - // Skip the slash. - // - pBaseName++; - } - - if (errorCode == SBG_NO_ERROR) - { - fprintf(stderr, "%-7s %s (%s:%" PRIu32 ") %s\n", pTypeStr, pFunctionName, pBaseName, line, pMessage); - } - else - { - fprintf(stderr, "%-7s err:%s %s (%s:%" PRIu32 ") %s\n", pTypeStr, sbgErrorCodeToString(errorCode), pFunctionName, pBaseName, line, pMessage); - } + const char *pTypeStr; + const char *pBaseName; + + assert(pFileName); + assert(pFunctionName); + assert(pCategory); + assert(pMessage); + + pTypeStr = sbgDebugLogTypeToStr(type); + pBaseName = strrchr(pFileName, '/'); + + if (!pBaseName) + { + pBaseName = pFileName; + } + else + { + // + // Skip the slash. + // + pBaseName++; + } + + if (errorCode == SBG_NO_ERROR) + { + fprintf(stderr, "%-7s %s (%s:%" PRIu32 ") %s\n", pTypeStr, pFunctionName, pBaseName, line, pMessage); + } + else + { + fprintf(stderr, "%-7s err:%s %s (%s:%" PRIu32 ") %s\n", pTypeStr, sbgErrorCodeToString(errorCode), pFunctionName, pBaseName, line, pMessage); + } } /*! * Read a string from a file. * - * \param[out] pString String. - * \param[in] pPath File path. - * \return SBG_NO_ERROR if successful. + * \param[out] pString String. + * \param[in] pPath File path. + * \return SBG_NO_ERROR if successful. */ static SbgErrorCode readStringFromFile(SbgString *pString, const char *pPath) { - SbgErrorCode errorCode; - FILE *pFile; - - assert(pString); - assert(pPath); - - pFile = fopen(pPath, "r"); - - if (pFile) - { - for (;;) - { - size_t size; - char buffer[4096]; - - size = fread(buffer, 1, sizeof(buffer) - 1, pFile); - - if (size != 0) - { - assert(size < sizeof(buffer)); - buffer[size] = '\0'; - - errorCode = sbgStringAppendCString(pString, buffer); - - if (errorCode == SBG_NO_ERROR) - { - char *pPtr; - - pPtr = memchr(buffer, '\0', size); - - if (pPtr) - { - break; - } - } - else - { - break; - } - } - else - { - if (ferror(pFile)) - { - errorCode = SBG_READ_ERROR; - SBG_LOG_ERROR(errorCode, "unable to read file %s", pPath); - } - else - { - errorCode = SBG_NO_ERROR; - } - - break; - } - } - - fclose(pFile); - } - else - { - errorCode = SBG_INVALID_PARAMETER; - SBG_LOG_ERROR(errorCode, "unable to open file %s", pPath); - } - - return errorCode; + SbgErrorCode errorCode; + FILE *pFile; + + assert(pString); + assert(pPath); + + pFile = fopen(pPath, "r"); + + if (pFile) + { + for (;;) + { + size_t size; + char buffer[4096]; + + size = fread(buffer, 1, sizeof(buffer) - 1, pFile); + + if (size != 0) + { + assert(size < sizeof(buffer)); + buffer[size] = '\0'; + + errorCode = sbgStringAppendCString(pString, buffer); + + if (errorCode == SBG_NO_ERROR) + { + char *pPtr; + + pPtr = memchr(buffer, '\0', size); + + if (pPtr) + { + break; + } + } + else + { + break; + } + } + else + { + if (ferror(pFile)) + { + errorCode = SBG_READ_ERROR; + SBG_LOG_ERROR(errorCode, "unable to read file %s", pPath); + } + else + { + errorCode = SBG_NO_ERROR; + } + + break; + } + } + + fclose(pFile); + } + else + { + errorCode = SBG_INVALID_PARAMETER; + SBG_LOG_ERROR(errorCode, "unable to open file %s", pPath); + } + + return errorCode; } /*! * Write a reply to an output file. * - * \param[in] pReply Reply. - * \param[in] writeStatus If true, print the status to the output file. - * \param[out] pOutputFile Output file. + * \param[in] pReply Reply. + * \param[in] writeStatus If true, print the status to the output file. + * \param[out] pOutputFile Output file. */ static SbgErrorCode writeReply(const SbgEComCmdApiReply *pReply, bool writeStatus, FILE *pOutputFile) { - SbgErrorCode errorCode; - - if (writeStatus) - { - int nrChars; - - nrChars = fprintf(pOutputFile, "%"PRIu16"\n", pReply->statusCode); - - if (nrChars >= 0) - { - errorCode = SBG_NO_ERROR; - } - else - { - errorCode = SBG_WRITE_ERROR; - SBG_LOG_ERROR(errorCode, "unable to write status code"); - } - } - else - { - errorCode = SBG_NO_ERROR; - } - - if (errorCode == SBG_NO_ERROR) - { - int nrChars; - - nrChars = fprintf(pOutputFile, "%s", pReply->pContent); - - if (nrChars >= 0) - { - errorCode = SBG_NO_ERROR; - } - else - { - errorCode = SBG_WRITE_ERROR; - SBG_LOG_ERROR(errorCode, "unable to write content"); - } - } - - return errorCode; + SbgErrorCode errorCode; + + if (writeStatus) + { + int nrChars; + + nrChars = fprintf(pOutputFile, "%"PRIu16"\n", pReply->statusCode); + + if (nrChars >= 0) + { + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_WRITE_ERROR; + SBG_LOG_ERROR(errorCode, "unable to write status code"); + } + } + else + { + errorCode = SBG_NO_ERROR; + } + + if (errorCode == SBG_NO_ERROR) + { + int nrChars; + + nrChars = fprintf(pOutputFile, "%s", pReply->pContent); + + if (nrChars >= 0) + { + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_WRITE_ERROR; + SBG_LOG_ERROR(errorCode, "unable to write content"); + } + } + + return errorCode; } /*! * Execute a GET or POST ECom command. * - * \param[in] pInterface Interface. - * \param[in] methodIsGet The GET command is executed if true, the POST command otherwise. - * \param[in] pPath URI path component. - * \param[in] nrAttempts Number of attempts. - * \param[in] timeout Time-out per attempt, in seconds. - * \param[in] pQuery Query string, may be NULL. - * \param[in] pBody Body, may be NULL. - * \param[in] writeStatus If true, print the status to the output file. - * \param[out] pStatusCode Status code. - * \param[in] pOutputFile Output file. - * \return SBG_NO_ERROR if successful. + * \param[in] pInterface Interface. + * \param[in] methodIsGet The GET command is executed if true, the POST command otherwise. + * \param[in] pPath URI path component. + * \param[in] nrAttempts Number of attempts. + * \param[in] timeout Time-out per attempt, in seconds. + * \param[in] pQuery Query string, may be NULL. + * \param[in] pBody Body, may be NULL. + * \param[in] writeStatus If true, print the status to the output file. + * \param[out] pStatusCode Status code. + * \param[in] pOutputFile Output file. + * \return SBG_NO_ERROR if successful. */ static SbgErrorCode execute(SbgInterface *pInterface, bool methodIsGet, const char *pPath, uint32_t nrAttempts, uint32_t timeout, - const char *pQuery, const char *pBody, bool writeStatus, uint16_t *pStatusCode, FILE *pOutputFile) + const char *pQuery, const char *pBody, bool writeStatus, uint16_t *pStatusCode, FILE *pOutputFile) { - SbgErrorCode errorCode; - SbgEComHandle ecomHandle; - - assert(pPath); - assert(pOutputFile); - - errorCode = sbgEComInit(&ecomHandle, pInterface); - - if (errorCode == SBG_NO_ERROR) - { - SbgEComCmdApiReply reply; - - sbgEComSetCmdTrialsAndTimeOut(&ecomHandle, nrAttempts, timeout * 1000); - - sbgEComCmdApiReplyConstruct(&reply); - - if (methodIsGet) - { - errorCode = sbgEComCmdApiGet(&ecomHandle, pPath, pQuery, &reply); - } - else - { - errorCode = sbgEComCmdApiPost(&ecomHandle, pPath, pQuery, pBody, &reply); - } - - if (errorCode == SBG_NO_ERROR) - { - errorCode = writeReply(&reply, writeStatus, pOutputFile); - - if (errorCode == SBG_NO_ERROR) - { - *pStatusCode = reply.statusCode; - } - } - else - { - SBG_LOG_ERROR(errorCode, "unable to execute command"); - } - - sbgEComCmdApiReplyDestroy(&reply); - - if (errorCode == SBG_NO_ERROR) - { - errorCode = sbgEComClose(&ecomHandle); - } - else - { - sbgEComClose(&ecomHandle); - } - } - - return errorCode; + SbgErrorCode errorCode; + SbgEComHandle ecomHandle; + + assert(pPath); + assert(pOutputFile); + + errorCode = sbgEComInit(&ecomHandle, pInterface); + + if (errorCode == SBG_NO_ERROR) + { + SbgEComCmdApiReply reply; + + sbgEComSetCmdTrialsAndTimeOut(&ecomHandle, nrAttempts, timeout * 1000); + + sbgEComCmdApiReplyConstruct(&reply); + + if (methodIsGet) + { + errorCode = sbgEComCmdApiGet(&ecomHandle, pPath, pQuery, &reply); + } + else + { + errorCode = sbgEComCmdApiPost(&ecomHandle, pPath, pQuery, pBody, &reply); + } + + if (errorCode == SBG_NO_ERROR) + { + errorCode = writeReply(&reply, writeStatus, pOutputFile); + + if (errorCode == SBG_NO_ERROR) + { + *pStatusCode = reply.statusCode; + } + } + else + { + SBG_LOG_ERROR(errorCode, "unable to execute command"); + } + + sbgEComCmdApiReplyDestroy(&reply); + + if (errorCode == SBG_NO_ERROR) + { + errorCode = sbgEComClose(&ecomHandle); + } + else + { + sbgEComClose(&ecomHandle); + } + } + + return errorCode; } //----------------------------------------------------------------------// @@ -422,358 +422,358 @@ static SbgErrorCode execute(SbgInterface *pInterface, bool methodIsGet, const ch /*! * Program entry point. * - * \param[in] argc Number of input arguments. - * \param[in] argv Input arguments as an array of strings. - * \return EXIT_SUCCESS if successful. + * \param[in] argc Number of input arguments. + * \param[in] argv Input arguments as an array of strings. + * \return EXIT_SUCCESS if successful. */ int main(int argc, char **argv) { - int exitCode = EXIT_SUCCESS; - bool printHelp = false; - - struct arg_lit *pHelpArg; - struct arg_lit *pVersionArg; - - struct arg_str *pUdpAddrArg; - struct arg_int *pUdpPortInArg; - struct arg_int *pUdpPortOutArg; - - struct arg_str *pSerialDeviceArg; - struct arg_int *pSerialBaudrateArg; - - struct arg_int *pNrAttemptsArg; - struct arg_int *pTimeoutArg; - - struct arg_lit *pGetMethodArg; - struct arg_lit *pPostMethodArg; - struct arg_str *pPathArg; - struct arg_str *pQueryArg; - struct arg_str *pBodyArg; - struct arg_file *pBodyFileArg; - struct arg_lit *pPrintStatus; - struct arg_file *pOutputFileArg; - struct arg_end *pEndArg; - - // - // TODO: add support for network interfaces. - // - void *argTable[] = - { - pHelpArg = arg_lit0( NULL, "help", "display this help and exit"), - pVersionArg = arg_lit0( NULL, "version", "display version info and exit"), - - pUdpAddrArg = arg_str0( "a", "addr-ip", "IP address", "open an UDP interface"), - pUdpPortInArg = arg_int0( "I", "udp-port-in", "UDP port in", "UDP port to receive data from (local)"), - pUdpPortOutArg = arg_int0( "O", "udp-port-out", "UDP port out", "UDP port to send data to (remote)"), - - pSerialDeviceArg = arg_str0( "s", "serial-device", "SERIAL_DEVICE", "open a serial interface"), - pSerialBaudrateArg = arg_int0( "r", "serial-baudrate", "SERIAL_BAUDRATE", "serial baudrate"), - - pNrAttemptsArg = arg_int0( "n", "nr-attempts", "NR_ATTEMPTS", "number of transaction attempts"), - pTimeoutArg = arg_int0( "t", "timeout", "TIMEOUT", "reply time-out, in seconds"), - - pGetMethodArg = arg_lit0( "g", "method-get", "use the GET method (default)"), - pPostMethodArg = arg_lit0( "p", "method-post", "use the POST method"), - pQueryArg = arg_str0( "q", "query", "QUERY", "query string, format=pretty&delta=true, format and delta options are optional"), - pBodyArg = arg_str0( "b", "body", "BODY", "body (POST method only)"), - pBodyFileArg = arg_file0( "B", "body-file", "BODY_FILE", "file containing the body (POST method only)"), - pPrintStatus = arg_lit0( "S", "print-status", "print the status code on the output stream"), - pOutputFileArg = arg_file0( "o", "output-file", "OUTPUT_FILE", "output file"), - pPathArg = arg_str1( NULL, NULL, "PATH", "GET or POST request path endpoint"), - - pEndArg = arg_end(20), - }; - - sbgCommonLibSetLogCallback(onLogCallback); - - if (arg_nullcheck(argTable) == 0) - { - int argError; - - argError = arg_parse(argc, argv, argTable); - - if (pHelpArg->count != 0) - { - printf("Usage: %s", PROGRAM_NAME); - arg_print_syntax(stdout, argTable, "\n\n"); - - - printf("Access a RESTful SBG ECom server.\n\n"); - printf(" Serial example: %s -s -r api/v1/settings -g\n", PROGRAM_NAME); - printf(" UDP example: %s -a -I -O api/v1/settings -g\n", PROGRAM_NAME); - - puts(""); - - arg_print_glossary(stdout, argTable, " %-50s %s\n"); - - puts(""); - printf("BODY or BODY_FILE may only be provided when using the POST method.\n"); - - puts(""); - printf("If provided, BODY_FILE may not contain binary data.\n"); - - puts(""); - printf("PATH is a URI path component such as api/v1/settings\n"); - - puts(""); - printExitCodeHelp(); - } - else if (pVersionArg->count != 0) - { - printf("%s\n", sbgEComGetVersionAsString()); - } - else if (argError == 0) - { - SbgInterface ecomInterface; - bool methodIsGet = true; - int nrAttempts = DEFAULT_CMD_NR_ATTEMPTS; - int timeout = DEFAULT_CMD_TIMEOUT; - bool writeStatus = false; - SbgString bodyStorage; - SbgString *pBody = NULL; - const char *pQuery = NULL; - const char *pPath = NULL; - - if (exitCode == EXIT_SUCCESS) - { - sbgStringConstructEmpty(&bodyStorage); - pBody = &bodyStorage; - } - - if (exitCode == EXIT_SUCCESS) - { - bool hasSerialConf = false; - bool hasUdpConf = false; - - // - // Can't open at the same time a serial and UDP interface so check it - // - if ( (pSerialDeviceArg->count != 0) && (pSerialBaudrateArg->count != 0) ) - { - hasSerialConf = true; - } - - if ( (pUdpAddrArg->count != 0) && (pUdpPortInArg->count != 0) && (pUdpPortOutArg->count != 0) ) - { - hasUdpConf = true; - } - - if ( (hasSerialConf && !hasUdpConf) || (!hasSerialConf && hasUdpConf) ) - { - SbgErrorCode errorCode; - - if (hasSerialConf) - { - errorCode = sbgInterfaceSerialCreate(&ecomInterface, pSerialDeviceArg->sval[0], pSerialBaudrateArg->ival[0]); - } - else if (hasUdpConf) - { - errorCode = sbgInterfaceUdpCreate(&ecomInterface, sbgNetworkIpFromString(pUdpAddrArg->sval[0]), pUdpPortOutArg->ival[0], pUdpPortInArg->ival[0]); - - if (errorCode == SBG_NO_ERROR) - { - // - // Enable connected mode to only send/receive commands to the designed host - // - sbgInterfaceUdpSetConnectedMode(&ecomInterface, true); - } - } - else - { - errorCode = SBG_INVALID_PARAMETER; - } - - if (errorCode == SBG_NO_ERROR) - { - FILE *pOutputFile = NULL; - - if ((pGetMethodArg->count != 0) && (pPostMethodArg->count == 0)) - { - methodIsGet = true; - } - else if ((pGetMethodArg->count == 0) && (pPostMethodArg->count != 0)) - { - methodIsGet = false; - } - else if ((pGetMethodArg->count != 0) && (pPostMethodArg->count != 0)) - { - exitCode = EXIT_FAILURE; - printHelp = true; - } - - if (exitCode == EXIT_SUCCESS) - { - if (pNrAttemptsArg->count != 0) - { - nrAttempts = pNrAttemptsArg->ival[0]; - - if (nrAttempts <= 0) - { - exitCode = EXIT_FAILURE; - printHelp = true; - } - } - - if (pTimeoutArg->count != 0) - { - timeout = pTimeoutArg->ival[0]; - - if (timeout <= 0) - { - exitCode = EXIT_FAILURE; - printHelp = true; - } - } - - if (pQueryArg->count != 0) - { - pQuery = pQueryArg->sval[0]; - } - } - - if (exitCode == EXIT_SUCCESS) - { - if (!methodIsGet && (pBodyArg->count != 0) && (pBodyFileArg->count == 0)) - { - errorCode = sbgStringAssignCString(pBody, pBodyArg->sval[0]); - - if (errorCode != SBG_NO_ERROR) - { - exitCode = EXIT_FAILURE; - } - } - else if (!methodIsGet && (pBodyArg->count == 0) && (pBodyFileArg->count != 0)) - { - errorCode = readStringFromFile(&bodyStorage, pBodyFileArg->filename[0]); - - if (errorCode != SBG_NO_ERROR) - { - exitCode = EXIT_FAILURE; - } - } - else if ((pBodyArg->count != 0) && (pBodyFileArg->count != 0)) - { - exitCode = EXIT_FAILURE; - printHelp = true; - } - } - - if (exitCode == EXIT_SUCCESS) - { - if (pPrintStatus->count != 0) - { - writeStatus = true; - } - } - - if (exitCode == EXIT_SUCCESS) - { - if (pOutputFileArg->count == 0) - { - pOutputFile = stdout; - } - else - { - pOutputFile = fopen(pOutputFileArg->filename[0], "w"); - - if (!pOutputFile) - { - SBG_LOG_ERROR(SBG_ERROR, "unable to open %s", pOutputFileArg->filename[0]); - exitCode = EXIT_FAILURE; - } - } - } - - if (exitCode == EXIT_SUCCESS) - { - assert(pPathArg->count != 0); - - pPath = pPathArg->sval[0]; - } - - if (exitCode == EXIT_SUCCESS) - { - const char *pBodyCString; - uint16_t statusCode; - - if (pBody) - { - pBodyCString = sbgStringGetCString(pBody); - } - else - { - pBodyCString = NULL; - } - - errorCode = execute(&ecomInterface, methodIsGet, pPath, nrAttempts, timeout, pQuery, pBodyCString, writeStatus, &statusCode, pOutputFile); - - if (errorCode == SBG_NO_ERROR) - { - exitCode = convertStatusCodeToExitCode(statusCode); - } - else - { - exitCode = EXIT_FAILURE; - } - } - - if (pOutputFile && (pOutputFile != stdout)) - { - int result; - - result = fclose(pOutputFile); - - if (result != 0) - { - SBG_LOG_ERROR(SBG_WRITE_ERROR, "unable to close %s", pOutputFileArg->filename[0]); - exitCode = EXIT_FAILURE; - } - } - - sbgInterfaceDestroy(&ecomInterface); - } - else - { - SBG_LOG_ERROR(errorCode, "unable to open the serial or UDP interface"); - exitCode = EXIT_FAILURE; - } - } - else if (hasSerialConf && hasUdpConf) - { - SBG_LOG_ERROR(SBG_ERROR, "please select either a serial or an UDP interface no both"); - exitCode = EXIT_FAILURE; - } - else - { - exitCode = EXIT_FAILURE; - printHelp = true; - } - } - - if (pBody) - { - sbgStringDestroy(pBody); - } - } - else - { - printHelp = true; - } - - if (printHelp) - { - arg_print_errors(stderr, pEndArg, PROGRAM_NAME); - fprintf(stderr, "Try '%s --help' for more information.\n", PROGRAM_NAME); - exitCode = EXIT_FAILURE; - } - - arg_freetable(argTable, SBG_ARRAY_SIZE(argTable)); - } - else - { - SBG_LOG_ERROR(SBG_MALLOC_FAILED, "unable to allocate memory"); - exitCode = EXIT_FAILURE; - } - - return exitCode; + int exitCode = EXIT_SUCCESS; + bool printHelp = false; + + struct arg_lit *pHelpArg; + struct arg_lit *pVersionArg; + + struct arg_str *pUdpAddrArg; + struct arg_int *pUdpPortInArg; + struct arg_int *pUdpPortOutArg; + + struct arg_str *pSerialDeviceArg; + struct arg_int *pSerialBaudrateArg; + + struct arg_int *pNrAttemptsArg; + struct arg_int *pTimeoutArg; + + struct arg_lit *pGetMethodArg; + struct arg_lit *pPostMethodArg; + struct arg_str *pPathArg; + struct arg_str *pQueryArg; + struct arg_str *pBodyArg; + struct arg_file *pBodyFileArg; + struct arg_lit *pPrintStatus; + struct arg_file *pOutputFileArg; + struct arg_end *pEndArg; + + // + // TODO: add support for network interfaces. + // + void *argTable[] = + { + pHelpArg = arg_lit0( NULL, "help", "display this help and exit"), + pVersionArg = arg_lit0( NULL, "version", "display version info and exit"), + + pUdpAddrArg = arg_str0( "a", "addr-ip", "IP address", "open an UDP interface"), + pUdpPortInArg = arg_int0( "I", "udp-port-in", "UDP port in", "UDP port to receive data from (local)"), + pUdpPortOutArg = arg_int0( "O", "udp-port-out", "UDP port out", "UDP port to send data to (remote)"), + + pSerialDeviceArg = arg_str0( "s", "serial-device", "SERIAL_DEVICE", "open a serial interface"), + pSerialBaudrateArg = arg_int0( "r", "serial-baudrate", "SERIAL_BAUDRATE", "serial baudrate"), + + pNrAttemptsArg = arg_int0( "n", "nr-attempts", "NR_ATTEMPTS", "number of transaction attempts"), + pTimeoutArg = arg_int0( "t", "timeout", "TIMEOUT", "reply time-out, in seconds"), + + pGetMethodArg = arg_lit0( "g", "method-get", "use the GET method (default)"), + pPostMethodArg = arg_lit0( "p", "method-post", "use the POST method"), + pQueryArg = arg_str0( "q", "query", "QUERY", "query string, format=pretty&delta=true, format and delta options are optional"), + pBodyArg = arg_str0( "b", "body", "BODY", "body (POST method only)"), + pBodyFileArg = arg_file0( "B", "body-file", "BODY_FILE", "file containing the body (POST method only)"), + pPrintStatus = arg_lit0( "S", "print-status", "print the status code on the output stream"), + pOutputFileArg = arg_file0( "o", "output-file", "OUTPUT_FILE", "output file"), + pPathArg = arg_str1( NULL, NULL, "PATH", "GET or POST request path endpoint"), + + pEndArg = arg_end(20), + }; + + sbgCommonLibSetLogCallback(onLogCallback); + + if (arg_nullcheck(argTable) == 0) + { + int argError; + + argError = arg_parse(argc, argv, argTable); + + if (pHelpArg->count != 0) + { + printf("Usage: %s", PROGRAM_NAME); + arg_print_syntax(stdout, argTable, "\n\n"); + + + printf("Access a RESTful SBG ECom server.\n\n"); + printf(" Serial example: %s -s -r api/v1/settings -g\n", PROGRAM_NAME); + printf(" UDP example: %s -a -I -O api/v1/settings -g\n", PROGRAM_NAME); + + puts(""); + + arg_print_glossary(stdout, argTable, " %-50s %s\n"); + + puts(""); + printf("BODY or BODY_FILE may only be provided when using the POST method.\n"); + + puts(""); + printf("If provided, BODY_FILE may not contain binary data.\n"); + + puts(""); + printf("PATH is a URI path component such as api/v1/settings\n"); + + puts(""); + printExitCodeHelp(); + } + else if (pVersionArg->count != 0) + { + printf("%s\n", sbgEComGetVersionAsString()); + } + else if (argError == 0) + { + SbgInterface ecomInterface; + bool methodIsGet = true; + int nrAttempts = DEFAULT_CMD_NR_ATTEMPTS; + int timeout = DEFAULT_CMD_TIMEOUT; + bool writeStatus = false; + SbgString bodyStorage; + SbgString *pBody = NULL; + const char *pQuery = NULL; + const char *pPath = NULL; + + if (exitCode == EXIT_SUCCESS) + { + sbgStringConstructEmpty(&bodyStorage); + pBody = &bodyStorage; + } + + if (exitCode == EXIT_SUCCESS) + { + bool hasSerialConf = false; + bool hasUdpConf = false; + + // + // Can't open at the same time a serial and UDP interface so check it + // + if ( (pSerialDeviceArg->count != 0) && (pSerialBaudrateArg->count != 0) ) + { + hasSerialConf = true; + } + + if ( (pUdpAddrArg->count != 0) && (pUdpPortInArg->count != 0) && (pUdpPortOutArg->count != 0) ) + { + hasUdpConf = true; + } + + if ( (hasSerialConf && !hasUdpConf) || (!hasSerialConf && hasUdpConf) ) + { + SbgErrorCode errorCode; + + if (hasSerialConf) + { + errorCode = sbgInterfaceSerialCreate(&ecomInterface, pSerialDeviceArg->sval[0], pSerialBaudrateArg->ival[0]); + } + else if (hasUdpConf) + { + errorCode = sbgInterfaceUdpCreate(&ecomInterface, sbgNetworkIpFromString(pUdpAddrArg->sval[0]), pUdpPortOutArg->ival[0], pUdpPortInArg->ival[0]); + + if (errorCode == SBG_NO_ERROR) + { + // + // Enable connected mode to only send/receive commands to the designed host + // + sbgInterfaceUdpSetConnectedMode(&ecomInterface, true); + } + } + else + { + errorCode = SBG_INVALID_PARAMETER; + } + + if (errorCode == SBG_NO_ERROR) + { + FILE *pOutputFile = NULL; + + if ((pGetMethodArg->count != 0) && (pPostMethodArg->count == 0)) + { + methodIsGet = true; + } + else if ((pGetMethodArg->count == 0) && (pPostMethodArg->count != 0)) + { + methodIsGet = false; + } + else if ((pGetMethodArg->count != 0) && (pPostMethodArg->count != 0)) + { + exitCode = EXIT_FAILURE; + printHelp = true; + } + + if (exitCode == EXIT_SUCCESS) + { + if (pNrAttemptsArg->count != 0) + { + nrAttempts = pNrAttemptsArg->ival[0]; + + if (nrAttempts <= 0) + { + exitCode = EXIT_FAILURE; + printHelp = true; + } + } + + if (pTimeoutArg->count != 0) + { + timeout = pTimeoutArg->ival[0]; + + if (timeout <= 0) + { + exitCode = EXIT_FAILURE; + printHelp = true; + } + } + + if (pQueryArg->count != 0) + { + pQuery = pQueryArg->sval[0]; + } + } + + if (exitCode == EXIT_SUCCESS) + { + if (!methodIsGet && (pBodyArg->count != 0) && (pBodyFileArg->count == 0)) + { + errorCode = sbgStringAssignCString(pBody, pBodyArg->sval[0]); + + if (errorCode != SBG_NO_ERROR) + { + exitCode = EXIT_FAILURE; + } + } + else if (!methodIsGet && (pBodyArg->count == 0) && (pBodyFileArg->count != 0)) + { + errorCode = readStringFromFile(&bodyStorage, pBodyFileArg->filename[0]); + + if (errorCode != SBG_NO_ERROR) + { + exitCode = EXIT_FAILURE; + } + } + else if ((pBodyArg->count != 0) && (pBodyFileArg->count != 0)) + { + exitCode = EXIT_FAILURE; + printHelp = true; + } + } + + if (exitCode == EXIT_SUCCESS) + { + if (pPrintStatus->count != 0) + { + writeStatus = true; + } + } + + if (exitCode == EXIT_SUCCESS) + { + if (pOutputFileArg->count == 0) + { + pOutputFile = stdout; + } + else + { + pOutputFile = fopen(pOutputFileArg->filename[0], "w"); + + if (!pOutputFile) + { + SBG_LOG_ERROR(SBG_ERROR, "unable to open %s", pOutputFileArg->filename[0]); + exitCode = EXIT_FAILURE; + } + } + } + + if (exitCode == EXIT_SUCCESS) + { + assert(pPathArg->count != 0); + + pPath = pPathArg->sval[0]; + } + + if (exitCode == EXIT_SUCCESS) + { + const char *pBodyCString; + uint16_t statusCode; + + if (pBody) + { + pBodyCString = sbgStringGetCString(pBody); + } + else + { + pBodyCString = NULL; + } + + errorCode = execute(&ecomInterface, methodIsGet, pPath, nrAttempts, timeout, pQuery, pBodyCString, writeStatus, &statusCode, pOutputFile); + + if (errorCode == SBG_NO_ERROR) + { + exitCode = convertStatusCodeToExitCode(statusCode); + } + else + { + exitCode = EXIT_FAILURE; + } + } + + if (pOutputFile && (pOutputFile != stdout)) + { + int result; + + result = fclose(pOutputFile); + + if (result != 0) + { + SBG_LOG_ERROR(SBG_WRITE_ERROR, "unable to close %s", pOutputFileArg->filename[0]); + exitCode = EXIT_FAILURE; + } + } + + sbgInterfaceDestroy(&ecomInterface); + } + else + { + SBG_LOG_ERROR(errorCode, "unable to open the serial or UDP interface"); + exitCode = EXIT_FAILURE; + } + } + else if (hasSerialConf && hasUdpConf) + { + SBG_LOG_ERROR(SBG_ERROR, "please select either a serial or an UDP interface no both"); + exitCode = EXIT_FAILURE; + } + else + { + exitCode = EXIT_FAILURE; + printHelp = true; + } + } + + if (pBody) + { + sbgStringDestroy(pBody); + } + } + else + { + printHelp = true; + } + + if (printHelp) + { + arg_print_errors(stderr, pEndArg, PROGRAM_NAME); + fprintf(stderr, "Try '%s --help' for more information.\n", PROGRAM_NAME); + exitCode = EXIT_FAILURE; + } + + arg_freetable(argTable, SBG_ARRAY_SIZE(argTable)); + } + else + { + SBG_LOG_ERROR(SBG_MALLOC_FAILED, "unable to allocate memory"); + exitCode = EXIT_FAILURE; + } + + return exitCode; }