From 3c335b44243e0efb2687abcb7bdfb2a3350cdf28 Mon Sep 17 00:00:00 2001 From: Raphael Siryani Date: Tue, 30 Jan 2024 17:50:25 +0100 Subject: [PATCH] Release 4.0.1987-stable --- CHANGELOG.md | 432 ++-- CMakeLists.txt | 73 +- README.md | 14 +- common/debug/sbgDebug.h | 8 +- common/interfaces/sbgInterface.c | 3 - common/interfaces/sbgInterfaceSerialUnix.c | 14 +- common/interfaces/sbgInterfaceSerialWin.c | 4 +- common/interfaces/sbgInterfaceUdp.c | 70 +- common/interfaces/sbgInterfaceUdp.h | 36 +- common/sbgCommon.c | 28 - common/sbgCommon.h | 15 - common/sbgTypes.h | 88 +- common/streamBuffer/sbgStreamBufferBE.h | 24 +- common/streamBuffer/sbgStreamBufferLE.h | 24 +- common/string/sbgString.c | 303 ++- common/string/sbgString.h | 90 + common/swap/sbgSwap.h | 8 +- doc/migrations.md | 70 +- examples/airDataInput/src/airDataInput.c | 12 +- examples/ellipseMinimal/src/ellipseMinimal.c | 22 +- .../src/ellipseOnboardMagCalib.c | 65 +- examples/hpInsMinimal/src/hpInsMinimal.c | 6 +- examples/pulseMinimal/src/pulseMinimal.c | 26 +- src/binaryLogs/sbgEComBinaryLogAirData.c | 66 - src/binaryLogs/sbgEComBinaryLogDepth.c | 45 - src/binaryLogs/sbgEComBinaryLogDiag.c | 49 - src/binaryLogs/sbgEComBinaryLogDvl.c | 55 - src/binaryLogs/sbgEComBinaryLogEkf.c | 183 -- src/binaryLogs/sbgEComBinaryLogEvent.c | 47 - src/binaryLogs/sbgEComBinaryLogGps.c | 206 -- src/binaryLogs/sbgEComBinaryLogGps.h | 433 ---- src/binaryLogs/sbgEComBinaryLogImu.c | 206 -- src/binaryLogs/sbgEComBinaryLogMag.c | 89 - src/binaryLogs/sbgEComBinaryLogOdometer.c | 43 - src/binaryLogs/sbgEComBinaryLogRawData.c | 40 - src/binaryLogs/sbgEComBinaryLogRtcm.c | 19 - src/binaryLogs/sbgEComBinaryLogRtcm.h | 73 - src/binaryLogs/sbgEComBinaryLogSat.c | 865 ------- src/binaryLogs/sbgEComBinaryLogShipMotion.c | 110 - src/binaryLogs/sbgEComBinaryLogStatus.c | 72 - src/binaryLogs/sbgEComBinaryLogStatus.h | 216 -- src/binaryLogs/sbgEComBinaryLogUsbl.c | 57 - src/binaryLogs/sbgEComBinaryLogUtc.c | 100 - src/binaryLogs/sbgEComBinaryLogUtc.h | 190 -- src/binaryLogs/sbgEComBinaryLogs.c | 183 -- src/binaryLogs/sbgEComBinaryLogs.h | 136 -- src/commands/sbgEComCmdAdvanced.h | 4 +- src/commands/sbgEComCmdApi.c | 4 +- src/commands/sbgEComCmdCommon.c | 50 +- src/commands/sbgEComCmdCommon.h | 14 +- src/commands/sbgEComCmdEthernet.c | 6 +- src/commands/sbgEComCmdEthernet.h | 16 +- src/commands/sbgEComCmdEvent.c | 4 +- src/commands/sbgEComCmdEvent.h | 10 +- src/commands/sbgEComCmdFeatures.h | 4 +- src/commands/sbgEComCmdGnss.c | 2 +- src/commands/sbgEComCmdInfo.h | 4 +- src/commands/sbgEComCmdInterface.h | 6 +- src/commands/sbgEComCmdMag.c | 8 +- src/commands/sbgEComCmdMag.h | 22 +- src/commands/sbgEComCmdOdo.h | 4 +- src/commands/sbgEComCmdOutput.h | 24 +- src/commands/sbgEComCmdSensor.h | 6 +- src/commands/sbgEComCmdSettings.h | 4 +- src/defs/sbgEComDefsGnss.h | 2 +- src/logs/sbgEComLog.c | 199 ++ src/logs/sbgEComLog.h | 148 ++ src/logs/sbgEComLogAirData.c | 68 + .../sbgEComLogAirData.h} | 39 +- .../sbgEComLogAutomotive.h} | 18 +- src/logs/sbgEComLogDepth.c | 47 + .../sbgEComLogDepth.h} | 45 +- src/logs/sbgEComLogDiag.c | 63 + .../sbgEComLogDiag.h} | 44 +- src/logs/sbgEComLogDvl.c | 57 + .../sbgEComLogDvl.h} | 45 +- src/logs/sbgEComLogEkf.c | 262 +++ .../sbgEComLogEkf.h} | 202 +- src/logs/sbgEComLogEkfRotAccel.c | 55 + src/logs/sbgEComLogEkfRotAccel.h | 116 + src/logs/sbgEComLogEvent.c | 49 + .../sbgEComLogEvent.h} | 39 +- src/logs/sbgEComLogGnssHdt.c | 228 ++ src/logs/sbgEComLogGnssHdt.h | 210 ++ src/logs/sbgEComLogGnssPos.c | 372 +++ src/logs/sbgEComLogGnssPos.h | 414 ++++ src/logs/sbgEComLogGnssVel.c | 162 ++ src/logs/sbgEComLogGnssVel.h | 198 ++ src/logs/sbgEComLogImu.c | 219 ++ .../sbgEComLogImu.h} | 169 +- src/logs/sbgEComLogMag.c | 92 + .../sbgEComLogMag.h} | 65 +- src/logs/sbgEComLogOdometer.c | 45 + .../sbgEComLogOdometer.h} | 43 +- src/logs/sbgEComLogRawData.c | 54 + .../sbgEComLogRawData.h} | 41 +- src/logs/sbgEComLogSat.c | 698 ++++++ .../sbgEComLogSat.h} | 271 +-- src/logs/sbgEComLogShipMotion.c | 92 + .../sbgEComLogShipMotion.h} | 51 +- src/logs/sbgEComLogStatus.c | 108 + src/logs/sbgEComLogStatus.h | 212 ++ src/logs/sbgEComLogUsbl.c | 59 + .../sbgEComLogUsbl.h} | 39 +- src/logs/sbgEComLogUtc.c | 281 +++ src/logs/sbgEComLogUtc.h | 281 +++ src/protocol/sbgEComProtocol.c | 29 +- src/protocol/sbgEComProtocol.h | 121 +- src/sbgECom.c | 15 +- src/sbgECom.h | 5 +- src/sbgEComIds.h | 40 +- src/sbgEComLib.h | 2 +- src/sbgEComVersion.h | 6 +- src/transfer/sbgEComTransfer.c | 69 +- src/transfer/sbgEComTransfer.h | 2 +- tools/sbgBasicLogger/README.md | 45 +- tools/sbgBasicLogger/src/loggerApp.cpp | 422 ++++ tools/sbgBasicLogger/src/loggerApp.h | 119 + .../src/loggerEntry/helpers/imuDataMean.cpp | 135 ++ .../src/loggerEntry/helpers/imuDataMean.h | 139 ++ .../src/loggerEntry/loggerEntryAidings.cpp | 172 ++ .../src/loggerEntry/loggerEntryAidings.h | 231 ++ .../src/loggerEntry/loggerEntryDvl.cpp | 76 + .../src/loggerEntry/loggerEntryDvl.h | 112 + .../src/loggerEntry/loggerEntryEkf.cpp | 211 ++ .../src/loggerEntry/loggerEntryEkf.h | 232 ++ .../loggerEntry/loggerEntryEkfRotAccel.cpp | 107 + .../src/loggerEntry/loggerEntryEkfRotAccel.h | 135 ++ .../src/loggerEntry/loggerEntryEvent.cpp | 117 + .../src/loggerEntry/loggerEntryEvent.h | 188 ++ .../src/loggerEntry/loggerEntryGeneral.cpp | 225 ++ .../src/loggerEntry/loggerEntryGeneral.h | 243 ++ .../src/loggerEntry/loggerEntryGnss.cpp | 290 +++ .../src/loggerEntry/loggerEntryGnss.h | 377 +++ .../src/loggerEntry/loggerEntryImu.cpp | 142 ++ .../src/loggerEntry/loggerEntryImu.h | 204 ++ .../src/loggerEntry/loggerEntryMag.cpp | 90 + .../src/loggerEntry/loggerEntryMag.h | 137 ++ .../src/loggerEntry/loggerEntryShipMotion.cpp | 85 + .../src/loggerEntry/loggerEntryShipMotion.h | 112 + .../src/loggerManager/loggerContext.cpp | 131 ++ .../src/loggerManager/loggerContext.h | 141 ++ .../src/loggerManager/loggerEntry.cpp | 139 ++ .../src/loggerManager/loggerEntry.h | 221 ++ .../src/loggerManager/loggerManager.cpp | 155 ++ .../src/loggerManager/loggerManager.h | 166 ++ .../src/loggerManager/loggerSettings.cpp | 222 ++ .../src/loggerManager/loggerSettings.h | 331 +++ tools/sbgBasicLogger/src/main.c | 1046 --------- tools/sbgBasicLogger/src/main.cpp | 211 ++ .../src/sbgBasicLoggerAccumulators.c | 58 - .../src/sbgBasicLoggerAccumulators.h | 115 - tools/sbgBasicLogger/src/sbgBasicLoggerFile.c | 88 - tools/sbgBasicLogger/src/sbgBasicLoggerFile.h | 60 - .../src/sbgBasicLoggerHandler.c | 2064 ----------------- .../src/sbgBasicLoggerHandler.h | 823 ------- tools/sbgEComApi/README.md | 57 +- tools/sbgEComApi/src/main.c | 95 +- 158 files changed, 12904 insertions(+), 8866 deletions(-) delete mode 100644 common/sbgCommon.c delete mode 100644 src/binaryLogs/sbgEComBinaryLogAirData.c delete mode 100644 src/binaryLogs/sbgEComBinaryLogDepth.c delete mode 100644 src/binaryLogs/sbgEComBinaryLogDiag.c delete mode 100644 src/binaryLogs/sbgEComBinaryLogDvl.c delete mode 100644 src/binaryLogs/sbgEComBinaryLogEkf.c delete mode 100644 src/binaryLogs/sbgEComBinaryLogEvent.c delete mode 100644 src/binaryLogs/sbgEComBinaryLogGps.c delete mode 100644 src/binaryLogs/sbgEComBinaryLogGps.h delete mode 100644 src/binaryLogs/sbgEComBinaryLogImu.c delete mode 100644 src/binaryLogs/sbgEComBinaryLogMag.c delete mode 100644 src/binaryLogs/sbgEComBinaryLogOdometer.c delete mode 100644 src/binaryLogs/sbgEComBinaryLogRawData.c delete mode 100644 src/binaryLogs/sbgEComBinaryLogRtcm.c delete mode 100644 src/binaryLogs/sbgEComBinaryLogRtcm.h delete mode 100644 src/binaryLogs/sbgEComBinaryLogSat.c delete mode 100644 src/binaryLogs/sbgEComBinaryLogShipMotion.c delete mode 100644 src/binaryLogs/sbgEComBinaryLogStatus.c delete mode 100644 src/binaryLogs/sbgEComBinaryLogStatus.h delete mode 100644 src/binaryLogs/sbgEComBinaryLogUsbl.c delete mode 100644 src/binaryLogs/sbgEComBinaryLogUtc.c delete mode 100644 src/binaryLogs/sbgEComBinaryLogUtc.h delete mode 100644 src/binaryLogs/sbgEComBinaryLogs.c delete mode 100644 src/binaryLogs/sbgEComBinaryLogs.h create mode 100644 src/logs/sbgEComLog.c create mode 100644 src/logs/sbgEComLog.h create mode 100644 src/logs/sbgEComLogAirData.c rename src/{binaryLogs/sbgEComBinaryLogAirData.h => logs/sbgEComLogAirData.h} (74%) rename src/{binaryLogs/sbgEComBinaryLogAutomotiveData.h => logs/sbgEComLogAutomotive.h} (83%) create mode 100644 src/logs/sbgEComLogDepth.c rename src/{binaryLogs/sbgEComBinaryLogDepth.h => logs/sbgEComLogDepth.h} (67%) create mode 100644 src/logs/sbgEComLogDiag.c rename src/{binaryLogs/sbgEComBinaryLogDiag.h => logs/sbgEComLogDiag.h} (63%) create mode 100644 src/logs/sbgEComLogDvl.c rename src/{binaryLogs/sbgEComBinaryLogDvl.h => logs/sbgEComLogDvl.h} (66%) create mode 100644 src/logs/sbgEComLogEkf.c rename src/{binaryLogs/sbgEComBinaryLogEkf.h => logs/sbgEComLogEkf.h} (60%) create mode 100644 src/logs/sbgEComLogEkfRotAccel.c create mode 100644 src/logs/sbgEComLogEkfRotAccel.h create mode 100644 src/logs/sbgEComLogEvent.c rename src/{binaryLogs/sbgEComBinaryLogEvent.h => logs/sbgEComLogEvent.h} (72%) create mode 100644 src/logs/sbgEComLogGnssHdt.c create mode 100644 src/logs/sbgEComLogGnssHdt.h create mode 100644 src/logs/sbgEComLogGnssPos.c create mode 100644 src/logs/sbgEComLogGnssPos.h create mode 100644 src/logs/sbgEComLogGnssVel.c create mode 100644 src/logs/sbgEComLogGnssVel.h create mode 100644 src/logs/sbgEComLogImu.c rename src/{binaryLogs/sbgEComBinaryLogImu.h => logs/sbgEComLogImu.h} (55%) create mode 100644 src/logs/sbgEComLogMag.c rename src/{binaryLogs/sbgEComBinaryLogMag.h => logs/sbgEComLogMag.h} (64%) create mode 100644 src/logs/sbgEComLogOdometer.c rename src/{binaryLogs/sbgEComBinaryLogOdometer.h => logs/sbgEComLogOdometer.h} (65%) create mode 100644 src/logs/sbgEComLogRawData.c rename src/{binaryLogs/sbgEComBinaryLogRawData.h => logs/sbgEComLogRawData.h} (65%) create mode 100644 src/logs/sbgEComLogSat.c rename src/{binaryLogs/sbgEComBinaryLogSat.h => logs/sbgEComLogSat.h} (57%) create mode 100644 src/logs/sbgEComLogShipMotion.c rename src/{binaryLogs/sbgEComBinaryLogShipMotion.h => logs/sbgEComLogShipMotion.h} (69%) create mode 100644 src/logs/sbgEComLogStatus.c create mode 100644 src/logs/sbgEComLogStatus.h create mode 100644 src/logs/sbgEComLogUsbl.c rename src/{binaryLogs/sbgEComBinaryLogUsbl.h => logs/sbgEComLogUsbl.h} (72%) create mode 100644 src/logs/sbgEComLogUtc.c create mode 100644 src/logs/sbgEComLogUtc.h create mode 100644 tools/sbgBasicLogger/src/loggerApp.cpp create mode 100644 tools/sbgBasicLogger/src/loggerApp.h create mode 100644 tools/sbgBasicLogger/src/loggerEntry/helpers/imuDataMean.cpp create mode 100644 tools/sbgBasicLogger/src/loggerEntry/helpers/imuDataMean.h create mode 100644 tools/sbgBasicLogger/src/loggerEntry/loggerEntryAidings.cpp create mode 100644 tools/sbgBasicLogger/src/loggerEntry/loggerEntryAidings.h create mode 100644 tools/sbgBasicLogger/src/loggerEntry/loggerEntryDvl.cpp create mode 100644 tools/sbgBasicLogger/src/loggerEntry/loggerEntryDvl.h create mode 100644 tools/sbgBasicLogger/src/loggerEntry/loggerEntryEkf.cpp create mode 100644 tools/sbgBasicLogger/src/loggerEntry/loggerEntryEkf.h create mode 100644 tools/sbgBasicLogger/src/loggerEntry/loggerEntryEkfRotAccel.cpp create mode 100644 tools/sbgBasicLogger/src/loggerEntry/loggerEntryEkfRotAccel.h create mode 100644 tools/sbgBasicLogger/src/loggerEntry/loggerEntryEvent.cpp create mode 100644 tools/sbgBasicLogger/src/loggerEntry/loggerEntryEvent.h create mode 100644 tools/sbgBasicLogger/src/loggerEntry/loggerEntryGeneral.cpp create mode 100644 tools/sbgBasicLogger/src/loggerEntry/loggerEntryGeneral.h create mode 100644 tools/sbgBasicLogger/src/loggerEntry/loggerEntryGnss.cpp create mode 100644 tools/sbgBasicLogger/src/loggerEntry/loggerEntryGnss.h create mode 100644 tools/sbgBasicLogger/src/loggerEntry/loggerEntryImu.cpp create mode 100644 tools/sbgBasicLogger/src/loggerEntry/loggerEntryImu.h create mode 100644 tools/sbgBasicLogger/src/loggerEntry/loggerEntryMag.cpp create mode 100644 tools/sbgBasicLogger/src/loggerEntry/loggerEntryMag.h create mode 100644 tools/sbgBasicLogger/src/loggerEntry/loggerEntryShipMotion.cpp create mode 100644 tools/sbgBasicLogger/src/loggerEntry/loggerEntryShipMotion.h create mode 100644 tools/sbgBasicLogger/src/loggerManager/loggerContext.cpp create mode 100644 tools/sbgBasicLogger/src/loggerManager/loggerContext.h create mode 100644 tools/sbgBasicLogger/src/loggerManager/loggerEntry.cpp create mode 100644 tools/sbgBasicLogger/src/loggerManager/loggerEntry.h create mode 100644 tools/sbgBasicLogger/src/loggerManager/loggerManager.cpp create mode 100644 tools/sbgBasicLogger/src/loggerManager/loggerManager.h create mode 100644 tools/sbgBasicLogger/src/loggerManager/loggerSettings.cpp create mode 100644 tools/sbgBasicLogger/src/loggerManager/loggerSettings.h delete mode 100644 tools/sbgBasicLogger/src/main.c create mode 100644 tools/sbgBasicLogger/src/main.cpp delete mode 100644 tools/sbgBasicLogger/src/sbgBasicLoggerAccumulators.c delete mode 100644 tools/sbgBasicLogger/src/sbgBasicLoggerAccumulators.h delete mode 100644 tools/sbgBasicLogger/src/sbgBasicLoggerFile.c delete mode 100644 tools/sbgBasicLogger/src/sbgBasicLoggerFile.h delete mode 100644 tools/sbgBasicLogger/src/sbgBasicLoggerHandler.c delete mode 100644 tools/sbgBasicLogger/src/sbgBasicLoggerHandler.h diff --git a/CHANGELOG.md b/CHANGELOG.md index cb3a339..c468291 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,303 +1,345 @@ # Change Log This change log lists all modifications for each sbgECom library release. -sbgECom C library change log issued on: 2022-11-15 -Copyright (C) 2022, SBG Systems SAS. All rights reserved. +sbgECom C library change log issued on: 2024-01-30 +Copyright (C) 2024, SBG Systems SAS. All rights reserved. ## Release Summary -The sbgECom 3.2.4011-stable release adds support to latest High Performance INS firmware 4.2 and ELLIPSE firmware 2.5 -New configurations and output logs have been added such as `SBG_ECOM_LOG_GPS#_SAT` binary log. +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. -It also adds two new commands sbgEComCmdApiGet and sbgEComCmdApiPost to support the new [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi/) over serial interfaces. +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. -A new extended sbgECom frame format has been introduced to support large payloads. -This new format is fully backward compatible and your code, as well as older firmware, should still work with this new sbgECom implementation. +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. -New tools and examples have been added to ease product evaluation and integration. - -This release also improves the overall code quality, documentation and examples. +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. ## Import Highlights for this release Please find below, the main improvements and modifications: - Support for latest firmware - - Support for new sbgInsRestApi GET/POST commands - - New sbgEComBasicLogger tool to convert logs to CSV files - - New sbgEComApi tool to use the sbgInsRestApi from command line - - Improved sbgECom frame for large payloads - - Improved ELLIPSE on-board magnetic calibration example - - Improve code quality - - Improved Doxygen documentation - - Improved CMake & GitHub support + - 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 + +## Release - 4.0.1987-stable + +### New Features + - SBGECOM-346 - Add spoofing, jamming and OSNMA status in SBG_ECOM_LOG_GPS#_POS + - SBGECOM-347 - Add in SBG_ECOM_LOG_GPS#_HDT number of SV tracked and used + - SBGECOM-364 - Add an option to change UDP connected/unconnected behavior + - SBGECOM-237 - Add callback to retrieve complete raw sbgECom frames on reception + - SBGECOM-330 - Add Ethernet Tx and Rx status flags in SBG_ECOM_LOG_STATUS message + - SBGECOM-341 - sbgBasicLogger: Rewritten from scratch using C++ 14 + - SBGECOM-342 - sbgBasicLogger: Export RTCM raw stream to a binary file that can be used for post processing + - SBGECOM-351 - Add numSvTracked in SBG_ECOM_LOG_GPS#_POS message + - SBGECOM-352 - BasicLogger: Add new outputs on SBG_ECOM_LOG_GPS#_XXXX logs + - SBGECOM-363 - sbgEComApi: Add Ethernet UDP interface support + - SBGECOM-365 - Added to SBG_ECOM_LOG_UTC_TIME internal clock quality indicators + - SBGECOM-368 - Add new SBG_ECOM_LOG_EKF_ROT_ACCEL_BODY message + - SBGECOM-369 - Add new SBG_ECOM_LOG_EKF_VELOCITY_BODY message + - SBGECOM-370 - Add new SBG_ECOM_LOG_EKF_ROT_ACCEL_NED message + +### Improvements + - SBGECOM-308 - Add support for clang enhanced compiler + - SBGECOM-337 - Improve on board magnetic calibration example to save to FLASH memory + - SBGECOM-338 - Improved SBG_ECOM_LOG_GPS#_VEL velocity standard deviation documentation + - SBGECOM-343 - sbgBasicLogger: Export GNSS raw stream to a binary file that can be used for post processing + - SBGECOM-344 - sbgBasicLogger: Added new option to export logs using UTC time + - SBGECOM-345 - sbgBasicLogger: Added a new option to skip logs until the INS has a valid UTC time + - SBGECOM-350 - Report unavailable numSvUsed in SBG_ECOM_LOG_GPS#_POS with 0xFF value + - SBGECOM-357 - Improved sbgECom log file naming and methods conventions + - SBGECOM-358 - Updated migration documentation from sbgECom 3 to 4 + - SBGECOM-361 - Improved sbgEComLogUtc enums/definitions/setters/getters naming + - SBGECOM-366 - sbgBasicLogger: Add an option to output status using decimal or hexadecimal representation + - SBGECOM-380 - Reduce stack usage for some sbgECom commands + - SBGECOM-367 - Improved sbgEComCmdOutput##### documentation + - SBGECOM-403 - Replaced __FUNCTION__ macro with standard C99 __func__ variable + +### Bug Fixes + - SBGECOM-383 - Updated CMD_API_GET ID (46) to (48) to fix conflict with CMD_GNSS_1_INSTALLATION (46) + - SBGECOM-359 - Fixed incompatible deprecated typedef with opencv such as uint64 + - SBGECOM-401 - Fixed invalid sleep usage in sbgEComReceiveCmd2 method + - SBGECOM-339 - Relaxed Cmake version requirements to 3.16 to support Ubuntu 20.04 + - SBGECOM-340 - Fixed SyncOut direct PPS comment in sbgEComCmdEvent.h + +### Removed feature + - SBGECOM-349 - Flagged SBG_ECOM_IMU_DATA as legacy and not to be used ## Release - 3.2.4011-stable ### New Features - - [SBGECOM-306] - Added built in CHANGELOG file in the repository - - [SBGECOM-307] - Add support for event markers in sbgBasicLogger - - [SBGECOM-309] - Add SBG_ECOM_LOG_GPS#_SAT message containing satellite information - - [SBGECOM-310] - Add NMEA message GPROT - - [SBGECOM-312] - sbgBasicLogger: handle sbgEComLogSat messages - - [SBGECOM-313] - Added a static motion profile for 27/7 operations on ELLIPSE - - [SBGECOM-314] - Added pedestrian motion profile that uses foot odometry - - [SBGECOM-317] - Added search capabilities for Doxygen documentation - - [SBGECOM-319] - Add support for the Teledyne Wayfinder DVL protocol - - [SBGECOM-323] - Add a new PSBGA NMEA message that output the INS atitude - - [SBGECOM-326] - Add NMEA GSV output message with satellite in view information - - [SBGECOM-327] - Added L-Band support for GNSS receiver channel reporting + - SBGECOM-306 - Added built in CHANGELOG file in the repository + - SBGECOM-307 - Add support for event markers in sbgBasicLogger + - SBGECOM-309 - Add SBG_ECOM_LOG_GPS#_SAT message containing satellite information + - SBGECOM-310 - Add NMEA message GPROT + - SBGECOM-312 - sbgBasicLogger: handle sbgEComLogSat messages + - SBGECOM-313 - Added a static motion profile for 27/7 operations on ELLIPSE + - SBGECOM-314 - Added pedestrian motion profile that uses foot odometer + - SBGECOM-317 - Added search capabilities for Doxygen documentation + - SBGECOM-319 - Add support for the Teledyne Wayfinder DVL protocol + - SBGECOM-323 - Add a new PSBGA NMEA message that output the INS attitude + - SBGECOM-326 - Add NMEA GSV output message with satellite in view information + - SBGECOM-327 - Added L-Band support for GNSS receiver channel reporting ### Improvements -- [SBGECOM-311] - Improve public CMakeLists -- [SBGECOM-315] - Enable sbgEComApi users to provide command execution parameters -- [SBGECOM-322] - Added Truck and Railway motion profiles -- [SBGECOM-325] - sbgBasicLogger: Improve project code and build script for code source public release -- [SBGECOM-328] - Improved doxygen documentation + - SBGECOM-311 - Improve public CMakeLists + - SBGECOM-315 - Enable sbgEComApi users to provide command execution parameters + - SBGECOM-322 - Added Truck and Railway motion profiles + - SBGECOM-325 - sbgBasicLogger: Improve project code and build script for code source public release + - SBGECOM-328 - Improved doxygen documentation ### Bug Fixes -- [SBGECOM-316] - Fix ELLIPSE settings import that was not working -- [SBGECOM-324] - Fix invalid time-out errors when calling sbgEComReceiveCmd2 + - SBGECOM-316 - Fix ELLIPSE settings import that was not working + - SBGECOM-324 - Fix invalid time-out errors when calling sbgEComReceiveCmd2 ## Release - 3.1.2358-stable ### New Features - - [SBGECOM-225] - Added KMB binary sensor output log for High Performance INS - - [SBGECOM-226] - Add a new log SBG_ECOM_LOG_RTCM_STREAM - - [SBGECOM-228] - Add new commands to implement GET/POST over sbgECom protocol - - [SBGECOM-229] - Now ELLIPSE-N and D can accept RTCM corrections over Port A - - [SBGECOM-231] - Add support for NMEA PPS frames - - [SBGECOM-253] - Add specific NMEA like output log WASSP - - [SBGECOM-279] - Add in SBG_ECOM_CMD_ADVANCED_CONF a GNSS option bitmask - - [SBGECOM-281] - Add a new sbgBasicLogger command line tool - - [SBGECOM-282] - Add a sbgEComApi CLI tool to access sbgInsRestApi over sbgECom - - [SBGECOM-283] - Add a minimal sbgECom example for PULSE IMU - - [SBGECOM-285] - Add in SBG_ECOM_CMD_ADVANCED_CONF an option to always output time in NMEA messages - - [SBGECOM-286] - Add in SBG_ECOM_CMD_ADVANCED_CONF an option select standard or extended NMEA mode + - SBGECOM-225 - Added KMB binary sensor output log for High Performance INS + - SBGECOM-226 - Add a new log SBG_ECOM_LOG_RTCM_STREAM + - SBGECOM-228 - Add new commands to implement GET/POST over sbgECom protocol + - SBGECOM-229 - Now ELLIPSE-N and D can accept RTCM corrections over Port A + - SBGECOM-231 - Add support for NMEA PPS frames + - SBGECOM-253 - Add specific NMEA like output log WASSP + - SBGECOM-279 - Add in SBG_ECOM_CMD_ADVANCED_CONF a GNSS option bitmask + - SBGECOM-281 - Add a new sbgBasicLogger command line tool + - SBGECOM-282 - Add a sbgEComApi CLI tool to access sbgInsRestApi over sbgECom + - SBGECOM-283 - Add a minimal sbgECom example for PULSE IMU + - SBGECOM-285 - Add in SBG_ECOM_CMD_ADVANCED_CONF an option to always output time in NMEA messages + - SBGECOM-286 - Add in SBG_ECOM_CMD_ADVANCED_CONF an option select standard or extended NMEA mode ### Improvements - - [SBGECOM-181] - Reduce stack usage and use new sbgEComReceiveCmd2 - - [SBGECOM-220] - Updated license to MIT - - [SBGECOM-232] - Updated protocol to support large transfer using multiple pages - - [SBGECOM-287] - Updated sbgEComSetReceiveLogCallback to remove the unused SbgErrorCode return - - [SBGECOM-301] - Improved ELLIPSE onboard magnetic calibration code example - - [SBGECOM-302] - Added new sbgEComPurgeIncoming method to discard rx data - - [SBGECOM-303] - Improved examples, they now use simple CLI arguments + - SBGECOM-181 - Reduce stack usage and use new sbgEComReceiveCmd2 + - SBGECOM-220 - Updated license to MIT + - SBGECOM-232 - Updated protocol to support large transfer using multiple pages + - SBGECOM-287 - Updated sbgEComSetReceiveLogCallback to remove the unused SbgErrorCode return + - SBGECOM-301 - Improved ELLIPSE onboard magnetic calibration code example + - SBGECOM-302 - Added new sbgEComPurgeIncoming method to discard rx data + - SBGECOM-303 - Improved examples, they now use simple CLI arguments ### Bug Fixes - - [SBGECOM-265] - Add missing 25ms period to output log message definitions - - [SBGECOM-267] - Add missing 40ms period to output log message definitions - - [SBGECOM-284] - Receive a command must not be blocking when timeout = 0 - - [SBGECOM-289] - Fix SBG_ECAN_MSG_GPS#_ALT NUM_SV & DIFF_CORR DBC/DBF definitions + - SBGECOM-265 - Add missing 25ms period to output log message definitions + - SBGECOM-267 - Add missing 40ms period to output log message definitions + - SBGECOM-284 - Receive a command must not be blocking when timeout = 0 + - SBGECOM-289 - Fix SBG_ECAN_MSG_GPS#_ALT NUM_SV & DIFF_CORR DBC/DBF definitions ## Release - 2.0.4536-stable ### New Features - - [SBGECOM-180] - Added NMEA GGK message output - - [SBGECOM-191] - Added in SBG_ECOM_LOG_GPS#_POS status report for all GNSS constellations - - [SBGECOM-194] - Add specific SBG_ECOM_THIRD_PARTY_ADA_01 output log - - [SBGECOM-208] - Added a README.md file with migration guidelines - - [SBGECOM-216] - Add Cobham SBG_ECOM_THIRD_PARTY_AT_ITINS output log support - - [SBGECOM-219] - Added CAN dbc and BusMaster definitions in sbgECom project + - SBGECOM-180 - Added NMEA GGK message output + - SBGECOM-191 - Added in SBG_ECOM_LOG_GPS#_POS status report for all GNSS constellations + - SBGECOM-194 - Add specific SBG_ECOM_THIRD_PARTY_ADA_01 output log + - SBGECOM-208 - Added a README.md file with migration guidelines + - SBGECOM-216 - Add Cobham SBG_ECOM_THIRD_PARTY_AT_ITINS output log support + - SBGECOM-219 - Added CAN dbc and BusMaster definitions in sbgECom project ### Improvements - - [SBGECOM-188] - Updated SBG_ECAN_MSG_ODO_VELOCITY from float field to integers - - [SBGECOM-196] - Added status field for SBG_ECAN_MSG_AUTO_TRACK_SLIP_CURV (0x220) message - - [SBGECOM-199] - Simplified SbgEComGnssModelsStdIds enum for ELLIPSE-N and ELLIPSE-D - - [SBGECOM-204] - Renamed SBG_ECOM_GNSS_MODEL_UBLOX_GPS_GLONASS to SBG_ECOM_GNSS_MODEL_INTERNAL - - [SBGECOM-207] - Reworked motion profile / aiding equipments errors models set/get API (removed SbgEComModelInfo) - - [SBGECOM-209] - Reworked and simplified GNSS model ids to comply with sbgECom 2.x - - [SBGECOM-212] - Added in SBG_ECOM_CMD_FEATURES gnss firmware version field + - SBGECOM-188 - Updated SBG_ECAN_MSG_ODO_VELOCITY from float field to integers + - SBGECOM-196 - Added status field for SBG_ECAN_MSG_AUTO_TRACK_SLIP_CURV (0x220) message + - SBGECOM-199 - Simplified SbgEComGnssModelsStdIds enum for ELLIPSE-N and ELLIPSE-D + - SBGECOM-204 - Renamed SBG_ECOM_GNSS_MODEL_UBLOX_GPS_GLONASS to SBG_ECOM_GNSS_MODEL_INTERNAL + - SBGECOM-207 - Reworked motion profile / aiding equipments errors models set/get API (removed SbgEComModelInfo) + - SBGECOM-209 - Reworked and simplified GNSS model ids to comply with sbgECom 2.x + - SBGECOM-212 - Added in SBG_ECOM_CMD_FEATURES gnss firmware version field ### Removed Features - - [SBGECOM-200] - Removed SBG_ECOM_GNSS_MODEL_UBLOX_HIGH_DYNAMICS, use SBG_ECOM_GNSS_MODEL_INTERNAL instead - - [SBGECOM-201] - Removed SBG_ECOM_GNSS_MODEL_ELLIPSE_D_INTERNAL, use SBG_ECOM_GNSS_MODEL_INTERNAL instead - - [SBGECOM-202] - Removed SBG_ECOM_GNSS_MODEL_UBLOX_HIGH_SPEED, use SBG_ECOM_GNSS_MODEL_INTERNAL instead - - [SBGECOM-203] - Removed SBG_ECOM_GNSS_MODEL_UBLOX_LOW_SPEED, use SBG_ECOM_GNSS_MODEL_INTERNAL instead - - [SBGECOM-206] - Removed deprecated methods sbgEComCmdGnss1GetLeverArmAlignment & sbgEComCmdGnss1SetLeverArmAlignment - - [SBGECOM-211] - Removed legacy IG-500 protocol support + - SBGECOM-200 - Removed SBG_ECOM_GNSS_MODEL_UBLOX_HIGH_DYNAMICS, use SBG_ECOM_GNSS_MODEL_INTERNAL instead + - SBGECOM-201 - Removed SBG_ECOM_GNSS_MODEL_ELLIPSE_D_INTERNAL, use SBG_ECOM_GNSS_MODEL_INTERNAL instead + - SBGECOM-202 - Removed SBG_ECOM_GNSS_MODEL_UBLOX_HIGH_SPEED, use SBG_ECOM_GNSS_MODEL_INTERNAL instead + - SBGECOM-203 - Removed SBG_ECOM_GNSS_MODEL_UBLOX_LOW_SPEED, use SBG_ECOM_GNSS_MODEL_INTERNAL instead + - SBGECOM-206 - Removed deprecated methods sbgEComCmdGnss1GetLeverArmAlignment & sbgEComCmdGnss1SetLeverArmAlignment + - SBGECOM-211 - Removed legacy IG-500 protocol support ## Release - 1.11.920-stable ### New Features - - [SBGECOM-123] - Implement DVL aiding configuration SBG_ECOM_CMD_DVL_#### - - [SBGECOM-126] - Implement AirData aiding configuration SBG_ECOM_CMD_AIRDATA_#### - - [SBGECOM-135] - Add configuration commands for CAN odometer support - - [SBGECOM-136] - Add sbgECom Log Event Output useful for virtual odometer - - [SBGECOM-137] - Add SBG_ECOM_LOG_DEPTH / SBG_ECAN_MSG_DEPTH_INFO/ALTITUDE output logs - - [SBGECOM-140] - Updated SBG_ECOM_CMD_AIDING_ASSIGNMENT command for AirData support - - [SBGECOM-141] - Add airDataInput demo project to show external sbgECom AirData aiding - - [SBGECOM-142] - Updated SBG_ECOM_LOG_PRESSURE to SBG_ECOM_LOG_AIR_DATA with airspeed - - [SBGECOM-143] - Updated SBG_ECAN_MSG_PRESSURE to SBG_ECAN_MSG_AIR_DATA with airspeed - - [SBGECOM-156] - Add baseline length field in SBG_ECOM_LOG_GPS#_HDT log - - [SBGECOM-162] - Add SBG_ECOM_LOG_DIAG message to send text - - [SBGECOM-163] - Add CAN output message with vehicle body velocity - - [SBGECOM-164] - Add CAN output message with track, slip and curvature indications - - [SBGECOM-168] - Add INDYN NMEA like message output for marine applications - - [SBGECOM-170] - Add SBG_ECOM_CMD_GNSS_1_INSTALLATION command to set/get GNSS lever arm - - [SBGECOM-173] - Add compatibility support Crossbow AHRS500 series - - [SBGECOM-174] - Basic Logger: Add support for error log messages - - [SBGECOM-175] - Basic Logger: Add support for UDP interfaces + - SBGECOM-123 - Implement DVL aiding configuration SBG_ECOM_CMD_DVL_#### + - SBGECOM-126 - Implement AirData aiding configuration SBG_ECOM_CMD_AIRDATA_#### + - SBGECOM-135 - Add configuration commands for CAN odometer support + - SBGECOM-136 - Add sbgECom Log Event Output useful for virtual odometer + - SBGECOM-137 - Add SBG_ECOM_LOG_DEPTH / SBG_ECAN_MSG_DEPTH_INFO/ALTITUDE output logs + - SBGECOM-140 - Updated SBG_ECOM_CMD_AIDING_ASSIGNMENT command for AirData support + - SBGECOM-141 - Add airDataInput demo project to show external sbgECom AirData aiding + - SBGECOM-142 - Updated SBG_ECOM_LOG_PRESSURE to SBG_ECOM_LOG_AIR_DATA with airspeed + - SBGECOM-143 - Updated SBG_ECAN_MSG_PRESSURE to SBG_ECAN_MSG_AIR_DATA with airspeed + - SBGECOM-156 - Add baseline length field in SBG_ECOM_LOG_GPS#_HDT log + - SBGECOM-162 - Add SBG_ECOM_LOG_DIAG message to send text + - SBGECOM-163 - Add CAN output message with vehicle body velocity + - SBGECOM-164 - Add CAN output message with track, slip and curvature indications + - SBGECOM-168 - Add INDYN NMEA like message output for marine applications + - SBGECOM-170 - Add SBG_ECOM_CMD_GNSS_1_INSTALLATION command to set/get GNSS lever arm + - SBGECOM-173 - Add compatibility support Crossbow AHRS500 series + - SBGECOM-174 - Basic Logger: Add support for error log messages + - SBGECOM-175 - Basic Logger: Add support for UDP interfaces ### Improvements - - [SBGECOM-132] - Updated SBG_ECOM_CMD_AIDING_ASSIGNMENT command for DVL support - - [SBGECOM-133] - Renamed DVL standard deviation as quality indicator in DVL log structure - - [SBGECOM-144] - Converted SBG_ECOM_AIDING_EM_LOG_RECV to SBG_ECOM_AIDING_DEPTH_RECV - - [SBGECOM-145] - Renamed SBG_ECOM_AIDING_PRESSURE_RECV to SBG_ECOM_AIDING_AIR_DATA_RECV - - [SBGECOM-146] - Renamed SBG_ECOM_SOL_PRESSURE_USED to SBG_ECOM_SOL_AIR_DATA_USED - - [SBGECOM-147] - Added SBG_ECOM_SOL_DEPTH_USED in EKF solution status - - [SBGECOM-165] - Updated CAN log default IDs for Ship Motion HP from 0x210 to 0x14A - - [SBGECOM-169] - Updated SBG_ECOM_THIRD_PARTY_IXBLUE_XXX logs to comply with naming conventions - - [SBGECOM-176] - Minor code cleanup and removed useless defines - - [SBGECOM-177] - Updated all C types to use standard ones uint32 -> uint32_t - - [SBGECOM-178] - Updated common lib code with improved organization + - SBGECOM-132 - Updated SBG_ECOM_CMD_AIDING_ASSIGNMENT command for DVL support + - SBGECOM-133 - Renamed DVL standard deviation as quality indicator in DVL log structure + - SBGECOM-144 - Converted SBG_ECOM_AIDING_EM_LOG_RECV to SBG_ECOM_AIDING_DEPTH_RECV + - SBGECOM-145 - Renamed SBG_ECOM_AIDING_PRESSURE_RECV to SBG_ECOM_AIDING_AIR_DATA_RECV + - SBGECOM-146 - Renamed SBG_ECOM_SOL_PRESSURE_USED to SBG_ECOM_SOL_AIR_DATA_USED + - SBGECOM-147 - Added SBG_ECOM_SOL_DEPTH_USED in EKF solution status + - SBGECOM-165 - Updated CAN log default IDs for Ship Motion HP from 0x210 to 0x14A + - SBGECOM-169 - Updated SBG_ECOM_THIRD_PARTY_IXBLUE_XXX logs to comply with naming conventions + - SBGECOM-176 - Minor code cleanup and removed useless defines + - SBGECOM-177 - Updated all C types to use standard ones uint32 -> uint32_t + - SBGECOM-178 - Updated common lib code with improved organization ### Removed Features - - [SBGECOM-100] - Removed deprecated sbgEComSetReceiveCallback method - - [SBGECOM-101] - Removed deprecated sbgEComCmdGnss1SetModel method - - [SBGECOM-102] - Removed deprecated sbgEComCmdSensorSetMotionProfile method - - [SBGECOM-103] - Removed deprecated sbgEComCmdMagSetModel method - - [SBGECOM-171] - SBG_ECOM_CMD_GNSS_1_LEVER_ARM_ALIGNMENT deprecated and replaced by SBG_ECOM_CMD_GNSS_1_INSTALLATION + - SBGECOM-100 - Removed deprecated sbgEComSetReceiveCallback method + - SBGECOM-101 - Removed deprecated sbgEComCmdGnss1SetModel method + - SBGECOM-102 - Removed deprecated sbgEComCmdSensorSetMotionProfile method + - SBGECOM-103 - Removed deprecated sbgEComCmdMagSetModel method + - SBGECOM-171 - SBG_ECOM_CMD_GNSS_1_LEVER_ARM_ALIGNMENT deprecated and replaced by SBG_ECOM_CMD_GNSS_1_INSTALLATION ## Release - 1.10.3692-stable ### New Features - - [SBGECOM-115] - Add Septentrio Internal GNSS model for new Ellipse D - - [SBGECOM-117] - Added getters for delta a angle / velocity / temperature for IMU Short log - - [SBGECOM-119] - Added new Swell Mode Ship Motion status flags + - SBGECOM-115 - Add Septentrio Internal GNSS model for new Ellipse D + - SBGECOM-117 - Added getters for delta a angle / velocity / temperature for IMU Short log + - SBGECOM-119 - Added new Swell Mode Ship Motion status flags ### Improvements - - [SBGECOM-118] - Improved sbgEComStartFrameGeneration & sbgEComFinalizeFrameGeneration methods - - [SBGECOM-122] - Increased time out for sbgEComCmdLicenseApply to support new ELLIPSE-D internal GNSS + - SBGECOM-118 - Improved sbgEComStartFrameGeneration & sbgEComFinalizeFrameGeneration methods + - SBGECOM-122 - Increased time out for sbgEComCmdLicenseApply to support new ELLIPSE-D internal GNSS ## Release - 1.9.706-stable ### New Features - - [SBGECOM-110] - Added DOLOG HRP proprietary message - - [SBGECOM-111] - Added a new short IMU log message also used for post processing - - [SBGECOM-112] - Add a heavy machinery motion profile definition for Ellipse series + - SBGECOM-110 - Added DOLOG HRP proprietary message + - SBGECOM-111 - Added a new short IMU log message also used for post processing + - SBGECOM-112 - Add a heavy machinery motion profile definition for Ellipse series ### Bug Fixes - - [SBGECOM-113] - Fixed invalid SBG_ECOM_CAN_RX/TX_OK comments + - SBGECOM-113 - Fixed invalid SBG_ECOM_CAN_RX/TX_OK comments ## Release - 1.8.2916-stable ### New Features - - [SBGECOM-95] - Added GPS number of SV used and diff corrections details in sbgECan protocol - - [SBGECOM-105] - Add compatibility with SBG_ECOM_CMD_VALIDITY_THRESHOLDS command - - [SBGECOM-108] - Added support for aiding assignment on Port E for ELLIPSE-E and N + - SBGECOM-95 - Added GPS number of SV used and diff corrections details in sbgECan protocol + - SBGECOM-105 - Add compatibility with SBG_ECOM_CMD_VALIDITY_THRESHOLDS command + - SBGECOM-108 - Added support for aiding assignment on Port E for ELLIPSE-E and N ### Improvements - - [SBGECOM-91] - Added sbgEComSetCmdTrialsAndTimeOut to setup the number of trials and default time out for commands + - SBGECOM-91 - Added sbgEComSetCmdTrialsAndTimeOut to setup the number of trials and default time out for commands ## Release - 1.7.235-stable ### New Features - - [SBGECOM-89] - Implement Kongsberg Binary 26 message output - - [SBGECOM-87] - Add an uptime indication in SBG_ECOM_LOG_STATUS - - [SBGECOM-86] - Added the command SBG_ECOM_CMD_ETHERNET_INFO to current device IP address - - [SBGECOM-85] - Added command SBG_ECOM_CMD_ETHERNET_CONF to define / retrieve the Ethernet configuration - - [SBGECOM-77] - Add new output log class for NMEA proprietary messages - - [SBGECOM-75] - Added UAV motion profile definition (for low dynamic rotary wing UAV applications) + - SBGECOM-89 - Implement Kongsberg Binary 26 message output + - SBGECOM-87 - Add an uptime indication in SBG_ECOM_LOG_STATUS + - SBGECOM-86 - Added the command SBG_ECOM_CMD_ETHERNET_INFO to current device IP address + - SBGECOM-85 - Added command SBG_ECOM_CMD_ETHERNET_CONF to define / retrieve the Ethernet configuration + - SBGECOM-77 - Add new output log class for NMEA proprietary messages + - SBGECOM-75 - Added UAV motion profile definition (for low dynamic rotary wing UAV applications) ### Improvements - - [SBGECOM-83] - Better use of size_t type instead of uint32 to comply with C standard and 64 bit platforms - - [SBGECOM-84] - Updated sbgCommonLib to latest revision + - SBGECOM-83 - Better use of size_t type instead of uint32 to comply with C standard and 64 bit platforms + - SBGECOM-84 - Updated sbgCommonLib to latest revision ### Removed Features - - [SBGECOM-79] - Removed deprecated "course" from the GNSS configurable aiding sources + - SBGECOM-79 - Removed deprecated "course" from the GNSS configurable aiding sources ## Release - 1.5.209-stable ### New Features - - [SBGECOM-72] - Added proprietary NMEA message PASHR for roll, pitch, heading, heave - - [SBGECOM-70] - Added SBG Proprietary NMEA message with acceleration and angular rate - - [SBGECOM-68] - Added SBG_ECOM_OUTPUT_MODE_DIV_5 flag for 40 Hz output - - [SBGECOM-66] - Added SBG_ECOM_GENERAL_CPU_OK status flag - - [SBGECOM-65] - Added sbgEComHandleOneLog method to return even if more logs are available - - [SBGECOM-64] - Added sbgEComSendAck method - - [SBGECOM-62] - Added sbgEComStartFrameGeneration and sbgEComFinalizeFrameGeneration methods - - [SBGECOM-59] - Added sbgECom log generation code - - [SBGECOM-57] - Added SBG_ECOM_LOG_FAST_IMU_DATA message definition - - [SBGECOM-40] - Added KVH third party output format id (SBG_ECOM_THIRD_PARTY_KVH) + - SBGECOM-72 - Added proprietary NMEA message PASHR for roll, pitch, heading, heave + - SBGECOM-70 - Added SBG Proprietary NMEA message with acceleration and angular rate + - SBGECOM-68 - Added SBG_ECOM_OUTPUT_MODE_DIV_5 flag for 40 Hz output + - SBGECOM-66 - Added SBG_ECOM_GENERAL_CPU_OK status flag + - SBGECOM-65 - Added sbgEComHandleOneLog method to return even if more logs are available + - SBGECOM-64 - Added sbgEComSendAck method + - SBGECOM-62 - Added sbgEComStartFrameGeneration and sbgEComFinalizeFrameGeneration methods + - SBGECOM-59 - Added sbgECom log generation code + - SBGECOM-57 - Added SBG_ECOM_LOG_FAST_IMU_DATA message definition + - SBGECOM-40 - Added KVH third party output format id (SBG_ECOM_THIRD_PARTY_KVH) ### Improvements - - [SBGECOM-74] - Switched unix projects to CMake - - [SBGECOM-73] - Added examples into the sbgECom - - [SBGECOM-63] - Updated sbgCommonLib to latest revision - - [SBGECOM-61] - Use stream buffer instead of basic buffer for sbgECom log parsing - - [SBGECOM-51] - Added SBF (Septentrio) protocol support on Ellipse-E - - [SBGECOM-50] - Added direct PPS from internal GNSS Sync Output (SBG_ECOM_CMD_SYNC_OUT_CONF) - - [SBGECOM-47] - Made the sbgECom 64 bit compatible - - [SBGECOM-46] - Switched project files to Visual Studio 2013 - - [SBGECOM-45] - Added a new callback method (sbgEComSetReceiveLogCallback) and deprecated the old one (sbgEComSetReceiveCallback) - - [SBGECOM-42] - Improved handling of MSG and CLASS fields in low level protocol functions + - SBGECOM-74 - Switched unix projects to CMake + - SBGECOM-73 - Added examples into the sbgECom + - SBGECOM-63 - Updated sbgCommonLib to latest revision + - SBGECOM-61 - Use stream buffer instead of basic buffer for sbgECom log parsing + - SBGECOM-51 - Added SBF (Septentrio) protocol support on Ellipse-E + - SBGECOM-50 - Added direct PPS from internal GNSS Sync Output (SBG_ECOM_CMD_SYNC_OUT_CONF) + - SBGECOM-47 - Made the sbgECom 64 bit compatible + - SBGECOM-46 - Switched project files to Visual Studio 2013 + - SBGECOM-45 - Added a new callback method (sbgEComSetReceiveLogCallback) and deprecated the old one (sbgEComSetReceiveCallback) + - SBGECOM-42 - Improved handling of MSG and CLASS fields in low level protocol functions ### Bug Fixes - - [SBGECOM-71] - Fixed sbgEComCmdGetInfo incorrect error code return when an invalid payload is received - - [SBGECOM-44] - Fixed Various incompatibilities in Big Endian platforms - - [SBGECOM-43] - Added output of NACK reasons in sbgECom configuration commands using the return code + - SBGECOM-71 - Fixed sbgEComCmdGetInfo incorrect error code return when an invalid payload is received + - SBGECOM-44 - Fixed Various incompatibilities in Big Endian platforms + - SBGECOM-43 - Added output of NACK reasons in sbgECom configuration commands using the return code ### Removed Features - - [SBGECOM-60] - Removed Ship Motion 1,2,3 and Ship Motion HP 1,2,3 due to new deported heave concepts + - SBGECOM-60 - Removed Ship Motion 1,2,3 and Ship Motion HP 1,2,3 due to new deported heave concepts ## Release - 1.4.3239-stable ### New Features - - [SBGECOM-28] - Added differential correction age, diff base id and num sv to the SBG_ECOM_LOG_GPS#_POS - - [SBGECOM-29] - Added GNSS raw data log for the second GNSS receiver - - [SBGECOM-30] - Added official support for Ellipse additional output interfaces PORT C and PORT E - - [SBGECOM-33] - Added big/little endian support for stream buffer - - [SBGECOM-34] - Added sbgPlatform.h file to setup platform specific configuration such as endianness + - SBGECOM-28 - Added differential correction age, diff base id and num sv to the SBG_ECOM_LOG_GPS#_POS + - SBGECOM-29 - Added GNSS raw data log for the second GNSS receiver + - SBGECOM-30 - Added official support for Ellipse additional output interfaces PORT C and PORT E + - SBGECOM-33 - Added big/little endian support for stream buffer + - SBGECOM-34 - Added sbgPlatform.h file to setup platform specific configuration such as endianness ### Improvements - - [SBGECOM-7] - Added support for both little and big endian platforms - - [SBGECOM-32] - Improved stream buffer error handling - - [SBGECOM-36] - Improved File naming and overall library organization - - [SBGECOM-37] - Modified firmware and software version numbering scheme - - [SBGECOM-38] - Increased raw GPS data buffer size from 2048 to 4096 bytes + - SBGECOM-7 - Added support for both little and big endian platforms + - SBGECOM-32 - Improved stream buffer error handling + - SBGECOM-36 - Improved File naming and overall library organization + - SBGECOM-37 - Modified firmware and software version numbering scheme + - SBGECOM-38 - Increased raw GPS data buffer size from 2048 to 4096 bytes ### Bug Fixes - - [SBGECOM-21] - Fixed SBG_ECOM_ETH#_RX_OK and SBG_ECOM_ETH#_TX_OK status definitions - - [SBGECOM-27] - Changed sbgEComHandle behavior so the error returned by receive call back is taken into account - - [SBGECOM-35] - Fixed improper comments in some configuration structures + - SBGECOM-21 - Fixed SBG_ECOM_ETH#_RX_OK and SBG_ECOM_ETH#_TX_OK status definitions + - SBGECOM-27 - Changed sbgEComHandle behavior so the error returned by receive call back is taken into account + - SBGECOM-35 - Fixed improper comments in some configuration structures ## Release - 1.3 ### New Features - - [SBGECOM-10] - Added sbgInterfaceChangeBaudrate for both windows and unix platforms - - [SBGECOM-19] - Added SBG_ECOM_LOG_PRESSURE log for depth sensors and altimeters - - [SBGECOM-25] - Added support for Ellipse series - - [SBGECOM-26] - Added SBG_ECOM_LOG_USBL log for USBL aiding equipments (beta) + - SBGECOM-10 - Added sbgInterfaceChangeBaudrate for both windows and unix platforms + - SBGECOM-19 - Added SBG_ECOM_LOG_PRESSURE log for depth sensors and altimeters + - SBGECOM-25 - Added support for Ellipse series + - SBGECOM-26 - Added SBG_ECOM_LOG_USBL log for USBL aiding equipments (beta) ### Improvements - - [SBGECOM-18] - Fixed Typos in GPS pos, Vel and Hdt Fix Status - - [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-18 - Fixed Typos in GPS pos, Vel and Hdt Fix Status + - 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 ## Release - 1.2 ### New Features - - [SBGECOM-14] - Added SBG_ECOM_LOG_SHIP_MOTION_HP logs for delayed heave output - - [SBGECOM-15] - Added sbgInterfaceSerialChangeBaudrate method to change the serial interface baud rate - - [SBGECOM-17] - Added SBG_ECOM_POS_FIXED / SBG_ECAN_POS_FIXED position type for GPS + - SBGECOM-14 - Added SBG_ECOM_LOG_SHIP_MOTION_HP logs for delayed heave output + - SBGECOM-15 - Added sbgInterfaceSerialChangeBaudrate method to change the serial interface baud rate + - SBGECOM-17 - Added SBG_ECOM_POS_FIXED / SBG_ECAN_POS_FIXED position type for GPS ### Improvements - - [SBGECOM-13] - Updated SBG_ECOM_LOG_SHIP_MOTION_XXX logs to add velocity and status data - - [SBGECOM-16] - Changed GPS OmniStar solution type to PPP ones for better compatibility with third party GPS + - SBGECOM-13 - Updated SBG_ECOM_LOG_SHIP_MOTION_XXX logs to add velocity and status data + - SBGECOM-16 - Changed GPS OmniStar solution type to PPP ones for better compatibility with third party GPS ### Removed Features - - [SBGECOM-11] - Removed heave status field from SBG_ECOM_LOG_STATUS log + - SBGECOM-11 - Removed heave status field from SBG_ECOM_LOG_STATUS log ## Release - 1.1 ### New Features - - [SBGECOM-1] - Added output log for DVL support - - [SBGECOM-3] - Added output for GPS 1 raw data in order to support post processing - - [SBGECOM-4] - Added event markers logs support - - [SBGECOM-6] - Added Unix support and build script - - [SBGECOM-8] - Added sbgEComReceiveAnyCmd method that return any received command that is not an output log - - [SBGECOM-9] - Added settings import and export command + - SBGECOM-1 - Added output log for DVL support + - SBGECOM-3 - Added output for GPS 1 raw data in order to support post processing + - SBGECOM-4 - Added event markers logs support + - SBGECOM-6 - Added Unix support and build script + - SBGECOM-8 - Added sbgEComReceiveAnyCmd method that return any received command that is not an output log + - SBGECOM-9 - Added settings import and export command ### Improvements - - [SBGECOM-2] - Added pitch information in the SbgLogGpsHdt GPS true heading log - - [SBGECOM-5] - Now sbgEComProtocolReceive method returns the received command even if the CRC is not valid + - SBGECOM-2 - Added pitch information in the SbgLogGpsHdt GPS true heading log + - SBGECOM-5 - Now sbgEComProtocolReceive method returns the received command even if the CRC is not valid diff --git a/CMakeLists.txt b/CMakeLists.txt index a051a98..7a6dc67 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,22 +1,34 @@ -cmake_minimum_required(VERSION 3.18 FATAL_ERROR) +cmake_minimum_required(VERSION 3.16 FATAL_ERROR) set(CMAKE_BUILD_TYPE Release CACHE STRING "build type") project(sbgECom) # -# Project configuration +# Compiler configuration # set(CMAKE_C_STANDARD 99) set(CMAKE_C_STANDARD_REQUIRED ON) set(CMAKE_C_EXTENSIONS ON) set(CMAKE_POSITION_INDEPENDENT_CODE ON) -option(BUILD_EXAMPLES "Build examples" OFF) -option(BUILD_TOOLS "Build tools" OFF) +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +# +# Project configuration +# +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() # -# sbgECom +# sbgECom library # add_library(${PROJECT_NAME} STATIC) @@ -91,20 +103,55 @@ if (BUILD_TOOLS) 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/main.c - ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/sbgBasicLoggerAccumulators.h - ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/sbgBasicLoggerFile.h - ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/sbgBasicLoggerHandler.h - ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/sbgBasicLoggerAccumulators.c - ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/sbgBasicLoggerFile.c - ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/sbgBasicLoggerHandler.c + ${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_include_directories(sbgBasicLogger PRIVATE ${argtable3_SOURCE_DIR}/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) diff --git a/README.md b/README.md index bbf7b0f..310e38d 100644 --- a/README.md +++ b/README.md @@ -15,7 +15,7 @@ The library is written and maintained by SBG Systems SAS. You can contact the su # Documentation -You can access the full online sbgECom Doxygen documentation [here](https://developer.sbg-systems.com/sbgECom/3.2). +You can access the full online sbgECom Doxygen documentation [here](https://developer.sbg-systems.com/sbgECom/4.0). 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. @@ -27,16 +27,20 @@ The library has been designed to be easily ported to any platform by just provid # Building sbgECom Library The sbgECom library and code examples are very easy to compile on any platform using CMake. -The library has no third party library dependencies making it very easy to build. +The sbgECom library and code samples have no third party library dependencies. + +However, if you would like to build sbgECom CLI tools such as the `sbgBasicLogger` or `sbgInsRestApi` there is a dependency on Argtable3. SBG Systems doesn't provide the sbgECom as a pre-compiled library for obvious and good reasons. ## Dependencies -SBG Systems has validated the following toolchain: +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) ## 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: @@ -48,6 +52,9 @@ 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. @@ -108,6 +115,7 @@ The `bin/tools` directory contains pre-compiled 64 bits binaries for Windows, Li ## 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. diff --git a/common/debug/sbgDebug.h b/common/debug/sbgDebug.h index a4879cd..35a6e71 100644 --- a/common/debug/sbgDebug.h +++ b/common/debug/sbgDebug.h @@ -66,7 +66,7 @@ typedef enum _SbgDebugLogType * \param[in] errorCode The error code that has thrown this error. * \param[in] format String litteral for the associated error message (you can use printf like string formating). */ -#define SBG_LOG_ERROR_CALL(errorCode, format, ...) sbgPlatformDebugLogMsg((const char*)__BASE_FILE__, (const char*)__FUNCTION__, __LINE__, SBG_DEBUG_LOG_CATEGORY, SBG_DEBUG_LOG_TYPE_ERROR, errorCode, format, ##__VA_ARGS__) +#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 @@ -79,7 +79,7 @@ typedef enum _SbgDebugLogType * \param[in] errorCode The error code that has thrown this warning. * \param[in] format String litteral for the associated warning message (you can use printf like string formating). */ -#define SBG_LOG_WARNING_CALL(errorCode, format, ...) sbgPlatformDebugLogMsg((const char*)__BASE_FILE__, (const char*)__FUNCTION__, __LINE__, SBG_DEBUG_LOG_CATEGORY, SBG_DEBUG_LOG_TYPE_WARNING, errorCode, format, ##__VA_ARGS__) +#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 @@ -91,7 +91,7 @@ typedef enum _SbgDebugLogType * Log an information message. * \param[in] format String litteral for the information message (you can use printf like string formating). */ -#define SBG_LOG_INFO_CALL(format, ...) sbgPlatformDebugLogMsg((const char*)__BASE_FILE__, (const char*)__FUNCTION__, __LINE__, SBG_DEBUG_LOG_CATEGORY, SBG_DEBUG_LOG_TYPE_INFO, SBG_NO_ERROR, format, ##__VA_ARGS__) +#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 @@ -103,7 +103,7 @@ typedef enum _SbgDebugLogType * Log an information message only in debug mode * \param[in] format String litteral for the information message (you can use printf like string formating). */ -#define SBG_LOG_DEBUG_CALL(format, ...) sbgPlatformDebugLogMsg((const char*)__BASE_FILE__, (const char*)__FUNCTION__, __LINE__, SBG_DEBUG_LOG_CATEGORY, SBG_DEBUG_LOG_TYPE_DEBUG, SBG_NO_ERROR, format, ##__VA_ARGS__) +#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 diff --git a/common/interfaces/sbgInterface.c b/common/interfaces/sbgInterface.c index 0b728ff..b3b5167 100644 --- a/common/interfaces/sbgInterface.c +++ b/common/interfaces/sbgInterface.c @@ -25,9 +25,6 @@ void sbgInterfaceZeroInit(SbgInterface *pInterface) { assert(pInterface); - // - // Make sure the whole struct is zero init - // memset(pInterface, 0x00, sizeof(*pInterface)); } diff --git a/common/interfaces/sbgInterfaceSerialUnix.c b/common/interfaces/sbgInterfaceSerialUnix.c index 88871d0..e4010f3 100644 --- a/common/interfaces/sbgInterfaceSerialUnix.c +++ b/common/interfaces/sbgInterfaceSerialUnix.c @@ -23,7 +23,7 @@ /*! - * 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. @@ -33,7 +33,7 @@ static uint32_t sbgInterfaceSerialGetBaudRateConst(uint32_t baudRate) uint32_t baudRateConst; // - // Create the right baud rate value for unix platforms + // Create the right baud rate value for Unix platforms // switch (baudRate) { @@ -141,14 +141,14 @@ static SbgErrorCode sbgInterfaceSerialDestroy(SbgInterface *pInterface) // // Get the internal serial handle // - pSerialHandle = (int*)pInterface->handle; + pSerialHandle = (int *)pInterface->handle; // // Close the port com // close((*pSerialHandle)); SBG_FREE(pSerialHandle); - pInterface->handle = NULL; + sbgInterfaceZeroInit(pInterface); return SBG_NO_ERROR; } @@ -212,7 +212,7 @@ static SbgErrorCode sbgInterfaceSerialFlush(SbgInterface *pInterface, uint32_t f } /*! - * Change the serial interface baud rate immediatly. + * 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. @@ -321,7 +321,7 @@ static SbgErrorCode sbgInterfaceSerialWrite(SbgInterface *pInterface, const void else { // - // An error has occured during the write + // An error has occurred during the write // fprintf(stderr, "sbgDeviceWrite: Unable to write to our device: %s\n", strerror(errno)); return SBG_WRITE_ERROR; @@ -443,7 +443,7 @@ SbgErrorCode sbgInterfaceSerialCreate(SbgInterface *pInterface, const char *devi if (fcntl((*pSerialHandle), F_SETFL, O_NONBLOCK) != -1) { // - // Retreive current options + // Retrieve current options // if (tcgetattr((*pSerialHandle), &options) != -1) { diff --git a/common/interfaces/sbgInterfaceSerialWin.c b/common/interfaces/sbgInterfaceSerialWin.c index de3b652..1bea74a 100644 --- a/common/interfaces/sbgInterfaceSerialWin.c +++ b/common/interfaces/sbgInterfaceSerialWin.c @@ -135,7 +135,7 @@ static SbgErrorCode sbgInterfaceSerialWrite(SbgInterface *pInterface, const void if (WriteFile(pSerialDevice, pCurrentBuffer, numBytesLeftToWrite, (LPDWORD)&numBytesWritten, NULL) == false) { // - // An error has occured during the write + // An error has occurred during the write // sbgGetWindowsErrorMsg(errorMsg, sizeof(errorMsg)); SBG_LOG_ERROR(SBG_WRITE_ERROR, "Write failed error: %s", errorMsg); @@ -400,7 +400,7 @@ SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceSerialCreate(SbgInterface *pInterfac if (PurgeComm(hSerialDevice, PURGE_TXABORT | PURGE_RXABORT | PURGE_TXCLEAR | PURGE_RXCLEAR)) { // - // Retreives current com state and com timeout + // Retrieve current com state and com timeout // if ( (GetCommState(hSerialDevice, &comState)) && (GetCommTimeouts(hSerialDevice, &comTimeOut)) ) { diff --git a/common/interfaces/sbgInterfaceUdp.c b/common/interfaces/sbgInterfaceUdp.c index 6f24653..d75aa10 100644 --- a/common/interfaces/sbgInterfaceUdp.c +++ b/common/interfaces/sbgInterfaceUdp.c @@ -43,6 +43,8 @@ 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. */ + 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. */ @@ -67,6 +69,21 @@ static SbgInterfaceUdp *sbgInterfaceUdpGet(SbgInterface *pInterface) return (SbgInterfaceUdp*)pInterface->handle; } +/*! + * Returns the UDP interface instance (const version) + * + * \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); + + return (const SbgInterfaceUdp*)pInterface->handle; +} + /*! * Initialize the socket API. * @@ -205,9 +222,9 @@ static SbgErrorCode sbgInterfaceUdpWrite(SbgInterface *pInterface, const void *p pUdpHandle = sbgInterfaceUdpGet(pInterface); - outAddr.sin_family = AF_INET; - outAddr.sin_addr.s_addr = pUdpHandle->remoteAddr; - outAddr.sin_port = htons((uint16_t)pUdpHandle->remotePort); + outAddr.sin_family = AF_INET; + outAddr.sin_addr.s_addr = pUdpHandle->remoteAddr; + outAddr.sin_port = htons((uint16_t)pUdpHandle->remotePort); while (bytesToWrite != 0) { @@ -273,20 +290,16 @@ static SbgErrorCode sbgInterfaceUdpRead(SbgInterface *pInterface, void *pBuffer, if (ret != -1) { - if ((pUdpHandle->remoteAddr == 0) && - (pUdpHandle->remotePort == 0)) + if (pUdpHandle->useConnected) { - pUdpHandle->remoteAddr = remoteAddr.sin_addr.s_addr; - pUdpHandle->remotePort = ntohs(remoteAddr.sin_port); - } - else if ((pUdpHandle->remoteAddr != remoteAddr.sin_addr.s_addr) || - (pUdpHandle->remotePort != ntohs(remoteAddr.sin_port))) - { - char remoteAddrString[16]; + 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_WARNING(SBG_READ_ERROR, "received data from invalid remote host (%s:%u)", remoteAddrString, ntohs(remoteAddr.sin_port)); - ret = 0; + 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; @@ -314,7 +327,6 @@ static SbgErrorCode sbgInterfaceUdpRead(SbgInterface *pInterface, void *pBuffer, return errorCode; } - //----------------------------------------------------------------------// //- Public functions -// //----------------------------------------------------------------------// @@ -340,6 +352,7 @@ SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceUdpCreate(SbgInterface *pInterface, if (pNewUdpHandle) { + pNewUdpHandle->useConnected = false; pNewUdpHandle->remoteAddr = remoteAddr; pNewUdpHandle->remotePort = remotePort; pNewUdpHandle->localPort = localPort; @@ -355,6 +368,9 @@ SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceUdpCreate(SbgInterface *pInterface, 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); @@ -423,7 +439,29 @@ SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceUdpCreate(SbgInterface *pInterface, return errorCode; } +SBG_COMMON_LIB_API void sbgInterfaceUdpSetConnectedMode(SbgInterface *pInterface, bool useConnected) +{ + SbgInterfaceUdp *pUdpHandle; + assert(pInterface); + assert(pInterface->type == SBG_IF_TYPE_ETH_UDP); + + pUdpHandle = sbgInterfaceUdpGet(pInterface); + + pUdpHandle->useConnected = useConnected; +} + +SBG_COMMON_LIB_API bool sbgInterfaceUdpGetConnectedMode(const SbgInterface *pInterface) +{ + const SbgInterfaceUdp *pUdpHandle; + + assert(pInterface); + assert(pInterface->type == SBG_IF_TYPE_ETH_UDP); + + pUdpHandle = sbgInterfaceUdpGetConst(pInterface); + + return pUdpHandle->useConnected; +} SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceUdpAllowBroadcast(SbgInterface *pInterface, bool allowBroadcast) { diff --git a/common/interfaces/sbgInterfaceUdp.h b/common/interfaces/sbgInterfaceUdp.h index a2f6cda..91fbbd4 100644 --- a/common/interfaces/sbgInterfaceUdp.h +++ b/common/interfaces/sbgInterfaceUdp.h @@ -47,18 +47,46 @@ extern "C" { /*! * Initialize an unconnected UDP interface for read and write operations. * - * If the remote address and port are zero, the interface waits for the first packet received on the - * local port and uses the source of that packet as its remote host. This provides a convenient way - * to create "server" UDP interfaces. + * Connected/Unconnected modes: + * + * The UDP socket is unconnected so any host sending data on localPort should be received. + * However, only the host specified in remoteAddr should received sent data on remotePort. + * + * You can enable the connected mode with the `sbgInterfaceUdpSetConnectedMode` method. + * If this case, only datagrams sent by the host with remoteAddr ip and remotePort + * will be received and any other traffic should be discarded. + * + * Broadcast: + * + * 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 Ehternet port on which the interface is listening. + * \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); +/*! + * Define if the UDP socket should use connected or unconnected mode. + * + * 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. + */ +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. + */ +SBG_COMMON_LIB_API bool sbgInterfaceUdpGetConnectedMode(const SbgInterface *pInterface); + /*! * Define if a socket can send broadcasted packets. * diff --git a/common/sbgCommon.c b/common/sbgCommon.c deleted file mode 100644 index 9691fde..0000000 --- a/common/sbgCommon.c +++ /dev/null @@ -1,28 +0,0 @@ -// sbgCommonLib headers -#include - -#ifdef USE_BUILD_NUMBER_GENERATOR -#include -#endif - -//----------------------------------------------------------------------// -//- Public functions -// -//----------------------------------------------------------------------// - -SBG_COMMON_LIB_API uint32_t sbgCommonLibGetVersion(void) -{ -#ifdef USE_BUILD_NUMBER_GENERATOR - return SBG_COMMON_LIB_VERSION; -#else - return 0; -#endif -} - -SBG_COMMON_LIB_API bool sbgCommonLibIsDebug(void) -{ -#ifdef NDEBUG - return false; -#else - return true; -#endif -} diff --git a/common/sbgCommon.h b/common/sbgCommon.h index c2cc8a1..b22adc2 100644 --- a/common/sbgCommon.h +++ b/common/sbgCommon.h @@ -125,21 +125,6 @@ extern "C" { #include "debug/sbgDebug.h" #include "platform/sbgPlatform.h" -//----------------------------------------------------------------------// -//- Public functions -// -//----------------------------------------------------------------------// - -/*! - * Retreive the current sbgCommonLib encoded version. - * \return The current sbgCommonLib version - */ -SBG_COMMON_LIB_API uint32_t sbgCommonLibGetVersion(void); - -/*! - * Tell if the library is compiled in debug mode or not - * \return True if it's compiled in debug, False otherwise - */ -SBG_COMMON_LIB_API bool sbgCommonLibIsDebug(void); #ifdef __cplusplus } diff --git a/common/sbgTypes.h b/common/sbgTypes.h index ca068ca..aa76780 100644 --- a/common/sbgTypes.h +++ b/common/sbgTypes.h @@ -61,21 +61,7 @@ #define SBG_MAX_UINT_56 (72057594037927935ull) //----------------------------------------------------------------------// -//- DEPRECATED: Scalar types definitions -// -//----------------------------------------------------------------------// -SBG_DEPRECATED_TYPEDEF(typedef unsigned char uint8); // 8 bits -SBG_DEPRECATED_TYPEDEF(typedef unsigned short uint16); // 16 bits -SBG_DEPRECATED_TYPEDEF(typedef unsigned int uint32); // 32 bits -SBG_DEPRECATED_TYPEDEF(typedef unsigned long long int uint64); // 64 bits - -SBG_DEPRECATED_TYPEDEF(typedef signed char int8); // 8 bits -SBG_DEPRECATED_TYPEDEF(typedef signed short int16); // 16 bits -SBG_DEPRECATED_TYPEDEF(typedef signed int int32); // 32 bits -SBG_DEPRECATED_TYPEDEF(typedef signed long long int int64); // 64 bits - - -//----------------------------------------------------------------------// -//- Misc types definitions -// +//- 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. */ @@ -86,94 +72,85 @@ typedef uint32_t sbgIpAddress; /*!< Define an IP v4 address stored in 4 /*! * Used to get a uint32_t from a uint8_t array. */ -typedef union _Uint8PtrToUint32Ptr +typedef union _SbgUint8PtrToUint32Ptr { uint8_t *m_pointerUint8; /*!< Set the address used to access the uint32_t. */ uint32_t *m_pointerUint32; /*!< Store the unint32 value. */ -} Uint8PtrToUint32Ptr; +} SbgUint8PtrToUint32Ptr; /*! * Union used to convert a buffer or 2 unit8 two's complement values to a int16_t */ -typedef union _Uint8ToInt16 +typedef union _SbgUint8ToInt16 { int16_t value; uint8_t buffer[2]; -} Uint8ToInt16; +} SbgUint8ToInt16; /*! * Union used to convert a buffer or 2 unit8 values to a uint16_t */ -typedef union _Uint8ToUint16 +typedef union _SbgUint8ToUint16 { uint16_t value; uint8_t buffer[2]; -} Uint8ToUint16; +} SbgUint8ToUint16; /*! * Union used to convert a buffer or 4 unit8 two's complement values to a int32_t */ -typedef union _Uint8ToInt32 +typedef union _SbgUint8ToInt32 { int32_t value; uint8_t buffer[4]; -} Uint8ToInt32; +} SbgUint8ToInt32; /*! * Union used to convert a buffer or 4 unit8 values to a uint32_t */ -typedef union _Uint8ToUint32 +typedef union _SbgUint8ToUint32 { uint32_t value; uint8_t buffer[4]; -} Uint8ToUint32; +} SbgUint8ToUint32; /*! * Union used to convert a buffer or 8 unit8 two's complement values to a int64_t */ -typedef union _Uint8ToInt64 +typedef union _SbgUint8ToInt64 { int64_t value; uint8_t buffer[8]; -} Uint8ToInt64; +} SbgUint8ToInt64; /*! * Union used to convert a buffer or 8 unit8 values to a uint64_t */ -typedef union _Uint8ToUint64 +typedef union _SbgUint8ToUint64 { uint64_t value; uint8_t buffer[8]; -} Uint8ToUint64; +} SbgUint8ToUint64; /*! * Union that allows type punning (access to a floating point number bits) */ -typedef union _FloatNint +typedef union _SbgFloatNint { float valF; int32_t valI; uint32_t valU; -} FloatNint; +} SbgFloatNint; /*! * Union that allows type punning (access to a double number bits) */ -typedef union _DoubleNint +typedef union _SbgDoubleNint { double valF; uint64_t valU; int64_t valI; -} DoubleNint; - -/*! - * Structure that splits a 64bits - */ -typedef struct _Split64 -{ - uint32_t high; - uint32_t low; -} Split64; +} SbgDoubleNint; /*! * Set of 3 int32_t @@ -191,4 +168,29 @@ typedef struct _Split64 int64_t v[3]; } SbgVector3ll; -#endif /* SBG_TYPES_H */ +//----------------------------------------------------------------------// +//- 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); +#endif + +#endif // SBG_TYPES_H diff --git a/common/streamBuffer/sbgStreamBufferBE.h b/common/streamBuffer/sbgStreamBufferBE.h index c14d89c..b78173f 100644 --- a/common/streamBuffer/sbgStreamBufferBE.h +++ b/common/streamBuffer/sbgStreamBufferBE.h @@ -185,7 +185,7 @@ SBG_INLINE uint16_t sbgStreamBufferReadUint16BE(SbgStreamBuffer *pHandle) */ SBG_INLINE int32_t sbgStreamBufferReadInt24BE(SbgStreamBuffer *pHandle) { - Uint8ToInt32 value; + SbgUint8ToInt32 value; assert(pHandle); @@ -251,7 +251,7 @@ SBG_INLINE int32_t sbgStreamBufferReadInt24BE(SbgStreamBuffer *pHandle) */ SBG_INLINE uint32_t sbgStreamBufferReadUint24BE(SbgStreamBuffer *pHandle) { - Uint8ToUint32 value; + SbgUint8ToUint32 value; assert(pHandle); @@ -459,7 +459,7 @@ SBG_INLINE uint32_t sbgStreamBufferReadUint32BE(SbgStreamBuffer *pHandle) */ SBG_INLINE int64_t sbgStreamBufferReadInt40BE(SbgStreamBuffer *pHandle) { - Uint8ToInt64 value; + SbgUint8ToInt64 value; assert(pHandle); @@ -529,7 +529,7 @@ SBG_INLINE int64_t sbgStreamBufferReadInt40BE(SbgStreamBuffer *pHandle) */ SBG_INLINE uint64_t sbgStreamBufferReadUint40BE(SbgStreamBuffer *pHandle) { - Uint8ToUint64 value; + SbgUint8ToUint64 value; assert(pHandle); @@ -599,7 +599,7 @@ SBG_INLINE uint64_t sbgStreamBufferReadUint40BE(SbgStreamBuffer *pHandle) */ SBG_INLINE int64_t sbgStreamBufferReadInt48BE(SbgStreamBuffer *pHandle) { - Uint8ToInt64 value; + SbgUint8ToInt64 value; assert(pHandle); @@ -671,7 +671,7 @@ SBG_INLINE int64_t sbgStreamBufferReadInt48BE(SbgStreamBuffer *pHandle) */ SBG_INLINE uint64_t sbgStreamBufferReadUint48BE(SbgStreamBuffer *pHandle) { - Uint8ToUint64 value; + SbgUint8ToUint64 value; assert(pHandle); @@ -743,7 +743,7 @@ SBG_INLINE uint64_t sbgStreamBufferReadUint48BE(SbgStreamBuffer *pHandle) */ SBG_INLINE int64_t sbgStreamBufferReadInt56BE(SbgStreamBuffer *pHandle) { - Uint8ToInt64 value; + SbgUint8ToInt64 value; assert(pHandle); @@ -817,7 +817,7 @@ SBG_INLINE int64_t sbgStreamBufferReadInt56BE(SbgStreamBuffer *pHandle) */ SBG_INLINE int64_t sbgStreamBufferReadUint56BE(SbgStreamBuffer *pHandle) { - Uint8ToUint64 value; + SbgUint8ToUint64 value; assert(pHandle); @@ -1076,7 +1076,7 @@ SBG_INLINE size_t sbgStreamBufferReadSizeT64BE(SbgStreamBuffer *pHandle) */ SBG_INLINE float sbgStreamBufferReadFloatBE(SbgStreamBuffer *pHandle) { - FloatNint floatInt; + SbgFloatNint floatInt; assert(pHandle); @@ -1123,7 +1123,7 @@ SBG_INLINE float sbgStreamBufferReadFloatBE(SbgStreamBuffer *pHandle) */ SBG_INLINE double sbgStreamBufferReadDoubleBE(SbgStreamBuffer *pHandle) { - DoubleNint doubleInt; + SbgDoubleNint doubleInt; assert(pHandle); @@ -1774,7 +1774,7 @@ SBG_INLINE SbgErrorCode sbgStreamBufferWriteSizeT64BE(SbgStreamBuffer *pHandle, */ SBG_INLINE SbgErrorCode sbgStreamBufferWriteFloatBE(SbgStreamBuffer *pHandle, float value) { - FloatNint floatInt; + SbgFloatNint floatInt; assert(pHandle); @@ -1806,7 +1806,7 @@ SBG_INLINE SbgErrorCode sbgStreamBufferWriteFloatBE(SbgStreamBuffer *pHandle, fl */ SBG_INLINE SbgErrorCode sbgStreamBufferWriteDoubleBE(SbgStreamBuffer *pHandle, double value) { - DoubleNint doubleInt; + SbgDoubleNint doubleInt; assert(pHandle); diff --git a/common/streamBuffer/sbgStreamBufferLE.h b/common/streamBuffer/sbgStreamBufferLE.h index e3dc97e..2a43ea9 100644 --- a/common/streamBuffer/sbgStreamBufferLE.h +++ b/common/streamBuffer/sbgStreamBufferLE.h @@ -185,7 +185,7 @@ SBG_INLINE uint16_t sbgStreamBufferReadUint16LE(SbgStreamBuffer *pHandle) */ SBG_INLINE int32_t sbgStreamBufferReadInt24LE(SbgStreamBuffer *pHandle) { - Uint8ToInt32 value; + SbgUint8ToInt32 value; assert(pHandle); @@ -251,7 +251,7 @@ SBG_INLINE int32_t sbgStreamBufferReadInt24LE(SbgStreamBuffer *pHandle) */ SBG_INLINE uint32_t sbgStreamBufferReadUint24LE(SbgStreamBuffer *pHandle) { - Uint8ToUint32 value; + SbgUint8ToUint32 value; assert(pHandle); @@ -459,7 +459,7 @@ SBG_INLINE uint32_t sbgStreamBufferReadUint32LE(SbgStreamBuffer *pHandle) */ SBG_INLINE int64_t sbgStreamBufferReadInt40LE(SbgStreamBuffer *pHandle) { - Uint8ToInt64 value; + SbgUint8ToInt64 value; assert(pHandle); @@ -529,7 +529,7 @@ SBG_INLINE int64_t sbgStreamBufferReadInt40LE(SbgStreamBuffer *pHandle) */ SBG_INLINE int64_t sbgStreamBufferReadUint40LE(SbgStreamBuffer *pHandle) { - Uint8ToUint64 value; + SbgUint8ToUint64 value; assert(pHandle); @@ -599,7 +599,7 @@ SBG_INLINE int64_t sbgStreamBufferReadUint40LE(SbgStreamBuffer *pHandle) */ SBG_INLINE int64_t sbgStreamBufferReadInt48LE(SbgStreamBuffer *pHandle) { - Uint8ToInt64 value; + SbgUint8ToInt64 value; assert(pHandle); @@ -671,7 +671,7 @@ SBG_INLINE int64_t sbgStreamBufferReadInt48LE(SbgStreamBuffer *pHandle) */ SBG_INLINE uint64_t sbgStreamBufferReadUint48LE(SbgStreamBuffer *pHandle) { - Uint8ToUint64 value; + SbgUint8ToUint64 value; assert(pHandle); @@ -743,7 +743,7 @@ SBG_INLINE uint64_t sbgStreamBufferReadUint48LE(SbgStreamBuffer *pHandle) */ SBG_INLINE int64_t sbgStreamBufferReadInt56LE(SbgStreamBuffer *pHandle) { - Uint8ToInt64 value; + SbgUint8ToInt64 value; assert(pHandle); @@ -817,7 +817,7 @@ SBG_INLINE int64_t sbgStreamBufferReadInt56LE(SbgStreamBuffer *pHandle) */ SBG_INLINE uint64_t sbgStreamBufferReadUint56LE(SbgStreamBuffer *pHandle) { - Uint8ToUint64 value; + SbgUint8ToUint64 value; assert(pHandle); @@ -1076,7 +1076,7 @@ SBG_INLINE size_t sbgStreamBufferReadSizeT64LE(SbgStreamBuffer *pHandle) */ SBG_INLINE float sbgStreamBufferReadFloatLE(SbgStreamBuffer *pHandle) { - FloatNint floatInt; + SbgFloatNint floatInt; assert(pHandle); @@ -1123,7 +1123,7 @@ SBG_INLINE float sbgStreamBufferReadFloatLE(SbgStreamBuffer *pHandle) */ SBG_INLINE double sbgStreamBufferReadDoubleLE(SbgStreamBuffer *pHandle) { - DoubleNint doubleInt; + SbgDoubleNint doubleInt; assert(pHandle); @@ -1772,7 +1772,7 @@ SBG_INLINE SbgErrorCode sbgStreamBufferWriteSizeT64LE(SbgStreamBuffer *pHandle, */ SBG_INLINE SbgErrorCode sbgStreamBufferWriteFloatLE(SbgStreamBuffer *pHandle, float value) { - FloatNint floatInt; + SbgFloatNint floatInt; assert(pHandle); @@ -1804,7 +1804,7 @@ SBG_INLINE SbgErrorCode sbgStreamBufferWriteFloatLE(SbgStreamBuffer *pHandle, fl */ SBG_INLINE SbgErrorCode sbgStreamBufferWriteDoubleLE(SbgStreamBuffer *pHandle, double value) { - DoubleNint doubleInt; + SbgDoubleNint doubleInt; assert(pHandle); diff --git a/common/string/sbgString.c b/common/string/sbgString.c index b7d36cd..b411b3d 100644 --- a/common/string/sbgString.c +++ b/common/string/sbgString.c @@ -18,17 +18,17 @@ * \param[in] value Integer value, must not be zero. * \return Number of leading zeros. */ -static uint32_t sbgStringCountLeadingZeros(size_t value) +static size_t sbgStringCountLeadingZeros(size_t value) { - uint32_t result; + size_t result; assert(value != 0); #if defined(__GNUC__) || defined(__clang__) result = __builtin_clzl((unsigned long)value); #else - size_t count; - size_t tmp; + size_t count; + size_t tmp; tmp = value; count = 0; @@ -1067,6 +1067,242 @@ SBG_COMMON_LIB_API void sbgStringTruncate(SbgString *pString, size_t length) //- Conversion methods -// //----------------------------------------------------------------------// +SBG_COMMON_LIB_API SbgErrorCode sbgStringFromInt8(SbgString *pString, int8_t value) +{ + assert(pString); + assert(!pString->readOnly); + + if (pString->errorCode == SBG_NO_ERROR) + { + size_t length; + size_t oldCapacity; + int result; + + result = snprintf(pString->pBuffer, pString->capacity, "%" PRId8, value); + + assert(result >= 0); + + length = (size_t)result; + + oldCapacity = pString->capacity; + + pString->errorCode = sbgStringResizeBuffer(pString, length + 1); + + if (pString->errorCode == SBG_NO_ERROR) + { + if (length >= oldCapacity) + { + snprintf(pString->pBuffer, pString->capacity, "%" PRId8, value); + } + + pString->length = (size_t)length; + } + } + + return pString->errorCode; +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringToInt8(const SbgString *pString, int8_t *pValue) +{ + SbgErrorCode errorCode; + int result; + int8_t value; + + assert(pString); + + result = sscanf(pString->pBuffer, "%" SCNd8, &value); + + if (result == 1) + { + *pValue = value; + + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_ERROR; + } + + return errorCode; +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringFromUint8(SbgString *pString, uint8_t value) +{ + assert(pString); + assert(!pString->readOnly); + + if (pString->errorCode == SBG_NO_ERROR) + { + size_t length; + size_t oldCapacity; + int result; + + result = snprintf(pString->pBuffer, pString->capacity, "%" PRIu8, value); + + assert(result >= 0); + + length = (size_t)result; + + oldCapacity = pString->capacity; + + pString->errorCode = sbgStringResizeBuffer(pString, length + 1); + + if (pString->errorCode == SBG_NO_ERROR) + { + if (length >= oldCapacity) + { + snprintf(pString->pBuffer, pString->capacity, "%" PRIu8, value); + } + + pString->length = (size_t)length; + } + } + + return pString->errorCode; +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringToUint8(const SbgString *pString, uint8_t *pValue) +{ + SbgErrorCode errorCode; + int result; + uint8_t value; + + assert(pString); + + result = sscanf(pString->pBuffer, "%" SCNu8, &value); + + if (result == 1) + { + *pValue = value; + + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_ERROR; + } + + return errorCode; +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringFromInt16(SbgString *pString, int16_t value) +{ + assert(pString); + assert(!pString->readOnly); + + if (pString->errorCode == SBG_NO_ERROR) + { + size_t length; + size_t oldCapacity; + int result; + + result = snprintf(pString->pBuffer, pString->capacity, "%" PRId16, value); + + assert(result >= 0); + + length = (size_t)result; + + oldCapacity = pString->capacity; + + pString->errorCode = sbgStringResizeBuffer(pString, length + 1); + + if (pString->errorCode == SBG_NO_ERROR) + { + if (length >= oldCapacity) + { + snprintf(pString->pBuffer, pString->capacity, "%" PRId16, value); + } + + pString->length = (size_t)length; + } + } + + return pString->errorCode; +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringToInt16(const SbgString *pString, int16_t *pValue) +{ + SbgErrorCode errorCode; + int result; + int16_t value; + + assert(pString); + + result = sscanf(pString->pBuffer, "%" SCNd16, &value); + + if (result == 1) + { + *pValue = value; + + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_ERROR; + } + + return errorCode; +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringFromUint16(SbgString *pString, uint16_t value) +{ + assert(pString); + assert(!pString->readOnly); + + if (pString->errorCode == SBG_NO_ERROR) + { + size_t length; + size_t oldCapacity; + int result; + + result = snprintf(pString->pBuffer, pString->capacity, "%" PRIu16, value); + + assert(result >= 0); + + length = (size_t)result; + + oldCapacity = pString->capacity; + + pString->errorCode = sbgStringResizeBuffer(pString, length + 1); + + if (pString->errorCode == SBG_NO_ERROR) + { + if (length >= oldCapacity) + { + snprintf(pString->pBuffer, pString->capacity, "%" PRIu16, value); + } + + pString->length = (size_t)length; + } + } + + return pString->errorCode; +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringToUint16(const SbgString *pString, uint16_t *pValue) +{ + SbgErrorCode errorCode; + int result; + uint16_t value; + + assert(pString); + + result = sscanf(pString->pBuffer, "%" SCNu16, &value); + + if (result == 1) + { + *pValue = value; + + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_ERROR; + } + + return errorCode; +} + SBG_COMMON_LIB_API SbgErrorCode sbgStringFromInt32(SbgString *pString, int32_t value) { assert(pString); @@ -1303,6 +1539,65 @@ SBG_COMMON_LIB_API SbgErrorCode sbgStringToUint64(const SbgString *pString, uint return errorCode; } +SBG_COMMON_LIB_API SbgErrorCode sbgStringFromFloat(SbgString *pString, float value) +{ + assert(pString); + assert(!pString->readOnly); + + if (pString->errorCode == SBG_NO_ERROR) + { + size_t length; + size_t oldCapacity; + int result; + + result = snprintf(pString->pBuffer, pString->capacity, "%f", value); + + assert(result >= 0); + + length = (size_t)result; + + oldCapacity = pString->capacity; + + pString->errorCode = sbgStringResizeBuffer(pString, length + 1); + + if (pString->errorCode == SBG_NO_ERROR) + { + if (length >= oldCapacity) + { + snprintf(pString->pBuffer, pString->capacity, "%f", value); + } + + pString->length = (size_t)length; + } + } + + return pString->errorCode; +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringToFloat(const SbgString *pString, float *pValue) +{ + SbgErrorCode errorCode; + int result; + float value; + + assert(pString); + + result = sscanf(pString->pBuffer, "%f", &value); + + if (result == 1) + { + *pValue = value; + + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_ERROR; + } + + return errorCode; +} + SBG_COMMON_LIB_API SbgErrorCode sbgStringFromDouble(SbgString *pString, double value) { assert(pString); diff --git a/common/string/sbgString.h b/common/string/sbgString.h index 3810ed1..c03bc19 100644 --- a/common/string/sbgString.h +++ b/common/string/sbgString.h @@ -522,6 +522,78 @@ SBG_COMMON_LIB_API void sbgStringTruncate(SbgString *pString, size_t length); //- Conversion methods -// //----------------------------------------------------------------------// +/*! + * 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. + */ +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. + */ +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. + */ +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. + */ +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. + */ +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. + */ +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. + */ +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. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgStringToUint16(const SbgString *pString, uint16_t *pValue); + /*! * Convert and assign a 32-bits signed integer value to a string. * @@ -594,6 +666,24 @@ SBG_COMMON_LIB_API SbgErrorCode sbgStringFromUint64(SbgString *pString, uint64_t */ 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. + */ +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. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgStringToFloat(const SbgString *pString, float *pValue); + /*! * Convert and assign a double value to a string. * diff --git a/common/swap/sbgSwap.h b/common/swap/sbgSwap.h index 25d2430..72026bd 100644 --- a/common/swap/sbgSwap.h +++ b/common/swap/sbgSwap.h @@ -48,6 +48,7 @@ extern "C" { /*! * Swap a uint16_t number. + * * \param[in] x The uint16_t to swap. * \return The swapped value. */ @@ -68,6 +69,7 @@ SBG_INLINE uint32_t sbgSwap32(uint32_t x) /*! * Swap a uint64_t number. + * * \param[in] x The uint64_t to swap. * \return The swapped value. */ @@ -94,12 +96,13 @@ SBG_INLINE uint64_t sbgSwap64(uint64_t x) /*! * Swap a float number. + * * \param[in] val The float to swap. * \return The swapped value. */ SBG_INLINE float sbgSwapFloat(float val) { - FloatNint tmpFloat; + SbgFloatNint tmpFloat; // // We use a union to do the type punning @@ -115,12 +118,13 @@ SBG_INLINE float sbgSwapFloat(float val) /*! * Swap a double number. + * * \param[in] val The double to swap. * \return The swapped value. */ SBG_INLINE double sbgSwapDouble(double val) { - DoubleNint tmpDouble; + SbgDoubleNint tmpDouble; // // We use a union to do the type punning diff --git a/doc/migrations.md b/doc/migrations.md index 09b1050..d978838 100644 --- a/doc/migrations.md +++ b/doc/migrations.md @@ -1,9 +1,77 @@ # Migrations These pages help you easily migrate your code from previous sbgECom major versions. +## From sbgECom v3.x + +sbgECom v4.x is a major improvement over sbgECom v3.x in term of features, code quality and organization. +SBG Systems always tries to minimize breaking changes between sbgECom versions but is also committed to avoid technical debts. + +The macro SBG_ECOM_USE_DEPRECATED_MACROS can be set to maximize backward compatibility and reduce existing code base rework. + +However, SBG Systems recommend to update existing code base and drop usage of legacy / deprecated definitions and methods. + +### Unified code namespace + +sbgECom namespace conventions were not aligned with SBG Systems quality standards and an evolution was needed. + +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. + +Legacy sbgECom v3 style: + +```c +SbgErrorCode sbgEComBinaryLogParseEkfEulerData(SbgStreamBuffer *pInputStream, SbgLogEkfEulerData *pOutputData); +``` + +New sbgECom v4 style: + +```c +SbgErrorCode sbgEComLogEkfEulerReadFromStream(SbgEComLogEkfEuler *pLogData, SbgStreamBuffer *pStreamBuffer); +``` + +### Breaking change for SBG_ECOM_CMD_API_GET + +The SBG_ECOM_CMD_API_GET command introduced in sbgECom v3 and only used for High Performance INS were using the ID **46**. +However, the command SBG_ECOM_CMD_GNSS_1_INSTALLATION used for ELLIPSE was also using ID 46 resulting in a conflict. + +To solve this issue, the command SBG_ECOM_CMD_API_GET ID has been updated to **48** and you must update your code. +This new command ID is used by High Performance INS firmware 5.x and above. + +> SBG_ECOM_CMD_API_GET command ID 46 changed to 48 is a breaking change that requires code update if used. + +### SBG_ECOM_LOG_GPS#_SAT code +In sbgECom v3.x, the list of satellites and signals were dynamically allocated using malloc/free. +This has been replaced by a static allocation as the SbgEComLogSatList struct is smaller than existing SbgEComLogUnion union size. +The new static struct size is 1993 bytes and the current SbgEComLogUnion size is 4086 bytes. + +As a result, the `sbgEComLogSatListConstruct` and `sbgEComLogSatListAdd` methods have been updated. +The `sbgEComLogSatListDestroy` has been removed as no more memory release is needed. + +The `SbgEComLogSatList` struct has been updated: + - `pSatData` has been replaced by `satData` + - `satDataArraySize` has been removed + +The `SbgEComLogSatEntry`struct has been updated: + - `pSignalData` has been replaced by `signalData` + - `signalDataArraySize` has been removed + +Please update your code to reflect these changes. The protocol itself has not changed. + +### Deprecated SBG_ECOM_LOG_IMU_DATA log + +The log SBG_ECOM_LOG_IMU_DATA is now considered to be deprecated and will be removed in future firmware and sbgECom updates. +Please use the log SBG_ECOM_LOG_IMU_SHORT to get high rate, unfiltered and straight IMU data. + +Please also checkout SBG_ECOM_LOG_EKF_ROT_ACCEL_BODY and SBG_ECOM_LOG_EKF_ROT_ACCEL_NED logs to get un-biased, earth rotation free +and gravity free vehicle (body) or navigation measurements. + +These two new logs are for now, only available for High Performance INS such as the EKinox Micro. + ## From sbgECom v1.x The sbgECom version 2.x change the C API even if the low level sbgECom protocol API remains backward compatible. -In otherwords, a C code written with sbgECom version 1.x will not compile directly with sbgECom versions 2.x and higher. +In otherworld, a C code written with sbgECom version 1.x will not compile directly with sbgECom versions 2.x and higher. But your old C code using sbgECom versions 1.x will still be able to correctly setup and configure your ELLIPSE product. ### GNSS module diff --git a/examples/airDataInput/src/airDataInput.c b/examples/airDataInput/src/airDataInput.c index 9c7fdbd..f1f323f 100644 --- a/examples/airDataInput/src/airDataInput.c +++ b/examples/airDataInput/src/airDataInput.c @@ -68,10 +68,10 @@ static float airDataInputRandFloat(float randMin, float randMax) */ static SbgErrorCode airDataInputSendOneLog(SbgEComHandle *pHandle) { - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgLogAirData airDataLog; - uint8_t outputBuffer[64]; - SbgStreamBuffer outputStream; + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComLogAirData airDataLog; + uint8_t outputBuffer[64]; + SbgStreamBuffer outputStream; assert(pHandle); @@ -103,7 +103,7 @@ static SbgErrorCode airDataInputSendOneLog(SbgEComHandle *pHandle) // sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - errorCode = sbgEComBinaryLogWriteAirData(&outputStream, &airDataLog); + errorCode = sbgEComLogAirDataWriteToStream(&airDataLog, &outputStream); if (errorCode == SBG_NO_ERROR) { @@ -193,7 +193,7 @@ static SbgErrorCode airDataInputProcess(SbgInterface *pInterface) } // - // Close the sbgEcom library + // Close the sbgECom library // sbgEComClose(&comHandle); } diff --git a/examples/ellipseMinimal/src/ellipseMinimal.c b/examples/ellipseMinimal/src/ellipseMinimal.c index 99b1f07..6b5f137 100644 --- a/examples/ellipseMinimal/src/ellipseMinimal.c +++ b/examples/ellipseMinimal/src/ellipseMinimal.c @@ -44,16 +44,16 @@ //----------------------------------------------------------------------// /*! - * Callback definition called each time a new log is received. + * 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. */ -SbgErrorCode onLogReceived(SbgEComHandle *pHandle, SbgEComClass msgClass, SbgEComMsgId msg, const SbgBinaryLogData *pLogData, void *pUserArg) +static SbgErrorCode onLogReceived(SbgEComHandle *pHandle, SbgEComClass msgClass, SbgEComMsgId msg, const SbgEComLogUnion *pLogData, void *pUserArg) { assert(pLogData); @@ -97,7 +97,7 @@ static SbgErrorCode getAndPrintProductInfo(SbgEComHandle *pECom) assert(pECom); // - // Get device inforamtions + // Get device information // errorCode = sbgEComCmdGetInfo(pECom, &deviceInfo); @@ -114,7 +114,7 @@ static SbgErrorCode getAndPrintProductInfo(SbgEComHandle *pECom) sbgVersionToStringEncoded(deviceInfo.hardwareRev, hwRevisionStr, sizeof(hwRevisionStr)); sbgVersionToStringEncoded(deviceInfo.firmwareRev, fmwVersionStr, sizeof(fmwVersionStr)); - printf(" Serial Number: %0.9"PRIu32"\n", deviceInfo.serialNumber); + 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); @@ -213,7 +213,7 @@ static SbgErrorCode ellipseMinimalProcess(SbgInterface *pInterface) } // - // Close the sbgEcom library + // Close the sbgECom library // sbgEComClose(&comHandle); } diff --git a/examples/ellipseOnboardMagCalib/src/ellipseOnboardMagCalib.c b/examples/ellipseOnboardMagCalib/src/ellipseOnboardMagCalib.c index 578a2c8..8d9f42b 100644 --- a/examples/ellipseOnboardMagCalib/src/ellipseOnboardMagCalib.c +++ b/examples/ellipseOnboardMagCalib/src/ellipseOnboardMagCalib.c @@ -46,7 +46,7 @@ //----------------------------------------------------------------------// /*! - * Retursn the last char entered by the user just before the user presses the enter key. + * 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. @@ -116,10 +116,10 @@ static SbgEComMagCalibMode askCalibrationMode(void) } /*! - * Display magnetic calibration results on the console. + * 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. + * \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) { @@ -259,7 +259,7 @@ static SbgErrorCode getAndPrintProductInfo(SbgEComHandle *pECom) assert(pECom); // - // Get device inforamtions + // Get device information // errorCode = sbgEComCmdGetInfo(pECom, &deviceInfo); @@ -276,7 +276,7 @@ static SbgErrorCode getAndPrintProductInfo(SbgEComHandle *pECom) sbgVersionToStringEncoded(deviceInfo.hardwareRev, hwRevisionStr, sizeof(hwRevisionStr)); sbgVersionToStringEncoded(deviceInfo.firmwareRev, fmwVersionStr, sizeof(fmwVersionStr)); - printf(" Serial Number: %0.9"PRIu32"\n", deviceInfo.serialNumber); + 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); @@ -301,7 +301,7 @@ static SbgErrorCode getAndPrintProductInfo(SbgEComHandle *pECom) * \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 optionnaly applied). + * \return SBG_NO_ERROR if a valid magnetic calibration has been computed (and optionally applied). */ static SbgErrorCode computeMagneticCalibration(SbgEComHandle *pHandle, SbgEComMagCalibMode mode, bool apply) { @@ -315,9 +315,6 @@ static SbgErrorCode computeMagneticCalibration(SbgEComHandle *pHandle, SbgEComMa // errorCode = sbgEComCmdMagComputeCalib(pHandle, &magCalibResults); - // - // Make sure that we were able to get magnetic calibration results - // if (errorCode == SBG_NO_ERROR) { // @@ -332,44 +329,48 @@ static SbgErrorCode computeMagneticCalibration(SbgEComHandle *pHandle, SbgEComMa // errorCode = sbgEComCmdMagSetCalibData(pHandle, magCalibResults.offset, magCalibResults.matrix); - // - // Make sure that the new magnetic calibration data has been updated - // if (errorCode == SBG_NO_ERROR) { - printf("The new magnetic calibration has been applied.\n"); - // - // Display the magnetic calibration status + // Magnetic calibration applied, save and reboot device to use it // - displayMagCalibResults(mode, &magCalibResults); + 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 { - fprintf(stderr, "ellipseOnboardMagCalib: Unable to upload new magnetic calibration data.\n"); + SBG_LOG_ERROR(errorCode, "Unable to apply new magnetic calibration."); } } else { - printf("A new magnetic calibration solution has been computed.\n"); - // // Display the magnetic calibration status // + printf("A new magnetic calibration solution has been computed but not applied.\n"); displayMagCalibResults(mode, &magCalibResults); } } else { - // - // Unable to compute a valid magnetic calibration - // - fprintf(stderr, "ellipseOnboardMagCalib: Unable to compute a valid magnetic calibration.\n"); + SBG_LOG_ERROR(errorCode, "Computed magnetic calibration is invalid. Please retry."); } } else { - fprintf(stderr, "ellipseOnboardMagCalib: Unable to get onboard magnetic calibration results.\n"); + SBG_LOG_ERROR(errorCode, "Unable to compute magnetic calibration."); } return errorCode; @@ -400,7 +401,7 @@ static SbgErrorCode ellipseOnBoardMagCalibProcess(SbgInterface *pInterface) // if (errorCode == SBG_NO_ERROR) { - printf("Welcome to the ELLIPSE onboard magnetic calibration example.\n"); + printf("Welcome to the ELLIPSE on-board magnetic calibration example.\n"); printf("sbgECom version %s\n\n", SBG_E_COM_VERSION_STR); // @@ -447,9 +448,9 @@ static SbgErrorCode ellipseOnBoardMagCalibProcess(SbgInterface *pInterface) printf( "Please rotate the device slowly...\n" "\n" - "You can compute as many magnetic calibration as you want wihtout loosing already acquiered points:\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 quit\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 : "); @@ -481,11 +482,11 @@ static SbgErrorCode ellipseOnBoardMagCalibProcess(SbgInterface *pInterface) } else { - SBG_LOG_ERROR(errorCode, "Unable to start the onboard magnetic calibration"); + SBG_LOG_ERROR(errorCode, "Unable to start the on-board magnetic calibration"); } // - // Close the sbgEcom library + // Close the sbgECom library // sbgEComClose(&comHandle); } @@ -541,13 +542,13 @@ int main(int argc, char **argv) } else { - SBG_LOG_ERROR(errorCode, "unable to open serial interface"); + SBG_LOG_ERROR(errorCode, "Unable to open serial interface"); exitCode = EXIT_FAILURE; } } else { - printf("Invalid input arguments, usage: pulseMinimal SERIAL_DEVICE SERIAL_BAUDRATE\n"); + SBG_LOG_ERROR(SBG_ERROR, "Invalid input arguments, usage: pulseMinimal SERIAL_DEVICE SERIAL_BAUDRATE"); exitCode = EXIT_FAILURE; } diff --git a/examples/hpInsMinimal/src/hpInsMinimal.c b/examples/hpInsMinimal/src/hpInsMinimal.c index 3a52b44..f7ef1cf 100644 --- a/examples/hpInsMinimal/src/hpInsMinimal.c +++ b/examples/hpInsMinimal/src/hpInsMinimal.c @@ -6,7 +6,7 @@ * \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. + * 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. @@ -55,7 +55,7 @@ * \param[in] pUserArg Optional user supplied argument. * \return SBG_NO_ERROR if the received log has been used successfully. */ -SbgErrorCode onLogReceived(SbgEComHandle *pHandle, SbgEComClass msgClass, SbgEComMsgId msg, const SbgBinaryLogData *pLogData, void *pUserArg) +static SbgErrorCode onLogReceived(SbgEComHandle *pHandle, SbgEComClass msgClass, SbgEComMsgId msg, const SbgEComLogUnion *pLogData, void *pUserArg) { SBG_UNUSED_PARAMETER(pHandle); SBG_UNUSED_PARAMETER(pUserArg); @@ -147,7 +147,7 @@ static SbgErrorCode hpInsMinimalProcess(SbgInterface *pInterface) } // - // Close the sbgEcom library + // Close the sbgECom library // sbgEComClose(&comHandle); } diff --git a/examples/pulseMinimal/src/pulseMinimal.c b/examples/pulseMinimal/src/pulseMinimal.c index 06ca21a..9afb009 100644 --- a/examples/pulseMinimal/src/pulseMinimal.c +++ b/examples/pulseMinimal/src/pulseMinimal.c @@ -58,7 +58,7 @@ * \param[in] pUserArg Optional user argument. * \return SBG_NO_ERROR if successful. */ -static SbgErrorCode pulseMinimalOnLogReceived(SbgEComHandle *pECom, SbgEComClass msgClass, SbgEComMsgId msg, const SbgBinaryLogData *pLogData, void *pUserArg) +static SbgErrorCode pulseMinimalOnLogReceived(SbgEComHandle *pECom, SbgEComClass msgClass, SbgEComMsgId msg, const SbgEComLogUnion *pLogData, void *pUserArg) { SBG_UNUSED_PARAMETER(pECom); SBG_UNUSED_PARAMETER(pUserArg); @@ -142,8 +142,8 @@ static SbgErrorCode pulseMinimalGetAndPrintProductInfo(SbgEComHandle *pECom) int ret; // - // This is a naive and simplistic way to parse a json content. - // It is recommanded to use a true json parser. + // 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, "{" \ @@ -168,7 +168,7 @@ static SbgErrorCode pulseMinimalGetAndPrintProductInfo(SbgEComHandle *pECom) else { errorCode = SBG_INVALID_PARAMETER; - SBG_LOG_ERROR(errorCode, "Received JSON is mal formatted"); + SBG_LOG_ERROR(errorCode, "Received JSON is malformed"); } } else @@ -185,7 +185,7 @@ static SbgErrorCode pulseMinimalGetAndPrintProductInfo(SbgEComHandle *pECom) * 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 environements. + * 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 @@ -218,8 +218,8 @@ static SbgErrorCode pulseMinimalJsonParseError(const char* pContent, uint32_t *p if (ret > 0) { // - // This is a naive and simplistic way to parse a json content. - // It is recommanded to use a true json parser. + // 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); @@ -227,7 +227,7 @@ static SbgErrorCode pulseMinimalJsonParseError(const char* pContent, uint32_t *p if (ret != 3) { errorCode = SBG_INVALID_PARAMETER; - SBG_LOG_ERROR(errorCode, "JSON payload mal formatted"); + SBG_LOG_ERROR(errorCode, "JSON payload malformed"); } } else @@ -258,8 +258,8 @@ static SbgErrorCode pulseMinimalChangeImuShortOutputRate(SbgEComHandle *pECom, u sbgEComCmdApiReplyConstruct(&reply); // - // This is a naive and simplistic way to build a json string. - // It is recommanded to use a true json generator. + // 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); @@ -275,7 +275,7 @@ static SbgErrorCode pulseMinimalChangeImuShortOutputRate(SbgEComHandle *pECom, u // if (reply.statusCode == 200) { - SBG_LOG_INFO("IMU Short output periode successfully configured"); + SBG_LOG_INFO("IMU Short output period successfully configured"); } else { @@ -284,8 +284,8 @@ static SbgErrorCode pulseMinimalChangeImuShortOutputRate(SbgEComHandle *pECom, u char details[128]; // - // Parse an error payload using a naive and simplistic approch - // You should rather use a true json parser such as cJson library + // 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)); diff --git a/src/binaryLogs/sbgEComBinaryLogAirData.c b/src/binaryLogs/sbgEComBinaryLogAirData.c deleted file mode 100644 index 25d3543..0000000 --- a/src/binaryLogs/sbgEComBinaryLogAirData.c +++ /dev/null @@ -1,66 +0,0 @@ -#include "sbgEComBinaryLogAirData.h" - -//----------------------------------------------------------------------// -//- Operations -// -//----------------------------------------------------------------------// - -SbgErrorCode sbgEComBinaryLogParseAirData(SbgStreamBuffer *pInputStream, SbgLogAirData *pOutputData) -{ - assert(pInputStream); - assert(pOutputData); - - // - // Read the frame payload - // - pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->status = sbgStreamBufferReadUint16LE(pInputStream); - - pOutputData->pressureAbs = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->altitude = sbgStreamBufferReadFloatLE(pInputStream); - - // - // The true airspeed fields have been added in version 2.0 - // - if (sbgStreamBufferGetSpace(pInputStream) > 0) - { - pOutputData->pressureDiff = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->trueAirspeed = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->airTemperature = sbgStreamBufferReadFloatLE(pInputStream); - } - else - { - pOutputData->pressureDiff = 0.0f; - pOutputData->trueAirspeed = 0.0f; - pOutputData->airTemperature = 0.0f; - } - - // - // Return if any error has occurred while parsing the frame - // - return sbgStreamBufferGetLastError(pInputStream); -} - -SbgErrorCode sbgEComBinaryLogWriteAirData(SbgStreamBuffer *pOutputStream, const SbgLogAirData *pInputData) -{ - assert(pOutputStream); - assert(pInputData); - - // - // Write the frame payload - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->status); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->pressureAbs); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->altitude); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->pressureDiff); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->trueAirspeed); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->airTemperature); - - // - // Return if any error has occurred while writing the frame - // - return sbgStreamBufferGetLastError(pOutputStream); -} diff --git a/src/binaryLogs/sbgEComBinaryLogDepth.c b/src/binaryLogs/sbgEComBinaryLogDepth.c deleted file mode 100644 index d87d268..0000000 --- a/src/binaryLogs/sbgEComBinaryLogDepth.c +++ /dev/null @@ -1,45 +0,0 @@ -#include "sbgEComBinaryLogDepth.h" - -//----------------------------------------------------------------------// -//- Public methods -// -//----------------------------------------------------------------------// - -SbgErrorCode sbgEComBinaryLogParseDepth(SbgStreamBuffer *pInputStream, SbgLogDepth *pOutputData) -{ - assert(pInputStream); - assert(pOutputData); - - // - // Read the frame payload - // - pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->status = sbgStreamBufferReadUint16LE(pInputStream); - - pOutputData->pressureAbs = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->altitude = sbgStreamBufferReadFloatLE(pInputStream); - - // - // Return if any error has occurred while parsing the frame - // - return sbgStreamBufferGetLastError(pInputStream); -} - -SbgErrorCode sbgEComBinaryLogWriteDepth(SbgStreamBuffer *pOutputStream, const SbgLogDepth *pInputData) -{ - assert(pOutputStream); - assert(pInputData); - - // - // Write the frame payload - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->status); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->pressureAbs); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->altitude); - - // - // Return if any error has occurred while writing the frame - // - return sbgStreamBufferGetLastError(pOutputStream); -} diff --git a/src/binaryLogs/sbgEComBinaryLogDiag.c b/src/binaryLogs/sbgEComBinaryLogDiag.c deleted file mode 100644 index dfcf0c3..0000000 --- a/src/binaryLogs/sbgEComBinaryLogDiag.c +++ /dev/null @@ -1,49 +0,0 @@ -// sbgCommonLib headers -#include -#include - -// Local headers -#include "sbgEComBinaryLogDiag.h" - -//----------------------------------------------------------------------// -//- Public methods -// -//----------------------------------------------------------------------// - -SbgErrorCode sbgEComBinaryLogParseDiagData(SbgStreamBuffer *pInputStream, SbgLogDiagData *pOutputData) -{ - assert(pInputStream); - assert(pOutputData); - - pOutputData->timestamp = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->type = (SbgDebugLogType)sbgStreamBufferReadUint8(pInputStream); - pOutputData->errorCode = (SbgErrorCode)sbgStreamBufferReadUint8(pInputStream); - - sbgStreamBufferReadBuffer(pInputStream, pOutputData->string, sbgStreamBufferGetSpace(pInputStream)); - pOutputData->string[sizeof(pOutputData->string) - 1] = '\0'; - - return sbgStreamBufferGetLastError(pInputStream); -} - -SbgErrorCode sbgEComBinaryLogWriteDiagData(SbgStreamBuffer *pOutputStream, const SbgLogDiagData *pInputData) -{ - size_t length; - - assert(pOutputStream); - assert(pInputData); - - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timestamp); - sbgStreamBufferWriteUint8(pOutputStream, pInputData->type); - sbgStreamBufferWriteUint8(pOutputStream, pInputData->errorCode); - - length = strlen(pInputData->string); - - if (length >= sizeof(pInputData->string)) - { - length = sizeof(pInputData->string) - 1; - } - - sbgStreamBufferWriteBuffer(pOutputStream, pInputData->string, length); - sbgStreamBufferWriteUint8(pOutputStream, 0); - - return sbgStreamBufferGetLastError(pOutputStream); -} diff --git a/src/binaryLogs/sbgEComBinaryLogDvl.c b/src/binaryLogs/sbgEComBinaryLogDvl.c deleted file mode 100644 index 0bdb392..0000000 --- a/src/binaryLogs/sbgEComBinaryLogDvl.c +++ /dev/null @@ -1,55 +0,0 @@ -#include "sbgEComBinaryLogDvl.h" - -//----------------------------------------------------------------------// -//- Public methods -// -//----------------------------------------------------------------------// - -SbgErrorCode sbgEComBinaryLogParseDvlData(SbgStreamBuffer *pInputStream, SbgLogDvlData *pOutputData) -{ - assert(pInputStream); - assert(pOutputData); - - // - // Read the frame payload - // - pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->status = sbgStreamBufferReadUint16LE(pInputStream); - - pOutputData->velocity[0] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->velocity[1] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->velocity[2] = sbgStreamBufferReadFloatLE(pInputStream); - - pOutputData->velocityQuality[0] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->velocityQuality[1] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->velocityQuality[2] = sbgStreamBufferReadFloatLE(pInputStream); - - // - // Return if any error has occurred while parsing the frame - // - return sbgStreamBufferGetLastError(pInputStream); -} - -SbgErrorCode sbgEComBinaryLogWriteDvlData(SbgStreamBuffer *pOutputStream, const SbgLogDvlData *pInputData) -{ - assert(pOutputStream); - assert(pInputData); - - // - // Write the frame payload - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->status); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocity[0]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocity[1]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocity[2]); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocityQuality[0]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocityQuality[1]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocityQuality[2]); - - // - // Return if any error has occurred while writing the frame - // - return sbgStreamBufferGetLastError(pOutputStream); -} diff --git a/src/binaryLogs/sbgEComBinaryLogEkf.c b/src/binaryLogs/sbgEComBinaryLogEkf.c deleted file mode 100644 index e95b9dd..0000000 --- a/src/binaryLogs/sbgEComBinaryLogEkf.c +++ /dev/null @@ -1,183 +0,0 @@ -#include "sbgEComBinaryLogEkf.h" - -//----------------------------------------------------------------------// -//- Public methods -// -//----------------------------------------------------------------------// - -SbgErrorCode sbgEComBinaryLogParseEkfEulerData(SbgStreamBuffer *pInputStream, SbgLogEkfEulerData *pOutputData) -{ - assert(pInputStream); - assert(pOutputData); - - // - // Read the frame payload - // - pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); - - pOutputData->euler[0] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->euler[1] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->euler[2] = sbgStreamBufferReadFloatLE(pInputStream); - - pOutputData->eulerStdDev[0] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->eulerStdDev[1] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->eulerStdDev[2] = sbgStreamBufferReadFloatLE(pInputStream); - - pOutputData->status = sbgStreamBufferReadUint32LE(pInputStream); - - // - // Return if any error has occurred while parsing the frame - // - return sbgStreamBufferGetLastError(pInputStream); -} - -SbgErrorCode sbgEComBinaryLogWriteEkfEulerData(SbgStreamBuffer *pOutputStream, const SbgLogEkfEulerData *pInputData) -{ - assert(pOutputStream); - assert(pInputData); - - // - // Write the frame payload - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->euler[0]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->euler[1]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->euler[2]); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->eulerStdDev[0]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->eulerStdDev[1]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->eulerStdDev[2]); - - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->status); - - // - // Return if any error has occurred while writing the frame - // - return sbgStreamBufferGetLastError(pOutputStream); -} - -SbgErrorCode sbgEComBinaryLogParseEkfQuatData(SbgStreamBuffer *pInputStream, SbgLogEkfQuatData *pOutputData) -{ - assert(pInputStream); - assert(pOutputData); - - // - // Read the frame payload - // - pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); - - pOutputData->quaternion[0] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->quaternion[1] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->quaternion[2] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->quaternion[3] = sbgStreamBufferReadFloatLE(pInputStream); - - pOutputData->eulerStdDev[0] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->eulerStdDev[1] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->eulerStdDev[2] = sbgStreamBufferReadFloatLE(pInputStream); - - pOutputData->status = sbgStreamBufferReadUint32LE(pInputStream); - - // - // Return if any error has occurred while parsing the frame - // - return sbgStreamBufferGetLastError(pInputStream); -} - -SbgErrorCode sbgEComBinaryLogWriteEkfQuatData(SbgStreamBuffer *pOutputStream, const SbgLogEkfQuatData *pInputData) -{ - assert(pOutputStream); - assert(pInputData); - - // - // Write the frame payload - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->quaternion[0]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->quaternion[1]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->quaternion[2]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->quaternion[3]); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->eulerStdDev[0]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->eulerStdDev[1]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->eulerStdDev[2]); - - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->status); - - // - // Return if any error has occurred while writing the frame - // - return sbgStreamBufferGetLastError(pOutputStream); -} - -SbgErrorCode sbgEComBinaryLogParseEkfNavData(SbgStreamBuffer *pInputStream, SbgLogEkfNavData *pOutputData) -{ - assert(pInputStream); - assert(pOutputData); - - // - // Read the frame payload - // - pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); - - pOutputData->velocity[0] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->velocity[1] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->velocity[2] = sbgStreamBufferReadFloatLE(pInputStream); - - pOutputData->velocityStdDev[0] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->velocityStdDev[1] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->velocityStdDev[2] = sbgStreamBufferReadFloatLE(pInputStream); - - pOutputData->position[0] = sbgStreamBufferReadDoubleLE(pInputStream); - pOutputData->position[1] = sbgStreamBufferReadDoubleLE(pInputStream); - pOutputData->position[2] = sbgStreamBufferReadDoubleLE(pInputStream); - - pOutputData->undulation = sbgStreamBufferReadFloatLE(pInputStream); - - pOutputData->positionStdDev[0] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->positionStdDev[1] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->positionStdDev[2] = sbgStreamBufferReadFloatLE(pInputStream); - - pOutputData->status = sbgStreamBufferReadUint32LE(pInputStream); - - // - // Return if any error has occurred while parsing the frame - // - return sbgStreamBufferGetLastError(pInputStream); -} - -SbgErrorCode sbgEComBinaryLogWriteEkfNavData(SbgStreamBuffer *pOutputStream, const SbgLogEkfNavData *pInputData) -{ - assert(pOutputStream); - assert(pInputData); - - // - // Write the frame payload - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocity[0]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocity[1]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocity[2]); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocityStdDev[0]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocityStdDev[1]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocityStdDev[2]); - - sbgStreamBufferWriteDoubleLE(pOutputStream, pInputData->position[0]); - sbgStreamBufferWriteDoubleLE(pOutputStream, pInputData->position[1]); - sbgStreamBufferWriteDoubleLE(pOutputStream, pInputData->position[2]); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->undulation); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->positionStdDev[0]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->positionStdDev[1]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->positionStdDev[2]); - - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->status); - - // - // Return if any error has occurred while writing the frame - // - return sbgStreamBufferGetLastError(pOutputStream); -} diff --git a/src/binaryLogs/sbgEComBinaryLogEvent.c b/src/binaryLogs/sbgEComBinaryLogEvent.c deleted file mode 100644 index 82ecaff..0000000 --- a/src/binaryLogs/sbgEComBinaryLogEvent.c +++ /dev/null @@ -1,47 +0,0 @@ -#include "sbgEComBinaryLogEvent.h" - -//----------------------------------------------------------------------// -//- Public methods -// -//----------------------------------------------------------------------// - -SbgErrorCode sbgEComBinaryLogParseEvent(SbgStreamBuffer *pInputStream, SbgLogEvent *pOutputData) -{ - assert(pInputStream); - assert(pOutputData); - - // - // Read the frame payload - // - pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->status = sbgStreamBufferReadUint16LE(pInputStream); - pOutputData->timeOffset0 = sbgStreamBufferReadUint16LE(pInputStream); - pOutputData->timeOffset1 = sbgStreamBufferReadUint16LE(pInputStream); - pOutputData->timeOffset2 = sbgStreamBufferReadUint16LE(pInputStream); - pOutputData->timeOffset3 = sbgStreamBufferReadUint16LE(pInputStream); - - // - // Return if any error has occurred while parsing the frame - // - return sbgStreamBufferGetLastError(pInputStream); -} - -SbgErrorCode sbgEComBinaryLogWriteEvent(SbgStreamBuffer *pOutputStream, const SbgLogEvent *pInputData) -{ - assert(pOutputStream); - assert(pInputData); - - // - // Write the frame payload - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->status); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->timeOffset0); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->timeOffset1); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->timeOffset2); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->timeOffset3); - - // - // Return if any error has occurred while writing the frame - // - return sbgStreamBufferGetLastError(pOutputStream); -} diff --git a/src/binaryLogs/sbgEComBinaryLogGps.c b/src/binaryLogs/sbgEComBinaryLogGps.c deleted file mode 100644 index d58e3a6..0000000 --- a/src/binaryLogs/sbgEComBinaryLogGps.c +++ /dev/null @@ -1,206 +0,0 @@ -#include "sbgEComBinaryLogGps.h" - -//----------------------------------------------------------------------// -//- Public methods -// -//----------------------------------------------------------------------// - -SbgErrorCode sbgEComBinaryLogParseGpsVelData(SbgStreamBuffer *pInputStream, SbgLogGpsVel *pOutputData) -{ - assert(pInputStream); - assert(pOutputData); - - // - // Read the frame payload - // - pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->status = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->timeOfWeek = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->velocity[0] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->velocity[1] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->velocity[2] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->velocityAcc[0] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->velocityAcc[1] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->velocityAcc[2] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->course = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->courseAcc = sbgStreamBufferReadFloatLE(pInputStream); - - // - // Return if any error has occurred while parsing the frame - // - return sbgStreamBufferGetLastError(pInputStream); -} - -SbgErrorCode sbgEComBinaryLogWriteGpsVelData(SbgStreamBuffer *pOutputStream, const SbgLogGpsVel *pInputData) -{ - assert(pOutputStream); - assert(pInputData); - - // - // Write the frame payload - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->status); - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeOfWeek); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocity[0]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocity[1]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocity[2]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocityAcc[0]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocityAcc[1]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocityAcc[2]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->course); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->courseAcc); - - // - // Return if any error has occurred while writing the frame - // - return sbgStreamBufferGetLastError(pOutputStream); -} - -SbgErrorCode sbgEComBinaryLogParseGpsPosData(SbgStreamBuffer *pInputStream, SbgLogGpsPos *pOutputData) -{ - assert(pInputStream); - assert(pOutputData); - - // - // Read the frame payload - // - pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->status = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->timeOfWeek = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->latitude = sbgStreamBufferReadDoubleLE(pInputStream); - pOutputData->longitude = sbgStreamBufferReadDoubleLE(pInputStream); - pOutputData->altitude = sbgStreamBufferReadDoubleLE(pInputStream); - pOutputData->undulation = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->latitudeAccuracy = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->longitudeAccuracy = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->altitudeAccuracy = sbgStreamBufferReadFloatLE(pInputStream); - - // - // Test if we have a additional information such as base station id (since version 1.4) - // - if (sbgStreamBufferGetSpace(pInputStream) >= 5) - { - // - // Read the additional information - // - pOutputData->numSvUsed = sbgStreamBufferReadUint8LE(pInputStream); - pOutputData->baseStationId = sbgStreamBufferReadUint16LE(pInputStream); - pOutputData->differentialAge = sbgStreamBufferReadUint16LE(pInputStream); - } - else - { - // - // Default the additional information - // - pOutputData->numSvUsed = 0; - pOutputData->baseStationId = 0xFFFF; - pOutputData->differentialAge = 0xFFFF; - } - - // - // Return if any error has occurred while parsing the frame - // - return sbgStreamBufferGetLastError(pInputStream); -} - -SbgErrorCode sbgEComBinaryLogWriteGpsPosData(SbgStreamBuffer *pOutputStream, const SbgLogGpsPos *pInputData) -{ - assert(pOutputStream); - assert(pInputData); - - // - // Write the frame payload - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->status); - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeOfWeek); - - sbgStreamBufferWriteDoubleLE(pOutputStream, pInputData->latitude); - sbgStreamBufferWriteDoubleLE(pOutputStream, pInputData->longitude); - sbgStreamBufferWriteDoubleLE(pOutputStream, pInputData->altitude); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->undulation); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->latitudeAccuracy); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->longitudeAccuracy); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->altitudeAccuracy); - - // - // Write the additional information added in version 1.4 - // - sbgStreamBufferWriteUint8LE(pOutputStream, pInputData->numSvUsed); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->baseStationId); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->differentialAge); - - // - // Return if any error has occurred while writing the frame - // - return sbgStreamBufferGetLastError(pOutputStream); -} - -SbgErrorCode sbgEComBinaryLogParseGpsHdtData(SbgStreamBuffer *pInputStream, SbgLogGpsHdt *pOutputData) -{ - assert(pInputStream); - assert(pOutputData); - - // - // Read the frame payload - // - pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->status = sbgStreamBufferReadUint16LE(pInputStream); - pOutputData->timeOfWeek = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->heading = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->headingAccuracy = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->pitch = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->pitchAccuracy = sbgStreamBufferReadFloatLE(pInputStream); - - // - // The baseline field have been added in version 2.0 - // - if (sbgStreamBufferGetSpace(pInputStream) > 0) - { - pOutputData->baseline = sbgStreamBufferReadFloatLE(pInputStream); - } - else - { - pOutputData->baseline = 0.0f; - } - - // - // Return if any error has occurred while parsing the frame - // - return sbgStreamBufferGetLastError(pInputStream); -} - -SbgErrorCode sbgEComBinaryLogWriteGpsHdtData(SbgStreamBuffer *pOutputStream, const SbgLogGpsHdt *pInputData) -{ - assert(pOutputStream); - assert(pInputData); - - // - // Write the frame payload - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->status); - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeOfWeek); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->heading); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->headingAccuracy); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->pitch); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->pitchAccuracy); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->baseline); - - // - // Return if any error has occurred while writing the frame - // - return sbgStreamBufferGetLastError(pOutputStream); -} - -SbgErrorCode sbgEComBinaryLogParseGpsRawData(SbgStreamBuffer *pInputStream, SbgLogRawData *pOutputData) -{ - return sbgEComBinaryLogParseRawData(pInputStream, pOutputData); -} - -SbgErrorCode sbgEComBinaryLogWriteGpsRawData(SbgStreamBuffer *pOutputStream, const SbgLogRawData *pInputData) -{ - return sbgEComBinaryLogWriteRawData(pOutputStream, pInputData); -} diff --git a/src/binaryLogs/sbgEComBinaryLogGps.h b/src/binaryLogs/sbgEComBinaryLogGps.h deleted file mode 100644 index 1db301d..0000000 --- a/src/binaryLogs/sbgEComBinaryLogGps.h +++ /dev/null @@ -1,433 +0,0 @@ -/*! - * \file sbgEComBinaryLogGps.h - * \ingroup binaryLogs - * \author SBG Systems - * \date 20 February 2013 - * - * \brief Parse received GNSS logs such as Position, Velocity and True Heading. - * - * \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 - */ - -#ifndef SBG_ECOM_BINARY_LOG_GPS_H -#define SBG_ECOM_BINARY_LOG_GPS_H - -// sbgCommonLib headers -#include -#include - -// Local headers -#include "sbgEComBinaryLogRawData.h" - -#ifdef __cplusplus -extern "C" { -#endif - -//----------------------------------------------------------------------// -//- Log GPS velocity const definitions -// -//----------------------------------------------------------------------// - -/*! - * Log GPS velocity status and type definitions. - */ -#define SBG_ECOM_GPS_VEL_STATUS_SHIFT (0u) /*!< Shift used to extract the GPS velocity status part. */ -#define SBG_ECOM_GPS_VEL_STATUS_MASK (0x0000003Fu) /*!< Mask used to keep only the GPS velocity status part. */ -#define SBG_ECOM_GPS_VEL_TYPE_SHIFT (6u) /*!< Shift used to extract the GPS velocity type part. */ -#define SBG_ECOM_GPS_VEL_TYPE_MASK (0x0000003Fu) /*!< Mask used to keep only the GPS velocity type part. */ - -//----------------------------------------------------------------------// -//- Log GPS position const definitions -// -//----------------------------------------------------------------------// - -/*! - * GPS position status and type definitions. - */ -#define SBG_ECOM_GPS_POS_STATUS_SHIFT (0u) /*!< Shift used to extract the GPS position status part. */ -#define SBG_ECOM_GPS_POS_STATUS_MASK (0x0000003Fu) /*!< Mask used to keep only the GPS position status part. */ -#define SBG_ECOM_GPS_POS_TYPE_SHIFT (6u) /*!< Shift used to extract the GPS position type part. */ -#define SBG_ECOM_GPS_POS_TYPE_MASK (0x0000003Fu) /*!< Mask used to keep only the GPS position type part. */ - -/*! - * GNSS signals definitions - */ -#define SBG_ECOM_GPS_POS_GPS_L1_USED (0x00000001u << 12) /*!< Set to 1 if GPS L1CA/L1P is used in solution. */ -#define SBG_ECOM_GPS_POS_GPS_L2_USED (0x00000001u << 13) /*!< Set to 1 if GPS L2P/L2C is used in solution. */ -#define SBG_ECOM_GPS_POS_GPS_L5_USED (0x00000001u << 14) /*!< Set to 1 if GPS L5 is used in solution. */ - -#define SBG_ECOM_GPS_POS_GLO_L1_USED (0x00000001u << 15) /*!< Set to 1 if GLONASS L1CA is used in solution. */ -#define SBG_ECOM_GPS_POS_GLO_L2_USED (0x00000001u << 16) /*!< Set to 1 if GLONASS L2C/L2P is used in solution. */ -#define SBG_ECOM_GPS_POS_GLO_L3_USED (0x00000001u << 17) /*!< Set to 1 if GLONASS L3 is used in solution. */ - -#define SBG_ECOM_GPS_POS_GAL_E1_USED (0x00000001u << 18) /*!< Set to 1 if Galileo E1 is used in solution. */ -#define SBG_ECOM_GPS_POS_GAL_E5A_USED (0x00000001u << 19) /*!< Set to 1 if Galileo E5a is used in solution. */ -#define SBG_ECOM_GPS_POS_GAL_E5B_USED (0x00000001u << 20) /*!< Set to 1 if Galileo E5b is used in solution. */ -#define SBG_ECOM_GPS_POS_GAL_E5ALT_USED (0x00000001u << 21) /*!< Set to 1 if Galileo E5 AltBoc is used in solution. */ -#define SBG_ECOM_GPS_POS_GAL_E6_USED (0x00000001u << 22) /*!< Set to 1 if Galileo E6 is used in solution. */ - -#define SBG_ECOM_GPS_POS_BDS_B1_USED (0x00000001u << 23) /*!< Set to 1 if BeiDou B1 is used in solution. */ -#define SBG_ECOM_GPS_POS_BDS_B2_USED (0x00000001u << 24) /*!< Set to 1 if BeiDou B2 is used in solution. */ -#define SBG_ECOM_GPS_POS_BDS_B3_USED (0x00000001u << 25) /*!< Set to 1 if BeiDou B3 is used in solution. */ - -#define SBG_ECOM_GPS_POS_QZSS_L1_USED (0x00000001u << 26) /*!< Set to 1 if QZSS L1CA is used in solution. */ -#define SBG_ECOM_GPS_POS_QZSS_L2_USED (0x00000001u << 27) /*!< Set to 1 if QZSS L2C is used in solution. */ -#define SBG_ECOM_GPS_POS_QZSS_L5_USED (0x00000001u << 28) /*!< Set to 1 if QZSS L5 is used in solution. */ - -//----------------------------------------------------------------------// -//- Log GPS HDT const definitions -// -//----------------------------------------------------------------------// - -/*! - * GPS HDT status definitions. - */ -#define SBG_ECOM_GPS_HDT_STATUS_SHIFT (0u) /*!< Shift used to extract the GPS HDT status part. */ -#define SBG_ECOM_GPS_HDT_STATUS_MASK (0x0000003Fu) /*!< Mask used to keep only the GPS HDT status part. */ - -/*! - * GPS HDT status bitmasks - */ -#define SBG_ECOM_GPS_HDT_BASELINE_VALID (0x0001 << 6) /*!< Set to 1 if the baseline length field is filled and valid. */ - -//----------------------------------------------------------------------// -//- Log GPS velocity enums definitions -// -//----------------------------------------------------------------------// - -/*! - * GPS velocity status definitions. - */ -typedef enum _SbgEComGpsVelStatus -{ - SBG_ECOM_VEL_SOL_COMPUTED = 0, /*!< A valid solution has been computed. */ - SBG_ECOM_VEL_INSUFFICIENT_OBS = 1, /*!< Not enough valid SV to compute a solution. */ - SBG_ECOM_VEL_INTERNAL_ERROR = 2, /*!< An internal error has occurred. */ - SBG_ECOM_VEL_LIMIT = 3 /*!< Velocity limit exceeded. */ -} SbgEComGpsVelStatus; - -/*! - * GPS velocity types definitions. - */ -typedef enum _SbgEComGpsVelType -{ - SBG_ECOM_VEL_NO_SOLUTION = 0, /*!< No valid velocity solution available. */ - SBG_ECOM_VEL_UNKNOWN_TYPE = 1, /*!< An unknown solution type has been computed. */ - SBG_ECOM_VEL_DOPPLER = 2, /*!< A Doppler velocity has been computed. */ - SBG_ECOM_VEL_DIFFERENTIAL = 3 /*!< A differential velocity has been computed between two positions. */ -} SbgEComGpsVelType; - -//----------------------------------------------------------------------// -//- Log GPS position enums definitions -// -//----------------------------------------------------------------------// - -/*! - * GPS position status definitions. - */ -typedef enum _SbgEComGpsPosStatus -{ - SBG_ECOM_POS_SOL_COMPUTED = 0, /*!< A valid solution has been computed. */ - SBG_ECOM_POS_INSUFFICIENT_OBS = 1, /*!< Not enough valid SV to compute a solution. */ - SBG_ECOM_POS_INTERNAL_ERROR = 2, /*!< An internal error has occurred. */ - SBG_ECOM_POS_HEIGHT_LIMIT = 3 /*!< The height limit has been exceeded. */ -} SbgEComGpsPosStatus; - -/*! - * GPS position types definitions. - */ -typedef enum _SbgEComGpsPosType -{ - SBG_ECOM_POS_NO_SOLUTION = 0, /*!< No valid solution available. */ - SBG_ECOM_POS_UNKNOWN_TYPE = 1, /*!< An unknown solution type has been computed. */ - SBG_ECOM_POS_SINGLE = 2, /*!< Single point solution position. */ - SBG_ECOM_POS_PSRDIFF = 3, /*!< Standard Pseudorange Differential Solution (DGPS). */ - SBG_ECOM_POS_SBAS = 4, /*!< SBAS satellite used for differential corrections. */ - SBG_ECOM_POS_OMNISTAR = 5, /*!< Omnistar VBS Position (L1 sub-meter). */ - SBG_ECOM_POS_RTK_FLOAT = 6, /*!< Floating RTK ambiguity solution (20 cms RTK). */ - SBG_ECOM_POS_RTK_INT = 7, /*!< Integer RTK ambiguity solution (2 cms RTK). */ - SBG_ECOM_POS_PPP_FLOAT = 8, /*!< Precise Point Positioning with float ambiguities. */ - SBG_ECOM_POS_PPP_INT = 9, /*!< Precise Point Positioning with fixed ambiguities. */ - SBG_ECOM_POS_FIXED = 10 /*!< Fixed location solution position. */ -} SbgEComGpsPosType; - -//----------------------------------------------------------------------// -//- Log GPS HDT enums definitions -// -//----------------------------------------------------------------------// - -/*! - * GPS HDT status definitions. - */ -typedef enum _SbgEComGpsHdtStatus -{ - SBG_ECOM_HDT_SOL_COMPUTED = 0, /*!< A valid solution has been computed. */ - SBG_ECOM_HDT_INSUFFICIENT_OBS = 1, /*!< Not enough valid SV to compute a solution. */ - SBG_ECOM_HDT_INTERNAL_ERROR = 2, /*!< An internal error has occurred. */ - SBG_ECOM_HDT_HEIGHT_LIMIT = 3 /*!< The height limit has been exceeded. */ -} SbgEComGpsHdtStatus; - -//----------------------------------------------------------------------// -//- Helpers methods for velocity status access -// -//----------------------------------------------------------------------// - -/*! - * Method used to read GPS velocity status from a status field. - * - * \param[in] status Status uint32_t value to extract the velocity status from it. - * \return The extracted velocity status. - */ -SBG_INLINE SbgEComGpsVelStatus sbgEComLogGpsVelGetStatus(uint32_t status) -{ - return (SbgEComGpsVelStatus)((status >> SBG_ECOM_GPS_VEL_STATUS_SHIFT) & SBG_ECOM_GPS_VEL_STATUS_MASK); -} - -/*! - * Method used to read GPS velocity type from a status field. - * - * \param[in] status Status uint32_t value to extract the velocity type from it. - * \return The extracted velocity type. - */ -SBG_INLINE SbgEComGpsVelType sbgEComLogGpsVelGetType(uint32_t status) -{ - return (SbgEComGpsVelType)((status >> SBG_ECOM_GPS_VEL_TYPE_SHIFT) & SBG_ECOM_GPS_VEL_TYPE_MASK); -} - -/*! - * Method used to write the GPS velocity status to a status field. - * - * \param[in] status The velocity status to set. - * \param[in] type The velocity type to set. - * \return The build GpsVelData status field. - */ -SBG_INLINE uint32_t sbgEComLogGpsVelBuildStatus(SbgEComGpsVelStatus status, SbgEComGpsVelType type) -{ - // - // Create the combined status field - // - return ((((uint32_t)status)&SBG_ECOM_GPS_VEL_STATUS_MASK) << SBG_ECOM_GPS_VEL_STATUS_SHIFT) | - ((((uint32_t)type)&SBG_ECOM_GPS_VEL_TYPE_MASK) << SBG_ECOM_GPS_VEL_TYPE_SHIFT); -} - -//----------------------------------------------------------------------// -//- Helpers methods for position status access -// -//----------------------------------------------------------------------// - -/*! - * Method used to read GPS position status from a status field. - * - * \param[in] status Status uint32_t value to extract the position status from it. - * \return The extracted position status. - */ -SBG_INLINE SbgEComGpsPosStatus sbgEComLogGpsPosGetStatus(uint32_t status) -{ - return (SbgEComGpsPosStatus)((status >> SBG_ECOM_GPS_POS_STATUS_SHIFT) & SBG_ECOM_GPS_POS_STATUS_MASK); -} - -/*! - * Method used to read GPS position type from a status field. - * - * \param[in] status Status uint32_t value to extract the position type from it. - * \return The extracted position type. - */ -SBG_INLINE SbgEComGpsPosType sbgEComLogGpsPosGetType(uint32_t status) -{ - return (SbgEComGpsPosType)((status >> SBG_ECOM_GPS_POS_TYPE_SHIFT) & SBG_ECOM_GPS_POS_TYPE_MASK); -} - -/*! - * Method used to write the GPS position status to a status field. - * - * \param[in] status The position status to set. - * \param[in] type The position type to set. - * \param[in] masks Bit mask to set. - * \return The build GpsPosData status field. - */ -SBG_INLINE uint32_t sbgEComLogGpsPosBuildStatus(SbgEComGpsPosStatus status, SbgEComGpsPosType type, uint32_t masks) -{ - // - // Create the combined status field - // - return ((((uint32_t)status)&SBG_ECOM_GPS_POS_STATUS_MASK) << SBG_ECOM_GPS_POS_STATUS_SHIFT) | - ((((uint32_t)type)&SBG_ECOM_GPS_POS_TYPE_MASK) << SBG_ECOM_GPS_POS_TYPE_SHIFT) | masks; -} - -//----------------------------------------------------------------------// -//- Helpers methods for HDT status access -// -//----------------------------------------------------------------------// - -/*! - * Method used to read GPS HDT status from a status field. - * - * \param[in] status Status uint32_t value to extract the HDT status from it. - * \return The extracted position status. - */ -SBG_INLINE SbgEComGpsHdtStatus sbgEComLogGpsHdtGetStatus(uint32_t status) -{ - return (SbgEComGpsHdtStatus)((status >> SBG_ECOM_GPS_HDT_STATUS_SHIFT) & SBG_ECOM_GPS_HDT_STATUS_MASK); -} - -/*! - * Method used to write the GPS HDT status to a status field. - * - * \param[in] status The HDT status to set. - * \param[in] masks Bit mask to set. - * \return The build GpsPosData status field. - */ -SBG_INLINE uint32_t sbgEComLogGpsHdtBuildStatus(SbgEComGpsHdtStatus status, uint32_t masks) -{ - // - // Create the combined status field - // - return ((((uint32_t)status)&SBG_ECOM_GPS_HDT_STATUS_MASK) << SBG_ECOM_GPS_HDT_STATUS_SHIFT) | masks; -} - -//----------------------------------------------------------------------// -//- Log structure definitions -// -//----------------------------------------------------------------------// - -/*! - * Structure that stores data for the SBG_ECOM_LOG_GPS#_VEL message. - */ -typedef struct _SbgLogGpsVel -{ - uint32_t timeStamp; /*!< Time in us since the sensor power up. */ - uint32_t status; /*!< GPS velocity status, type and bitmask. */ - uint32_t timeOfWeek; /*!< GPS time of week in ms. */ - float velocity[3]; /*!< GPS North, East, Down velocity in m.s^-1. */ - float velocityAcc[3]; /*!< GPS North, East, Down velocity 1 sigma accuracy in m.s^-1. */ - float course; /*!< Track ground course in degrees. */ - float courseAcc; /*!< Course accuracy in degrees. */ -} SbgLogGpsVel; - -/*! - * Structure that stores data for the SBG_ECOM_LOG_GPS#_POS message. - */ -typedef struct _SbgLogGpsPos -{ - uint32_t timeStamp; /*!< Time in us since the sensor power up. */ - uint32_t status; /*!< GPS position status, type and bitmask. */ - uint32_t timeOfWeek; /*!< GPS time of week in ms. */ - double latitude; /*!< Latitude in degrees, positive north. */ - double longitude; /*!< Longitude in degrees, positive east. */ - double altitude; /*!< Altitude above Mean Sea Level in meters. */ - float undulation; /*!< Altitude difference between the geoid and the Ellipsoid in meters (Height above Ellipsoid = altitude + undulation). */ - float latitudeAccuracy; /*!< 1 sigma latitude accuracy in meters. */ - float longitudeAccuracy; /*!< 1 sigma longitude accuracy in meters. */ - float altitudeAccuracy; /*!< 1 sigma altitude accuracy in meters. */ - uint8_t numSvUsed; /*!< Number of space vehicles used to compute the solution (since version 1.4). */ - uint16_t baseStationId; /*!< Base station id for differential corrections (0-4095). Set to 0xFFFF if differential corrections are not used (since version 1.4). */ - uint16_t differentialAge; /*!< Differential correction age in 0.01 seconds. Set to 0XFFFF if differential corrections are not used (since version 1.4). */ -} SbgLogGpsPos; - -/*! - * Structure that stores data for the SBG_ECOM_LOG_GPS#_HDT message. - */ -typedef struct _SbgLogGpsHdt -{ - uint32_t timeStamp; /*!< Time in us since the sensor power up. */ - uint16_t status; /*!< GPS HDT status, type and bitmask. */ - uint32_t timeOfWeek; /*!< GPS time of week in ms. */ - float heading; /*!< GPS true heading in degrees. */ - float headingAccuracy; /*!< 1 sigma GPS true heading accuracy in degrees. */ - float pitch; /*!< GPS pitch angle measured from the master to the rover in degrees. */ - float pitchAccuracy; /*!< 1 signa GPS pitch angle accuarcy in degrees. */ - float baseline; /*!< The distance between the main and aux antenna in meters. */ -} SbgLogGpsHdt; - -//----------------------------------------------------------------------// -//- Public methods -// -//----------------------------------------------------------------------// - -/*! - * Parse data for the SBG_ECOM_LOG_GPS#_VEL message and fill the corresponding structure. - * - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseGpsVelData(SbgStreamBuffer *pInputStream, SbgLogGpsVel *pOutputData); - -/*! - * Write data for the SBG_ECOM_LOG_GPS#_VEL message to the output stream buffer from the provided structure. - * - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteGpsVelData(SbgStreamBuffer *pOutputStream, const SbgLogGpsVel *pInputData); - -/*! - * Parse data for the SBG_ECOM_LOG_GPS#_POS message and fill the corresponding structure. - * - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseGpsPosData(SbgStreamBuffer *pInputStream, SbgLogGpsPos *pOutputData); - -/*! - * Write data for the SBG_ECOM_LOG_GPS#_POS message to the output stream buffer from the provided structure. - * - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteGpsPosData(SbgStreamBuffer *pOutputStream, const SbgLogGpsPos *pInputData); - -/*! - * Parse data for the SBG_ECOM_LOG_GPS#_HDT message and fill the corresponding structure. - * - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseGpsHdtData(SbgStreamBuffer *pInputStream, SbgLogGpsHdt *pOutputData); - -/*! - * Write data for the SBG_ECOM_LOG_GPS#_HDT message to the output stream buffer from the provided structure. - * - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteGpsHdtData(SbgStreamBuffer *pOutputStream, const SbgLogGpsHdt *pInputData); - -/*! - * Parse data for the SBG_ECOM_LOG_GPS#_RAW message and fill the corresponding structure. - * - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseGpsRawData(SbgStreamBuffer *pInputStream, SbgLogRawData *pOutputData); - -/*! - * Write data for the SBG_ECOM_LOG_GPS#_RAW message to the output stream buffer from the provided structure. - * - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteGpsRawData(SbgStreamBuffer *pOutputStream, const SbgLogRawData *pInputData); - -#ifdef __cplusplus -} -#endif - -#endif // SBG_ECOM_BINARY_LOG_GPS_H diff --git a/src/binaryLogs/sbgEComBinaryLogImu.c b/src/binaryLogs/sbgEComBinaryLogImu.c deleted file mode 100644 index 279daf6..0000000 --- a/src/binaryLogs/sbgEComBinaryLogImu.c +++ /dev/null @@ -1,206 +0,0 @@ -#include "sbgEComBinaryLogImu.h" - -//----------------------------------------------------------------------// -//- Public getters -// -//----------------------------------------------------------------------// - -float sbgLogImuShortGetDeltaAngle(const SbgLogImuShort *pImuShort, size_t idx) -{ - assert(pImuShort); - assert(idx < 3); - - return pImuShort->deltaAngle[idx] / 67108864.0f; -} - -float sbgLogImuShortGetDeltaVelocity(const SbgLogImuShort *pImuShort, size_t idx) -{ - assert(pImuShort); - assert(idx < 3); - - return pImuShort->deltaVelocity[idx] / 1048576.0f; -} - -float sbgLogImuShortGetTemperature(const SbgLogImuShort *pImuShort) -{ - assert(pImuShort); - - return pImuShort->temperature / 256.0f; -} - -//----------------------------------------------------------------------// -//- Public methods -// -//----------------------------------------------------------------------// - -SbgErrorCode sbgEComBinaryLogParseImuData(SbgStreamBuffer *pInputStream, SbgLogImuData *pOutputData) -{ - assert(pInputStream); - assert(pOutputData); - - // - // Read the frame payload - // - pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->status = sbgStreamBufferReadUint16LE(pInputStream); - - pOutputData->accelerometers[0] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->accelerometers[1] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->accelerometers[2] = sbgStreamBufferReadFloatLE(pInputStream); - - pOutputData->gyroscopes[0] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->gyroscopes[1] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->gyroscopes[2] = sbgStreamBufferReadFloatLE(pInputStream); - - pOutputData->temperature = sbgStreamBufferReadFloatLE(pInputStream); - - pOutputData->deltaVelocity[0] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->deltaVelocity[1] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->deltaVelocity[2] = sbgStreamBufferReadFloatLE(pInputStream); - - pOutputData->deltaAngle[0] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->deltaAngle[1] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->deltaAngle[2] = sbgStreamBufferReadFloatLE(pInputStream); - - // - // Return if any error has occurred while parsing the frame - // - return sbgStreamBufferGetLastError(pInputStream); -} - -SbgErrorCode sbgEComBinaryLogWriteImuData(SbgStreamBuffer *pOutputStream, const SbgLogImuData *pInputData) -{ - assert(pOutputStream); - assert(pInputData); - - // - // Write the frame payload - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->status); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->accelerometers[0]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->accelerometers[1]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->accelerometers[2]); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->gyroscopes[0]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->gyroscopes[1]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->gyroscopes[2]); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->temperature); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->deltaVelocity[0]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->deltaVelocity[1]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->deltaVelocity[2]); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->deltaAngle[0]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->deltaAngle[1]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->deltaAngle[2]); - - // - // Return if any error has occurred while writing the frame - // - return sbgStreamBufferGetLastError(pOutputStream); -} - -SbgErrorCode sbgEComBinaryLogParseImuShort(SbgStreamBuffer *pInputStream, SbgLogImuShort *pOutputData) -{ - assert(pInputStream); - assert(pOutputData); - - // - // Read the frame payload - // - pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->status = sbgStreamBufferReadUint16LE(pInputStream); - - pOutputData->deltaVelocity[0] = sbgStreamBufferReadInt32LE(pInputStream); - pOutputData->deltaVelocity[1] = sbgStreamBufferReadInt32LE(pInputStream); - pOutputData->deltaVelocity[2] = sbgStreamBufferReadInt32LE(pInputStream); - - pOutputData->deltaAngle[0] = sbgStreamBufferReadInt32LE(pInputStream); - pOutputData->deltaAngle[1] = sbgStreamBufferReadInt32LE(pInputStream); - pOutputData->deltaAngle[2] = sbgStreamBufferReadInt32LE(pInputStream); - - pOutputData->temperature = sbgStreamBufferReadInt16LE(pInputStream); - - // - // Return if any error has occurred while parsing the frame - // - return sbgStreamBufferGetLastError(pInputStream); -} - -SbgErrorCode sbgEComBinaryLogWriteImuShort(SbgStreamBuffer *pOutputStream, const SbgLogImuShort *pInputData) -{ - assert(pOutputStream); - assert(pInputData); - - // - // Write the frame payload - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->status); - - sbgStreamBufferWriteInt32LE(pOutputStream, pInputData->deltaVelocity[0]); - sbgStreamBufferWriteInt32LE(pOutputStream, pInputData->deltaVelocity[1]); - sbgStreamBufferWriteInt32LE(pOutputStream, pInputData->deltaVelocity[2]); - - sbgStreamBufferWriteInt32LE(pOutputStream, pInputData->deltaAngle[0]); - sbgStreamBufferWriteInt32LE(pOutputStream, pInputData->deltaAngle[1]); - sbgStreamBufferWriteInt32LE(pOutputStream, pInputData->deltaAngle[2]); - - sbgStreamBufferWriteInt16LE(pOutputStream, pInputData->temperature); - - // - // Return if any error has occurred while writing the frame - // - return sbgStreamBufferGetLastError(pOutputStream); -} - -SbgErrorCode sbgEComBinaryLogParseFastImuData(SbgStreamBuffer *pInputStream, SbgLogFastImuData *pOutputData) -{ - assert(pInputStream); - assert(pOutputData); - - // - // Read the frame payload - // - pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->status = sbgStreamBufferReadUint16LE(pInputStream); - - pOutputData->accelerometers[0] = (float)sbgStreamBufferReadInt16LE(pInputStream) * 0.01f; - pOutputData->accelerometers[1] = (float)sbgStreamBufferReadInt16LE(pInputStream) * 0.01f; - pOutputData->accelerometers[2] = (float)sbgStreamBufferReadInt16LE(pInputStream) * 0.01f; - - pOutputData->gyroscopes[0] = (float)sbgStreamBufferReadInt16LE(pInputStream) * 0.001f; - pOutputData->gyroscopes[1] = (float)sbgStreamBufferReadInt16LE(pInputStream) * 0.001f; - pOutputData->gyroscopes[2] = (float)sbgStreamBufferReadInt16LE(pInputStream) * 0.001f; - - // - // Return if any error has occurred while parsing the frame - // - return sbgStreamBufferGetLastError(pInputStream); -} - -SbgErrorCode sbgEComBinaryLogWriteFastImuData(SbgStreamBuffer *pOutputStream, const SbgLogFastImuData *pInputData) -{ - assert(pOutputStream); - assert(pInputData); - - // - // Write the frame payload - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->status); - - sbgStreamBufferWriteInt16LE(pOutputStream, (int16_t)(pInputData->accelerometers[0] * 100.0f)); - sbgStreamBufferWriteInt16LE(pOutputStream, (int16_t)(pInputData->accelerometers[1] * 100.0f)); - sbgStreamBufferWriteInt16LE(pOutputStream, (int16_t)(pInputData->accelerometers[2] * 100.0f)); - - sbgStreamBufferWriteInt16LE(pOutputStream, (int16_t)(pInputData->gyroscopes[0] * 1000.0f)); - sbgStreamBufferWriteInt16LE(pOutputStream, (int16_t)(pInputData->gyroscopes[1] * 1000.0f)); - sbgStreamBufferWriteInt16LE(pOutputStream, (int16_t)(pInputData->gyroscopes[2] * 1000.0f)); - - // - // Return if any error has occurred while writing the frame - // - return sbgStreamBufferGetLastError(pOutputStream); -} diff --git a/src/binaryLogs/sbgEComBinaryLogMag.c b/src/binaryLogs/sbgEComBinaryLogMag.c deleted file mode 100644 index efa30d5..0000000 --- a/src/binaryLogs/sbgEComBinaryLogMag.c +++ /dev/null @@ -1,89 +0,0 @@ -#include "sbgEComBinaryLogMag.h" - -//----------------------------------------------------------------------// -//- Public methods -// -//----------------------------------------------------------------------// - -SbgErrorCode sbgEComBinaryLogParseMagData(SbgStreamBuffer *pInputStream, SbgLogMag *pOutputData) -{ - assert(pInputStream); - assert(pOutputData); - - // - // Read the frame payload - // - pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->status = sbgStreamBufferReadUint16LE(pInputStream); - - pOutputData->magnetometers[0] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->magnetometers[1] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->magnetometers[2] = sbgStreamBufferReadFloatLE(pInputStream); - - pOutputData->accelerometers[0] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->accelerometers[1] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->accelerometers[2] = sbgStreamBufferReadFloatLE(pInputStream); - - // - // Return if any error has occurred while parsing the frame - // - return sbgStreamBufferGetLastError(pInputStream); -} - -SbgErrorCode sbgEComBinaryLogWriteMagData(SbgStreamBuffer *pOutputStream, const SbgLogMag *pInputData) -{ - assert(pOutputStream); - assert(pInputData); - - // - // Write the frame payload - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->status); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->magnetometers[0]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->magnetometers[1]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->magnetometers[2]); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->accelerometers[0]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->accelerometers[1]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->accelerometers[2]); - - // - // Return if any error has occurred while writing the frame - // - return sbgStreamBufferGetLastError(pOutputStream); -} - -SbgErrorCode sbgEComBinaryLogParseMagCalibData(SbgStreamBuffer *pInputStream, SbgLogMagCalib *pOutputData) -{ - assert(pInputStream); - assert(pOutputData); - - // - // Read the frame payload - // - pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->reserved = sbgStreamBufferReadUint16LE(pInputStream); - - // - // Read the raw magnetic calibration data buffer - // - return sbgStreamBufferReadBuffer(pInputStream, pOutputData->magData, sizeof(pOutputData->magData)); -} - -SbgErrorCode sbgEComBinaryLogWriteMagCalibData(SbgStreamBuffer *pOutputStream, const SbgLogMagCalib *pInputData) -{ - assert(pOutputStream); - assert(pInputData); - - // - // Write the frame payload - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->reserved); - - // - // Write the raw magnetic calibration data buffer - // - return sbgStreamBufferWriteBuffer(pOutputStream, pInputData->magData, sizeof(pInputData->magData)); -} diff --git a/src/binaryLogs/sbgEComBinaryLogOdometer.c b/src/binaryLogs/sbgEComBinaryLogOdometer.c deleted file mode 100644 index 075337c..0000000 --- a/src/binaryLogs/sbgEComBinaryLogOdometer.c +++ /dev/null @@ -1,43 +0,0 @@ -#include "sbgEComBinaryLogOdometer.h" - -//----------------------------------------------------------------------// -//- Public methods -// -//----------------------------------------------------------------------// - -SbgErrorCode sbgEComBinaryLogParseOdometerData(SbgStreamBuffer *pInputStream, SbgLogOdometerData *pOutputData) -{ - assert(pInputStream); - assert(pOutputData); - - // - // Read the frame payload - // - pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->status = sbgStreamBufferReadUint16LE(pInputStream); - - pOutputData->velocity = sbgStreamBufferReadFloatLE(pInputStream); - - // - // Return if any error has occurred while parsing the frame - // - return sbgStreamBufferGetLastError(pInputStream); -} - -SbgErrorCode sbgEComBinaryLogWriteOdometerData(SbgStreamBuffer *pOutputStream, const SbgLogOdometerData *pInputData) -{ - assert(pOutputStream); - assert(pInputData); - - // - // Write the frame payload - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->status); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocity); - - // - // Return if any error has occurred while writing the frame - // - return sbgStreamBufferGetLastError(pOutputStream); -} diff --git a/src/binaryLogs/sbgEComBinaryLogRawData.c b/src/binaryLogs/sbgEComBinaryLogRawData.c deleted file mode 100644 index 71ed37f..0000000 --- a/src/binaryLogs/sbgEComBinaryLogRawData.c +++ /dev/null @@ -1,40 +0,0 @@ -// sbgCommonLib headers -#include - -// Local headers -#include "sbgEComBinaryLogRawData.h" - -//----------------------------------------------------------------------// -//- Public methods -// -//----------------------------------------------------------------------// - -SbgErrorCode sbgEComBinaryLogParseRawData(SbgStreamBuffer *pInputStream, SbgLogRawData *pOutputData) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - size_t payloadSize; - - assert(pInputStream); - assert(pOutputData); - - payloadSize = sbgStreamBufferGetSize(pInputStream); - - if (payloadSize <= SBG_ECOM_RAW_DATA_MAX_BUFFER_SIZE) - { - errorCode = sbgStreamBufferReadBuffer(pInputStream, pOutputData->rawBuffer, payloadSize); - pOutputData->bufferSize = payloadSize; - } - else - { - errorCode = SBG_BUFFER_OVERFLOW; - } - - return errorCode; -} - -SbgErrorCode sbgEComBinaryLogWriteRawData(SbgStreamBuffer *pOutputStream, const SbgLogRawData *pInputData) -{ - assert(pOutputStream); - assert(pInputData); - - return sbgStreamBufferWriteBuffer(pOutputStream, pInputData->rawBuffer, pInputData->bufferSize); -} diff --git a/src/binaryLogs/sbgEComBinaryLogRtcm.c b/src/binaryLogs/sbgEComBinaryLogRtcm.c deleted file mode 100644 index fcfce45..0000000 --- a/src/binaryLogs/sbgEComBinaryLogRtcm.c +++ /dev/null @@ -1,19 +0,0 @@ -// sbgCommonLib headers -#include - -// Local headers -#include "sbgEComBinaryLogRtcm.h" - -//----------------------------------------------------------------------// -//- Public methods -// -//----------------------------------------------------------------------// - -SbgErrorCode sbgEComBinaryLogParseRtcmRawData(SbgStreamBuffer *pInputStream, SbgLogRawData *pOutputData) -{ - return sbgEComBinaryLogParseRawData(pInputStream, pOutputData); -} - -SbgErrorCode sbgEComBinaryLogWriteRtcmRawData(SbgStreamBuffer *pOutputStream, const SbgLogRawData *pInputData) -{ - return sbgEComBinaryLogWriteRawData(pOutputStream, pInputData); -} diff --git a/src/binaryLogs/sbgEComBinaryLogRtcm.h b/src/binaryLogs/sbgEComBinaryLogRtcm.h deleted file mode 100644 index 00e38bf..0000000 --- a/src/binaryLogs/sbgEComBinaryLogRtcm.h +++ /dev/null @@ -1,73 +0,0 @@ -/*! - * \file sbgEComBinaryLogRtcm.h - * \ingroup binaryLogs - * \author SBG Systems - * \date 16 November 2020 - * - * \brief Parse RTCM data stream logs as received by the INS. - * - * \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 - */ - -#ifndef SBG_ECOM_BINARY_LOG_RTCM_H -#define SBG_ECOM_BINARY_LOG_RTCM_H - -// sbgCommonLib headers -#include -#include - -// Local headers -#include "sbgEComBinaryLogRawData.h" - -#ifdef __cplusplus -extern "C" { -#endif - -//----------------------------------------------------------------------// -//- Public methods -// -//----------------------------------------------------------------------// - -/*! - * Parse data for the SBG_ECOM_LOG_RTCM_RAW message and fill the corresponding structure. - * - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseRtcmRawData(SbgStreamBuffer *pInputStream, SbgLogRawData *pOutputData); - -/*! - * Write data for the SBG_ECOM_LOG_RTCM_RAW message to the output stream buffer from the provided structure. - * - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteRtcmRawData(SbgStreamBuffer *pOutputStream, const SbgLogRawData *pInputData); - -#ifdef __cplusplus -} -#endif - -#endif // SBG_ECOM_BINARY_LOG_RTCM_H diff --git a/src/binaryLogs/sbgEComBinaryLogSat.c b/src/binaryLogs/sbgEComBinaryLogSat.c deleted file mode 100644 index 503c0bc..0000000 --- a/src/binaryLogs/sbgEComBinaryLogSat.c +++ /dev/null @@ -1,865 +0,0 @@ -/*! - * \file sbgEComBinaryLogSat.h - * \author SBG Systems - * \date 1 March 2022 - * - * \brief Handle binary satellite logs. - * - * \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 - -// Project headers -#include - -// Local headers -#include "sbgEComBinaryLogSat.h" - -//----------------------------------------------------------------------// -//- 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_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_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. */ - -//----------------------------------------------------------------------// -//- Private functions -// -//----------------------------------------------------------------------// - -/*! - * 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. - */ -#define sbgEComBinaryLogSatGetField(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. - */ -#define sbgEComBinaryLogSatSetField(flags, value, offset, mask) (flags) &= ~((mask) << (offset)); (flags) |= ((value) & (mask)) << (offset) - - - -/*! - * Check the value of a health status. - * - * \param[in] healthStatus Health status. - * \return SBG_NO_ERROR if successful. - */ -static SbgErrorCode sbgEComBinaryLogSatCheckHealthStatus(uint8_t healthStatus) -{ - SbgErrorCode errorCode = SBG_INVALID_FRAME; - - switch (healthStatus) - { - case SBG_ECOM_SAT_HEALTH_STATUS_UNKNOWN: - case SBG_ECOM_SAT_HEALTH_STATUS_HEALTHY: - case SBG_ECOM_SAT_HEALTH_STATUS_UNHEALTHY: - errorCode = SBG_NO_ERROR; - break; - } - - if (errorCode != SBG_NO_ERROR) - { - SBG_LOG_ERROR(errorCode, "invalid health status: %" PRIu8, healthStatus); - } - - return errorCode; -} - -/*! - * Check the value of a tracking status. - * - * \param[in] trackingStatus Tracking status. - * \return SBG_NO_ERROR if successful. - */ -static SbgErrorCode sbgEComBinaryLogSatCheckTrackingStatus(uint8_t trackingStatus) -{ - SbgErrorCode errorCode = SBG_INVALID_FRAME; - - switch (trackingStatus) - { - case SBG_ECOM_SAT_TRACKING_STATUS_UNKNOWN: - case SBG_ECOM_SAT_TRACKING_STATUS_SEARCHING: - case SBG_ECOM_SAT_TRACKING_STATUS_TRACKING_UNKNOWN: - case SBG_ECOM_SAT_TRACKING_STATUS_TRACKING_NOT_USED: - case SBG_ECOM_SAT_TRACKING_STATUS_TRACKING_REJECTED: - case SBG_ECOM_SAT_TRACKING_STATUS_TRACKING_USED: - errorCode = SBG_NO_ERROR; - break; - } - - if (errorCode != SBG_NO_ERROR) - { - SBG_LOG_ERROR(errorCode, "invalid tracking status: %" PRIu8, trackingStatus); - } - - return errorCode; -} - -/*! - * Returns tracking status string from enum value - * - * \param[in] trackingStatus Tracking status enum to convert - * \return Tracking status as a read only C string. - */ -static const char *sbgEComBinaryLogSatTrackingStatusToStr(SbgEComSatTrackingStatus trackingStatus) -{ - static const char *enumToStrLut[] = - { - [SBG_ECOM_SAT_TRACKING_STATUS_UNKNOWN] = "unkown", - [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 health status string from enum value - * - * \param[in] trackingStatus Tracking status enum to convert - * \return Tracking status as a read only C string. - */ -static const char *sbgEComBinaryLogSatHealthStatusToStr(SbgEComSatHealthStatus healthStatus) -{ - static const char *enumToStrLut[] = - { - [SBG_ECOM_SAT_HEALTH_STATUS_UNKNOWN] = "unkown", - [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]; - } -} - -/*! - * Parse signal data from a stream buffer. - * - * \param[in] pStreamBuffer Stream buffer. - * \param[out] pSignalData Signal data. - * \return SBG_NO_ERROR if successful. - */ -static SbgErrorCode sbgEComBinaryLogParseSignalData(SbgStreamBuffer *pStreamBuffer, SbgLogSatSignalData *pSignalData) -{ - SbgErrorCode errorCode; - uint8_t signalId; - - assert(pSignalData); - - signalId = sbgStreamBufferReadUint8(pStreamBuffer); - pSignalData->flags = sbgStreamBufferReadUint8(pStreamBuffer); - pSignalData->snr = sbgStreamBufferReadUint8(pStreamBuffer); - - errorCode = sbgStreamBufferGetLastError(pStreamBuffer); - - if (errorCode == SBG_NO_ERROR) - { - if (sbgEComSignalIdIsValid(signalId)) - { - uint8_t healthStatus; - - pSignalData->id = signalId; - - healthStatus = sbgEComBinaryLogSatGetField(pSignalData->flags, SBG_ECOM_LOG_SAT_HEALTH_STATUS_OFFSET, SBG_ECOM_LOG_SAT_HEALTH_STATUS_MASK); - - errorCode = sbgEComBinaryLogSatCheckHealthStatus(healthStatus); - - if (errorCode == SBG_NO_ERROR) - { - uint8_t trackingStatus; - - trackingStatus = sbgEComBinaryLogSatGetField(pSignalData->flags, SBG_ECOM_LOG_SAT_TRACKING_STATUS_OFFSET, SBG_ECOM_LOG_SAT_TRACKING_STATUS_MASK); - - errorCode = sbgEComBinaryLogSatCheckTrackingStatus(trackingStatus); - } - } - else - { - errorCode = SBG_INVALID_FRAME; - SBG_LOG_ERROR(errorCode, "invalid signal ID: %" PRIu8, signalId); - } - } - - return errorCode; -} - -/*! - * Check the value of an elevation status. - * - * \param[in] elevationStatus Elevation status value. - * \return SBG_NO_ERROR if successful. - */ -static SbgErrorCode sbgEComBinaryLogSatCheckElevationStatus(uint8_t elevationStatus) -{ - SbgErrorCode errorCode = SBG_INVALID_FRAME; - - switch (elevationStatus) - { - case SBG_ECOM_SAT_ELEVATION_STATUS_UNKNOWN: - case SBG_ECOM_SAT_ELEVATION_STATUS_SETTING: - case SBG_ECOM_SAT_ELEVATION_STATUS_RISING: - errorCode = SBG_NO_ERROR; - break; - } - - if (errorCode != SBG_NO_ERROR) - { - SBG_LOG_ERROR(errorCode, "invalid elevation status: %" PRIu8, elevationStatus); - } - - return errorCode; -} - -/*! - * Parse satellite data from a stream buffer. - * - * \param[in] pStreamBuffer Stream buffer. - * \param[out] pSatGroupData Satellite group data. - * \return SBG_NO_ERROR if successful. - */ -static SbgErrorCode sbgEComBinaryLogParseSatData(SbgStreamBuffer *pStreamBuffer, SbgLogSatData *pSatData) -{ - SbgErrorCode errorCode; - - assert(pSatData); - - pSatData->id = sbgStreamBufferReadUint8(pStreamBuffer); - pSatData->elevation = sbgStreamBufferReadInt8(pStreamBuffer); - pSatData->azimuth = sbgStreamBufferReadUint16(pStreamBuffer); - pSatData->flags = sbgStreamBufferReadUint16(pStreamBuffer); - pSatData->nrSignals = sbgStreamBufferReadUint8(pStreamBuffer); - pSatData->signalDataArraySize = pSatData->nrSignals; - - errorCode = sbgStreamBufferGetLastError(pStreamBuffer); - - if (errorCode == SBG_NO_ERROR) - { - if (pSatData->nrSignals <= SBG_ECOM_SAT_MAX_NR_SIGNALS) - { - pSatData->pSignalData = malloc(sizeof(*pSatData->pSignalData) * pSatData->signalDataArraySize); - - if (pSatData->pSignalData) - { - uint8_t constellationId; - - constellationId = sbgEComBinaryLogSatGetField(pSatData->flags, SBG_ECOM_LOG_SAT_CONSTELLATION_ID_OFFSET, SBG_ECOM_LOG_SAT_CONSTELLATION_ID_MASK); - - if (sbgEComConstellationIdIsValid(constellationId)) - { - uint8_t elevationStatus; - - elevationStatus = sbgEComBinaryLogSatGetField(pSatData->flags, SBG_ECOM_LOG_SAT_ELEVATION_STATUS_OFFSET, SBG_ECOM_LOG_SAT_ELEVATION_STATUS_MASK); - - errorCode = sbgEComBinaryLogSatCheckElevationStatus(elevationStatus); - - if (errorCode == SBG_NO_ERROR) - { - uint8_t healthStatus; - - healthStatus = sbgEComBinaryLogSatGetField(pSatData->flags, SBG_ECOM_LOG_SAT_HEALTH_STATUS_OFFSET, SBG_ECOM_LOG_SAT_HEALTH_STATUS_MASK); - - errorCode = sbgEComBinaryLogSatCheckHealthStatus(healthStatus); - - if (errorCode == SBG_NO_ERROR) - { - uint8_t trackingStatus; - - trackingStatus = sbgEComBinaryLogSatGetField(pSatData->flags, SBG_ECOM_LOG_SAT_TRACKING_STATUS_OFFSET, SBG_ECOM_LOG_SAT_TRACKING_STATUS_MASK); - - errorCode = sbgEComBinaryLogSatCheckTrackingStatus(trackingStatus); - - if (errorCode == SBG_NO_ERROR) - { - for (size_t i = 0; i < pSatData->nrSignals; i++) - { - errorCode = sbgEComBinaryLogParseSignalData(pStreamBuffer, &pSatData->pSignalData[i]); - - if (errorCode != SBG_NO_ERROR) - { - break; - } - } - } - } - } - } - else - { - errorCode = SBG_INVALID_FRAME; - SBG_LOG_ERROR(errorCode, "invalid constellation id: %" PRIu8, constellationId); - } - - if (errorCode != SBG_NO_ERROR) - { - free(pSatData->pSignalData); - pSatData->pSignalData = NULL; - } - } - else - { - errorCode = SBG_MALLOC_FAILED; - SBG_LOG_ERROR(errorCode, "unable to allocate signal data array"); - } - } - else - { - errorCode = SBG_INVALID_FRAME; - SBG_LOG_ERROR(errorCode, "invalid number of signals: %zu", pSatData->nrSignals); - } - } - - return errorCode; -} - -/*! - * Satellite data constructor. - * - * \param[in] pSatData Satellite data. - * \param[in] nrSignals Number of signals. - * \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 SBG_NO_ERROR if successful. - */ -static SbgErrorCode sbgLogSatDataConstruct(SbgLogSatData *pSatData, size_t nrSignals, uint8_t id, int8_t elevation, uint16_t azimuth, SbgEComConstellationId constellationId, SbgEComSatElevationStatus elevationStatus, SbgEComSatHealthStatus healthStatus, SbgEComSatTrackingStatus trackingStatus) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint16_t flags = 0; - - assert(pSatData); - assert(nrSignals <= SBG_ECOM_SAT_MAX_NR_SIGNALS); - - sbgEComBinaryLogSatSetField(flags, constellationId, SBG_ECOM_LOG_SAT_CONSTELLATION_ID_OFFSET, SBG_ECOM_LOG_SAT_CONSTELLATION_ID_MASK); - sbgEComBinaryLogSatSetField(flags, elevationStatus, SBG_ECOM_LOG_SAT_ELEVATION_STATUS_OFFSET, SBG_ECOM_LOG_SAT_ELEVATION_STATUS_MASK); - sbgEComBinaryLogSatSetField(flags, healthStatus, SBG_ECOM_LOG_SAT_HEALTH_STATUS_OFFSET, SBG_ECOM_LOG_SAT_HEALTH_STATUS_MASK); - sbgEComBinaryLogSatSetField(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->signalDataArraySize = nrSignals; - pSatData->nrSignals = 0; - - pSatData->pSignalData = malloc(sizeof(*pSatData->pSignalData) * pSatData->signalDataArraySize); - - if (!pSatData->pSignalData) - { - errorCode = SBG_MALLOC_FAILED; - SBG_LOG_ERROR(errorCode, "unable to allocate signal data array"); - } - - return errorCode; -} - -/*! - * Satellite data destructor. - * - * \param[in] pSatData Satellite data. - */ -static void sbgLogSatDataDestroy(SbgLogSatData *pSatData) -{ - assert(pSatData); - - free(pSatData->pSignalData); - pSatData->pSignalData = NULL; -} - -/*! - * Select one of two health statuses according to their priority rules. - * - * \param[in] healthStatus1 First health status. - * \param[in] healthStatus2 Second health status. - */ -static SbgEComSatHealthStatus sbgEComBinaryLogSatSelectHealthStatus(SbgEComSatHealthStatus healthStatus1, SbgEComSatHealthStatus healthStatus2) -{ - SbgEComSatHealthStatus healthStatus; - - if (healthStatus2 > healthStatus1) - { - healthStatus = healthStatus2; - } - else - { - healthStatus = healthStatus1; - } - - return healthStatus; -} - -/*! - * Select one of two tracking statuses according to their priority rules. - * - * \param[in] trackingStatus1 First tracking status. - * \param[in] trackingStatus2 Second tracking status. - */ -static SbgEComSatTrackingStatus sbgEComBinaryLogSatSelectTrackingStatus(SbgEComSatTrackingStatus trackingStatus1, SbgEComSatTrackingStatus trackingStatus2) -{ - SbgEComSatTrackingStatus trackingStatus; - - if (trackingStatus2 > trackingStatus1) - { - trackingStatus = trackingStatus2; - } - else - { - trackingStatus = trackingStatus1; - } - - return trackingStatus; -} - -/*! - * Update the health and tracking statuses of satellite data. - * - * \param[in] pSatData Satellite data. - * \param[in] healthStatus Health status. - * \param[in] trackingStatus Tracking status. - */ -static void sbgLogSatDataUpdateStatus(SbgLogSatData *pSatData, SbgEComSatHealthStatus healthStatus, SbgEComSatTrackingStatus trackingStatus) -{ - uint16_t flags; - - assert(pSatData); - - flags = pSatData->flags; - - healthStatus = sbgEComBinaryLogSatSelectHealthStatus(sbgLogSatDataGetHealthStatus(pSatData), healthStatus); - trackingStatus = sbgEComBinaryLogSatSelectTrackingStatus(sbgLogSatDataGetTrackingStatus(pSatData), trackingStatus); - - sbgEComBinaryLogSatSetField(flags, healthStatus, SBG_ECOM_LOG_SAT_HEALTH_STATUS_OFFSET, SBG_ECOM_LOG_SAT_HEALTH_STATUS_MASK); - sbgEComBinaryLogSatSetField(flags, trackingStatus, SBG_ECOM_LOG_SAT_TRACKING_STATUS_OFFSET, SBG_ECOM_LOG_SAT_TRACKING_STATUS_MASK); - - pSatData->flags = flags; -} - -//----------------------------------------------------------------------// -//- Public functions -// -//----------------------------------------------------------------------// - -SbgErrorCode sbgEComBinaryLogParseSatGroupData(SbgStreamBuffer *pStreamBuffer, SbgLogSatGroupData *pSatGroupData) -{ - SbgErrorCode errorCode; - - assert(pSatGroupData); - assert(pStreamBuffer); - - pSatGroupData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); - pSatGroupData->reserved = sbgStreamBufferReadUint32LE(pStreamBuffer); - pSatGroupData->nrSatellites = sbgStreamBufferReadUint8(pStreamBuffer); - pSatGroupData->satDataArraySize = pSatGroupData->nrSatellites; - - errorCode = sbgStreamBufferGetLastError(pStreamBuffer); - - if (errorCode == SBG_NO_ERROR) - { - if (pSatGroupData->nrSatellites <= SBG_ECOM_SAT_MAX_NR_SATELLITES) - { - pSatGroupData->pSatData = malloc(sizeof(*pSatGroupData->pSatData) * pSatGroupData->satDataArraySize); - - if (pSatGroupData->pSatData) - { - for (size_t i = 0; i < pSatGroupData->nrSatellites; i++) - { - errorCode = sbgEComBinaryLogParseSatData(pStreamBuffer, &pSatGroupData->pSatData[i]); - - if (errorCode != SBG_NO_ERROR) - { - break; - } - } - - if (errorCode != SBG_NO_ERROR) - { - free(pSatGroupData->pSatData); - pSatGroupData->pSatData = NULL; - } - } - else - { - errorCode = SBG_MALLOC_FAILED; - SBG_LOG_ERROR(errorCode, "unable to allocate satellite data array"); - } - } - else - { - errorCode = SBG_INVALID_FRAME; - SBG_LOG_ERROR(errorCode, "invalid number of satellites: %zu", pSatGroupData->nrSatellites); - } - } - - return errorCode; -} - -SbgErrorCode sbgEComBinaryLogWriteSatGroupData(SbgStreamBuffer *pStreamBuffer, const SbgLogSatGroupData *pSatGroupData) -{ - assert(pStreamBuffer); - assert(pSatGroupData); - assert(pSatGroupData->nrSatellites <= UINT8_MAX); - - sbgStreamBufferWriteUint32LE(pStreamBuffer, pSatGroupData->timeStamp); - sbgStreamBufferWriteUint32LE(pStreamBuffer, pSatGroupData->reserved); - sbgStreamBufferWriteUint8(pStreamBuffer, (uint8_t)pSatGroupData->nrSatellites); - - for (size_t i = 0; i < pSatGroupData->nrSatellites; i++) - { - const SbgLogSatData *pSatData = &pSatGroupData->pSatData[i]; - - assert(pSatData->nrSignals <= UINT8_MAX); - - sbgStreamBufferWriteUint8(pStreamBuffer, pSatData->id); - sbgStreamBufferWriteInt8(pStreamBuffer, pSatData->elevation); - sbgStreamBufferWriteUint16(pStreamBuffer, pSatData->azimuth); - sbgStreamBufferWriteUint16(pStreamBuffer, pSatData->flags); - sbgStreamBufferWriteUint8(pStreamBuffer, (uint8_t)pSatData->nrSignals); - - for (size_t j = 0; j < pSatData->nrSignals; j++) - { - const SbgLogSatSignalData *pSignalData = &pSatData->pSignalData[j]; - - sbgStreamBufferWriteUint8(pStreamBuffer, pSignalData->id); - sbgStreamBufferWriteUint8(pStreamBuffer, pSignalData->flags); - sbgStreamBufferWriteUint8(pStreamBuffer, pSignalData->snr); - } - } - - return sbgStreamBufferGetLastError(pStreamBuffer); -} - -SbgErrorCode sbgLogSatGroupDataConstruct(SbgLogSatGroupData *pSatGroupData, size_t nrSatellites, uint32_t timeStamp) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - - assert(pSatGroupData); - assert(nrSatellites <= SBG_ECOM_SAT_MAX_NR_SATELLITES); - - pSatGroupData->timeStamp = timeStamp; - pSatGroupData->reserved = 0; - pSatGroupData->nrSatellites = 0; - pSatGroupData->satDataArraySize = nrSatellites; - - pSatGroupData->pSatData = malloc(sizeof(*pSatGroupData->pSatData) * pSatGroupData->satDataArraySize); - - if (!pSatGroupData->pSatData) - { - errorCode = SBG_MALLOC_FAILED; - SBG_LOG_ERROR(errorCode, "unable to allocate satellite data array"); - } - - return SBG_NO_ERROR; -} - -void sbgLogSatGroupDataDestroy(SbgLogSatGroupData *pSatGroupData) -{ - assert(pSatGroupData); - - for (size_t i = 0; i < pSatGroupData->nrSatellites; i++) - { - sbgLogSatDataDestroy(&pSatGroupData->pSatData[i]); - } - - free(pSatGroupData->pSatData); - pSatGroupData->pSatData = NULL; -} - -SbgLogSatData *sbgLogSatGroupDataAdd(SbgLogSatGroupData *pSatGroupData, size_t nrSignals, uint8_t id, int8_t elevation, uint16_t azimuth, SbgEComConstellationId constellationId, SbgEComSatElevationStatus elevationStatus, SbgEComSatHealthStatus healthStatus, SbgEComSatTrackingStatus trackingStatus) -{ - SbgLogSatData *pSatData = NULL; - - assert(pSatGroupData); - - if (pSatGroupData->nrSatellites < pSatGroupData->satDataArraySize) - { - SbgErrorCode errorCode; - SbgLogSatData *pTmpSatData; - - pTmpSatData = &pSatGroupData->pSatData[pSatGroupData->nrSatellites]; - - errorCode = sbgLogSatDataConstruct(pTmpSatData, nrSignals, id, elevation, azimuth, constellationId, elevationStatus, healthStatus, trackingStatus); - - if (errorCode == SBG_NO_ERROR) - { - pSatData = pTmpSatData; - pSatGroupData->nrSatellites++; - } - } - - return pSatData; -} - -SbgLogSatData *sbgLogSatGroupGet(SbgLogSatGroupData *pSatGroupData, uint8_t id) -{ - SbgLogSatData *pSatData = NULL; - - assert(pSatGroupData); - - for (size_t i = 0; i < pSatGroupData->nrSatellites; i++) - { - if (pSatGroupData->pSatData[i].id == id) - { - pSatData = &pSatGroupData->pSatData[i]; - break; - } - } - - return pSatData; -} - -SbgEComConstellationId sbgLogSatDataGetConstellationId(const SbgLogSatData *pSatData) -{ - uint8_t value; - - assert(pSatData); - - value = sbgEComBinaryLogSatGetField(pSatData->flags, SBG_ECOM_LOG_SAT_CONSTELLATION_ID_OFFSET, SBG_ECOM_LOG_SAT_CONSTELLATION_ID_MASK); - - return (SbgEComConstellationId)value; -} - -const char *sbgLogSatDataGetConstellationIdAsStr(const SbgLogSatData *pSatData) -{ - return sbgEComConstellationToStr(sbgLogSatDataGetConstellationId(pSatData)); -} - -SbgEComSatElevationStatus sbgLogSatDataGetElevationStatus(const SbgLogSatData *pSatData) -{ - uint8_t value; - - assert(pSatData); - - value = sbgEComBinaryLogSatGetField(pSatData->flags, SBG_ECOM_LOG_SAT_ELEVATION_STATUS_OFFSET, SBG_ECOM_LOG_SAT_ELEVATION_STATUS_MASK); - - return (SbgEComSatElevationStatus)value; -} - -const char *sbgLogSatDataGetElevationStatusAsStr(const SbgLogSatData *pSatData) -{ - SbgEComSatElevationStatus enumIdx; - static const char *enumToStrLut[] = - { - [SBG_ECOM_SAT_ELEVATION_STATUS_UNKNOWN] = "unkown", - [SBG_ECOM_SAT_ELEVATION_STATUS_SETTING] = "setting", - [SBG_ECOM_SAT_ELEVATION_STATUS_RISING] = "rising", - }; - - assert(pSatData); - - enumIdx = sbgLogSatDataGetElevationStatus(pSatData); - - if (enumIdx < SBG_ARRAY_SIZE(enumToStrLut)) - { - return enumToStrLut[enumIdx]; - } - else - { - return enumToStrLut[SBG_ECOM_SAT_ELEVATION_STATUS_UNKNOWN]; - } -} - -SbgEComSatHealthStatus sbgLogSatDataGetHealthStatus(const SbgLogSatData *pSatData) -{ - uint8_t value; - - assert(pSatData); - - value = sbgEComBinaryLogSatGetField(pSatData->flags, SBG_ECOM_LOG_SAT_HEALTH_STATUS_OFFSET, SBG_ECOM_LOG_SAT_HEALTH_STATUS_MASK); - - return (SbgEComSatHealthStatus)value; -} - -const char *sbgLogSatDataGetHealthStatusAsStr(const SbgLogSatData *pSatData) -{ - assert(pSatData); - - return sbgEComBinaryLogSatHealthStatusToStr(sbgLogSatDataGetHealthStatus(pSatData)); -} - -SbgEComSatTrackingStatus sbgLogSatDataGetTrackingStatus(const SbgLogSatData *pSatData) -{ - uint8_t value; - - assert(pSatData); - - value = sbgEComBinaryLogSatGetField(pSatData->flags, SBG_ECOM_LOG_SAT_TRACKING_STATUS_OFFSET, SBG_ECOM_LOG_SAT_TRACKING_STATUS_MASK); - - return (SbgEComSatTrackingStatus)value; -} - -const char *sbgLogSatDataGetTrackingStatusAsStr(const SbgLogSatData *pSatData) -{ - assert(pSatData); - - return sbgEComBinaryLogSatTrackingStatusToStr(sbgLogSatDataGetTrackingStatus(pSatData)); -} - -SbgLogSatSignalData *sbgLogSatDataAdd(SbgLogSatData *pSatData, SbgEComSignalId id, SbgEComSatHealthStatus healthStatus, SbgEComSatTrackingStatus trackingStatus, bool snrValid, uint8_t snr) -{ - SbgLogSatSignalData *pSignalData = NULL; - - assert(pSatData); - - if (pSatData->nrSignals < pSatData->signalDataArraySize) - { - uint8_t flags = 0; - - sbgEComBinaryLogSatSetField(flags, healthStatus, SBG_ECOM_LOG_SAT_HEALTH_STATUS_OFFSET, SBG_ECOM_LOG_SAT_HEALTH_STATUS_MASK); - sbgEComBinaryLogSatSetField(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; - } - - pSignalData = &pSatData->pSignalData[pSatData->nrSignals]; - - pSignalData->id = id; - pSignalData->flags = flags; - pSignalData->snr = snr; - - pSatData->nrSignals++; - - sbgLogSatDataUpdateStatus(pSatData, healthStatus, trackingStatus); - } - - return pSignalData; -} - -SbgLogSatSignalData *sbgLogSatDataGet(SbgLogSatData *pSatData, SbgEComSignalId id) -{ - SbgLogSatSignalData *pSignalData = NULL; - - assert(pSatData); - - for (size_t i = 0; i < pSatData->nrSignals; i++) - { - if (pSatData->pSignalData[i].id == id) - { - pSignalData = &pSatData->pSignalData[i]; - break; - } - } - - return pSignalData; -} - -const char *sbgLogSatSignalDataGetSignalIdAsStr(const SbgLogSatSignalData *pSignalData) -{ - return sbgEComSignalToStr(pSignalData->id); -} - -bool sbgLogSatSignalDataSnrIsValid(const SbgLogSatSignalData *pSignalData) -{ - if (pSignalData->flags&SBG_ECOM_LOG_SAT_SIGNAL_SNR_VALID) - { - return true; - } - else - { - return false; - } -} - -SbgEComSatHealthStatus sbgLogSatSignalDataGetHealthStatus(const SbgLogSatSignalData *pSignalData) -{ - uint8_t value; - - assert(pSignalData); - - value = sbgEComBinaryLogSatGetField(pSignalData->flags, SBG_ECOM_LOG_SAT_HEALTH_STATUS_OFFSET, SBG_ECOM_LOG_SAT_HEALTH_STATUS_MASK); - - return (SbgEComSatHealthStatus)value; -} - -const char *sbgLogSatSignalDataGetHealthStatusAsStr(const SbgLogSatSignalData *pSignalData) -{ - assert(pSignalData); - - return sbgEComBinaryLogSatHealthStatusToStr(sbgLogSatSignalDataGetHealthStatus(pSignalData)); -} - -SbgEComSatTrackingStatus sbgLogSatSignalDataGetTrackingStatus(const SbgLogSatSignalData *pSignalData) -{ - uint8_t value; - - assert(pSignalData); - - value = sbgEComBinaryLogSatGetField(pSignalData->flags, SBG_ECOM_LOG_SAT_TRACKING_STATUS_OFFSET, SBG_ECOM_LOG_SAT_TRACKING_STATUS_MASK); - - return (SbgEComSatTrackingStatus)value; -} - -const char *sbgLogSatSignalDataGetTrackingStatusAsStr(const SbgLogSatSignalData *pSignalData) -{ - assert(pSignalData); - - return sbgEComBinaryLogSatTrackingStatusToStr(sbgLogSatSignalDataGetTrackingStatus(pSignalData)); -} diff --git a/src/binaryLogs/sbgEComBinaryLogShipMotion.c b/src/binaryLogs/sbgEComBinaryLogShipMotion.c deleted file mode 100644 index d89f210..0000000 --- a/src/binaryLogs/sbgEComBinaryLogShipMotion.c +++ /dev/null @@ -1,110 +0,0 @@ -#include "sbgEComBinaryLogShipMotion.h" - -//----------------------------------------------------------------------// -//- Public methods -// -//----------------------------------------------------------------------// - -SbgErrorCode sbgEComBinaryLogParseShipMotionData(SbgStreamBuffer *pInputStream, SbgLogShipMotionData *pOutputData) -{ - assert(pInputStream); - assert(pOutputData); - - // - // Read the frame payload - // - pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); - - // - // Read the main heave period in seconds - // - pOutputData->mainHeavePeriod = sbgStreamBufferReadFloatLE(pInputStream); - - // - // Read the surge, sway and heave ship motion - // - pOutputData->shipMotion[0] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->shipMotion[1] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->shipMotion[2] = sbgStreamBufferReadFloatLE(pInputStream); - - // - // Read the ship accelerations - // - pOutputData->shipAccel[0] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->shipAccel[1] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->shipAccel[2] = sbgStreamBufferReadFloatLE(pInputStream); - - // - // Test if we have a additional information such as ship velocity and status (since version 1.4) - // - if (sbgStreamBufferGetSpace(pInputStream) >= 14) - { - // - // Read new outputs - // - pOutputData->shipVel[0] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->shipVel[1] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->shipVel[2] = sbgStreamBufferReadFloatLE(pInputStream); - - pOutputData->status = sbgStreamBufferReadUint16LE(pInputStream); - } - else - { - // - // Those outputs are not available in previous versions - // - pOutputData->shipVel[0] = 0.0f; - pOutputData->shipVel[1] = 0.0f; - pOutputData->shipVel[2] = 0.0f; - - pOutputData->status = 0; - } - - // - // Return if any error has occurred while parsing the frame - // - return sbgStreamBufferGetLastError(pInputStream); -} - -SbgErrorCode sbgEComBinaryLogWriteShipMotionData(SbgStreamBuffer *pOutputStream, const SbgLogShipMotionData *pInputData) -{ - assert(pOutputStream); - assert(pInputData); - - // - // Write the frame payload - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); - - // - // Write the main heave period in seconds - // - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->mainHeavePeriod); - - // - // Write the surge, sway and heave ship motion - // - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->shipMotion[0]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->shipMotion[1]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->shipMotion[2]); - - // - // Write the ship accelerations - // - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->shipAccel[0]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->shipAccel[1]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->shipAccel[2]); - - // - // Write additional inforamtion added in version 1.4 - // - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->shipVel[0]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->shipVel[1]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->shipVel[2]); - - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->status); - - // - // Return if any error has occurred while writing the frame - // - return sbgStreamBufferGetLastError(pOutputStream); -} diff --git a/src/binaryLogs/sbgEComBinaryLogStatus.c b/src/binaryLogs/sbgEComBinaryLogStatus.c deleted file mode 100644 index 2c83251..0000000 --- a/src/binaryLogs/sbgEComBinaryLogStatus.c +++ /dev/null @@ -1,72 +0,0 @@ -#include "sbgEComBinaryLogStatus.h" - -//----------------------------------------------------------------------// -//- Public methods -// -//----------------------------------------------------------------------// - -SbgErrorCode sbgEComBinaryLogParseStatusData(SbgStreamBuffer *pInputStream, SbgLogStatusData *pOutputData) -{ - assert(pInputStream); - assert(pOutputData); - - // - // Read the frame payload - // - pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->generalStatus = sbgStreamBufferReadUint16LE(pInputStream); - pOutputData->comStatus2 = sbgStreamBufferReadUint16LE(pInputStream); - pOutputData->comStatus = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->aidingStatus = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->reserved2 = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->reserved3 = sbgStreamBufferReadUint16LE(pInputStream); - - // - // Test if we have a additional information such as uptime (since version 1.7) - // - if (sbgStreamBufferGetSpace(pInputStream) >= sizeof(uint32_t)) - { - // - // Read the additional information - // - pOutputData->uptime = sbgStreamBufferReadUint32LE(pInputStream); - } - else - { - // - // Default the additional information - // - pOutputData->uptime = 0; - } - - // - // Return if any error has occurred while parsing the frame - // - return sbgStreamBufferGetLastError(pInputStream); -} - -SbgErrorCode sbgEComBinaryLogWriteStatusData(SbgStreamBuffer *pOutputStream, const SbgLogStatusData *pInputData) -{ - assert(pOutputStream); - assert(pInputData); - - // - // Write the frame payload - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->generalStatus); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->comStatus2); - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->comStatus); - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->aidingStatus); - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->reserved2); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->reserved3); - - // - // Write the additional information added in version 1.7 - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->uptime); - - // - // Return if any error has occurred while writing the frame - // - return sbgStreamBufferGetLastError(pOutputStream); -} diff --git a/src/binaryLogs/sbgEComBinaryLogStatus.h b/src/binaryLogs/sbgEComBinaryLogStatus.h deleted file mode 100644 index 601305a..0000000 --- a/src/binaryLogs/sbgEComBinaryLogStatus.h +++ /dev/null @@ -1,216 +0,0 @@ -/*! - * \file sbgEComBinaryLogStatus.h - * \ingroup binaryLogs - * \author SBG Systems - * \date 03 April 2013 - * - * \brief Parse logs used to report device status. - * - * \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 - */ - -#ifndef SBG_ECOM_BINARY_LOG_STATUS_H -#define SBG_ECOM_BINARY_LOG_STATUS_H - -// sbgCommonLib headers -#include -#include - -#ifdef __cplusplus -extern "C" { -#endif - -//----------------------------------------------------------------------// -//- General status definitions -// -//----------------------------------------------------------------------// -#define SBG_ECOM_GENERAL_MAIN_POWER_OK (0x0001u << 0) /*!< Set to 1 when main power supply is OK. */ -#define SBG_ECOM_GENERAL_IMU_POWER_OK (0x0001u << 1) /*!< Set to 1 when IMU power supply is OK. */ -#define SBG_ECOM_GENERAL_GPS_POWER_OK (0x0001u << 2) /*!< Set to 1 when GPS power supply is OK. */ -#define SBG_ECOM_GENERAL_SETTINGS_OK (0x0001u << 3) /*!< Set to 1 if settings where correctly loaded. */ -#define SBG_ECOM_GENERAL_TEMPERATURE_OK (0x0001u << 4) /*!< Set to 1 when temperature is within specified limits. */ -#define SBG_ECOM_GENERAL_DATALOGGER_OK (0x0001u << 5) /*!< Set to 1 when the datalogger is working correctly. */ -#define SBG_ECOM_GENERAL_CPU_OK (0x0001u << 6) /*!< Set to 1 if the CPU headroom is correct.*/ - -//----------------------------------------------------------------------// -//- Communication status definitions -// -//----------------------------------------------------------------------// - -/*! - * Communication CAN status definitions. - */ -#define SBG_ECOM_CAN_STATUS_SHIFT (28u) /*!< Shift used to access the CAN status part. */ -#define SBG_ECOM_CAN_STATUS_MASK (0x00000007u) /*!< Mask used to keep only the CAN status part. */ - -/*! - * Communication status bit mask definitions. - */ -#define SBG_ECOM_PORTA_VALID (0x00000001u << 0) /*!< Set to 0 in case of low level communication error. */ -#define SBG_ECOM_PORTB_VALID (0x00000001u << 1) /*!< Set to 0 in case of low level communication error. */ -#define SBG_ECOM_PORTC_VALID (0x00000001u << 2) /*!< Set to 0 in case of low level communication error. */ -#define SBG_ECOM_PORTD_VALID (0x00000001u << 3) /*!< Set to 0 in case of low level communication error. */ -#define SBG_ECOM_PORTE_VALID (0x00000001u << 4) /*!< Set to 0 in case of low level communication error. */ - -#define SBG_ECOM_PORTA_RX_OK (0x00000001u << 5) /*!< Set to 0 in case of error on PORT A input. */ -#define SBG_ECOM_PORTA_TX_OK (0x00000001u << 6) /*!< Set to 0 in case of error on PORT A output. */ -#define SBG_ECOM_PORTB_RX_OK (0x00000001u << 7) /*!< Set to 0 in case of error on PORT B input. */ -#define SBG_ECOM_PORTB_TX_OK (0x00000001u << 8) /*!< Set to 0 in case of error on PORT B output. */ -#define SBG_ECOM_PORTC_RX_OK (0x00000001u << 9) /*!< Set to 0 in case of error on PORT C input. */ -#define SBG_ECOM_PORTC_TX_OK (0x00000001u << 10) /*!< Set to 0 in case of error on PORT C output. */ -#define SBG_ECOM_PORTD_RX_OK (0x00000001u << 11) /*!< Set to 0 in case of error on PORT D input. */ -#define SBG_ECOM_PORTD_TX_OK (0x00000001u << 12) /*!< Set to 0 in case of error on PORT D input. */ -#define SBG_ECOM_PORTE_RX_OK (0x00000001u << 13) /*!< Set to 0 in case of error on PORT E input. */ -#define SBG_ECOM_PORTE_TX_OK (0x00000001u << 14) /*!< Set to 0 in case of error on PORT D input. */ - -#define SBG_ECOM_ETH0_VALID (0x00000001u << 15) /*!< Set to 0 in case of error on ETH0. */ -#define SBG_ECOM_ETH1_VALID (0x00000001u << 16) /*!< Set to 0 in case of error on ETH1. */ -#define SBG_ECOM_ETH2_VALID (0x00000001u << 17) /*!< Set to 0 in case of error on ETH2. */ -#define SBG_ECOM_ETH3_VALID (0x00000001u << 18) /*!< Set to 0 in case of error on ETH3. */ -#define SBG_ECOM_ETH4_VALID (0x00000001u << 19) /*!< Set to 0 in case of error on ETH4. */ - -#define SBG_ECOM_CAN_VALID (0x00000001u << 25) /*!< Set to 0 in case of low level communication error. */ -#define SBG_ECOM_CAN_RX_OK (0x00000001u << 26) /*!< Set to 0 in case of error on CAN Bus input buffer. */ -#define SBG_ECOM_CAN_TX_OK (0x00000001u << 27) /*!< Set to 0 in case of error on CAN Bus output buffer. */ - -/*! - * Second communication status bit mask definitions. - */ -#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. */ - -/*! - * Communication status for the CAN Bus. - */ -typedef enum _SbgEComCanBusStatus -{ - SBG_ECOM_CAN_BUS_OFF = 0, /*!< Bus OFF operation due to too much errors. */ - SBG_ECOM_CAN_BUS_TX_RX_ERR = 1, /*!< Errors on Tx or Rx. */ - SBG_ECOM_CAN_BUS_OK = 2, /*!< Bus OK. */ - SBG_ECOM_CAN_BUS_ERROR = 3 /*!< Bus error. */ -} SbgEComCanBusStatus; - -//----------------------------------------------------------------------// -//- Aiding status definitions -// -//----------------------------------------------------------------------// -#define SBG_ECOM_AIDING_GPS1_POS_RECV (0x00000001u << 0) /*!< Set to 1 when valid GPS 1 position data is received. */ -#define SBG_ECOM_AIDING_GPS1_VEL_RECV (0x00000001u << 1) /*!< Set to 1 when valid GPS 1 velocity data is received. */ -#define SBG_ECOM_AIDING_GPS1_HDT_RECV (0x00000001u << 2) /*!< Set to 1 when valid GPS 1 true heading data is received. */ -#define SBG_ECOM_AIDING_GPS1_UTC_RECV (0x00000001u << 3) /*!< Set to 1 when valid GPS 1 UTC time data is received. */ -#define SBG_ECOM_AIDING_GPS2_POS_RECV (0x00000001u << 4) /*!< Set to 1 when valid GPS 2 position data is received. */ -#define SBG_ECOM_AIDING_GPS2_VEL_RECV (0x00000001u << 5) /*!< Set to 1 when valid GPS 2 velocity data is received. */ -#define SBG_ECOM_AIDING_GPS2_HDT_RECV (0x00000001u << 6) /*!< Set to 1 when valid GPS 2 true heading data is received. */ -#define SBG_ECOM_AIDING_GPS2_UTC_RECV (0x00000001u << 7) /*!< Set to 1 when valid GPS 2 UTC time data is received. */ -#define SBG_ECOM_AIDING_MAG_RECV (0x00000001u << 8) /*!< Set to 1 when valid Magnetometer data is received. */ -#define SBG_ECOM_AIDING_ODO_RECV (0x00000001u << 9) /*!< Set to 1 when Odometer pulse is received. */ -#define SBG_ECOM_AIDING_DVL_RECV (0x00000001u << 10) /*!< Set to 1 when valid DVL data is received. */ -#define SBG_ECOM_AIDING_USBL_RECV (0x00000001u << 11) /*!< Set to 1 when valid USBL data is received. */ -#define SBG_ECOM_AIDING_DEPTH_RECV (0x00000001u << 12) /*!< Set to 1 when valid Depth Log data is received. */ -#define SBG_ECOM_AIDING_AIR_DATA_RECV (0x00000001u << 13) /*!< Set to 1 when valid Air Data (altitude and/or true airspeed) is received. */ -#define SBG_ECOM_AIDING_USER_POS_RECV (0x00000001u << 14) /*!< Set to 1 when valid user position data is received. */ -#define SBG_ECOM_AIDING_USER_VEL_RECV (0x00000001u << 15) /*!< Set to 1 when valid user velocity data is received. */ -#define SBG_ECOM_AIDING_USER_HEADING_RECV (0x00000001u << 16) /*!< Set to 1 when valid user heading data is received. */ - -//----------------------------------------------------------------------// -//- Status definitions -// -//----------------------------------------------------------------------// - -/*! - * Stores global status data. - */ -typedef struct _SbgLogStatusData -{ - uint32_t timeStamp; /*!< Time in us since the sensor power up. */ - uint16_t generalStatus; /*!< General status bitmask and enums. */ - uint32_t comStatus; /*!< Communication status bitmask and enums. */ - uint16_t comStatus2; /*!< Second communication status bitmask and enums. */ - uint32_t aidingStatus; /*!< Aiding equipments status bitmask and enums. */ - uint32_t reserved2; /*!< Reserved status field for future use. */ - uint16_t reserved3; /*!< Reserved status field for future use. */ - uint32_t uptime; /*!< System uptime in seconds. */ -} SbgLogStatusData; - -//----------------------------------------------------------------------// -//- Public getters & helpers -// -//----------------------------------------------------------------------// - -/*! - * Method used to read the CAN bus status from a communication status field. - * - * \param[in] status Status field to extract the CAN bus status from it. - * \return The extracted CAN bus status. - */ -SBG_INLINE SbgEComCanBusStatus sbgEComLogStatusGetCanStatus(uint32_t status) -{ - return (SbgEComCanBusStatus)((status >> SBG_ECOM_CAN_STATUS_SHIFT) & SBG_ECOM_CAN_STATUS_MASK); -} - -/*! - * Method used to write the CAN bus status field. - * - * \param[in] canStatus The CAN bus status to set. - * \param[in] masks Bit mask to set. - * \return The build communication status field. - */ -SBG_INLINE uint32_t sbgEComLogStatusBuildCommunicationStatus(SbgEComCanBusStatus canStatus, uint32_t masks) -{ - // - // Create the combined status field - // - return ((((uint32_t)canStatus)&SBG_ECOM_CAN_STATUS_MASK) << SBG_ECOM_CAN_STATUS_SHIFT) | masks; -} - -//----------------------------------------------------------------------// -//- Public methods -// -//----------------------------------------------------------------------// - -/*! - * Parse data for the SBG_ECOM_LOG_STATUS message and fill the corresponding structure. - * - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseStatusData(SbgStreamBuffer *pInputStream, SbgLogStatusData *pOutputData); - -/*! - * Write data for the SBG_ECOM_LOG_STATUS message to the output stream buffer from the provided structure. - * - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteStatusData(SbgStreamBuffer *pOutputStream, const SbgLogStatusData *pInputData); - -#ifdef __cplusplus -} -#endif - -#endif // SBG_ECOM_BINARY_LOG_STATUS_H diff --git a/src/binaryLogs/sbgEComBinaryLogUsbl.c b/src/binaryLogs/sbgEComBinaryLogUsbl.c deleted file mode 100644 index 67491d0..0000000 --- a/src/binaryLogs/sbgEComBinaryLogUsbl.c +++ /dev/null @@ -1,57 +0,0 @@ -#include "sbgEComBinaryLogUsbl.h" - -//----------------------------------------------------------------------// -//- Public methods -// -//----------------------------------------------------------------------// - -SbgErrorCode sbgEComBinaryLogParseUsblData(SbgStreamBuffer *pInputStream, SbgLogUsblData *pOutputData) -{ - assert(pInputStream); - assert(pOutputData); - - // - // Read the frame payload - // - pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->status = sbgStreamBufferReadUint16LE(pInputStream); - - pOutputData->latitude = sbgStreamBufferReadDoubleLE(pInputStream); - pOutputData->longitude = sbgStreamBufferReadDoubleLE(pInputStream); - - pOutputData->depth = sbgStreamBufferReadFloatLE(pInputStream); - - pOutputData->latitudeAccuracy = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->longitudeAccuracy = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->depthAccuracy = sbgStreamBufferReadFloatLE(pInputStream); - - // - // Return if any error has occurred while parsing the frame - // - return sbgStreamBufferGetLastError(pInputStream); -} - -SbgErrorCode sbgEComBinaryLogWriteUsblData(SbgStreamBuffer *pOutputStream, const SbgLogUsblData *pInputData) -{ - assert(pOutputStream); - assert(pInputData); - - // - // Write the frame payload - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->status); - - sbgStreamBufferWriteDoubleLE(pOutputStream, pInputData->latitude); - sbgStreamBufferWriteDoubleLE(pOutputStream, pInputData->longitude); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->depth); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->latitudeAccuracy); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->longitudeAccuracy); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->depthAccuracy); - - // - // Return if any error has occurred while writing the frame - // - return sbgStreamBufferGetLastError(pOutputStream); -} diff --git a/src/binaryLogs/sbgEComBinaryLogUtc.c b/src/binaryLogs/sbgEComBinaryLogUtc.c deleted file mode 100644 index d803438..0000000 --- a/src/binaryLogs/sbgEComBinaryLogUtc.c +++ /dev/null @@ -1,100 +0,0 @@ -#include "sbgEComBinaryLogUtc.h" - -//----------------------------------------------------------------------// -//- Private global definitions -// -//----------------------------------------------------------------------// - -/*!< Lookup table for clock status enum */ -static const char *gClockStatusStr[] = -{ - [SBG_ECOM_CLOCK_ERROR] = "SBG_ECOM_CLOCK_ERROR", - [SBG_ECOM_CLOCK_FREE_RUNNING] = "SBG_ECOM_CLOCK_FREE_RUNNING", - [SBG_ECOM_CLOCK_STEERING] = "SBG_ECOM_CLOCK_STEERING", - [SBG_ECOM_CLOCK_VALID] = "SBG_ECOM_CLOCK_VALID" -}; - -/*!< Lookup table for UTC status enum */ -static const char *gUtcStatusStr[] = -{ - [SBG_ECOM_UTC_INVALID] = "SBG_ECOM_UTC_INVALID", - [SBG_ECOM_UTC_NO_LEAP_SEC] = "SBG_ECOM_UTC_NO_LEAP_SEC", - [SBG_ECOM_UTC_VALID] = "SBG_ECOM_UTC_VALID", -}; - -//----------------------------------------------------------------------// -//- Public methods -// -//----------------------------------------------------------------------// - -const char *sbgEcomLogUtcGetClockStatusAsString(const SbgLogUtcData *pLogUtc) -{ - SbgEComClockStatus clockStatus; - - assert(pLogUtc); - - clockStatus = sbgEComLogUtcGetClockStatus(pLogUtc->status); - assert(clockStatus < SBG_ARRAY_SIZE(gClockStatusStr)); - - return gClockStatusStr[clockStatus]; -} - -const char *sbgEcomLogUtcGetUtcStatusAsString(const SbgLogUtcData *pLogUtc) -{ - SbgEComClockUtcStatus utcStatus; - - assert(pLogUtc); - - utcStatus = sbgEComLogUtcGetClockUtcStatus(pLogUtc->status); - assert(utcStatus < SBG_ARRAY_SIZE(gUtcStatusStr)); - - return gUtcStatusStr[utcStatus]; -} - -SbgErrorCode sbgEComBinaryLogParseUtcData(SbgStreamBuffer *pInputStream, SbgLogUtcData *pOutputData) -{ - assert(pInputStream); - assert(pOutputData); - - // - // Read the frame payload - // - pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->status = sbgStreamBufferReadUint16LE(pInputStream); - pOutputData->year = sbgStreamBufferReadUint16LE(pInputStream); - pOutputData->month = sbgStreamBufferReadInt8LE(pInputStream); - pOutputData->day = sbgStreamBufferReadInt8LE(pInputStream); - pOutputData->hour = sbgStreamBufferReadInt8LE(pInputStream); - pOutputData->minute = sbgStreamBufferReadInt8LE(pInputStream); - pOutputData->second = sbgStreamBufferReadInt8LE(pInputStream); - pOutputData->nanoSecond = sbgStreamBufferReadInt32LE(pInputStream); - pOutputData->gpsTimeOfWeek = sbgStreamBufferReadUint32LE(pInputStream); - - // - // Return if any error has occurred while parsing the frame - // - return sbgStreamBufferGetLastError(pInputStream); -} - -SbgErrorCode sbgEComBinaryLogWriteUtcData(SbgStreamBuffer *pOutputStream, const SbgLogUtcData *pInputData) -{ - assert(pOutputStream); - assert(pInputData); - - // - // Write the frame payload - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->status); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->year); - sbgStreamBufferWriteInt8LE(pOutputStream, pInputData->month); - sbgStreamBufferWriteInt8LE(pOutputStream, pInputData->day); - sbgStreamBufferWriteInt8LE(pOutputStream, pInputData->hour); - sbgStreamBufferWriteInt8LE(pOutputStream, pInputData->minute); - sbgStreamBufferWriteInt8LE(pOutputStream, pInputData->second); - sbgStreamBufferWriteInt32LE(pOutputStream, pInputData->nanoSecond); - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->gpsTimeOfWeek); - - // - // Return if any error has occurred while writing the frame - // - return sbgStreamBufferGetLastError(pOutputStream); -} diff --git a/src/binaryLogs/sbgEComBinaryLogUtc.h b/src/binaryLogs/sbgEComBinaryLogUtc.h deleted file mode 100644 index 39561bb..0000000 --- a/src/binaryLogs/sbgEComBinaryLogUtc.h +++ /dev/null @@ -1,190 +0,0 @@ -/*! - * \file sbgEComBinaryLogUtc.h - * \ingroup binaryLogs - * \author SBG Systems - * \date 20 February 2013 - * - * \brief Parse logs used to report device UTC time. - * - * \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 - */ - -#ifndef SBG_ECOM_BINARY_LOG_UTC_H -#define SBG_ECOM_BINARY_LOG_UTC_H - -// sbgCommonLib headers -#include -#include - -#ifdef __cplusplus -extern "C" { -#endif - -//----------------------------------------------------------------------// -//- Clock status definitions -// -//----------------------------------------------------------------------// - -/*! - * Clock status and UTC time status definitions. - */ -#define SBG_ECOM_CLOCK_STATUS_SHIFT (1u) /*!< Shift used to extract the clock status part. */ -#define SBG_ECOM_CLOCK_STATUS_MASK (0x000Fu) /*!< Mask used to keep only the clock status part. */ -#define SBG_ECOM_CLOCK_UTC_STATUS_SHIFT (6u) /*!< Shift used to extract the clock UTC status part. */ -#define SBG_ECOM_CLOCK_UTC_STATUS_MASK (0x000Fu) /*!< Mask used to keep only the clock UTC status part. */ - -/*! - * Clock status mask definitions. - */ -#define SBG_ECOM_CLOCK_STABLE_INPUT (0x0001u << 0) /*!< Set to 1 if a stable input clock could be used to synchronized the internal clock. */ -#define SBG_ECOM_CLOCK_UTC_SYNC (0x0001u << 5) /*!< The UTC time is synchronized with a PPS. */ - -/*! - * Clock status enum. - */ -typedef enum _SbgEComClockStatus -{ - SBG_ECOM_CLOCK_ERROR = 0, /*!< An error has occurred on the clock estimation. */ - SBG_ECOM_CLOCK_FREE_RUNNING = 1, /*!< The clock is only based on the internal crystal. */ - SBG_ECOM_CLOCK_STEERING = 2, /*!< A PPS has been detected and the clock is converging to it. */ - SBG_ECOM_CLOCK_VALID = 3 /*!< The clock has converged to the PPS and is within 500ns. */ -} SbgEComClockStatus; - -/*! - * Status for the UTC time data. - */ -typedef enum _SbgEComClockUtcStatus -{ - SBG_ECOM_UTC_INVALID = 0, /*!< The UTC time is not known, we are just propagating the UTC time internally. */ - SBG_ECOM_UTC_NO_LEAP_SEC = 1, /*!< We have received valid UTC time information but we don't have the leap seconds information. */ - SBG_ECOM_UTC_VALID = 2 /*!< We have received valid UTC time data with valid leap seconds. */ -} SbgEComClockUtcStatus; - -//----------------------------------------------------------------------// -//- Clock status helpers methods -// -//----------------------------------------------------------------------// - -/*! - * Method used to read the clock status from a status field. - * - * \param[in] status Status field to extract the clock status from it. - * \return The extracted clock status. - */ -SBG_INLINE SbgEComClockStatus sbgEComLogUtcGetClockStatus(uint16_t status) -{ - return (SbgEComClockStatus)((status >> SBG_ECOM_CLOCK_STATUS_SHIFT) & SBG_ECOM_CLOCK_STATUS_MASK); -} - -/*! - * Method used to read the UTC time status from a clock status field. - * - * \param[in] status Status field to extract the UTC time status from it. - * \return The extracted UTC time status. - */ -SBG_INLINE SbgEComClockUtcStatus sbgEComLogUtcGetClockUtcStatus(uint16_t status) -{ - return (SbgEComClockUtcStatus)((status >> SBG_ECOM_CLOCK_UTC_STATUS_SHIFT) & SBG_ECOM_CLOCK_UTC_STATUS_MASK); -} - -/*! - * Method used to write the clock status field. - * - * \param[in] clockStatus The clock status to set. - * \param[in] utcStatus The UTC time status to set. - * \param[in] masks Bit mask to set. - * \return The build clock status field. - */ -SBG_INLINE uint16_t sbgEComLogUtcBuildClockStatus(SbgEComClockStatus clockStatus, SbgEComClockUtcStatus utcStatus, uint16_t masks) -{ - // - // Create the combined status field - // - return ((((uint16_t)clockStatus)&SBG_ECOM_CLOCK_STATUS_MASK) << SBG_ECOM_CLOCK_STATUS_SHIFT) | - ((((uint16_t)utcStatus)&SBG_ECOM_CLOCK_UTC_STATUS_MASK) << SBG_ECOM_CLOCK_UTC_STATUS_SHIFT) | masks; -} - -//----------------------------------------------------------------------// -//- Log structure definitions -// -//----------------------------------------------------------------------// - -/*! - * Structure that stores data for the SBG_ECOM_LOG_UTC_TIME message. - */ -typedef struct _SbgLogUtcData -{ - uint32_t timeStamp; /*!< Time in us since the sensor power up. */ - uint16_t status; /*!< UTC time and clock status information */ - uint16_t year; /*!< Year for example: 2013. */ - int8_t month; /*!< Month in year [1 .. 12]. */ - int8_t day; /*!< Day in month [1 .. 31]. */ - int8_t hour; /*!< Hour in day [0 .. 23]. */ - int8_t minute; /*!< Minute in hour [0 .. 59]. */ - int8_t second; /*!< Second in minute [0 .. 60]. (60 is used only when a leap second is added) */ - int32_t nanoSecond; /*!< Nanosecond of current second in ns. */ - uint32_t gpsTimeOfWeek; /*!< GPS time of week in ms. */ -} SbgLogUtcData; - -//----------------------------------------------------------------------// -//- Public methods -// -//----------------------------------------------------------------------// - -/*! - * Returns the clock status as a NULL terminated C string. - * - * \param[in] pLogUtc UTC log instance. - * \return The clock status as a C string. - */ -const char *sbgEcomLogUtcGetClockStatusAsString(const SbgLogUtcData *pLogUtc); - - -/*! - * Returns the UTC status as a NULL terminated C string. - * - * \param[in] pLogUtc UTC log instance. - * \return The UTC status as a C string. - */ -const char *sbgEcomLogUtcGetUtcStatusAsString(const SbgLogUtcData *pLogUtc); - -/*! - * Parse data for the SBG_ECOM_LOG_UTC_DATA message and fill the corresponding structure. - * - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseUtcData(SbgStreamBuffer *pInputStream, SbgLogUtcData *pOutputData); - -/*! - * Write data for the SBG_ECOM_LOG_UTC_DATA message to the output stream buffer from the provided structure. - * - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteUtcData(SbgStreamBuffer *pOutputStream, const SbgLogUtcData *pInputData); - -#ifdef __cplusplus -} -#endif - -#endif // SBG_ECOM_BINARY_LOG_UTC_H diff --git a/src/binaryLogs/sbgEComBinaryLogs.c b/src/binaryLogs/sbgEComBinaryLogs.c deleted file mode 100644 index 883e0b2..0000000 --- a/src/binaryLogs/sbgEComBinaryLogs.c +++ /dev/null @@ -1,183 +0,0 @@ -// sbgCommonLib headers -#include -#include - -// Project headers -#include - -// Local headers -#include "sbgEComBinaryLogAirData.h" -#include "sbgEComBinaryLogDepth.h" -#include "sbgEComBinaryLogDiag.h" -#include "sbgEComBinaryLogDvl.h" -#include "sbgEComBinaryLogEkf.h" -#include "sbgEComBinaryLogEvent.h" -#include "sbgEComBinaryLogGps.h" -#include "sbgEComBinaryLogImu.h" -#include "sbgEComBinaryLogMag.h" -#include "sbgEComBinaryLogOdometer.h" -#include "sbgEComBinaryLogRtcm.h" -#include "sbgEComBinaryLogShipMotion.h" -#include "sbgEComBinaryLogStatus.h" -#include "sbgEComBinaryLogUsbl.h" -#include "sbgEComBinaryLogUtc.h" -#include "sbgEComBinaryLogSat.h" -#include "sbgEComBinaryLogs.h" - -//----------------------------------------------------------------------// -//- Public methods -// -//----------------------------------------------------------------------// - -SbgErrorCode sbgEComBinaryLogParse(SbgEComClass msgClass, SbgEComMsgId msg, const void *pPayload, size_t payloadSize, SbgBinaryLogData *pOutputData) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgStreamBuffer inputStream; - - assert(pPayload); - assert(payloadSize > 0); - assert(pOutputData); - - // - // 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 (msg) - { - case SBG_ECOM_LOG_STATUS: - errorCode = sbgEComBinaryLogParseStatusData(&inputStream, &pOutputData->statusData); - break; - case SBG_ECOM_LOG_IMU_DATA: - errorCode = sbgEComBinaryLogParseImuData(&inputStream, &pOutputData->imuData); - break; - case SBG_ECOM_LOG_IMU_SHORT: - errorCode = sbgEComBinaryLogParseImuShort(&inputStream, &pOutputData->imuShort); - break; - case SBG_ECOM_LOG_EKF_EULER: - errorCode = sbgEComBinaryLogParseEkfEulerData(&inputStream, &pOutputData->ekfEulerData); - break; - case SBG_ECOM_LOG_EKF_QUAT: - errorCode = sbgEComBinaryLogParseEkfQuatData(&inputStream, &pOutputData->ekfQuatData); - break; - case SBG_ECOM_LOG_EKF_NAV: - errorCode = sbgEComBinaryLogParseEkfNavData(&inputStream, &pOutputData->ekfNavData); - break; - case SBG_ECOM_LOG_SHIP_MOTION: - case SBG_ECOM_LOG_SHIP_MOTION_HP: - errorCode = sbgEComBinaryLogParseShipMotionData(&inputStream, &pOutputData->shipMotionData); - break; - case SBG_ECOM_LOG_ODO_VEL: - errorCode = sbgEComBinaryLogParseOdometerData(&inputStream, &pOutputData->odometerData); - break; - case SBG_ECOM_LOG_UTC_TIME: - errorCode = sbgEComBinaryLogParseUtcData(&inputStream, &pOutputData->utcData); - break; - case SBG_ECOM_LOG_GPS1_VEL: - case SBG_ECOM_LOG_GPS2_VEL: - errorCode = sbgEComBinaryLogParseGpsVelData(&inputStream, &pOutputData->gpsVelData); - break; - case SBG_ECOM_LOG_GPS1_POS: - case SBG_ECOM_LOG_GPS2_POS: - errorCode = sbgEComBinaryLogParseGpsPosData(&inputStream, &pOutputData->gpsPosData); - break; - case SBG_ECOM_LOG_GPS1_HDT: - case SBG_ECOM_LOG_GPS2_HDT: - errorCode = sbgEComBinaryLogParseGpsHdtData(&inputStream, &pOutputData->gpsHdtData); - break; - case SBG_ECOM_LOG_GPS1_RAW: - case SBG_ECOM_LOG_GPS2_RAW: - errorCode = sbgEComBinaryLogParseGpsRawData(&inputStream, &pOutputData->gpsRawData); - break; - case SBG_ECOM_LOG_GPS1_SAT: - case SBG_ECOM_LOG_GPS2_SAT: - errorCode = sbgEComBinaryLogParseSatGroupData(&inputStream, &pOutputData->satGroupData); - break; - case SBG_ECOM_LOG_RTCM_RAW: - errorCode = sbgEComBinaryLogParseRtcmRawData(&inputStream, &pOutputData->rtcmRawData); - break; - case SBG_ECOM_LOG_MAG: - errorCode = sbgEComBinaryLogParseMagData(&inputStream, &pOutputData->magData); - break; - case SBG_ECOM_LOG_MAG_CALIB: - errorCode = sbgEComBinaryLogParseMagCalibData(&inputStream, &pOutputData->magCalibData); - break; - case SBG_ECOM_LOG_DVL_BOTTOM_TRACK: - errorCode = sbgEComBinaryLogParseDvlData(&inputStream, &pOutputData->dvlData); - break; - case SBG_ECOM_LOG_DVL_WATER_TRACK: - errorCode = sbgEComBinaryLogParseDvlData(&inputStream, &pOutputData->dvlData); - break; - case SBG_ECOM_LOG_AIR_DATA: - errorCode = sbgEComBinaryLogParseAirData(&inputStream, &pOutputData->airData); - break; - case SBG_ECOM_LOG_USBL: - errorCode = sbgEComBinaryLogParseUsblData(&inputStream, &pOutputData->usblData); - break; - case SBG_ECOM_LOG_DEPTH: - errorCode = sbgEComBinaryLogParseDepth(&inputStream, &pOutputData->depthData); - break; - case SBG_ECOM_LOG_EVENT_A: - case SBG_ECOM_LOG_EVENT_B: - case SBG_ECOM_LOG_EVENT_C: - case SBG_ECOM_LOG_EVENT_D: - case SBG_ECOM_LOG_EVENT_E: - case SBG_ECOM_LOG_EVENT_OUT_A: - case SBG_ECOM_LOG_EVENT_OUT_B: - errorCode = sbgEComBinaryLogParseEvent(&inputStream, &pOutputData->eventMarker); - break; - case SBG_ECOM_LOG_DIAG: - errorCode = sbgEComBinaryLogParseDiagData(&inputStream, &pOutputData->diagData); - break; - - default: - errorCode = SBG_ERROR; - } - } - else if (msgClass == SBG_ECOM_CLASS_LOG_ECOM_1) - { - // - // Parse the message depending on the message ID - // - switch ((SbgEComLog1)msg) - { - case SBG_ECOM_LOG_FAST_IMU_DATA: - errorCode = sbgEComBinaryLogParseFastImuData(&inputStream, &pOutputData->fastImuData); - break; - default: - errorCode = SBG_ERROR; - } - } - else - { - // - // Un-handled message class - // - errorCode = SBG_ERROR; - } - - return errorCode; -} - -void sbgEComBinaryLogCleanup(SbgBinaryLogData *pLogData, SbgEComClass msgClass, SbgEComMsgId msgId) -{ - assert(pLogData); - - if (msgClass == SBG_ECOM_CLASS_LOG_ECOM_0) - { - switch (msgId) - { - case SBG_ECOM_LOG_GPS1_SAT: - case SBG_ECOM_LOG_GPS2_SAT: - sbgLogSatGroupDataDestroy(&pLogData->satGroupData); - break; - } - } -} diff --git a/src/binaryLogs/sbgEComBinaryLogs.h b/src/binaryLogs/sbgEComBinaryLogs.h deleted file mode 100644 index 9782a16..0000000 --- a/src/binaryLogs/sbgEComBinaryLogs.h +++ /dev/null @@ -1,136 +0,0 @@ -/*! - * \file sbgEComBinaryLogs.h - * \ingroup binaryLogs - * \author SBG Systems - * \date 06 February 2013 - * - * \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 - * - * 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 binaryLogs Binary Logs - * \brief All messages and logs that can be output by the device. - */ - -#ifndef SBG_ECOM_BINARY_LOGS_H -#define SBG_ECOM_BINARY_LOGS_H - - // sbgCommonLib headers -#include - -// Project headers -#include - -// Local headers -#include "sbgEComBinaryLogAirData.h" -#include "sbgEComBinaryLogDepth.h" -#include "sbgEComBinaryLogDiag.h" -#include "sbgEComBinaryLogDvl.h" -#include "sbgEComBinaryLogEkf.h" -#include "sbgEComBinaryLogEvent.h" -#include "sbgEComBinaryLogGps.h" -#include "sbgEComBinaryLogImu.h" -#include "sbgEComBinaryLogMag.h" -#include "sbgEComBinaryLogOdometer.h" -#include "sbgEComBinaryLogRawData.h" -#include "sbgEComBinaryLogRtcm.h" -#include "sbgEComBinaryLogSat.h" -#include "sbgEComBinaryLogShipMotion.h" -#include "sbgEComBinaryLogStatus.h" -#include "sbgEComBinaryLogUsbl.h" -#include "sbgEComBinaryLogUtc.h" - -#ifdef __cplusplus -extern "C" { -#endif - -//----------------------------------------------------------------------// -//- Log structure definitions -// -//----------------------------------------------------------------------// - -/*! - * Union used to store received logs data. - */ -typedef union _SbgBinaryLogData -{ - SbgLogStatusData statusData; /*!< Stores data for the SBG_ECOM_LOG_STATUS message. */ - SbgLogImuData imuData; /*!< Stores data for the SBG_ECOM_LOG_IMU_DATA message. */ - SbgLogImuShort imuShort; /*!< Stores data for the SBG_ECOM_LOG_IMU_SHORT message. */ - SbgLogEkfEulerData ekfEulerData; /*!< Stores data for the SBG_ECOM_LOG_EKF_EULER message. */ - SbgLogEkfQuatData ekfQuatData; /*!< Stores data for the SBG_ECOM_LOG_EKF_QUAT message. */ - SbgLogEkfNavData ekfNavData; /*!< Stores data for the SBG_ECOM_LOG_EKF_NAV message. */ - SbgLogShipMotionData shipMotionData; /*!< Stores data for the SBG_ECOM_LOG_SHIP_MOTION or SBG_ECOM_LOG_SHIP_MOTION_HP message. */ - SbgLogOdometerData odometerData; /*!< Stores data for the SBG_ECOM_LOG_ODO_VEL message. */ - SbgLogUtcData utcData; /*!< Stores data for the SBG_ECOM_LOG_UTC_TIME message. */ - SbgLogGpsPos gpsPosData; /*!< Stores data for the SBG_ECOM_LOG_GPS_POS message. */ - SbgLogGpsVel gpsVelData; /*!< Stores data for the SBG_ECOM_LOG_GPS#_VEL message. */ - SbgLogGpsHdt gpsHdtData; /*!< Stores data for the SBG_ECOM_LOG_GPS#_HDT message. */ - SbgLogRawData gpsRawData; /*!< Stores data for the SBG_ECOM_LOG_GPS#_RAW message. */ - SbgLogRawData rtcmRawData; /*!< Stores data for the SBG_ECOM_LOG_RTCM_RAW message. */ - SbgLogMag magData; /*!< Stores data for the SBG_ECOM_LOG_MAG message. */ - SbgLogMagCalib magCalibData; /*!< Stores data for the SBG_ECOM_LOG_MAG_CALIB message. */ - SbgLogDvlData dvlData; /*!< Stores data for the SBG_ECOM_LOG_DVL_BOTTOM_TRACK message. */ - SbgLogAirData airData; /*!< Stores data for the SBG_ECOM_LOG_AIR_DATA message. */ - SbgLogUsblData usblData; /*!< Stores data for the SBG_ECOM_LOG_USBL message. */ - SbgLogDepth depthData; /*!< Stores data for the SBG_ECOM_LOG_DEPTH message */ - SbgLogEvent eventMarker; /*!< Stores data for the SBG_ECOM_LOG_EVENT_# message. */ - SbgLogDiagData diagData; /*!< Stores data for the SBG_ECOM_LOG_DIAG message. */ - SbgLogSatGroupData satGroupData; /*!< Stores data for the SBG_ECOM_LOG_SAT message. */ - - /* Fast logs */ - SbgLogFastImuData fastImuData; /*!< Stores Fast Imu Data for 1KHz output */ - -} SbgBinaryLogData; - -//----------------------------------------------------------------------// -//- Public methods -// -//----------------------------------------------------------------------// - -/*! - * Parse an incoming log and fill the output union. - * - * \param[in] msgClass Received message class - * \param[in] msg Received message ID - * \param[in] pPayload Read only pointer on the payload buffer. - * \param[in] payloadSize Payload size in bytes. - * \param[out] pOutputData Pointer on the output union that stores parsed data. - */ -SbgErrorCode sbgEComBinaryLogParse(SbgEComClass msgClass, SbgEComMsgId msg, const void *pPayload, size_t payloadSize, SbgBinaryLogData *pOutputData); - -/*! - * Clean up resources allocated during parsing, if any. - * - * \param[in] pLogData Log data. - * \param[in] msgClass Message class. - * \param[in] msgId Message ID. - */ -void sbgEComBinaryLogCleanup(SbgBinaryLogData *pLogData, SbgEComClass msgClass, SbgEComMsgId msgId); - -#ifdef __cplusplus -} -#endif - -#endif // SBG_ECOM_BINARY_LOGS_H diff --git a/src/commands/sbgEComCmdAdvanced.h b/src/commands/sbgEComCmdAdvanced.h index 23a7415..d5fa38e 100644 --- a/src/commands/sbgEComCmdAdvanced.h +++ b/src/commands/sbgEComCmdAdvanced.h @@ -44,7 +44,7 @@ extern "C" { #endif //----------------------------------------------------------------------// -//- Advanced definitions -// +//- Advanced definitions -// //----------------------------------------------------------------------// /*! @@ -69,7 +69,7 @@ typedef enum _SbgEComTimeReferenceSrc #define SBG_ECOM_NMEA_OPT_FORCE_UTC (uint32_t)(0x00000001 << 4) /*!< Always output time in NMEA messages even if invalid */ //----------------------------------------------------------------------// -//- Advanced configurations -// +//- Advanced configurations -// //----------------------------------------------------------------------// /*! diff --git a/src/commands/sbgEComCmdApi.c b/src/commands/sbgEComCmdApi.c index ebb832a..4766a9b 100644 --- a/src/commands/sbgEComCmdApi.c +++ b/src/commands/sbgEComCmdApi.c @@ -88,7 +88,7 @@ static SbgErrorCode sbgEComCmdApiReplyParsePayload(SbgEComCmdApiReply *pReply) //- Public methods -// //----------------------------------------------------------------------// -void sbgEComCmdApiReplyConstruct(SbgEComCmdApiReply *pReply) //TODO: minuscule ds un commit séparé +void sbgEComCmdApiReplyConstruct(SbgEComCmdApiReply *pReply) { assert(pReply); @@ -98,7 +98,7 @@ void sbgEComCmdApiReplyConstruct(SbgEComCmdApiReply *pReply) //TODO: minuscule d pReply->pContent = NULL; } -void sbgEComCmdApiReplyDestroy(SbgEComCmdApiReply *pReply) //TODO: minuscule ds un commit séparé +void sbgEComCmdApiReplyDestroy(SbgEComCmdApiReply *pReply) { assert(pReply); diff --git a/src/commands/sbgEComCmdCommon.c b/src/commands/sbgEComCmdCommon.c index 99d09c7..816f278 100644 --- a/src/commands/sbgEComCmdCommon.c +++ b/src/commands/sbgEComCmdCommon.c @@ -91,15 +91,15 @@ SbgErrorCode sbgEComReceiveAnyCmd2(SbgEComHandle *pHandle, uint8_t *pMsgClass, u { if (pHandle->pReceiveLogCallback) { - SbgBinaryLogData logData; + SbgEComLogUnion logData; - errorCode = sbgEComBinaryLogParse((SbgEComClass)receivedMsgClass, receivedMsgId, sbgEComProtocolPayloadGetBuffer(pPayload), sbgEComProtocolPayloadGetSize(pPayload), &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); - sbgEComBinaryLogCleanup(&logData, (SbgEComClass)receivedMsgClass, (SbgEComMsgId)receivedMsgId); + sbgEComLogCleanup(&logData, (SbgEComClass)receivedMsgClass, (SbgEComMsgId)receivedMsgId); } } } @@ -118,9 +118,12 @@ SbgErrorCode sbgEComReceiveAnyCmd2(SbgEComHandle *pHandle, uint8_t *pMsgClass, u 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); @@ -136,7 +139,15 @@ SbgErrorCode sbgEComReceiveAnyCmd2(SbgEComHandle *pHandle, uint8_t *pMsgClass, u } else { - errorCode = SBG_NOT_READY; + if (errorCode == SBG_NO_ERROR) + { + errorCode = SBG_TIME_OUT; + } + else + { + errorCode = SBG_NOT_READY; + } + break; } } @@ -227,9 +238,8 @@ SbgErrorCode sbgEComReceiveCmd2(SbgEComHandle *pHandle, uint8_t msgClass, uint8_ if ((errorCode == SBG_NO_ERROR) && (ackMsgClass == msgClass) && (ackMsgId == msgId)) { // - // If a successful ACK is expected, the caller should instead explicitely wait for - // it. As a result, consider receiving a "successful ACK" instead of an actual message - // with the requested class/ID an error. + // 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) { @@ -248,11 +258,7 @@ SbgErrorCode sbgEComReceiveCmd2(SbgEComHandle *pHandle, uint8_t msgClass, uint8_ { sbgSleep(1); } - else - { - break; - } - + now = sbgGetTime(); if ((now - start) >= timeOut) @@ -281,13 +287,10 @@ SbgErrorCode sbgEComWaitForAck(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t sbgEComProtocolPayloadConstruct(&receivedPayload); // - // Try to receive the ACK + // 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); - // - // Test if an ACK frame has been received - // if (errorCode == SBG_NO_ERROR) { // @@ -303,7 +306,7 @@ SbgErrorCode sbgEComWaitForAck(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t 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 + // 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); @@ -311,25 +314,18 @@ SbgErrorCode sbgEComWaitForAck(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t if ((ackMsg == msg) && (ackClass == msgClass)) { - // - // Parse the error code and return it - // errorCode = (SbgErrorCode)sbgStreamBufferReadUint16LE(&inputStream); } else { - // - // We have received an ACK but not for this frame! - // errorCode = SBG_INVALID_FRAME; + SBG_LOG_WARNING(errorCode, "received ACK mismatch. expecting %#"PRIx8":%#"PRIx8" got %#"PRIx8":%#"PRIx8, msgClass, msg, ackClass, ackMsg); } } else { - // - // The ACK is invalid - // errorCode = SBG_INVALID_FRAME; + SBG_LOG_WARNING(errorCode, "payload size is invalid for an ACK."); } } diff --git a/src/commands/sbgEComCmdCommon.h b/src/commands/sbgEComCmdCommon.h index 422265b..d0e1d1d 100644 --- a/src/commands/sbgEComCmdCommon.h +++ b/src/commands/sbgEComCmdCommon.h @@ -44,7 +44,7 @@ extern "C" { #endif //----------------------------------------------------------------------// -//- Defintions -// +//- Definitions -// //----------------------------------------------------------------------// #define SBG_ECOM_DEFAULT_CMD_TIME_OUT (500) /*!< Default time out in ms for commands reception. */ @@ -89,9 +89,9 @@ typedef enum _SbgEComAxisDirection * \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_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, - * SBG_TIME_OUT if no command message was received within the specified time out. */ SbgErrorCode sbgEComReceiveAnyCmd(SbgEComHandle *pHandle, uint8_t *pMsgClass, uint8_t *pMsgId, void *pData, size_t *pSize, size_t maxSize, uint32_t timeOut); @@ -114,9 +114,9 @@ SbgErrorCode sbgEComReceiveAnyCmd(SbgEComHandle *pHandle, uint8_t *pMsgClass, ui * \param[out] pMsgId 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. + * \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); @@ -164,7 +164,7 @@ SbgErrorCode sbgEComReceiveCmd(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t * \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_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. */ diff --git a/src/commands/sbgEComCmdEthernet.c b/src/commands/sbgEComCmdEthernet.c index 5fd9f40..ea89606 100644 --- a/src/commands/sbgEComCmdEthernet.c +++ b/src/commands/sbgEComCmdEthernet.c @@ -90,7 +90,7 @@ SbgErrorCode sbgEComEthernetGetConf(SbgEComHandle *pHandle, SbgEComEthernetConf for (trial = 0; trial < pHandle->numTrials; trial++) { // - // Send the command with no payload to retreive the network configuration + // 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); @@ -145,7 +145,7 @@ SbgErrorCode sbgEComEthernetSetConf(SbgEComHandle *pHandle, const SbgEComEtherne { SbgErrorCode errorCode = SBG_NO_ERROR; uint32_t trial; - uint8_t outputBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; + uint8_t outputBuffer[24]; SbgStreamBuffer outputStream; assert(pHandle); @@ -224,7 +224,7 @@ SbgErrorCode sbgEComEthernetInfo(SbgEComHandle *pHandle, SbgEComEthernetConf *pE for (trial = 0; trial < pHandle->numTrials; trial++) { // - // Send the command with no payload to retreive the network configuration + // 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); diff --git a/src/commands/sbgEComCmdEthernet.h b/src/commands/sbgEComCmdEthernet.h index b0b98cc..6044fb5 100644 --- a/src/commands/sbgEComCmdEthernet.h +++ b/src/commands/sbgEComCmdEthernet.h @@ -44,7 +44,7 @@ extern "C" { #endif //----------------------------------------------------------------------// -//- Ethernet interface configuration -// +//- Ethernet interface configuration -// //----------------------------------------------------------------------// /*! @@ -61,7 +61,7 @@ typedef enum _SbgEComEthernetMode */ typedef struct _SbgEComEthernetConf { - SbgEComEthernetMode mode; /*!< Define how the device will acquiere its IP address, either DHCP or Static. */ + 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. */ @@ -76,11 +76,11 @@ 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. - * You should rather use sbgEComEthernetInfo to retreive the current assigned IP. + * Warning: 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 Poiner to a SbgEComEthernetConf struct that holds the read configuration from the device. + * \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); @@ -89,19 +89,19 @@ SbgErrorCode sbgEComEthernetGetConf(SbgEComHandle *pHandle, SbgEComEthernetConf * Set the configuration for the Ethernet interface. * * \param[in] pHandle A valid sbgECom handle. - * \param[in] pEthernetConf Poiner to a SbgEComEthernetConf struct that holds the new configuration to apply. + * \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); /*! - * Get the current assigned and used IP address as well as network inforamtion. + * Get the current assigned and used IP address as well as network information. * * In opposition to sbgEComEthernetGetConf, this method will not return the Ethernet configuration. * It will rather return the IP address currently used by the device. * * \param[in] pHandle A valid sbgECom handle. - * \param[out] pEthernetConf Poiner to a SbgEComEthernetConf struct that holds the read IP settings from the device. + * \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 0766cdb..6303e6e 100644 --- a/src/commands/sbgEComCmdEvent.c +++ b/src/commands/sbgEComCmdEvent.c @@ -90,7 +90,7 @@ SbgErrorCode sbgEComCmdSyncInSetConf(SbgEComHandle *pHandle, SbgEComSyncInId syn { SbgErrorCode errorCode = SBG_NO_ERROR; uint32_t trial; - uint8_t outputBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; + uint8_t outputBuffer[8]; SbgStreamBuffer outputStream; assert(pHandle); @@ -230,7 +230,7 @@ SbgErrorCode sbgEComCmdSyncOutSetConf(SbgEComHandle *pHandle, SbgEComSyncOutId s { SbgErrorCode errorCode = SBG_NO_ERROR; uint32_t trial; - uint8_t outputBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; + uint8_t outputBuffer[16]; SbgStreamBuffer outputStream; assert(pHandle); diff --git a/src/commands/sbgEComCmdEvent.h b/src/commands/sbgEComCmdEvent.h index 240ff47..8d6b955 100644 --- a/src/commands/sbgEComCmdEvent.h +++ b/src/commands/sbgEComCmdEvent.h @@ -44,7 +44,7 @@ extern "C" { #endif //----------------------------------------------------------------------// -//- Event definitions -// +//- Event definitions -// //----------------------------------------------------------------------// /*! @@ -99,7 +99,9 @@ typedef enum _SbgEComSyncOutFunction SBG_ECOM_SYNC_OUT_MODE_EVENT_IN_D = 10006, /*!< Output is generated on a Sync In D event. */ SBG_ECOM_SYNC_OUT_MODE_DIRECT_PPS = 10100, /*!< The internal GNSS PPS signal is directly routed to the Sync Out. - This mode is only valid for ELLIPSE-N with hardware revisions above 1.2.1.0. + 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; @@ -115,7 +117,7 @@ typedef enum _SbgEComSyncOutPolarity } SbgEComSyncOutPolarity; //----------------------------------------------------------------------// -//- Event configurations -// +//- Event configurations -// //----------------------------------------------------------------------// /*! @@ -124,7 +126,7 @@ 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 us)*/ + int32_t delay; /*!< Delay to take into account for the sync in. (in ns)*/ } SbgEComSyncInConf; /*! diff --git a/src/commands/sbgEComCmdFeatures.h b/src/commands/sbgEComCmdFeatures.h index 080370e..0e311db 100644 --- a/src/commands/sbgEComCmdFeatures.h +++ b/src/commands/sbgEComCmdFeatures.h @@ -100,8 +100,8 @@ typedef enum _SbgEComGnssType 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_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; diff --git a/src/commands/sbgEComCmdGnss.c b/src/commands/sbgEComCmdGnss.c index 9dd1c8b..91d28c6 100644 --- a/src/commands/sbgEComCmdGnss.c +++ b/src/commands/sbgEComCmdGnss.c @@ -32,7 +32,7 @@ static SbgErrorCode sbgEComCmdGnssSetModelId(SbgEComHandle *pHandle, SbgEComGnss * Retrieve GNSS error model id. * * \param[in] pHandle A valid sbgECom handle. - * \param[out] pModelId Retreived model id. + * \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. */ diff --git a/src/commands/sbgEComCmdInfo.h b/src/commands/sbgEComCmdInfo.h index 26abc4e..9ec2004 100644 --- a/src/commands/sbgEComCmdInfo.h +++ b/src/commands/sbgEComCmdInfo.h @@ -44,14 +44,14 @@ extern "C" { #endif //----------------------------------------------------------------------// -//- Device info definitions -// +//- Device info definitions -// //----------------------------------------------------------------------// /* Misc */ #define SBG_ECOM_INFO_PRODUCT_CODE_LENGTH (32) //----------------------------------------------------------------------// -//- Device Info structure -// +//- Device Info structure -// //----------------------------------------------------------------------// /*! diff --git a/src/commands/sbgEComCmdInterface.h b/src/commands/sbgEComCmdInterface.h index c2a69b5..97567c9 100644 --- a/src/commands/sbgEComCmdInterface.h +++ b/src/commands/sbgEComCmdInterface.h @@ -44,7 +44,7 @@ extern "C" { #endif //----------------------------------------------------------------------// -//- Serial interface definitions -// +//- Serial interface definitions -// //----------------------------------------------------------------------// /*! @@ -78,7 +78,7 @@ typedef enum _SbgEComPortMode } SbgEComPortMode; //----------------------------------------------------------------------// -//- Serial interface configuration -// +//- Serial interface configuration -// //----------------------------------------------------------------------// /*! @@ -91,7 +91,7 @@ typedef struct _SbgEComInterfaceConf } SbgEComInterfaceConf; //----------------------------------------------------------------------// -//- CAN interface definitions -// +//- CAN interface definitions -// //----------------------------------------------------------------------// /*! diff --git a/src/commands/sbgEComCmdMag.c b/src/commands/sbgEComCmdMag.c index 1bf5470..f3c25cd 100644 --- a/src/commands/sbgEComCmdMag.c +++ b/src/commands/sbgEComCmdMag.c @@ -10,7 +10,7 @@ #include "sbgEComCmdMag.h" //----------------------------------------------------------------------// -//- Magnetometer commands -// +//- Magnetometer commands -// //----------------------------------------------------------------------// SbgErrorCode sbgEComCmdMagSetModelId(SbgEComHandle *pHandle, SbgEComMagModelsStdId modelId) @@ -248,7 +248,7 @@ SbgErrorCode sbgEComCmdMagSetRejection(SbgEComHandle *pHandle, const SbgEComMagR } //----------------------------------------------------------------------// -//- Magnetometer onboard calibration commands -// +//- Magnetometer on-board calibration commands -// //----------------------------------------------------------------------// SbgErrorCode sbgEComCmdMagStartCalib(SbgEComHandle *pHandle, SbgEComMagCalibMode mode, SbgEComMagCalibBandwidth bandwidth) @@ -266,7 +266,7 @@ SbgErrorCode sbgEComCmdMagStartCalib(SbgEComHandle *pHandle, SbgEComMagCalibMode errorCode = sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); // - // Write the calibration mode and bandwith + // Write the calibration mode and bandwidth // sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)mode); sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)bandwidth); @@ -347,7 +347,7 @@ SbgErrorCode sbgEComCmdMagComputeCalib(SbgEComHandle *pHandle, SbgEComMagCalibRe if (errorCode == SBG_NO_ERROR) { // - // Try to read the device answer for 5 s because the onboard magnetic computation can take some time + // 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); diff --git a/src/commands/sbgEComCmdMag.h b/src/commands/sbgEComCmdMag.h index bb708b9..95db273 100644 --- a/src/commands/sbgEComCmdMag.h +++ b/src/commands/sbgEComCmdMag.h @@ -4,7 +4,7 @@ * \author SBG Systems * \date 11 June 2014 * - * \brief Magnetometer aiding module configuration & onboard 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 @@ -47,11 +47,11 @@ extern "C" { #endif //----------------------------------------------------------------------// -//- Magnetometer definitions -// +//- Magnetometer definitions -// //----------------------------------------------------------------------// /*! - * Define if the onboard magnetic calibration should acquiere points for a 3D or 2D calibration. + * Define if the on-board magnetic calibration should acquire points for a 3D or 2D calibration. */ typedef enum _SbgEComMagCalibMode { @@ -74,7 +74,7 @@ typedef enum _SbgEComMagCalibBandwidth } SbgEComMagCalibBandwidth; /*! - * General quality indicator of an onboard magnetic calibration. + * General quality indicator of an on-board magnetic calibration. */ typedef enum _SbgEComMagCalibQuality { @@ -85,7 +85,7 @@ typedef enum _SbgEComMagCalibQuality } SbgEComMagCalibQuality; /*! - * Confidence indicator on results of an onbard magnetic calibration. + * Confidence indicator on results of an on-bard magnetic calibration. */ typedef enum _SbgEComMagCalibConfidence { @@ -97,7 +97,7 @@ typedef enum _SbgEComMagCalibConfidence } SbgEComMagCalibConfidence; /*! - * Status bit masks used to report advanced inforamtion on the onboard 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. */ @@ -116,7 +116,7 @@ typedef enum _SbgEComMagModelsStdIds } SbgEComMagModelsStdId; //----------------------------------------------------------------------// -//- Magnetometer configuration -// +//- Magnetometer configuration -// //----------------------------------------------------------------------// /*! @@ -128,7 +128,7 @@ typedef struct _SbgEComMagRejectionConf } SbgEComMagRejectionConf; /*! - * Helper structure to retrieve onboard magnetic calibration results. + * Helper structure to retrieve on-board magnetic calibration results. */ typedef struct _SbgEComMagCalibResults { @@ -155,7 +155,7 @@ typedef struct _SbgEComMagCalibResults } SbgEComMagCalibResults; //----------------------------------------------------------------------// -//- Magnetometer commands -// +//- Magnetometer commands -// //----------------------------------------------------------------------// /*! @@ -205,7 +205,7 @@ SbgErrorCode sbgEComCmdMagSetRejection(SbgEComHandle *pHandle, const SbgEComMagR SbgErrorCode sbgEComCmdMagSetCalibData(SbgEComHandle *pHandle, const float *pOffset, const float *pMatrix); //----------------------------------------------------------------------// -//- Magnetometer onboard calibration commands -// +//- Magnetometer on-board calibration commands -// //----------------------------------------------------------------------// /*! @@ -227,7 +227,7 @@ 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 onboard magnetic calibration results and status. + * \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); diff --git a/src/commands/sbgEComCmdOdo.h b/src/commands/sbgEComCmdOdo.h index ae9e005..defd28e 100644 --- a/src/commands/sbgEComCmdOdo.h +++ b/src/commands/sbgEComCmdOdo.h @@ -70,7 +70,7 @@ typedef struct _SbgEComOdoRejectionConf /*! * CAN odometer channels definition - * A channel is an inforamtion that can be decoded / used by the device. + * A channel is an information that can be decoded / used by the device. */ typedef enum _SbgEComCmdOdoCanChannel { @@ -166,7 +166,7 @@ SbgErrorCode sbgEComCmdOdoSetRejection(SbgEComHandle *pHandle, const SbgEComOdoR * Retrieve the CAN odometer configuration for a specific CAN information channel * * \param[in] pHandle A valid sbgECom handle. - * \param[in] canChannel The CAN channel to retreive associated DBC configuration. + * \param[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. */ diff --git a/src/commands/sbgEComCmdOutput.h b/src/commands/sbgEComCmdOutput.h index 6c6bdbe..e1e8919 100644 --- a/src/commands/sbgEComCmdOutput.h +++ b/src/commands/sbgEComCmdOutput.h @@ -44,11 +44,11 @@ extern "C" { #endif //----------------------------------------------------------------------// -//- Public definitions -// +//- Public definitions -// //----------------------------------------------------------------------// /*! - * List of ouput ports available. + * List of output ports available. */ typedef enum _SbgEComOutputPort { @@ -101,7 +101,7 @@ typedef enum _SbgEComOutputMonitoringPoint //----------------------------------------------------------------------// /*! - * Retrieve the configuration of one the message on one of the output interfaces. + * 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. @@ -113,7 +113,7 @@ typedef enum _SbgEComOutputMonitoringPoint SbgErrorCode sbgEComCmdOutputGetConf(SbgEComHandle *pHandle, SbgEComOutputPort outputPort, SbgEComClass classId, SbgEComMsgId msgId, SbgEComOutputMode *pMode); /*! - * Set the configuration of one the message on one of the output interfaces. + * 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. @@ -125,7 +125,7 @@ SbgErrorCode sbgEComCmdOutputGetConf(SbgEComHandle *pHandle, SbgEComOutputPort o SbgErrorCode sbgEComCmdOutputSetConf(SbgEComHandle *pHandle, SbgEComOutputPort outputPort, SbgEComClass classId, SbgEComMsgId msgId, SbgEComOutputMode mode); /*! - * Retrieve the enable of one of the output class message on one of the output interfaces. + * 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. @@ -136,7 +136,7 @@ SbgErrorCode sbgEComCmdOutputSetConf(SbgEComHandle *pHandle, SbgEComOutputPort o SbgErrorCode sbgEComCmdOutputClassGetEnable(SbgEComHandle *pHandle, SbgEComOutputPort outputPort, SbgEComClass classId, bool *pEnable); /*! - * Set the enable of one of the output class message on one of the output interfaces. + * 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. @@ -147,7 +147,7 @@ SbgErrorCode sbgEComCmdOutputClassGetEnable(SbgEComHandle *pHandle, SbgEComOutpu SbgErrorCode sbgEComCmdOutputClassSetEnable(SbgEComHandle *pHandle, SbgEComOutputPort outputPort, SbgEComClass classId, bool enable); /*! - * Retrieve the configuration of one the message on the CAN interface. + * Retrieve the configuration of one message for the CAN interface. * * \param[in] pHandle A valid sbgECom handle. * \param[in] internalId The internal message id. @@ -159,7 +159,7 @@ SbgErrorCode sbgEComCmdOutputClassSetEnable(SbgEComHandle *pHandle, SbgEComOutpu SbgErrorCode sbgEComCmdCanOutputGetConf(SbgEComHandle *pHandle, SbgECanMessageId internalId, SbgEComOutputMode *pMode, uint32_t *pUserId, bool *pExtended); /*! - * Set the configuration of one the message on the CAN interface + * Set the configuration of one message for the CAN interface. * * \param[in] pHandle A valid sbgECom handle. * \param[in] internalId The internal message id. @@ -171,21 +171,21 @@ SbgErrorCode sbgEComCmdCanOutputGetConf(SbgEComHandle *pHandle, SbgECanMessageId SbgErrorCode sbgEComCmdCanOutputSetConf(SbgEComHandle *pHandle, SbgECanMessageId internalId, SbgEComOutputMode mode, uint32_t userId, bool extended); /*! - * Retrieve the NMEA talker id of one of the output interfaces. + * 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. + * \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 of one of the output interfaces. + * 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 containint the new nmea talker id. + * \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.h b/src/commands/sbgEComCmdSensor.h index 218c671..77970e3 100644 --- a/src/commands/sbgEComCmdSensor.h +++ b/src/commands/sbgEComCmdSensor.h @@ -4,7 +4,7 @@ * \author SBG Systems * \date 11 June 2014 * - * \brief Motion profile, aiding assignement & 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 @@ -47,7 +47,7 @@ extern "C" { #endif //----------------------------------------------------------------------// -//- Sensor definitions -// +//- Sensor definitions -// //----------------------------------------------------------------------// /*! @@ -100,7 +100,7 @@ typedef enum _SbgEComMotionProfileStdIds 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 odometry */ + 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. */ diff --git a/src/commands/sbgEComCmdSettings.h b/src/commands/sbgEComCmdSettings.h index 9f82ca1..9e33abc 100644 --- a/src/commands/sbgEComCmdSettings.h +++ b/src/commands/sbgEComCmdSettings.h @@ -44,11 +44,11 @@ extern "C" { #endif //----------------------------------------------------------------------// -//- Settings action definition -// +//- Settings action definition -// //----------------------------------------------------------------------// /*! - * Defintion of all the settings actions available. + * Definition of all the settings actions available. */ typedef enum _SbgEComSettingsAction { diff --git a/src/defs/sbgEComDefsGnss.h b/src/defs/sbgEComDefsGnss.h index 295b7bd..f06ef71 100644 --- a/src/defs/sbgEComDefsGnss.h +++ b/src/defs/sbgEComDefsGnss.h @@ -112,7 +112,7 @@ typedef enum _SbgEComSignalId SBG_ECOM_SIGNAL_ID_GALILEO_E6_A = 78, // - // Beidou constellation (100 to 149) + // BeiDou constellation (100 to 149) // SBG_ECOM_SIGNAL_ID_BEIDOU_B1IQ = 100, SBG_ECOM_SIGNAL_ID_BEIDOU_B1I = 101, diff --git a/src/logs/sbgEComLog.c b/src/logs/sbgEComLog.c new file mode 100644 index 0000000..1cdab6b --- /dev/null +++ b/src/logs/sbgEComLog.c @@ -0,0 +1,199 @@ +// sbgCommonLib headers +#include +#include + +// Project headers +#include + +// Local headers +#include "sbgEComLog.h" +#include "sbgEComLogAirData.h" +#include "sbgEComLogDepth.h" +#include "sbgEComLogDiag.h" +#include "sbgEComLogDvl.h" +#include "sbgEComLogEkf.h" +#include "sbgEComLogEkfRotAccel.h" +#include "sbgEComLogEvent.h" +#include "sbgEComLogGnssHdt.h" +#include "sbgEComLogGnssPos.h" +#include "sbgEComLogGnssVel.h" +#include "sbgEComLogImu.h" +#include "sbgEComLogMag.h" +#include "sbgEComLogOdometer.h" +#include "sbgEComLogRawData.h" +#include "sbgEComLogSat.h" +#include "sbgEComLogShipMotion.h" +#include "sbgEComLogStatus.h" +#include "sbgEComLogUsbl.h" +#include "sbgEComLogUtc.h" + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComLogParse(SbgEComClass msgClass, SbgEComMsgId msgId, const void *pPayload, size_t payloadSize, SbgEComLogUnion *pLogData) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgStreamBuffer inputStream; + + 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); + + // + // 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; + + 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; +} + +void sbgEComLogCleanup(SbgEComLogUnion *pLogData, SbgEComClass msgClass, SbgEComMsgId msgId) +{ + assert(pLogData); + + SBG_UNUSED_PARAMETER(pLogData); + SBG_UNUSED_PARAMETER(msgClass); + SBG_UNUSED_PARAMETER(msgId); + + // + // Nothing to do for now + // +} + +//----------------------------------------------------------------------// +//- DEPRECATED - Used for backward compatibility -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComBinaryLogParse(SbgEComClass msgClass, SbgEComMsgId msg, const void *pPayload, size_t payloadSize, SbgEComLogUnion *pLogData) +{ + return sbgEComLogParse(msgClass, msg, pPayload, payloadSize, pLogData); +} diff --git a/src/logs/sbgEComLog.h b/src/logs/sbgEComLog.h new file mode 100644 index 0000000..331237f --- /dev/null +++ b/src/logs/sbgEComLog.h @@ -0,0 +1,148 @@ +/*! + * \file sbgEComLog.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 06 February 2013 + * + * \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 + * + * 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 binaryLogs Binary Logs + * \brief All messages and logs that can be output by the device. + */ + +#ifndef SBG_ECOM_LOG_H +#define SBG_ECOM_LOG_H + + // sbgCommonLib headers +#include + +// Project headers +#include + +// Local headers +#include "sbgEComLogAirData.h" +#include "sbgEComLogDepth.h" +#include "sbgEComLogDiag.h" +#include "sbgEComLogDvl.h" +#include "sbgEComLogEkf.h" +#include "sbgEComLogEkfRotAccel.h" +#include "sbgEComLogEvent.h" +#include "sbgEComLogGnssHdt.h" +#include "sbgEComLogGnssPos.h" +#include "sbgEComLogGnssVel.h" +#include "sbgEComLogImu.h" +#include "sbgEComLogMag.h" +#include "sbgEComLogOdometer.h" +#include "sbgEComLogRawData.h" +#include "sbgEComLogSat.h" +#include "sbgEComLogShipMotion.h" +#include "sbgEComLogStatus.h" +#include "sbgEComLogUsbl.h" +#include "sbgEComLogUtc.h" + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Public definitions -// +//----------------------------------------------------------------------// + +/*! + * 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 */ + +} SbgEComLogUnion; + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * 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. + */ +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. + */ +void sbgEComLogCleanup(SbgEComLogUnion *pLogData, SbgEComClass msgClass, SbgEComMsgId msgId); + +//----------------------------------------------------------------------// +//- DEPRECATED - Used for backward compatibility -// +//----------------------------------------------------------------------// + +SBG_DEPRECATED_TYPEDEF(typedef union _SbgEComLogUnion SbgBinaryLogData); + +SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogParse(SbgEComClass msgClass, SbgEComMsgId msg, const void *pPayload, size_t payloadSize, SbgEComLogUnion *pLogData)); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_LOG_H diff --git a/src/logs/sbgEComLogAirData.c b/src/logs/sbgEComLogAirData.c new file mode 100644 index 0000000..ba0d1aa --- /dev/null +++ b/src/logs/sbgEComLogAirData.c @@ -0,0 +1,68 @@ +#include "sbgEComLogAirData.h" + +//----------------------------------------------------------------------// +//- Operations -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComLogAirDataReadFromStream(SbgEComLogAirData *pLogData, SbgStreamBuffer *pStreamBuffer) +{ + assert(pLogData); + assert(pStreamBuffer); + + pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); + pLogData->status = sbgStreamBufferReadUint16LE(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; + } + + return sbgStreamBufferGetLastError(pStreamBuffer); +} + +SbgErrorCode sbgEComLogAirDataWriteToStream(const SbgEComLogAirData *pLogData, SbgStreamBuffer *pStreamBuffer) +{ + assert(pLogData); + assert(pStreamBuffer); + + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); + sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->status); + + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->pressureAbs); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->altitude); + + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->pressureDiff); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->trueAirspeed); + + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->airTemperature); + + return sbgStreamBufferGetLastError(pStreamBuffer); +} + +//----------------------------------------------------------------------// +//- DEPRECATED - Used for backward compatibility -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComBinaryLogParseAirData(SbgStreamBuffer *pStreamBuffer, SbgEComLogAirData *pLogData) +{ + return sbgEComLogAirDataReadFromStream(pLogData, pStreamBuffer); +} + +SbgErrorCode sbgEComBinaryLogWriteAirData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogAirData *pLogData) +{ + return sbgEComLogAirDataWriteToStream(pLogData, pStreamBuffer); +} diff --git a/src/binaryLogs/sbgEComBinaryLogAirData.h b/src/logs/sbgEComLogAirData.h similarity index 74% rename from src/binaryLogs/sbgEComBinaryLogAirData.h rename to src/logs/sbgEComLogAirData.h index 8909378..2a27901 100644 --- a/src/binaryLogs/sbgEComBinaryLogAirData.h +++ b/src/logs/sbgEComLogAirData.h @@ -1,5 +1,5 @@ /*! - * \file sbgEComBinaryLogAirData.h + * \file sbgEComLogAirData.h * \ingroup binaryLogs * \author SBG Systems * \date 20 February 2019 @@ -33,8 +33,8 @@ * \endlicense */ -#ifndef SBG_ECOM_BINARY_LOG_AIR_DATA_H -#define SBG_ECOM_BINARY_LOG_AIR_DATA_H +#ifndef SBG_ECOM_LOG_AIR_DATA_H +#define SBG_ECOM_LOG_AIR_DATA_H // sbgCommonLib headers #include @@ -65,7 +65,7 @@ extern "C" { /*! * Log structure for AirData. */ -typedef struct _SbgLogAirData +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. */ @@ -74,7 +74,7 @@ typedef struct _SbgLogAirData float pressureDiff; /*!< Raw differential pressure measured by the pitot tube in Pascal. */ float trueAirspeed; /*!< True airspeed measured by a pitot tube in m.s^-1 and positive forward. */ float airTemperature; /*!< Outside air temperature in °C that could be used to compute true airspeed from differential pressure. */ -} SbgLogAirData; +} SbgEComLogAirData; //----------------------------------------------------------------------// //- Public methods -// @@ -82,24 +82,33 @@ typedef struct _SbgLogAirData /*! * Parse data for the SBG_ECOM_LOG_AIR_DATA message and fill the corresponding structure. - * - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. + * + * \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 sbgEComBinaryLogParseAirData(SbgStreamBuffer *pInputStream, SbgLogAirData *pOutputData); +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[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. + * \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 sbgEComBinaryLogWriteAirData(SbgStreamBuffer *pOutputStream, const SbgLogAirData *pInputData); +SbgErrorCode sbgEComLogAirDataWriteToStream(const SbgEComLogAirData *pLogData, SbgStreamBuffer *pStreamBuffer); + +//----------------------------------------------------------------------// +//- DEPRECATED - Used for backward compatibility -// +//----------------------------------------------------------------------// + +SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogAirData SbgLogAirData); + +SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogParseAirData(SbgStreamBuffer *pStreamBuffer, SbgEComLogAirData *pLogData)); +SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogWriteAirData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogAirData *pLogData)); #ifdef __cplusplus } #endif -#endif // SBG_ECOM_BINARY_LOG_AIR_DATA_H +#endif // SBG_ECOM_LOG_AIR_DATA_H diff --git a/src/binaryLogs/sbgEComBinaryLogAutomotiveData.h b/src/logs/sbgEComLogAutomotive.h similarity index 83% rename from src/binaryLogs/sbgEComBinaryLogAutomotiveData.h rename to src/logs/sbgEComLogAutomotive.h index df57e48..589a6a7 100644 --- a/src/binaryLogs/sbgEComBinaryLogAutomotiveData.h +++ b/src/logs/sbgEComLogAutomotive.h @@ -1,5 +1,5 @@ /*! - * \file sbgEComBinaryLogAutomotiveData.h + * \file sbgEComLogAutomotive.h * \ingroup binaryLogs * \author SBG Systems * \date 22 April 2020 @@ -30,8 +30,8 @@ * \endlicense */ -#ifndef SBG_ECOM_BINARY_LOG_AUTOMOTIVE_H -#define SBG_ECOM_BINARY_LOG_AUTOMOTIVE_H +#ifndef SBG_ECOM_LOG_AUTOMOTIVE_H +#define SBG_ECOM_LOG_AUTOMOTIVE_H // sbgCommonLib headers #include @@ -59,16 +59,22 @@ extern "C" { /*! * Log structure for automotive data. */ -typedef struct _SbgLogAutoData +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. */ -} SbgLogAutoData; +} SbgEComLogAutomotive; + +//----------------------------------------------------------------------// +//- DEPRECATED - Used for backward compatibility -// +//----------------------------------------------------------------------// + +SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogAutomotive SbgLogAutoData); #ifdef __cplusplus } #endif -#endif // SBG_ECOM_BINARY_LOG_AUTOMOTIVE_H +#endif // SBG_ECOM_LOG_AUTOMOTIVE_H diff --git a/src/logs/sbgEComLogDepth.c b/src/logs/sbgEComLogDepth.c new file mode 100644 index 0000000..04773e8 --- /dev/null +++ b/src/logs/sbgEComLogDepth.c @@ -0,0 +1,47 @@ +#include "sbgEComLogDepth.h" + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComLogDepthReadFromStream(SbgEComLogDepth *pLogData, SbgStreamBuffer *pStreamBuffer) +{ + assert(pLogData); + assert(pStreamBuffer); + + pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); + pLogData->status = sbgStreamBufferReadUint16LE(pStreamBuffer); + + pLogData->pressureAbs = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->altitude = sbgStreamBufferReadFloatLE(pStreamBuffer); + + return sbgStreamBufferGetLastError(pStreamBuffer); +} + +SbgErrorCode sbgEComLogDepthWriteToStream(const SbgEComLogDepth *pLogData, SbgStreamBuffer *pStreamBuffer) +{ + assert(pLogData); + assert(pStreamBuffer); + + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); + sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->status); + + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->pressureAbs); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->altitude); + + return sbgStreamBufferGetLastError(pStreamBuffer); +} + +//----------------------------------------------------------------------// +//- DEPRECATED - Used for backward compatibility -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComBinaryLogParseDepth(SbgStreamBuffer *pStreamBuffer, SbgEComLogDepth *pLogData) +{ + return sbgEComLogDepthReadFromStream(pLogData, pStreamBuffer); +} + +SbgErrorCode sbgEComBinaryLogWriteDepth(SbgStreamBuffer *pStreamBuffer, const SbgEComLogDepth *pLogData) +{ + return sbgEComLogDepthWriteToStream(pLogData, pStreamBuffer); +} diff --git a/src/binaryLogs/sbgEComBinaryLogDepth.h b/src/logs/sbgEComLogDepth.h similarity index 67% rename from src/binaryLogs/sbgEComBinaryLogDepth.h rename to src/logs/sbgEComLogDepth.h index df142e3..2df87c1 100644 --- a/src/binaryLogs/sbgEComBinaryLogDepth.h +++ b/src/logs/sbgEComLogDepth.h @@ -1,12 +1,12 @@ /*! - * \file sbgEComBinaryLogDepth.h + * \file sbgEComLogDepth.h * \ingroup binaryLogs * \author SBG Systems * \date 20 February 2019 * - * \brief Parse received subsea depth measurement logs. + * \brief Parse received sub-sea depth measurement logs. * - * Depth sensor are used for subsea navigation to improve height. + * 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 @@ -32,8 +32,8 @@ * \endlicense */ -#ifndef SBG_ECOM_BINARY_LOG_DEPTH_H -#define SBG_ECOM_BINARY_LOG_DEPTH_H +#ifndef SBG_ECOM_LOG_DEPTH_H +#define SBG_ECOM_LOG_DEPTH_H // sbgCommonLib headers #include @@ -59,15 +59,15 @@ extern "C" { //----------------------------------------------------------------------// /*! - * Log structure for Depth sensor measurement (subsea). + * Log structure for Depth sensor measurement (sub-sea). */ -typedef struct _SbgLogDepth +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. */ -} SbgLogDepth; +} SbgEComLogDepth; //----------------------------------------------------------------------// //- Public methods -// @@ -75,24 +75,33 @@ typedef struct _SbgLogDepth /*! * Parse data for the SBG_ECOM_LOG_DEPTH message and fill the corresponding structure. - * - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. + * + * \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 sbgEComBinaryLogParseDepth(SbgStreamBuffer *pInputStream, SbgLogDepth *pOutputData); +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[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. + * \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 sbgEComBinaryLogWriteDepth(SbgStreamBuffer *pOutputStream, const SbgLogDepth *pInputData); +SbgErrorCode sbgEComLogDepthWriteToStream(const SbgEComLogDepth *pLogData, SbgStreamBuffer *pStreamBuffer); + +//----------------------------------------------------------------------// +//- DEPRECATED - Used for backward compatibility -// +//----------------------------------------------------------------------// + +SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogDepth SbgLogDepth); + +SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogParseDepth(SbgStreamBuffer *pStreamBuffer, SbgEComLogDepth *pLogData)); +SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogWriteDepth(SbgStreamBuffer *pStreamBuffer, const SbgEComLogDepth *pLogData)); #ifdef __cplusplus } #endif -#endif // SBG_ECOM_BINARY_LOG_DEPTH_H +#endif // SBG_ECOM_LOG_DEPTH_H diff --git a/src/logs/sbgEComLogDiag.c b/src/logs/sbgEComLogDiag.c new file mode 100644 index 0000000..469fe41 --- /dev/null +++ b/src/logs/sbgEComLogDiag.c @@ -0,0 +1,63 @@ +// sbgCommonLib headers +#include +#include + +// Local headers +#include "sbgEComLogDiag.h" + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComLogDiagReadFromStream(SbgEComLogDiagData *pLogData, SbgStreamBuffer *pStreamBuffer) +{ + assert(pLogData); + assert(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'; + + return sbgStreamBufferGetLastError(pStreamBuffer); +} + +SbgErrorCode sbgEComLogDiagWriteToStream(const SbgEComLogDiagData *pLogData, SbgStreamBuffer *pStreamBuffer) +{ + size_t length; + + assert(pLogData); + assert(pStreamBuffer); + + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timestamp); + sbgStreamBufferWriteUint8(pStreamBuffer, pLogData->type); + sbgStreamBufferWriteUint8(pStreamBuffer, pLogData->errorCode); + + length = strlen(pLogData->string); + + if (length >= sizeof(pLogData->string)) + { + length = sizeof(pLogData->string) - 1; + } + + sbgStreamBufferWriteBuffer(pStreamBuffer, pLogData->string, length); + sbgStreamBufferWriteUint8(pStreamBuffer, 0); + + return sbgStreamBufferGetLastError(pStreamBuffer); +} + +//----------------------------------------------------------------------// +//- DEPRECATED - Used for backward compatibility -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComBinaryLogParseDiagData(SbgStreamBuffer *pStreamBuffer, SbgEComLogDiagData *pLogData) +{ + return sbgEComLogDiagReadFromStream(pLogData, pStreamBuffer); +} + +SbgErrorCode sbgEComBinaryLogWriteDiagData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogDiagData *pLogData) +{ + return sbgEComLogDiagWriteToStream(pLogData, pStreamBuffer); +} diff --git a/src/binaryLogs/sbgEComBinaryLogDiag.h b/src/logs/sbgEComLogDiag.h similarity index 63% rename from src/binaryLogs/sbgEComBinaryLogDiag.h rename to src/logs/sbgEComLogDiag.h index 4c8dc07..15cce5b 100644 --- a/src/binaryLogs/sbgEComBinaryLogDiag.h +++ b/src/logs/sbgEComLogDiag.h @@ -1,5 +1,5 @@ /*! - * \file sbgEComBinaryLogDiag.h + * \file sbgEComLogDiag.h * \ingroup binaryLogs * \author SBG Systems * \date 12 June 2019 @@ -30,8 +30,8 @@ * \endlicense */ -#ifndef SBG_ECOM_BINARY_LOG_DIAG_H -#define SBG_ECOM_BINARY_LOG_DIAG_H +#ifndef SBG_ECOM_LOG_DIAG_H +#define SBG_ECOM_LOG_DIAG_H // sbgCommonLib headers #include @@ -60,39 +60,47 @@ extern "C" { /*! * Diagnostic log structure. */ -typedef struct _SbgLogDiagData +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. */ -} SbgLogDiagData; +} SbgEComLogDiagData; //----------------------------------------------------------------------// //- Public methods -// //----------------------------------------------------------------------// /*! - * Parse data for SBG_ECOM_LOG_DIAG messages and fill the corresponding structure. - * - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. + * 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. */ -SbgErrorCode sbgEComBinaryLogParseDiagData(SbgStreamBuffer *pInputStream, SbgLogDiagData *pOutputData); - +SbgErrorCode sbgEComLogDiagReadFromStream(SbgEComLogDiagData *pLogData, SbgStreamBuffer *pStreamBuffer); /*! - * Write data for SBG_ECOM_LOG_DIAG messages to the output stream buffer from the provided structure. + * Write data for the SBG_ECOM_LOG_DIAG message to the output stream buffer from the provided structure. * - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. + * \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 sbgEComBinaryLogWriteDiagData(SbgStreamBuffer *pOutputStream, const SbgLogDiagData *pInputData); +SbgErrorCode sbgEComLogDiagWriteToStream(const SbgEComLogDiagData *pLogData, SbgStreamBuffer *pStreamBuffer); + +//----------------------------------------------------------------------// +//- DEPRECATED - Used for backward compatibility -// +//----------------------------------------------------------------------// + +SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogDiagData SbgLogDiagData); + +SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogParseDiagData(SbgStreamBuffer *pStreamBuffer, SbgEComLogDiagData *pLogData)); +SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogWriteDiagData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogDiagData *pLogData)); #ifdef __cplusplus } #endif -#endif // SBG_ECOM_BINARY_LOG_DIAG_H +#endif // SBG_ECOM_LOG_DIAG_H diff --git a/src/logs/sbgEComLogDvl.c b/src/logs/sbgEComLogDvl.c new file mode 100644 index 0000000..ae8fcfe --- /dev/null +++ b/src/logs/sbgEComLogDvl.c @@ -0,0 +1,57 @@ +#include "sbgEComLogDvl.h" + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComLogDvlReadFromStream(SbgEComLogDvl *pLogData, SbgStreamBuffer *pStreamBuffer) +{ + assert(pLogData); + assert(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->velocityQuality[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->velocityQuality[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->velocityQuality[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); + + return sbgStreamBufferGetLastError(pStreamBuffer); +} + +SbgErrorCode sbgEComLogDvlWriteToStream(const SbgEComLogDvl *pLogData, SbgStreamBuffer *pStreamBuffer) +{ + assert(pLogData); + assert(pStreamBuffer); + + 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->velocityQuality[0]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocityQuality[1]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocityQuality[2]); + + return sbgStreamBufferGetLastError(pStreamBuffer); +} + +//----------------------------------------------------------------------// +//- DEPRECATED - Used for backward compatibility -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComBinaryLogParseDvlData(SbgStreamBuffer *pStreamBuffer, SbgEComLogDvl *pLogData) +{ + return sbgEComLogDvlReadFromStream(pLogData, pStreamBuffer); +} + +SbgErrorCode sbgEComBinaryLogWriteDvlData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogDvl *pLogData) +{ + return sbgEComLogDvlWriteToStream(pLogData, pStreamBuffer); +} diff --git a/src/binaryLogs/sbgEComBinaryLogDvl.h b/src/logs/sbgEComLogDvl.h similarity index 66% rename from src/binaryLogs/sbgEComBinaryLogDvl.h rename to src/logs/sbgEComLogDvl.h index fe7a8bc..ba78db3 100644 --- a/src/binaryLogs/sbgEComBinaryLogDvl.h +++ b/src/logs/sbgEComLogDvl.h @@ -1,5 +1,5 @@ /*! - * \file sbgEComBinaryLogDvl.h + * \file sbgEComLogDvl.h * \ingroup binaryLogs * \author SBG Systems * \date 05 June 2013 @@ -30,8 +30,8 @@ * \endlicense */ -#ifndef SBG_ECOM_BINARY_LOG_DVL_H -#define SBG_ECOM_BINARY_LOG_DVL_H +#ifndef SBG_ECOM_LOG_DVL_H +#define SBG_ECOM_LOG_DVL_H // sbgCommonLib headers #include @@ -56,41 +56,50 @@ extern "C" { //----------------------------------------------------------------------// /*! - * Log structure for DVL data. + * Log structure for DVL 3D velocity measurement. */ -typedef struct _SbgLogDvlData +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. */ -} SbgLogDvlData; +} SbgEComLogDvl; //----------------------------------------------------------------------// //- Public methods -// //----------------------------------------------------------------------// /*! - * Parse data for the SBG_ECOM_LOG_DVL_BOTTOM_TRACK / SBG_ECOM_LOG_DVL_WATER_TRACK message and fill the corresponding structure. + * Parse data for the SBG_ECOM_LOG_DVL_BOTTOM_TRACK / SBG_ECOM_LOG_DVL_WATER_TRACK messages and fill the corresponding structure. * - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. + * \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 sbgEComBinaryLogParseDvlData(SbgStreamBuffer *pInputStream, SbgLogDvlData *pOutputData); +SbgErrorCode sbgEComLogDvlReadFromStream(SbgEComLogDvl *pLogData, SbgStreamBuffer *pStreamBuffer); /*! - * Write data for the SBG_ECOM_LOG_DVL_BOTTOM_TRACK / SBG_ECOM_LOG_DVL_WATER_TRACK message to the output stream buffer from the provided structure. - * - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. + * 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. */ -SbgErrorCode sbgEComBinaryLogWriteDvlData(SbgStreamBuffer *pOutputStream, const SbgLogDvlData *pInputData); +SbgErrorCode sbgEComLogDvlWriteToStream(const SbgEComLogDvl *pLogData, SbgStreamBuffer *pStreamBuffer); + +//----------------------------------------------------------------------// +//- DEPRECATED - Used for backward compatibility -// +//----------------------------------------------------------------------// + +SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogDvl SbgLogDvlData); + +SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogParseDvlData(SbgStreamBuffer *pStreamBuffer, SbgEComLogDvl *pLogData)); +SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogWriteDvlData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogDvl *pLogData)); #ifdef __cplusplus } #endif -#endif // SBG_ECOM_BINARY_LOG_DVL_H +#endif // SBG_ECOM_LOG_DVL_H diff --git a/src/logs/sbgEComLogEkf.c b/src/logs/sbgEComLogEkf.c new file mode 100644 index 0000000..018fa7f --- /dev/null +++ b/src/logs/sbgEComLogEkf.c @@ -0,0 +1,262 @@ +#include "sbgEComLogEkf.h" + +//----------------------------------------------------------------------// +//- Private definitions -// +//----------------------------------------------------------------------// + +/*! + * 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. */ + +//----------------------------------------------------------------------// +//- Public methods (SbgEComLogEkfEuler) -// +//----------------------------------------------------------------------// + +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); + + return sbgStreamBufferGetLastError(pStreamBuffer); +} + +SbgErrorCode sbgEComLogEkfEulerWriteToStream(const SbgEComLogEkfEuler *pLogData, SbgStreamBuffer *pStreamBuffer) +{ + assert(pStreamBuffer); + assert(pLogData); + + 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]); + + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->status); + + return sbgStreamBufferGetLastError(pStreamBuffer); +} + +//----------------------------------------------------------------------// +//- Public methods (SbgEComLogEkfQuat) -// +//----------------------------------------------------------------------// + +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); +} + +SbgErrorCode sbgEComLogEkfQuatWriteToStream(const SbgEComLogEkfQuat *pLogData, SbgStreamBuffer *pStreamBuffer) +{ + assert(pStreamBuffer); + assert(pLogData); + + 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->eulerStdDev[0]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->eulerStdDev[1]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->eulerStdDev[2]); + + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->status); + + return sbgStreamBufferGetLastError(pStreamBuffer); +} + +//----------------------------------------------------------------------// +//- Public methods (SbgEComLogEkfNav) -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComLogEkfNavReadFromStream(SbgEComLogEkfNav *pLogData, SbgStreamBuffer *pStreamBuffer) +{ + assert(pStreamBuffer); + assert(pLogData); + + pLogData->timeStamp = sbgStreamBufferReadUint32LE(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->position[0] = sbgStreamBufferReadDoubleLE(pStreamBuffer); + pLogData->position[1] = sbgStreamBufferReadDoubleLE(pStreamBuffer); + pLogData->position[2] = sbgStreamBufferReadDoubleLE(pStreamBuffer); + + pLogData->undulation = sbgStreamBufferReadFloatLE(pStreamBuffer); + + pLogData->positionStdDev[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->positionStdDev[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->positionStdDev[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); + + pLogData->status = sbgStreamBufferReadUint32LE(pStreamBuffer); + + return sbgStreamBufferGetLastError(pStreamBuffer); +} + +SbgErrorCode sbgEComLogEkfNavWriteToStream(const SbgEComLogEkfNav *pLogData, SbgStreamBuffer *pStreamBuffer) +{ + assert(pStreamBuffer); + assert(pLogData); + + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); + + 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]); + + sbgStreamBufferWriteDoubleLE(pStreamBuffer, pLogData->position[0]); + sbgStreamBufferWriteDoubleLE(pStreamBuffer, pLogData->position[1]); + sbgStreamBufferWriteDoubleLE(pStreamBuffer, pLogData->position[2]); + + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->undulation); + + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->positionStdDev[0]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->positionStdDev[1]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->positionStdDev[2]); + + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->status); + + return sbgStreamBufferGetLastError(pStreamBuffer); +} + +//----------------------------------------------------------------------// +//- Public methods (SbgEComLogEkfVelBody) -// +//----------------------------------------------------------------------// + +void sbgEComLogEkfVelBodyConstruct(SbgEComLogEkfVelBody *pLogData) +{ + assert(pLogData); + + memset(pLogData, 0x00, sizeof(*pLogData)); +} + +SbgErrorCode sbgEComLogEkfVelBodyReadFromStream(SbgEComLogEkfVelBody *pLogData, SbgStreamBuffer *pStreamBuffer) +{ + assert(pStreamBuffer); + assert(pLogData); + + pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); + pLogData->status = sbgStreamBufferReadUint32LE(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); +} + +SbgErrorCode sbgEComLogEkfVelBodyWriteToStream(const SbgEComLogEkfVelBody *pLogData, SbgStreamBuffer *pStreamBuffer) +{ + assert(pStreamBuffer); + assert(pLogData); + + 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->velocityStdDev[0]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocityStdDev[1]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocityStdDev[2]); + + return sbgStreamBufferGetLastError(pStreamBuffer); +} + +//----------------------------------------------------------------------// +//- Public setters/getters -// +//----------------------------------------------------------------------// + +SbgEComSolutionMode sbgEComLogEkfGetSolutionMode(uint32_t status) +{ + 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; +} + +//----------------------------------------------------------------------// +//- DEPRECATED - Used for backward compatibility -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComBinaryLogParseEkfEulerData(SbgStreamBuffer *pStreamBuffer, SbgEComLogEkfEuler *pLogData) +{ + return sbgEComLogEkfEulerReadFromStream(pLogData, pStreamBuffer); +} + +SbgErrorCode sbgEComBinaryLogWriteEkfEulerData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogEkfEuler *pLogData) +{ + return sbgEComLogEkfEulerWriteToStream(pLogData, pStreamBuffer); +} + +SbgErrorCode sbgEComBinaryLogParseEkfQuatData(SbgStreamBuffer *pStreamBuffer, SbgEComLogEkfQuat *pLogData) +{ + return sbgEComLogEkfQuatReadFromStream(pLogData, pStreamBuffer); +} + +SbgErrorCode sbgEComBinaryLogWriteEkfQuatData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogEkfQuat *pLogData) +{ + return sbgEComLogEkfQuatWriteToStream(pLogData, pStreamBuffer); +} + +SbgErrorCode sbgEComBinaryLogParseEkfNavData(SbgStreamBuffer *pStreamBuffer, SbgEComLogEkfNav *pLogData) +{ + return sbgEComLogEkfNavReadFromStream(pLogData, pStreamBuffer); +} + +SbgErrorCode sbgEComBinaryLogWriteEkfNavData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogEkfNav *pLogData) +{ + return sbgEComLogEkfNavWriteToStream(pLogData, pStreamBuffer); +} diff --git a/src/binaryLogs/sbgEComBinaryLogEkf.h b/src/logs/sbgEComLogEkf.h similarity index 60% rename from src/binaryLogs/sbgEComBinaryLogEkf.h rename to src/logs/sbgEComLogEkf.h index 896cd4b..6c24205 100644 --- a/src/binaryLogs/sbgEComBinaryLogEkf.h +++ b/src/logs/sbgEComLogEkf.h @@ -1,5 +1,5 @@ /*! - * \file sbgEComBinaryLogEkf.h + * \file sbgEComLogEkf.h * \ingroup binaryLogs * \author SBG Systems * \date 25 February 2013 @@ -30,8 +30,8 @@ * \endlicense */ -#ifndef SBG_ECOM_BINARY_LOG_EKF_H -#define SBG_ECOM_BINARY_LOG_EKF_H +#ifndef SBG_ECOM_LOG_EKF_H +#define SBG_ECOM_LOG_EKF_H // sbgCommonLib headers #include @@ -45,17 +45,11 @@ extern "C" { //- Solution status definitions -// //----------------------------------------------------------------------// -/*! - * Solution status mode definitions. - */ -#define SBG_ECOM_SOLUTION_MODE_SHIFT (0u) /*!< Shift used to extract the clock status part. */ -#define SBG_ECOM_SOLUTION_MODE_MASK (0x0000000Fu) /*!< Mask used to keep only the clock status part. */ - /*! * Solution bit masks definitions. */ #define SBG_ECOM_SOL_ATTITUDE_VALID (0x00000001u << 4) /*!< Set to 1 if attitude data is reliable (Roll/Pitch error < 0,5°). */ -#define SBG_ECOM_SOL_HEADING_VALID (0x00000001u << 5) /*!< Set to 1 if geading data is reliable (Heading error < 1°). */ +#define SBG_ECOM_SOL_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). */ @@ -76,7 +70,7 @@ extern "C" { #define SBG_ECOM_SOL_AIR_DATA_USED (0x00000001u << 25) /*!< Set to 1 if AirData (altimeter and/or true airspeed) is used in solution (data used and valid since 3s). */ #define SBG_ECOM_SOL_ZUPT_USED (0x00000001u << 26) /*!< Set to 1 if a ZUPT is used in solution (data used and valid since 3s). */ #define SBG_ECOM_SOL_ALIGN_VALID (0x00000001u << 27) /*!< Set to 1 if sensor alignment and calibration parameters are valid */ -#define SBG_ECOM_SOL_DEPTH_USED (0x00000001u << 28) /*!< Set to 1 if Depth sensor (for subsea navigation) is used in solution (data used and valid since 3s). */ +#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). */ /*! * Solution filter mode enum. @@ -90,37 +84,6 @@ typedef enum _SbgEComSolutionMode SBG_ECOM_SOL_MODE_NAV_POSITION = 4 /*!< Nominal mode, the Kalman filter computes all parameters (attitude, velocity, position). Absolute position is provided. */ } SbgEComSolutionMode; -//----------------------------------------------------------------------// -//- Solution status helpers methods -// -//----------------------------------------------------------------------// - -/*! - * Method used to read the solution mode from a solution status field. - * - * \param[in] status Status uint32_t value to extract the solution mode from it. - * \return The extracted solution mode. - */ -SBG_INLINE SbgEComSolutionMode sbgEComLogEkfGetSolutionMode(uint32_t status) -{ - return (SbgEComSolutionMode)((status >> SBG_ECOM_SOLUTION_MODE_SHIFT) & SBG_ECOM_SOLUTION_MODE_MASK); -} - -/*! - * Method used to write the solution status field. - * - * \param[in] solutionMode The solution mode to set. - * \param[in] masks Bit mask to set. - * \return The build solution status field. - */ -SBG_INLINE uint32_t sbgEComLogEkfBuildSolutionStatus(SbgEComSolutionMode solutionMode, uint32_t masks) -{ - // - // Create the combined status field - // - return ((((uint32_t)solutionMode)&SBG_ECOM_SOLUTION_MODE_MASK) << SBG_ECOM_SOLUTION_MODE_SHIFT) | masks; -} - - //----------------------------------------------------------------------// //- Log structure definitions -// //----------------------------------------------------------------------// @@ -128,29 +91,29 @@ SBG_INLINE uint32_t sbgEComLogEkfBuildSolutionStatus(SbgEComSolutionMode solutio /*! * EKF computed orientation using euler angles. */ -typedef struct _SbgLogEkfEulerData +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. */ -} SbgLogEkfEulerData; +} SbgEComLogEkfEuler; /*! * EFK computed orientation using quaternion. */ -typedef struct _SbgLogEkfQuatData +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. */ -} SbgLogEkfQuatData; +} SbgEComLogEkfQuat; /*! * EFK computed navigation data. */ -typedef struct _SbgLogEkfNavData +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. */ @@ -160,68 +123,155 @@ typedef struct _SbgLogEkfNavData float undulation; /*!< Altitude difference between the geoid and the Ellipsoid in meters (Height above Ellipsoid = altitude + undulation). */ float positionStdDev[3]; /*!< Latitude, longitude and altitude 1 sigma standard deviation in meters. */ uint32_t status; /*!< EKF solution status bitmask and enum. */ -} SbgLogEkfNavData; +} SbgEComLogEkfNav; + +/*! + * INS body velocity computed by the EKF filter. + */ +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. */ +} SbgEComLogEkfVelBody; //----------------------------------------------------------------------// -//- Public methods -// +//- Public methods (SbgEComLogEkfEuler) -// //----------------------------------------------------------------------// /*! * Parse data for the SBG_ECOM_LOG_EKF_EULER message and fill the corresponding structure. * - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. + * \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 sbgEComBinaryLogParseEkfEulerData(SbgStreamBuffer *pInputStream, SbgLogEkfEulerData *pOutputData); +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[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. + * + * \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 sbgEComBinaryLogWriteEkfEulerData(SbgStreamBuffer *pOutputStream, const SbgLogEkfEulerData *pInputData); +SbgErrorCode sbgEComLogEkfEulerWriteToStream(const SbgEComLogEkfEuler *pLogData, SbgStreamBuffer *pStreamBuffer); + +//----------------------------------------------------------------------// +//- Public methods (SbgEComLogEkfQuat) -// +//----------------------------------------------------------------------// /*! * Parse data for the SBG_ECOM_LOG_EKF_QUAT message and fill the corresponding structure. * - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. + * \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 sbgEComBinaryLogParseEkfQuatData(SbgStreamBuffer *pInputStream, SbgLogEkfQuatData *pOutputData); +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[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. + * + * \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 sbgEComBinaryLogWriteEkfQuatData(SbgStreamBuffer *pOutputStream, const SbgLogEkfQuatData *pInputData); +SbgErrorCode sbgEComLogEkfQuatWriteToStream(const SbgEComLogEkfQuat *pLogData, SbgStreamBuffer *pStreamBuffer); + +//----------------------------------------------------------------------// +//- Public methods (SbgEComLogEkfNav) -// +//----------------------------------------------------------------------// /*! * Parse data for the SBG_ECOM_LOG_EKF_NAV message and fill the corresponding structure. * - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. + * \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 sbgEComBinaryLogParseEkfNavData(SbgStreamBuffer *pInputStream, SbgLogEkfNavData *pOutputData); +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. + */ +SbgErrorCode sbgEComLogEkfNavWriteToStream(const SbgEComLogEkfNav *pLogData, SbgStreamBuffer *pStreamBuffer); + +//----------------------------------------------------------------------// +//- Public methods (SbgEComLogEkfVelBody) -// +//----------------------------------------------------------------------// + +/*! + * Construct an empty / zero initialized instance. + * + * \param[in] pLogData Log instance. + */ +void sbgEComLogEkfVelBodyConstruct(SbgEComLogEkfVelBody *pLogData); + +/*! + * Read SBG_ECOM_LOG_EKF_VEL_BODY message from a stream buffer. * - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. + * \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 sbgEComBinaryLogWriteEkfNavData(SbgStreamBuffer *pOutputStream, const SbgLogEkfNavData *pInputData); +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. + */ +SbgErrorCode sbgEComLogEkfVelBodyWriteToStream(const SbgEComLogEkfVelBody *pLogData, SbgStreamBuffer *pStreamBuffer); + +//----------------------------------------------------------------------// +//- Public setters/getters -// +//----------------------------------------------------------------------// + +/*! + * 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. + */ +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. + */ +uint32_t sbgEComLogEkfBuildSolutionStatus(SbgEComSolutionMode solutionMode, uint32_t masks); + +//----------------------------------------------------------------------// +//- 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(SbgErrorCode sbgEComBinaryLogParseEkfEulerData(SbgStreamBuffer *pStreamBuffer, SbgEComLogEkfEuler *pLogData)); +SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogWriteEkfEulerData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogEkfEuler *pLogData)); + +SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogParseEkfQuatData(SbgStreamBuffer *pStreamBuffer, SbgEComLogEkfQuat *pLogData)); +SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogWriteEkfQuatData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogEkfQuat *pLogData)); + +SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogParseEkfNavData(SbgStreamBuffer *pStreamBuffer, SbgEComLogEkfNav *pLogData)); +SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogWriteEkfNavData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogEkfNav *pLogData)); + #ifdef __cplusplus } #endif -#endif // SBG_ECOM_BINARY_LOG_EKF_H +#endif // SBG_ECOM_LOG_EKF_H diff --git a/src/logs/sbgEComLogEkfRotAccel.c b/src/logs/sbgEComLogEkfRotAccel.c new file mode 100644 index 0000000..169ebda --- /dev/null +++ b/src/logs/sbgEComLogEkfRotAccel.c @@ -0,0 +1,55 @@ +// sbgCommonLib headers +#include +#include + +// Local headers +#include "sbgEComLogEkfRotAccel.h" + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +void sbgEComLogEkfRotAccelConstruct(SbgEComLogEkfRotAccel *pLogData) +{ + assert(pLogData); + + memset(pLogData, 0x00, sizeof(*pLogData)); +} + +SbgErrorCode sbgEComLogEkfRotAccelReadFromStream(SbgEComLogEkfRotAccel *pLogData, SbgStreamBuffer *pStreamBuffer) +{ + assert(pStreamBuffer); + assert(pLogData); + + pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); + pLogData->status = sbgStreamBufferReadUint32LE(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); + + return sbgStreamBufferGetLastError(pStreamBuffer); +} + +SbgErrorCode sbgEComLogEkfRotAccelWriteToStream(const SbgEComLogEkfRotAccel *pLogData, SbgStreamBuffer *pStreamBuffer) +{ + assert(pStreamBuffer); + assert(pLogData); + + 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->acceleration[0]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->acceleration[1]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->acceleration[2]); + + return sbgStreamBufferGetLastError(pStreamBuffer); +} diff --git a/src/logs/sbgEComLogEkfRotAccel.h b/src/logs/sbgEComLogEkfRotAccel.h new file mode 100644 index 0000000..2944962 --- /dev/null +++ b/src/logs/sbgEComLogEkfRotAccel.h @@ -0,0 +1,116 @@ +/*! + * \file sbgEComLogEkfRotAccel.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 12 June 2023 + * + * \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. + * + * The measurements are either expressed in body or NED frames. Please double + * check if you need body or NED outputs are they are very different. + * + * For example, a rotation along the DOWN axis represents how fast the INS heading + * is changing whatever the roll/pitch is. + * + * A rotation along the Z axis is simply the gyroscope Z reading corrected for + * bias, scale factor errors and earth rotation. + * + * The rate are corrected for sensor bias, scale factor error and earth rotation + * has also been removed. In short, it should returns zero if the INS is + * perfectly still. + * + * The lateral accelerations are corrected for sensor bias, scale factor and the + * 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 + * + * 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_EKF_ROT_ACCEL_H +#define SBG_ECOM_LOG_EKF_ROT_ACCEL_H + +// sbgCommonLib headers +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Public definitions -// +//----------------------------------------------------------------------// + +/*! + * INS compensated body or North, East, Down rotation rates and linear accelerations. + */ +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). */ +} SbgEComLogEkfRotAccel; + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * Construct an empty / zero initialized 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. + */ +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. + */ +SbgErrorCode sbgEComLogEkfRotAccelWriteToStream(const SbgEComLogEkfRotAccel *pLogData, SbgStreamBuffer *pStreamBuffer); + +//----------------------------------------------------------------------// +//- Public setters/getters -// +//----------------------------------------------------------------------// + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_LOG_EKF_ROT_ACCEL_H diff --git a/src/logs/sbgEComLogEvent.c b/src/logs/sbgEComLogEvent.c new file mode 100644 index 0000000..38fe8b6 --- /dev/null +++ b/src/logs/sbgEComLogEvent.c @@ -0,0 +1,49 @@ +#include "sbgEComLogEvent.h" + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComLogEventReadFromStream(SbgEComLogEvent *pLogData, SbgStreamBuffer *pStreamBuffer) +{ + 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); + + 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); +} + +//----------------------------------------------------------------------// +//- DEPRECATED - Used for backward compatibility -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComBinaryLogParseEvent(SbgStreamBuffer *pStreamBuffer, SbgEComLogEvent *pLogData) +{ + return sbgEComLogEventReadFromStream(pLogData, pStreamBuffer); +} + +SbgErrorCode sbgEComBinaryLogWriteEvent(SbgStreamBuffer *pStreamBuffer, const SbgEComLogEvent *pLogData) +{ + return sbgEComLogEventWriteToStream(pLogData, pStreamBuffer); +} diff --git a/src/binaryLogs/sbgEComBinaryLogEvent.h b/src/logs/sbgEComLogEvent.h similarity index 72% rename from src/binaryLogs/sbgEComBinaryLogEvent.h rename to src/logs/sbgEComLogEvent.h index c1c40e0..2e18707 100644 --- a/src/binaryLogs/sbgEComBinaryLogEvent.h +++ b/src/logs/sbgEComLogEvent.h @@ -1,5 +1,5 @@ /*! - * \file sbgEComBinaryLogEvent.h + * \file sbgEComLogEvent.h * \ingroup binaryLogs * \author SBG Systems * \date 28 October 2013 @@ -30,8 +30,8 @@ * \endlicense */ -#ifndef SBG_ECOM_BINARY_LOG_EVENT_H -#define SBG_ECOM_BINARY_LOG_EVENT_H +#ifndef SBG_ECOM_LOG_EVENT_H +#define SBG_ECOM_LOG_EVENT_H // sbgCommonLib headers #include @@ -61,7 +61,7 @@ extern "C" { /*! * Structure that stores data for the SBG_ECOM_LOG_EVENT_# message. */ -typedef struct _SbgLogEvent +typedef struct _SbgEComLogEvent { uint32_t timeStamp; /*!< Measurement time since the sensor power up. */ uint16_t status; /*!< Events status bitmask. */ @@ -69,7 +69,7 @@ typedef struct _SbgLogEvent uint16_t timeOffset1; /*!< Time offset for the third received event. */ uint16_t timeOffset2; /*!< Time offset for the fourth received event. */ uint16_t timeOffset3; /*!< Time offset for the fifth received event. */ -} SbgLogEvent; +} SbgEComLogEvent; //----------------------------------------------------------------------// //- Public methods -// @@ -78,23 +78,32 @@ typedef struct _SbgLogEvent /*! * Parse data for the SBG_ECOM_LOG_EVENT_# message and fill the corresponding structure. * - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. + * \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 sbgEComBinaryLogParseEvent(SbgStreamBuffer *pInputStream, SbgLogEvent *pOutputData); +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[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. + * + * \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 sbgEComBinaryLogWriteEvent(SbgStreamBuffer *pOutputStream, const SbgLogEvent *pInputData); +SbgErrorCode sbgEComLogEventWriteToStream(const SbgEComLogEvent *pLogData, SbgStreamBuffer *pStreamBuffer); + +//----------------------------------------------------------------------// +//- DEPRECATED - Used for backward compatibility -// +//----------------------------------------------------------------------// + +SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogEvent SbgLogEvent); + +SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogParseEvent(SbgStreamBuffer *pStreamBuffer, SbgEComLogEvent *pLogData)); +SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogWriteEvent(SbgStreamBuffer *pStreamBuffer, const SbgEComLogEvent *pLogData)); #ifdef __cplusplus } #endif -#endif // SBG_ECOM_BINARY_LOG_EVENT_H +#endif // SBG_ECOM_LOG_EVENT_H diff --git a/src/logs/sbgEComLogGnssHdt.c b/src/logs/sbgEComLogGnssHdt.c new file mode 100644 index 0000000..7912e16 --- /dev/null +++ b/src/logs/sbgEComLogGnssHdt.c @@ -0,0 +1,228 @@ +// sbgCommonLib headers +#include +#include + +// Local headers +#include "sbgEComLogGnssHdt.h" + +//----------------------------------------------------------------------// +//- 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_BASELINE_VALID (0x0001 << 6) /*!< Set to 1 if the baseline length field is filled and valid. */ + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +void sbgEComLogGnssHdtZeroInit(SbgEComLogGnssHdt *pLogData) +{ + assert(pLogData); + + memset(pLogData, 0x00, sizeof(*pLogData)); + + 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); +} + +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); +} + +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); +} + +//----------------------------------------------------------------------// +//- Public setters/getters -// +//----------------------------------------------------------------------// + +void sbgEComLogGnssHdtSetStatus(SbgEComLogGnssHdt *pLogData, SbgEComGnssHdtStatus status) +{ + 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; +} + +SbgEComGnssHdtStatus sbgEComLogGnssHdtGetStatus(const SbgEComLogGnssHdt *pLogData) +{ + assert(pLogData); + + 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; + } +} + +bool sbgEComLogGnssHdtPitchIsValid(const SbgEComLogGnssHdt *pLogData) +{ + 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; + } +} + +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; + } +} + +bool sbgEComLogGnssHdtNumSvTrackedIsValid(const SbgEComLogGnssHdt *pLogData) +{ + 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; + } +} + +//----------------------------------------------------------------------// +//- DEPRECATED - Used for backward compatibility -// +//----------------------------------------------------------------------// + +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; +} + +SbgEComGnssHdtStatus sbgEComLogGpsHdtGetStatus(uint32_t status) +{ + 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); +} + +SbgErrorCode sbgEComBinaryLogWriteGpsHdtData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogGnssHdt *pLogData) +{ + return sbgEComLogGnssHdtWriteToStream(pLogData, pStreamBuffer); +} diff --git a/src/logs/sbgEComLogGnssHdt.h b/src/logs/sbgEComLogGnssHdt.h new file mode 100644 index 0000000..8f552bd --- /dev/null +++ b/src/logs/sbgEComLogGnssHdt.h @@ -0,0 +1,210 @@ +/*! + * \file sbgEComLogGnssHdt.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 05 May 2023 + * + * \brief GNSS True Heading related logs. + * + * \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 + */ + +#ifndef SBG_ECOM_LOG_GNSS_HDT_H +#define SBG_ECOM_LOG_GNSS_HDT_H + +// sbgCommonLib headers +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Public definitions -// +//----------------------------------------------------------------------// + +/*! + * GNSS HDT status definitions. + */ +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. */ +} SbgEComGnssHdtStatus; + +//----------------------------------------------------------------------// +//- Log structure definitions -// +//----------------------------------------------------------------------// + +/*! + * Structure that stores data for the SBG_ECOM_LOG_GPS#_HDT message. + */ +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) */ +} SbgEComLogGnssHdt; + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * Zero initialize the message struct. + * + * \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. + */ +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. + */ +SbgErrorCode sbgEComLogGnssHdtWriteToStream(const SbgEComLogGnssHdt *pLogData, SbgStreamBuffer *pStreamBuffer); + +//----------------------------------------------------------------------// +//- Public setters/getters -// +//----------------------------------------------------------------------// + +/*! + * Set the true heading solution status. + * + * \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. + */ +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. + */ +bool sbgEComLogGnssHdtHeadingIsValid(const SbgEComLogGnssHdt *pLogData); + +/*! + * Returns true if the pitch information is valid. + * + * Some GNSS receivers don't provide pitch information between antenna 1 and 2. + * 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. + */ +bool sbgEComLogGnssHdtPitchIsValid(const SbgEComLogGnssHdt *pLogData); + +/*! + * Set if the baseline field is provided with a valid value. + * + * 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. + */ +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. + */ +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. + */ +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. + */ +bool sbgEComLogGnssHdtNumSvUsedIsValid(const SbgEComLogGnssHdt *pLogData); + +//----------------------------------------------------------------------// +//- DEPRECATED - Used for backward compatibility -// +//----------------------------------------------------------------------// + +#ifdef SBG_ECOM_USE_DEPRECATED_MACROS + #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) +#endif + +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)); + +SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogParseGpsHdtData(SbgStreamBuffer *pStreamBuffer, SbgEComLogGnssHdt *pLogData)); +SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogWriteGpsHdtData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogGnssHdt *pLogData)); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_LOG_GNSS_HDT_H diff --git a/src/logs/sbgEComLogGnssPos.c b/src/logs/sbgEComLogGnssPos.c new file mode 100644 index 0000000..91addc0 --- /dev/null +++ b/src/logs/sbgEComLogGnssPos.c @@ -0,0 +1,372 @@ +// sbgCommonLib headers +#include +#include + +// Local headers +#include "sbgEComLogGnssPos.h" + +//----------------------------------------------------------------------// +//- 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_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. */ + +//----------------------------------------------------------------------// +//- 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_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. */ + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +void sbgEComLogGnssPosZeroInit(SbgEComLogGnssPos *pLogData) +{ + assert(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; + + 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); +} + +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); +} + +SbgErrorCode sbgEComLogGnssPosWriteToStream(const SbgEComLogGnssPos *pLogData, SbgStreamBuffer *pStreamBuffer) +{ + assert(pLogData); + assert(pStreamBuffer); + + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->status); + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeOfWeek); + + sbgStreamBufferWriteDoubleLE(pStreamBuffer, pLogData->latitude); + sbgStreamBufferWriteDoubleLE(pStreamBuffer, pLogData->longitude); + sbgStreamBufferWriteDoubleLE(pStreamBuffer, pLogData->altitude); + + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->undulation); + + 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->numSvTracked); + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->statusExt); + + return sbgStreamBufferGetLastError(pStreamBuffer); +} + +//----------------------------------------------------------------------// +//- Public setters/getters -// +//----------------------------------------------------------------------// + +void sbgEComLogGnssPosSetStatus(SbgEComLogGnssPos *pLogData, SbgEComGnssPosStatus status) +{ + 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; +} + +SbgEComGnssPosStatus sbgEComLogGnssPosGetStatus(const SbgEComLogGnssPos *pLogData) +{ + assert(pLogData); + + 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); + + 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); + + 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); + + 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; + } +} + +bool sbgEComLogGnssPosNumSvTrackedIsValid(const SbgEComLogGnssPos *pLogData) +{ + 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; + } +} + +bool sbgEComLogGnssPosBaseStationIdIsValid(const SbgEComLogGnssPos *pLogData) +{ + 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; + } +} + +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; + } +} + +float sbgEComLogGnssPosGetDifferentialAge(const SbgEComLogGnssPos *pLogData) +{ + float diffAgeSeconds; + + assert(pLogData); + + if (pLogData->differentialAge == UINT16_MAX) + { + diffAgeSeconds = NAN; + } + else + { + diffAgeSeconds = pLogData->differentialAge / 100.0f; + } + + return diffAgeSeconds; +} + +void sbgEComLogGnssPosSetIfmStatus(SbgEComLogGnssPos *pLogData, SbgEComGnssIfmStatus status) +{ + 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; +} + +SbgEComGnssIfmStatus sbgEComLogGnssPosGetIfmStatus(const SbgEComLogGnssPos *pLogData) +{ + assert(pLogData); + + 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); + + 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); +} + +void sbgEComLogGnssPosSetOsnmaStatus(SbgEComLogGnssPos *pLogData, SbgEComGnssOsnmaStatus status) +{ + 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; +} + +SbgEComGnssOsnmaStatus sbgEComLogGnssPosGetOsnmaStatus(const SbgEComLogGnssPos *pLogData) +{ + return (SbgEComGnssOsnmaStatus)((pLogData->statusExt >> SBG_ECOM_LOG_GNSS_POS_OSNMA_SHIFT)&SBG_ECOM_LOG_GNSS_POS_OSNMA_MASK); +} +//----------------------------------------------------------------------// +//- DEPRECATED - Used for backward compatibility -// +//----------------------------------------------------------------------// + +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; +} + +SbgEComGnssPosStatus sbgEComLogGpsPosGetStatus(uint32_t status) +{ + 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); +} + +SbgErrorCode sbgEComBinaryLogParseGpsPosData(SbgStreamBuffer *pStreamBuffer, SbgEComLogGnssPos *pLogData) +{ + return sbgEComLogGnssPosReadFromStream(pLogData, pStreamBuffer); +} + +SbgErrorCode sbgEComBinaryLogWriteGpsPosData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogGnssPos *pLogData) +{ + return sbgEComLogGnssPosWriteToStream(pLogData, pStreamBuffer); +} diff --git a/src/logs/sbgEComLogGnssPos.h b/src/logs/sbgEComLogGnssPos.h new file mode 100644 index 0000000..2149367 --- /dev/null +++ b/src/logs/sbgEComLogGnssPos.h @@ -0,0 +1,414 @@ +/*! + * \file sbgEComLogGnssPos.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 09 May 2023 + * + * \brief GNSS position related logs. + * + * \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 + */ + +#ifndef SBG_ECOM_LOG_GPS_POS_H +#define SBG_ECOM_LOG_GPS_POS_H + +// sbgCommonLib headers +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Public definitions -// +//----------------------------------------------------------------------// + +/*! + * 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_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_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. */ + +/*! + * 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. */ +} SbgEComGnssPosStatus; + +/*! + * GNSS position types definitions. + */ +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. */ +} SbgEComGnssPosType; + +/*! + * GNSS interference monitoring and mitigation indicator. + */ +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. */ +} SbgEComGnssIfmStatus; + +/*! + * GNSS spoofing monitoring and mitigation indicator. + */ +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. */ +} SbgEComGnssSpoofingStatus; + +/*! + * GNSS Galileo OSNMA Status. + */ +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. */ +} SbgEComGnssOsnmaStatus; + +//----------------------------------------------------------------------// +//- Log structure definitions -// +//----------------------------------------------------------------------// + +/*! + * Structure that stores data for the SBG_ECOM_LOG_GPS#_POS message. + */ +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). */ +} SbgEComLogGnssPos; + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * Zero initialize the message struct. + * + * \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. + */ +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. + */ +SbgErrorCode sbgEComLogGnssPosWriteToStream(const SbgEComLogGnssPos *pLogData, SbgStreamBuffer *pStreamBuffer); + +//----------------------------------------------------------------------// +//- Public setters/getters -// +//----------------------------------------------------------------------// + +/*! + * Set the GNSS position solution status. + * + * \param[in] pLogData Log instance. + * \param[in] status The solution status to set. + */ +void sbgEComLogGnssPosSetStatus(SbgEComLogGnssPos *pLogData, SbgEComGnssPosStatus status); + +/*! + * Returns the GNSS position solution status. + * + * Note: Method doesn't follow standard naming conventions because of legacy sbgEComLogGnssPosGetStatus method. + * + * \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. + */ +void sbgEComLogGnssPosSetType(SbgEComLogGnssPos *pLogData, SbgEComGnssPosType posType); + +/*! + * Returns the GNSS position solution type. + * + * Note: Method doesn't follow standard naming conventions because of legacy sbgEComLogGnssPosGetStatus method. + * + * \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. + */ +void sbgEComLogGnssPosSetSignalsUsed(SbgEComLogGnssPos *pLogData, uint32_t signalMask); + +/*! + * Returns true if the signal(s) are used in the solution. + * + * 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. + */ +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. + */ +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. + */ +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. + */ +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. + */ +bool sbgEComLogGnssPosDifferentialAgeIsValid(const SbgEComLogGnssPos *pLogData); + +/*! + * Set the differential correction age in seconds. + * + * 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). + */ +void sbgEComLogGnssPosSetDifferentialAge(SbgEComLogGnssPos *pLogData, float differentialAge); + +/*! + * Returns the differential correction age in seconds. + * + * If there is no valid differential age, returns NaN. + * + * \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. + */ +void sbgEComLogGnssPosSetIfmStatus(SbgEComLogGnssPos *pLogData, SbgEComGnssIfmStatus status); + +/*! + * Returns the GNSS position solution 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. + */ +void sbgEComLogGnssPosSetSpoofingStatus(SbgEComLogGnssPos *pLogData, SbgEComGnssSpoofingStatus status); + +/*! + * Returns the GNSS 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. + */ +void sbgEComLogGnssPosSetOsnmaStatus(SbgEComLogGnssPos *pLogData, SbgEComGnssOsnmaStatus status); + +/*! + * Returns the GNSS Galileo OSNMA status. + * + * \param[in] pLogData Log instance. + * \return The Galileo OSNMA status. + */ +SbgEComGnssOsnmaStatus sbgEComLogGnssPosGetOsnmaStatus(const SbgEComLogGnssPos *pLogData); + +//----------------------------------------------------------------------// +//- DEPRECATED - Used for backward compatibility -// +//----------------------------------------------------------------------// + +#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) +#endif + +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)); +SBG_DEPRECATED(SbgEComGnssPosType sbgEComLogGpsPosGetType(uint32_t status)); + +SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogParseGpsPosData(SbgStreamBuffer *pStreamBuffer, SbgEComLogGnssPos *pLogData)); +SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogWriteGpsPosData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogGnssPos *pLogData)); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_LOG_GNSS_POS_H diff --git a/src/logs/sbgEComLogGnssVel.c b/src/logs/sbgEComLogGnssVel.c new file mode 100644 index 0000000..3dd6c48 --- /dev/null +++ b/src/logs/sbgEComLogGnssVel.c @@ -0,0 +1,162 @@ +// sbgCommonLib headers +#include +#include + +// Local headers +#include "sbgEComLogGnssVel.h" + +//----------------------------------------------------------------------// +//- 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_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 -// +//----------------------------------------------------------------------// + +void sbgEComLogGnssVelZeroInit(SbgEComLogGnssVel *pLogData) +{ + assert(pLogData); + + memset(pLogData, 0x00, sizeof(*pLogData)); + + 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); +} + +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); +} + +SbgErrorCode sbgEComLogGnssVelWriteToStream(const SbgEComLogGnssVel *pLogData, SbgStreamBuffer *pStreamBuffer) +{ + assert(pLogData); + assert(pStreamBuffer); + + 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->velocityAcc[0]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocityAcc[1]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocityAcc[2]); + + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->course); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->courseAcc); + + return sbgStreamBufferGetLastError(pStreamBuffer); +} + +//----------------------------------------------------------------------// +//- Public setters/getters -// +//----------------------------------------------------------------------// + +void sbgEComLogGnssVelSetStatus(SbgEComLogGnssVel *pLogData, SbgEComGnssVelStatus status) +{ + 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; +} + +SbgEComGnssVelStatus sbgEComLogGnssVelGetStatus(const SbgEComLogGnssVel *pLogData) +{ + assert(pLogData); + + 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); + + 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); + + 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; + } +} + +//----------------------------------------------------------------------// +//- DEPRECATED - Used for backward compatibility -// +//----------------------------------------------------------------------// + +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); +} + +SbgEComGnssVelStatus sbgEComLogGpsVelGetStatus(uint32_t status) +{ + 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); +} + +SbgErrorCode sbgEComBinaryLogParseGpsVelData(SbgStreamBuffer *pStreamBuffer, SbgEComLogGnssVel *pLogData) +{ + return sbgEComLogGnssVelReadFromStream(pLogData, pStreamBuffer); +} + +SbgErrorCode sbgEComBinaryLogWriteGpsVelData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogGnssVel *pLogData) +{ + return sbgEComLogGnssVelWriteToStream(pLogData, pStreamBuffer); +} diff --git a/src/logs/sbgEComLogGnssVel.h b/src/logs/sbgEComLogGnssVel.h new file mode 100644 index 0000000..6d9c38c --- /dev/null +++ b/src/logs/sbgEComLogGnssVel.h @@ -0,0 +1,198 @@ +/*! + * \file sbgEComLogGnssVel.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 09 May 2023 + * + * \brief GNSS velocity logs. + * + * \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 + */ + +#ifndef SBG_ECOM_LOG_GNSS_VEL_H +#define SBG_ECOM_LOG_GNSS_VEL_H + +// sbgCommonLib headers +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Public definitions -// +//----------------------------------------------------------------------// + +/*! + * GNSS velocity status definitions. + */ +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. */ +} SbgEComGnssVelStatus; + +/*! + * GNSS velocity types definitions. + */ +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. */ +} SbgEComGnssVelType; + +//----------------------------------------------------------------------// +//- Log structure definitions -// +//----------------------------------------------------------------------// + +/*! + * 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. + */ +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). */ +} SbgEComLogGnssVel; + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * Zero initialize the message struct. + * + * \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. + */ +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. + */ +SbgErrorCode sbgEComLogGnssVelWriteToStream(const SbgEComLogGnssVel *pLogData, SbgStreamBuffer *pStreamBuffer); + +//----------------------------------------------------------------------// +//- Public setters/getters -// +//----------------------------------------------------------------------// + +/*! + * Set the GNSS position solution status. + * + * \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. + */ +SbgEComGnssVelStatus sbgEComLogGnssVelGetStatus(const SbgEComLogGnssVel *pLogData); + +/*! + * Set the GNSS position solution type. + * + * \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. + */ +SbgEComGnssVelType sbgEComLogGnssVelGetType(const SbgEComLogGnssVel *pLogData); + +/*! + * Returns true if the velocity down component is available/valid. + * + * 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. + */ +bool sbgEComLogGnssVelDownVelocityIsValid(const SbgEComLogGnssVel *pLogData); + +//----------------------------------------------------------------------// +//- DEPRECATED - Used for backward compatibility -// +//----------------------------------------------------------------------// + +#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) +#endif + +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)); +SBG_DEPRECATED(SbgEComGnssVelType sbgEComLogGpsVelGetType(uint32_t status)); + +SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogParseGpsVelData(SbgStreamBuffer *pStreamBuffer, SbgEComLogGnssVel *pLogData)); +SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogWriteGpsVelData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogGnssVel *pLogData)); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_LOG_GNSS_VEL_H diff --git a/src/logs/sbgEComLogImu.c b/src/logs/sbgEComLogImu.c new file mode 100644 index 0000000..e156b0f --- /dev/null +++ b/src/logs/sbgEComLogImu.c @@ -0,0 +1,219 @@ +#include "sbgEComLogImu.h" + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComLogImuLegacyReadFromStream(SbgEComLogImuLegacy *pLogData, SbgStreamBuffer *pStreamBuffer) +{ + 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->gyroscopes[0] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->gyroscopes[1] = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->gyroscopes[2] = sbgStreamBufferReadFloatLE(pStreamBuffer); + + pLogData->temperature = 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); + + return sbgStreamBufferGetLastError(pStreamBuffer); +} + +SbgErrorCode sbgEComLogImuLegacyWriteToStream(const SbgEComLogImuLegacy *pLogData, SbgStreamBuffer *pStreamBuffer) +{ + 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]); + + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->gyroscopes[0]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->gyroscopes[1]); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->gyroscopes[2]); + + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->temperature); + + 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); + + pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); + pLogData->status = sbgStreamBufferReadUint16LE(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->temperature = sbgStreamBufferReadInt16LE(pStreamBuffer); + + return sbgStreamBufferGetLastError(pStreamBuffer); +} + +SbgErrorCode sbgEComLogImuShortWriteToStream(const SbgEComLogImuShort *pLogData, SbgStreamBuffer *pStreamBuffer) +{ + assert(pStreamBuffer); + assert(pLogData); + + 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->deltaAngle[0]); + sbgStreamBufferWriteInt32LE(pStreamBuffer, pLogData->deltaAngle[1]); + sbgStreamBufferWriteInt32LE(pStreamBuffer, pLogData->deltaAngle[2]); + + sbgStreamBufferWriteInt16LE(pStreamBuffer, pLogData->temperature); + + return sbgStreamBufferGetLastError(pStreamBuffer); +} + +SbgErrorCode sbgEComLogImuFastLegacyReadFromStream(SbgEComLogImuFastLegacy *pLogData, SbgStreamBuffer *pStreamBuffer) +{ + assert(pStreamBuffer); + assert(pLogData); + + 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->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); +} + +SbgErrorCode sbgEComLogImuFastLegacyWriteToStream(const SbgEComLogImuFastLegacy *pLogData, SbgStreamBuffer *pStreamBuffer) +{ + 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)); + + return sbgStreamBufferGetLastError(pStreamBuffer); +} + +//----------------------------------------------------------------------// +//- Public setters/getters -// +//----------------------------------------------------------------------// + +float sbgEComLogImuShortGetDeltaAngle(const SbgEComLogImuShort *pImuShort, size_t idx) +{ + assert(pImuShort); + assert(idx < 3); + + return pImuShort->deltaAngle[idx] / 67108864.0f; +} + +float sbgEComLogImuShortGetDeltaVelocity(const SbgEComLogImuShort *pImuShort, size_t idx) +{ + assert(pImuShort); + assert(idx < 3); + + return pImuShort->deltaVelocity[idx] / 1048576.0f; +} + +float sbgEComLogImuShortGetTemperature(const SbgEComLogImuShort *pImuShort) +{ + assert(pImuShort); + + return pImuShort->temperature / 256.0f; +} + +//----------------------------------------------------------------------// +//- DEPRECATED - Used for backward compatibility -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComBinaryLogParseImuData(SbgStreamBuffer *pStreamBuffer, SbgEComLogImuLegacy *pLogData) +{ + return sbgEComLogImuLegacyReadFromStream(pLogData, pStreamBuffer); +} + +SbgErrorCode sbgEComBinaryLogWriteImuData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogImuLegacy *pLogData) +{ + return sbgEComLogImuLegacyWriteToStream(pLogData, pStreamBuffer); +} + +SbgErrorCode sbgEComBinaryLogParseImuShort(SbgStreamBuffer *pStreamBuffer, SbgEComLogImuShort *pLogData) +{ + return sbgEComLogImuShortReadFromStream(pLogData, pStreamBuffer); +} + +SbgErrorCode sbgEComBinaryLogWriteImuShort(SbgStreamBuffer *pStreamBuffer, const SbgEComLogImuShort *pLogData) +{ + return sbgEComLogImuShortWriteToStream(pLogData, pStreamBuffer); +} + +SbgErrorCode sbgEComBinaryLogParseFastImuData(SbgStreamBuffer *pStreamBuffer, SbgEComLogImuFastLegacy *pLogData) +{ + return sbgEComLogImuFastLegacyReadFromStream(pLogData, pStreamBuffer); +} + +SbgErrorCode sbgEComBinaryLogWriteFastImuData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogImuFastLegacy *pLogData) +{ + return sbgEComLogImuFastLegacyWriteToStream(pLogData, pStreamBuffer); +} + +float sbgLogImuShortGetDeltaAngle(const SbgEComLogImuShort *pImuShort, size_t idx) +{ + return sbgEComLogImuShortGetDeltaAngle(pImuShort, idx); +} + +float sbgLogImuShortGetDeltaVelocity(const SbgEComLogImuShort *pImuShort, size_t idx) +{ + return sbgEComLogImuShortGetDeltaVelocity(pImuShort, idx); +} + +float sbgLogImuShortGetTemperature(const SbgEComLogImuShort *pImuShort) +{ + return sbgEComLogImuShortGetTemperature(pImuShort); +} diff --git a/src/binaryLogs/sbgEComBinaryLogImu.h b/src/logs/sbgEComLogImu.h similarity index 55% rename from src/binaryLogs/sbgEComBinaryLogImu.h rename to src/logs/sbgEComLogImu.h index 4ad17a8..2d78337 100644 --- a/src/binaryLogs/sbgEComBinaryLogImu.h +++ b/src/logs/sbgEComLogImu.h @@ -1,5 +1,5 @@ /*! - * \file sbgEComBinaryLogImu.h + * \file sbgEComLogImu.h * \ingroup binaryLogs * \author SBG Systems * \date 25 February 2013 @@ -30,8 +30,8 @@ * \endlicense */ -#ifndef SBG_ECOM_BINARY_LOG_IMU_H -#define SBG_ECOM_BINARY_LOG_IMU_H +#ifndef SBG_ECOM_LOG_IMU_H +#define SBG_ECOM_LOG_IMU_H // sbgCommonLib headers #include @@ -68,132 +68,171 @@ extern "C" { /*! * 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. + * + * 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 */ -typedef struct _SbgLogImuData +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. */ - float deltaAngle[3]; /*!< X, Y, Z delta angle in rad.s^-1. */ -} SbgLogImuData; + 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) */ +} SbgEComLogImuLegacy; /*! * Structure that stores data for the SBG_ECOM_LOG_IMU_SHORT message. - * This message is only sent asynchronously and is the preferred log for post processing. + * + * This message is sent asynchronously and must be used for post processing. */ -typedef struct _SbgLogImuShort +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. */ -} SbgLogImuShort; +} 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 */ -typedef struct _SbgLogFastImuData +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. */ -} SbgLogFastImuData; +} SbgEComLogImuFastLegacy; //----------------------------------------------------------------------// -//- Public getters -// +//- Public methods -// //----------------------------------------------------------------------// /*! - * Return from an IMU Short log, the X, Y or Z delta angle value in rad.s^-1 + * Parse data for the SBG_ECOM_LOG_IMU_DATA message and fill the corresponding structure. * - * \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[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. */ -float sbgLogImuShortGetDeltaAngle(const SbgLogImuShort *pImuShort, size_t idx); +SbgErrorCode sbgEComLogImuLegacyReadFromStream(SbgEComLogImuLegacy *pLogData, SbgStreamBuffer *pStreamBuffer); /*! - * 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. + * 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. */ -float sbgLogImuShortGetDeltaVelocity(const SbgLogImuShort *pImuShort, size_t idx); +SbgErrorCode sbgEComLogImuLegacyWriteToStream(const SbgEComLogImuLegacy *pLogData, SbgStreamBuffer *pStreamBuffer); /*! - * Return from an IMU Short log, the temperature in °C + * Parse data for the SBG_ECOM_LOG_IMU_SHORT message and fill the corresponding structure. * - * \param[in] pImuShort Input IMU short message instance. - * \return The converted temperature in °C + * \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. */ -float sbgLogImuShortGetTemperature(const SbgLogImuShort *pImuShort); - -//----------------------------------------------------------------------// -//- Public methods -// -//----------------------------------------------------------------------// +SbgErrorCode sbgEComLogImuShortReadFromStream(SbgEComLogImuShort *pLogData, SbgStreamBuffer *pStreamBuffer); /*! - * Parse data for the SBG_ECOM_LOG_IMU_DATA message and fill the corresponding structure. - * - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. + * 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. */ -SbgErrorCode sbgEComBinaryLogParseImuData(SbgStreamBuffer *pInputStream, SbgLogImuData *pOutputData); +SbgErrorCode sbgEComLogImuShortWriteToStream(const SbgEComLogImuShort *pLogData, SbgStreamBuffer *pStreamBuffer); /*! - * Write data for the SBG_ECOM_LOG_IMU_DATA message to the output stream buffer from the provided structure. + * Parse data for the SBG_ECOM_LOG_FAST_IMU_DATA message and fill the corresponding structure. * - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. + * \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 sbgEComBinaryLogWriteImuData(SbgStreamBuffer *pOutputStream, const SbgLogImuData *pInputData); +SbgErrorCode sbgEComLogImuFastLegacyReadFromStream(SbgEComLogImuFastLegacy *pLogData, SbgStreamBuffer *pStreamBuffer); /*! - * Parse data for the SBG_ECOM_LOG_IMU_SHORT message and fill the corresponding structure. - * - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. + * 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. */ -SbgErrorCode sbgEComBinaryLogParseImuShort(SbgStreamBuffer *pInputStream, SbgLogImuShort *pOutputData); +SbgErrorCode sbgEComLogImuFastLegacyWriteToStream(const SbgEComLogImuFastLegacy *pLogData, SbgStreamBuffer *pStreamBuffer); + +//----------------------------------------------------------------------// +//- Public setters/getters -// +//----------------------------------------------------------------------// /*! - * Write data for the SBG_ECOM_LOG_IMU_SHORT message to the output stream buffer from the provided structure. + * Return from an IMU Short log, the X, Y or Z delta angle value in rad.s^-1 * - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. + * \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. */ -SbgErrorCode sbgEComBinaryLogWriteImuShort(SbgStreamBuffer *pOutputStream, const SbgLogImuShort *pInputData); +float sbgEComLogImuShortGetDeltaAngle(const SbgEComLogImuShort *pImuShort, size_t idx); /*! - * Parse data for the SBG_ECOM_LOG_FAST_IMU_DATA message and fill the corresponding structure. + * Return from an IMU Short log, the X, Y or Z delta velocity value in m.s^-2 * - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. + * \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. */ -SbgErrorCode sbgEComBinaryLogParseFastImuData(SbgStreamBuffer *pInputStream, SbgLogFastImuData *pOutputData); +float sbgEComLogImuShortGetDeltaVelocity(const SbgEComLogImuShort *pImuShort, size_t idx); /*! - * Write data for the SBG_ECOM_LOG_FAST_IMU_DATA message to the output stream buffer from the provided structure. + * Return from an IMU Short log, the temperature in °C * - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. + * \param[in] pImuShort Input IMU short message instance. + * \return The converted temperature in °C */ -SbgErrorCode sbgEComBinaryLogWriteFastImuData(SbgStreamBuffer *pOutputStream, const SbgLogFastImuData *pInputData); +float sbgEComLogImuShortGetTemperature(const SbgEComLogImuShort *pImuShort); + +//----------------------------------------------------------------------// +//- 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(SbgErrorCode sbgEComBinaryLogParseImuData(SbgStreamBuffer *pStreamBuffer, SbgEComLogImuLegacy *pLogData)); +SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogWriteImuData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogImuLegacy *pLogData)); + +SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogParseImuShort(SbgStreamBuffer *pStreamBuffer, SbgEComLogImuShort *pLogData)); +SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogWriteImuShort(SbgStreamBuffer *pStreamBuffer, const SbgEComLogImuShort *pLogData)); + +SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogParseFastImuData(SbgStreamBuffer *pStreamBuffer, SbgEComLogImuFastLegacy *pLogData)); +SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogWriteFastImuData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogImuFastLegacy *pLogData)); + +SBG_DEPRECATED(float sbgLogImuShortGetDeltaAngle(const SbgEComLogImuShort *pImuShort, size_t idx)); +SBG_DEPRECATED(float sbgLogImuShortGetDeltaVelocity(const SbgEComLogImuShort *pImuShort, size_t idx)); +SBG_DEPRECATED(float sbgLogImuShortGetTemperature(const SbgEComLogImuShort *pImuShort)); #ifdef __cplusplus } #endif -#endif // SBG_ECOM_BINARY_LOG_IMU_H +#endif // SBG_ECOM_LOG_IMU_H diff --git a/src/logs/sbgEComLogMag.c b/src/logs/sbgEComLogMag.c new file mode 100644 index 0000000..29da35f --- /dev/null +++ b/src/logs/sbgEComLogMag.c @@ -0,0 +1,92 @@ +#include "sbgEComLogMag.h" + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComLogMagReadFromStream(SbgEComLogMag *pLogData, SbgStreamBuffer *pStreamBuffer) +{ + assert(pStreamBuffer); + assert(pLogData); + + 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); + + 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)); +} + +//----------------------------------------------------------------------// +//- DEPRECATED - Used for backward compatibility -// +//----------------------------------------------------------------------// + +SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogMag SbgLogMag); +SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogMagCalib SbgLogMagCalib); + +SbgErrorCode sbgEComBinaryLogParseMagData(SbgStreamBuffer *pStreamBuffer, SbgEComLogMag *pLogData) +{ + 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); +} diff --git a/src/binaryLogs/sbgEComBinaryLogMag.h b/src/logs/sbgEComLogMag.h similarity index 64% rename from src/binaryLogs/sbgEComBinaryLogMag.h rename to src/logs/sbgEComLogMag.h index 4a52bc8..cf78260 100644 --- a/src/binaryLogs/sbgEComBinaryLogMag.h +++ b/src/logs/sbgEComLogMag.h @@ -1,5 +1,5 @@ /*! - * \file sbgEComBinaryLogMag.h + * \file sbgEComLogMag.h * \ingroup binaryLogs * \author SBG Systems * \date 12 March 2013 @@ -30,8 +30,8 @@ * \endlicense */ -#ifndef SBG_ECOM_BINARY_LOG_MAG_H -#define SBG_ECOM_BINARY_LOG_MAG_H +#ifndef SBG_ECOM_LOG_MAG_H +#define SBG_ECOM_LOG_MAG_H // sbgCommonLib headers #include @@ -68,23 +68,23 @@ extern "C" { /*! * Structure that stores data for the SBG_ECOM_LOG_MAG message. */ -typedef struct _SbgLogMag +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. */ -} SbgLogMag; +} SbgEComLogMag; /*! * Structure that stores data for the SBG_ECOM_LOG_MAG_CALIB message. */ -typedef struct _SbgLogMagCalib +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. */ -} SbgLogMagCalib; +} SbgEComLogMagCalib; //----------------------------------------------------------------------// //- Public methods -// @@ -93,41 +93,54 @@ typedef struct _SbgLogMagCalib /*! * Parse data for the SBG_ECOM_LOG_MAG message and fill the corresponding structure. * - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. + * \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 sbgEComBinaryLogParseMagData(SbgStreamBuffer *pInputStream, SbgLogMag *pOutputData); +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[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. + * + * \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 sbgEComBinaryLogWriteMagData(SbgStreamBuffer *pOutputStream, const SbgLogMag *pInputData); +SbgErrorCode sbgEComLogMagWriteToStream(const SbgEComLogMag *pLogData, SbgStreamBuffer *pStreamBuffer); /*! * Parse data for the SBG_ECOM_LOG_MAG_CALIB message and fill the corresponding structure. * - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. + * \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 sbgEComBinaryLogParseMagCalibData(SbgStreamBuffer *pInputStream, SbgLogMagCalib *pOutputData); +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[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. + * + * \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 sbgEComBinaryLogWriteMagCalibData(SbgStreamBuffer *pOutputStream, const SbgLogMagCalib *pInputData); +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(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 -#endif // SBG_ECOM_BINARY_LOG_MAG_H +#endif // SBG_ECOM_LOG_MAG_H diff --git a/src/logs/sbgEComLogOdometer.c b/src/logs/sbgEComLogOdometer.c new file mode 100644 index 0000000..d9c4894 --- /dev/null +++ b/src/logs/sbgEComLogOdometer.c @@ -0,0 +1,45 @@ +#include "sbgEComLogOdometer.h" + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComLogOdometerReadFromStream(SbgEComLogOdometer *pLogData, SbgStreamBuffer *pStreamBuffer) +{ + assert(pStreamBuffer); + assert(pLogData); + + pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); + pLogData->status = sbgStreamBufferReadUint16LE(pStreamBuffer); + + pLogData->velocity = sbgStreamBufferReadFloatLE(pStreamBuffer); + + return sbgStreamBufferGetLastError(pStreamBuffer); +} + +SbgErrorCode sbgEComLogOdometerWriteToStream(const SbgEComLogOdometer *pLogData, SbgStreamBuffer *pStreamBuffer) +{ + assert(pStreamBuffer); + assert(pLogData); + + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); + sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->status); + + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->velocity); + + return sbgStreamBufferGetLastError(pStreamBuffer); +} + +//----------------------------------------------------------------------// +//- DEPRECATED - Used for backward compatibility -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComBinaryLogParseOdometerData(SbgStreamBuffer *pStreamBuffer, SbgEComLogOdometer *pLogData) +{ + return sbgEComLogOdometerReadFromStream(pLogData, pStreamBuffer); +} + +SbgErrorCode sbgEComBinaryLogWriteOdometerData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogOdometer *pLogData) +{ + return sbgEComLogOdometerWriteToStream(pLogData, pStreamBuffer); +} diff --git a/src/binaryLogs/sbgEComBinaryLogOdometer.h b/src/logs/sbgEComLogOdometer.h similarity index 65% rename from src/binaryLogs/sbgEComBinaryLogOdometer.h rename to src/logs/sbgEComLogOdometer.h index a7b3f69..ec8041e 100644 --- a/src/binaryLogs/sbgEComBinaryLogOdometer.h +++ b/src/logs/sbgEComLogOdometer.h @@ -1,10 +1,10 @@ /*! - * \file sbgEComBinaryLogOdometer.h + * \file sbgEComLogOdometer.h * \ingroup binaryLogs * \author SBG Systems * \date 25 February 2013 * - * \brief Parse recevied 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 @@ -30,8 +30,8 @@ * \endlicense */ -#ifndef SBG_ECOM_BINARY_LOG_ODOMETER_H -#define SBG_ECOM_BINARY_LOG_ODOMETER_H +#ifndef SBG_ECOM_LOG_ODOMETER_H +#define SBG_ECOM_LOG_ODOMETER_H // sbgCommonLib headers #include @@ -56,14 +56,14 @@ extern "C" { //----------------------------------------------------------------------// /*! - * Log structure for odometer data. + * Log structure for odometer/DMI velocity measurements. */ -typedef struct _SbgLogOdometerData +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. */ -} SbgLogOdometerData; +} SbgEComLogOdometer; //----------------------------------------------------------------------// //- Public methods -// @@ -72,23 +72,32 @@ typedef struct _SbgLogOdometerData /*! * Parse data for the SBG_ECOM_LOG_ODO_VEL message and fill the corresponding structure. * - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. + * \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 sbgEComBinaryLogParseOdometerData(SbgStreamBuffer *pInputStream, SbgLogOdometerData *pOutputData); +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[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. + * + * \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 sbgEComBinaryLogWriteOdometerData(SbgStreamBuffer *pOutputStream, const SbgLogOdometerData *pInputData); +SbgErrorCode sbgEComLogOdometerWriteToStream(const SbgEComLogOdometer *pLogData, SbgStreamBuffer *pStreamBuffer); + +//----------------------------------------------------------------------// +//- DEPRECATED - Used for backward compatibility -// +//----------------------------------------------------------------------// + +SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogOdometer SbgLogOdometerData); + +SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogParseOdometerData(SbgStreamBuffer *pStreamBuffer, SbgEComLogOdometer *pLogData)); +SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogWriteOdometerData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogOdometer *pLogData)); #ifdef __cplusplus } #endif -#endif // SBG_ECOM_BINARY_LOG_ODOMETER_H +#endif // SBG_ECOM_LOG_ODOMETER_H diff --git a/src/logs/sbgEComLogRawData.c b/src/logs/sbgEComLogRawData.c new file mode 100644 index 0000000..1c6365f --- /dev/null +++ b/src/logs/sbgEComLogRawData.c @@ -0,0 +1,54 @@ +// sbgCommonLib headers +#include + +// Local headers +#include "sbgEComLogRawData.h" + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComLogRawDataReadFromStream(SbgEComLogRawData *pLogData, SbgStreamBuffer *pStreamBuffer) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + size_t payloadSize; + + assert(pStreamBuffer); + assert(pLogData); + + 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; + } + + return errorCode; +} + +SbgErrorCode sbgEComLogRawDataWriteToStream(const SbgEComLogRawData *pLogData, SbgStreamBuffer *pStreamBuffer) +{ + assert(pStreamBuffer); + assert(pLogData); + + return sbgStreamBufferWriteBuffer(pStreamBuffer, pLogData->rawBuffer, pLogData->bufferSize); +} + +//----------------------------------------------------------------------// +//- DEPRECATED - Used for backward compatibility -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComBinaryLogParseRawData(SbgStreamBuffer *pStreamBuffer, SbgEComLogRawData *pLogData) +{ + return sbgEComLogRawDataReadFromStream(pLogData, pStreamBuffer); +} + +SbgErrorCode sbgEComBinaryLogWriteRawData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogRawData *pLogData) +{ + return sbgEComLogRawDataWriteToStream(pLogData, pStreamBuffer); +} diff --git a/src/binaryLogs/sbgEComBinaryLogRawData.h b/src/logs/sbgEComLogRawData.h similarity index 65% rename from src/binaryLogs/sbgEComBinaryLogRawData.h rename to src/logs/sbgEComLogRawData.h index 57108c7..0769d35 100644 --- a/src/binaryLogs/sbgEComBinaryLogRawData.h +++ b/src/logs/sbgEComLogRawData.h @@ -1,10 +1,10 @@ /*! - * \file sbgEComBinaryLogRawData.h + * \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 data. + * \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 @@ -30,8 +30,8 @@ * \endlicense */ -#ifndef SBG_ECOM_BINARY_LOG_RAW_DATA_H -#define SBG_ECOM_BINARY_LOG_RAW_DATA_H +#ifndef SBG_ECOM_LOG_RAW_DATA_H +#define SBG_ECOM_LOG_RAW_DATA_H // sbgCommonLib headers #include @@ -54,11 +54,11 @@ extern "C" { /*! * Structure that stores raw data message. */ -typedef struct _SbgLogRawData +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. */ -} SbgLogRawData; +} SbgEComLogRawData; //----------------------------------------------------------------------// //- Public methods -// @@ -66,24 +66,33 @@ typedef struct _SbgLogRawData /*! * Parse raw data message and fill the corresponding structure. - * - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. + * + * \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 sbgEComBinaryLogParseRawData(SbgStreamBuffer *pInputStream, SbgLogRawData *pOutputData); +SbgErrorCode sbgEComLogRawDataReadFromStream(SbgEComLogRawData *pLogData, SbgStreamBuffer *pStreamBuffer); /*! * Write raw data message to the output stream buffer from the provided structure. * - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. + * \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 sbgEComBinaryLogWriteRawData(SbgStreamBuffer *pOutputStream, const SbgLogRawData *pInputData); +SbgErrorCode sbgEComLogRawDataWriteToStream(const SbgEComLogRawData *pLogData, SbgStreamBuffer *pStreamBuffer); + +//----------------------------------------------------------------------// +//- DEPRECATED - Used for backward compatibility -// +//----------------------------------------------------------------------// + +SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogRawData SbgLogRawData); + +SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogParseRawData(SbgStreamBuffer *pStreamBuffer, SbgEComLogRawData *pLogData)); +SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogWriteRawData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogRawData *pLogData)); #ifdef __cplusplus } #endif -#endif // SBG_ECOM_BINARY_LOG_RAW_DATA_H +#endif // SBG_ECOM_LOG_RAW_DATA_H diff --git a/src/logs/sbgEComLogSat.c b/src/logs/sbgEComLogSat.c new file mode 100644 index 0000000..58ee8f3 --- /dev/null +++ b/src/logs/sbgEComLogSat.c @@ -0,0 +1,698 @@ +/*! + * \file sbgEComLogSat.c + * \author SBG Systems + * \date 1 March 2022 + * + * \brief Handle binary satellite logs. + * + * \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 + +// Project headers +#include + +// Local headers +#include "sbgEComLogSat.h" + +//----------------------------------------------------------------------// +//- 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_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_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. */ + +//----------------------------------------------------------------------// +//- Private functions -// +//----------------------------------------------------------------------// + +/*! + * 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. + */ +#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. + */ +#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. + */ +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]; + } +} + +/*! + * Returns the best tracking status between the two inputs. + * + * \param[in] trackingStatus1 First tracking status. + * \param[in] trackingStatus2 Second tracking status. + */ +static SbgEComSatTrackingStatus sbgEComLogSatSelectTrackingStatus(SbgEComSatTrackingStatus trackingStatus1, SbgEComSatTrackingStatus trackingStatus2) +{ + SbgEComSatTrackingStatus trackingStatus; + + if (trackingStatus2 > trackingStatus1) + { + trackingStatus = trackingStatus2; + } + else + { + trackingStatus = trackingStatus1; + } + + 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. + */ +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", + }; + + 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. + */ +static SbgEComSatHealthStatus sbgEComLogSatSelectHealthStatus(SbgEComSatHealthStatus healthStatus1, SbgEComSatHealthStatus healthStatus2) +{ + SbgEComSatHealthStatus healthStatus; + + if (healthStatus2 > healthStatus1) + { + healthStatus = healthStatus2; + } + else + { + healthStatus = healthStatus1; + } + + 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. + */ +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", + }; + + if (elevationStatus < SBG_ARRAY_SIZE(enumToStrLut)) + { + return enumToStrLut[elevationStatus]; + } + else + { + return enumToStrLut[SBG_ECOM_SAT_ELEVATION_STATUS_UNKNOWN]; + } +} + +//----------------------------------------------------------------------// +//- Private functions (SbgLogSatSignalData) -// +//----------------------------------------------------------------------// + +/*! + * 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. + */ +static SbgErrorCode sbgEComLogSatSignalReadFromStream(SbgEComLogSatSignal *pSatSignal, SbgStreamBuffer *pStreamBuffer) +{ + assert(pSatSignal); + assert(pStreamBuffer); + + pSatSignal->id = sbgStreamBufferReadUint8LE(pStreamBuffer); + pSatSignal->flags = sbgStreamBufferReadUint8LE(pStreamBuffer); + pSatSignal->snr = sbgStreamBufferReadUint8LE(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. + */ +static SbgErrorCode sbgEComLogSatSignalWriteToStream(const SbgEComLogSatSignal *pSatSignal, SbgStreamBuffer *pStreamBuffer) +{ + assert(pSatSignal); + assert(pStreamBuffer); + + sbgStreamBufferWriteUint8LE(pStreamBuffer, pSatSignal->id); + sbgStreamBufferWriteUint8LE(pStreamBuffer, pSatSignal->flags); + sbgStreamBufferWriteUint8LE(pStreamBuffer, pSatSignal->snr); + + return sbgStreamBufferGetLastError(pStreamBuffer); +} + +//----------------------------------------------------------------------// +//- Private functions (SbgEComLogSatEntry) -// +//----------------------------------------------------------------------// + +/*! + * 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. + */ +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; + + 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); + + pSatData->id = id; + pSatData->elevation = elevation; + pSatData->azimuth = azimuth; + pSatData->flags = flags; + pSatData->nrSignals = 0; +} + +/*! + * Update the health and tracking summary statuses of satellite data. + * + * The SbgEComLogSatEntry stores a synthetic health and tracking statues. + * This synthetic status is computed from each signal status using priority rules. + * + * For example: + * - 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. + */ +static void sbgEComLogSatEntryUpdateStatus(SbgEComLogSatEntry *pSatData, SbgEComSatHealthStatus healthStatus, SbgEComSatTrackingStatus trackingStatus) +{ + uint16_t flags; + + assert(pSatData); + + flags = pSatData->flags; + + 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); + + 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. + */ +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; +} + +/*! + * 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. + */ +static SbgErrorCode sbgEComLogSatEntryWriteToStream(const SbgEComLogSatEntry *pSatData, SbgStreamBuffer *pStreamBuffer) +{ + SbgErrorCode errorCode; + + 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); + + 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) + { + break; + } + } + } + + return errorCode; +} + +//----------------------------------------------------------------------// +//- Public setters/getters (SbgEComLogSatSignal) -// +//----------------------------------------------------------------------// + +const char *sbgEComLogSatSignalGetSignalIdAsStr(const SbgEComLogSatSignal *pSignalData) +{ + assert(pSignalData); + + return sbgEComSignalToStr(pSignalData->id); +} + +bool sbgEComLogSatSignalSnrIsValid(const SbgEComLogSatSignal *pSignalData) +{ + assert(pSignalData); + + return (pSignalData->flags&SBG_ECOM_LOG_SAT_SIGNAL_SNR_VALID?true:false); +} + +SbgEComSatHealthStatus sbgEComLogSatSignalGetHealthStatus(const SbgEComLogSatSignal *pSignalData) +{ + assert(pSignalData); + + 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); + + return sbgEComLogSatHealthStatusToStr(sbgEComLogSatSignalGetHealthStatus(pSignalData)); +} + +SbgEComSatTrackingStatus sbgEComLogSatSignalGetTrackingStatus(const SbgEComLogSatSignal *pSignalData) +{ + assert(pSignalData); + + 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); + + return sbgEComLogSatTrackingStatusToStr(sbgEComLogSatSignalGetTrackingStatus(pSignalData)); +} + +//----------------------------------------------------------------------// +//- Public methods (SbgEComLogSatEntry) -// +//----------------------------------------------------------------------// + +SbgEComLogSatSignal *sbgEComLogSatEntryAdd(SbgEComLogSatEntry *pSatData, SbgEComSignalId id, SbgEComSatHealthStatus healthStatus, SbgEComSatTrackingStatus trackingStatus, bool snrValid, uint8_t snr) +{ + SbgEComLogSatSignal *pSignalData = NULL; + + assert(pSatData); + + 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); + + if (snrValid) + { + flags |= SBG_ECOM_LOG_SAT_SIGNAL_SNR_VALID; + } + + pSignalData = &pSatData->signalData[pSatData->nrSignals]; + pSatData->nrSignals++; + + 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"); + } + + return pSignalData; +} + +//----------------------------------------------------------------------// +//- Public setters/getters (SbgEComLogSatEntry) -// +//----------------------------------------------------------------------// + +SbgEComLogSatSignal *sbgEComLogSatEntryGet(SbgEComLogSatEntry *pSatData, SbgEComSignalId id) +{ + SbgEComLogSatSignal *pSignalData = NULL; + + assert(pSatData); + + for (size_t i = 0; i < pSatData->nrSignals; i++) + { + if (pSatData->signalData[i].id == id) + { + pSignalData = &pSatData->signalData[i]; + break; + } + } + + return pSignalData; +} + +SbgEComConstellationId sbgEComLogSatEntryGetConstellationId(const SbgEComLogSatEntry *pSatData) +{ + assert(pSatData); + + 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)); +} + +SbgEComSatElevationStatus sbgEComLogSatEntryGetElevationStatus(const SbgEComLogSatEntry *pSatData) +{ + assert(pSatData); + + 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)); +} + +SbgEComSatHealthStatus sbgEComLogSatEntryGetHealthStatus(const SbgEComLogSatEntry *pSatData) +{ + assert(pSatData); + + 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); + + return sbgEComLogSatHealthStatusToStr(sbgEComLogSatEntryGetHealthStatus(pSatData)); +} + +SbgEComSatTrackingStatus sbgEComLogSatEntryGetTrackingStatus(const SbgEComLogSatEntry *pSatData) +{ + assert(pSatData); + + 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); + + return sbgEComLogSatTrackingStatusToStr(sbgEComLogSatEntryGetTrackingStatus(pSatData)); +} + +//----------------------------------------------------------------------// +//- Public methods (SbgEComLogSatList) -// +//----------------------------------------------------------------------// + +void sbgEComLogSatListConstruct(SbgEComLogSatList *pSatList, uint32_t timeStamp) +{ + 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; + + assert(pSatList); + + 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."); + } + + 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 sbgEComLogSatListWriteToStream(const SbgEComLogSatList *pSatList, SbgStreamBuffer *pStreamBuffer) +{ + SbgErrorCode errorCode; + + assert(pSatList); + assert(pSatList->nrSatellites <= UINT8_MAX); + assert(pStreamBuffer); + + sbgStreamBufferWriteUint32LE(pStreamBuffer, pSatList->timeStamp); + sbgStreamBufferWriteUint32LE(pStreamBuffer, pSatList->reserved); + sbgStreamBufferWriteUint8LE(pStreamBuffer, (uint8_t)pSatList->nrSatellites); + + 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) + { + break; + } + } + } + + return errorCode; +} + +//----------------------------------------------------------------------// +//- Public setters/getters (SbgEComLogSatList) -// +//----------------------------------------------------------------------// + +SbgEComLogSatEntry *sbgEComLogSatListGet(SbgEComLogSatList *pSatList, uint8_t id) +{ + SbgEComLogSatEntry *pSatData = NULL; + + assert(pSatList); + + for (size_t i = 0; i < pSatList->nrSatellites; i++) + { + if (pSatList->satData[i].id == id) + { + pSatData = &pSatList->satData[i]; + break; + } + } + + return pSatData; +} + +//----------------------------------------------------------------------// +//- DEPRECATED - Used for backward compatibility -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComBinaryLogParseSatGroupData(SbgStreamBuffer *pStreamBuffer, SbgEComLogSatList *pSatList) +{ + return sbgEComLogSatListReadFromStream(pSatList, pStreamBuffer); +} + +SbgErrorCode sbgEComBinaryLogWriteSatGroupData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogSatList *pSatList) +{ + return sbgEComLogSatListWriteToStream(pSatList, pStreamBuffer); +} diff --git a/src/binaryLogs/sbgEComBinaryLogSat.h b/src/logs/sbgEComLogSat.h similarity index 57% rename from src/binaryLogs/sbgEComBinaryLogSat.h rename to src/logs/sbgEComLogSat.h index bdb5f8e..c35c38c 100644 --- a/src/binaryLogs/sbgEComBinaryLogSat.h +++ b/src/logs/sbgEComLogSat.h @@ -1,5 +1,5 @@ /*! - * \file sbgEComBinaryLogSat.h + * \file sbgEComLogSat.h * \ingroup binaryLogs * \author SBG Systems * \date 1 March 2022 @@ -30,8 +30,8 @@ * \endlicense */ -#ifndef SBG_ECOM_BINARY_LOG_SAT_H -#define SBG_ECOM_BINARY_LOG_SAT_H +#ifndef SBG_ECOM_LOG_SAT_H +#define SBG_ECOM_LOG_SAT_H // sbgCommonLib headers #include @@ -74,10 +74,10 @@ 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 beeing searched and can't be used yet. */ + 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 tracjed and is rejected from 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; //----------------------------------------------------------------------// @@ -114,19 +114,19 @@ typedef enum _SbgEComSatElevationStatus //----------------------------------------------------------------------// /*! - * Satellite signal data. + * Information and status for a single signal for a Satellite. * - * The flags include the snr available, health and tracking statuses. + * The flags include the SNR available, health and tracking statuses. */ -typedef struct _SbgLogSatSignalData +typedef struct _SbgEComLogSatSignal { SbgEComSignalId id; /*!< Signal ID. */ uint8_t flags; /*!< Flags. */ uint8_t snr; /*!< Signal-to-noise ratio, in dB. */ -} SbgLogSatSignalData; +} SbgEComLogSatSignal; /*! - * Satellite data. + * Information and status for a single satellite. * * The flags include the constellation ID, the elevation status, the health status, and the tracking status. * @@ -144,94 +144,111 @@ typedef struct _SbgLogSatSignalData * But if those satellite data are initially set with a healthy health status, and all three signals added have the * unknown health status, the satellite data health status remains healthy. */ -typedef struct _SbgLogSatData +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. */ - size_t signalDataArraySize; /*!< Size of the signal data array. */ - SbgLogSatSignalData *pSignalData; /*!< Signal data array. */ -} SbgLogSatData; + 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; /*! - * Satellite group data. + * List of visible satellites. */ -typedef struct _SbgLogSatGroupData +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. */ - size_t satDataArraySize; /*!< Size of the satellite data array. */ - SbgLogSatData *pSatData; /*!< Satellite data array. */ -} SbgLogSatGroupData; + 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; //----------------------------------------------------------------------// -//- Public functions -// +//- Public setters/getters (SbgEComLogSatSignal) -// //----------------------------------------------------------------------// /*! - * Parse satellite group data from a stream buffer. + * Get a 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. * - * This function constructs the satellite group data, which must be destroyed once not needed any more. + * \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] pStreamBuffer Stream buffer. - * \param[out] pSatGroupData Satellite group data. - * \return SBG_NO_ERROR if successful. + * \param[in] pSignalData Signal data. + * \return Health status. */ -SbgErrorCode sbgEComBinaryLogParseSatGroupData(SbgStreamBuffer *pStreamBuffer, SbgLogSatGroupData *pSatGroupData); +SbgEComSatHealthStatus sbgEComLogSatSignalGetHealthStatus(const SbgEComLogSatSignal *pSignalData); /*! - * Write satellite group data to a stream buffer. + * Get the health status of signal data as a read only C string. * - * \param[out] pStreamBuffer Stream buffer. - * \param[in] pSatGroupData Satellite group data. - * \return SBG_NO_ERROR if successful. + * \param[in] pSignalData Signal data. + * \return Health status as a read only C string. */ -SbgErrorCode sbgEComBinaryLogWriteSatGroupData(SbgStreamBuffer *pStreamBuffer, const SbgLogSatGroupData *pSatGroupData); +const char *sbgEComLogSatSignalGetHealthStatusAsStr(const SbgEComLogSatSignal *pSignalData); /*! - * Satellite group data constructor. + * Get the tracking status of signal data. * - * \param[in] pSatGroupData Satellite group data. - * \param[in] nrSatellites Number of satellites. - * \param[in] timeStamp Time stamp, in us. - * \return SBG_NO_ERROR if successful. + * \param[in] pSignalData Signal data. + * \return Tracking status. */ -SbgErrorCode sbgLogSatGroupDataConstruct(SbgLogSatGroupData *pSatGroupData, size_t nrSatellites, uint32_t timeStamp); +SbgEComSatTrackingStatus sbgEComLogSatSignalGetTrackingStatus(const SbgEComLogSatSignal *pSignalData); /*! - * Satellite group data destructor. + * Get the tracking status of signal data as a read only C string. * - * \param[in] pSatGroupData Satellite group data. + * \param[in] pSignalData Signal data. + * \return Tracking status as a read only C string. */ -void sbgLogSatGroupDataDestroy(SbgLogSatGroupData *pSatGroupData); +const char *sbgEComLogSatSignalGetTrackingStatusAsStr(const SbgEComLogSatSignal *pSignalData); + +//----------------------------------------------------------------------// +//- Public methods (SbgEComLogSatEntry) -// +//----------------------------------------------------------------------// /*! - * Add satellite data to satellite group data. + * Add signal data to satellite data. * - * \param[in] pSatGroupData Satellite group data. - * \param[in] nrSignals Number of signals. - * \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. + * 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. - * \return Satellite data, NULL if an error occurs. + * \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. */ -SbgLogSatData *sbgLogSatGroupDataAdd(SbgLogSatGroupData *pSatGroupData, size_t nrSignals, uint8_t id, int8_t elevation, uint16_t azimuth, SbgEComConstellationId constellationId, SbgEComSatElevationStatus elevationStatus, SbgEComSatHealthStatus healthStatus, SbgEComSatTrackingStatus trackingStatus); +SbgEComLogSatSignal *sbgEComLogSatEntryAdd(SbgEComLogSatEntry *pSatData, SbgEComSignalId id, SbgEComSatHealthStatus healthStatus, SbgEComSatTrackingStatus trackingStatus, bool snrValid, uint8_t snr); + +//----------------------------------------------------------------------// +//- Public setters/getters (SbgEComLogSatEntry) -// +//----------------------------------------------------------------------// /*! - * Get satellite data from satellite group data. + * Get signal data from satellite data. * - * \param[in] pSatGroupData Satellite group data. - * \param[in] id Satellite ID. - * \return Satellite data, NULL if not found. + * \param[in] pSatData Satellite data. + * \param[in] id Signal ID. + * \return Signal data, NULL if not found. */ -SbgLogSatData *sbgLogSatGroupGet(SbgLogSatGroupData *pSatGroupData, uint8_t id); +SbgEComLogSatSignal *sbgEComLogSatEntryGet(SbgEComLogSatEntry *pSatData, SbgEComSignalId id); /*! * Get the constellation ID of satellite data. @@ -239,7 +256,7 @@ SbgLogSatData *sbgLogSatGroupGet(SbgLogSatGroupData *pSatGroupData, uint8_t id); * \param[in] pSatData Satellite data. * \return Constellation ID. */ -SbgEComConstellationId sbgLogSatDataGetConstellationId(const SbgLogSatData *pSatData); +SbgEComConstellationId sbgEComLogSatEntryGetConstellationId(const SbgEComLogSatEntry *pSatData); /*! * Get the constellation ID of satellite data as a read only C string. @@ -247,7 +264,7 @@ SbgEComConstellationId sbgLogSatDataGetConstellationId(const SbgLogSatData *pSat * \param[in] pSatData Satellite data. * \return Constellation ID as a read only C string. */ -const char *sbgLogSatDataGetConstellationIdAsStr(const SbgLogSatData *pSatData); +const char *sbgEComLogSatEntryGetConstellationIdAsStr(const SbgEComLogSatEntry *pSatData); /*! * Get the elevation status of satellite data. @@ -255,7 +272,7 @@ const char *sbgLogSatDataGetConstellationIdAsStr(const SbgLogSatData *pSatData); * \param[in] pSatData Satellite data. * \return Elevation status. */ -SbgEComSatElevationStatus sbgLogSatDataGetElevationStatus(const SbgLogSatData *pSatData); +SbgEComSatElevationStatus sbgEComLogSatEntryGetElevationStatus(const SbgEComLogSatEntry *pSatData); /*! * Get the elevation status of satellite data as a read only C string. @@ -263,7 +280,7 @@ SbgEComSatElevationStatus sbgLogSatDataGetElevationStatus(const SbgLogSatData *p * \param[in] pSatData Satellite data. * \return Elevation status as a read only C string. */ -const char *sbgLogSatDataGetElevationStatusAsStr(const SbgLogSatData *pSatData); +const char *sbgEComLogSatEntryGetElevationStatusAsStr(const SbgEComLogSatEntry *pSatData); /*! * Get the health status of satellite data. @@ -271,7 +288,7 @@ const char *sbgLogSatDataGetElevationStatusAsStr(const SbgLogSatData *pSatData); * \param[in] pSatData Satellite data. * \return Health status. */ -SbgEComSatHealthStatus sbgLogSatDataGetHealthStatus(const SbgLogSatData *pSatData); +SbgEComSatHealthStatus sbgEComLogSatEntryGetHealthStatus(const SbgEComLogSatEntry *pSatData); /*! * Get the health status of satellite data as a read only C string. @@ -279,7 +296,7 @@ SbgEComSatHealthStatus sbgLogSatDataGetHealthStatus(const SbgLogSatData *pSatDat * \param[in] pSatData Satellite data. * \return Health status as a read only C string. */ -const char *sbgLogSatDataGetHealthStatusAsStr(const SbgLogSatData *pSatData); +const char *sbgEComLogSatEntryGetHealthStatusAsStr(const SbgEComLogSatEntry *pSatData); /*! * Get the tracking status of satellite data. @@ -287,7 +304,7 @@ const char *sbgLogSatDataGetHealthStatusAsStr(const SbgLogSatData *pSatData); * \param[in] pSatData Satellite data. * \return Tracking status. */ -SbgEComSatTrackingStatus sbgLogSatDataGetTrackingStatus(const SbgLogSatData *pSatData); +SbgEComSatTrackingStatus sbgEComLogSatEntryGetTrackingStatus(const SbgEComLogSatEntry *pSatData); /*! * Get the tracking status of satellite data as a read only C string. @@ -295,83 +312,79 @@ SbgEComSatTrackingStatus sbgLogSatDataGetTrackingStatus(const SbgLogSatData *pSa * \param[in] pSatData Satellite data. * \return Tracking status as a read only C string. */ -const char *sbgLogSatDataGetTrackingStatusAsStr(const SbgLogSatData *pSatData); +const char *sbgEComLogSatEntryGetTrackingStatusAsStr(const SbgEComLogSatEntry *pSatData); + +//----------------------------------------------------------------------// +//- Public methods (SbgEComLogSatList) -// +//----------------------------------------------------------------------// /*! - * Add signal data to satellite data. - * - * The health and tracking statuses of the satellite data are updated according to their respective - * priority rules. + * Satellite list constructor. * - * \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] pSatList Satellite list instance. + * \param[in] timeStamp Time stamp, in us. */ -SbgLogSatSignalData *sbgLogSatDataAdd(SbgLogSatData *pSatData, SbgEComSignalId id, SbgEComSatHealthStatus healthStatus, SbgEComSatTrackingStatus trackingStatus, bool snrValid, uint8_t snr); +void sbgEComLogSatListConstruct(SbgEComLogSatList *pSatList, uint32_t timeStamp); /*! - * Get signal data from satellite data. + * Add a satellite entry to the list of satellites. * - * \param[in] pSatData Satellite data. - * \param[in] id Signal ID. - * \return Signal data, NULL if not found. + * \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. */ -SbgLogSatSignalData *sbgLogSatDataGet(SbgLogSatData *pSatData, SbgEComSignalId id); +SbgEComLogSatEntry *sbgEComLogSatListAdd(SbgEComLogSatList *pSatList, uint8_t id, int8_t elevation, uint16_t azimuth, SbgEComConstellationId constellationId, SbgEComSatElevationStatus elevationStatus, SbgEComSatHealthStatus healthStatus, SbgEComSatTrackingStatus trackingStatus); /*! - * Get a signal id as a read only C string. + * Parse data for the SBG_ECOM_LOG_GPS#_SAT message and fill the corresponding structure. * - * \param[in] pSignalData Signal data. - * \return Signal id as a read only C string. - */ -const char *sbgLogSatSignalDataGetSignalIdAsStr(const SbgLogSatSignalData *pSignalData); - -/*! - * Returns true if the SNR value is valid. - * - * \param[in] pSignalData Signal data. - * \return true if the SNR value is valid. + * \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. */ -bool sbgLogSatSignalDataSnrIsValid(const SbgLogSatSignalData *pSignalData); +SbgErrorCode sbgEComLogSatListReadFromStream(SbgEComLogSatList *pSatList, SbgStreamBuffer *pStreamBuffer); /*! - * Get the health status of signal data. + * Write data for the SBG_ECOM_LOG_GPS#_SAT message to the output stream buffer from the provided structure. * - * \param[in] pSignalData Signal data. - * \return Health status. + * \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. */ -SbgEComSatHealthStatus sbgLogSatSignalDataGetHealthStatus(const SbgLogSatSignalData *pSignalData); +SbgErrorCode sbgEComLogSatListWriteToStream(const SbgEComLogSatList *pSatList, SbgStreamBuffer *pStreamBuffer); -/*! - * 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. - */ -const char *sbgLogSatSignalDataGetHealthStatusAsStr(const SbgLogSatSignalData *pSignalData); +//----------------------------------------------------------------------// +//- Public setters/getters (SbgEComLogSatList) -// +//----------------------------------------------------------------------// /*! - * Get the tracking status of signal data. + * Get a satellite entry from its ID. * - * \param[in] pSignalData Signal data. - * \return Tracking status. + * \param[in] pSatList Satellite list instance. + * \param[in] id Satellite ID. + * \return Satellite data, NULL if not found. */ -SbgEComSatTrackingStatus sbgLogSatSignalDataGetTrackingStatus(const SbgLogSatSignalData *pSignalData); +SbgEComLogSatEntry *sbgEComLogSatListGet(SbgEComLogSatList *pSatList, uint8_t id); -/*! - * 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. - */ -const char *sbgLogSatSignalDataGetTrackingStatusAsStr(const SbgLogSatSignalData *pSignalData); +//----------------------------------------------------------------------// +//- 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(SbgErrorCode sbgEComBinaryLogParseSatGroupData(SbgStreamBuffer *pStreamBuffer, SbgEComLogSatList *pSatList)); +SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogWriteSatGroupData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogSatList *pSatList)); #ifdef __cplusplus } #endif -#endif // SBG_ECOM_BINARY_LOG_SAT_H +#endif // SBG_ECOM_LOG_SAT_H diff --git a/src/logs/sbgEComLogShipMotion.c b/src/logs/sbgEComLogShipMotion.c new file mode 100644 index 0000000..2db2d1b --- /dev/null +++ b/src/logs/sbgEComLogShipMotion.c @@ -0,0 +1,92 @@ +#include "sbgEComLogShipMotion.h" + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +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); +} + +SbgErrorCode sbgEComLogShipMotionWriteToStream(const SbgEComLogShipMotion *pLogData, SbgStreamBuffer *pStreamBuffer) +{ + assert(pStreamBuffer); + assert(pLogData); + + 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->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]); + + sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->status); + + return sbgStreamBufferGetLastError(pStreamBuffer); +} + +//----------------------------------------------------------------------// +//- DEPRECATED - Used for backward compatibility -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComBinaryLogParseShipMotionData(SbgStreamBuffer *pStreamBuffer, SbgEComLogShipMotion *pLogData) +{ + return sbgEComLogShipMotionReadFromStream(pLogData, pStreamBuffer); +} + +SbgErrorCode sbgEComBinaryLogWriteShipMotionData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogShipMotion *pLogData) +{ + return sbgEComLogShipMotionWriteToStream(pLogData, pStreamBuffer); +} diff --git a/src/binaryLogs/sbgEComBinaryLogShipMotion.h b/src/logs/sbgEComLogShipMotion.h similarity index 69% rename from src/binaryLogs/sbgEComBinaryLogShipMotion.h rename to src/logs/sbgEComLogShipMotion.h index 81b9651..7671c76 100644 --- a/src/binaryLogs/sbgEComBinaryLogShipMotion.h +++ b/src/logs/sbgEComLogShipMotion.h @@ -1,5 +1,5 @@ /*! - * \file sbgEComBinaryLogShipMotion.h + * \file sbgEComLogShipMotion.h * \ingroup binaryLogs * \author SBG Systems * \date 30 March 2013 @@ -30,8 +30,8 @@ * \endlicense */ -#ifndef SBG_ECOM_BINARY_LOG_SHIP_MOTION_H -#define SBG_ECOM_BINARY_LOG_SHIP_MOTION_H +#ifndef SBG_ECOM_LOG_SHIP_MOTION_H +#define SBG_ECOM_LOG_SHIP_MOTION_H // sbgCommonLib headers #include @@ -57,13 +57,15 @@ extern "C" { //----------------------------------------------------------------------// /*! - * Structure that stores data for the SBG_ECOM_LOG_SHIP_MOTION or SBG_ECOM_LOG_SHIP_MOTION_HP message.
- * The data are expressed in the standard NED Ekinox coordiante frame. - * Surge is positive forward, sway is positive right and heave is positive down.
- * Note that status flag should be read before using the different parameters because it will provide validity information + * Structure that stores data for the SBG_ECOM_LOG_SHIP_MOTION or SBG_ECOM_LOG_SHIP_MOTION_HP message. + * + * The data are expressed in the standard NED Inertial Navigation System coordinate frame. + * Surge is positive forward, sway is positive right and heave is positive down. + * + * Note that status flag should be read before using the different parameters because it will provide validity information * about all included outputs. Some frames may not provide the heave period or surge/sway axes for example */ -typedef struct _SbgLogShipMotionData +typedef struct _SbgEComLogShipMotion { uint32_t timeStamp; /*!< Time in us since the sensor power up. */ uint16_t status; /*!< Ship Motion data status bitmask */ @@ -71,32 +73,41 @@ typedef struct _SbgLogShipMotionData float shipMotion[3]; /*!< Surge, sway and heave in meters. */ float shipAccel[3]; /*!< Surge, sway and heave ship Acceleration in m.s^-2. */ float shipVel[3]; /*!< Surge, sway and heave velocities */ -} SbgLogShipMotionData; +} SbgEComLogShipMotion; //----------------------------------------------------------------------// //- Public methods -// //----------------------------------------------------------------------// /*! - * Parse data for the SBG_ECOM_LOG_SHIP_MOTION or SBG_ECOM_LOG_SHIP_MOTION_HP message and fill the corresponding structure. + * Parse data for the SBG_ECOM_LOG_SHIP_MOTION or SBG_ECOM_LOG_SHIP_MOTION_HP message and fill the corresponding structure. * - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. + * \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 sbgEComBinaryLogParseShipMotionData(SbgStreamBuffer *pInputStream, SbgLogShipMotionData *pOutputData); +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[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. + * + * \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 sbgEComBinaryLogWriteShipMotionData(SbgStreamBuffer *pOutputStream, const SbgLogShipMotionData *pInputData); +SbgErrorCode sbgEComLogShipMotionWriteToStream(const SbgEComLogShipMotion *pLogData, SbgStreamBuffer *pStreamBuffer); + +//----------------------------------------------------------------------// +//- DEPRECATED - Used for backward compatibility -// +//----------------------------------------------------------------------// + +SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogShipMotion SbgLogShipMotionData); + +SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogParseShipMotionData(SbgStreamBuffer *pStreamBuffer, SbgEComLogShipMotion *pLogData)); +SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogWriteShipMotionData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogShipMotion *pLogData)); #ifdef __cplusplus } #endif -#endif // SBG_ECOM_BINARY_LOG_SHIP_MOTION_H +#endif // SBG_ECOM_LOG_SHIP_MOTION_H diff --git a/src/logs/sbgEComLogStatus.c b/src/logs/sbgEComLogStatus.c new file mode 100644 index 0000000..0286535 --- /dev/null +++ b/src/logs/sbgEComLogStatus.c @@ -0,0 +1,108 @@ +#include "sbgEComLogStatus.h" + +//----------------------------------------------------------------------// +//- Private definitions -// +//----------------------------------------------------------------------// + +/*! + * 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. */ + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +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); +} + +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); +} + +//----------------------------------------------------------------------// +//- Public setters/getters -// +//----------------------------------------------------------------------// + +void sbgEComLogStatusSetCanBusStatus(SbgEComLogStatus *pLogData, SbgEComCanBusStatus status) +{ + 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; +} + +SbgEComCanBusStatus sbgEComLogStatusGetCanBusStatus(const SbgEComLogStatus *pLogData) +{ + assert(pLogData); + + return (SbgEComCanBusStatus)((pLogData->comStatus >> SBG_ECOM_LOG_STATUS_CAN_BUS_STATUS_SHIFT)&SBG_ECOM_LOG_STATUS_CAN_BUS_STATUS_MASK); +} + +//----------------------------------------------------------------------// +//- DEPRECATED - Used for backward compatibility -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComBinaryLogParseStatusData(SbgStreamBuffer *pStreamBuffer, SbgEComLogStatus *pLogData) +{ + return sbgEComLogStatusReadFromStream(pLogData, pStreamBuffer); +} + +SbgErrorCode sbgEComBinaryLogWriteStatusData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogStatus *pLogData) +{ + 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); +} + +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; +} diff --git a/src/logs/sbgEComLogStatus.h b/src/logs/sbgEComLogStatus.h new file mode 100644 index 0000000..e117410 --- /dev/null +++ b/src/logs/sbgEComLogStatus.h @@ -0,0 +1,212 @@ +/*! + * \file sbgEComLogStatus.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 03 April 2013 + * + * \brief Parse logs used to report device status. + * + * \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 + */ + +#ifndef SBG_ECOM_LOG_STATUS_H +#define SBG_ECOM_LOG_STATUS_H + +// sbgCommonLib headers +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- General status definitions -// +//----------------------------------------------------------------------// +#define SBG_ECOM_GENERAL_MAIN_POWER_OK (0x0001u << 0) /*!< Set to 1 when main power supply is OK. */ +#define SBG_ECOM_GENERAL_IMU_POWER_OK (0x0001u << 1) /*!< Set to 1 when IMU power supply is OK. */ +#define SBG_ECOM_GENERAL_GPS_POWER_OK (0x0001u << 2) /*!< Set to 1 when GPS power supply is OK. */ +#define SBG_ECOM_GENERAL_SETTINGS_OK (0x0001u << 3) /*!< Set to 1 if settings where correctly loaded. */ +#define SBG_ECOM_GENERAL_TEMPERATURE_OK (0x0001u << 4) /*!< Set to 1 when temperature is within specified limits. */ +#define SBG_ECOM_GENERAL_DATALOGGER_OK (0x0001u << 5) /*!< Set to 1 when the datalogger is working correctly. */ +#define SBG_ECOM_GENERAL_CPU_OK (0x0001u << 6) /*!< Set to 1 if the CPU headroom is correct.*/ + +//----------------------------------------------------------------------// +//- Communication status definitions -// +//----------------------------------------------------------------------// + +/*! + * Communication 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. */ + +/*! + * 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. */ +} 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. */ + +//----------------------------------------------------------------------// +//- Aiding status definitions -// +//----------------------------------------------------------------------// +#define SBG_ECOM_AIDING_GPS1_POS_RECV (0x00000001u << 0) /*!< Set to 1 when valid GPS 1 position data is received. */ +#define SBG_ECOM_AIDING_GPS1_VEL_RECV (0x00000001u << 1) /*!< Set to 1 when valid GPS 1 velocity data is received. */ +#define SBG_ECOM_AIDING_GPS1_HDT_RECV (0x00000001u << 2) /*!< Set to 1 when valid GPS 1 true heading data is received. */ +#define SBG_ECOM_AIDING_GPS1_UTC_RECV (0x00000001u << 3) /*!< Set to 1 when valid GPS 1 UTC time data is received. */ +#define SBG_ECOM_AIDING_GPS2_POS_RECV (0x00000001u << 4) /*!< Set to 1 when valid GPS 2 position data is received. */ +#define SBG_ECOM_AIDING_GPS2_VEL_RECV (0x00000001u << 5) /*!< Set to 1 when valid GPS 2 velocity data is received. */ +#define SBG_ECOM_AIDING_GPS2_HDT_RECV (0x00000001u << 6) /*!< Set to 1 when valid GPS 2 true heading data is received. */ +#define SBG_ECOM_AIDING_GPS2_UTC_RECV (0x00000001u << 7) /*!< Set to 1 when valid GPS 2 UTC time data is received. */ +#define SBG_ECOM_AIDING_MAG_RECV (0x00000001u << 8) /*!< Set to 1 when valid Magnetometer data is received. */ +#define SBG_ECOM_AIDING_ODO_RECV (0x00000001u << 9) /*!< Set to 1 when Odometer pulse is received. */ +#define SBG_ECOM_AIDING_DVL_RECV (0x00000001u << 10) /*!< Set to 1 when valid DVL data is received. */ +#define SBG_ECOM_AIDING_USBL_RECV (0x00000001u << 11) /*!< Set to 1 when valid USBL data is received. */ +#define SBG_ECOM_AIDING_DEPTH_RECV (0x00000001u << 12) /*!< Set to 1 when valid Depth Log data is received. */ +#define SBG_ECOM_AIDING_AIR_DATA_RECV (0x00000001u << 13) /*!< Set to 1 when valid Air Data (altitude and/or true airspeed) is received. */ +#define SBG_ECOM_AIDING_USER_POS_RECV (0x00000001u << 14) /*!< Set to 1 when valid user position data is received. */ +#define SBG_ECOM_AIDING_USER_VEL_RECV (0x00000001u << 15) /*!< Set to 1 when valid user velocity data is received. */ +#define SBG_ECOM_AIDING_USER_HEADING_RECV (0x00000001u << 16) /*!< Set to 1 when valid user heading data is received. */ + +//----------------------------------------------------------------------// +//- Status definitions -// +//----------------------------------------------------------------------// + +/*! + * Stores global status data. + */ +typedef struct _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. */ +} SbgEComLogStatus; + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * 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. + */ +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. + */ +SbgErrorCode sbgEComLogStatusWriteToStream(const SbgEComLogStatus *pLogData, SbgStreamBuffer *pStreamBuffer); + +//----------------------------------------------------------------------// +//- Public getters & helpers -// +//----------------------------------------------------------------------// + +/*! + * Defines the CAN bus status. + * + * \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. + */ +SbgEComCanBusStatus sbgEComLogStatusGetCanBusStatus(const SbgEComLogStatus *pLogData); + +//----------------------------------------------------------------------// +//- DEPRECATED - Used for backward compatibility -// +//----------------------------------------------------------------------// + +SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogStatus SbgLogStatusData); + +SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogParseStatusData(SbgStreamBuffer *pStreamBuffer, SbgEComLogStatus *pLogData)); +SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogWriteStatusData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogStatus *pLogData)); + +SBG_DEPRECATED(SbgEComCanBusStatus sbgEComLogStatusGetCanStatus(uint32_t status)); +SBG_DEPRECATED(uint32_t sbgEComLogStatusBuildCommunicationStatus(SbgEComCanBusStatus canStatus, uint32_t masks)); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_LOG_STATUS_H diff --git a/src/logs/sbgEComLogUsbl.c b/src/logs/sbgEComLogUsbl.c new file mode 100644 index 0000000..d31a9c9 --- /dev/null +++ b/src/logs/sbgEComLogUsbl.c @@ -0,0 +1,59 @@ +#include "sbgEComLogUsbl.h" + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComLogUsblReadFromStream(SbgEComLogUsbl *pLogData, SbgStreamBuffer *pStreamBuffer) +{ + assert(pStreamBuffer); + assert(pLogData); + + pLogData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); + pLogData->status = sbgStreamBufferReadUint16LE(pStreamBuffer); + + pLogData->latitude = sbgStreamBufferReadDoubleLE(pStreamBuffer); + pLogData->longitude = sbgStreamBufferReadDoubleLE(pStreamBuffer); + + pLogData->depth = sbgStreamBufferReadFloatLE(pStreamBuffer); + + pLogData->latitudeAccuracy = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->longitudeAccuracy = sbgStreamBufferReadFloatLE(pStreamBuffer); + pLogData->depthAccuracy = sbgStreamBufferReadFloatLE(pStreamBuffer); + + return sbgStreamBufferGetLastError(pStreamBuffer); +} + +SbgErrorCode sbgEComLogUsblWriteToStream(const SbgEComLogUsbl *pLogData, SbgStreamBuffer *pStreamBuffer) +{ + assert(pStreamBuffer); + assert(pLogData); + + sbgStreamBufferWriteUint32LE(pStreamBuffer, pLogData->timeStamp); + sbgStreamBufferWriteUint16LE(pStreamBuffer, pLogData->status); + + sbgStreamBufferWriteDoubleLE(pStreamBuffer, pLogData->latitude); + sbgStreamBufferWriteDoubleLE(pStreamBuffer, pLogData->longitude); + + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->depth); + + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->latitudeAccuracy); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->longitudeAccuracy); + sbgStreamBufferWriteFloatLE(pStreamBuffer, pLogData->depthAccuracy); + + return sbgStreamBufferGetLastError(pStreamBuffer); +} + +//----------------------------------------------------------------------// +//- DEPRECATED - Used for backward compatibility -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComBinaryLogParseUsblData(SbgStreamBuffer *pStreamBuffer, SbgEComLogUsbl *pLogData) +{ + return sbgEComLogUsblReadFromStream(pLogData, pStreamBuffer); +} + +SbgErrorCode sbgEComBinaryLogWriteUsblData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogUsbl *pLogData) +{ + return sbgEComLogUsblWriteToStream(pLogData, pStreamBuffer); +} diff --git a/src/binaryLogs/sbgEComBinaryLogUsbl.h b/src/logs/sbgEComLogUsbl.h similarity index 72% rename from src/binaryLogs/sbgEComBinaryLogUsbl.h rename to src/logs/sbgEComLogUsbl.h index d97f60f..a630e43 100644 --- a/src/binaryLogs/sbgEComBinaryLogUsbl.h +++ b/src/logs/sbgEComLogUsbl.h @@ -1,5 +1,5 @@ /*! - * \file sbgEComBinaryLogUsbl.h + * \file sbgEComLogUsbl.h * \ingroup binaryLogs * \author SBG Systems * \date 02 June 2014 @@ -32,8 +32,8 @@ * \endlicense */ -#ifndef SBG_ECOM_BINARY_LOG_USBL_H -#define SBG_ECOM_BINARY_LOG_USBL_H +#ifndef SBG_ECOM_LOG_USBL_H +#define SBG_ECOM_LOG_USBL_H // sbgCommonLib headers #include @@ -61,7 +61,7 @@ extern "C" { /*! * Log structure for USBL data. */ -typedef struct _SbgLogUsblData +typedef struct _SbgEComLogUsbl { uint32_t timeStamp; /*!< Time in us since the sensor power up. */ uint16_t status; /*!< USBL system status bitmask. */ @@ -74,7 +74,7 @@ typedef struct _SbgLogUsblData float latitudeAccuracy; /*!< 1 sigma latitude accuracy in meters. */ float longitudeAccuracy; /*!< 1 sigma longitude accuracy in meters. */ float depthAccuracy; /*!< 1 sigma depth accuracy in meters. */ -} SbgLogUsblData; +} SbgEComLogUsbl; //----------------------------------------------------------------------// //- Public methods -// @@ -83,23 +83,32 @@ typedef struct _SbgLogUsblData /*! * Parse data for the SBG_ECOM_LOG_USBL message and fill the corresponding structure. * - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. + * \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 sbgEComBinaryLogParseUsblData(SbgStreamBuffer *pInputStream, SbgLogUsblData *pOutputData); +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[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. + * + * \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 sbgEComBinaryLogWriteUsblData(SbgStreamBuffer *pOutputStream, const SbgLogUsblData *pInputData); +SbgErrorCode sbgEComLogUsblWriteToStream(const SbgEComLogUsbl *pLogData, SbgStreamBuffer *pStreamBuffer); + +//----------------------------------------------------------------------// +//- DEPRECATED - Used for backward compatibility -// +//----------------------------------------------------------------------// + +SBG_DEPRECATED_TYPEDEF(typedef struct _SbgEComLogUsbl SbgLogUsblData); + +SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogParseUsblData(SbgStreamBuffer *pStreamBuffer, SbgEComLogUsbl *pLogData)); +SBG_DEPRECATED(SbgErrorCode sbgEComBinaryLogWriteUsblData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogUsbl *pLogData)); #ifdef __cplusplus } #endif -#endif // SBG_ECOM_BINARY_LOG_USBL_H +#endif // SBG_ECOM_LOG_USBL_H diff --git a/src/logs/sbgEComLogUtc.c b/src/logs/sbgEComLogUtc.c new file mode 100644 index 0000000..8e4394b --- /dev/null +++ b/src/logs/sbgEComLogUtc.c @@ -0,0 +1,281 @@ +// sbgCommonLib headers +#include +#include + +// Local headers +#include "sbgEComLogUtc.h" + +//----------------------------------------------------------------------// +//- Private methods -// +//----------------------------------------------------------------------// + +/*! + * 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_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 -// +//----------------------------------------------------------------------// + +void sbgEComLogUtcZeroInit(SbgEComLogUtc *pLogData) +{ + assert(pLogData); + + memset(pLogData, 0x00, sizeof(*pLogData)); + + 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); +} + +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); +} + +//----------------------------------------------------------------------// +//- Public setters/getters -// +//----------------------------------------------------------------------// + +void sbgEComLogUtcSetClockState(SbgEComLogUtc *pLogData, SbgEComClockState status) +{ + 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; +} + +SbgEComClockState sbgEComLogUtcGetClockState(const SbgEComLogUtc *pLogData) +{ + assert(pLogData); + + 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"; + } +} + +void sbgEComLogUtcSetUtcStatus(SbgEComLogUtc *pLogData, SbgEComUtcStatus status) +{ + 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; +} + +SbgEComUtcStatus sbgEComLogUtcGetUtcStatus(const SbgEComLogUtc *pLogData) +{ + assert(pLogData); + + 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"; + } +} + +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; + } +} + +bool sbgEComLogUtcHasClockInput(const SbgEComLogUtc *pLogData) +{ + assert(pLogData); + + 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; + } +} + +bool sbgEComLogUtcTimeIsAccurate(const SbgEComLogUtc *pLogData) +{ + assert(pLogData); + + return (pLogData->status&SBG_ECOM_LOG_UTC_STATUS_UTC_IS_ACCURATE?true:false); +} + +bool sbgEComLogUtcTimeClkBiasStdIsValid(const SbgEComLogUtc *pLogData) +{ + assert(pLogData); + + return (isnan(pLogData->clkBiasStd)?false:true); +} + +bool sbgEComLogUtcTimeClkSfErrorStdIsValid(const SbgEComLogUtc *pLogData) +{ + assert(pLogData); + + return (isnan(pLogData->clkSfErrorStd)?false:true); +} + +bool sbgEComLogUtcTimeClkResidualErrorIsValid(const SbgEComLogUtc *pLogData) +{ + assert(pLogData); + + return (isnan(pLogData->clkResidualError)?false:true); +} + +//----------------------------------------------------------------------// +//- DEPRECATED - Used for backward compatibility -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComBinaryLogParseUtcData(SbgStreamBuffer *pStreamBuffer, SbgEComLogUtc *pLogData) +{ + return sbgEComLogUtcReadFromStream(pLogData, pStreamBuffer); +} + +SbgErrorCode sbgEComBinaryLogWriteUtcData(SbgStreamBuffer *pStreamBuffer, const SbgEComLogUtc *pLogData) +{ + 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); +} + +const char *sbgEcomLogUtcGetClockStatusAsString(const SbgEComLogUtc *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); +} + +const char *sbgEcomLogUtcGetUtcStatusAsString(const SbgEComLogUtc *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; +} diff --git a/src/logs/sbgEComLogUtc.h b/src/logs/sbgEComLogUtc.h new file mode 100644 index 0000000..3c5d200 --- /dev/null +++ b/src/logs/sbgEComLogUtc.h @@ -0,0 +1,281 @@ +/*! + * \file sbgEComLogUtc.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 20 February 2013 + * + * \brief Parse logs used to report device UTC time. + * + * \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 + */ + +#ifndef SBG_ECOM_LOG_UTC_H +#define SBG_ECOM_LOG_UTC_H + +// sbgCommonLib headers +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Public definitions -// +//----------------------------------------------------------------------// + +/*! + * Internal clock alignment to PPS algorithm state. + */ +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. */ +} SbgEComClockState; + +/*! + * Status for the GNSS UTC time information. + */ +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. */ +} SbgEComUtcStatus; + +//----------------------------------------------------------------------// +//- Log structure definitions -// +//----------------------------------------------------------------------// + +/*! + * Structure that stores data for the SBG_ECOM_LOG_UTC_TIME message. + */ +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) */ +} SbgEComLogUtc; + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * Zero initialize the message struct. + * + * \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. + */ +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. + */ +SbgErrorCode sbgEComLogUtcWriteToStream(const SbgEComLogUtc *pLogData, SbgStreamBuffer *pStreamBuffer); + +//----------------------------------------------------------------------// +//- Public setters/getters -// +//----------------------------------------------------------------------// + +/*! + * Set the clock alignment state. + * + * \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. + */ +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. + */ +const char *sbgEComLogUtcGetClockStateAsString(const SbgEComLogUtc *pLogData); + +/*! + * Set the UTC time information status. + * + * \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. + */ +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. + */ +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. + */ +void sbgEComLogUtcSetHasClockInput(SbgEComLogUtc *pLogData, bool hasClockInput); + +/*! + * Returns true if a valid and stable PPS signal is being received to align the INS clock. + * + * Use this status to determine if the INS is constantly correcting the clock with a PPS signal + * or propagating the clock with its internal crystal using latest estimated bias and scale factor corrections. + * + * 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. + */ +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. + */ +void sbgEComLogUtcTimeSetIsAccurate(SbgEComLogUtc *pLogData, bool utcIsAccurate); + +/*! + * Returns true if the UTC time information is considered to be accurate. + * + * The PPS signal is used to compensate for processing and transmission delays to + * precisely timestamp the received UTC time information. + * + * During nominal INS operations with good GNSS reception, this method should return true. + * + * 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. + */ +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. + */ +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. + */ +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. + */ +bool sbgEComLogUtcTimeClkResidualErrorIsValid(const SbgEComLogUtc *pLogData); + +//----------------------------------------------------------------------// +//- DEPRECATED - Used for backward compatibility -// +//----------------------------------------------------------------------// + +#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) +#endif + +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)); + +SBG_DEPRECATED(SbgEComClockState sbgEComLogUtcGetClockStatus(uint16_t status)); +SBG_DEPRECATED(const char *sbgEcomLogUtcGetClockStatusAsString(const SbgEComLogUtc *pLogUtc)); + +SBG_DEPRECATED(SbgEComUtcStatus sbgEComLogUtcGetClockUtcStatus(uint16_t status)); +SBG_DEPRECATED(const char *sbgEcomLogUtcGetUtcStatusAsString(const SbgEComLogUtc *pLogUtc)); + +SBG_DEPRECATED(uint16_t sbgEComLogUtcBuildClockStatus(SbgEComClockState clockState, SbgEComUtcStatus utcStatus, uint16_t masks)); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_LOG_UTC_H diff --git a/src/protocol/sbgEComProtocol.c b/src/protocol/sbgEComProtocol.c index 6766376..0c6ca93 100644 --- a/src/protocol/sbgEComProtocol.c +++ b/src/protocol/sbgEComProtocol.c @@ -121,6 +121,8 @@ static SbgErrorCode sbgEComProtocolFindSyncBytes(SbgEComProtocol *pProtocol, siz SbgErrorCode errorCode; assert(pProtocol); + assert(pOffset); + assert(pProtocol->rxBufferSize > 0); errorCode = SBG_NOT_READY; @@ -365,6 +367,18 @@ static SbgErrorCode sbgEComProtocolFindFrame(SbgEComProtocol *pProtocol, uint8_t // 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) @@ -849,10 +863,9 @@ SbgErrorCode sbgEComProtocolInit(SbgEComProtocol *pProtocol, SbgInterface *pInte assert(pProtocol); assert(pInterface); + memset(pProtocol, 0x00, sizeof(*pProtocol)); + pProtocol->pLinkedInterface = pInterface; - pProtocol->rxBufferSize = 0; - pProtocol->discardSize = 0; - pProtocol->nextLargeTxId = 0; sbgEComProtocolResetLargeTransfer(pProtocol); @@ -889,7 +902,7 @@ SbgErrorCode sbgEComProtocolPurgeIncoming(SbgEComProtocol *pProtocol) sbgEComProtocolClearLargeTransfer(pProtocol); // - // Try to read all iconming data for at least 100 ms and trash them + // Try to read all incoming data for at least 100 ms and trash them /// timeStamp = sbgGetTime(); @@ -1087,6 +1100,14 @@ SbgErrorCode sbgEComProtocolReceive2(SbgEComProtocol *pProtocol, uint8_t *pMsgCl return errorCode; } +void sbgEComProtocolSetOnFrameReceivedCb(SbgEComProtocol *pProtocol, SbgEComProtocolFrameCb pOnFrameReceivedCb, void *pUserArg) +{ + assert(pProtocol); + + pProtocol->pReceiveFrameCb = pOnFrameReceivedCb; + pProtocol->pUserArg = pUserArg; +} + SbgErrorCode sbgEComStartFrameGeneration(SbgStreamBuffer *pOutputStream, uint8_t msgClass, uint8_t msg, size_t *pStreamCursor) { assert(pOutputStream); diff --git a/src/protocol/sbgEComProtocol.h b/src/protocol/sbgEComProtocol.h index bf251d1..9e5620e 100644 --- a/src/protocol/sbgEComProtocol.h +++ b/src/protocol/sbgEComProtocol.h @@ -75,6 +75,29 @@ extern "C" { #define SBG_ECOM_RX_TIME_OUT (450) /*!< Default time out for new frame reception. */ +//----------------------------------------------------------------------// +//- Callbacks definitions -// +//----------------------------------------------------------------------// + +/*! + * Forward declaration. + */ +typedef struct _SbgEComProtocol SbgEComProtocol; + +/*! + * Function called each time a valid sbgECom frame is received. + * + * 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. + */ +typedef void (*SbgEComProtocolFrameCb)(SbgEComProtocol *pProtocol, uint8_t msgClass, uint8_t msgId, SbgStreamBuffer *pReceivedFrame, void *pUserArg); + //----------------------------------------------------------------------// //- Structure definitions -// //----------------------------------------------------------------------// @@ -97,7 +120,7 @@ typedef struct _SbgEComProtocolPayload * * The member variables related to large transfers are valid if and only if the large buffer is valid. */ -typedef struct _SbgEComProtocol +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. */ @@ -105,6 +128,12 @@ typedef struct _SbgEComProtocol 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. // @@ -115,7 +144,7 @@ typedef struct _SbgEComProtocol 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. */ -} SbgEComProtocol; +}; //----------------------------------------------------------------------// //- Public methods (SbgEComProtocolPayload) -// @@ -171,17 +200,17 @@ void *sbgEComProtocolPayloadMoveBuffer(SbgEComProtocolPayload *pPayload); /*! * Initialize the protocol system used to communicate with the product and return the created handle. * - * \param[in] pProtocol Pointer on an allocated protocol structure to initialize. - * \param[in] pInterface Interface to use for read/write operations. - * \return SBG_NO_ERROR if we have initialised the protocol system. + * \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 handle to close. - * \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); @@ -192,7 +221,7 @@ SbgErrorCode sbgEComProtocolClose(SbgEComProtocol *pProtocol); * * WARNING: This method is blocking for 100ms and actively tries to read incoming data. * - * \param[in] pProtocol A valid SbgEComProtocol handle. + * \param[in] pProtocol A valid protocol instance. * \return SBG_NO_ERROR if the incoming data has been purged successfully. */ SbgErrorCode sbgEComProtocolPurgeIncoming(SbgEComProtocol *pProtocol); @@ -203,27 +232,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 handle. - * \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 handle. - * \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); @@ -239,15 +268,29 @@ 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 handle. - * \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); +/*! + * Define the optional function called each time a valid sbgECom frame is received. + * + * This callback is useful to intercept raw sbgECom frames at a low level. + * You can call this method with NULL parameters to uninstall the callback. + * + * 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. + */ +void sbgEComProtocolSetOnFrameReceivedCb(SbgEComProtocol *pProtocol, SbgEComProtocolFrameCb pOnFrameReceivedCb, void *pUserArg); + /*! * Initialize an output stream for an sbgECom frame generation. * @@ -255,12 +298,12 @@ SbgErrorCode sbgEComProtocolReceive2(SbgEComProtocol *pProtocol, uint8_t *pMsgCl * * 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 begining 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); @@ -270,10 +313,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/sbgECom.c b/src/sbgECom.c index f6fd54f..d3c6f9d 100644 --- a/src/sbgECom.c +++ b/src/sbgECom.c @@ -1,6 +1,11 @@ -#include "sbgECom.h" +// sbgCommonLib headers +#include +#include #include + +// Local headers #include "commands/sbgEComCmdCommon.h" +#include "sbgECom.h" //----------------------------------------------------------------------// //- Public methods -// @@ -50,7 +55,7 @@ SbgErrorCode sbgEComClose(SbgEComHandle *pHandle) SbgErrorCode sbgEComHandleOneLog(SbgEComHandle *pHandle) { SbgErrorCode errorCode = SBG_NO_ERROR; - SbgBinaryLogData logData; + SbgEComLogUnion logData; uint8_t receivedMsg; uint8_t receivedMsgClass; size_t payloadSize; @@ -76,7 +81,7 @@ SbgErrorCode sbgEComHandleOneLog(SbgEComHandle *pHandle) // // The received frame is a binary log one // - errorCode = sbgEComBinaryLogParse((SbgEComClass)receivedMsgClass, (SbgEComMsgId)receivedMsg, payloadData, payloadSize, &logData); + errorCode = sbgEComLogParse((SbgEComClass)receivedMsgClass, (SbgEComMsgId)receivedMsg, payloadData, payloadSize, &logData); // // Test if the incoming log has been parsed successfully @@ -97,7 +102,7 @@ SbgErrorCode sbgEComHandleOneLog(SbgEComHandle *pHandle) // // Clean up resources allocated during parsing, if any. // - sbgEComBinaryLogCleanup(&logData, (SbgEComClass)receivedMsgClass, (SbgEComMsgId)receivedMsg); + sbgEComLogCleanup(&logData, (SbgEComClass)receivedMsgClass, (SbgEComMsgId)receivedMsg); } else { @@ -240,7 +245,7 @@ void sbgEComErrorToString(SbgErrorCode errorCode, char errorMsg[256]) strcpy(errorMsg, "SBG_DEVICE_NOT_FOUND: A device couldn't be founded or opened."); break; case SBG_OPERATION_CANCELLED: - strcpy(errorMsg, "SBG_OPERATION_CANCELLED: An operation has been cancelled by a user."); + 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."); diff --git a/src/sbgECom.h b/src/sbgECom.h index 17a7689..e70815b 100644 --- a/src/sbgECom.h +++ b/src/sbgECom.h @@ -44,12 +44,13 @@ extern "C" { // sbgCommonLib headers #include +#include // Local headers #include "sbgECanId.h" #include "sbgEComIds.h" +#include "logs/sbgEComLog.h" #include "protocol/sbgEComProtocol.h" -#include "binaryLogs/sbgEComBinaryLogs.h" //----------------------------------------------------------------------// //- Predefinitions -// @@ -74,7 +75,7 @@ typedef struct _SbgEComHandle SbgEComHandle; * \param[in] pUserArg Optional user supplied argument. * \return SBG_NO_ERROR if the received log has been used successfully. */ -typedef SbgErrorCode (*SbgEComReceiveLogFunc)(SbgEComHandle *pHandle, SbgEComClass msgClass, SbgEComMsgId msg, const SbgBinaryLogData *pLogData, void *pUserArg); +typedef SbgErrorCode (*SbgEComReceiveLogFunc)(SbgEComHandle *pHandle, SbgEComClass msgClass, SbgEComMsgId msg, const SbgEComLogUnion *pLogData, void *pUserArg); //----------------------------------------------------------------------// //- Structures definitions -// diff --git a/src/sbgEComIds.h b/src/sbgEComIds.h index dfe39f8..389ef67 100644 --- a/src/sbgEComIds.h +++ b/src/sbgEComIds.h @@ -76,7 +76,7 @@ typedef enum _SbgEComLog SBG_ECOM_LOG_UTC_TIME = 2, /*!< Provides UTC time reference */ - SBG_ECOM_LOG_IMU_DATA = 3, /*!< Includes IMU status, acc., gyro, temp delta speeds and delta angles values */ + SBG_ECOM_LOG_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) */ @@ -90,14 +90,10 @@ typedef enum _SbgEComLog SBG_ECOM_LOG_GPS1_VEL = 13, /*!< GPS velocities from primary or secondary GPS receiver */ SBG_ECOM_LOG_GPS1_POS = 14, /*!< GPS positions from primary or secondary GPS receiver */ SBG_ECOM_LOG_GPS1_HDT = 15, /*!< GPS true heading from dual antenna system */ - SBG_ECOM_LOG_GPS1_RAW = 31, /*!< GPS 1 raw data for post processing. */ - SBG_ECOM_LOG_GPS1_SAT = 50, /*!< GPS 1 Satellite data. */ SBG_ECOM_LOG_GPS2_VEL = 16, /*!< GPS 2 velocity log data. */ SBG_ECOM_LOG_GPS2_POS = 17, /*!< GPS 2 position log data. */ SBG_ECOM_LOG_GPS2_HDT = 18, /*!< GPS 2 true heading log data. */ - SBG_ECOM_LOG_GPS2_RAW = 38, /*!< GPS 2 raw data for post processing. */ - SBG_ECOM_LOG_GPS2_SAT = 51, /*!< GNSS2 Satellite data. */ SBG_ECOM_LOG_ODO_VEL = 19, /*!< Provides odometer velocity */ @@ -110,23 +106,34 @@ typedef enum _SbgEComLog 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_GPS1_RAW = 31, /*!< GPS 1 raw data for post processing. */ + SBG_ECOM_LOG_SHIP_MOTION_HP = 32, /*!< Return delayed ship motion such as surge, sway, heave. */ SBG_ECOM_LOG_AIR_DATA = 36, /*!< Air Data aiding such as barometric altimeter and true air speed. */ - SBG_ECOM_LOG_USBL = 37, /*!< Raw USBL position data for subsea navigation. */ + SBG_ECOM_LOG_USBL = 37, /*!< Raw USBL position data for sub-sea navigation. */ + + SBG_ECOM_LOG_GPS2_RAW = 38, /*!< GPS 2 raw data for post processing. */ - SBG_ECOM_LOG_IMU_SHORT = 44, /*!< Short IMU message recommended for post processing usages. */ + 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_EVENT_OUT_A = 45, /*!< Event marker used to time stamp each generated Sync Out A signal. */ SBG_ECOM_LOG_EVENT_OUT_B = 46, /*!< Event marker used to time stamp each generated Sync Out B signal. */ - SBG_ECOM_LOG_DEPTH = 47, /*!< Depth sensor measurement log used for subsea navigation. */ + SBG_ECOM_LOG_DEPTH = 47, /*!< Depth sensor measurement log used for sub-sea navigation. */ SBG_ECOM_LOG_DIAG = 48, /*!< Diagnostic log. */ SBG_ECOM_LOG_RTCM_RAW = 49, /*!< RTCM raw data. */ + SBG_ECOM_LOG_GPS1_SAT = 50, /*!< GPS 1 Satellite data. */ + SBG_ECOM_LOG_GPS2_SAT = 51, /*!< GNSS2 Satellite 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_ECOM_NUM_MESSAGES /*!< Helper definition to know the number of ECom messages */ } SbgEComLog; @@ -140,7 +147,7 @@ typedef enum _SbgEComLog1MsgId } SbgEComLog1; /*! - * Enum that defines all the available Nmea output logs from the sbgECom library. + * Enum that defines all the available NMEA output logs from the sbgECom library. */ typedef enum _SbgEComNmeaLog { @@ -158,7 +165,7 @@ typedef enum _SbgEComNmeaLog } SbgEComNmeaLog; /*! - * Enum that defines all the available Proprietary Nmea output logs from the sbgECom library. + * Enum that defines all the available Proprietary NMEA output logs from the sbgECom library. */ typedef enum _SbgEComIdNmea1Log { @@ -192,17 +199,17 @@ typedef enum _SbgEComIdThirdParty SBG_ECOM_THIRD_PARTY_KVH = 1, /*!< Roll, Pitch, Yaw */ SBG_ECOM_THIRD_PARTY_PD0 = 2, /*!< Teledyne PD0 DVL proprietary frame. */ - SBG_ECOM_THIRD_PARTY_SIMRAD_1000 = 3, /*!< Konsberg SimRad 1000 proprietary frame that outputs Roll, Pitch and Heading. */ - SBG_ECOM_THIRD_PARTY_SIMRAD_3000 = 4, /*!< Konsberg SimRad 3000 proprietary frame that outputs Roll, Pitch and Heading. */ + SBG_ECOM_THIRD_PARTY_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, /*!< Konsberg Seapth Binary Log 26 used for MBES FM mode. */ + 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_KONGSBERG_MB = 10, /*!< Kongsberg multibeam 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 */ } SbgEComIdThirdParty; @@ -242,7 +249,6 @@ typedef enum _SbgEComCmd /* GNSS configuration */ SBG_ECOM_CMD_GNSS_1_MODEL_ID = 17, /*!< Set/get GNSS model information */ SBG_ECOM_CMD_GNSS_1_LEVER_ARM_ALIGNMENT = 18, /*!< DEPRECATED: GNSS installation configuration (lever arm, antenna alignments) */ - SBG_ECOM_CMD_GNSS_1_INSTALLATION = 46, /*!< Define or retrieve the GNSS 1 main and secondary lever arms configuration. */ SBG_ECOM_CMD_GNSS_1_REJECT_MODES = 19, /*!< GNSS aiding rejection modes configuration. */ /* Odometer configuration */ @@ -295,9 +301,11 @@ typedef enum _SbgEComCmd /* 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. */ + /* REST API related commands */ - SBG_ECOM_CMD_API_GET = 46, /*!< Command equivalent to the HTTP GET method for a REST API. */ 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 */ diff --git a/src/sbgEComLib.h b/src/sbgEComLib.h index c437b61..7a2beb8 100644 --- a/src/sbgEComLib.h +++ b/src/sbgEComLib.h @@ -55,8 +55,8 @@ extern "C" { #include "sbgECanId.h" #include "sbgEComIds.h" #include "commands/sbgEComCmd.h" +#include "logs/sbgEComLog.h" #include "protocol/sbgEComProtocol.h" -#include "binaryLogs/sbgEComBinaryLogs.h" #include "sbgEComVersion.h" #include "sbgEComGetVersion.h" diff --git a/src/sbgEComVersion.h b/src/sbgEComVersion.h index 2fe6083..2ed547a 100644 --- a/src/sbgEComVersion.h +++ b/src/sbgEComVersion.h @@ -45,9 +45,9 @@ extern "C" { //- Version definitions -// //----------------------------------------------------------------------// -#define SBG_E_COM_VERSION_MAJOR 3 -#define SBG_E_COM_VERSION_MINOR 2 -#define SBG_E_COM_VERSION_REV 4011 +#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 SBG_VERSION_SOFTWARE(SBG_E_COM_VERSION_MAJOR,SBG_E_COM_VERSION_MINOR,SBG_E_COM_VERSION_REV,SBG_E_COM_VERSION_BUILD) diff --git a/src/transfer/sbgEComTransfer.c b/src/transfer/sbgEComTransfer.c index 4096908..8212ac5 100644 --- a/src/transfer/sbgEComTransfer.c +++ b/src/transfer/sbgEComTransfer.c @@ -12,14 +12,15 @@ //----------------------------------------------------------------------// //- Private methods -// //----------------------------------------------------------------------// + /*! * 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) { @@ -77,13 +78,13 @@ static SbgErrorCode sbgEComTransferSendInit(SbgEComHandle *pHandle, uint8_t msgC /*! * 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) { @@ -144,10 +145,10 @@ static SbgErrorCode sbgEComTransferSendData(SbgEComHandle *pHandle, uint8_t msgC /*! * 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) { @@ -200,11 +201,11 @@ static SbgErrorCode sbgEComTransferSendEnd(SbgEComHandle *pHandle, uint8_t msgCl /*! * 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) { @@ -239,7 +240,7 @@ static SbgErrorCode sbgEComTransferReceiveInit(SbgEComHandle *pHandle, uint8_t m if (errorCode == SBG_NO_ERROR) { // - // Wait for reponse, the device should respond with a ECOM_TRANSFER_START command and the transfer size + // 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); @@ -306,13 +307,13 @@ static SbgErrorCode sbgEComTransferReceiveInit(SbgEComHandle *pHandle, uint8_t m /*! * 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) { @@ -349,7 +350,7 @@ static SbgErrorCode sbgEComTransferReceiveData(SbgEComHandle *pHandle, uint8_t m if (errorCode == SBG_NO_ERROR) { // - // Wait for reponse, the device should respond with a ECOM_TRANSFER_DATA, the offset from the start of the transfer and the data payload + // 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); @@ -399,10 +400,10 @@ static SbgErrorCode sbgEComTransferReceiveData(SbgEComHandle *pHandle, uint8_t m /*! * 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) { diff --git a/src/transfer/sbgEComTransfer.h b/src/transfer/sbgEComTransfer.h index 1a22700..76184d4 100644 --- a/src/transfer/sbgEComTransfer.h +++ b/src/transfer/sbgEComTransfer.h @@ -51,7 +51,7 @@ extern "C" { #define SBG_ECOM_TRANSFER_PACKET_SIZE (512u) /*!< Max packet size transmitted in a single frame */ //----------------------------------------------------------------------// -//- Communication protocol structs and definitions -// +//- Communication protocol struct and definitions -// //----------------------------------------------------------------------// /*! diff --git a/tools/sbgBasicLogger/README.md b/tools/sbgBasicLogger/README.md index 0ee1616..1b15887 100644 --- a/tools/sbgBasicLogger/README.md +++ b/tools/sbgBasicLogger/README.md @@ -1,14 +1,27 @@ # sbgBasicLogger -The sbgBasicLogger let you log and display sbgECom binary messages into CSV like text files. +The sbgBasicLogger let you log and display sbgECom binary messages into CSV like text files. +The provided code source is written in C++ 14 to ensure maximum compatibility. ## Output -The output is as close as possible as the sbgECom message definition. -You can both output incoming messages in text files and/or display the data in the terminal. +The output is as close as possible as the sbgECom message definition. +You can output the data in files as well as in the terminal. + +### Standard logs +Most sbgECom logs are output as CSV like text files with an optional header. +Each file only contains one sbgECom log type. The log are written line by line. +The tool can be used to extract error/warning/info messages from `SBG_ECOM_LOG_DIAG` messages. + +### RAW GNSS data & RTCM +This tool can also extract the RAW GNSS data stream `SBG_ECOM_LOG_RTCM_RAW` to a binary file. +For example an ELLIPSE-D Generation 3 has an internal ublox receiver so you should get a binary file containing ubx messages directly. + +It is the same for real time differential correction data stream `SBG_ECOM_LOG_GPS#_RAW`. +The tool can store the received stream to a binary file if you would like to extract RTCM data. ## Decimation -The tool can also decimate incoming IMU data using a simple moving average filter. -You can specify decimation to apply before displayed the data to the terminal and also one before writing the data to text files. +The tool can also decimate incoming IMU data using a simple moving average filter. +You can decimate incoming data using two decimation factors. One for data displayed in the console and one for data written in text files. Only the following logs are decimated: - IMU short @@ -16,15 +29,25 @@ Only the following logs are decimated: - IMU data ## Interfaces -The sbgBasicLogger can be used to parse incoming data from a serial or an Ethernet UDP interface. -You can also select an binary file containing raw sbgECom dump. This tool can thus be used to easily export sbgECom data to CVS like text files. +The sbgBasicLogger can be used to parse incoming data from a serial or an Ethernet UDP interface. +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. +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. + +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 `. # Usage The sbgBasicLogger implements a simple to use command line interface (CLI): ```sh -sbgBasicLogger [-hvwpH] [-a IP address] [-I UDP port in] [-O UDP port out] [-s SERIAL_DEVICE] [-r SERIAL_BAUDRATE] [-i INPUT-FILE] [--dir=DIRECTORY] [-d FILE DECIMATION] [-c CONSOLE DECIMATION] +sbgBasicLogger [-hvwpHt] [-a IP address] [-I UDP port in] [-O UDP port out] [-s SERIAL_DEVICE] [-r SERIAL_BAUDRATE] [-i INPUT-FILE] [-o DIRECTORY] [-d FILE DECIMATION] [-c CONSOLE DECIMATION] [-m timestamp or utcIso8601] ``` ## Serial example and only print on console @@ -62,9 +85,11 @@ sbgBasicLogger -i -p -r, --serial-baudrate=SERIAL_BAUDRATE serial baudrate -i, --input-file=INPUT-FILE input file -w, --write-logs write logs in different files - --dir=DIRECTORY directory to write logs into + -o, --dir=DIRECTORY directory to write logs into -d, --file-decimation=FILE DECIMATION file decimation -c, --console-decimation=CONSOLE DECIMATION output stream decimation -p, --print-logs print the logs on the output stream -H, --disable-header disable header for files -``` \ No newline at end of file + -m, --time-mode=timestamp or utcIso8601 select time base to output + -t, --discard-invalid-time discard data without a valid UTC time +``` diff --git a/tools/sbgBasicLogger/src/loggerApp.cpp b/tools/sbgBasicLogger/src/loggerApp.cpp new file mode 100644 index 0000000..45ed226 --- /dev/null +++ b/tools/sbgBasicLogger/src/loggerApp.cpp @@ -0,0 +1,422 @@ +// STL headers +#include +#include +#include +#include +#include + +// sbgCommonLib headers +#include + +// sbgECom headers +#include +#include + +// Argtable3 headers +#include + +// Project headers +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +// Local headers +#include "loggerApp.h" + +namespace sbg +{ +//----------------------------------------------------------------------// +//- Private methods -// +//----------------------------------------------------------------------// + +std::string CLoggerApp::getApplicationName() const +{ + return "sbgBasicLogger"; +} + +void CLoggerApp::createLogger(const CLoggerSettings &settings) +{ + m_manager = std::make_unique(settings); + + // + // Register all aiding logs + // + m_manager->registerLog(); + m_manager->registerLog(); + m_manager->registerLog(); + m_manager->registerLog(); + + // + // DVL aiding + /// + m_manager->registerLog(); + m_manager->registerLog(); + + // + // Register EKF logs + // + m_manager->registerLog(); + m_manager->registerLog(); + m_manager->registerLog(); + m_manager->registerLog(); + m_manager->registerLog(); + m_manager->registerLog(); + + // + // Register event in/out logs + // + m_manager->registerLog(); + m_manager->registerLog(); + m_manager->registerLog(); + m_manager->registerLog(); + m_manager->registerLog(); + m_manager->registerLog(); + m_manager->registerLog(); + + // + // Register general logs + // + m_manager->registerLog(); + m_manager->registerLog(); + m_manager->registerLog(); + m_manager->registerLog(); + + // + // Register GNSS 1 logs + // + m_manager->registerLog(); + m_manager->registerLog(); + m_manager->registerLog(); + m_manager->registerLog(); + m_manager->registerLog(); + + // + // Register GNSS 2 logs + // + m_manager->registerLog(); + m_manager->registerLog(); + m_manager->registerLog(); + m_manager->registerLog(); + m_manager->registerLog(); + + // + // IMU logs + // + m_manager->registerLog(); + m_manager->registerLog(); + m_manager->registerLog(); + + // + // Magnetometer logs + // + m_manager->registerLog(); + m_manager->registerLog(); + + // + // Ship Motion Logs + // + m_manager->registerLog(); + m_manager->registerLog(); + +} + +void CLoggerApp::process() +{ + CLoggerManager::StreamStatus streamStatus; + + // + // Stop when the end of stream is reached or the user cancels + // If there is no more data in the input stream, just wait until new data arrives + // + do + { + streamStatus = m_manager->processOneLog(); + + if (streamStatus == CLoggerManager::StreamStatus::NoMoreData) + { + sbgSleep(1); + } + } while ( (streamStatus != CLoggerManager::StreamStatus::EndOfStream) && m_continueCb() ); +} + +CLoggerSettings CLoggerApp::processArgs(int argc, char **argv) +{ + CLoggerSettings settings; + + 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_file *pInputFileArg; + struct arg_lit *pWriteLogsArg; + struct arg_str *pWriteLogsDirArg; + struct arg_int *pFileDecimationArg; + struct arg_int *pScreenDecimationArg; + struct arg_lit *pPrintLogsArg; + struct arg_lit *pLogsHeaderArg; + struct arg_str *pStatusFormatArg; + struct arg_str *pTimeModeArg; + struct arg_lit *pDiscardInvalidTimeArg; + struct arg_end *pEndArg; + + void *argTable[] = + { + pHelpArg = arg_lit0( "h", "help", "display this help and exit"), + pVersionArg = arg_lit0( "v", "version", "display sbgECom version 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 in"), + pUdpPortOutArg = arg_int0( "O", "udp-port-out", "UDP port out", "UDP port out"), + + pSerialDeviceArg = arg_str0( "s", "serial-device", "SERIAL_DEVICE", "open a serial interface"), + pSerialBaudrateArg = arg_int0( "r", "serial-baudrate", "SERIAL_BAUDRATE", "serial baudrate"), + + pInputFileArg = arg_file0( "i", "input-file", "INPUT-FILE", "input file"), + + pWriteLogsArg = arg_lit0( "w", "write-logs", "write logs in different files"), + pWriteLogsDirArg = arg_str0( "o", "dir", "DIRECTORY", "directory to write logs into"), + + pFileDecimationArg = arg_int0( "d", "file-decimation", "FILE DECIMATION", "file decimation"), + pScreenDecimationArg = arg_int0( "c", "console-decimation", "CONSOLE DECIMATION", "output stream decimation"), + + pPrintLogsArg = arg_lit0( "p", "print-logs", "print the logs on the output stream"), + pLogsHeaderArg = arg_lit0( "H", "disable-header", "disable header for files"), + + pStatusFormatArg = arg_str0( "f", "status-format", "decimal or hexadecimal", "select the format to use for status fields"), + pTimeModeArg = arg_str0( "m", "time-mode", "timestamp or utcIso8601", "select time base to output"), + pDiscardInvalidTimeArg = arg_lit0( "t", "discard-invalid-time", "discard data without a valid UTC time"), + + pEndArg = arg_end(20), + }; + + if (arg_nullcheck(argTable) != 0) + { + throw std::bad_alloc(); + } + + // + // Parse input arguments and the extract settings + // + int argError = arg_parse(argc, argv, argTable); + + try + { + // + // special case: '--help' and '--version' take precedence over error reporting + // + if (pHelpArg->count != 0) + { + std::cout << "Usage: " << getApplicationName(); + arg_print_syntax(stdout, argTable, "\n\n"); + + std::cout << "Manage sbgECom logs in text format.\n\n" + << "Serial example: " << getApplicationName() << " -s -r -p\n" + << " UDP example: " << getApplicationName() << " -a -I -O -p\n" + << " File example: " << getApplicationName() << " -i -p\n" + << std::endl; + + std::cout << "Logs affected by decimation:\n" + << " - IMU short\n" + << " - IMU fast\n" + << " - IMU data\n" + << std::endl; + + arg_print_glossary(stdout, argTable, " %-50s %s\n"); + } + else if (pVersionArg->count != 0) + { + std::cout << sbgEComGetVersionAsString() << std::endl; + } + else if (argError != 0) + { + throw std::invalid_argument(""); + } + else + { + // + // Parse and set all output related configuration such as decimation, headers etc + // + if (pLogsHeaderArg->count == 0) + { + settings.setWriteHeaderToFile(true); + } + + if (pWriteLogsArg->count != 0) + { + settings.setWriteToFile(true); + + if (pFileDecimationArg->count != 0) + { + settings.setFileDecimation(pFileDecimationArg->ival[0]); + } + + if (pWriteLogsDirArg->count != 0) + { + settings.setBasePath(pWriteLogsDirArg->sval[0]); + } + } + + if (pPrintLogsArg->count != 0) + { + settings.setWriteToConsole(true); + + if (pScreenDecimationArg->count != 0) + { + settings.setConsoleDecimation(pScreenDecimationArg->ival[0]); + } + } + + if (pStatusFormatArg->count != 0) + { + const std::string statusFmt = pStatusFormatArg->sval[0]; + + if (statusFmt == "decimal") + { + settings.setStatusFormat(CLoggerSettings::StatusFormat::Decimal); + } + else if (statusFmt == "hexadecimal") + { + settings.setStatusFormat(CLoggerSettings::StatusFormat::Hexadecimal); + } + else + { + throw std::invalid_argument("invalid status-format argument."); + } + } + + if (pTimeModeArg->count != 0) + { + const std::string timeMode = pTimeModeArg->sval[0]; + + if (timeMode == "timestamp") + { + settings.setTimeMode(CLoggerSettings::TimeMode::TimeStamp); + } + else if (timeMode == "utcIso8601") + { + settings.setTimeMode(CLoggerSettings::TimeMode::UtcIso8601); + } + else + { + throw std::invalid_argument("invalid time-mode argument."); + } + } + + if (pDiscardInvalidTimeArg->count != 0) + { + settings.setDiscardInvalidTime(true); + } + + if (!settings.isOutputConfValid()) + { + throw std::invalid_argument("Invalid mode, missing \"-p\" and/or \"-w\" arguments"); + } + + // + // Parse and set input interface related configuration, user should select only one interface + // + try + { + if ( (pSerialDeviceArg->count != 0) && (pSerialBaudrateArg->count != 0) ) + { + CLoggerSettings::Serial serialConf; + + serialConf.location = pSerialDeviceArg->sval[0]; + serialConf.baudRate = pSerialBaudrateArg->ival[0]; + + settings.setSerialConf(serialConf); + } + + if ( (pUdpAddrArg->count != 0) && (pUdpPortInArg->count != 0) && (pUdpPortOutArg->count != 0) ) + { + CLoggerSettings::Udp udpConf; + + udpConf.remoteIp = pUdpAddrArg->sval[0]; + udpConf.remotePort = pUdpPortOutArg->ival[0]; + udpConf.listenPort = pUdpPortInArg->ival[0]; + + settings.setUdpConf(udpConf); + } + + if (pInputFileArg->count != 0) + { + settings.setFileConf(pInputFileArg->filename[0]); + } + } + catch (...) + { + throw std::invalid_argument("Please select only one input interface."); + } + + if (!settings.hasInterfaceConf()) + { + throw std::invalid_argument("Please select at least one input interface among serial, file or UDP."); + } + } + } + catch (std::exception &e) + { + arg_print_errors(stderr, pEndArg, getApplicationName().c_str()); + arg_freetable(argTable, SBG_ARRAY_SIZE(argTable)); + + throw e; + } + + arg_freetable(argTable, SBG_ARRAY_SIZE(argTable)); + + return settings; +} + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +void CLoggerApp::setContinueCallback(std::function continueCb) noexcept +{ + m_continueCb = continueCb; +} + +int CLoggerApp::exec(int argc, char **argv) noexcept +{ + int exitCode = EXIT_SUCCESS; + + try + { + CLoggerSettings settings = processArgs(argc, argv); + + if (settings.isValid()) + { + createLogger(settings); + process(); + m_manager.reset(nullptr); + } + } + catch (std::exception &e) + { + if (strlen(e.what()) > 0) + { + std::cout << getApplicationName() << ": " << e.what() << std::endl; + } + + std::cout << "Try '" << getApplicationName() << " --help' for more information." << std::endl; + exitCode = EXIT_FAILURE; + } + + return exitCode; +} + +}; // namespace sbg diff --git a/tools/sbgBasicLogger/src/loggerApp.h b/tools/sbgBasicLogger/src/loggerApp.h new file mode 100644 index 0000000..0ba2e41 --- /dev/null +++ b/tools/sbgBasicLogger/src/loggerApp.h @@ -0,0 +1,119 @@ +/*! + * \file loggerApp.h + * \author SBG Systems + * \date March 10, 2023 + * + * \brief Basic Logger main application entry point. + * + * \copyright Copyright (C) 2023, 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 + * 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. + * + * \endlicense + */ + +#ifndef SBG_LOGGER_APP_H +#define SBG_LOGGER_APP_H + +// STL headers +#include +#include + +// sbgCommonLib headers +#include + +// Project headers +#include +#include +#include + +//----------------------------------------------------------------------// +//- Structure definitions -// +//----------------------------------------------------------------------// + +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. */ + + //----------------------------------------------------------------------// + //- 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); + + /*! + * 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); + + 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; + + /*! + * 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.cpp b/tools/sbgBasicLogger/src/loggerEntry/helpers/imuDataMean.cpp new file mode 100644 index 0000000..b82d9b3 --- /dev/null +++ b/tools/sbgBasicLogger/src/loggerEntry/helpers/imuDataMean.cpp @@ -0,0 +1,135 @@ +// STL headers +#include + +// sbgCommonLib headers +#include + +// sbgECom headers +#include + +// Local headers +#include "imuDataMean.h" + +namespace sbg +{ + +//----------------------------------------------------------------------// +//- CImuDataMean -// +//----------------------------------------------------------------------// + +CImuDataMean::CImuDataMean() +{ + reset(); +} + +int32_t CImuDataMean::add(const SbgEComLogImuLegacy &imuData) +{ + m_timeStamp = imuData.timeStamp; + m_status |= (uint32_t)imuData.status; + + m_accelerometer[0] += imuData.deltaVelocity[0]; + m_accelerometer[1] += imuData.deltaVelocity[1]; + m_accelerometer[2] += imuData.deltaVelocity[2]; + + m_gyroscope[0] += imuData.deltaAngle[0]; + m_gyroscope[1] += imuData.deltaAngle[1]; + m_gyroscope[2] += imuData.deltaAngle[2]; + + m_temperature += imuData.temperature; + m_numAccValues += 1; + + return m_numAccValues; +} + +int32_t CImuDataMean::add(const SbgEComLogImuShort &imuData) +{ + m_timeStamp = imuData.timeStamp; + m_status |= (uint32_t)imuData.status; + + m_accelerometer[0] += sbgEComLogImuShortGetDeltaVelocity(&imuData, 0); + m_accelerometer[1] += sbgEComLogImuShortGetDeltaVelocity(&imuData, 1); + m_accelerometer[2] += sbgEComLogImuShortGetDeltaVelocity(&imuData, 2); + + m_gyroscope[0] += sbgEComLogImuShortGetDeltaAngle(&imuData, 0); + m_gyroscope[1] += sbgEComLogImuShortGetDeltaAngle(&imuData, 1); + m_gyroscope[2] += sbgEComLogImuShortGetDeltaAngle(&imuData, 2); + + m_temperature += sbgEComLogImuShortGetTemperature(&imuData); + m_numAccValues += 1; + + return m_numAccValues; +} + +int32_t CImuDataMean::add(const SbgEComLogImuFastLegacy &imuData) +{ + m_timeStamp = imuData.timeStamp; + m_status |= (uint32_t)imuData.status; + + m_accelerometer[0] += imuData.accelerometers[0]; + m_accelerometer[1] += imuData.accelerometers[1]; + m_accelerometer[2] += imuData.accelerometers[2]; + + m_gyroscope[0] += imuData.gyroscopes[0]; + m_gyroscope[1] += imuData.gyroscopes[1]; + m_gyroscope[2] += imuData.gyroscopes[2]; + + m_numAccValues += 1; + + return m_numAccValues; +} + +void CImuDataMean::reset() +{ + m_timeStamp = 0; + m_status = 0; + + m_accelerometer[0] = 0; + m_accelerometer[1] = 0; + m_accelerometer[2] = 0; + + m_gyroscope[0] = 0; + m_gyroscope[1] = 0; + m_gyroscope[2] = 0; + + m_temperature = 0; + m_numAccValues = 0; +} + +uint32_t CImuDataMean::getTimeStamp() const +{ + return m_timeStamp; +} + +uint32_t CImuDataMean::getStatus() const +{ + return m_status; +} + +double CImuDataMean::getAccelerometer(size_t idx) const +{ + assert(idx < m_accelerometer.size()); + assert(m_numAccValues > 0); + + return m_accelerometer[idx] / m_numAccValues; +} + +double CImuDataMean::getGyroscope(size_t idx) const +{ + assert(idx < m_gyroscope.size()); + assert(m_numAccValues > 0); + + return m_gyroscope[idx] / m_numAccValues; +} + +double CImuDataMean::getGyroscopeDeg(size_t idx) const +{ + return sbgRadToDegd(getGyroscope(idx)); +} + +double CImuDataMean::getTemperature() const +{ + assert(m_numAccValues > 0); + + return m_temperature / m_numAccValues; +} +}; // Namespace sbg diff --git a/tools/sbgBasicLogger/src/loggerEntry/helpers/imuDataMean.h b/tools/sbgBasicLogger/src/loggerEntry/helpers/imuDataMean.h new file mode 100644 index 0000000..34387ab --- /dev/null +++ b/tools/sbgBasicLogger/src/loggerEntry/helpers/imuDataMean.h @@ -0,0 +1,139 @@ +/*! + * \file imuDataMean.h + * \author SBG Systems + * \date March 14, 2023 + * + * \brief Implemented simple moving average filter for IMU data entries. + * + * \copyright Copyright (C) 2023, 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 + * 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. + * + * \endlicense + */ + +#ifndef SBG_IMU_DATA_MEAN_H +#define SBG_IMU_DATA_MEAN_H + +// STL headers +#include + +// sbgCommonLib headers +#include + +// sbgECom headers +#include + +namespace sbg +{ + //----------------------------------------------------------------------// + //- 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: + + /*! + * 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; + + /*! + * 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; + + 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.cpp b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryAidings.cpp new file mode 100644 index 0000000..98cfcbd --- /dev/null +++ b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryAidings.cpp @@ -0,0 +1,172 @@ +// STL headers +#include +#include + +// sbgCommonLib headers +#include + +// sbgECom headers +#include + +// Project headers +#include +#include + +// Local headers +#include "loggerEntryAidings.h" + +namespace sbg +{ + +//----------------------------------------------------------------------// +//- CLoggerEntryOdometer -// +//----------------------------------------------------------------------// + +std::string CLoggerEntryOdometer::getName() const +{ + return "odometer"; +} + +void CLoggerEntryOdometer::writeHeaderToFile(const CLoggerContext &context) +{ + m_outFile << context.getTimeColTitle() << "\tstatus\tvelocity\n"; + m_outFile << context.getTimeUnit() << "\t(na)\t(m.s^-1)\n"; +} + +void CLoggerEntryOdometer::writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogOdometer &data = logData.odometerData; + + m_outFile << context.fmtTime(data.timeStamp) << "\t" + << context.fmtStatus(data.status) << "\t" + << data.velocity << "\n"; +} + +void CLoggerEntryOdometer::writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogOdometer &data = logData.odometerData; + + std::cout << std::setw(12) << getName() << ": " + << std::setw(12) << context.fmtStatus(data.status) + << std::setw(12) << data.velocity << "\n"; +} + +//----------------------------------------------------------------------// +//- CLoggerEntryAirData -// +//----------------------------------------------------------------------// + +std::string CLoggerEntryAirData::getName() const +{ + return "airData"; +} + +void CLoggerEntryAirData::writeHeaderToFile(const CLoggerContext &context) +{ + m_outFile << context.getTimeColTitle() << "\tstatus\tpressureAbs\taltitude\tpressureDiff\ttrueAirspeed\tairTemperature\n"; + m_outFile << context.getTimeUnit() << "\t(na)\t(Pa)\t(m)\t(Pa)\t(ms.^-1)\t(degC)\n"; +} + +void CLoggerEntryAirData::writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogAirData &data = logData.airData; + + m_outFile << context.fmtTime(data.timeStamp) << "\t" + << context.fmtStatus(data.status) << "\t" + << data.pressureAbs << "\t" + << data.altitude << "\t" + << data.pressureDiff << "\t" + << data.trueAirspeed << "\t" + << data.airTemperature << "\n"; +} + +void CLoggerEntryAirData::writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogAirData &data = logData.airData; + + std::cout << std::setw(12) << getName() << ": " + << std::setw(12) << context.fmtStatus(data.status) + << std::setw(12) << data.pressureAbs + << std::setw(12) << data.altitude + << std::setw(12) << data.pressureDiff + << std::setw(12) << data.trueAirspeed + << std::setw(12) << data.airTemperature << "\n"; +} + +//----------------------------------------------------------------------// +//- CLoggerEntryDepth -// +//----------------------------------------------------------------------// +std::string CLoggerEntryDepth::getName() const +{ + return "depth"; +} + +void CLoggerEntryDepth::writeHeaderToFile(const CLoggerContext &context) +{ + m_outFile << context.getTimeColTitle() << "\tstatus\tpressureAbs\taltitude\n"; + m_outFile << context.getTimeUnit() << "\t(na)\t(Pa)\t(m)\n"; +} + +void CLoggerEntryDepth::writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogDepth &data = logData.depthData; + + m_outFile << context.fmtTime(data.timeStamp) << "\t" + << context.fmtStatus(data.status) << "\t" + << data.pressureAbs << "\t" + << data.altitude << "\n"; +} + +void CLoggerEntryDepth::writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogDepth &data = logData.depthData; + + std::cout << std::setw(12) << getName() << ": " + << std::setw(12) << context.fmtStatus(data.status) + << std::setw(12) << data.pressureAbs + << std::setw(12) << data.altitude << "\n"; +} + + +//----------------------------------------------------------------------// +//- CLoggerEntryUsbl -// +//----------------------------------------------------------------------// +std::string CLoggerEntryUsbl::getName() const +{ + return "usbl"; +} + +void CLoggerEntryUsbl::writeHeaderToFile(const CLoggerContext &context) +{ + m_outFile << context.getTimeColTitle() << "\tstatus\tlatitude\tlongitude\tdepth\tlatitudeStd\tlongitudeStd\tdepthStd\n"; + m_outFile << context.getTimeUnit() << "\t(na)\t(deg)\t(deg)\t(m)\t(m)\t(m)\t(m)\n"; +} + +void CLoggerEntryUsbl::writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogUsbl &data = logData.usblData; + + m_outFile << context.fmtTime(data.timeStamp) << "\t" + << context.fmtStatus(data.status) << "\t" + << data.latitude << "\t" + << data.longitude << "\t" + << data.depth << "\t" + << data.latitudeAccuracy << "\t" + << data.longitudeAccuracy << "\t" + << data.depthAccuracy << "\n"; +} + +void CLoggerEntryUsbl::writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogUsbl &data = logData.usblData; + + std::cout << std::setw(12) << getName() << ": " + << std::setw(12) << context.fmtStatus(data.status) + << std::setw(12) << data.latitude + << std::setw(12) << data.longitude + << std::setw(12) << data.depth + << std::setw(12) << data.latitudeAccuracy + << std::setw(12) << data.longitudeAccuracy + << std::setw(12) << data.depthAccuracy << "\n"; +} + +}; // Namespace sbg diff --git a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryAidings.h b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryAidings.h new file mode 100644 index 0000000..0a8eee6 --- /dev/null +++ b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryAidings.h @@ -0,0 +1,231 @@ +/*! + * \file loggerEntryAidings.h + * \author SBG Systems + * \date March 14, 2023 + * + * \brief Implement odometer velocity (DMI) aiding + * + * \copyright Copyright (C) 2023, 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 + * 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. + * + * \endlicense + */ + +#ifndef SBG_LOGGER_ENTRY_AIDINGS_H +#define SBG_LOGGER_ENTRY_AIDINGS_H + +// sbgCommonLib headers +#include + +// sbgECom headers +#include + +// Project headers +#include +#include + +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; + }; +}; + +#endif // SBG_LOGGER_ENTRY_AIDINGS_H diff --git a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryDvl.cpp b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryDvl.cpp new file mode 100644 index 0000000..d7a40ec --- /dev/null +++ b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryDvl.cpp @@ -0,0 +1,76 @@ +// STL headers +#include +#include + +// sbgCommonLib headers +#include + +// sbgECom headers +#include + +// Project headers +#include +#include + +// Local headers +#include "loggerEntryDvl.h" + +namespace sbg +{ +//----------------------------------------------------------------------// +//- CLoggerEntryDvl -// +//----------------------------------------------------------------------// + +void CLoggerEntryDvl::writeHeaderToFile(const CLoggerContext &context) +{ + m_outFile << context.getTimeColTitle() << "\tstatus\tvelX\tvelY\tvelZ\tvelQualX\tvelQualY\tvelQualZ\n"; + m_outFile << context.getTimeUnit() << "\t(na)\t(m.s^-1)\t(m.s^-1)\t(m.s^-1)\t(m.s^-1)\t(m.s^-1)\t(m.s^-1)\t(deg)\t(deg)\n"; +} + +void CLoggerEntryDvl::writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogDvl &data = logData.dvlData; + + m_outFile << context.fmtTime(data.timeStamp) << "\t" + << context.fmtStatus(data.status) << "\t" + << data.velocity[0] << "\t" + << data.velocity[1] << "\t" + << data.velocity[2] << "\t" + << data.velocityQuality[0] << "\t" + << data.velocityQuality[1] << "\t" + << data.velocityQuality[2] << "\n"; +} + +void CLoggerEntryDvl::writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogDvl &data = logData.dvlData; + + std::cout << std::setw(12) << getName() << ": " + << std::setw(12) << context.fmtStatus(data.status) + << std::setw(12) << data.velocity[0] + << std::setw(12) << data.velocity[1] + << std::setw(12) << data.velocity[2] + << std::setw(12) << data.velocityQuality[0] + << std::setw(12) << data.velocityQuality[1] + << std::setw(12) << data.velocityQuality[2] << "\n"; +} + +//----------------------------------------------------------------------// +//- CLoggerEntryDvlBottom -// +//----------------------------------------------------------------------// + +std::string CLoggerEntryDvlBottom::getName() const +{ + return "dvlBottom"; +} + +//----------------------------------------------------------------------// +//- CLoggerEntryDvlWater -// +//----------------------------------------------------------------------// + +std::string CLoggerEntryDvlWater::getName() const +{ + return "dvlWater"; +} + +}; // Namespace sbg diff --git a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryDvl.h b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryDvl.h new file mode 100644 index 0000000..1fd57a5 --- /dev/null +++ b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryDvl.h @@ -0,0 +1,112 @@ +/*! + * \file loggerEntryDvl.h + * \author SBG Systems + * \date March 14, 2023 + * + * \brief Implement DVL velocity aidings log handlers + * + * \copyright Copyright (C) 2023, 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 + * 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. + * + * \endlicense + */ + +#ifndef SBG_LOGGER_ENTRY_DVL_H +#define SBG_LOGGER_ENTRY_DVL_H + +// sbgCommonLib headers +#include + +// sbgECom headers +#include + +// Project headers +#include +#include + +namespace sbg +{ + //----------------------------------------------------------------------// + //- DVL Generic Implementation -// + //----------------------------------------------------------------------// + + /*! + * 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 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; + }; + + //----------------------------------------------------------------------// + //- DVL Specializations -// + //----------------------------------------------------------------------// + + /*! + * 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; + }; + + /*! + * 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; + }; +}; + +#endif // SBG_LOGGER_ENTRY_DVL_H diff --git a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryEkf.cpp b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryEkf.cpp new file mode 100644 index 0000000..e338491 --- /dev/null +++ b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryEkf.cpp @@ -0,0 +1,211 @@ +// STL headers +#include +#include + +// sbgCommonLib headers +#include + +// sbgECom headers +#include + +// Project headers +#include +#include + +// Local headers +#include "loggerEntryEkf.h" + +namespace sbg +{ + +//----------------------------------------------------------------------// +//- CLoggerEntryEkfEuler -// +//----------------------------------------------------------------------// + +std::string CLoggerEntryEkfEuler::getName() const +{ + return "euler"; +} + +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"; +} + +void CLoggerEntryEkfEuler::writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogEkfEuler &data = logData.ekfEulerData; + + 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"; +} + +void CLoggerEntryEkfEuler::writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogEkfEuler &data = logData.ekfEulerData; + + 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"; +} + + +//----------------------------------------------------------------------// +//- CLoggerEntryEkfQuat -// +//----------------------------------------------------------------------// + +std::string CLoggerEntryEkfQuat::getName() const +{ + return "quat"; +} + +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"; +} + +void CLoggerEntryEkfQuat::writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogEkfQuat &data = logData.ekfQuatData; + + m_outFile << context.fmtTime(data.timeStamp) << "\t" + << context.fmtStatus(data.status) << "\t" + << data.quaternion[0] << "\t" + << data.quaternion[1] << "\t" + << data.quaternion[2] << "\t" + << data.quaternion[3] << "\t" + << data.eulerStdDev[0] << "\t" + << data.eulerStdDev[1] << "\t" + << data.eulerStdDev[2] << "\n"; +} + +void CLoggerEntryEkfQuat::writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogEkfQuat &data = logData.ekfQuatData; + + std::cout << std::setw(12) << getName() << ": " + << std::setw(12) << context.fmtStatus(data.status) + << std::setw(12) << data.quaternion[0] + << 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"; +} + +//----------------------------------------------------------------------// +//- CLoggerEntryEkfNav -// +//----------------------------------------------------------------------// + +std::string CLoggerEntryEkfNav::getName() const +{ + return "nav"; +} + +void CLoggerEntryEkfNav::writeHeaderToFile(const CLoggerContext &context) +{ + m_outFile << context.getTimeColTitle() << "\tstatus\tvelN\tvelE\tvelD\tvelStdN\tvelStdE\tvelStdD\tlatitude\tlongitude\taltitude\tlatitudeStd\tlongitudeStd\taltitudeStd\tundulation\n"; + m_outFile << context.getTimeUnit() << "\t(na)\t(m.s^-1)\t(m.s^-1)\t(m.s^-1)\t(m.s^-1)\t(m.s^-1)\t(m.s^-1)\t(deg)\t(deg)\t(m)\t(m)\t(m)\t(m)\t(m)\n"; +} + +void CLoggerEntryEkfNav::writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogEkfNav &data = logData.ekfNavData; + + m_outFile << context.fmtTime(data.timeStamp) << "\t" + << context.fmtStatus(data.status) << "\t" + << data.velocity[0] << "\t" + << data.velocity[1] << "\t" + << data.velocity[2] << "\t" + << data.velocityStdDev[0] << "\t" + << data.velocityStdDev[1] << "\t" + << data.velocityStdDev[2] << "\t" + << data.position[0] << "\t" + << data.position[1] << "\t" + << data.position[2] << "\t" + << data.positionStdDev[0] << "\t" + << data.positionStdDev[1] << "\t" + << data.positionStdDev[2] << "\t" + << data.undulation << "\n"; +} + +void CLoggerEntryEkfNav::writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogEkfNav &data = logData.ekfNavData; + + std::cout << std::setw(12) << getName() << ": " + << std::setw(12) << context.fmtStatus(data.status) + << std::setw(12) << data.velocity[0] + << std::setw(12) << data.velocity[1] + << std::setw(12) << data.velocity[2] + << std::setw(12) << data.velocityStdDev[0] + << std::setw(12) << data.velocityStdDev[1] + << std::setw(12) << data.velocityStdDev[2] + << std::setw(12) << data.position[0] + << std::setw(12) << data.position[1] + << std::setw(12) << data.position[2] + << std::setw(12) << data.positionStdDev[0] + << std::setw(12) << data.positionStdDev[1] + << std::setw(12) << data.positionStdDev[2] + << std::setw(12) << data.undulation << "\n"; +} + + +//----------------------------------------------------------------------// +//- CLoggerEntryEkfVelBody -// +//----------------------------------------------------------------------// + +std::string CLoggerEntryEkfVelBody::getName() const +{ + return "velBody"; +} + +void CLoggerEntryEkfVelBody::writeHeaderToFile(const CLoggerContext &context) +{ + m_outFile << context.getTimeColTitle() << "\tstatus\tvelX\tvelY\tvelZ\tvelStdX\tvelStdY\tvelStdZ\n"; + m_outFile << context.getTimeUnit() << "\t(na)\t(m.s^-1)\t(m.s^-1)\t(m.s^-1)\t(m.s^-1)\t(m.s^-1)\t(m.s^-1)\n"; +} + +void CLoggerEntryEkfVelBody::writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogEkfVelBody &data = logData.ekfVelBody; + + m_outFile << context.fmtTime(data.timeStamp) << "\t" + << context.fmtStatus(data.status) << "\t" + << data.velocity[0] << "\t" + << data.velocity[1] << "\t" + << data.velocity[2] << "\t" + << data.velocityStdDev[0] << "\t" + << data.velocityStdDev[1] << "\t" + << data.velocityStdDev[2] << "\n"; +} + +void CLoggerEntryEkfVelBody::writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogEkfVelBody &data = logData.ekfVelBody; + + std::cout << std::setw(12) << getName() << ": " + << std::setw(12) << context.fmtStatus(data.status) + << std::setw(12) << data.velocity[0] + << std::setw(12) << data.velocity[1] + << std::setw(12) << data.velocity[2] + << std::setw(12) << data.velocityStdDev[0] + << std::setw(12) << data.velocityStdDev[1] + << std::setw(12) << data.velocityStdDev[2] << "\n"; +} + +}; // Namespace sbg diff --git a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryEkf.h b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryEkf.h new file mode 100644 index 0000000..f3ae648 --- /dev/null +++ b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryEkf.h @@ -0,0 +1,232 @@ +/*! + * \file loggerEntryEkf.h + * \author SBG Systems + * \date March 14, 2023 + * + * \brief Implemented INS output such as attitude & navigation logs. + * + * \copyright Copyright (C) 2023, 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 + * 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. + * + * \endlicense + */ + +#ifndef SBG_LOGGER_ENTRY_EKF_H +#define SBG_LOGGER_ENTRY_EKF_H + +// sbgCommonLib headers +#include + +// sbgECom headers +#include + +// Project headers +#include +#include + +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; + + }; +}; + +#endif // SBG_LOGGER_ENTRY_EKF_H diff --git a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryEkfRotAccel.cpp b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryEkfRotAccel.cpp new file mode 100644 index 0000000..6495989 --- /dev/null +++ b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryEkfRotAccel.cpp @@ -0,0 +1,107 @@ +// STL headers +#include +#include + +// sbgCommonLib headers +#include + +// sbgECom headers +#include + +// Project headers +#include +#include + +// Local headers +#include "loggerEntryEkfRotAccel.h" + +namespace sbg +{ + +//----------------------------------------------------------------------// +//- CLoggerEntryEkfRotAccelBody -// +//----------------------------------------------------------------------// + +std::string CLoggerEntryEkfRotAccelBody::getName() const +{ + return "rotAccelBody"; +} + +void CLoggerEntryEkfRotAccelBody::writeHeaderToFile(const CLoggerContext &context) +{ + m_outFile << context.getTimeColTitle() << "\tstatus\taccelX\taccelY\taccelZ\trateX\trateY\trateZ\n"; + m_outFile << context.getTimeUnit() << "\t(na)\t(m.s^-2)\t(m.s^-2)\t(m.s^-2)\t(deg.s^-1)\t(deg.s^-1)\t(deg.s^-1)\n"; +} + +void CLoggerEntryEkfRotAccelBody::writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogEkfRotAccel &data = logData.ekfRotAccel; + + m_outFile << context.fmtTime(data.timeStamp) << "\t" + << context.fmtStatus(data.status) << "\t" + << data.acceleration[0] << "\t" + << data.acceleration[1] << "\t" + << data.acceleration[2] << "\t" + << sbgRadToDegf(data.rate[0]) << "\t" + << sbgRadToDegf(data.rate[1]) << "\t" + << sbgRadToDegf(data.rate[2]) << "\n"; +} + +void CLoggerEntryEkfRotAccelBody::writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogEkfRotAccel &data = logData.ekfRotAccel; + + std::cout << std::setw(12) << getName() << ": " + << std::setw(12) << context.fmtStatus(data.status) + << std::setw(12) << data.acceleration[0] + << std::setw(12) << data.acceleration[1] + << std::setw(12) << data.acceleration[2] + << std::setw(12) << sbgRadToDegf(data.rate[0]) + << std::setw(12) << sbgRadToDegf(data.rate[1]) + << std::setw(12) << sbgRadToDegf(data.rate[2]) << "\n"; +} + +//----------------------------------------------------------------------// +//- CLoggerEntryEkfRotAccelNed -// +//----------------------------------------------------------------------// + +std::string CLoggerEntryEkfRotAccelNed::getName() const +{ + return "rotAccelNed"; +} + +void CLoggerEntryEkfRotAccelNed::writeHeaderToFile(const CLoggerContext &context) +{ + m_outFile << context.getTimeColTitle() << "\tstatus\taccelN\taccelE\taccelD\trateN\trateE\trateD\n"; + m_outFile << context.getTimeUnit() << "\t(na)\t(m.s^-2)\t(m.s^-2)\t(m.s^-2)\t(deg.s^-1)\t(deg.s^-1)\t(deg.s^-1)\n"; +} + +void CLoggerEntryEkfRotAccelNed::writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogEkfRotAccel &data = logData.ekfRotAccel; + + m_outFile << context.fmtTime(data.timeStamp) << "\t" + << context.fmtStatus(data.status) << "\t" + << data.acceleration[0] << "\t" + << data.acceleration[1] << "\t" + << data.acceleration[2] << "\t" + << sbgRadToDegf(data.rate[0]) << "\t" + << sbgRadToDegf(data.rate[1]) << "\t" + << sbgRadToDegf(data.rate[2]) << "\n"; +} + +void CLoggerEntryEkfRotAccelNed::writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogEkfRotAccel &data = logData.ekfRotAccel; + + std::cout << std::setw(12) << getName() << ": " + << std::setw(12) << context.fmtStatus(data.status) + << std::setw(12) << data.acceleration[0] + << std::setw(12) << data.acceleration[1] + << std::setw(12) << data.acceleration[2] + << std::setw(12) << sbgRadToDegf(data.rate[0]) + << std::setw(12) << sbgRadToDegf(data.rate[1]) + << std::setw(12) << sbgRadToDegf(data.rate[2]) << "\n"; +} + +}; // Namespace sbg diff --git a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryEkfRotAccel.h b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryEkfRotAccel.h new file mode 100644 index 0000000..f05a5d0 --- /dev/null +++ b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryEkfRotAccel.h @@ -0,0 +1,135 @@ +/*! + * \file loggerEntryEkfRotAccel.h + * \author SBG Systems + * \date June 12, 2023 + * + * \brief Implements body/NED rotation rate and accelerations. + * + * \copyright Copyright (C) 2023, 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 + * 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. + * + * \endlicense + */ + +#ifndef SBG_LOGGER_ENTRY_EKF_ROT_ACCEL_H +#define SBG_LOGGER_ENTRY_EKF_ROT_ACCEL_H + +// sbgCommonLib headers +#include + +// sbgECom headers +#include + +// Project headers +#include +#include + +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; + }; +}; + +#endif // SBG_LOGGER_ENTRY_EKF_ROT_ACCEL_H diff --git a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryEvent.cpp b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryEvent.cpp new file mode 100644 index 0000000..83d0fc2 --- /dev/null +++ b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryEvent.cpp @@ -0,0 +1,117 @@ +// STL headers +#include +#include + +// sbgCommonLib headers +#include + +// sbgECom headers +#include + +// Project headers +#include +#include + +// Local headers +#include "loggerEntryEvent.h" + +namespace sbg +{ +//----------------------------------------------------------------------// +//- CLoggerEntryEvent -// +//----------------------------------------------------------------------// + +void CLoggerEntryEvent::writeHeaderToFile(const CLoggerContext &context) +{ + m_outFile << context.getTimeColTitle() << "\tstatus\ttimeOffset0\ttimeOffset1\ttimeOffset2\ttimeOffset3\n"; + m_outFile << context.getTimeUnit() << "\t(na)\t(us)\t(us)\t(us)\t(us)\n"; +} + +void CLoggerEntryEvent::writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogEvent &data = logData.eventMarker; + + m_outFile << context.fmtTime(data.timeStamp) << "\t" + << context.fmtStatus(data.status) << "\t" + << (uint32_t)data.timeOffset0 << "\t" + << (uint32_t)data.timeOffset1 << "\t" + << (uint32_t)data.timeOffset2 << "\t" + << (uint32_t)data.timeOffset3 << "\n"; +} + +void CLoggerEntryEvent::writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogEvent &data = logData.eventMarker; + + std::cout << std::setw(12) << getName() << ": " + << std::setw(12) << context.fmtStatus(data.status) + << std::setw(12) << (uint32_t)data.timeOffset0 + << std::setw(12) << (uint32_t)data.timeOffset1 + << std::setw(12) << (uint32_t)data.timeOffset2 + << std::setw(12) << (uint32_t)data.timeOffset3 << "\n"; +} + +//----------------------------------------------------------------------// +//- CLoggerEntryEventInA -// +//----------------------------------------------------------------------// + +std::string CLoggerEntryEventInA::getName() const +{ + return "eventInA"; +} + +//----------------------------------------------------------------------// +//- CLoggerEntryEventInB -// +//----------------------------------------------------------------------// + +std::string CLoggerEntryEventInB::getName() const +{ + return "eventInB"; +} + +//----------------------------------------------------------------------// +//- CLoggerEntryEventInC -// +//----------------------------------------------------------------------// + +std::string CLoggerEntryEventInC::getName() const +{ + return "eventInC"; +} + +//----------------------------------------------------------------------// +//- CLoggerEntryEventInD -// +//----------------------------------------------------------------------// + +std::string CLoggerEntryEventInD::getName() const +{ + return "eventInD"; +} + +//----------------------------------------------------------------------// +//- CLoggerEntryEventInE -// +//----------------------------------------------------------------------// + +std::string CLoggerEntryEventInE::getName() const +{ + return "eventInE"; +} + +//----------------------------------------------------------------------// +//- CLoggerEntryEventOutA -// +//----------------------------------------------------------------------// + +std::string CLoggerEntryEventOutA::getName() const +{ + return "eventOutA"; +} + +//----------------------------------------------------------------------// +//- CLoggerEntryEventOutB -// +//----------------------------------------------------------------------// + +std::string CLoggerEntryEventOutB::getName() const +{ + return "eventOutB"; +} + +}; // Namespace sbg diff --git a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryEvent.h b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryEvent.h new file mode 100644 index 0000000..eac8e8c --- /dev/null +++ b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryEvent.h @@ -0,0 +1,188 @@ +/*! + * \file loggerEntryEvent.h + * \author SBG Systems + * \date March 14, 2023 + * + * \brief Implement Sync In/Out event handlers. + * + * \copyright Copyright (C) 2023, 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 + * 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. + * + * \endlicense + */ + +#ifndef SBG_LOGGER_ENTRY_EVENT_H +#define SBG_LOGGER_ENTRY_EVENT_H + +// sbgCommonLib headers +#include + +// sbgECom headers +#include + +// Project headers +#include +#include + +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; + }; + +}; + +#endif // SBG_LOGGER_ENTRY_EVENT_H diff --git a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryGeneral.cpp b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryGeneral.cpp new file mode 100644 index 0000000..ba6b10f --- /dev/null +++ b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryGeneral.cpp @@ -0,0 +1,225 @@ +// STL headers +#include +#include + +// sbgCommonLib headers +#include + +// sbgECom headers +#include + +// Project headers +#include +#include + +// Local headers +#include "loggerEntryGeneral.h" + +namespace sbg +{ + +//----------------------------------------------------------------------// +//- CLoggerEntryUtcTime -// +//----------------------------------------------------------------------// + +std::string CLoggerEntryUtcTime::getName() const +{ + return "utcTime"; +} + +bool CLoggerEntryUtcTime::preProcess(CLoggerContext &context, const SbgEComLogUnion &logData) +{ + context.setUtcTime(logData.utcData); + + return true; +} + +void CLoggerEntryUtcTime::writeHeaderToFile(const CLoggerContext &context) +{ + m_outFile << context.getTimeColTitle() << "\tstatus\tgpsTimeOfWeek\tyear\tmonth\tday\thour\tminute\tsecond\tnanosecond\tclkBiasStd\tclkSfErrorStd\tclkResidualError\n"; + m_outFile << context.getTimeUnit() << "\t(na)\t(ms)\t(yyyy)\t(mm)\t(dd)\t(hh)\t(mm)\t(s)\t(ns)\t(us)\t(ppm)\t(us)\n"; +} + +void CLoggerEntryUtcTime::writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogUtc &data = logData.utcData; + + m_outFile << context.fmtTime(data.timeStamp) << "\t" + << context.fmtStatus(data.status) << "\t" + << (uint32_t)data.gpsTimeOfWeek << "\t" + << (int32_t)data.year << "\t" + << (int32_t)data.month << "\t" + << (int32_t)data.day << "\t" + << (int32_t)data.hour << "\t" + << (int32_t)data.minute << "\t" + << (int32_t)data.second << "\t" + << (int32_t)data.nanoSecond << "\t" + << data.clkBiasStd*1e6f << "\t" + << data.clkSfErrorStd*1e9f << "\t" + << data.clkResidualError*1e6f << "\n"; +} + +void CLoggerEntryUtcTime::writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogUtc &data = logData.utcData; + + std::cout << std::setw(12) << getName() << ": " + << std::setw(12) << context.fmtStatus(data.status) + << std::setw(12) << (uint32_t)data.gpsTimeOfWeek + << std::setw(12) << (int32_t)data.year + << std::setw(12) << (int32_t)data.month + << std::setw(12) << (int32_t)data.day + << std::setw(12) << (int32_t)data.hour + << std::setw(12) << (int32_t)data.minute + << std::setw(12) << (int32_t)data.second + << std::setw(12) << (int32_t)data.nanoSecond + << std::setw(12) << data.clkBiasStd*1e6f + << std::setw(12) << data.clkSfErrorStd*1e9f + << std::setw(12) << data.clkResidualError*1e6f << "\n"; +} + +//----------------------------------------------------------------------// +//- CLoggerEntryStatus -// +//----------------------------------------------------------------------// + +std::string CLoggerEntryStatus::getName() const +{ + return "status"; +} + +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"; +} + +void CLoggerEntryStatus::writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogStatus &data = logData.statusData; + + m_outFile << context.fmtTime(data.timeStamp) << "\t" + << context.fmtStatus(data.generalStatus) << "\t" + << context.fmtStatus(data.comStatus) << "\t" + << context.fmtStatus(data.comStatus2) << "\t" + << context.fmtStatus(data.aidingStatus) << "\n"; +} + +void CLoggerEntryStatus::writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogStatus &data = logData.statusData; + + std::cout << std::setw(12) << getName() << ": " + << 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"; +} + +//----------------------------------------------------------------------// +//- CLoggerEntryDiag -// +//----------------------------------------------------------------------// + +std::string CLoggerEntryDiag::getName() const +{ + return "diag"; +} + +bool CLoggerEntryDiag::getDiscardData(const CLoggerContext &context) const +{ + SBG_UNUSED_PARAMETER(context); + + return false; +} + +void CLoggerEntryDiag::writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogDiagData &data = logData.diagData; + + if (data.type == SBG_DEBUG_LOG_TYPE_ERROR) + { + m_outFile << context.fmtTime(data.timestamp) + << " *ERR * " + << data.string + << " [" << sbgErrorCodeToString(data.errorCode) << "]" + << std::endl; + } + else if (data.type == SBG_DEBUG_LOG_TYPE_WARNING) + { + m_outFile << context.fmtTime(data.timestamp) + << " *WARN * " + << data.string + << " [" << sbgErrorCodeToString(data.errorCode) << "]" + << std::endl; + } + else if (data.type == SBG_DEBUG_LOG_TYPE_INFO) + { + m_outFile << context.fmtTime(data.timestamp) + << " *INFO * " + << data.string + << std::endl; + } + else if (data.type == SBG_DEBUG_LOG_TYPE_DEBUG) + { + m_outFile << context.fmtTime(data.timestamp) + << " *DBG * " + << data.string + << std::endl; + } + else + { + m_outFile << context.fmtTime(data.timestamp) + << " *UKN * [" << sbgErrorCodeToString(data.errorCode) << "]: " + << data.string + << std::endl; + } +} + +void CLoggerEntryDiag::writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogDiagData &data = logData.diagData; + + SBG_UNUSED_PARAMETER(context); + + if (data.type == SBG_DEBUG_LOG_TYPE_ERROR) + { + std::cout << " *ERR * " + << data.string + << " [" << sbgErrorCodeToString(data.errorCode) << "]" + << std::endl; + } + else if (data.type == SBG_DEBUG_LOG_TYPE_WARNING) + { + std::cout << " *WARN * " + << data.string + << " [" << sbgErrorCodeToString(data.errorCode) << "]" + << std::endl; + } +} + +//----------------------------------------------------------------------// +//- CLoggerEntryRtcmRaw -// +//----------------------------------------------------------------------// + +std::string CLoggerEntryRtcmRaw::getName() const +{ + return "rtcm"; +} + +std::string CLoggerEntryRtcmRaw::getFileName() const +{ + return getName() + ".bin"; +} + +bool CLoggerEntryRtcmRaw::isBinary() const +{ + return true; +} + +void CLoggerEntryRtcmRaw::writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + SBG_UNUSED_PARAMETER(context); + + m_outFile.write(reinterpret_cast(logData.rtcmRawData.rawBuffer), logData.rtcmRawData.bufferSize); +} + +}; // Namespace sbg diff --git a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryGeneral.h b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryGeneral.h new file mode 100644 index 0000000..63e5dbd --- /dev/null +++ b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryGeneral.h @@ -0,0 +1,243 @@ +/*! + * \file loggerEntryGeneral.h + * \author SBG Systems + * \date March 07, 2023 + * + * \brief Implement general sbgECom log such as status/utc/etc + * + * \copyright Copyright (C) 2023, 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 + * 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. + * + * \endlicense + */ + +#ifndef SBG_LOGGER_ENTRY_GENERAL_H +#define SBG_LOGGER_ENTRY_GENERAL_H + +// sbgCommonLib headers +#include + +// sbgECom headers +#include + +// Project headers +#include +#include + +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; + }; +}; + +#endif // SBG_LOGGER_ENTRY_GENERAL_H diff --git a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryGnss.cpp b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryGnss.cpp new file mode 100644 index 0000000..97ac830 --- /dev/null +++ b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryGnss.cpp @@ -0,0 +1,290 @@ +// STL headers +#include +#include + +// sbgCommonLib headers +#include + +// sbgECom headers +#include + +// Project headers +#include +#include + +// Local headers +#include "loggerEntryGnss.h" + +namespace sbg +{ +//----------------------------------------------------------------------// +//- CLoggerEntryGnssVel -// +//----------------------------------------------------------------------// + +void CLoggerEntryGnssVel::writeHeaderToFile(const CLoggerContext &context) +{ + m_outFile << context.getTimeColTitle() << "\tstatus\tgpsTow\tvelN\tvelE\tvelD\tvelStdN\tvelStdE\tvelStdD\ttrack\ttrackStd\n"; + m_outFile << context.getTimeUnit() << "\t(na)\t(ms)\t(m.s^-1)\t(m.s^-1)\t(m.s^-1)\t(m.s^-1)\t(m.s^-1)\t(m.s^-1)\t(deg)\t(deg)\n"; +} + +void CLoggerEntryGnssVel::writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogGnssVel &data = logData.gpsVelData; + + m_outFile << context.fmtTime(data.timeStamp) << "\t" + << context.fmtStatus(data.status) << "\t" + << data.timeOfWeek << "\t" + << data.velocity[0] << "\t" + << data.velocity[1] << "\t" + << data.velocity[2] << "\t" + << data.velocityAcc[0] << "\t" + << data.velocityAcc[1] << "\t" + << data.velocityAcc[2] << "\t" + << data.course << "\t" + << data.courseAcc << "\n"; +} + +void CLoggerEntryGnssVel::writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogGnssVel &data = logData.gpsVelData; + + std::cout << std::setw(12) << getName() << ": " + << std::setw(12) << context.fmtStatus(data.status) + << std::setw(12) << data.timeOfWeek + << std::setw(12) << data.velocity[0] + << std::setw(12) << data.velocity[1] + << std::setw(12) << data.velocity[2] + << std::setw(12) << data.velocityAcc[0] + << std::setw(12) << data.velocityAcc[1] + << std::setw(12) << data.velocityAcc[2] + << std::setw(12) << data.course + << std::setw(12) << data.courseAcc << "\n"; +} + +//----------------------------------------------------------------------// +//- CLoggerEntryGnssPos -// +//----------------------------------------------------------------------// + +void CLoggerEntryGnssPos::writeHeaderToFile(const CLoggerContext &context) +{ + m_outFile << context.getTimeColTitle() << "\tstatus\tstatusExt\tgpsTow\tlatitude\tlongitude\taltitude\tundulation\tlatitudeStd\tlongitudeStd\taltitudeStd\tnumSvTracked\tnumSvUsed\tbaseStationId\tdiffAge\n"; + m_outFile << context.getTimeUnit() << "\t(na)\t(na)\t(ms)\t(deg)\t(deg)\t(m)\t(m)\t(m)\t(m)\t(m)\t(na)\t(na)\t(na)\t(0.01s)\n"; +} + +void CLoggerEntryGnssPos::writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogGnssPos &data = logData.gpsPosData; + + m_outFile << context.fmtTime(data.timeStamp) << "\t" + << context.fmtStatus(data.status) << "\t" + << context.fmtStatus(data.statusExt) << "\t" + << data.timeOfWeek << "\t" + << data.latitude << "\t" + << data.longitude << "\t" + << data.altitude << "\t" + << data.undulation << "\t" + << data.latitudeAccuracy << "\t" + << data.longitudeAccuracy << "\t" + << data.altitudeAccuracy << "\t" + << (uint32_t)data.numSvTracked << "\t" + << (uint32_t)data.numSvUsed << "\t" + << (uint32_t)data.baseStationId << "\t" + << (uint32_t)data.differentialAge << "\n"; +} + +void CLoggerEntryGnssPos::writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogGnssPos &data = logData.gpsPosData; + + std::cout << std::setw(12) << getName() << ": " + << std::setw(12) << context.fmtStatus(data.status) + << std::setw(12) << context.fmtStatus(data.statusExt) + << std::setw(12) << data.timeOfWeek + << std::setw(12) << data.latitude + << std::setw(12) << data.longitude + << std::setw(12) << data.altitude + << std::setw(12) << data.undulation + << std::setw(12) << data.latitudeAccuracy + << std::setw(12) << data.longitudeAccuracy + << std::setw(12) << data.altitudeAccuracy + << std::setw(12) << (uint32_t)data.numSvTracked + << std::setw(12) << (uint32_t)data.numSvUsed + << std::setw(12) << (uint32_t)data.baseStationId + << std::setw(12) << (uint32_t)data.differentialAge << "\n"; +} + +//----------------------------------------------------------------------// +//- CLoggerEntryGnssHdt -// +//----------------------------------------------------------------------// + +void CLoggerEntryGnssHdt::writeHeaderToFile(const CLoggerContext &context) +{ + m_outFile << context.getTimeColTitle() << "\tstatus\tgpsTow\theading\theadingStd\tpitch\tpitchStd\tbaseline\tnumSvTracked\tnumSvUsed\n"; + m_outFile << context.getTimeUnit() << "\t(na)\t(ms)\t(deg)\t(deg)\t(deg)\t(deg)\t(m)\t(na)\t(na)\n"; +} + +void CLoggerEntryGnssHdt::writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogGnssHdt &data = logData.gpsHdtData; + + m_outFile << context.fmtTime(data.timeStamp) << "\t" + << context.fmtStatus(data.status) << "\t" + << data.timeOfWeek << "\t" + << data.heading << "\t" + << data.headingAccuracy << "\t" + << data.pitch << "\t" + << data.pitchAccuracy << "\t" + << data.baseline << "\t" + << data.numSvTracked << "\t" + << data.numSvUsed << "\n"; +} + +void CLoggerEntryGnssHdt::writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogGnssHdt &data = logData.gpsHdtData; + + std::cout << std::setw(12) << getName() << ": " + << std::setw(12) << context.fmtStatus(data.status) + << std::setw(12) << data.timeOfWeek + << std::setw(12) << data.heading + << std::setw(12) << data.headingAccuracy + << 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"; +} + +//----------------------------------------------------------------------// +//- CLoggerEntryGnssRaw -// +//----------------------------------------------------------------------// + +std::string CLoggerEntryGnssRaw::getFileName() const +{ + return getName() + ".bin"; +} + +bool CLoggerEntryGnssRaw::isBinary() const +{ + return true; +} + +void CLoggerEntryGnssRaw::writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + SBG_UNUSED_PARAMETER(context); + + m_outFile.write(reinterpret_cast(logData.gpsRawData.rawBuffer), logData.gpsRawData.bufferSize); +} + +//----------------------------------------------------------------------// +//- CLoggerEntryGnssSat -// +//----------------------------------------------------------------------// + +void CLoggerEntryGnssSat::writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogSatList &data = logData.satGroupData; + + m_outFile << context.fmtTime(data.timeStamp) << " | Satellites: " << std::setw(3) << data.nrSatellites << "====================================================================\n"; +} + +void CLoggerEntryGnssSat::writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogSatList &data = logData.satGroupData; + + SBG_UNUSED_PARAMETER(context); + + std::cout << std::setw(12) << getName() << ": " + << std::setw(12) << data.nrSatellites << "\n"; +} + +//----------------------------------------------------------------------// +//- CLoggerEntryGnss1Vel -// +//----------------------------------------------------------------------// + +std::string CLoggerEntryGnss1Vel::getName() const +{ + return "gnss1Vel"; +} + +//----------------------------------------------------------------------// +//- CLoggerEntryGnss1Pos -// +//----------------------------------------------------------------------// + +std::string CLoggerEntryGnss1Pos::getName() const +{ + return "gnss1Pos"; +} + +//----------------------------------------------------------------------// +//- CLoggerEntryGnss1Hdt -// +//----------------------------------------------------------------------// + +std::string CLoggerEntryGnss1Hdt::getName() const +{ + return "gnss1Hdt"; +} + +//----------------------------------------------------------------------// +//- CLoggerEntryGnss1Raw -// +//----------------------------------------------------------------------// + +std::string CLoggerEntryGnss1Raw::getName() const +{ + return "gnss1Raw"; +} + +//----------------------------------------------------------------------// +//- CLoggerEntryGnss1Sat -// +//----------------------------------------------------------------------// + +std::string CLoggerEntryGnss1Sat::getName() const +{ + return "gnss1Sat"; +} + +//----------------------------------------------------------------------// +//- CLoggerEntryGnss2Vel -// +//----------------------------------------------------------------------// + +std::string CLoggerEntryGnss2Vel::getName() const +{ + return "gnss2Vel"; +} + +//----------------------------------------------------------------------// +//- CLoggerEntryGnss2Pos -// +//----------------------------------------------------------------------// + +std::string CLoggerEntryGnss2Pos::getName() const +{ + return "gnss2Pos"; +} + +//----------------------------------------------------------------------// +//- CLoggerEntryGnss2Hdt -// +//----------------------------------------------------------------------// + +std::string CLoggerEntryGnss2Hdt::getName() const +{ + return "gnss2Hdt"; +} + +//----------------------------------------------------------------------// +//- CLoggerEntryGnss2Raw -// +//----------------------------------------------------------------------// + +std::string CLoggerEntryGnss2Raw::getName() const +{ + return "gnss2Raw"; +} + +//----------------------------------------------------------------------// +//- CLoggerEntryGnss2Sat -// +//----------------------------------------------------------------------// + +std::string CLoggerEntryGnss2Sat::getName() const +{ + return "gnss2Sat"; +} + +}; // Namespace sbg diff --git a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryGnss.h b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryGnss.h new file mode 100644 index 0000000..6a3d568 --- /dev/null +++ b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryGnss.h @@ -0,0 +1,377 @@ +/*! + * \file loggerEntryGnss.h + * \author SBG Systems + * \date March 06, 2023 + * + * \brief Implement GNSS log handlers + * + * \copyright Copyright (C) 2023, 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 + * 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. + * + * \endlicense + */ + +#ifndef SBG_LOGGER_ENTRY_GNSS_H +#define SBG_LOGGER_ENTRY_GNSS_H + +// sbgCommonLib headers +#include + +// sbgECom headers +#include + +// Project headers +#include +#include + +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; + }; +}; + +#endif // SBG_LOGGER_ENTRY_GNSS_H diff --git a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryImu.cpp b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryImu.cpp new file mode 100644 index 0000000..21606f8 --- /dev/null +++ b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryImu.cpp @@ -0,0 +1,142 @@ +// STL headers +#include +#include + +// sbgCommonLib headers +#include + +// sbgECom headers +#include + +// Project headers +#include +#include + +// Local headers +#include "loggerEntryImu.h" +#include "helpers/imuDataMean.h" + +namespace sbg +{ +//----------------------------------------------------------------------// +//- CLoggerEntryImu -// +//----------------------------------------------------------------------// + +template +void CLoggerEntryImu::writeImuDataToFile(const CLoggerContext &context, const T &imuData) +{ + int32_t accValues = m_fileValues.add(imuData); + + if (accValues >= context.getSettings().getFileDecimation()) + { + m_outFile << context.fmtTime(m_fileValues.getTimeStamp()) << "\t" + << context.fmtStatus(m_fileValues.getStatus()) << "\t" + << m_fileValues.getAccelerometer(0) << "\t" + << m_fileValues.getAccelerometer(1) << "\t" + << m_fileValues.getAccelerometer(2) << "\t" + << m_fileValues.getGyroscopeDeg(0) << "\t" + << m_fileValues.getGyroscopeDeg(1) << "\t" + << m_fileValues.getGyroscopeDeg(2) << "\t" + << m_fileValues.getTemperature() << "\n"; + + m_fileValues.reset(); + } +} + +template +void CLoggerEntryImu::writeImuDataToConsole(const CLoggerContext &context, const T &imuData) +{ + int32_t accValues = m_consoleValues.add(imuData); + + if (accValues >= context.getSettings().getConsoleDecimation()) + { + std::cout << std::setw(12) << getName() << ": " + << std::setw(12) << context.fmtStatus(m_consoleValues.getStatus()) + << std::setw(12) << m_consoleValues.getAccelerometer(0) + << std::setw(12) << m_consoleValues.getAccelerometer(1) + << std::setw(12) << m_consoleValues.getAccelerometer(2) + << std::setw(12) << m_consoleValues.getGyroscopeDeg(0) + << std::setw(12) << m_consoleValues.getGyroscopeDeg(1) + << std::setw(12) << m_consoleValues.getGyroscopeDeg(2) + << std::setw(12) << m_consoleValues.getTemperature() << "\n"; + + m_consoleValues.reset(); + } +} + +void CLoggerEntryImu::writeHeaderToFile(const CLoggerContext &context) +{ + m_outFile << context.getTimeColTitle() << "\tstatus\taccelX\taccelY\taccelZ\tgyroX\tgyroY\tgyroZ\ttemperature\n"; + m_outFile << context.getTimeUnit() << "\t(na)\t(m.s^-2)\t(m.s^-2)\t(m.s^-2)\t(deg.s^-1)\t(deg.s^-1)\t(deg.s^-1)\t(degC)\n"; +} + +//----------------------------------------------------------------------// +//- CLoggerEntryImuData -// +//----------------------------------------------------------------------// + +std::string CLoggerEntryImuData::getName() const +{ + return "imuData"; +} + +void CLoggerEntryImuData::writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogImuLegacy &data = logData.imuData; + + writeImuDataToFile(context, data); +} + +void CLoggerEntryImuData::writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogImuLegacy &data = logData.imuData; + + writeImuDataToConsole(context, data); +} + +//----------------------------------------------------------------------// +//- CLoggerEntryImuShort -// +//----------------------------------------------------------------------// + +std::string CLoggerEntryImuShort::getName() const +{ + return "imuShort"; +} + +void CLoggerEntryImuShort::writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogImuShort &data = logData.imuShort; + + writeImuDataToFile(context, data); +} + +void CLoggerEntryImuShort::writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogImuShort &data = logData.imuShort; + + writeImuDataToConsole(context, data); +} + +//----------------------------------------------------------------------// +//- CLoggerEntryImuFast -// +//----------------------------------------------------------------------// + +std::string CLoggerEntryImuFast::getName() const +{ + return "imuFast"; +} + +void CLoggerEntryImuFast::writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogImuFastLegacy &data = logData.fastImuData; + + writeImuDataToFile(context, data); +} + +void CLoggerEntryImuFast::writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogImuFastLegacy &data = logData.fastImuData; + + writeImuDataToConsole(context, data); + +} +}; // Namespace sbg diff --git a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryImu.h b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryImu.h new file mode 100644 index 0000000..c4befef --- /dev/null +++ b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryImu.h @@ -0,0 +1,204 @@ +/*! + * \file loggerEntryImu.h + * \author SBG Systems + * \date March 14, 2023 + * + * \brief Implemented IMU with optional decimation. + * + * \copyright Copyright (C) 2023, 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 + * 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. + * + * \endlicense + */ + +#ifndef SBG_LOGGER_ENTRY_IMU_H +#define SBG_LOGGER_ENTRY_IMU_H + +// sbgCommonLib headers +#include + +// sbgECom headers +#include + +// Project headers +#include +#include + +// Local headers +#include "helpers/imuDataMean.h" + +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; + }; +}; + +#endif // SBG_LOGGER_ENTRY_IMU_H diff --git a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryMag.cpp b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryMag.cpp new file mode 100644 index 0000000..07c5f9f --- /dev/null +++ b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryMag.cpp @@ -0,0 +1,90 @@ +// STL headers +#include +#include + +// sbgCommonLib headers +#include + +// sbgECom headers +#include + +// Project headers +#include +#include + +// Local headers +#include "loggerEntryMag.h" + +namespace sbg +{ + +//----------------------------------------------------------------------// +//- CLoggerEntryMag -// +//----------------------------------------------------------------------// + +std::string CLoggerEntryMag::getName() const +{ + return "mag"; +} + +void CLoggerEntryMag::writeHeaderToFile(const CLoggerContext &context) +{ + m_outFile << context.getTimeColTitle() << "\tstatus\tmagX\tmagY\tmagZ\taccelX\taccelY\taccelZ\n"; + m_outFile << context.getTimeUnit() << "\t(na)\t(au)\t(au)\t(au)\t(m.s^-2)\t(m.s^-2)\t(m.s^-2)\n"; +} + +void CLoggerEntryMag::writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogMag &data = logData.magData; + + m_outFile << context.fmtTime(data.timeStamp) << "\t" + << context.fmtStatus(data.status) << "\t" + << data.magnetometers[0] << "\t" + << data.magnetometers[1] << "\t" + << data.magnetometers[2] << "\t" + << data.accelerometers[0] << "\t" + << data.accelerometers[1] << "\t" + << data.accelerometers[2] << "\n"; +} + +void CLoggerEntryMag::writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogMag &data = logData.magData; + + std::cout << std::setw(12) << getName() << ": " + << std::setw(12) << context.fmtStatus(data.status) + << std::setw(12) << data.magnetometers[0] + << std::setw(12) << data.magnetometers[1] + << std::setw(12) << data.magnetometers[2] + << std::setw(12) << data.accelerometers[0] + << std::setw(12) << data.accelerometers[1] + << std::setw(12) << data.accelerometers[2] << "\n"; +} + +//----------------------------------------------------------------------// +//- CLoggerEntryMagCalib -// +//----------------------------------------------------------------------// + +std::string CLoggerEntryMagCalib::getName() const +{ + return "magCalib"; +} + +std::string CLoggerEntryMagCalib::getFileName() const +{ + return getName() + ".bin"; +} + +bool CLoggerEntryMagCalib::isBinary() const +{ + return true; +} + +void CLoggerEntryMagCalib::writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + SBG_UNUSED_PARAMETER(context); + + m_outFile.write(reinterpret_cast(logData.magCalibData.magData), sizeof(logData.magCalibData.magData)); +} + +}; // Namespace sbg diff --git a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryMag.h b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryMag.h new file mode 100644 index 0000000..acb0653 --- /dev/null +++ b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryMag.h @@ -0,0 +1,137 @@ +/*! + * \file loggerEntryMag.h + * \author SBG Systems + * \date March 13, 2023 + * + * \brief Implemented magnetometers releated logs. + * + * \copyright Copyright (C) 2023, 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 + * 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. + * + * \endlicense + */ + +#ifndef SBG_LOGGER_ENTRY_MAG_H +#define SBG_LOGGER_ENTRY_MAG_H + +// sbgCommonLib headers +#include + +// sbgECom headers +#include + +// Project headers +#include +#include + +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; + }; +}; + +#endif // SBG_LOGGER_ENTRY_MAG_H diff --git a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryShipMotion.cpp b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryShipMotion.cpp new file mode 100644 index 0000000..5b028e8 --- /dev/null +++ b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryShipMotion.cpp @@ -0,0 +1,85 @@ +// STL headers +#include +#include + +// sbgCommonLib headers +#include + +// sbgECom headers +#include + +// Project headers +#include +#include + +// Local headers +#include "loggerEntryShipMotion.h" + +namespace sbg +{ + +//----------------------------------------------------------------------// +//- CLoggerEntryShipMotion -// +//----------------------------------------------------------------------// + +void CLoggerEntryShipMotion::writeHeaderToFile(const CLoggerContext &context) +{ + m_outFile << context.getTimeColTitle() << "\tstatus\theavePeriod\tsurge\tsway\theave\tsurgeAccel\tswayAccel\theaveAccel\tsurgeVel\tswayVel\theaveVel\n"; + m_outFile << context.getTimeUnit() << "\t(na)\t(s)\t(m)\t(m)\t(m)\t(m.s^-2)\t(m.s^-2\t(m.s^-2)\t(m.s^-1)\t(m.s^-1)\t(m.s^-1)\n"; +} + +void CLoggerEntryShipMotion::writeDataToFile(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogShipMotion &data = logData.shipMotionData; + + m_outFile << context.fmtTime(data.timeStamp) << "\t" + << context.fmtStatus(data.status) << "\t" + << data.mainHeavePeriod << "\t" + << data.shipMotion[0] << "\t" + << data.shipMotion[1] << "\t" + << data.shipMotion[2] << "\t" + << data.shipAccel[0] << "\t" + << data.shipAccel[1] << "\t" + << data.shipAccel[2] << "\t" + << data.shipVel[0] << "\t" + << data.shipVel[1] << "\t" + << data.shipVel[2] << "\n"; +} + +void CLoggerEntryShipMotion::writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + const SbgEComLogShipMotion &data = logData.shipMotionData; + + std::cout << std::setw(12) << getName() << ": " + << std::setw(12) << context.fmtStatus(data.status) + << std::setw(12) << data.mainHeavePeriod + << std::setw(12) << data.shipMotion[0] + << std::setw(12) << data.shipMotion[1] + << std::setw(12) << data.shipMotion[2] + << std::setw(12) << data.shipAccel[0] + << std::setw(12) << data.shipAccel[1] + << std::setw(12) << data.shipAccel[2] + << std::setw(12) << data.shipVel[0] + << std::setw(12) << data.shipVel[1] + << std::setw(12) << data.shipVel[2] << "\n"; +} + +//----------------------------------------------------------------------// +//- CLoggerEntryShipMotionRt -// +//----------------------------------------------------------------------// + +std::string CLoggerEntryShipMotionRt::getName() const +{ + return "shipMotion"; +} + +//----------------------------------------------------------------------// +//- CLoggerEntryShipMotionHp -// +//----------------------------------------------------------------------// + +std::string CLoggerEntryShipMotionHp::getName() const +{ + return "shipMotionHp"; +} + +}; // Namespace sbg diff --git a/tools/sbgBasicLogger/src/loggerEntry/loggerEntryShipMotion.h b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryShipMotion.h new file mode 100644 index 0000000..0f36ae2 --- /dev/null +++ b/tools/sbgBasicLogger/src/loggerEntry/loggerEntryShipMotion.h @@ -0,0 +1,112 @@ +/*! + * \file loggerEntryShipMotion.h + * \author SBG Systems + * \date March 14, 2023 + * + * \brief Implemented Ship Motion logs (heave/surge/sway). + * + * \copyright Copyright (C) 2023, 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 + * 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. + * + * \endlicense + */ + +#ifndef SBG_LOGGER_ENTRY_SHIP_MOTION_H +#define SBG_LOGGER_ENTRY_SHIP_MOTION_H + +// sbgCommonLib headers +#include + +// sbgECom headers +#include + +// Project headers +#include +#include + +namespace sbg +{ + //----------------------------------------------------------------------// + //- Ship Motion Generic Implementation -// + //----------------------------------------------------------------------// + + /*! + * 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 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; + }; + + //----------------------------------------------------------------------// + //- Ship Motion Specializations -// + //----------------------------------------------------------------------// + + /*! + * 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; + }; + + /*! + * 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; + }; +}; + +#endif // SBG_LOGGER_ENTRY_SHIP_MOTION_H diff --git a/tools/sbgBasicLogger/src/loggerManager/loggerContext.cpp b/tools/sbgBasicLogger/src/loggerManager/loggerContext.cpp new file mode 100644 index 0000000..ae3116a --- /dev/null +++ b/tools/sbgBasicLogger/src/loggerManager/loggerContext.cpp @@ -0,0 +1,131 @@ +// STL headers +#include +#include +#include +#include +#include + +// sbgCommonLib headers +#include + +// sbgECom headers +#include + +// Local headers +#include "loggerContext.h" +#include "loggerSettings.h" + +namespace sbg +{ + +//----------------------------------------------------------------------// +//- Constructor / destructor -// +//----------------------------------------------------------------------// + +CLoggerContext::CLoggerContext(const CLoggerSettings &settings): +m_settings(settings) +{ +} + +//----------------------------------------------------------------------// +//- Getters/settings -// +//----------------------------------------------------------------------// + +const CLoggerSettings &CLoggerContext::getSettings() const +{ + return m_settings; +} + +void CLoggerContext::setUtcTime(const SbgEComLogUtc &utcTime) +{ + m_lastUtcTime = utcTime; + + if (sbgEComLogUtcGetUtcStatus(&m_lastUtcTime) != SBG_ECOM_UTC_STATUS_INVALID) + { + m_utcTimeIsValid = true; + } +} + +bool CLoggerContext::isUtcTimeValid() const +{ + return m_utcTimeIsValid; +} + +std::string CLoggerContext::getTimeColTitle() const +{ + if (getSettings().getTimeMode() == CLoggerSettings::TimeMode::UtcIso8601) + { + return "iso8601Time"; + } + else + { + return "timestamp"; + } +} + +std::string CLoggerContext::getTimeUnit() const +{ + if (getSettings().getTimeMode() == CLoggerSettings::TimeMode::UtcIso8601) + { + return "(yyyy-mm-ddThh:mm:ss.sssZ)"; + } + else + { + return "(us)"; + } +} + +std::string CLoggerContext::fmtTime(uint32_t timeStampUs) const +{ + char fullTimeStr[sizeof("yyyy-mm-ddThh:mm:ss.sssZ")]; + + 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; + + utcTime.tm_hour = m_lastUtcTime.hour; + utcTime.tm_min = m_lastUtcTime.minute; + utcTime.tm_sec = m_lastUtcTime.second; + + // + // Output standard UTC time only + // + utcTime.tm_isdst = 0; + + // + // Apply the offset between the latest UTC time and the current time stamp + // + 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; + + 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); + } + else + { + std::snprintf(fullTimeStr, sizeof(fullTimeStr), "%*" PRIu32, (int)sizeof(fullTimeStr)-1, timeStampUs); + } + } + else + { + std::snprintf(fullTimeStr, sizeof(fullTimeStr), "%10" PRIu32, timeStampUs); + } + + return fullTimeStr; +} + +}; // Namespace sbg diff --git a/tools/sbgBasicLogger/src/loggerManager/loggerContext.h b/tools/sbgBasicLogger/src/loggerManager/loggerContext.h new file mode 100644 index 0000000..2269dcf --- /dev/null +++ b/tools/sbgBasicLogger/src/loggerManager/loggerContext.h @@ -0,0 +1,141 @@ +/*! + * \file loggerContext.h + * \author SBG Systems + * \date March 06, 2023 + * + * \brief Define the logger context such as current UTC time. + * + * \copyright Copyright (C) 2023, 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 + * 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. + * + * \endlicense + */ + +#ifndef SBG_LOGGER_CONTEXT_H +#define SBG_LOGGER_CONTEXT_H + +// STL headers +#include +#include +#include + +// sbgCommonLib headers +#include + +// sbgECom headers +#include + +// Local headers +#include "loggerSettings.h" + +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. */ + }; +}; + +#endif // SBG_LOGGER_SETTINGS_H diff --git a/tools/sbgBasicLogger/src/loggerManager/loggerEntry.cpp b/tools/sbgBasicLogger/src/loggerManager/loggerEntry.cpp new file mode 100644 index 0000000..0eaf536 --- /dev/null +++ b/tools/sbgBasicLogger/src/loggerManager/loggerEntry.cpp @@ -0,0 +1,139 @@ +// STL headers +#include +#include +#include +#include + +// sbgCommonLib headers +#include +#include + +// sbgECom headers +#include + +// Local headers +#include "loggerEntry.h" +#include "loggerContext.h" + +namespace sbg +{ + +//----------------------------------------------------------------------// +//- Constructor/destructor -// +//----------------------------------------------------------------------// + +ILoggerEntry::~ILoggerEntry() +{ +} + +//----------------------------------------------------------------------// +//- Public getters -// +//----------------------------------------------------------------------// + +std::string ILoggerEntry::getFileName() const +{ + return getName() + ".txt"; +} + +bool ILoggerEntry::isBinary() const +{ + return false; +} + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +void ILoggerEntry::process(CLoggerContext &context, const SbgEComLogUnion &logData) +{ + if (preProcess(context, logData)) + { + if (!getDiscardData(context)) + { + if (context.getSettings().getWriteToConsole()) + { + // + // Define the float format for console output + // We do it every time we have to output to console in case it has been changed by any other std::cout operation + // + std::cout << std::fixed; + + writeDataToConsole(context, logData); + } + + if (context.getSettings().getWriteToFile()) + { + createFile(context); + + if (context.getSettings().getWriteHeaderToFile() && !m_headerWritten) + { + writeHeaderToFile(context); + m_headerWritten = true; + } + + writeDataToFile(context, logData); + } + } + } +} + +//----------------------------------------------------------------------// +//- Private methods -// +//----------------------------------------------------------------------// + +bool ILoggerEntry::getDiscardData(const CLoggerContext &context) const +{ + if (context.getSettings().getDiscardInvalidTime()) + { + return !context.isUtcTimeValid(); + } + else + { + return false; + } +} + +void ILoggerEntry::createFile(const CLoggerContext &context) +{ + if (!m_outFile.is_open()) + { + std::ofstream::openmode mode; + + if (isBinary()) + { + mode = std::ofstream::out | std::ofstream::binary; + } + else + { + mode = std::ofstream::out; + } + + m_outFile.open(context.getSettings().getBasePath() + getFileName(), mode); + + // + // Configure the file output stream float format. + // + m_outFile << std::setprecision(9) << std::fixed; + } +} + +bool ILoggerEntry::preProcess(CLoggerContext &context, const SbgEComLogUnion &logData) +{ + SBG_UNUSED_PARAMETER(context); + SBG_UNUSED_PARAMETER(logData); + + return true; +} + +void ILoggerEntry::writeHeaderToFile(const CLoggerContext &context) +{ + SBG_UNUSED_PARAMETER(context); +} + +void ILoggerEntry::writeDataToConsole(const CLoggerContext &context, const SbgEComLogUnion &logData) +{ + SBG_UNUSED_PARAMETER(context); + SBG_UNUSED_PARAMETER(logData); +} + +}; // Namespace sbg diff --git a/tools/sbgBasicLogger/src/loggerManager/loggerEntry.h b/tools/sbgBasicLogger/src/loggerManager/loggerEntry.h new file mode 100644 index 0000000..7c37187 --- /dev/null +++ b/tools/sbgBasicLogger/src/loggerManager/loggerEntry.h @@ -0,0 +1,221 @@ +/*! + * \file loggerEntry.h + * \author SBG Systems + * \date July 30, 2021 + * + * \brief All log handlers should implement ILoggerEntryKey. + * + * \copyright Copyright (C) 2022, 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 + * 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. + * + * \endlicense + */ + +#ifndef SBG_LOGGER_ENTRY_H +#define SBG_LOGGER_ENTRY_H + +// STL headers +#include +#include +#include + +// sbgCommonLib headers +#include + +// sbgECom headers +#include + +// Local headers +#include "loggerContext.h" + +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); + } + }; +}; + +#endif // SBG_LOGGER_ENTRY_H diff --git a/tools/sbgBasicLogger/src/loggerManager/loggerManager.cpp b/tools/sbgBasicLogger/src/loggerManager/loggerManager.cpp new file mode 100644 index 0000000..4c969df --- /dev/null +++ b/tools/sbgBasicLogger/src/loggerManager/loggerManager.cpp @@ -0,0 +1,155 @@ +// STL headers +#include +#include + +// sbgCommonLib headers +#include +#include +#include +#include +#include + +// sbgECom headers +#include + +// Local headers +#include "loggerContext.h" +#include "loggerEntry.h" +#include "loggerManager.h" +#include "loggerSettings.h" + +namespace sbg +{ + +//----------------------------------------------------------------------// +//- Constructor/destructor -// +//----------------------------------------------------------------------// + +CLoggerManager::CLoggerManager(const CLoggerSettings &settings): +m_context(settings) +{ + openInterface(); + + if (sbgEComInit(&m_ecomHandle, &m_interface) != SBG_NO_ERROR) + { + throw std::runtime_error("unable to init sbgECom library"); + } + + sbgEComSetReceiveLogCallback(&m_ecomHandle, onSbgEComLogReceived, this); +} + +CLoggerManager::~CLoggerManager() +{ + sbgEComClose(&m_ecomHandle); + sbgInterfaceDestroy(&m_interface); +} + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +CLoggerManager::StreamStatus CLoggerManager::processOneLog() +{ + SbgErrorCode errorCode; + StreamStatus streamStatus; + + errorCode = sbgEComHandleOneLog(&m_ecomHandle); + + if (errorCode == SBG_NOT_READY) + { + if (sbgInterfaceTypeGet(&m_interface) == SBG_IF_TYPE_FILE) + { + streamStatus = StreamStatus::EndOfStream; + } + else + { + streamStatus = StreamStatus::NoMoreData; + } + } + else + { + streamStatus = StreamStatus::HasMoreData; + } + + return streamStatus; +} + +//----------------------------------------------------------------------// +//- Private methods -// +//----------------------------------------------------------------------// + +void CLoggerManager::openInterface() +{ + auto interfaceMode = m_context.getSettings().getInterfaceMode(); + + if (interfaceMode == CLoggerSettings::InterfaceMode::Serial) + { + SbgErrorCode errorCode = sbgInterfaceSerialCreate(&m_interface, m_context.getSettings().getSerialConf().location.c_str(), + m_context.getSettings().getSerialConf().baudRate); + + if (errorCode != SBG_NO_ERROR) + { + throw std::invalid_argument("unable to open serial interface " + m_context.getSettings().getSerialConf().location); + } + } + else if (interfaceMode == CLoggerSettings::InterfaceMode::Udp) + { + SbgErrorCode errorCode = sbgInterfaceUdpCreate(&m_interface, sbgNetworkIpFromString(m_context.getSettings().getUdpConf().remoteIp.c_str()), + m_context.getSettings().getUdpConf().remotePort, + m_context.getSettings().getUdpConf().listenPort); + + if (errorCode == SBG_NO_ERROR) + { + // + // Enable connected mode to only keep data from the specified host/port + // + sbgInterfaceUdpSetConnectedMode(&m_interface, true); + } + else + { + throw std::invalid_argument("unable to open UDP interface " + m_context.getSettings().getUdpConf().remoteIp); + } + } + else if (interfaceMode == CLoggerSettings::InterfaceMode::File) + { + SbgErrorCode errorCode = sbgInterfaceFileOpen(&m_interface, m_context.getSettings().getFileConf().c_str()); + + if (errorCode != SBG_NO_ERROR) + { + throw std::invalid_argument("unable to open file " + m_context.getSettings().getFileConf()); + } + } + else + { + throw std::logic_error("no valid interface configuration!"); + } +} + +SbgErrorCode CLoggerManager::onSbgEComLogReceived(SbgEComHandle *pHandle, SbgEComClass msgClass, SbgEComMsgId msgId, const SbgEComLogUnion *pLogData, void *pUserArg) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + CLoggerManager *pLogManager; + + assert(pHandle); + assert(pLogData); + assert(pUserArg); + + pLogManager = static_cast(pUserArg); + pLogManager->processLog(msgClass, msgId, *pLogData); + + return errorCode; +} + +void CLoggerManager::processLog(SbgEComClass msgClass, SbgEComMsgId msgId, const SbgEComLogUnion &logData) +{ + try + { + m_logList.at(sbgEComComputeKey(msgClass, msgId))->process(m_context, logData); + } + catch (std::out_of_range &) + { + SBG_LOG_WARNING(SBG_ERROR, "Unknown log %u:%u", msgClass, msgId); + } +} + +}; // namespace sbg diff --git a/tools/sbgBasicLogger/src/loggerManager/loggerManager.h b/tools/sbgBasicLogger/src/loggerManager/loggerManager.h new file mode 100644 index 0000000..635e9e8 --- /dev/null +++ b/tools/sbgBasicLogger/src/loggerManager/loggerManager.h @@ -0,0 +1,166 @@ +/*! + * \file loggerManager.h + * \author SBG Systems + * \date March 03, 2023 + * + * \brief Basic logger implementation. + * + * \copyright Copyright (C) 2023, 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 + * 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. + * + * \endlicense + */ + +#ifndef SBG_LOGGER_MANAGER_H +#define SBG_LOGGER_MANAGER_H + +// STL headers +#include +#include + +// sbgCommonLib headers +#include + +// sbgECom headers +#include + +// Local headers +#include "loggerEntry.h" +#include "loggerContext.h" +#include "loggerSettings.h" + +//----------------------------------------------------------------------// +//- Structure definitions -// +//----------------------------------------------------------------------// + +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. */ + }; +} + +#endif // SBG_BASIC_LOGGER_HANDLER_H diff --git a/tools/sbgBasicLogger/src/loggerManager/loggerSettings.cpp b/tools/sbgBasicLogger/src/loggerManager/loggerSettings.cpp new file mode 100644 index 0000000..066a832 --- /dev/null +++ b/tools/sbgBasicLogger/src/loggerManager/loggerSettings.cpp @@ -0,0 +1,222 @@ +// STL headers +#include +#include +#include + +// sbgCommonLib headers +#include + +// sbgECom headers +#include + +// Local headers +#include "loggerSettings.h" + +namespace sbg +{ +//----------------------------------------------------------------------// +//- Public getters & setters -// +//----------------------------------------------------------------------// + +void CLoggerSettings::setBasePath(const std::string &basePath) +{ + m_basePath = basePath; + + std::replace(m_basePath.begin(), m_basePath.end(), '\\', '/'); + + if (m_basePath.back() != '/') + { + m_basePath.append("/"); + } +} + +const std::string &CLoggerSettings::getBasePath() const +{ + return m_basePath; +} + +void CLoggerSettings::setWriteToConsole(bool writeToConsole) +{ + m_writeToConsole = writeToConsole; +} + +bool CLoggerSettings::getWriteToConsole() const +{ + return m_writeToConsole; +} + +void CLoggerSettings::setWriteToFile(bool writeToFile) +{ + m_writeToFile = writeToFile; +} + +bool CLoggerSettings::getWriteToFile() const +{ + return m_writeToFile; +} + +void CLoggerSettings::setWriteHeaderToFile(bool writeHeaderToFile) +{ + m_writeHeaderToFile = writeHeaderToFile; +} + +bool CLoggerSettings::getWriteHeaderToFile() const +{ + return m_writeHeaderToFile; +} + +void CLoggerSettings::setFileDecimation(int32_t fileDecimation) +{ + if ( (fileDecimation > 0) && (fileDecimation < 10000) ) + { + m_fileDecimation = fileDecimation; + } + else + { + throw std::invalid_argument("file decimation should be within 0 to 10000"); + } +} + +int32_t CLoggerSettings::getFileDecimation() const +{ + return m_fileDecimation; +} + +void CLoggerSettings::setConsoleDecimation(int32_t consoleDecimation) +{ + if ( (consoleDecimation > 0) && (consoleDecimation < 10000) ) + { + m_consoleDecimation = consoleDecimation; + } + else + { + throw std::invalid_argument("console decimation should be within 0 to 10000"); + } +} + +int32_t CLoggerSettings::getConsoleDecimation() const +{ + return m_consoleDecimation; +} + +void CLoggerSettings::setDiscardInvalidTime(bool discardInvalidTime) +{ + m_discardInvalidTime = discardInvalidTime; +} + +bool CLoggerSettings::getDiscardInvalidTime() const +{ + return m_discardInvalidTime; +} + +void CLoggerSettings::setTimeMode(TimeMode timeMode) +{ + m_timeMode = timeMode; +} + +CLoggerSettings::TimeMode CLoggerSettings::getTimeMode() const +{ + return m_timeMode; +} + +void CLoggerSettings::setStatusFormat(CLoggerSettings::StatusFormat statusFormat) +{ + m_statusFormat = statusFormat; +} + +CLoggerSettings::StatusFormat CLoggerSettings::getStatusFormat() const +{ + return m_statusFormat; +} + +bool CLoggerSettings::isOutputConfValid() const +{ + if (m_writeToFile || m_writeToConsole) + { + return true; + } + else + { + return false; + } +} + +CLoggerSettings::InterfaceMode CLoggerSettings::getInterfaceMode() const +{ + return m_interfaceMode; +} + +bool CLoggerSettings::hasInterfaceConf() const +{ + return (m_interfaceMode != InterfaceMode::Unknown); +} + +void CLoggerSettings::setSerialConf(const Serial &serialConf) +{ + if (hasInterfaceConf()) + { + throw std::logic_error("serial conf overrides a previous interface conf."); + } + + m_serialConf = serialConf; + m_interfaceMode = InterfaceMode::Serial; +} + +const CLoggerSettings::Serial &CLoggerSettings::getSerialConf() const +{ + if (m_interfaceMode != InterfaceMode::Serial) + { + throw std::logic_error("no valid serial conf set."); + } + + return m_serialConf; +} + +void CLoggerSettings::setUdpConf(const Udp &udpConf) +{ + if (hasInterfaceConf()) + { + throw std::logic_error("UDP conf overrides a previous interface conf."); + } + + m_udpConf = udpConf; + m_interfaceMode = InterfaceMode::Udp; +} + +const CLoggerSettings::Udp &CLoggerSettings::getUdpConf() const +{ + if (m_interfaceMode != InterfaceMode::Udp) + { + throw std::logic_error("no valid UDP conf set."); + } + + return m_udpConf; +} + +void CLoggerSettings::setFileConf(const std::string &fileConf) +{ + if (hasInterfaceConf()) + { + throw std::logic_error("file conf overrides a previous interface conf."); + } + + m_fileConf = fileConf; + m_interfaceMode = InterfaceMode::File; +} + +const std::string &CLoggerSettings::getFileConf() const +{ + if (m_interfaceMode != InterfaceMode::File) + { + throw std::logic_error("no valid file conf set."); + } + + return m_fileConf; +} + +bool CLoggerSettings::isValid() const +{ + return isOutputConfValid() && hasInterfaceConf(); +} + +}; // Namespace sbg diff --git a/tools/sbgBasicLogger/src/loggerManager/loggerSettings.h b/tools/sbgBasicLogger/src/loggerManager/loggerSettings.h new file mode 100644 index 0000000..2f1f3a0 --- /dev/null +++ b/tools/sbgBasicLogger/src/loggerManager/loggerSettings.h @@ -0,0 +1,331 @@ +/*! + * \file loggerSettings.h + * \author SBG Systems + * \date March 06, 2023 + * + * \brief Define the logger settings. + * + * \copyright Copyright (C) 2023, 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 + * 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. + * + * \endlicense + */ + +#ifndef SBG_LOGGER_SETTINGS_H +#define SBG_LOGGER_SETTINGS_H + +// STL headers +#include + +// sbgCommonLib headers +#include + +// sbgECom headers +#include + +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. */ + }; +}; + +#endif // SBG_LOGGER_SETTINGS_H diff --git a/tools/sbgBasicLogger/src/main.c b/tools/sbgBasicLogger/src/main.c deleted file mode 100644 index a59a1a9..0000000 --- a/tools/sbgBasicLogger/src/main.c +++ /dev/null @@ -1,1046 +0,0 @@ -/*! - * \file main.c - * \author SBG Systems - * \date July 29, 2021 - * - * \brief Tool to manage SBG logs in text format. - * - * \copyright Copyright (C) 2022, 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 - * 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. - * - * \endlicense - */ - -// sbgCommonLib headers -#include -#include -#include -#include -#include -#include - -// sbgECom headers -#include -#include -#include - -// Argtable3 headers -#include - -// Local headers -#include "sbgBasicLoggerHandler.h" - -//----------------------------------------------------------------------// -//- Constant definitions -// -//----------------------------------------------------------------------// - -/*! - * Program name. - */ -#define PROGRAM_NAME "sbgBasicLogger" - -//----------------------------------------------------------------------// -//- Structure definitions -// -//----------------------------------------------------------------------// - -/*! - * User arguments. - */ -typedef struct _UserArgs -{ - bool writeHeader; /*!< True to write data header. */ - bool writingFiles; /*!< True to write data into files. */ - size_t fileDecimation; /*!< File decimation. */ - bool consoleEnabled; /*!< True to enable console. */ - size_t consoleDecimation; /*!< Console decimation. */ - SbgString path; /*!< Output path string to write log files into. */ - const char *pPathStr; /*!< Output path string shortcut. */ -} UserArgs; - -/*! - * SbgBasicLoggerManager. - */ -typedef struct _SbgBasicLoggerManager -{ - UserArgs userArgs; /*!< User arguments. */ - - SbgEComHandle ecomHandle; /*!< ECom handler. */ - SbgInterface ecomInterface; /*!< SBG interface. */ - - SbgLogUtcData lastUtcData; /*!< Latest UTC time reference used to time stamp messages. */ - - SbgBasicLoggerImuHandler imuShort; /*!< IMU short logs handler. */ - SbgBasicLoggerImuHandler imu; /*!< IMU logs handler. */ - SbgBasicLoggerImuHandler imuFast; /*!< IMU fast logs handler. */ - SbgBasicLoggerStatusHandler status; /*!< Status logs handler. */ - SbgBasicLoggerDiagHandler diag; /*!< Diagnostic logs handler. */ - SbgBasicLoggerUtcHandler utc; /*!< Diagnostic logs handler. */ - - SbgBasicLoggerEkfEulerHandler ekfEuler; /*!< EKF Euler logs handler. */ - SbgBasicLoggerEkfQuatHandler ekfQuat; /*!< EKF quat logs handler. */ - SbgBasicLoggerEkfNavHandler ekfNav; /*!< EKF nav logs handler. */ - - SbgBasicLoggerShipMotionHandler shipMotion; /*!< Ship motion logs handler. */ - SbgBasicLoggerShipMotionHpHandler shipMotionHp; /*!< Ship motion HP logs handler. */ - - SbgBasicLoggerGpsHdtHandler gps1Hdt; /*!< GPS 1 hdt logs handler. */ - SbgBasicLoggerGpsPosHandler gps1Pos; /*!< GPS 1 pos logs handler. */ - SbgBasicLoggerGpsVelHandler gps1Vel; /*!< GPS 1 vel logs handler. */ - SbgBasicLoggerGpsRawHandler gps1Raw; /*!< GPS 1 raw logs handler. */ - SbgBasicLoggerGpsSatHandler gps1Sat; /*!< GPS 1 Satellite in View logs handler. */ - - SbgBasicLoggerGpsHdtHandler gps2Hdt; /*!< GPS 2 hdt logs handler. */ - SbgBasicLoggerGpsPosHandler gps2Pos; /*!< GPS 2 pos logs handler. */ - SbgBasicLoggerGpsVelHandler gps2Vel; /*!< GPS 2 vel logs handler. */ - SbgBasicLoggerGpsRawHandler gps2Raw; /*!< GPS 2 raw logs handler. */ - SbgBasicLoggerGpsSatHandler gps2Sat; /*!< GPS 2 Satellite in View logs handler. */ - - SbgBasicLoggerOdometerHandler odometer; /*!< Odometer logs handler. */ - SbgBasicLoggerDvlHandler dvlBottom; /*!< DVL bottom logs handler. */ - SbgBasicLoggerDvlHandler dvlWater; /*!< DVL water logs handler. */ - SbgBasicLoggerAirHandler air; /*!< Air logs handler. */ - SbgBasicLoggerUsblHandler usbl; /*!< USBL logs handler. */ - SbgBasicLoggerDepthHandler depth; /*!< Depth logs handler. */ - SbgBasicLoggerRawRtcmHandler rawRtcm; /*!< Raw RTCM logs handler. */ - - SbgBasicLoggerEventHandler eventInA; /*!< Event in A logs handler. */ - SbgBasicLoggerEventHandler eventInB; /*!< Event in B logs handler. */ - SbgBasicLoggerEventHandler eventInC; /*!< Event in C logs handler. */ - SbgBasicLoggerEventHandler eventInD; /*!< Event in D logs handler. */ - SbgBasicLoggerEventHandler eventInE; /*!< Event in E logs handler. */ - - SbgBasicLoggerEventHandler eventOutA; /*!< Event out A logs handler. */ - SbgBasicLoggerEventHandler eventOutB; /*!< Event out B logs handler. */ - - SbgBasicLoggerMagHandler mag; /*!< Mag logs handler. */ -} SbgBasicLoggerManager; - -//----------------------------------------------------------------------// -//- Private variables -// -//----------------------------------------------------------------------// - -/*! - * SBG basic logger singleton. - */ -static SbgBasicLoggerManager gSbgBasicLogger; - -//----------------------------------------------------------------------// -//- Private functions -// -//----------------------------------------------------------------------// - -/*! - * 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] type Associated log message type. - * \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 logType, SbgErrorCode errorCode, const char *pMessage) -{ - assert(pFunctionName); - assert(pCategory); - assert(pMessage); - - SBG_UNUSED_PARAMETER(pFileName); - SBG_UNUSED_PARAMETER(pCategory); - - switch (logType) - { - case SBG_DEBUG_LOG_TYPE_ERROR: - fprintf(stderr, "*ERR * %10s %s(%"PRIu32"): %s\n\r", sbgErrorCodeToString(errorCode), pFunctionName, line, pMessage); - break; - case SBG_DEBUG_LOG_TYPE_WARNING: - fprintf(stderr, "*WARN* %10s %s(%"PRIu32"): %s\n\r", sbgErrorCodeToString(errorCode), pFunctionName, line, pMessage); - break; - case SBG_DEBUG_LOG_TYPE_INFO: - fprintf(stderr, "*INFO* %s(%"PRIu32"): %s\n\r", pFunctionName, line, pMessage); - break; - case SBG_DEBUG_LOG_TYPE_DEBUG: - fprintf(stderr, "*DBG * %s(%"PRIu32") : % s\n\r", pFunctionName, line, pMessage); - break; - default: - fprintf(stderr, "error: unexpected logType (0x%x)\n\r", logType); - break; - } -} - -//----------------------------------------------------------------------// -//- SbgBasicLoggerManager methods -// -//----------------------------------------------------------------------// - -/*! - * Process an UTC data. - * - * \param[in] pManager SBG basic logger. - * \param[in] pUtc UTC data. - */ -static void sbgBasicLoggerManagerProcessUtc(SbgBasicLoggerManager *pManager, const SbgLogUtcData *pUtcData) -{ - assert(pManager); - assert(pUtcData); - - pManager->lastUtcData = *pUtcData; - sbgBasicLoggerUtcHandlerProcess(&pManager->utc, pUtcData); -} - -/*! - * Process a new IMU short data. - * - * \param[in] pManager SBG basic logger. - * \param[in] pImuShort New IMU short data. - */ -static void sbgBasicLoggerManagerProcessImuShort(SbgBasicLoggerManager *pManager, const SbgLogImuShort *pImuShort) -{ - SbgBasicLoggerImu newData; - - assert(pManager); - assert(pImuShort); - - newData.timestamp = pImuShort->timeStamp; - newData.status = pImuShort->status; - newData.temperature = sbgLogImuShortGetTemperature(pImuShort); - - for (size_t i = 0; i < 3; i++) - { - newData.deltaAngle[i] = sbgLogImuShortGetDeltaAngle(pImuShort, i); - newData.deltaVelocity[i] = sbgLogImuShortGetDeltaVelocity(pImuShort, i); - } - - sbgBasicLoggerImuHandlerProcess(&pManager->imuShort, &newData); -} - -/*! - * Process a new IMU data. - * - * \param[in] pManager SBG basic logger. - * \param[in] pImu New IMU data. - */ -static void sbgBasicLoggerManagerProcessImu(SbgBasicLoggerManager *pManager, const SbgLogImuData *pImu) -{ - SbgBasicLoggerImu newData; - - assert(pManager); - assert(pImu); - - newData.timestamp = pImu->timeStamp; - newData.status = pImu->status; - newData.temperature = pImu->temperature; - - for (size_t i = 0; i < 3; i++) - { - newData.deltaAngle[i] = pImu->deltaAngle[i]; - newData.deltaVelocity[i] = pImu->deltaVelocity[i]; - } - - sbgBasicLoggerImuHandlerProcess(&pManager->imu, &newData); -} - -/*! - * Process a new IMU fast data. - * - * \param[in] pManager SBG basic logger. - * \param[in] pImuShort New IMU fast data. - */ -static void sbgBasicLoggerManagerProcessImuFast(SbgBasicLoggerManager *pManager, const SbgLogFastImuData *pFast) -{ - SbgBasicLoggerImu newData; - - assert(pManager); - assert(pFast); - - newData.timestamp = pFast->timeStamp; - newData.status = pFast->status; - newData.temperature = 0.0f; - - for (size_t i = 0; i < 3; i++) - { - newData.deltaAngle[i] = pFast->gyroscopes[i]; - newData.deltaVelocity[i] = pFast->accelerometers[i]; - } - - sbgBasicLoggerImuHandlerProcess(&pManager->imuFast, &newData); -} - -/*! - * 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 onLogReceived(SbgEComHandle *pECom, SbgEComClass msgClass, SbgEComMsgId msg, const SbgBinaryLogData *pLogData, void *pUserArg) -{ - SbgBasicLoggerManager *pManager; - - assert(pLogData); - assert(pUserArg); - - SBG_UNUSED_PARAMETER(pECom); - - pManager = (SbgBasicLoggerManager*)pUserArg; - - if (msgClass == SBG_ECOM_CLASS_LOG_ECOM_0) - { - switch (msg) - { - case SBG_ECOM_LOG_STATUS: - sbgBasicLoggerStatusHandlerProcess(&pManager->status, &pLogData->statusData); - break; - - case SBG_ECOM_LOG_UTC_TIME: - sbgBasicLoggerManagerProcessUtc(pManager, &pLogData->utcData); - break; - - case SBG_ECOM_LOG_IMU_DATA: - sbgBasicLoggerManagerProcessImu(pManager, &pLogData->imuData); - break; - - case SBG_ECOM_LOG_MAG: - sbgBasicLoggerMagHandlerProcess(&pManager->mag, &pLogData->magData); - break; - - case SBG_ECOM_LOG_MAG_CALIB: - // - // Known but not sbgBasicLoggerManagerProcessed. - // - break; - - case SBG_ECOM_LOG_EKF_EULER: - sbgBasicLoggerEkfEulerHandlerProcess(&pManager->ekfEuler, &pLogData->ekfEulerData); - break; - - case SBG_ECOM_LOG_EKF_QUAT: - sbgBasicLoggerEkfQuatHandlerProcess(&pManager->ekfQuat, &pLogData->ekfQuatData); - break; - - case SBG_ECOM_LOG_EKF_NAV: - sbgBasicLoggerEkfNavHandlerProcess(&pManager->ekfNav, &pLogData->ekfNavData); - break; - - case SBG_ECOM_LOG_SHIP_MOTION: - sbgBasicLoggerShipMotionHandlerProcess(&pManager->shipMotion, &pLogData->shipMotionData); - break; - - case SBG_ECOM_LOG_GPS1_VEL: - sbgBasicLoggerGpsVelHandlerProcess(&pManager->gps1Vel, &pLogData->gpsVelData); - break; - - case SBG_ECOM_LOG_GPS1_POS: - sbgBasicLoggerGpsPosHandlerProcess(&pManager->gps1Pos, &pLogData->gpsPosData); - break; - - case SBG_ECOM_LOG_GPS1_HDT: - sbgBasicLoggerGpsHdtHandlerProcess(&pManager->gps1Hdt, &pLogData->gpsHdtData); - break; - - case SBG_ECOM_LOG_GPS1_RAW: - sbgBasicLoggerGpsRawHandlerProcess(&pManager->gps1Raw, &pLogData->gpsRawData); - break; - - case SBG_ECOM_LOG_GPS1_SAT: - sbgBasicLoggerGpsSatHandlerProcess(&pManager->gps1Sat, &pLogData->satGroupData); - break; - - case SBG_ECOM_LOG_GPS2_VEL: - sbgBasicLoggerGpsVelHandlerProcess(&pManager->gps2Vel, &pLogData->gpsVelData); - break; - - case SBG_ECOM_LOG_GPS2_POS: - sbgBasicLoggerGpsPosHandlerProcess(&pManager->gps2Pos, &pLogData->gpsPosData); - break; - - case SBG_ECOM_LOG_GPS2_HDT: - sbgBasicLoggerGpsHdtHandlerProcess(&pManager->gps2Hdt, &pLogData->gpsHdtData); - break; - - case SBG_ECOM_LOG_GPS2_RAW: - sbgBasicLoggerGpsRawHandlerProcess(&pManager->gps2Raw, &pLogData->gpsRawData); - break; - - case SBG_ECOM_LOG_GPS2_SAT: - sbgBasicLoggerGpsSatHandlerProcess(&pManager->gps2Sat, &pLogData->satGroupData); - break; - - case SBG_ECOM_LOG_ODO_VEL: - sbgBasicLoggerOdometerHandlerProcess(&pManager->odometer, &pLogData->odometerData); - break; - - case SBG_ECOM_LOG_EVENT_A: - sbgBasicLoggerEventHandlerProcess(&pManager->eventInA, &pLogData->eventMarker); - break; - - case SBG_ECOM_LOG_EVENT_B: - sbgBasicLoggerEventHandlerProcess(&pManager->eventInB, &pLogData->eventMarker); - break; - - case SBG_ECOM_LOG_EVENT_C: - sbgBasicLoggerEventHandlerProcess(&pManager->eventInC, &pLogData->eventMarker); - break; - - case SBG_ECOM_LOG_EVENT_D: - sbgBasicLoggerEventHandlerProcess(&pManager->eventInD, &pLogData->eventMarker); - break; - - case SBG_ECOM_LOG_EVENT_E: - sbgBasicLoggerEventHandlerProcess(&pManager->eventInE, &pLogData->eventMarker); - break; - - case SBG_ECOM_LOG_DVL_BOTTOM_TRACK: - sbgBasicLoggerDvlHandlerProcess(&pManager->dvlBottom, &pLogData->dvlData); - break; - - case SBG_ECOM_LOG_DVL_WATER_TRACK: - sbgBasicLoggerDvlHandlerProcess(&pManager->dvlWater, &pLogData->dvlData); - break; - - case SBG_ECOM_LOG_SHIP_MOTION_HP: - sbgBasicLoggerShipMotionHpHandlerProcess(&pManager->shipMotionHp, &pLogData->shipMotionData); - break; - - case SBG_ECOM_LOG_AIR_DATA: - sbgBasicLoggerAirHandlerProcess(&pManager->air, &pLogData->airData); - break; - - case SBG_ECOM_LOG_USBL: - sbgBasicLoggerUsblHandlerProcess(&pManager->usbl, &pLogData->usblData); - break; - - case SBG_ECOM_LOG_IMU_SHORT: - sbgBasicLoggerManagerProcessImuShort(pUserArg, &pLogData->imuShort); - break; - - case SBG_ECOM_LOG_EVENT_OUT_A: - sbgBasicLoggerEventHandlerProcess(&pManager->eventOutA, &pLogData->eventMarker); - break; - - case SBG_ECOM_LOG_EVENT_OUT_B: - sbgBasicLoggerEventHandlerProcess(&pManager->eventOutB, &pLogData->eventMarker); - break; - - case SBG_ECOM_LOG_DEPTH: - sbgBasicLoggerDepthHandlerProcess(&pManager->depth, &pLogData->depthData); - break; - - case SBG_ECOM_LOG_DIAG: - sbgBasicLoggerDiagHandlerProcess(&pManager->diag, &pManager->lastUtcData, &pLogData->diagData); - break; - - case SBG_ECOM_LOG_RTCM_RAW: - sbgBasicLoggerRawRtcmHandlerProcess(&pManager->rawRtcm, &pLogData->rtcmRawData); - break; - - default: - fprintf(stdout, "unkwown log: %u", msg); - break; - } - } - else if (msgClass == SBG_ECOM_CLASS_LOG_ECOM_1) - { - switch (msg) - { - case SBG_ECOM_LOG_FAST_IMU_DATA: - sbgBasicLoggerManagerProcessImuFast(pUserArg, &pLogData->fastImuData); - break; - - default: - fprintf(stdout, "unkwown log1: %u", msg); - break; - } - } - else if (msgClass == SBG_ECOM_CLASS_LOG_NMEA_0) - { - // - // Known but not sbgBasicLoggerManagerProcessed. - // - } - else if (msgClass == SBG_ECOM_CLASS_LOG_NMEA_1) - { - // - // Known but not sbgBasicLoggerManagerProcessed. - // - } - else if (msgClass == SBG_ECOM_CLASS_LOG_THIRD_PARTY_0) - { - // - // Known but not sbgBasicLoggerManagerProcessed. - // - } - else if (msgClass == SBG_ECOM_CLASS_LOG_CMD_0) - { - // - // Known but not sbgBasicLoggerManagerProcessed. - // - } - else - { - fprintf(stdout, "unkwown class: %u", msgClass); - } - - return SBG_NO_ERROR; -} - -/*! - * Construct all log handlers. - * - * \param[in] pManager SBG basic logger. - */ -static void sbgBasicLoggerConstructLogHandlers(SbgBasicLoggerManager *pManager) -{ - assert(pManager); - - sbgBasicLoggerImuShortHandlerConstruct(&pManager->imuShort, - pManager->userArgs.consoleEnabled, - pManager->userArgs.consoleDecimation, - pManager->userArgs.writingFiles, - pManager->userArgs.pPathStr, - pManager->userArgs.fileDecimation, - pManager->userArgs.writeHeader); - - sbgBasicLoggerImuHandlerConstruct(&pManager->imu, - pManager->userArgs.consoleEnabled, - pManager->userArgs.consoleDecimation, - pManager->userArgs.writingFiles, - pManager->userArgs.pPathStr, - pManager->userArgs.fileDecimation, - pManager->userArgs.writeHeader); - - sbgBasicLoggerImuFastHandlerConstruct(&pManager->imuFast, - pManager->userArgs.consoleEnabled, - pManager->userArgs.consoleDecimation, - pManager->userArgs.writingFiles, - pManager->userArgs.pPathStr, - pManager->userArgs.fileDecimation, - pManager->userArgs.writeHeader); - - - sbgBasicLoggerDiagHandlerConstruct(&pManager->diag, - pManager->userArgs.consoleEnabled, - pManager->userArgs.writingFiles, - pManager->userArgs.pPathStr); - - sbgBasicLoggerUtcHandlerConstruct(&pManager->utc, - pManager->userArgs.consoleEnabled, - pManager->userArgs.writingFiles, - pManager->userArgs.pPathStr, - pManager->userArgs.writeHeader); - - sbgBasicLoggerStatusHandlerConstruct(&pManager->status, - pManager->userArgs.consoleEnabled, - pManager->userArgs.writingFiles, - pManager->userArgs.pPathStr, - pManager->userArgs.writeHeader); - - sbgBasicLoggerEkfEulerHandlerConstruct(&pManager->ekfEuler, - pManager->userArgs.consoleEnabled, - pManager->userArgs.writingFiles, - pManager->userArgs.pPathStr, - pManager->userArgs.writeHeader); - - sbgBasicLoggerEkfQuatHandlerConstruct(&pManager->ekfQuat, - pManager->userArgs.consoleEnabled, - pManager->userArgs.writingFiles, - pManager->userArgs.pPathStr, - pManager->userArgs.writeHeader); - - sbgBasicLoggerEkfNavHandlerConstruct(&pManager->ekfNav, - pManager->userArgs.consoleEnabled, - pManager->userArgs.writingFiles, - pManager->userArgs.pPathStr, - pManager->userArgs.writeHeader); - - sbgBasicLoggerShipMotionHandlerConstruct(&pManager->shipMotion, - pManager->userArgs.consoleEnabled, - pManager->userArgs.writingFiles, - pManager->userArgs.pPathStr, - pManager->userArgs.writeHeader); - - sbgBasicLoggerShipMotionHpHandlerConstruct(&pManager->shipMotionHp, - pManager->userArgs.consoleEnabled, - pManager->userArgs.writingFiles, - pManager->userArgs.pPathStr, - pManager->userArgs.writeHeader); - - sbgBasicLoggerGpsHdt1HandlerConstruct(&pManager->gps1Hdt, - pManager->userArgs.consoleEnabled, - pManager->userArgs.writingFiles, - pManager->userArgs.pPathStr, - pManager->userArgs.writeHeader); - - sbgBasicLoggerGpsPos1HandlerConstruct(&pManager->gps1Pos, - pManager->userArgs.consoleEnabled, - pManager->userArgs.writingFiles, - pManager->userArgs.pPathStr, - pManager->userArgs.writeHeader); - - sbgBasicLoggerGpsVel1HandlerConstruct(&pManager->gps1Vel, - pManager->userArgs.consoleEnabled, - pManager->userArgs.writingFiles, - pManager->userArgs.pPathStr, - pManager->userArgs.writeHeader); - - sbgBasicLoggerGps1RawHandlerConstruct(&pManager->gps1Raw, - pManager->userArgs.consoleEnabled, - pManager->userArgs.writingFiles, - pManager->userArgs.pPathStr); - - sbgBasicLoggerGps1SatHandlerConstruct(&pManager->gps1Sat, - pManager->userArgs.consoleEnabled, - pManager->userArgs.writingFiles, - pManager->userArgs.pPathStr); - - sbgBasicLoggerGpsHdt2HandlerConstruct(&pManager->gps2Hdt, - pManager->userArgs.consoleEnabled, - pManager->userArgs.writingFiles, - pManager->userArgs.pPathStr, - pManager->userArgs.writeHeader); - - sbgBasicLoggerGpsPos2HandlerConstruct(&pManager->gps2Pos, - pManager->userArgs.consoleEnabled, - pManager->userArgs.writingFiles, - pManager->userArgs.pPathStr, - pManager->userArgs.writeHeader); - - sbgBasicLoggerGpsVel2HandlerConstruct(&pManager->gps2Vel, - pManager->userArgs.consoleEnabled, - pManager->userArgs.writingFiles, - pManager->userArgs.pPathStr, - pManager->userArgs.writeHeader); - - sbgBasicLoggerGps2RawHandlerConstruct(&pManager->gps2Raw, - pManager->userArgs.consoleEnabled, - pManager->userArgs.writingFiles, - pManager->userArgs.pPathStr); - - sbgBasicLoggerGps2SatHandlerConstruct(&pManager->gps2Sat, - pManager->userArgs.consoleEnabled, - pManager->userArgs.writingFiles, - pManager->userArgs.pPathStr); - - sbgBasicLoggerOdometerHandlerConstruct(&pManager->odometer, - pManager->userArgs.consoleEnabled, - pManager->userArgs.writingFiles, - pManager->userArgs.pPathStr, - pManager->userArgs.writeHeader); - - sbgBasicLoggerDvlBottomHandlerConstruct(&pManager->dvlBottom, - pManager->userArgs.consoleEnabled, - pManager->userArgs.writingFiles, - pManager->userArgs.pPathStr, - pManager->userArgs.writeHeader); - - sbgBasicLoggerDvlWaterHandlerConstruct(&pManager->dvlWater, - pManager->userArgs.consoleEnabled, - pManager->userArgs.writingFiles, - pManager->userArgs.pPathStr, - pManager->userArgs.writeHeader); - - sbgBasicLoggerAirHandlerConstruct(&pManager->air, - pManager->userArgs.consoleEnabled, - pManager->userArgs.writingFiles, - pManager->userArgs.pPathStr, - pManager->userArgs.writeHeader); - - sbgBasicLoggerUsblHandlerConstruct(&pManager->usbl, - pManager->userArgs.consoleEnabled, - pManager->userArgs.writingFiles, - pManager->userArgs.pPathStr, - pManager->userArgs.writeHeader); - - sbgBasicLoggerDepthHandlerConstruct(&pManager->depth, - pManager->userArgs.consoleEnabled, - pManager->userArgs.writingFiles, - pManager->userArgs.pPathStr, - pManager->userArgs.writeHeader); - - sbgBasicLoggerRawRtcmHandlerConstruct(&pManager->rawRtcm, - pManager->userArgs.consoleEnabled, - pManager->userArgs.writingFiles, - pManager->userArgs.pPathStr); - - sbgBasicLoggerEventInAHandlerConstruct(&pManager->eventInA, - pManager->userArgs.consoleEnabled, - pManager->userArgs.writingFiles, - pManager->userArgs.pPathStr, - pManager->userArgs.writeHeader); - - sbgBasicLoggerEventInBHandlerConstruct(&pManager->eventInB, - pManager->userArgs.consoleEnabled, - pManager->userArgs.writingFiles, - pManager->userArgs.pPathStr, - pManager->userArgs.writeHeader); - - sbgBasicLoggerEventInCHandlerConstruct(&pManager->eventInC, - pManager->userArgs.consoleEnabled, - pManager->userArgs.writingFiles, - pManager->userArgs.pPathStr, - pManager->userArgs.writeHeader); - - sbgBasicLoggerEventInDHandlerConstruct(&pManager->eventInD, - pManager->userArgs.consoleEnabled, - pManager->userArgs.writingFiles, - pManager->userArgs.pPathStr, - pManager->userArgs.writeHeader); - - sbgBasicLoggerEventInEHandlerConstruct(&pManager->eventInE, - pManager->userArgs.consoleEnabled, - pManager->userArgs.writingFiles, - pManager->userArgs.pPathStr, - pManager->userArgs.writeHeader); - - sbgBasicLoggerEventOutAHandlerConstruct(&pManager->eventOutA, - pManager->userArgs.consoleEnabled, - pManager->userArgs.writingFiles, - pManager->userArgs.pPathStr, - pManager->userArgs.writeHeader); - - sbgBasicLoggerEventOutBHandlerConstruct(&pManager->eventOutB, - pManager->userArgs.consoleEnabled, - pManager->userArgs.writingFiles, - pManager->userArgs.pPathStr, - pManager->userArgs.writeHeader); - - sbgBasicLoggerMagHandlerConstruct(&pManager->mag, - pManager->userArgs.consoleEnabled, - pManager->userArgs.writingFiles, - pManager->userArgs.pPathStr, - pManager->userArgs.writeHeader); -} - -/*! - * Receive logs. - * - * \param[in] pManager Sbg Basic logger. - */ -static SbgErrorCode sbgBasicLoggerManagerReceiveLogs(SbgBasicLoggerManager *pManager) -{ - SbgErrorCode errorCode; - - assert(pManager); - - errorCode = sbgEComInit(&pManager->ecomHandle, &pManager->ecomInterface); - - if (errorCode == SBG_NO_ERROR) - { - sbgBasicLoggerConstructLogHandlers(pManager); - - sbgEComSetReceiveLogCallback(&pManager->ecomHandle, onLogReceived, pManager); - - while (1) - { - errorCode = sbgEComHandle(&pManager->ecomHandle); - - if (errorCode == SBG_NOT_READY) - { - if (pManager->ecomInterface.type == SBG_IF_TYPE_FILE) - { - break; - } - - // - // Save CPU time. - // - sbgSleep(1); - } - } - - sbgEComClose(&pManager->ecomHandle); - } - - return errorCode; -} - -//----------------------------------------------------------------------// -// Public functions // -//----------------------------------------------------------------------// - -/*! - * 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) -{ - SbgBasicLoggerManager *pManager; - 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_file *pInputFileArg; - struct arg_lit *pWriteLogs; - struct arg_str *pWriteLogsDirArg; - struct arg_int *pFileDecimationArg; - struct arg_int *pScreenDecimationArg; - struct arg_lit *pPrintLogs; - struct arg_lit *pLogsHeader; - struct arg_end *pEndArg; - - void *argTable[] = - { - pHelpArg = arg_lit0( "h", "help", "display this help and exit"), - pVersionArg = arg_lit0( "v", "version", "display sbgECom version 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 in"), - pUdpPortOutArg = arg_int0( "O", "udp-port-out", "UDP port out", "UDP port out"), - - pSerialDeviceArg = arg_str0( "s", "serial-device", "SERIAL_DEVICE", "open a serial interface"), - pSerialBaudrateArg = arg_int0( "r", "serial-baudrate", "SERIAL_BAUDRATE", "serial baudrate"), - - pInputFileArg = arg_file0( "i", "input-file", "INPUT-FILE", "input file"), - - pWriteLogs = arg_lit0( "w", "write-logs", "write logs in different files"), - pWriteLogsDirArg = arg_str0( NULL, "dir", "DIRECTORY", "directory to write logs into"), - - pFileDecimationArg = arg_int0( "d", "file-decimation", "FILE DECIMATION", "file decimation"), - pScreenDecimationArg = arg_int0( "c", "console-decimation", "CONSOLE DECIMATION", "output stream decimation"), - - pPrintLogs = arg_lit0( "p", "print-logs", "print the logs on the output stream"), - - pLogsHeader = arg_lit0( "H", "disable-header", "disable header for files"), - - pEndArg = arg_end(20), - }; - - memset(&gSbgBasicLogger, 0, sizeof(gSbgBasicLogger)); - - pManager = &gSbgBasicLogger; - - 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("Manage sbgECom logs in text format.\n\n"); - - printf("Serial example: %s -s -r -p\n", PROGRAM_NAME); - printf(" UDP example: %s -a -I -O -p\n", PROGRAM_NAME); - printf(" File example: %s -i -p\n\n", PROGRAM_NAME); - - printf("Logs affected by decimation:\n\t- IMU short\n\t- IMU fast\n\t- IMU data\n\n"); - - arg_print_glossary(stdout, argTable, " %-50s %s\n"); - } - else if (pVersionArg->count != 0) - { - printf("%s\n", sbgEComGetVersionAsString()); - } - else if (argError == 0) - { - if (pLogsHeader->count != 0) - { - pManager->userArgs.writeHeader = false; - } - else - { - pManager->userArgs.writeHeader = true; - } - - if (pWriteLogs->count != 0) - { - pManager->userArgs.writingFiles = true; - pManager->userArgs.fileDecimation = 0; - - if (pWriteLogsDirArg->count != 0) - { - SbgErrorCode errorCode; - - errorCode = sbgStringConstructCString(&pManager->userArgs.path, pWriteLogsDirArg->sval[0]); - - if (errorCode == SBG_NO_ERROR) - { - if ((errorCode == SBG_NO_ERROR) && !sbgStringEndsWith(&pManager->userArgs.path, "/")) - { - sbgStringAppendCString(&pManager->userArgs.path, "/"); - } - } - else - { - fprintf(stderr, "Invalid path format.\n"); - exitCode = EXIT_FAILURE; - printHelp = true; - } - } - else - { - sbgStringConstructCString(&pManager->userArgs.path, ""); - } - - pManager->userArgs.pPathStr = sbgStringGetCString(&pManager->userArgs.path); - } - - if (pFileDecimationArg->count != 0) - { - pManager->userArgs.fileDecimation = pFileDecimationArg->ival[0]; - } - else - { - pManager->userArgs.fileDecimation = 1; - } - - if (pPrintLogs->count != 0) - { - pManager->userArgs.consoleEnabled = true; - pManager->userArgs.consoleDecimation = 0; - } - - if (pScreenDecimationArg->count != 0) - { - pManager->userArgs.consoleDecimation = pScreenDecimationArg->ival[0]; - } - else - { - pManager->userArgs.consoleDecimation = 1; - } - - if ((pSerialDeviceArg->count != 0) && (pSerialBaudrateArg->count != 0) && - ((pInputFileArg->count != 0) || - (pUdpAddrArg->count != 0) || - (pUdpPortInArg->count != 0) || - (pUdpPortOutArg->count != 0))) - { - fprintf(stderr, "Too many arguments.\n"); - exitCode = EXIT_FAILURE; - printHelp = true; - } - - if ((pInputFileArg->count != 0) && - ((pSerialDeviceArg->count != 0) || - (pSerialBaudrateArg->count != 0) || - (pUdpAddrArg->count != 0) || - (pUdpPortInArg->count != 0) || - (pUdpPortOutArg->count != 0))) - { - fprintf(stderr, "Too many arguments.\n"); - exitCode = EXIT_FAILURE; - printHelp = true; - } - - if ((pUdpAddrArg->count != 0) && (pUdpPortInArg->count != 0) && (pUdpPortOutArg->count != 0) && - ((pInputFileArg->count != 0) || - (pSerialDeviceArg->count != 0) || - (pSerialBaudrateArg->count != 0))) - { - fprintf(stderr, "Too many arguments.\n"); - exitCode = EXIT_FAILURE; - printHelp = true; - } - - if (!pManager->userArgs.consoleEnabled && !pManager->userArgs.writingFiles) - { - fprintf(stderr, "Specify if you want to print (-p) logs on console or write (-w) into files.\n"); - exitCode = EXIT_FAILURE; - printHelp = true; - } - - if (exitCode == EXIT_SUCCESS) - { - if ((pSerialDeviceArg->count != 0) && (pSerialBaudrateArg->count != 0)) - { - SbgErrorCode errorCode; - - errorCode = sbgInterfaceSerialCreate(&pManager->ecomInterface, pSerialDeviceArg->sval[0], pSerialBaudrateArg->ival[0]); - - if (errorCode != SBG_NO_ERROR) - { - SBG_LOG_ERROR(errorCode, "unable to open serial interface"); - exitCode = EXIT_FAILURE; - } - } - else if (pInputFileArg->count != 0) - { - SbgErrorCode errorCode; - - errorCode = sbgInterfaceFileOpen(&pManager->ecomInterface, pInputFileArg->filename[0]); - - if (errorCode != SBG_NO_ERROR) - { - SBG_LOG_ERROR(errorCode, "unable to open file interface"); - exitCode = EXIT_FAILURE; - } - } - else if ((pUdpAddrArg->count != 0) && (pUdpPortInArg->count != 0) && (pUdpPortOutArg->count != 0)) - { - SbgErrorCode errorCode; - - errorCode = sbgInterfaceUdpCreate(&pManager->ecomInterface, sbgNetworkIpFromString(pUdpAddrArg->sval[0]), pUdpPortOutArg->ival[0], pUdpPortInArg->ival[0]); - - if (errorCode != SBG_NO_ERROR) - { - SBG_LOG_ERROR(errorCode, "unable to open file interface"); - exitCode = EXIT_FAILURE; - } - } - else - { - exitCode = EXIT_FAILURE; - printHelp = true; - } - } - - if (exitCode == EXIT_SUCCESS) - { - SbgErrorCode errorCode; - - errorCode = sbgBasicLoggerManagerReceiveLogs(pManager); - - if (errorCode != SBG_NO_ERROR) - { - exitCode = EXIT_FAILURE; - } - - sbgInterfaceDestroy(&pManager->ecomInterface); - } - } - 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/tools/sbgBasicLogger/src/main.cpp b/tools/sbgBasicLogger/src/main.cpp new file mode 100644 index 0000000..e3ded51 --- /dev/null +++ b/tools/sbgBasicLogger/src/main.cpp @@ -0,0 +1,211 @@ +/*! + * \file main.cpp + * \author SBG Systems + * \date March 22, 2023 + * + * \brief Simple console application that parse and output sbgECom logs to CSV like files. + * + * \copyright Copyright (C) 2022, 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 + * 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. + * + * \endlicense + */ + +// Standard Library +#include + +// STL headers +#include +#include +#include + +// sbgCommonLib headers +#include + +// sbgECom headers +#include +#include + + +// Local headers +#include "loggerApp.h" + +//----------------------------------------------------------------------// +//- Global variables -// +//----------------------------------------------------------------------// + +/*! + * Clear this flag to cancel current processing. + */ +std::atomic_flag gShouldContinue = ATOMIC_FLAG_INIT; + +/*! + * Flag cleared once the processing has been terminated (either canceled or completed). + */ +std::atomic_flag gProgramRunning = ATOMIC_FLAG_INIT; + +/*! + * Stores the main running thread to detect if signal handlers are called from an other thread. + */ +std::thread::id gMainThreadId; + +//----------------------------------------------------------------------// +//- Private functions -// +//----------------------------------------------------------------------// + +/*! + * 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] type Associated log message type. + * \param[in] errorCode Associated error code or SBG_NO_ERROR for INFO & DEBUG level logs. + * \param[in] pMessage The message to log. + */ +static void onMessageCallback(const char *pFileName, const char *pFunctionName, uint32_t line, const char *pCategory, SbgDebugLogType logType, SbgErrorCode errorCode, const char *pMessage) +{ + assert(pFunctionName); + assert(pCategory); + assert(pMessage); + + SBG_UNUSED_PARAMETER(pFileName); + SBG_UNUSED_PARAMETER(pCategory); + + switch (logType) + { + case SBG_DEBUG_LOG_TYPE_ERROR: + std::cout << "*ERR * " << std::setw(20) << std::left << sbgErrorCodeToString(errorCode) << " " << pFunctionName << "(" << line << "): " << pMessage << std::endl; + break; + case SBG_DEBUG_LOG_TYPE_WARNING: + std::cout << "*WARN* " << std::setw(20) << std::left << sbgErrorCodeToString(errorCode) << " " << pFunctionName << "(" << line << "): " << pMessage << std::endl; + break; + case SBG_DEBUG_LOG_TYPE_INFO: + std::cout << "*INFO* " << pFunctionName << "(" << line << "): " << pMessage << std::endl; + break; + case SBG_DEBUG_LOG_TYPE_DEBUG: + std::cout << "*DBG * " << pFunctionName << "(" << line << "): " << pMessage << std::endl; + break; + default: + std::cout << "*UKN * " << std::setw(20) << std::left << sbgErrorCodeToString(errorCode) << " " << pFunctionName << "(" << line << "): " << pMessage << std::endl; + break; + } +} + +/*! + * 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 processing (main thread) to stop processing through continueProcessingCb + // + gShouldContinue.clear(); + + // + // On Windows platforms the signal handler is called from an other thread + // than the main running thread. + // + // Once the flag is cleared, we should wait until the application exits gracefully. + // This is required to properly flush and close all opened files. + // + // However, on Unix platforms, the signal handler is called from the same thread. + // In this case, we must skip this test as it will wait forever + // + if (gMainThreadId != std::this_thread::get_id()) + { + // + // Wait until the program has exited gracefully + // + while (gProgramRunning.test_and_set()); + } +} + +/*! + * 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 +} + +/*! + * Function periodically called by the logger to continue or cancel processing. + * + * \return true to continue the processing + * false to immediately stop the processing. + */ +static bool continueProcessingCb() +{ + return gShouldContinue.test_and_set(); +} + +//----------------------------------------------------------------------// +// Public functions // +//----------------------------------------------------------------------// + +/*! + * 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) +{ + sbg::CLoggerApp loggerApp; + int retCode; + + // + // Register sbgECom generic error logger to print to cout + // + sbgCommonLibSetLogCallback(onMessageCallback); + + // + // Register console signal events to intercept exit / CTRL-C signals + // and exit the application gracefully. + // + // This is required to properly flush and close output file. + // + gMainThreadId = std::this_thread::get_id(); + registerSignalsEvents(); + + // + // Register a callback with a atomic flag used to cancel processing at any time + // + gShouldContinue.test_and_set(); + loggerApp.setContinueCallback(continueProcessingCb); + + // + // Start the main console application and notify when processing has exited + // + gProgramRunning.test_and_set(); + retCode = loggerApp.exec(argc, argv); + gProgramRunning.clear(); + + return retCode; +} diff --git a/tools/sbgBasicLogger/src/sbgBasicLoggerAccumulators.c b/tools/sbgBasicLogger/src/sbgBasicLoggerAccumulators.c deleted file mode 100644 index b127303..0000000 --- a/tools/sbgBasicLogger/src/sbgBasicLoggerAccumulators.c +++ /dev/null @@ -1,58 +0,0 @@ -// sbgCommonLib -#include - -// sbgECom headers -#include - -// Local header -#include "sbgBasicLoggerAccumulators.h" - -//----------------------------------------------------------------------// -//- Private methods for SbgEComBasicLoggerAcc -// -//----------------------------------------------------------------------// - -void sbgBasicLoggerImuAccZeroInit(SbgBasicLoggerImuAcc *pAccumulator) -{ - assert(pAccumulator); - - memset(pAccumulator, 0x00, sizeof(*pAccumulator)); -} - -void sbgBasicLoggerImuAccAdd(SbgBasicLoggerImuAcc *pAccumulator, const SbgBasicLoggerImu *pNewImuData) -{ - assert(pAccumulator); - assert(pNewImuData); - - pAccumulator->imuAccumulated.timestamp = pNewImuData->timestamp; - pAccumulator->imuAccumulated.status |= pNewImuData->status; - - for (size_t i = 0; i < 3; i++) - { - pAccumulator->imuAccumulated.deltaAngle[i] += pNewImuData->deltaAngle[i]; - pAccumulator->imuAccumulated.deltaVelocity[i] += pNewImuData->deltaVelocity[i]; - } - - pAccumulator->imuAccumulated.temperature += pNewImuData->temperature; - - pAccumulator->nrAcc++; -} - -void sbgBasicLoggerImuAccGet(SbgBasicLoggerImuAcc *pAccumulator, SbgBasicLoggerImu *pDecimatedImu) -{ - assert(pAccumulator); - assert(pDecimatedImu); - - pDecimatedImu->timestamp = pAccumulator->imuAccumulated.timestamp; - pDecimatedImu->status = pAccumulator->imuAccumulated.status; - - for (size_t i = 0; i < 3; i++) - { - pDecimatedImu->deltaAngle[i] = pAccumulator->imuAccumulated.deltaAngle[i] / (double)pAccumulator->nrAcc; - pDecimatedImu->deltaVelocity[i] = pAccumulator->imuAccumulated.deltaVelocity[i] / (double)pAccumulator->nrAcc; - } - - pDecimatedImu->temperature = pAccumulator->imuAccumulated.temperature / (double)pAccumulator->nrAcc; - - sbgBasicLoggerImuAccZeroInit(pAccumulator); -} - diff --git a/tools/sbgBasicLogger/src/sbgBasicLoggerAccumulators.h b/tools/sbgBasicLogger/src/sbgBasicLoggerAccumulators.h deleted file mode 100644 index f636fd5..0000000 --- a/tools/sbgBasicLogger/src/sbgBasicLoggerAccumulators.h +++ /dev/null @@ -1,115 +0,0 @@ -/*! - * \file sbgBasicLoggerAccumulators.h - * \author SBG Systems - * \date July 29, 2021 - * - * \brief Helper accumulators for IMU data. - * - * \copyright Copyright (C) 2022, 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 - * 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. - * - * \endlicense - */ - -#ifndef SBG_BASIC_LOGGER_ACCUMULATORS_H -#define SBG_BASIC_LOGGER_ACCUMULATORS_H - -// sbgCommonLib headers -#include - -// sbgECom headers -#include - -//----------------------------------------------------------------------// -//- Structure definitions -// -//----------------------------------------------------------------------// - -/*! - * Define a set of IMU data - */ -typedef struct _SbgBasicLoggerImu -{ - uint32_t timestamp; /*!< The data time stamp in us */ - uint16_t status; /*!< IMU data status as defined in sbgECom logs */ - double deltaAngle[3]; /*!< X, Y, Z delta angles values in rad.s^-1 */ - double deltaVelocity[3]; /*!< X, Y, Z delta velocities values in m.s^-2 */ - double temperature; /*!< IMU temperature in °C. */ -} SbgBasicLoggerImu; - -/*! - * Used to accumulate and decimate gyros / accels values. - */ -typedef struct _SbgBasicLoggerImuAcc -{ - size_t nrAcc; /*!< Number of accumulated samples. */ - SbgBasicLoggerImu imuAccumulated; /*!< Accumulated IMU data. */ -} SbgBasicLoggerImuAcc; - - -/*! - * Define a set of raw IMU data. - */ -typedef struct _SbgBasicLoggerImuRaw -{ - uint32_t timestamp; /*!< Time in us since the sensor power up. */ - uint16_t status; /*!< IMU status bitmask. */ - - int64_t rawAccelerometers[3]; /*!< X,Y,Z raw accelerometers signed data. No scale factor defined. */ - int64_t rawGyroscopes[3]; /*!< X,Y,Z raw gyroscopes signed data. No scale factor defined. */ - int64_t rawMagnetometers[3]; /*!< X,Y,Z raw magnetometers signed data. No scale factor defined. */ - - int64_t rawTempAccels[3]; /*!< X,Y,Z raw accelerometers temperature signed data. No scale factor defined. */ - int64_t rawTempGyros[3]; /*!< X,Y,Z raw gyroscopes temperature signed data. No scale factor defined. */ - int64_t rawTempMags[3]; /*!< X,Y,Z raw magnetometers temperature signed data. No scale factor defined. */ - - int64_t rawAuxValues[3]; /*!< Spare raw sensor values for specifics needs. */ - int64_t rawTempAuxValues[3]; /*!< Spare raw sensor temperature values for specifics needs. */ -} SbgBasicLoggerImuRaw; - -/*! - * Used to accumulate and decimate raw IMU values. - */ -typedef struct _SbgBasicLoggerRawImuAcc -{ - size_t nrAcc; /*!< Number of accumulated samples. */ - SbgBasicLoggerImuRaw rawImuAcc; /*!< Accumulated raw IMU data. */ -} SbgBasicLoggerRawImuAcc; - -//----------------------------------------------------------------------// -//- Private methods for SbgEComBasicLoggerAcc -// -//----------------------------------------------------------------------// - -/*! - * Zero initialize / reset an IMU accumulator instance. - * - * \param[in] pAccumulator Instance on the IMU accumulator to reset. - */ -void sbgBasicLoggerImuAccZeroInit(SbgBasicLoggerImuAcc *pAccumulator); - -/*! - * Accumulate a new set of IMU data - * - * \param[in] pAccumulator Instance on the IMU accumulator. - * \param[in] pNewImuData New set of IMU data to accumulate - */ -void sbgBasicLoggerImuAccAdd(SbgBasicLoggerImuAcc *pAccumulator, const SbgBasicLoggerImu *pNewImuData); - -/*! - * Compute the decimated IMU values and reset the accumulator for next iteration. - * - * \param[in] pAccumulator Instance on the IMU accumulator. - * \param[out] pDecimatedImu The decimated IMU values. - */ -void sbgBasicLoggerImuAccGet(SbgBasicLoggerImuAcc *pAccumulator, SbgBasicLoggerImu *pDecimatedImu); - - -#endif // SBG_BASIC_LOGGER_ACCUMULATORS_H diff --git a/tools/sbgBasicLogger/src/sbgBasicLoggerFile.c b/tools/sbgBasicLogger/src/sbgBasicLoggerFile.c deleted file mode 100644 index 89a958f..0000000 --- a/tools/sbgBasicLogger/src/sbgBasicLoggerFile.c +++ /dev/null @@ -1,88 +0,0 @@ -// sbgCommonLib headers -#include - -// sbgECom headers -#include -#include - -// Local headers -#include "sbgBasicLoggerFile.h" - -//----------------------------------------------------------------------// -//- Private variables -// -//----------------------------------------------------------------------// - -static SbgBasicLoggerFileDesc gSbgBasicLoggerFileDescs[] = -{ - { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_STATUS, .pName = "STATUS", .pFileName = "log_status.txt" }, - { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_UTC_TIME, .pName = "UTC", .pFileName = "log_utc.txt" }, - { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_DIAG, .pName = "DIAG", .pFileName = "log_diag.txt" }, - { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_IMU_DATA, .pName = "IMU_DATA", .pFileName = "log_imu.txt" }, - { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_IMU_SHORT, .pName = "IMU_SHORT", .pFileName = "log_imuShort.txt" }, - { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_MAG, .pName = "MAG", .pFileName = "log_mag.txt" }, - { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_MAG_CALIB, .pName = "MAG_CALIB", .pFileName = "log_magCalib.txt" }, - - { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_EKF_EULER, .pName = "EULER", .pFileName = "log_euler.txt" }, - { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_EKF_QUAT, .pName = "QUAT", .pFileName = "log_quat.txt" }, - { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_EKF_NAV, .pName = "NAV", .pFileName = "log_nav.txt" }, - - { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_SHIP_MOTION, .pName = "SHIP_MOTION", .pFileName = "log_shipMotion.txt" }, - { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_SHIP_MOTION_HP, .pName = "SHIP_MOTION_HP", .pFileName = "log_shipMotionHP.txt" }, - - { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_GPS1_VEL, .pName = "GPS1_VEL", .pFileName = "log_gps1Vel.txt" }, - { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_GPS1_POS, .pName = "GPS1_POS", .pFileName = "log_gps1Pos.txt" }, - { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_GPS1_HDT, .pName = "GPS1_HDT", .pFileName = "log_gps1Hdt.txt" }, - { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_GPS1_RAW, .pName = "GPS1_RAW", .pFileName = "log_gps1Raw.txt" }, - { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_GPS1_SAT, .pName = "GPS1_SAT", .pFileName = "log_gps1Sat.txt" }, - - { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_GPS2_VEL, .pName = "GPS2_VEL", .pFileName = "log_gps2Vel.txt" }, - { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_GPS2_POS, .pName = "GPS2_POS", .pFileName = "log_gps2Pos.txt" }, - { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_GPS2_HDT, .pName = "GPS2_HDT", .pFileName = "log_gps2Hdt.txt" }, - { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_GPS2_RAW, .pName = "GPS2_RAW", .pFileName = "log_gps2Raw.txt" }, - { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_GPS2_SAT, .pName = "GPS2_SAT", .pFileName = "log_gps2Sat.txt" }, - - { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_RTCM_RAW, .pName = "RTCM_RAW", .pFileName = "log_rtcmRaw.txt" }, - - { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_ODO_VEL, .pName = "ODO_VEL", .pFileName = "log_odo.txt" }, - { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_DVL_BOTTOM_TRACK, .pName = "DVL_BOTTOM", .pFileName = "log_dvl_bottom.txt" }, - { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_DVL_WATER_TRACK, .pName = "DVL_WATER", .pFileName = "log_dvl_water.txt" }, - { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_AIR_DATA, .pName = "AIR_DATA", .pFileName = "log_air.txt" }, - { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_USBL, .pName = "USBL", .pFileName = "log_usbl.txt" }, - { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_DEPTH, .pName = "DEPTH", .pFileName = "log_depth.txt" }, - - { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_EVENT_A, .pName = "EVENT_IN_A", .pFileName = "log_eventInA.txt" }, - { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_EVENT_B, .pName = "EVENT_IN_B", .pFileName = "log_eventInB.txt" }, - { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_EVENT_C, .pName = "EVENT_IN_C", .pFileName = "log_eventInC.txt" }, - { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_EVENT_D, .pName = "EVENT_IN_D", .pFileName = "log_eventInD.txt" }, - { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_EVENT_E, .pName = "EVENT_IN_E", .pFileName = "log_eventInE.txt" }, - - { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_EVENT_OUT_A, .pName = "EVENT_OUT_A", .pFileName = "log_eventOutA.txt" }, - { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_EVENT_OUT_B, .pName = "EVENT_OUT_B", .pFileName = "log_eventOutB.txt" }, - - { .class = SBG_ECOM_CLASS_LOG_ECOM_1, .id = SBG_ECOM_LOG_FAST_IMU_DATA, .pName = "IMU_FAST", .pFileName = "log_imuFast.txt" }, - -}; - -//----------------------------------------------------------------------// -//- Public functions -// -//----------------------------------------------------------------------// - -const SbgBasicLoggerFileDesc *sbgBasicLoggerFileGetDesc(SbgEComClass msgClass, SbgEComMsgId msgId) -{ - const SbgBasicLoggerFileDesc *pDesc = NULL; - - for (size_t i = 0; i < SBG_ARRAY_SIZE(gSbgBasicLoggerFileDescs); i++) - { - const SbgBasicLoggerFileDesc *pTmpDesc; - - pTmpDesc = &gSbgBasicLoggerFileDescs[i]; - - if ((pTmpDesc->class == msgClass) && (pTmpDesc->id == msgId)) - { - pDesc = pTmpDesc; - break; - } - } - - return pDesc; -} diff --git a/tools/sbgBasicLogger/src/sbgBasicLoggerFile.h b/tools/sbgBasicLogger/src/sbgBasicLoggerFile.h deleted file mode 100644 index c92badc..0000000 --- a/tools/sbgBasicLogger/src/sbgBasicLoggerFile.h +++ /dev/null @@ -1,60 +0,0 @@ -/*! - * \file sbgBasicLoggerAccumulators.h - * \author SBG Systems - * \date July 29, 2021 - * - * \brief Ease output log files management. - * - * \copyright Copyright (C) 2022, 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 - * 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. - * - * \endlicense - */ - -#ifndef SBG_BASIC_LOGGER_FILE_H -#define SBG_BASIC_LOGGER_FILE_H - -// sbgCommonLib headers -#include - -// sbgECom headers -#include - -//----------------------------------------------------------------------// -//- Structure definitions -// -//----------------------------------------------------------------------// - -/*! - * SBG basic logger file descriptor. - */ -typedef struct _SbgBasicLoggerFileDesc -{ - SbgEComClass class; /*!< Message class. */ - SbgEComMsgId id; /*!< Message ID. */ - const char *pName; /*!< The log name. */ - const char *pFileName; /*!< File name suffix. */ -} SbgBasicLoggerFileDesc; - -//----------------------------------------------------------------------// -//- Public functions -// -//----------------------------------------------------------------------// - -/*! - * Return the file descriptor. - * - * \param[in] msgClass Message class. - * \param[in] msgId Message ID. - * \return File descriptor. - */ -const SbgBasicLoggerFileDesc *sbgBasicLoggerFileGetDesc(SbgEComClass msgClass, SbgEComMsgId msgId); - -#endif // SBG_BASIC_LOGGER_FILE_H diff --git a/tools/sbgBasicLogger/src/sbgBasicLoggerHandler.c b/tools/sbgBasicLogger/src/sbgBasicLoggerHandler.c deleted file mode 100644 index 64bea96..0000000 --- a/tools/sbgBasicLogger/src/sbgBasicLoggerHandler.c +++ /dev/null @@ -1,2064 +0,0 @@ -// sbgCommonLib headers -#include -#include - -// sbgECom headers -#include - -// Local headers -#include "sbgBasicLoggerAccumulators.h" -#include "sbgBasicLoggerHandler.h" - -//----------------------------------------------------------------------// -//- Private functions -// -//----------------------------------------------------------------------// - -/*! - * Write IMU short data into an output stream. - * - * \param[in] pImuShort IMU short data. - * \param[in] pStream Output stream. - */ -static void sbgBasicLoggerHandlerWriteImu(const SbgBasicLoggerImu *pImuShort, void *pStream) -{ - assert(pImuShort); - assert(pStream); - - fprintf(pStream, "%"PRIu32"\t%"PRIu32"\t%0.12lf\t%0.12lf\t%0.12lf\t%0.12lf\t%0.12lf\t%0.12lf\t%0.3lf\n", - pImuShort->timestamp, pImuShort->status, - pImuShort->deltaVelocity[0], pImuShort->deltaVelocity[1], pImuShort->deltaVelocity[2], - sbgRadToDegd(pImuShort->deltaAngle[0]), sbgRadToDegd(pImuShort->deltaAngle[1]), sbgRadToDegd(pImuShort->deltaAngle[2]), - pImuShort->temperature); - - fflush(pStream); -} - -/*! - * Write IMU data header into an output stream. - * - * \param[in] pStream Output stream. - */ -static void sbgBasicLoggerHandlerWriteImuHeader(void *pStream) -{ - assert(pStream); - - fprintf(pStream, "timestamp\tstatus\tdeltaVelX\tdeltaVelY\tdeltaVelZ\tdeltaAngleX\tdeltaAngleY\tdeltaAngleZ\ttemperature\n"); - fprintf(pStream, "(us)\t(N/A)\t(m.s^-2)\t(m.s^-2)\t(m.s^-2)\t(deg.s^-1)\t(deg.s^-1)\t(deg.s^-1)\t(°C)\n"); - fflush(pStream); -} - - -/*! - * Write a diag data into an output stream. - * - * \param[in] pDiag Diag data. - * \param[in] pStream Output stream. - */ -static void sbgBasicLoggerHandlerWriteDiag(const SbgLogDiagData *pDiag, const char *pUtcTimeStr, void *pStream) -{ - assert(pDiag); - assert(pUtcTimeStr); - assert(pStream); - - switch (pDiag->type) - { - case SBG_DEBUG_LOG_TYPE_ERROR: - fprintf(pStream, "%s *ERR * [%s]: %s\n", pUtcTimeStr, sbgErrorCodeToString(pDiag->errorCode), pDiag->string); - break; - case SBG_DEBUG_LOG_TYPE_WARNING: - fprintf(pStream, "%s *WARN* [%s]: %s\n", pUtcTimeStr, sbgErrorCodeToString(pDiag->errorCode), pDiag->string); - break; - case SBG_DEBUG_LOG_TYPE_INFO: - fprintf(pStream, "%s *INFO* %s\n", pUtcTimeStr, pDiag->string); - break; - case SBG_DEBUG_LOG_TYPE_DEBUG: - fprintf(pStream, "%s *DBG * %s\n", pUtcTimeStr, pDiag->string); - break; - default: - fprintf(pStream, "%s *UKN * [%s]: %s\n", pUtcTimeStr, sbgErrorCodeToString(pDiag->errorCode), pDiag->string); - } - - fflush(pStream); -} - -/*! - * Write an UTC data header into an output stream. - * - * \param[in] pStream Output stream. - */ -static void sbgBasicLoggerHandlerWriteUtcHeader(void *pStream) -{ - assert(pStream); - - fprintf(pStream, "timestamp\tstatus\tgpsTimeOfWeek\tyear\tmonth\tday\thour\tminute\tsecond\tnanosecond\n"); - fprintf(pStream, "(us)\t(NA)\t(ms)\t(yyyy)\t(mm)\t(dd)\t(hh)\t(mm)\t(ss)\t(ns)\n"); - fflush(pStream); -} - -/*! - * Write an UTC data into an output stream. - * - * \param[in] pUtc UTC data. - * \param[in] pStream Output stream. - */ -static void sbgBasicLoggerHandlerWriteUtc(const SbgLogUtcData *pUtc, void *pStream) -{ - assert(pUtc); - assert(pStream); - - fprintf(pStream, "%"PRIu32"\t%"PRIu16"\t%"PRIu32"\t%"PRIu16"\t%0.2"PRId8"\t%0.2"PRId8"\t%0.2"PRId8"\t%0.2"PRId8"\t%0.2"PRId8"\t%"PRId32"\n", - pUtc->timeStamp, - pUtc->status, - pUtc->gpsTimeOfWeek, - pUtc->year, - pUtc->month, - pUtc->day, - pUtc->hour, - pUtc->minute, - pUtc->second, - pUtc->nanoSecond - ); - fflush(pStream); -} - -/*! - * Write a status data into an output stream. - * - * \param[in] pStream Output stream. - */ -static void sbgBasicLoggerHandlerWriteStatusHeader(void *pStream) -{ - assert(pStream); - - fprintf(pStream, "timestamp\tgeneral\tcom2\tcom\taiding\n"); - fprintf(pStream, "(us)\t(NA)\t(NA)\t(NA)\t(NA)\n"); - fflush(pStream); -} - -/*! - * Write a status data into an output stream. - * - * \param[in] pStatus Status data. - * \param[in] pStream Output stream. - */ -static void sbgBasicLoggerHandlerWriteStatus(const SbgLogStatusData *pStatus, void *pStream) -{ - assert(pStatus); - assert(pStream); - - fprintf(pStream, "%" PRIu32 "\t%" PRIu16 "\t%" PRIu16 "\t%" PRIu32 "\t%" PRIu32 "\n", - pStatus->timeStamp, - pStatus->generalStatus, - pStatus->comStatus2, - pStatus->comStatus, - pStatus->aidingStatus - ); - - fflush(pStream); -} - -/*! - * Write an EKF Euler data header into an output stream. - * - * \param[in] pStream Output stream. - */ -static void sbgBasicLoggerHandlerWriteEkfEulerHeader(void *pStream) -{ - assert(pStream); - - fprintf(pStream, "timestamp\tstatus\troll\tpitch\tyaw\trollStd\tpitchStd\tyawStd\n"); - fprintf(pStream, "(us)\t(NA)\t(deg)\t(deg)\t(deg)\t(deg)\t(deg)\t(deg)\n"); - fflush(pStream); -} - -/*! - * Write an EKF Euler data into an output stream. - * - * \param[in] pData EKF Euler data. - * \param[in] pStream Output stream. - */ -static void sbgBasicLoggerHandlerWriteEkfEuler(const SbgLogEkfEulerData *pData, void *pStream) -{ - assert(pData); - assert(pStream); - - fprintf(pStream, "%"PRIu32"\t%"PRIu32"\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\n", - pData->timeStamp, - pData->status, - sbgRadToDegf(pData->euler[0]), - sbgRadToDegf(pData->euler[1]), - sbgRadToDegf(pData->euler[2]), - sbgRadToDegf(pData->eulerStdDev[0]), - sbgRadToDegf(pData->eulerStdDev[1]), - sbgRadToDegf(pData->eulerStdDev[2]) - ); - fflush(pStream); -} - -/*! - * Write an EKF Euler data header into an output stream. - * - * \param[in] pStream Output stream. - */ -static void sbgBasicLoggerHandlerWriteEkfQuatHeader(void *pStream) -{ - assert(pStream); - - fprintf(pStream, "timestamp\tstatus\tW\tX\tY\tZ\trollStd\tpitchStd\tyawStd\n"); - fprintf(pStream, "(us)\t(NA)\t(NA)\t(NA)\t(NA)\t(NA)\t(deg)\t(deg)\t(deg)\n"); - fflush(pStream); -} - -/*! - * Write an EKF quat data into an output stream. - * - * \param[in] pData EKF quat data. - * \param[in] pStream Output stream. - */ -static void sbgBasicLoggerHandlerWriteEkfQuat(const SbgLogEkfQuatData *pData, void *pStream) -{ - assert(pData); - assert(pStream); - - fprintf(pStream, "%"PRIu32"\t%"PRIu32"\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\n", - pData->timeStamp, - pData->status, - pData->quaternion[0], - pData->quaternion[1], - pData->quaternion[2], - pData->quaternion[3], - sbgRadToDegf(pData->eulerStdDev[0]), - sbgRadToDegf(pData->eulerStdDev[1]), - sbgRadToDegf(pData->eulerStdDev[2]) - ); - fflush(pStream); -} - -/*! - * Write an EKF nav data header into an output stream. - * - * \param[in] pStream Output stream. - */ -static void sbgBasicLoggerHandlerWriteEkfNavHeader(void *pStream) -{ - assert(pStream); - - fprintf(pStream, "timestamp\tstatus\tvelN\tvelE\tvelD\tvelStdN\tvelStdE\tvelStdD\tlatitude\tlongitude\taltitude\tlatitudeStd\tlongitudeStd\taltitudeStd\tundulation\n"); - fprintf(pStream, "(us)\t(NA)\t(m.s-1)\t(m.s-1)\t(m.s-1)\t(m.s-1)\t(m.s-1)\t(m.s-1)\t(deg)\t(deg)\t(m)\t(m)\t(m)\t(m)\t(m)\n"); - fflush(pStream); -} - -/*! - * Write an EKF nav data into an output stream. - * - * \param[in] pData EKF nav data. - * \param[in] pStream Output stream. - */ -static void sbgBasicLoggerHandlerWriteEkfNav(const SbgLogEkfNavData *pData, void *pStream) -{ - assert(pData); - assert(pStream); - - fprintf(pStream, "%"PRIu32"\t%"PRIu32"\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\n", - pData->timeStamp, - pData->status, - pData->velocity[0], - pData->velocity[1], - pData->velocity[2], - pData->velocityStdDev[0], - pData->velocityStdDev[1], - pData->velocityStdDev[2], - pData->position[0], - pData->position[1], - pData->position[2], - pData->positionStdDev[0], - pData->positionStdDev[1], - pData->positionStdDev[2], - pData->undulation - ); - fflush(pStream); -} - -/*! - * Write a ship motion data header into an output stream. - * - * \param[in] pStream Output stream. - */ -static void sbgBasicLoggerHandlerWriteShipMotionHeader(void *pStream) -{ - assert(pStream); - fprintf(pStream, "timestamp\tstatus\tsurge\tsway\theave\tsurgeVel\tswayVel\theaveVel\tsurgeAccel\tswayAccel\theaveAccel\theavePeriod\n"); - fprintf(pStream, "(us)\t(NA)\t(m)\t(m)\t(m)\t(m)\t(m)\t(m)\t(m)\t(m)\t(m)\t(s)\n"); - fflush(pStream); -} - -/*! - * Write a ship motion data into an output stream. - * - * \param[in] pData Ship motion data. - * \param[in] pStream Output stream. - */ -static void sbgBasicLoggerHandlerWriteShipMotion(const SbgLogShipMotionData *pData, void *pStream) -{ - assert(pData); - assert(pStream); - - fprintf(pStream, "%"PRIu32"\t%"PRIu16"\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n", - pData->timeStamp, - pData->status, - pData->shipMotion[0], - pData->shipMotion[1], - pData->shipMotion[2], - pData->shipVel[0], - pData->shipVel[1], - pData->shipVel[2], - pData->shipAccel[0], - pData->shipAccel[1], - pData->shipAccel[2], - pData->mainHeavePeriod - ); - fflush(pStream); -} - -/*! - * Write a GPS vel data header into an output stream. - * - * \param[in] pStream Output stream. - */ -static void sbgBasicLoggerHandlerWriteGpsVelHeader(void *pStream) -{ - assert(pStream); - fprintf(pStream, "timestamp\tstatus\ttimeOfWeek\tvelN\tvelE\tvelD\tvelNStd\tvelEStd\tvelDStd\tcourse\tcourseStd\n"); - fprintf(pStream, "(us)\t(NA)\t(ms)\t(m.s-1)\t(m.s-1)\t(m.s-1)\t(m.s-1)\t(m.s-1)\t(m.s-1)\t(degrees)\t(degrees)\n"); - fflush(pStream); -} - -/*! - * Write a GPS vel data into an output stream. - * - * \param[in] pData GPS vel data. - * \param[in] pStream Output stream. - */ -static void sbgBasicLoggerHandlerWriteGpsVel(const SbgLogGpsVel *pData, void *pStream) -{ - assert(pData); - assert(pStream); - - fprintf(pStream, "%"PRIu32"\t%"PRIu32"\t%"PRIu32"\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\n", - pData->timeStamp, - pData->status, - pData->timeOfWeek, - pData->velocity[0], - pData->velocity[1], - pData->velocity[2], - pData->velocityAcc[0], - pData->velocityAcc[1], - pData->velocityAcc[2], - pData->course, - pData->courseAcc - ); - fflush(pStream); -} - -/*! - * Write a GPS data header into an output stream. - * - * \param[in] pStream Output stream. - */ -static void sbgBasicLoggerHandlerWriteGpsHeader(void *pStream) -{ - assert(pStream); - fprintf(pStream, "timestamp\ttimeOfWeek\tstatus\taltitude\n"); - fprintf(pStream, "(us)\t(ms)\t(NA)\t(m)\n"); - fflush(pStream); -} - -/*! - * Write a GPS data into an output stream. - * - * \param[in] pData GPS data. - * \param[in] pStream Output stream. - */ -static void sbgBasicLoggerHandlerWriteGps(const SbgLogGpsPos *pData, void *pStream) -{ - assert(pData); - assert(pStream); - - fprintf(pStream, "%"PRIu32"\t%"PRIu32"\t%"PRIu32"\t%lf\n", - pData->timeStamp, - pData->timeOfWeek, - pData->status, - pData->altitude); - fflush(pStream); -} - -/*! - * Write a GPS hdt data header into an output stream. - * - * \param[in] pStream Output stream. - */ -static void sbgBasicLoggerHandlerWriteGpsHdtHeader(void *pStream) -{ - assert(pStream); - fprintf(pStream, "timestamp\tstatus\ttimeOfWeek\theading\theadingStd\tpitch\tpitchStd\tbaseline\n"); - fprintf(pStream, "(us)\t(NA)\t(ms)\t(degrees)\t(degrees)\t(degrees)\t(degrees)\t(m)\n"); - fflush(pStream); -} - -/*! - * Write a GPS hdt data into an output stream. - * - * \param[in] pData GPS hdt data. - * \param[in] pStream Output stream. - */ -static void sbgBasicLoggerHandlerWriteGpsHdt(const SbgLogGpsHdt *pData, void *pStream) -{ - assert(pData); - assert(pStream); - - fprintf(pStream, "%"PRIu32"\t%"PRIu16"\t%"PRIu32"\t%lf\t%lf\t%lf\t%lf\t%lf\n", - pData->timeStamp, - pData->status, - pData->timeOfWeek, - pData->heading, - pData->headingAccuracy, - pData->pitch, - pData->pitchAccuracy, - pData->baseline - ); - fflush(pStream); -} - -/*! - * Write a GPS Satellite data header into an output stream. - * - * \param[in] pStream Output stream. - */ -static void sbgBasicLoggerHandlerWriteGpsSatHeader(void *pStream) -{ - assert(pStream); -} - -/*! - * Write a GPS Satellite data into an output stream. - * - * \param[in] pData GPS satellite data. - * \param[in] pStream Output stream. - */ -static void sbgBasicLoggerHandlerWriteGpsSat(const SbgLogSatGroupData *pData, void *pStream) -{ - size_t satelliteIdx; - size_t signalIdx; - - assert(pData); - assert(pStream); - - fprintf(pStream, "%10"PRIu32" us | Satellites: %3zu ====================================================================\n", - pData->timeStamp, - pData->nrSatellites); - - for (satelliteIdx = 0; satelliteIdx < pData->nrSatellites; satelliteIdx++) - { - const SbgLogSatData *pSatEntry = &pData->pSatData[satelliteIdx]; - SbgEComSatElevationStatus elevationStatus; - char elevationStatusChr; - - elevationStatus = sbgLogSatDataGetElevationStatus(pSatEntry); - - if (elevationStatus == SBG_ECOM_SAT_ELEVATION_STATUS_SETTING) - { - elevationStatusChr = 's'; - } - else if (elevationStatus == SBG_ECOM_SAT_ELEVATION_STATUS_RISING) - { - elevationStatusChr = 'r'; - } - else - { - elevationStatusChr = '*'; - } - - fprintf(pStream, " [ID: %2"PRIu8" %-8s] Elevation: %3"PRIi8"%c Azimuth: %3"PRIi16" nrSignals: %2zu %-10s%-10s\n", - pSatEntry->id, - sbgLogSatDataGetConstellationIdAsStr(pSatEntry), - pSatEntry->elevation, - elevationStatusChr, - pSatEntry->azimuth, - pSatEntry->nrSignals, - sbgLogSatDataGetHealthStatusAsStr(pSatEntry), - sbgLogSatDataGetTrackingStatusAsStr(pSatEntry)); - - for (signalIdx = 0; signalIdx < pSatEntry->nrSignals; signalIdx++) - { - const SbgLogSatSignalData *pSignalEntry = &pSatEntry->pSignalData[signalIdx]; - char snrStr[16]; - - if (sbgLogSatSignalDataSnrIsValid(pSignalEntry)) - { - sprintf(snrStr, "%-3"PRIu8, pSignalEntry->snr); - } - else - { - strcpy(snrStr, " -"); - } - - fprintf(pStream, " %-14s SNR: %3s %-10s %-10s\n", - sbgLogSatSignalDataGetSignalIdAsStr(pSignalEntry), - snrStr, - sbgLogSatSignalDataGetHealthStatusAsStr(pSignalEntry), - sbgLogSatSignalDataGetTrackingStatusAsStr(pSignalEntry)); - } - } - - fflush(pStream); -} - -/*! - * Write an odometer data header into an output stream. - * - * \param[in] pStream Output stream. - */ -static void sbgBasicLoggerHandlerWriteOdometerHeader(void *pStream) -{ - assert(pStream); - fprintf(pStream, "timestamp\tstatus\tvelocity\n"); - fprintf(pStream, "(us)\t(NA)\t(m.s-1)\n"); - fflush(pStream); -} - -/*! - * Write an odometer data into an output stream. - * - * \param[in] pData Odometer data. - * \param[in] pStream Output stream. - */ -static void sbgBasicLoggerHandlerWriteOdometer(const SbgLogOdometerData *pData, void *pStream) -{ - assert(pData); - assert(pStream); - - fprintf(pStream, "%"PRIu32"\t%"PRIu16"\t%lf\n", - pData->timeStamp, - pData->status, - pData->velocity - ); - fflush(pStream); -} - -/*! - * Write a DVL data header into an output stream. - * - * \param[in] pStream Output stream. - */ -static void sbgBasicLoggerHandlerWriteDvlHeader(void *pStream) -{ - assert(pStream); - fprintf(pStream, "timestamp\tstatus\tvelX\tvelY\tvelZ\tvelQualityX\tvelQualityY\tvelQualityZ\n"); - fprintf(pStream, "(us)\t(NA)\t(m.s-1)\t(m.s-1)\t(m.s-1)\t(m.s-1)\t(m.s-1)\t(m.s-1)\t(m.s-1)\n"); - fflush(pStream); -} - -/*! - * Write an DVL data into an output stream. - * - * \param[in] pData DVL data. - * \param[in] pStream Output stream. - */ -static void sbgBasicLoggerHandlerWriteDvl(const SbgLogDvlData *pData, void *pStream) -{ - assert(pData); - assert(pStream); - - fprintf(pStream, "%"PRIu32"\t%"PRIu16"\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\n", - pData->timeStamp, - pData->status, - pData->velocity[0], - pData->velocity[1], - pData->velocity[2], - pData->velocityQuality[0], - pData->velocityQuality[1], - pData->velocityQuality[2] - ); - fflush(pStream); -} - -/*! - * Write an air data header into an output stream. - * - * \param[in] pStream Output stream. - */ -static void sbgBasicLoggerHandlerWriteAirHeader(void *pStream) -{ - assert(pStream); - fprintf(pStream, "timestamp\tstatus\tpressure\taltitude\tpressureDiff\tairSpeed\tairTemp\n"); - fprintf(pStream, "(us)\t(NA)\t(pascal)\t(meters)\t(pascal)\t(m.s-1)\t(celcius degrees)\n"); - fflush(pStream); -} - -/*! - * Write an air data into an output stream. - * - * \param[in] pData Air data. - * \param[in] pStream Output stream. - */ -static void sbgBasicLoggerHandlerWriteAir(const SbgLogAirData *pData, void *pStream) -{ - assert(pData); - assert(pStream); - - fprintf(pStream, "%"PRIu32"\t%"PRIu16"\t%lf\t%lf\t%lf\t%lf\t%lf\n", - pData->timeStamp, - pData->status, - pData->pressureAbs, - pData->altitude, - pData->pressureDiff, - pData->trueAirspeed, - pData->airTemperature - ); - fflush(pStream); -} - -/*! - * Write an USBL data header into an output stream. - * - * \param[in] pStream Output stream. - */ -static void sbgBasicLoggerHandlerWriteUsblHeader(void *pStream) -{ - assert(pStream); - fprintf(pStream, "timestamp\tstatus\tlatitude\tlongitude\tdepth\tlatitudeStd\tlongitudeStd\tdepthStd\n"); - fprintf(pStream, "(us)\t(NA)\t(degrees)\t(degrees)\t(meters)\t(meters)\t(meters)\t(meters)\n"); - fflush(pStream); -} - -/*! - * Write an USBL data into an output stream. - * - * \param[in] pData Air data. - * \param[in] pStream Output stream. - */ -static void sbgBasicLoggerHandlerWriteUsbl(const SbgLogUsblData *pData, void *pStream) -{ - assert(pData); - assert(pStream); - - fprintf(pStream, "%"PRIu32"\t%"PRIu16"\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\n", - pData->timeStamp, - pData->status, - pData->latitude, - pData->longitude, - pData->depth, - pData->latitudeAccuracy, - pData->longitudeAccuracy, - pData->depthAccuracy - ); - fflush(pStream); -} - -/*! - * Write a depth data header into an output stream. - * - * \param[in] pStream Output stream. - */ -static void sbgBasicLoggerHandlerWriteDepthHeader(void *pStream) -{ - assert(pStream); - fprintf(pStream, "timestamp\tstatus\tpressure\tdepth\n"); - fprintf(pStream, "(us)\t(NA)\t(pascal)\t(meters)\n"); - fflush(pStream); -} - -/*! - * Write an depth data into an output stream. - * - * \param[in] pData Depth data. - * \param[in] pStream Output stream. - */ -static void sbgBasicLoggerHandlerWriteDepth(const SbgLogDepth *pData, void *pStream) -{ - assert(pData); - assert(pStream); - - fprintf(pStream, "%"PRIu32"\t%"PRIu16"\t%lf\t%lf\n", - pData->timeStamp, - pData->status, - pData->pressureAbs, - pData->altitude - ); - fflush(pStream); -} - -/*! - * Write a GPS raw data into an output stream. - * - * \param[in] pData GPS raw data. - * \param[in] pStream Output stream. - */ -static void sbgBasicLoggerHandlerWriteRawRtcm(const SbgLogRawData *pData, void *pStream) -{ - assert(pData); - assert(pStream); - - fprintf(pStream, "%zu\n", - pData->bufferSize); - fflush(pStream); -} - -/*! - * Write a GPS raw data into an output stream. - * - * \param[in] pData GPS raw data. - * \param[in] pStream Output stream. - */ -static void sbgBasicLoggerHandlerWriteGpsRaw(const SbgLogRawData *pData, void *pStream) -{ - assert(pData); - assert(pStream); - - fprintf(pStream, "%zu\n", - pData->bufferSize); - fflush(pStream); -} - -/*! - * Write event data header into an output stream. - * - * \param[in] pStream Output stream. - */ -static void sbgBasicLoggerHandlerWriteEventHeader(void *pStream) -{ - assert(pStream); - - fprintf(pStream, "timestamp\tstatus\ttimeOffset0\ttimeOffset1\ttimeOffset2\ttimeOffset3\n"); - fflush(pStream); -} - -/*! - * Write event data into an output stream. - * - * \param[in] pEvent Event data. - * \param[in] pStream Output stream. - */ -static void sbgBasicLoggerHandlerWriteEvent(const SbgLogEvent *pEvent, void *pStream) -{ - assert(pEvent); - assert(pStream); - - fprintf(pStream, "%"PRIu32"\t%"PRIu16"\t%"PRIu16"\t%"PRIu16"\t%"PRIu16"\t%"PRIu16"\n", - pEvent->timeStamp, - pEvent->status, - pEvent->timeOffset0, - pEvent->timeOffset1, - pEvent->timeOffset2, - pEvent->timeOffset3); - fflush(pStream); -} - -/*! - * Write mag data into an output stream. - * - * \param[in] pStream Output stream. - */ -static void sbgBasicLoggerHandlerWriteMagHeader(void *pStream) -{ - assert(pStream); - - fprintf(pStream, "timestamp\tstatus\tmagX\tmagY\tmagZ\taccelX\taccelY\taccelZ\n"); - fprintf(pStream, "(us)\t(NA)\t(A.U)\t(A.U)\t(A.U)\t(m.s^-2)\t(m.s^-2)\t(m.s^-2)\n"); - fflush(pStream); -} - -/*! - * Write mag data into an output stream. - * - * \param[in] pMag Mag data. - * \param[in] pStream Output stream. - */ -static void sbgBasicLoggerHandlerWriteMag(const SbgLogMag *pMag, void *pStream) -{ - assert(pMag); - assert(pStream); - - fprintf(pStream, "%"PRIu32"\t%"PRIu16"\t%f\t%f\t%f\t%f\t%f\t%f\n", - pMag->timeStamp, - pMag->status, - pMag->magnetometers[0], pMag->magnetometers[1], pMag->magnetometers[2], - pMag->accelerometers[0], pMag->accelerometers[1], pMag->accelerometers[2]); - fflush(pStream); -} - -static SbgErrorCode sbgBasicLoggerHandlerGetUtcTimeFromTimeStamp(uint32_t timeStamp, SbgLogUtcData *pLastUtcData, struct tm *pRefUtcTimestamp, uint32_t *pUs) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - - assert(pLastUtcData); - assert(pRefUtcTimestamp); - assert(pUs); - - if (sbgEComLogUtcGetClockUtcStatus(pLastUtcData->status) != SBG_ECOM_UTC_INVALID) - { - time_t nrSeconds; - int64_t nrMicroseconds; - int32_t elapsedTime; - - pRefUtcTimestamp->tm_sec = pLastUtcData->second; - pRefUtcTimestamp->tm_min = pLastUtcData->minute; - pRefUtcTimestamp->tm_hour = pLastUtcData->hour; - pRefUtcTimestamp->tm_mday = pLastUtcData->day; - pRefUtcTimestamp->tm_mon = pLastUtcData->month - 1; - pRefUtcTimestamp->tm_year = pLastUtcData->year - 1900; - pRefUtcTimestamp->tm_isdst = -1; - - nrSeconds = mktime(pRefUtcTimestamp); - nrMicroseconds = (int64_t)nrSeconds * 1000000; - elapsedTime = timeStamp - pLastUtcData->timeStamp; - nrMicroseconds += elapsedTime; - - nrSeconds = nrMicroseconds / 1000000; - nrMicroseconds %= 1000000; - - *pRefUtcTimestamp = *localtime(&nrSeconds); - *pUs = (uint32_t)nrMicroseconds; - } - else - { - errorCode = SBG_ERROR; - } - - return errorCode; -} - -//----------------------------------------------------------------------// -//- sbgBasicLoggerCommonHandler methods -// -//----------------------------------------------------------------------// - -/*! - * Check if a handler is initialized. - * - * \param[in] pHandler Common handler. - * \return True if initialized. - */ -static bool sbgBasicLoggerCommonHandlerInitialized(SbgBasicLoggerCommonHandler *pHandler) -{ - assert(pHandler); - - if (pHandler->pDesc) - { - return true; - } - else - { - return false; - } -} - -/*! - * Common handler constructor. - * - * \param[in] pHandler IMU short handler. - * \param[in] consoleEnabled True to print IMU short data on console. - * \param[in] writingFile True to write IMU short data into a file. - */ -static void sbgBasicLoggerCommonHandlerConstruct(SbgBasicLoggerCommonHandler *pHandler, bool consoleEnabled, size_t consoleDecimation, bool writingFile, const char *pPathStr, size_t fileDecimation, bool writeHeader, SbgEComClass class, SbgEComMsgId id) -{ - assert(pHandler); - - pHandler->consoleEnabled = consoleEnabled; - pHandler->writingFile = writingFile; - pHandler->pDesc = sbgBasicLoggerFileGetDesc(class, id); - pHandler->pFILE = NULL; - - pHandler->consoleDecimation = consoleDecimation; - pHandler->fileDecimation = fileDecimation; - pHandler->writeHeader = writeHeader; - pHandler->headerWritten = false; - - pHandler->pPathStr = pPathStr; - pHandler->class = class; - pHandler->id = id; -} - -/*! - * Check if a file is opened. - * - * \param[in] pHandler Common handler. - * \return True if opened. - */ -static bool sbgBasicLoggerCommonHandlerFileOpened(SbgBasicLoggerCommonHandler *pHandler) -{ - assert(sbgBasicLoggerCommonHandlerInitialized(pHandler)); - return pHandler->pFILE; -} - -/*! - * Check if writing file is enabled. - * - * \param[in] pHandler Common handler. - * \return True if enabled. - */ -static bool sbgBasicLoggerCommonHandlerWritingFile(SbgBasicLoggerCommonHandler *pHandler) -{ - assert(sbgBasicLoggerCommonHandlerInitialized(pHandler)); - return pHandler->writingFile; -} - -/*! - * Check if console is enabled. - * - * \param[in] pHandler Common handler. - * \return True if enabled. - */ -static bool sbgBasicLoggerCommonHandlerConsoleEnabled(SbgBasicLoggerCommonHandler *pHandler) -{ - assert(sbgBasicLoggerCommonHandlerInitialized(pHandler)); - return pHandler->consoleEnabled; -} - -/*! - * Check if header writing is enabled. - * - * \param[in] pHandler Common handler. - * \return True if enabled. - */ -static bool sbgBasicLoggerCommonHandlerHeaderWriting(SbgBasicLoggerCommonHandler *pHandler) -{ - assert(sbgBasicLoggerCommonHandlerInitialized(pHandler)); - return pHandler->writeHeader; -} - -/*! - * Check if header have been written. - * - * \param[in] pHandler Common handler. - * \return True if written. - */ -static bool sbgBasicLoggerCommonHandlerHeaderWritten(SbgBasicLoggerCommonHandler *pHandler) -{ - assert(sbgBasicLoggerCommonHandlerInitialized(pHandler)); - return pHandler->headerWritten; -} - -/*! - * Get the path of the handler. - * - * \param[in] pHandler Common handler. - * \return File path. - */ -static FILE *sbgBasicLoggerCommonHandlerGetPath(SbgBasicLoggerCommonHandler *pHandler) -{ - if (!sbgBasicLoggerCommonHandlerFileOpened(pHandler)) - { - if (pHandler->pDesc) - { - SbgErrorCode errorCode; - SbgString fileStr; - SbgString pathStr; - - errorCode = sbgStringConstructCString(&pathStr, pHandler->pPathStr); - - if (errorCode == SBG_NO_ERROR) - { - errorCode = sbgStringConstructCString(&fileStr, pHandler->pDesc->pFileName); - - if (errorCode == SBG_NO_ERROR) - { - errorCode = sbgStringAppend(&pathStr, &fileStr); - - if (errorCode == SBG_NO_ERROR) - { - pHandler->pFILE = fopen(sbgStringGetCString(&pathStr), "wt"); - } - - sbgStringDestroy(&fileStr); - } - - sbgStringDestroy(&pathStr); - } - } - } - - return pHandler->pFILE; -} - -/*! - * Write log info on an output stream. - * - * \param[in] pHandler Common handler. - * \param[in] pStream Output stream. - */ -static void sbgBasicLoggerCommonHandlerWriteLogInfo(SbgBasicLoggerCommonHandler *pHandler, void *pStream) -{ - const SbgBasicLoggerFileDesc *pMsgDesc; - - assert(sbgBasicLoggerCommonHandlerInitialized(pHandler)); - assert(pStream); - - pMsgDesc = sbgBasicLoggerFileGetDesc(pHandler->class, pHandler->id); - - if (pMsgDesc) - { - fprintf(pStream, "%14s: ", pMsgDesc->pName); - } - else - { - fprintf(pStream, "class:%0.2u id:%0.2u: ", pHandler->class, pHandler->id); - } -} - -//----------------------------------------------------------------------// -//- Public functions -// -//----------------------------------------------------------------------// - -//----------------------------------------------------------------------// -//- IMU short handler methods -// -//----------------------------------------------------------------------// - -void sbgBasicLoggerImuShortHandlerConstruct(SbgBasicLoggerImuHandler *pHandler, bool consoleEnabled, size_t consoleDecimation, bool writingFile, const char *pPathStr, size_t fileDecimation, bool writeHeader) -{ - assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - - sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, consoleDecimation, writingFile, pPathStr, fileDecimation, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_IMU_SHORT); - - sbgBasicLoggerImuAccZeroInit(&pHandler->consoleAcc); - sbgBasicLoggerImuAccZeroInit(&pHandler->fileAcc); -} - -void sbgBasicLoggerImuHandlerConstruct(SbgBasicLoggerImuHandler *pHandler, bool consoleEnabled, size_t consoleDecimation, bool writingFile, const char *pPathStr, size_t fileDecimation, bool writeHeader) -{ - assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - - sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, consoleDecimation, writingFile, pPathStr, fileDecimation, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_IMU_DATA); - - sbgBasicLoggerImuAccZeroInit(&pHandler->consoleAcc); - sbgBasicLoggerImuAccZeroInit(&pHandler->fileAcc); -} - -void sbgBasicLoggerImuFastHandlerConstruct(SbgBasicLoggerImuHandler *pHandler, bool consoleEnabled, size_t consoleDecimation, bool writingFile, const char *pPathStr, size_t fileDecimation, bool writeHeader) -{ - assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - - sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, consoleDecimation, writingFile, pPathStr, fileDecimation, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_1, SBG_ECOM_LOG_FAST_IMU_DATA); - - sbgBasicLoggerImuAccZeroInit(&pHandler->consoleAcc); - sbgBasicLoggerImuAccZeroInit(&pHandler->fileAcc); -} - -void sbgBasicLoggerImuHandlerProcess(SbgBasicLoggerImuHandler *pHandler, const SbgBasicLoggerImu *pNewData) -{ - assert(pNewData); - assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - - if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) - { - FILE *pOutputFile; - - pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); - - if (pOutputFile) - { - if ((sbgBasicLoggerCommonHandlerHeaderWriting(&pHandler->commonHandler)) && (!sbgBasicLoggerCommonHandlerHeaderWritten(&pHandler->commonHandler))) - { - sbgBasicLoggerHandlerWriteImuHeader(pHandler->commonHandler.pFILE); - pHandler->commonHandler.headerWritten = true; - } - - sbgBasicLoggerImuAccAdd(&pHandler->fileAcc, pNewData); - - if (pHandler->fileAcc.nrAcc >= pHandler->commonHandler.fileDecimation) - { - SbgBasicLoggerImu decimated; - - sbgBasicLoggerImuAccGet(&pHandler->fileAcc, &decimated); - - sbgBasicLoggerHandlerWriteImu(&decimated, pOutputFile); - } - } - else - { - SBG_LOG_ERROR(SBG_NULL_POINTER, "no file found for IMU short log"); - } - } - - if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) - { - sbgBasicLoggerImuAccAdd(&pHandler->consoleAcc, pNewData); - - if (pHandler->consoleAcc.nrAcc >= pHandler->commonHandler.consoleDecimation) - { - SbgBasicLoggerImu decimated; - - sbgBasicLoggerImuAccGet(&pHandler->consoleAcc, &decimated); - - sbgBasicLoggerCommonHandlerWriteLogInfo(&pHandler->commonHandler, stdout); - sbgBasicLoggerHandlerWriteImu(&decimated, stdout); - } - } -} - -//----------------------------------------------------------------------// -//- IMU cluster handler methods -// -//----------------------------------------------------------------------// - - -//----------------------------------------------------------------------// -//- Diagnostic handler methods -// -//----------------------------------------------------------------------// - -void sbgBasicLoggerDiagHandlerConstruct(SbgBasicLoggerDiagHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr) -{ - assert(pHandler); - assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, 0, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_DIAG); -} - -void sbgBasicLoggerDiagHandlerProcess(SbgBasicLoggerDiagHandler *pHandler, SbgLogUtcData *pLastUtcData, const SbgLogDiagData *pDiag) -{ - struct tm utcTime; - char utcTimeStr[128]; - uint32_t us; - - assert(pHandler); - assert(pDiag); - assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - - if (sbgBasicLoggerHandlerGetUtcTimeFromTimeStamp(pDiag->timestamp, pLastUtcData, &utcTime, &us) == SBG_NO_ERROR) - { - char dateStr[64]; - - strftime(dateStr, sizeof(dateStr), "%Y-%m-%dT%H:%M:%S", &utcTime); - snprintf(utcTimeStr, sizeof(utcTimeStr), "%s:%.3" PRIu32 "Z", dateStr, us / 1000); - } - else - { - snprintf(utcTimeStr, sizeof(utcTimeStr), "%10u", pDiag->timestamp); - } - - if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) - { - FILE *pOutputFile; - - pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); - - if (pOutputFile) - { - sbgBasicLoggerHandlerWriteDiag(pDiag, utcTimeStr, pOutputFile); - } - } - - if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) - { - sbgBasicLoggerHandlerWriteDiag(pDiag, utcTimeStr, stdout); - } -} - -//----------------------------------------------------------------------// -//- UTC handler methods -// -//----------------------------------------------------------------------// - -void sbgBasicLoggerUtcHandlerConstruct(SbgBasicLoggerUtcHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) -{ - assert(pHandler); - assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_UTC_TIME); -} - -void sbgBasicLoggerUtcHandlerProcess(SbgBasicLoggerUtcHandler *pHandler, const SbgLogUtcData *pUtc) -{ - assert(pHandler); - assert(pUtc); - assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - - if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) - { - FILE *pOutputFile; - - pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); - - if (pOutputFile) - { - if (sbgBasicLoggerCommonHandlerHeaderWriting(&pHandler->commonHandler) && (!sbgBasicLoggerCommonHandlerHeaderWritten(&pHandler->commonHandler))) - { - sbgBasicLoggerHandlerWriteUtcHeader(pHandler->commonHandler.pFILE); - pHandler->commonHandler.headerWritten = true; - } - - sbgBasicLoggerHandlerWriteUtc(pUtc, pOutputFile); - } - } - - if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) - { - sbgBasicLoggerCommonHandlerWriteLogInfo(&pHandler->commonHandler, stdout); - sbgBasicLoggerHandlerWriteUtc(pUtc, stdout); - } -} - -//----------------------------------------------------------------------// -//- Status handler methods -// -//----------------------------------------------------------------------// - -void sbgBasicLoggerStatusHandlerConstruct(SbgBasicLoggerStatusHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) -{ - assert(pHandler); - assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_STATUS); -} - -void sbgBasicLoggerStatusHandlerProcess(SbgBasicLoggerStatusHandler *pHandler, const SbgLogStatusData *pStatus) -{ - assert(pHandler); - assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - assert(pStatus); - - if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) - { - FILE *pOutputFile; - - pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); - - if (pOutputFile) - { - if (sbgBasicLoggerCommonHandlerHeaderWriting(&pHandler->commonHandler) && (!sbgBasicLoggerCommonHandlerHeaderWritten(&pHandler->commonHandler))) - { - sbgBasicLoggerHandlerWriteStatusHeader(pHandler->commonHandler.pFILE); - pHandler->commonHandler.headerWritten = true; - } - - sbgBasicLoggerHandlerWriteStatus(pStatus, pOutputFile); - } - } - - if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) - { - sbgBasicLoggerCommonHandlerWriteLogInfo(&pHandler->commonHandler, stdout); - sbgBasicLoggerHandlerWriteStatus(pStatus, stdout); - } -} - -//----------------------------------------------------------------------// -//- EKF Euler handler methods -// -//----------------------------------------------------------------------// - -void sbgBasicLoggerEkfEulerHandlerConstruct(SbgBasicLoggerEkfEulerHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) -{ - assert(pHandler); - assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_EKF_EULER); -} - -void sbgBasicLoggerEkfEulerHandlerProcess(SbgBasicLoggerEkfEulerHandler *pHandler, const SbgLogEkfEulerData *pEkfEuler) -{ - assert(pHandler); - assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - assert(pEkfEuler); - - if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) - { - FILE *pOutputFile; - - pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); - - if (pOutputFile) - { - if (sbgBasicLoggerCommonHandlerHeaderWriting(&pHandler->commonHandler) && (!sbgBasicLoggerCommonHandlerHeaderWritten(&pHandler->commonHandler))) - { - sbgBasicLoggerHandlerWriteEkfEulerHeader(pHandler->commonHandler.pFILE); - pHandler->commonHandler.headerWritten = true; - } - - sbgBasicLoggerHandlerWriteEkfEuler(pEkfEuler, pOutputFile); - } - } - - if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) - { - sbgBasicLoggerCommonHandlerWriteLogInfo(&pHandler->commonHandler, stdout); - sbgBasicLoggerHandlerWriteEkfEuler(pEkfEuler, stdout); - } -} - -//----------------------------------------------------------------------// -//- EKF quat handler methods -// -//----------------------------------------------------------------------// - -void sbgBasicLoggerEkfQuatHandlerConstruct(SbgBasicLoggerEkfQuatHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) -{ - assert(pHandler); - assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_EKF_QUAT); -} - -void sbgBasicLoggerEkfQuatHandlerProcess(SbgBasicLoggerEkfQuatHandler *pHandler, const SbgLogEkfQuatData *pEkfQuat) -{ - assert(pHandler); - assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - assert(pEkfQuat); - - if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) - { - FILE *pOutputFile; - - pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); - - if (pOutputFile) - { - if (sbgBasicLoggerCommonHandlerHeaderWriting(&pHandler->commonHandler) && (!sbgBasicLoggerCommonHandlerHeaderWritten(&pHandler->commonHandler))) - { - sbgBasicLoggerHandlerWriteEkfQuatHeader(pHandler->commonHandler.pFILE); - pHandler->commonHandler.headerWritten = true; - } - - sbgBasicLoggerHandlerWriteEkfQuat(pEkfQuat, pOutputFile); - } - } - - if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) - { - sbgBasicLoggerCommonHandlerWriteLogInfo(&pHandler->commonHandler, stdout); - sbgBasicLoggerHandlerWriteEkfQuat(pEkfQuat, stdout); - } -} - -//----------------------------------------------------------------------// -//- EKF nav handler methods -// -//----------------------------------------------------------------------// - -void sbgBasicLoggerEkfNavHandlerConstruct(SbgBasicLoggerEkfNavHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) -{ - assert(pHandler); - assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_EKF_NAV); -} - -void sbgBasicLoggerEkfNavHandlerProcess(SbgBasicLoggerEkfNavHandler *pHandler, const SbgLogEkfNavData *pEkfNav) -{ - assert(pHandler); - assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - assert(pEkfNav); - - if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) - { - FILE *pOutputFile; - - pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); - - if (pOutputFile) - { - if (sbgBasicLoggerCommonHandlerHeaderWriting(&pHandler->commonHandler) && (!sbgBasicLoggerCommonHandlerHeaderWritten(&pHandler->commonHandler))) - { - sbgBasicLoggerHandlerWriteEkfNavHeader(pHandler->commonHandler.pFILE); - pHandler->commonHandler.headerWritten = true; - } - - sbgBasicLoggerHandlerWriteEkfNav(pEkfNav, pOutputFile); - } - } - - if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) - { - sbgBasicLoggerCommonHandlerWriteLogInfo(&pHandler->commonHandler, stdout); - sbgBasicLoggerHandlerWriteEkfNav(pEkfNav, stdout); - } -} - -//----------------------------------------------------------------------// -//- Ship motion methods -// -//----------------------------------------------------------------------// - -void sbgBasicLoggerShipMotionHandlerConstruct(SbgBasicLoggerShipMotionHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) -{ - assert(pHandler); - assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_SHIP_MOTION); -} - -void sbgBasicLoggerShipMotionHandlerProcess(SbgBasicLoggerShipMotionHandler *pHandler, const SbgLogShipMotionData *pShipMotion) -{ - assert(pHandler); - assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - assert(pShipMotion); - - if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) - { - FILE *pOutputFile; - - pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); - - if (pOutputFile) - { - if (sbgBasicLoggerCommonHandlerHeaderWriting(&pHandler->commonHandler) && (!sbgBasicLoggerCommonHandlerHeaderWritten(&pHandler->commonHandler))) - { - sbgBasicLoggerHandlerWriteShipMotionHeader(pHandler->commonHandler.pFILE); - pHandler->commonHandler.headerWritten = true; - } - - sbgBasicLoggerHandlerWriteShipMotion(pShipMotion, pOutputFile); - } - } - - if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) - { - sbgBasicLoggerCommonHandlerWriteLogInfo(&pHandler->commonHandler, stdout); - sbgBasicLoggerHandlerWriteShipMotion(pShipMotion, stdout); - } -} - -//----------------------------------------------------------------------// -//- Ship motion HP methods -// -//----------------------------------------------------------------------// - -void sbgBasicLoggerShipMotionHpHandlerConstruct(SbgBasicLoggerShipMotionHpHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) -{ - assert(pHandler); - assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_SHIP_MOTION_HP); -} - -void sbgBasicLoggerShipMotionHpHandlerProcess(SbgBasicLoggerShipMotionHpHandler *pHandler, const SbgLogShipMotionData *pShipMotion) -{ - assert(pHandler); - assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - assert(pShipMotion); - - if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) - { - FILE *pOutputFile; - - pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); - - if (pOutputFile) - { - if (sbgBasicLoggerCommonHandlerHeaderWriting(&pHandler->commonHandler) && (!sbgBasicLoggerCommonHandlerHeaderWritten(&pHandler->commonHandler))) - { - sbgBasicLoggerHandlerWriteShipMotionHeader(pHandler->commonHandler.pFILE); - pHandler->commonHandler.headerWritten = true; - } - - sbgBasicLoggerHandlerWriteShipMotion(pShipMotion, pOutputFile); - } - } - - if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) - { - sbgBasicLoggerCommonHandlerWriteLogInfo(&pHandler->commonHandler, stdout); - sbgBasicLoggerHandlerWriteShipMotion(pShipMotion, stdout); - } -} - -//----------------------------------------------------------------------// -//- GPS vel handler methods -// -//----------------------------------------------------------------------// - -void sbgBasicLoggerGpsVel1HandlerConstruct(SbgBasicLoggerGpsVelHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) -{ - assert(pHandler); - assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_GPS1_VEL); -} - -void sbgBasicLoggerGpsVel2HandlerConstruct(SbgBasicLoggerGpsVelHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) -{ - assert(pHandler); - assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_GPS2_VEL); -} - -void sbgBasicLoggerGpsVelHandlerProcess(SbgBasicLoggerGpsVelHandler *pHandler, const SbgLogGpsVel *pGps) -{ - assert(pHandler); - assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - assert(pGps); - - if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) - { - FILE *pOutputFile; - - pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); - - if (pOutputFile) - { - if (sbgBasicLoggerCommonHandlerHeaderWriting(&pHandler->commonHandler) && (!sbgBasicLoggerCommonHandlerHeaderWritten(&pHandler->commonHandler))) - { - sbgBasicLoggerHandlerWriteGpsVelHeader(pHandler->commonHandler.pFILE); - pHandler->commonHandler.headerWritten = true; - } - - sbgBasicLoggerHandlerWriteGpsVel(pGps, pOutputFile); - } - } - - if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) - { - sbgBasicLoggerCommonHandlerWriteLogInfo(&pHandler->commonHandler, stdout); - sbgBasicLoggerHandlerWriteGpsVel(pGps, stdout); - } -} - -//----------------------------------------------------------------------// -//- GPS pos handler methods -// -//----------------------------------------------------------------------// - -void sbgBasicLoggerGpsPos1HandlerConstruct(SbgBasicLoggerGpsPosHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) -{ - assert(pHandler); - assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_GPS1_POS); -} - -void sbgBasicLoggerGpsPos2HandlerConstruct(SbgBasicLoggerGpsPosHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) -{ - assert(pHandler); - assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_GPS2_POS); -} - -void sbgBasicLoggerGpsPosHandlerProcess(SbgBasicLoggerGpsPosHandler *pHandler, const SbgLogGpsPos *pGps) -{ - assert(pHandler); - assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - assert(pGps); - - if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) - { - FILE *pOutputFile; - - pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); - - if (pOutputFile) - { - if (sbgBasicLoggerCommonHandlerHeaderWriting(&pHandler->commonHandler) && (!sbgBasicLoggerCommonHandlerHeaderWritten(&pHandler->commonHandler))) - { - sbgBasicLoggerHandlerWriteGpsHeader(pHandler->commonHandler.pFILE); - pHandler->commonHandler.headerWritten = true; - } - - sbgBasicLoggerHandlerWriteGps(pGps, pOutputFile); - } - } - - if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) - { - sbgBasicLoggerCommonHandlerWriteLogInfo(&pHandler->commonHandler, stdout); - sbgBasicLoggerHandlerWriteGps(pGps, stdout); - } -} - -//----------------------------------------------------------------------// -//- GPS hdt handler methods -// -//----------------------------------------------------------------------// - -void sbgBasicLoggerGpsHdt1HandlerConstruct(SbgBasicLoggerGpsHdtHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) -{ - assert(pHandler); - assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_GPS1_HDT); -} - -void sbgBasicLoggerGpsHdt2HandlerConstruct(SbgBasicLoggerGpsHdtHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) -{ - assert(pHandler); - assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_GPS2_HDT); -} - -void sbgBasicLoggerGpsHdtHandlerProcess(SbgBasicLoggerGpsHdtHandler *pHandler, const SbgLogGpsHdt *pGps) -{ - assert(pHandler); - assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - assert(pGps); - - if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) - { - FILE *pOutputFile; - - pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); - - if (pOutputFile) - { - if (sbgBasicLoggerCommonHandlerHeaderWriting(&pHandler->commonHandler) && (!sbgBasicLoggerCommonHandlerHeaderWritten(&pHandler->commonHandler))) - { - sbgBasicLoggerHandlerWriteGpsHdtHeader(pHandler->commonHandler.pFILE); - pHandler->commonHandler.headerWritten = true; - } - - sbgBasicLoggerHandlerWriteGpsHdt(pGps, pOutputFile); - } - } - - if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) - { - sbgBasicLoggerCommonHandlerWriteLogInfo(&pHandler->commonHandler, stdout); - sbgBasicLoggerHandlerWriteGpsHdt(pGps, stdout); - } -} - -//----------------------------------------------------------------------// -//- Raw GPS handler methods -// -//----------------------------------------------------------------------// - -void sbgBasicLoggerGps1RawHandlerConstruct(SbgBasicLoggerGpsRawHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr) -{ - assert(pHandler); - assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, 0, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_GPS1_RAW); -} - -void sbgBasicLoggerGps2RawHandlerConstruct(SbgBasicLoggerGpsRawHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr) -{ - assert(pHandler); - assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, 0, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_GPS2_RAW); -} - -void sbgBasicLoggerGpsRawHandlerProcess(SbgBasicLoggerGpsRawHandler *pHandler, const SbgLogRawData *pRawGps) -{ - assert(pHandler); - assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - assert(pRawGps); - - if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) - { - FILE *pOutputFile; - - pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); - - if (pOutputFile) - { - sbgBasicLoggerHandlerWriteGpsRaw(pRawGps, pOutputFile); - } - } - - if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) - { - sbgBasicLoggerCommonHandlerWriteLogInfo(&pHandler->commonHandler, stdout); - sbgBasicLoggerHandlerWriteGpsRaw(pRawGps, stdout); - } -} - -//----------------------------------------------------------------------// -//- GNSS Satellite in View handler methods -// -//----------------------------------------------------------------------// - -void sbgBasicLoggerGps1SatHandlerConstruct(SbgBasicLoggerGpsSatHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr) -{ - assert(pHandler); - assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - - sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, 0, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_GPS1_SAT); -} - -void sbgBasicLoggerGps2SatHandlerConstruct(SbgBasicLoggerGpsSatHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr) -{ - assert(pHandler); - assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - - sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, 0, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_GPS2_SAT); -} - -void sbgBasicLoggerGpsSatHandlerProcess(SbgBasicLoggerGpsSatHandler *pHandler, const SbgLogSatGroupData *pData) -{ - assert(pHandler); - assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - assert(pData); - - if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) - { - FILE *pOutputFile; - - pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); - - if (pOutputFile) - { - if (sbgBasicLoggerCommonHandlerHeaderWriting(&pHandler->commonHandler) && (!sbgBasicLoggerCommonHandlerHeaderWritten(&pHandler->commonHandler))) - { - sbgBasicLoggerHandlerWriteGpsSatHeader(pHandler->commonHandler.pFILE); - pHandler->commonHandler.headerWritten = true; - } - - sbgBasicLoggerHandlerWriteGpsSat(pData, pOutputFile); - } - } - - if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) - { - sbgBasicLoggerCommonHandlerWriteLogInfo(&pHandler->commonHandler, stdout); - sbgBasicLoggerHandlerWriteGpsSat(pData, stdout); - } -} -//----------------------------------------------------------------------// -//- Odometer handler methods -// -//----------------------------------------------------------------------// - -void sbgBasicLoggerOdometerHandlerConstruct(SbgBasicLoggerOdometerHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) -{ - assert(pHandler); - assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_ODO_VEL); -} - -void sbgBasicLoggerOdometerHandlerProcess(SbgBasicLoggerOdometerHandler *pHandler, const SbgLogOdometerData *pData) -{ - assert(pHandler); - assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - assert(pData); - - if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) - { - FILE *pOutputFile; - - pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); - - if (pOutputFile) - { - if (sbgBasicLoggerCommonHandlerHeaderWriting(&pHandler->commonHandler) && (!sbgBasicLoggerCommonHandlerHeaderWritten(&pHandler->commonHandler))) - { - sbgBasicLoggerHandlerWriteOdometerHeader(pHandler->commonHandler.pFILE); - pHandler->commonHandler.headerWritten = true; - } - - sbgBasicLoggerHandlerWriteOdometer(pData, pOutputFile); - } - } - - if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) - { - sbgBasicLoggerCommonHandlerWriteLogInfo(&pHandler->commonHandler, stdout); - sbgBasicLoggerHandlerWriteOdometer(pData, stdout); - } -} - -//----------------------------------------------------------------------// -//- DVL handler methods -// -//----------------------------------------------------------------------// - -void sbgBasicLoggerDvlBottomHandlerConstruct(SbgBasicLoggerDvlHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) -{ - assert(pHandler); - assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_DVL_BOTTOM_TRACK); -} - -void sbgBasicLoggerDvlWaterHandlerConstruct(SbgBasicLoggerDvlHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) -{ - assert(pHandler); - assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_DVL_WATER_TRACK); -} - -void sbgBasicLoggerDvlHandlerProcess(SbgBasicLoggerDvlHandler *pHandler, const SbgLogDvlData *pData) -{ - assert(pHandler); - assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - assert(pData); - - if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) - { - FILE *pOutputFile; - - pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); - - if (pOutputFile) - { - if (sbgBasicLoggerCommonHandlerHeaderWriting(&pHandler->commonHandler) && (!sbgBasicLoggerCommonHandlerHeaderWritten(&pHandler->commonHandler))) - { - sbgBasicLoggerHandlerWriteDvlHeader(pHandler->commonHandler.pFILE); - pHandler->commonHandler.headerWritten = true; - } - - sbgBasicLoggerHandlerWriteDvl(pData, pOutputFile); - } - } - - if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) - { - sbgBasicLoggerCommonHandlerWriteLogInfo(&pHandler->commonHandler, stdout); - sbgBasicLoggerHandlerWriteDvl(pData, stdout); - } -} - -//----------------------------------------------------------------------// -//- Air handler methods -// -//----------------------------------------------------------------------// - -void sbgBasicLoggerAirHandlerConstruct(SbgBasicLoggerAirHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) -{ - assert(pHandler); - assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_AIR_DATA); -} - -void sbgBasicLoggerAirHandlerProcess(SbgBasicLoggerAirHandler *pHandler, const SbgLogAirData *pData) -{ - assert(pHandler); - assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - assert(pData); - - if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) - { - FILE *pOutputFile; - - pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); - - if (pOutputFile) - { - if (sbgBasicLoggerCommonHandlerHeaderWriting(&pHandler->commonHandler) && (!sbgBasicLoggerCommonHandlerHeaderWritten(&pHandler->commonHandler))) - { - sbgBasicLoggerHandlerWriteAirHeader(pHandler->commonHandler.pFILE); - pHandler->commonHandler.headerWritten = true; - } - - sbgBasicLoggerHandlerWriteAir(pData, pOutputFile); - } - } - - if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) - { - sbgBasicLoggerCommonHandlerWriteLogInfo(&pHandler->commonHandler, stdout); - sbgBasicLoggerHandlerWriteAir(pData, stdout); - } -} - -//----------------------------------------------------------------------// -//- USBL handler methods -// -//----------------------------------------------------------------------// - -void sbgBasicLoggerUsblHandlerConstruct(SbgBasicLoggerUsblHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) -{ - assert(pHandler); - assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_USBL); -} - -void sbgBasicLoggerUsblHandlerProcess(SbgBasicLoggerUsblHandler *pHandler, const SbgLogUsblData *pData) -{ - assert(pHandler); - assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - assert(pData); - - if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) - { - FILE *pOutputFile; - - pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); - - if (pOutputFile) - { - if (sbgBasicLoggerCommonHandlerHeaderWriting(&pHandler->commonHandler) && (!sbgBasicLoggerCommonHandlerHeaderWritten(&pHandler->commonHandler))) - { - sbgBasicLoggerHandlerWriteUsblHeader(pHandler->commonHandler.pFILE); - pHandler->commonHandler.headerWritten = true; - } - - sbgBasicLoggerHandlerWriteUsbl(pData, pOutputFile); - } - } - - if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) - { - sbgBasicLoggerCommonHandlerWriteLogInfo(&pHandler->commonHandler, stdout); - sbgBasicLoggerHandlerWriteUsbl(pData, stdout); - } -} - -//----------------------------------------------------------------------// -//- Depth handler methods -// -//----------------------------------------------------------------------// - -void sbgBasicLoggerDepthHandlerConstruct(SbgBasicLoggerDepthHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) -{ - assert(pHandler); - assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_DEPTH); -} - -void sbgBasicLoggerDepthHandlerProcess(SbgBasicLoggerDepthHandler *pHandler, const SbgLogDepth *pData) -{ - assert(pHandler); - assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - assert(pData); - - if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) - { - FILE *pOutputFile; - - pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); - - if (pOutputFile) - { - if (sbgBasicLoggerCommonHandlerHeaderWriting(&pHandler->commonHandler) && (!sbgBasicLoggerCommonHandlerHeaderWritten(&pHandler->commonHandler))) - { - sbgBasicLoggerHandlerWriteDepthHeader(pHandler->commonHandler.pFILE); - pHandler->commonHandler.headerWritten = true; - } - - sbgBasicLoggerHandlerWriteDepth(pData, pOutputFile); - } - } - - if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) - { - sbgBasicLoggerHandlerWriteDepth(pData, stdout); - } -} - -//----------------------------------------------------------------------// -//- Raw RTCM handler methods -// -//----------------------------------------------------------------------// - -void sbgBasicLoggerRawRtcmHandlerConstruct(SbgBasicLoggerRawRtcmHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr) -{ - assert(pHandler); - assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, 0, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_RTCM_RAW); -} - -void sbgBasicLoggerRawRtcmHandlerProcess(SbgBasicLoggerRawRtcmHandler *pHandler, const SbgLogRawData *pData) -{ - assert(pHandler); - assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - assert(pData); - - if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) - { - FILE *pOutputFile; - - pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); - - if (pOutputFile) - { - sbgBasicLoggerHandlerWriteRawRtcm(pData, pOutputFile); - } - } - - if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) - { - sbgBasicLoggerCommonHandlerWriteLogInfo(&pHandler->commonHandler, stdout); - sbgBasicLoggerHandlerWriteRawRtcm(pData, stdout); - } -} - -//----------------------------------------------------------------------// -//- Events handler methods -// -//----------------------------------------------------------------------// - -void sbgBasicLoggerEventInAHandlerConstruct(SbgBasicLoggerEventHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) -{ - assert(pHandler); - assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_EVENT_A); -} - -void sbgBasicLoggerEventInBHandlerConstruct(SbgBasicLoggerEventHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) -{ - assert(pHandler); - assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_EVENT_B); -} - -void sbgBasicLoggerEventInCHandlerConstruct(SbgBasicLoggerEventHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) -{ - assert(pHandler); - assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_EVENT_C); -} - -void sbgBasicLoggerEventInDHandlerConstruct(SbgBasicLoggerEventHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) -{ - assert(pHandler); - assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_EVENT_D); -} - -void sbgBasicLoggerEventInEHandlerConstruct(SbgBasicLoggerEventHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) -{ - assert(pHandler); - assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_EVENT_E); -} - -void sbgBasicLoggerEventOutAHandlerConstruct(SbgBasicLoggerEventHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) -{ - assert(pHandler); - assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_EVENT_OUT_A); -} - -void sbgBasicLoggerEventOutBHandlerConstruct(SbgBasicLoggerEventHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) -{ - assert(pHandler); - assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_EVENT_OUT_B); -} - -void sbgBasicLoggerEventHandlerProcess(SbgBasicLoggerEventHandler *pHandler, const SbgLogEvent *pEvent) -{ - assert(pHandler); - assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - assert(pEvent); - - if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) - { - FILE *pOutputFile; - - pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); - - if (pOutputFile) - { - if (sbgBasicLoggerCommonHandlerHeaderWriting(&pHandler->commonHandler) && (!sbgBasicLoggerCommonHandlerHeaderWritten(&pHandler->commonHandler))) - { - sbgBasicLoggerHandlerWriteEventHeader(pHandler->commonHandler.pFILE); - pHandler->commonHandler.headerWritten = true; - } - - sbgBasicLoggerHandlerWriteEvent(pEvent, pOutputFile); - } - } - - if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) - { - sbgBasicLoggerCommonHandlerWriteLogInfo(&pHandler->commonHandler, stdout); - sbgBasicLoggerHandlerWriteEvent(pEvent, stdout); - } -} - -//----------------------------------------------------------------------// -//- Mag handler methods -// -//----------------------------------------------------------------------// - -void sbgBasicLoggerMagHandlerConstruct(SbgBasicLoggerMagHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) -{ - assert(pHandler); - assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_MAG); -} - -void sbgBasicLoggerMagHandlerProcess(SbgBasicLoggerMagHandler *pHandler, const SbgLogMag *pMag) -{ - assert(pHandler); - assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); - assert(pMag); - - if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) - { - FILE *pOutputFile; - - pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); - - if (pOutputFile) - { - if (sbgBasicLoggerCommonHandlerHeaderWriting(&pHandler->commonHandler) && (!sbgBasicLoggerCommonHandlerHeaderWritten(&pHandler->commonHandler))) - { - sbgBasicLoggerHandlerWriteMagHeader(pHandler->commonHandler.pFILE); - pHandler->commonHandler.headerWritten = true; - } - - sbgBasicLoggerHandlerWriteMag(pMag, pOutputFile); - } - } - - if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) - { - sbgBasicLoggerCommonHandlerWriteLogInfo(&pHandler->commonHandler, stdout); - sbgBasicLoggerHandlerWriteMag(pMag, stdout); - } -} diff --git a/tools/sbgBasicLogger/src/sbgBasicLoggerHandler.h b/tools/sbgBasicLogger/src/sbgBasicLoggerHandler.h deleted file mode 100644 index 6b93367..0000000 --- a/tools/sbgBasicLogger/src/sbgBasicLoggerHandler.h +++ /dev/null @@ -1,823 +0,0 @@ -/*! - * \file sbgBasicLoggerHandler.h - * \author SBG Systems - * \date July 30, 2021 - * - * \brief Helper to handle logs. - * - * \copyright Copyright (C) 2022, 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 - * 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. - * - * \endlicense - */ - -#ifndef SBG_BASIC_LOGGER_HANDLER_H -#define SBG_BASIC_LOGGER_HANDLER_H - -// sbgCommonLib headers -#include - -// sbgECom headers -#include - -// Local headers -#include "sbgBasicLoggerAccumulators.h" -#include "sbgBasicLoggerFile.h" - -//----------------------------------------------------------------------// -//- Structure definitions -// -//----------------------------------------------------------------------// - -/*! - * Common log handler. - */ -typedef struct _SbgBasicLoggerCommonHandler -{ - bool consoleEnabled; /*!< True to print data on the console. */ - size_t consoleDecimation; /*!< Console decimation. */ - size_t fileDecimation; /*!< File decimation. */ - bool writingFile; /*!< True to save data into a file. */ - bool writeHeader; /*!< True to write header. */ - bool headerWritten; /*!< True if headers have been written. */ - const SbgBasicLoggerFileDesc *pDesc; /*!< File descriptor. */ - FILE *pFILE; /*!< File. */ - - const char *pPathStr; /*!< String path directory. */ - SbgEComClass class; /*!< Handler class. */ - SbgEComMsgId id; /*!< Handler message ID. */ -} SbgBasicLoggerCommonHandler; - -/*! - * IMU short log handler. - */ -typedef struct _SbgBasicLoggerImuHandler -{ - SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ - - SbgBasicLoggerImuAcc consoleAcc; /*!< Decimated IMU data for the console. */ - SbgBasicLoggerImuAcc fileAcc; /*!< Decimated IMU data for the file. */ -} SbgBasicLoggerImuHandler; - - -/*! - * Diag log handler. - */ -typedef struct _SbgBasicLoggerDiagHandler -{ - SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ -} SbgBasicLoggerDiagHandler; - -/*! - * UTC log handler. - */ -typedef struct _SbgBasicLoggerUtcHandler -{ - SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ -} SbgBasicLoggerUtcHandler; - -/*! - * Status log handler. - */ -typedef struct _SbgBasicLoggerStatusHandler -{ - SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ -} SbgBasicLoggerStatusHandler; - -/*! - * EKF Euler log handler. - */ -typedef struct _SbgBasicLoggerEkfEulerHandler -{ - SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ -} SbgBasicLoggerEkfEulerHandler; - -/*! - * EKF quat log handler. - */ -typedef struct _SbgBasicLoggerEkfQuatHandler -{ - SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ -} SbgBasicLoggerEkfQuatHandler; - -/*! - * EKF nav log handler. - */ -typedef struct _SbgBasicLoggerEkfNavHandler -{ - SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ -} SbgBasicLoggerEkfNavHandler; - -/*! - * Ship motion log handler. - */ -typedef struct _SbgBasicLoggerShipMotionHandler -{ - SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ -} SbgBasicLoggerShipMotionHandler; - -/*! - * Ship motion HP log handler. - */ -typedef struct _SbgBasicLoggerShipMotionHpHandler -{ - SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ -} SbgBasicLoggerShipMotionHpHandler; - -/*! - * GPS vel log handler. - */ -typedef struct _SbgBasicLoggerGpsVelHandler -{ - SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ -} SbgBasicLoggerGpsVelHandler; - -/*! - * GPS pos log handler. - */ -typedef struct _SbgBasicLoggerGpsPosHandler -{ - SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ -} SbgBasicLoggerGpsPosHandler; - -/*! - * GPS hdt log handler. - */ -typedef struct _SbgBasicLoggerGpsHdtHandler -{ - SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ -} SbgBasicLoggerGpsHdtHandler; - -/*! - * GPS RAW log handler. - */ -typedef struct _SbgBasicLoggerGpsRawHandler -{ - SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ -} SbgBasicLoggerGpsRawHandler; - -/*! - * GPS Satellite in View log handler. - */ -typedef struct _SbgBasicLoggerGpsSatHandler -{ - SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ -} SbgBasicLoggerGpsSatHandler; - -/*! - * Odometer log handler. - */ -typedef struct _SbgBasicLoggerOdometerHandler -{ - SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ -} SbgBasicLoggerOdometerHandler; - -/*! - * DVL log handler. - */ -typedef struct _SbgBasicLoggerDvlHandler -{ - SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ -} SbgBasicLoggerDvlHandler; - -/*! - * Air log handler. - */ -typedef struct _SbgBasicLoggerAirHandler -{ - SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ -} SbgBasicLoggerAirHandler; - -/*! - * USBL log handler. - */ -typedef struct _SbgBasicLoggerUsblHandler -{ - SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ -} SbgBasicLoggerUsblHandler; - -/*! - * Depth log handler. - */ -typedef struct _SbgBasicLoggerDepthHandler -{ - SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ -} SbgBasicLoggerDepthHandler; - -/*! - * RTCM log handler. - */ -typedef struct _SbgBasicLoggerRawRtcmHandler -{ - SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ -} SbgBasicLoggerRawRtcmHandler; - -/*! - * Event log handler. - */ -typedef struct _SbgBasicLoggerEventHandler -{ - SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ -} SbgBasicLoggerEventHandler; - -/*! - * Magnetometer log handler. - */ -typedef struct _SbgBasicLoggerMagHandler -{ - SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ -} SbgBasicLoggerMagHandler; - -//----------------------------------------------------------------------// -//- Public functions -// -//----------------------------------------------------------------------// - -/*! - * IMU short handler constructor. - * - * \param[in] pHandler IMU short handler. - * \param[in] consoleEnabled True to print IMU short data on console. - * \param[in] consoleDecimation Console decimation. - * \param[in] writingFile True to write IMU short data into a file. - * \param[in] pPathStr Path to write file logs. - * \param[in] fileDecimation File decimation. - * \param[in] writeHeader True to print and write header. - */ -void sbgBasicLoggerImuShortHandlerConstruct(SbgBasicLoggerImuHandler *pHandler, bool consoleEnabled, size_t consoleDecimation, bool writingFile, const char *pPathStr, size_t fileDecimation, bool writeHeader); - -/*! - * IMU data handler constructor. - * - * \param[in] pHandler IMU data handler. - * \param[in] consoleEnabled True to print IMU short data on console. - * \param[in] consoleDecimation Console decimation. - * \param[in] writingFile True to write IMU short data into a file. - * \param[in] pPathStr Path to write file logs. - * \param[in] fileDecimation File decimation. - * \param[in] writeHeader True to print and write header. - */ -void sbgBasicLoggerImuHandlerConstruct(SbgBasicLoggerImuHandler *pHandler, bool consoleEnabled, size_t consoleDecimation, bool writingFile, const char *pPathStr, size_t fileDecimation, bool writeHeader); - -/*! - * IMU fast handler constructor. - * - * \param[in] pHandler IMU fast data handler. - * \param[in] consoleEnabled True to print IMU short data on console. - * \param[in] consoleDecimation Console decimation. - * \param[in] writingFile True to write IMU short data into a file. - * \param[in] pPathStr Path to write file logs. - * \param[in] fileDecimation File decimation. - * \param[in] writeHeader True to print and write header. - */ -void sbgBasicLoggerImuFastHandlerConstruct(SbgBasicLoggerImuHandler *pHandler, bool consoleEnabled, size_t consoleDecimation, bool writingFile, const char *pPathStr, size_t fileDecimation, bool writeHeader); - -/*! - * Process an IMU short handler with a new data. - * - * \param[in] pHandler IMU short handler. - * \param[in] pNewData New IMU short data. - */ -void sbgBasicLoggerImuHandlerProcess(SbgBasicLoggerImuHandler *pHandler, const SbgBasicLoggerImu *pNewData); - - -/*! - * Diagnostic handler constructor. - * - * \param[in] pHandler Diag handler. - * \param[in] consoleEnabled True to print diag data on console. - * \param[in] writingFile True to write diag data into a file. - * \param[in] pPathStr Path to write file logs. - */ -void sbgBasicLoggerDiagHandlerConstruct(SbgBasicLoggerDiagHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr); - -/*! - * Process a diagnostic data. - * - * \param[in] pHandler Diag log handler. - * \param[in] pLastUtcData Last UTC data, used of timestamp. - * \param[in] pDiag Diagnostic data. - */ -void sbgBasicLoggerDiagHandlerProcess(SbgBasicLoggerDiagHandler *pHandler, SbgLogUtcData *pLastUtcData, const SbgLogDiagData *pDiag); - -/*! - * UTC handler constructor. - * - * \param[in] pHandler UTC handler. - * \param[in] consoleEnabled True to print UTC data on console. - * \param[in] writingFile True to write UTC data into a file. - * \param[in] pPathStr Path to write file logs. - * \param[in] writeHeader True to print and write header. - */ -void sbgBasicLoggerUtcHandlerConstruct(SbgBasicLoggerUtcHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); - -/*! - * Process an UTC data. - * - * \param[in] pHandler UTC log handler. - * \param[in] pLastUtcData Last UTC data, used of timestamp. - * \param[in] pDiag Diagnostic data. - */ -void sbgBasicLoggerUtcHandlerProcess(SbgBasicLoggerUtcHandler *pHandler, const SbgLogUtcData *pUtc); - -/*! - * Status handler constructor. - * - * \param[in] pHandler Status handler. - * \param[in] consoleEnabled True to print status data on console. - * \param[in] writingFile True to write status data into a file. - * \param[in] pPathStr Path to write file logs. - * \param[in] writeHeader True to print and write header. - */ -void sbgBasicLoggerStatusHandlerConstruct(SbgBasicLoggerStatusHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); - -/*! - * Process a status data. - * - * \param[in] pHandler Status log handler. - * \param[in] pDiag Status data. - */ -void sbgBasicLoggerStatusHandlerProcess(SbgBasicLoggerStatusHandler *pHandler, const SbgLogStatusData *pStatus); - -/*! - * EKF Euler handler constructor. - * - * \param[in] pHandler EKF Euler handler. - * \param[in] consoleEnabled True to print EKF Euler data on console. - * \param[in] writingFile True to write EKF Euler data into a file. - * \param[in] pPathStr Path to write file logs. - * \param[in] writeHeader True to print and write header. - */ -void sbgBasicLoggerEkfEulerHandlerConstruct(SbgBasicLoggerEkfEulerHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); - -/*! - * Process a EKF Euler data. - * - * \param[in] pHandler EKF Euler handler. - * \param[in] pEkfEuler EKF Euler data. - */ -void sbgBasicLoggerEkfEulerHandlerProcess(SbgBasicLoggerEkfEulerHandler *pHandler, const SbgLogEkfEulerData *pEkfEuler); - -/*! - * EKF quat handler constructor. - * - * \param[in] pHandler EKF quat handler. - * \param[in] consoleEnabled True to print EKF quat data on console. - * \param[in] writingFile True to write EKF quat data into a file. - * \param[in] pPathStr Path to write file logs. - * \param[in] writeHeader True to print and write header. - */ -void sbgBasicLoggerEkfQuatHandlerConstruct(SbgBasicLoggerEkfQuatHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); - -/*! - * Process a EKF quat data. - * - * \param[in] pHandler EKF quat handler. - * \param[in] pEkfQuat EKF quat data. - */ -void sbgBasicLoggerEkfQuatHandlerProcess(SbgBasicLoggerEkfQuatHandler *pHandler, const SbgLogEkfQuatData *pEkfEuler); - -/*! - * EKF nav handler constructor. - * - * \param[in] pHandler EKF nav handler. - * \param[in] consoleEnabled True to print EKF nav data on console. - * \param[in] writingFile True to write EKF nav data into a file. - * \param[in] pPathStr Path to write file logs. - * \param[in] writeHeader True to print and write header. - */ -void sbgBasicLoggerEkfNavHandlerConstruct(SbgBasicLoggerEkfNavHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); - -/*! - * Process a EKF nav data. - * - * \param[in] pHandler EKF nav handler. - * \param[in] pEkfEuler EKF nav data. - */ -void sbgBasicLoggerEkfNavHandlerProcess(SbgBasicLoggerEkfNavHandler *pHandler, const SbgLogEkfNavData *pEkfEuler); - -/*! - * Ship motion handler constructor. - * - * \param[in] pHandler Ship motion handler. - * \param[in] consoleEnabled True to print ship motion data on console. - * \param[in] writingFile True to write ship motion data into a file. - * \param[in] pPathStr Path to write file logs. - * \param[in] writeHeader True to print and write header. - */ -void sbgBasicLoggerShipMotionHandlerConstruct(SbgBasicLoggerShipMotionHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); - -/*! - * Process a ship motion data. - * - * \param[in] pHandler SHip motion handler. - * \param[in] pShipMotion Ship motion data. - */ -void sbgBasicLoggerShipMotionHandlerProcess(SbgBasicLoggerShipMotionHandler *pHandler, const SbgLogShipMotionData *pShipMotion); - -/*! - * Ship motion HP handler constructor. - * - * \param[in] pHandler Ship motion HP handler. - * \param[in] consoleEnabled True to print ship motion HP data on console. - * \param[in] writingFile True to write ship motion HP data into a file. - * \param[in] pPathStr Path to write file logs. - * \param[in] writeHeader True to print and write header. - */ -void sbgBasicLoggerShipMotionHpHandlerConstruct(SbgBasicLoggerShipMotionHpHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); - -/*! - * Process a ship motion HP data. - * - * \param[in] pHandler Ship motion HP handler. - * \param[in] pShipMotion Ship motion HP data. - */ -void sbgBasicLoggerShipMotionHpHandlerProcess(SbgBasicLoggerShipMotionHpHandler *pHandler, const SbgLogShipMotionData *pShipMotion); - -/*! - * GPS 1 vel handler constructor. - * - * \param[in] pHandler GPS vel handler. - * \param[in] consoleEnabled True to print GPS vel data on console. - * \param[in] writingFile True to write GPS vel data into a file. - * \param[in] pPathStr Path to write file logs. - * \param[in] writeHeader True to print and write header. - */ -void sbgBasicLoggerGpsVel1HandlerConstruct(SbgBasicLoggerGpsVelHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); - -/*! - * GPS 2 vel handler constructor. - * - * \param[in] pHandler GPS vel handler. - * \param[in] consoleEnabled True to print GPS vel data on console. - * \param[in] writingFile True to write GPS vel data into a file. - * \param[in] pPathStr Path to write file logs. - * \param[in] writeHeader True to print and write header. - */ -void sbgBasicLoggerGpsVel2HandlerConstruct(SbgBasicLoggerGpsVelHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); - -/*! - * Process a GPS vel data. - * - * \param[in] pHandler GPS vel handler. - * \param[in] pGps GPS vel data. - */ -void sbgBasicLoggerGpsVelHandlerProcess(SbgBasicLoggerGpsVelHandler *pHandler, const SbgLogGpsVel *pGps); - -/*! - * GPS handler constructor. - * - * \param[in] pHandler GPS handler. - * \param[in] consoleEnabled True to print GPS data on console. - * \param[in] writingFile True to write GPS data into a file. - * \param[in] pPathStr Path to write file logs. - * \param[in] writeHeader True to print and write header. - */ -void sbgBasicLoggerGpsPos1HandlerConstruct(SbgBasicLoggerGpsPosHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); - -/*! - * GPS 2 handler constructor. - * - * \param[in] pHandler GPS handler. - * \param[in] consoleEnabled True to print GPS data on console. - * \param[in] writingFile True to write GPS data into a file. - * \param[in] pPathStr Path to write file logs. - * \param[in] writeHeader True to print and write header. - */ -void sbgBasicLoggerGpsPos2HandlerConstruct(SbgBasicLoggerGpsPosHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); - -/*! - * Process a GPS data. - * - * \param[in] pHandler GPS handler. - * \param[in] pGps GPS data. - */ -void sbgBasicLoggerGpsPosHandlerProcess(SbgBasicLoggerGpsPosHandler *pHandler, const SbgLogGpsPos *pGps); - -/*! - * GPS 1 hdt handler constructor. - * - * \param[in] pHandler GPS hdt handler. - * \param[in] consoleEnabled True to print GPS vel data on console. - * \param[in] writingFile True to write GPS vel data into a file. - * \param[in] pPathStr Path to write file logs. - * \param[in] writeHeader True to print and write header. - */ -void sbgBasicLoggerGpsHdt1HandlerConstruct(SbgBasicLoggerGpsHdtHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); - -/*! - * GPS 2 hdt handler constructor. - * - * \param[in] pHandler GPS vel handler. - * \param[in] consoleEnabled True to print GPS hdt data on console. - * \param[in] writingFile True to write GPS hdt data into a file. - * \param[in] writeHeader True to print and write header. - */ -void sbgBasicLoggerGpsHdt2HandlerConstruct(SbgBasicLoggerGpsHdtHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); - -/*! - * Process a GPS hdt data. - * - * \param[in] pHandler GPS vel handler. - * \param[in] pGps GPS vel data. - */ -void sbgBasicLoggerGpsHdtHandlerProcess(SbgBasicLoggerGpsHdtHandler *pHandler, const SbgLogGpsHdt *pGps); - -/*! - * GPS raw handler constructor. - * - * \param[in] pHandler GPS raw handler. - * \param[in] consoleEnabled True to print GPS data on console. - * \param[in] writingFile True to write GPS data into a file. - * \param[in] pPathStr Path to write file logs. - * \param[in] writeHeader True to print and write header. - */ -void sbgBasicLoggerGps1RawHandlerConstruct(SbgBasicLoggerGpsRawHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr); - -/*! - * GPS raw handler constructor. - * - * \param[in] pHandler GPS raw handler. - * \param[in] consoleEnabled True to print GPS data on console. - * \param[in] writingFile True to write GPS data into a file. - * \param[in] pPathStr Path to write file logs. - * \param[in] writeHeader True to print and write header. - */ -void sbgBasicLoggerGps2RawHandlerConstruct(SbgBasicLoggerGpsRawHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr); - -/*! - * Process a GPS raw data. - * - * \param[in] pHandler GPS raw handler. - * \param[in] pGps GPS raw data. - */ -void sbgBasicLoggerGpsRawHandlerProcess(SbgBasicLoggerGpsRawHandler *pHandler, const SbgLogRawData *pRawGps); - -/*! - * GPS 1 Satellite in View handler constructor. - * - * \param[in] pHandler GPS Sat handler. - * \param[in] consoleEnabled True to print GPS data on console. - * \param[in] writingFile True to write GPS data into a file. - * \param[in] pPathStr Path to write file logs. - * \param[in] writeHeader True to print and write header. - */ -void sbgBasicLoggerGps1SatHandlerConstruct(SbgBasicLoggerGpsSatHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr); - -/*! - * GPS 2 Satellite in View handler constructor. - * - * \param[in] pHandler GPS Sat handler. - * \param[in] consoleEnabled True to print GPS data on console. - * \param[in] writingFile True to write GPS data into a file. - * \param[in] pPathStr Path to write file logs. - * \param[in] writeHeader True to print and write header. - */ -void sbgBasicLoggerGps2SatHandlerConstruct(SbgBasicLoggerGpsSatHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr); - -/*! - * Process a GPS Satellite in View data. - * - * \param[in] pHandler GPS Sat handler. - * \param[in] pData GPS Sat data. - */ -void sbgBasicLoggerGpsSatHandlerProcess(SbgBasicLoggerGpsSatHandler *pHandler, const SbgLogSatGroupData *pData); - -/*! - * Odometer handler constructor. - * - * \param[in] pHandler Odometer handler. - * \param[in] consoleEnabled True to print data on console. - * \param[in] writingFile True to write data into a file. - * \param[in] pPathStr Path to write file logs. - * \param[in] writeHeader True to print and write header. - */ -void sbgBasicLoggerOdometerHandlerConstruct(SbgBasicLoggerOdometerHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); - -/*! - * Process an odometer data. - * - * \param[in] pHandler Odometer handler. - * \param[in] pData Odometer data. - */ -void sbgBasicLoggerOdometerHandlerProcess(SbgBasicLoggerOdometerHandler *pHandler, const SbgLogOdometerData *pData); - -/*! - * DVL bottom handler constructor. - * - * \param[in] pHandler DVL bottom handler. - * \param[in] consoleEnabled True to print data on console. - * \param[in] writingFile True to write data into a file. - * \param[in] pPathStr Path to write file logs. - * \param[in] writeHeader True to print and write header. - */ -void sbgBasicLoggerDvlBottomHandlerConstruct(SbgBasicLoggerDvlHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); - -/*! - * DVL water handler constructor. - * - * \param[in] pHandler DVL water handler. - * \param[in] consoleEnabled True to print data on console. - * \param[in] writingFile True to write data into a file. - * \param[in] pPathStr Path to write file logs. - * \param[in] writeHeader True to print and write header. - */ -void sbgBasicLoggerDvlWaterHandlerConstruct(SbgBasicLoggerDvlHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); - -/*! - * Process a DVL data. - * - * \param[in] pHandler DVL handler. - * \param[in] pData DVL data. - */ -void sbgBasicLoggerDvlHandlerProcess(SbgBasicLoggerDvlHandler *pHandler, const SbgLogDvlData *pData); - -/*! - * Air handler constructor. - * - * \param[in] pHandler Air handler. - * \param[in] consoleEnabled True to print data on console. - * \param[in] writingFile True to write data into a file. - * \param[in] pPathStr Path to write file logs. - * \param[in] writeHeader True to print and write header. - */ -void sbgBasicLoggerAirHandlerConstruct(SbgBasicLoggerAirHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); - -/*! - * Process an air data. - * - * \param[in] pHandler Air handler. - * \param[in] pData Air data. - */ -void sbgBasicLoggerAirHandlerProcess(SbgBasicLoggerAirHandler *pHandler, const SbgLogAirData *pData); - -/*! - * USBL handler constructor. - * - * \param[in] pHandler USBL handler. - * \param[in] consoleEnabled True to print data on console. - * \param[in] writingFile True to write data into a file. - * \param[in] pPathStr Path to write file logs. - * \param[in] writeHeader True to print and write header. - */ -void sbgBasicLoggerUsblHandlerConstruct(SbgBasicLoggerUsblHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); - -/*! - * Process a USBL data. - * - * \param[in] pHandler USBL handler. - * \param[in] pData USBL data. - */ -void sbgBasicLoggerUsblHandlerProcess(SbgBasicLoggerUsblHandler *pHandler, const SbgLogUsblData *pData); - -/*! - * Depth handler constructor. - * - * \param[in] pHandler Depth handler. - * \param[in] consoleEnabled True to print data on console. - * \param[in] writingFile True to write data into a file. - * \param[in] pPathStr Path to write file logs. - * \param[in] writeHeader True to print and write header. - */ -void sbgBasicLoggerDepthHandlerConstruct(SbgBasicLoggerDepthHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); - -/*! - * Process a depth data. - * - * \param[in] pHandler Depth handler. - * \param[in] pData Depth data. - */ -void sbgBasicLoggerDepthHandlerProcess(SbgBasicLoggerDepthHandler *pHandler, const SbgLogDepth *pData); - -/*! - * Raw RTCM handler constructor. - * - * \param[in] pHandler Raw RTCM handler. - * \param[in] consoleEnabled True to print data on console. - * \param[in] writingFile True to write data into a file. - * \param[in] pPathStr Path to write file logs. - * \param[in] writeHeader True to print and write header. - */ -void sbgBasicLoggerRawRtcmHandlerConstruct(SbgBasicLoggerRawRtcmHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr); - -/*! - * Process a raw RTCM data. - * - * \param[in] pHandler Raw RTCM handler. - * \param[in] pData Raw RTCM data. - */ -void sbgBasicLoggerRawRtcmHandlerProcess(SbgBasicLoggerRawRtcmHandler *pHandler, const SbgLogRawData *pData); - - -/*! - * Event in A handler constructor. - * - * \param[in] pHandler Event handler. - * \param[in] consoleEnabled True to print data on console. - * \param[in] writingFile True to write data into a file. - * \param[in] pPathStr Path to write file logs. - * \param[in] writeHeader True to print and write header. - */ -void sbgBasicLoggerEventInAHandlerConstruct(SbgBasicLoggerEventHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); - -/*! - * Event in B handler constructor. - * - * \param[in] pHandler Event handler. - * \param[in] consoleEnabled True to print data on console. - * \param[in] writingFile True to write data into a file. - * \param[in] pPathStr Path to write file logs. - * \param[in] writeHeader True to print and write header. - */ -void sbgBasicLoggerEventInBHandlerConstruct(SbgBasicLoggerEventHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); - -/*! - * Event in C handler constructor. - * - * \param[in] pHandler Event handler. - * \param[in] consoleEnabled True to print data on console. - * \param[in] writingFile True to write data into a file. - * \param[in] pPathStr Path to write file logs. - * \param[in] writeHeader True to print and write header. - */ -void sbgBasicLoggerEventInCHandlerConstruct(SbgBasicLoggerEventHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); - -/*! - * Event in D handler constructor. - * - * \param[in] pHandler Event handler. - * \param[in] consoleEnabled True to print data on console. - * \param[in] writingFile True to write data into a file. - * \param[in] pPathStr Path to write file logs. - * \param[in] writeHeader True to print and write header. - */ -void sbgBasicLoggerEventInDHandlerConstruct(SbgBasicLoggerEventHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); - -/*! - * Event in E handler constructor. - * - * \param[in] pHandler Event handler. - * \param[in] consoleEnabled True to print data on console. - * \param[in] writingFile True to write data into a file. - * \param[in] pPathStr Path to write file logs. - * \param[in] writeHeader True to print and write header. - */ -void sbgBasicLoggerEventInEHandlerConstruct(SbgBasicLoggerEventHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); - -/*! - * Event out A handler constructor. - * - * \param[in] pHandler Event handler. - * \param[in] consoleEnabled True to print data on console. - * \param[in] writingFile True to write data into a file. - * \param[in] pPathStr Path to write file logs. - * \param[in] writeHeader True to print and write header. - */ -void sbgBasicLoggerEventOutAHandlerConstruct(SbgBasicLoggerEventHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); - -/*! - * Event out B handler constructor. - * - * \param[in] pHandler Event handler. - * \param[in] consoleEnabled True to print data on console. - * \param[in] writingFile True to write data into a file. - * \param[in] pPathStr Path to write file logs. - * \param[in] writeHeader True to print and write header. - */ -void sbgBasicLoggerEventOutBHandlerConstruct(SbgBasicLoggerEventHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); - -/*! - * Process an event data. - * - * \param[in] pHandler Event handler. - * \param[in] pEvent Event data. - */ -void sbgBasicLoggerEventHandlerProcess(SbgBasicLoggerEventHandler *pHandler, const SbgLogEvent *pEvent); - -/*! - * Mag handler constructor. - * - * \param[in] pHandler Mag handler. - * \param[in] consoleEnabled True to print mag data on console. - * \param[in] writingFile True to write mag data into a file. - * \param[in] pPathStr Path to write file logs. - * \param[in] writeHeader True to print and write header. - */ -void sbgBasicLoggerMagHandlerConstruct(SbgBasicLoggerMagHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); - -/*! - * Process a mag data. - * - * \param[in] pHandler Mag handler. - * \param[in] pMag Mag data. - */ -void sbgBasicLoggerMagHandlerProcess(SbgBasicLoggerMagHandler *pHandler, const SbgLogMag *pMag); - -#endif // SBG_BASIC_LOGGER_HANDLER_H diff --git a/tools/sbgEComApi/README.md b/tools/sbgEComApi/README.md index cccc961..15c5df0 100644 --- a/tools/sbgEComApi/README.md +++ b/tools/sbgEComApi/README.md @@ -3,7 +3,7 @@ The sbgEComApi command line tool let you read/update your product configuration through the JSON REST API. A typical REST API works using GET / POST requests other HTTP protocols. -The sbgEComApi tool encapsulate GET/POST requests so it can be used over a serial interface. +The sbgEComApi tool encapsulate GET/POST requests so it can be used over a serial or UDP interfaces. This tool is perfect to create complex and automated configuration scripts with no code. # Usage @@ -40,31 +40,54 @@ You should get the following response: {"title":"request successful","needReboot":false} ``` +## UDP access + +High Performance INS support sbgECom commands only on the Ethernet interface 0 and on a +default internal interface. + +This default internal interface can't be configured and always use the same settings. +It listen on the port 52140 and respond on port 52141. + +The example below shows how to retrieve the device settings using this default interface: + +```sh +sbgEComApi.exe -a 10.10.0.12 -I 52141 -O 52140 /api/v1/settings -g +``` + # Options You can access the tool help using the --help argument. ``` -Usage: sbgEComApi [-gpS] [--help] [--version] -s SERIAL_DEVICE -r SERIAL_BAUDRATE [-q QUERY] [-b BODY] [-B BODY_FILE] [-o OUTPUT_FILE] PATH +Usage: sbgEComApi [-gpS] [--help] [--version] [-a IP address] [-I UDP port in] [-O UDP port out] [-s SERIAL_DEVICE] [-r SERIAL_BAUDRATE] [-n NR_ATTEMPTS] [-t TIMEOUT] [-q QUERY] [-b BODY] [-B BODY_FILE] [-o OUTPUT_FILE] PATH + Access a RESTful SBG ECom server. - --help display this help and exit - --version display version info and exit - -s, --serial-device=SERIAL_DEVICE open a serial interface - -r, --serial-baudrate=SERIAL_BAUDRATE serial baudrate - -g, --method-get use the GET method (default) - -p, --method-post use the POST method - -q, --query=QUERY query string - -b, --body=BODY body (POST method only) - -B, --body-file=BODY_FILE file containing the body (POST method only) - -S, --print-status print the status code on the output stream - -o, --output-file=OUTPUT_FILE output file - PATH path + Serial example: sbgEComApi -s -r api/v1/settings -g + UDP example: sbgEComApi -a -I -O api/v1/settings -g + + --help display this help and exit + --version display version info and exit + -a, --addr-ip=IP address open an UDP interface + -I, --udp-port-in=UDP port in UDP port to receive data from (local) + -O, --udp-port-out=UDP port out UDP port to send data to (remote) + -s, --serial-device=SERIAL_DEVICE open a serial interface + -r, --serial-baudrate=SERIAL_BAUDRATE serial baudrate + -n, --nr-attempts=NR_ATTEMPTS number of transaction attempts + -t, --timeout=TIMEOUT reply time-out, in seconds + -g, --method-get use the GET method (default) + -p, --method-post use the POST method + -q, --query=QUERY query string, format=pretty&delta=true, format and delta options are optional + -b, --body=BODY body (POST method only) + -B, --body-file=BODY_FILE file containing the body (POST method only) + -S, --print-status print the status code on the output stream + -o, --output-file=OUTPUT_FILE output file + PATH GET or POST request path endpoint BODY or BODY_FILE may only be provided when using the POST method. If provided, BODY_FILE may not contain binary data. -PATH is a URI path component. +PATH is a URI path component such as api/v1/settings Exit codes : 0: 200 OK @@ -75,6 +98,6 @@ Exit codes : 69: 409 CONFLICT 82: 422 UNPROCESSABLE ENTITY 100: 200 INTERNAL SERVER ERROR -If an error occurs and is unrelated to the status code, or if the status code is unknown, -return EXIT_FAILURE (1). + +EXIT_FAILURE for a general error unrelated to the status code or if the status code is unknown. ``` \ No newline at end of file diff --git a/tools/sbgEComApi/src/main.c b/tools/sbgEComApi/src/main.c index d6ff9d6..140b0a3 100644 --- a/tools/sbgEComApi/src/main.c +++ b/tools/sbgEComApi/src/main.c @@ -24,6 +24,8 @@ #include #include #include +#include +#include #include // sbgECom headers @@ -146,7 +148,7 @@ static int convertStatusCodeToExitCode(uint16_t statusCode) * * \param[in] pDesc Exit code descriptor. */ -static void print_exit_code_mapping(const ExitCodeDesc *pDesc) +static void printExitCodeMapping(const ExitCodeDesc *pDesc) { printf(" %3u: %3u %s\n", pDesc->exitCode, pDesc->statusCode, pDesc->pMessage); } @@ -154,17 +156,17 @@ static void print_exit_code_mapping(const ExitCodeDesc *pDesc) /*! * Print help about exit codes. */ -static void print_exit_code_help(void) +static void printExitCodeHelp(void) { printf("Exit codes :\n"); for (size_t i = 0; i < SBG_ARRAY_SIZE(gExitCodeDescs); i++) { - print_exit_code_mapping(&gExitCodeDescs[i]); + printExitCodeMapping(&gExitCodeDescs[i]); } - printf("If an error occurs and is unrelated to the status code, or if the status code is unknown,\n"); - printf("return EXIT_FAILURE.\n"); + puts(""); + printf("EXIT_FAILURE for a general error unrelated to the status code or if the status code is unknown.\n\n"); } /*! @@ -431,10 +433,17 @@ int main(int argc, char **argv) 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; @@ -452,18 +461,25 @@ int main(int argc, char **argv) { pHelpArg = arg_lit0( NULL, "help", "display this help and exit"), pVersionArg = arg_lit0( NULL, "version", "display version info and exit"), - pSerialDeviceArg = arg_str1( "s", "serial-device", "SERIAL_DEVICE", "open a serial interface"), - pSerialBaudrateArg = arg_int1( "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)"), + + 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 optionnal"), + 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", "path"), + pPathArg = arg_str1( NULL, NULL, "PATH", "GET or POST request path endpoint"), pEndArg = arg_end(20), }; @@ -479,9 +495,16 @@ int main(int argc, char **argv) if (pHelpArg->count != 0) { printf("Usage: %s", PROGRAM_NAME); - arg_print_syntax(stdout, argTable, "\n"); + arg_print_syntax(stdout, argTable, "\n\n"); + + printf("Access a RESTful SBG ECom server.\n\n"); - arg_print_glossary(stdout, argTable, " %-25s %s\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"); @@ -490,10 +513,10 @@ int main(int argc, char **argv) printf("If provided, BODY_FILE may not contain binary data.\n"); puts(""); - printf("PATH is a URI path component.\n"); + printf("PATH is a URI path component such as api/v1/settings\n"); puts(""); - print_exit_code_help(); + printExitCodeHelp(); } else if (pVersionArg->count != 0) { @@ -519,11 +542,46 @@ int main(int argc, char **argv) if (exitCode == EXIT_SUCCESS) { - if ((pSerialDeviceArg->count != 0) && (pSerialBaudrateArg->count != 0)) + 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; - errorCode = sbgInterfaceSerialCreate(&ecomInterface, pSerialDeviceArg->sval[0], pSerialBaudrateArg->ival[0]); + 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) { @@ -676,10 +734,15 @@ int main(int argc, char **argv) } else { - SBG_LOG_ERROR(errorCode, "unable to open serial interface"); + 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;