diff --git a/CMakeLists.txt b/CMakeLists.txt index 0cc21b0..e311ac4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -29,11 +29,18 @@ add_executable( src/xdacallback.cpp ) +add_custom_command( + TARGET xsens_mti_node + PRE_LINK + COMMAND make + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/lib/xspublic +) + target_include_directories(xsens_mti_node PUBLIC $ $) -ament_target_dependencies(xsens_mti_node +ament_target_dependencies(xsens_mti_node rclcpp tf2 tf2_ros @@ -60,6 +67,9 @@ install(DIRECTORY launch param rviz urdf DESTINATION share/${PROJECT_NAME} ) +ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/env-hooks/${PROJECT_NAME}.dsv.in") + + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights diff --git a/README.txt b/README.txt index 3ac9a10..32876f1 100644 --- a/README.txt +++ b/README.txt @@ -1,10 +1,10 @@ =[ Xsens MTi driver for ROS 2.0]============================================================ Documentation: - You can find the full documentation in "/doc/xsensdeviceapi/doc/html/index.html" under "ROS MTi driver" section. The SDK can be downloaded from https://www.xsens.com/software-downloads. Please note, this is a 3rd Party driver with no official support. Check the compatibility section for the compatible devices. For official support on Xsens MTi products, please refer to Xsens knowledge base: https://base.xsens.com + You can find the full documentation in "/doc/xsensdeviceapi/doc/html/index.html" under "ROS MTi driver" section. The SDK can be downloaded from https://www.xsens.com/software-downloads. Please note, this is a 3rd Party driver built from MTSDK2021.2 with no official support. Check the compatibility section for the compatible devices. For official support on Xsens MTi products, please refer to Xsens knowledge base: https://base.xsens.com Prerequisites: - - ROS 2.0 Foxy + - ROS 2.0 Galactic/Foxy - C/C++ Compiler: GCC 5.4.0 or MSVC 14.0 - C++14 @@ -12,15 +12,15 @@ Building: - Copy bluespace_ai_xsens_mti_driver folder into your ROS 2.0 workspace 'src' folder. Make sure the permissions are set to o+rw on your files and directories. - - Build xspublic from your ament workspace: - $ pushd src/bluespace_ai_xsens_ros_mti_driver/lib/xspublic && make && popd - - Build Xsens MTi driver package: $ colcon build - Source workspace: $ source install/setup.bash +Note: Building of 'xspublic' from the ament workspace has been automated in the CMake script. To build it manually, run the following from the ROS2.0 workspace root: + $ pushd src/bluespace_ai_xsens_ros_mti_driver/lib/xspublic && make && popd + Changes in this release compared to the Xsens ROS 1.0 driver open source: - Added ROS 2.0 support: The ROS 1.0 wrapper node was modified to work with ROS 2.0. - Migration to declared configuration parameters: ROS 1.0 driver supported undeclared parameters. Since ROS 2.0 guidelines discourage undeclared parameters, there are some minor modifications in the xsens_mti_node.yaml to support all the configuration capabilities from ROS1.0. For more details check the config file. diff --git a/env-hooks/bluespace_ai_xsens_mti_driver.dsv.in b/env-hooks/bluespace_ai_xsens_mti_driver.dsv.in new file mode 100644 index 0000000..c9db325 --- /dev/null +++ b/env-hooks/bluespace_ai_xsens_mti_driver.dsv.in @@ -0,0 +1 @@ +prepend-non-duplicate;GAZEBO_MODEL_PATH;share diff --git a/lib/xspublic/xscommon/abstractadditionallogger.h b/lib/xspublic/xscommon/abstractadditionallogger.h index ac6ba46..6b233c6 100644 --- a/lib/xspublic/xscommon/abstractadditionallogger.h +++ b/lib/xspublic/xscommon/abstractadditionallogger.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -71,7 +71,8 @@ /*! \class AbstractAdditionalLogger Interface for all additional loggers that can be attached to the Journaller */ -class AbstractAdditionalLogger { +class AbstractAdditionalLogger +{ public: /*! \brief Destructor */ virtual ~AbstractAdditionalLogger() {} @@ -106,7 +107,7 @@ class AbstractAdditionalLogger { \param[in] function The name of the function from which the logging originates \param[in] msg The actual log message */ - virtual void log(JournalLogLevel level, char const * file, int line, char const * function, std::string const & msg) = 0; + virtual void log(JournalLogLevel level, char const* file, int line, char const* function, std::string const& msg) = 0; /*! \brief Write line to the log without decoration (timestamp, thread) \param[in] level The log level @@ -115,7 +116,7 @@ class AbstractAdditionalLogger { \param[in] function The name of the function from which the logging originates \param[in] msg The actual log message */ - virtual void logNoDecoration(JournalLogLevel level, char const * file, int line, char const * function, std::string const & msg) = 0; + virtual void logNoDecoration(JournalLogLevel level, char const* file, int line, char const* function, std::string const& msg) = 0; }; #endif diff --git a/lib/xspublic/xscommon/additionalloggerbase.cpp b/lib/xspublic/xscommon/additionalloggerbase.cpp index 794ec08..9d0c808 100644 --- a/lib/xspublic/xscommon/additionalloggerbase.cpp +++ b/lib/xspublic/xscommon/additionalloggerbase.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -138,7 +138,7 @@ bool AdditionalLoggerBase::logLevel(JournalLogLevel level) const /*! \copydoc AbstractAdditionalLogger::logNoDecoration The AdditionalLoggerBase simply forwards this call to the normal log method but this method can be overridden in derived classes */ -void AdditionalLoggerBase::logNoDecoration(JournalLogLevel level, char const * file, int line, char const * function, std::string const & msg) +void AdditionalLoggerBase::logNoDecoration(JournalLogLevel level, char const* file, int line, char const* function, std::string const& msg) { log(level, file, line, function, msg); } diff --git a/lib/xspublic/xscommon/additionalloggerbase.h b/lib/xspublic/xscommon/additionalloggerbase.h index 201d149..9a899f5 100644 --- a/lib/xspublic/xscommon/additionalloggerbase.h +++ b/lib/xspublic/xscommon/additionalloggerbase.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -84,7 +84,7 @@ class AdditionalLoggerBase : public virtual AbstractAdditionalLogger bool logLevel(JournalLogLevel level) const override; - void logNoDecoration(JournalLogLevel level, char const * file, int line, char const * function, std::string const & msg) override; + void logNoDecoration(JournalLogLevel level, char const* file, int line, char const* function, std::string const& msg) override; protected: virtual void onLogLevelChanged(JournalLogLevel newLevel, JournalLogLevel oldLevel); diff --git a/lib/xspublic/xscommon/calltracer.h b/lib/xspublic/xscommon/calltracer.h index dbdc924..ad21ada 100644 --- a/lib/xspublic/xscommon/calltracer.h +++ b/lib/xspublic/xscommon/calltracer.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -63,9 +63,9 @@ // #ifdef CALLTRACER_H -XSENS_COMPILER_WARNING("X0008", "calltracer.h should only be included ONCE and NEVER in a header file") + XSENS_COMPILER_WARNING("X0008", "calltracer.h should only be included ONCE and NEVER in a header file") #else -#define CALLTRACER_H + #define CALLTRACER_H #endif //#define TRACECALLS // define before including this header file to enable semi-automatic calltracing @@ -80,7 +80,8 @@ XSENS_COMPILER_WARNING("X0008", "calltracer.h should only be included ONCE and N during creation and another one when it gets destroyed, if a minimum time has passed since construction. */ -class CallTracer { +class CallTracer +{ public: Journaller* m_journal; std::string m_msg; diff --git a/lib/xspublic/xscommon/common_qdebug.h b/lib/xspublic/xscommon/common_qdebug.h index 88428bc..aec37a9 100644 --- a/lib/xspublic/xscommon/common_qdebug.h +++ b/lib/xspublic/xscommon/common_qdebug.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -66,29 +66,29 @@ #define COMMON_QDEBUG_H #ifdef __cplusplus -#include + #include -#define EXT_DEBUG_BODY(msg) __FILE__ << "(" << __LINE__ << "):" << __FUNCTION__ << msg + #define EXT_DEBUG_BODY(msg) __FILE__ << "(" << __LINE__ << "):" << __FUNCTION__ << msg -#ifdef XSENS_RELEASE - #define QTRACE(msg) ((void)0) - #define QDEBUG(msg) ((void)0) - #define QDEBUG_ACCURATE(msg) ((void)0) -#else -// #ifdef NO_QTRACE + #ifdef XSENS_RELEASE #define QTRACE(msg) ((void)0) -// #else -// #define QTRACE(msg) qDebug() << __FUNCTION__ << msg -// #endif - #define QDEBUG(msg) qDebug() << __FUNCTION__ << msg + #define QDEBUG(msg) ((void)0) + #define QDEBUG_ACCURATE(msg) ((void)0) + #else + // #ifdef NO_QTRACE + #define QTRACE(msg) ((void)0) + // #else + // #define QTRACE(msg) qDebug() << __FUNCTION__ << msg + // #endif + #define QDEBUG(msg) qDebug() << __FUNCTION__ << msg -// #include "xqtime.h" -// #define QDEBUG_ACCURATE(msg) qDebug() << xqLogTime() << __FUNCTION__ << msg -#endif // XSENS_RELEASE + // #include "xqtime.h" + // #define QDEBUG_ACCURATE(msg) qDebug() << xqLogTime() << __FUNCTION__ << msg + #endif // XSENS_RELEASE -#define QWARNING(msg) qWarning() << EXT_DEBUG_BODY(msg) -#define QCRITICAL(msg) qCritical() << EXT_DEBUG_BODY(msg) -#define QFATAL(msg) do not use this macro! qFatal has weird side-effects! + #define QWARNING(msg) qWarning() << EXT_DEBUG_BODY(msg) + #define QCRITICAL(msg) qCritical() << EXT_DEBUG_BODY(msg) + #define QFATAL(msg) do not use this macro! qFatal has weird side-effects! #endif -#endif // file guard +#endif diff --git a/lib/xspublic/xscommon/consolelogger.cpp b/lib/xspublic/xscommon/consolelogger.cpp index 36d44c9..45544d5 100644 --- a/lib/xspublic/xscommon/consolelogger.cpp +++ b/lib/xspublic/xscommon/consolelogger.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -86,7 +86,7 @@ ConsoleLogger::ConsoleLogger(JournalLogLevel logLevel, JournalLogLevel errLevel) \param[in] function The name of the function from which the logging originates \param[in] msg The actual log message */ -void ConsoleLogger::log(JournalLogLevel level, char const * file, int line, char const * function, std::string const & msg) +void ConsoleLogger::log(JournalLogLevel level, char const* file, int line, char const* function, std::string const& msg) { (void)file; (void)line; @@ -94,13 +94,13 @@ void ConsoleLogger::log(JournalLogLevel level, char const * file, int line, char size_t p = strlen(function); while (p > 0) { - if (function[p-1] == ':') + if (function[p - 1] == ':') break; --p; } if (level >= logLevel()) - std::cout << function+p << ": " << msg << std::endl; + std::cout << function + p << ": " << msg << std::endl; if (level >= debugLevel()) - std::cerr << function+p << ": " << msg << std::endl; + std::cerr << function + p << ": " << msg << std::endl; } diff --git a/lib/xspublic/xscommon/consolelogger.h b/lib/xspublic/xscommon/consolelogger.h index 4114a16..cfc6d71 100644 --- a/lib/xspublic/xscommon/consolelogger.h +++ b/lib/xspublic/xscommon/consolelogger.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -72,7 +72,7 @@ class ConsoleLogger : public AdditionalLoggerBase public: ConsoleLogger(JournalLogLevel logLevel, JournalLogLevel errLevel); - void log(JournalLogLevel level, char const * file, int line, char const * function, std::string const & msg) override; + void log(JournalLogLevel level, char const* file, int line, char const* function, std::string const& msg) override; }; #endif diff --git a/lib/xspublic/xscommon/enumexpandersbase.cpp b/lib/xspublic/xscommon/enumexpandersbase.cpp index 78b4cf1..775ac71 100644 --- a/lib/xspublic/xscommon/enumexpandersbase.cpp +++ b/lib/xspublic/xscommon/enumexpandersbase.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -64,7 +64,7 @@ #include "enumexpandersbase.h" -extern Journaller* gJournal; +extern Journaller* gJournal; //lint !e752 JLENUMEXPANDER(XsResultValue, JLENUMCASE(XRV_OK) //!< 0: Operation was performed successfully @@ -187,7 +187,8 @@ JLENUMEXPANDER(XsResultValue, JLENUMCASE(XRV_GNSSCONFIGURATIONERROR) //!< 401: A configuration item was refused by the GNSS module JLENUMCASE(XRV_GNSSCOMMTIMEOUT) //!< 402: The communication with the GNSS module timed out JLENUMCASE(XRV_GNSSERROR) //!< 403: Communication between the device and the GNSS module failed - JLENUMCASE(XRV_DEVICE_NOT_CALIBRATED) //!< 404: The EMTS of the device does not contain calibration data + JLENUMCASE(XRV_DEVICE_NOT_CALIBRATED) //!< 404: The EMTS of the device does not contain calibration data + JLENUMCASE(XRV_FILE_SYSTEM_CORRUPT) //!< 405: The internal file system of the device has become corrupt ) JLENUMEXPANDERHEX(XsXbusMessageId, @@ -422,8 +423,8 @@ JLENUMEXPANDERHEX(XsXbusMessageId, JLENUMCASE(XMID_GnssStatus) // 0xA7, JLENUMCASE(XMID_AdjustUtcTime) // 0xA8, JLENUMCASE(XMID_AdjustUtcTimeAck) // 0xA9, - JLENUMCASE(XMID_ReqManufacturerId) // 0xAA, - JLENUMCASE(XMID_ManufacturerId) // 0xAB, + JLENUMCASE(XMID_ReqAssemblyId) // 0xAA, + JLENUMCASE(XMID_AssemblyId) // 0xAB, JLENUMCASE(XMID_ReqAccessControlList) // 0xAE, JLENUMCASE(XMID_AccessControlList) // 0xAF, //JLENUMCASE(XMID_SetAccessControlList) // 0xAE, @@ -534,11 +535,32 @@ JLENUMEXPANDERHEX(XsXbusMessageId, JLENUMCASE(XMID_ForwardGnssDataAck) JLENUMCASE(XMID_ReqProductVariant) JLENUMCASE(XMID_ProductVariant) -); + JLENUMCASE(XMID_XsbData) + JLENUMCASE(XMID_UnlockMaintenanceMode) + JLENUMCASE(XMID_UnlockMaintenanceModeAck) +) + +JLENUMEXPANDERBITS(XsOption, + JLENUMCASEBITS(XSO_Calibrate) + JLENUMCASEBITS(XSO_Orientation) + JLENUMCASEBITS(XSO_KeepLastLiveData) + JLENUMCASEBITS(XSO_RetainLiveData) + JLENUMCASEBITS(XSO_RetainBufferedData) + JLENUMCASEBITS(XSO_OrientationInLiveStream) + JLENUMCASEBITS(XSO_OrientationInBufferedStream) + JLENUMCASEBITS(XSO_ApplyOrientationResetToCalData) + JLENUMCASEBITS(XSO_InterpolateMissingData) + JLENUMCASEBITS(XSO_SkipDataBundling) + JLENUMCASEBITS(XSO_ExpectNoRetransmissionsInFile) + JLENUMCASEBITS(XSO_Reserved) + JLENUMCASEBITSNONE(XSO_None) +) static void forceEnumExpanderInclusion() { volatile bool blah = false; JLWRITEG("blah" << XRV_OK << blah); JLWRITEG("blah" << XMID_AcceptMtw << blah); + JLWRITEG("blah" << XSO_None << blah); + (void) blah; } diff --git a/lib/xspublic/xscommon/enumexpandersbase.h b/lib/xspublic/xscommon/enumexpandersbase.h index 8fded02..3cb3a37 100644 --- a/lib/xspublic/xscommon/enumexpandersbase.h +++ b/lib/xspublic/xscommon/enumexpandersbase.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -68,8 +68,10 @@ #include "journaller.h" #include #include +#include JLENUMEXPPROTO(XsResultValue); JLENUMEXPPROTO(XsXbusMessageId); +JLENUMEXPPROTO(XsOption); #endif diff --git a/lib/xspublic/xscommon/extendedenum.h b/lib/xspublic/xscommon/extendedenum.h new file mode 100644 index 0000000..a219181 --- /dev/null +++ b/lib/xspublic/xscommon/extendedenum.h @@ -0,0 +1,175 @@ + +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions, and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions, and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the names of the copyright holders nor the names of their contributors +// may be used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY +// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL +// THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT +// OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY OR +// TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.THE LAWS OF THE NETHERLANDS +// SHALL BE EXCLUSIVELY APPLICABLE AND ANY DISPUTES SHALL BE FINALLY SETTLED UNDER THE RULES +// OF ARBITRATION OF THE INTERNATIONAL CHAMBER OF COMMERCE IN THE HAGUE BY ONE OR MORE +// ARBITRATORS APPOINTED IN ACCORDANCE WITH SAID RULES. +// + + +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions, and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions, and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the names of the copyright holders nor the names of their contributors +// may be used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY +// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL +// THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT +// OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY OR +// TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.THE LAWS OF THE NETHERLANDS +// SHALL BE EXCLUSIVELY APPLICABLE AND ANY DISPUTES SHALL BE FINALLY SETTLED UNDER THE RULES +// OF ARBITRATION OF THE INTERNATIONAL CHAMBER OF COMMERCE IN THE HAGUE BY ONE OR MORE +// ARBITRATORS APPOINTED IN ACCORDANCE WITH SAID RULES. +// + +#ifndef EXTENDEDENUM_H +#define EXTENDEDENUM_H + +#ifdef __cplusplus + +/*! \brief This template can be used to extend an existing enum with new values. + + Example, extend QNetworkReply::NetworkError with 2 new values: + enum ICloudServiceError : int + { + CSE_GenericParsingError = QNetworkReply::NetworkError::UnknownServerError+1, + CSE_FileCreationError, + }; + + typedef ExtendedEnum CloudServiceError; + + + It can now be used as such: + void func(QNetworkReply::NetworkError err) + void func(CloudServiceError err) + void func(ICloudServiceError err) + + switch(err.value()) + { + case QNetworkReply::NoError: + case CSE_GenericParsingError: + } + + if (err == QNetworkReply::NoError) + if (err == CSE_GenericParsingError) + + etc... + + \note There is no way for the compiler to ensure that the enum values do not overlap! +*/ +template +class ExtendedEnum +{ +public: + //! \brief default constructor, initializes to defValue template parameter + inline ExtendedEnum() + : m_value(defValue) + { + } + + //! \brief copy constructor + inline ExtendedEnum(ExtendedEnum const& e) + : m_value(e.m_value) + { + } + + //! \brief Initializing constructor, initializes to \a e + inline ExtendedEnum(ExtensionEnumT e) + : m_value(static_cast(e)) + { + } + + //! \brief Initializing constructor, initializes to \a e + inline ExtendedEnum(BaseEnumT e) + : m_value(static_cast(e)) + { + } + + //! \brief Initializing constructor, initializes to equivalent of integer value \a val + explicit inline ExtendedEnum(int val) + : m_value(val) + { + } + + //! \brief Implicit conversion operator to ExtensionEnumT + inline operator ExtensionEnumT() const + { + return static_cast(m_value); + } + + //! \brief Implicit conversion operator to BaseEnumT + inline operator BaseEnumT() const + { + return static_cast(m_value); + } + + //! \brief Assignment operator, copies from \a e + inline ExtendedEnum& operator = (ExtendedEnum const& e) = default; + + //! \brief Assignment operator, copies from \a e + inline ExtendedEnum& operator = (ExtensionEnumT e) + { + m_value = static_cast(e); + return *this; + } + + //! \brief Assignment operator, copies from \a e + inline ExtendedEnum& operator = (BaseEnumT e) + { + m_value = static_cast(e); + return *this; + } + + //! \brief Returns the integer value, useful in a switch + inline int value() const + { + return m_value; + } + +private: + int m_value; //!< The contained value, stored as int +}; + +#endif + +#endif diff --git a/lib/xspublic/xscommon/fwupdate.c b/lib/xspublic/xscommon/fwupdate.c new file mode 100644 index 0000000..1b7ccca --- /dev/null +++ b/lib/xspublic/xscommon/fwupdate.c @@ -0,0 +1,531 @@ + +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions, and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions, and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the names of the copyright holders nor the names of their contributors +// may be used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY +// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL +// THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT +// OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY OR +// TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.THE LAWS OF THE NETHERLANDS +// SHALL BE EXCLUSIVELY APPLICABLE AND ANY DISPUTES SHALL BE FINALLY SETTLED UNDER THE RULES +// OF ARBITRATION OF THE INTERNATIONAL CHAMBER OF COMMERCE IN THE HAGUE BY ONE OR MORE +// ARBITRATORS APPOINTED IN ACCORDANCE WITH SAID RULES. +// + + +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions, and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions, and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the names of the copyright holders nor the names of their contributors +// may be used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY +// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL +// THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT +// OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY OR +// TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.THE LAWS OF THE NETHERLANDS +// SHALL BE EXCLUSIVELY APPLICABLE AND ANY DISPUTES SHALL BE FINALLY SETTLED UNDER THE RULES +// OF ARBITRATION OF THE INTERNATIONAL CHAMBER OF COMMERCE IN THE HAGUE BY ONE OR MORE +// ARBITRATORS APPOINTED IN ACCORDANCE WITH SAID RULES. +// + +#include "fwupdate.h" +#include "string.h" +#include "xbus.h" + +#ifndef LOG + #define LOG(...) +#endif + +#define XMID_FIRMWARE_UPDATE (0xF2) + +#define FWUP_DEFAULT_SLICE_SIZE (64) + + +/*! \brief Firmware updater commands +*/ +#define FWUP_READY (unsigned char)0x52 //'R' +#define FWUP_OK (unsigned char)0x53 //'S' +#define FWUP_CSERROR (unsigned char)0x45 //'E' +#define FWUP_CRITICAL (unsigned char)0x46 //'F' +#define FWUP_FINISHED (unsigned char)0x46 //'F' +#define FWUP_PAGE (unsigned char)0x50 //'P' +#define FWUP_HEADER (unsigned char)0x53 //'S' +#define FWUP_PAGESLICE (unsigned char)0x54 //'T' +#define FWUP_STARTED (unsigned char)0x41 //'A' +#define FWUP_OTHER (unsigned char)0x4F //'O' + + +/*! \brief Helper function for converting a firmware updater command to a string +*/ +const char* commandToString(uint8_t command) +{ + switch (command) + { + case FWUP_READY: + return ("FWUP_READY"); + case FWUP_HEADER: + return ("FWUP_HEADER"); + case FWUP_FINISHED: + return ("FWUP_FINISHED"); + case FWUP_PAGE: + return ("FWUP_PAGE"); + case FWUP_PAGESLICE: + return ("FWUP_PAGESLICE"); + case FWUP_STARTED: + return ("FWUP_STARTED"); + case FWUP_OTHER: + return ("FWUP_OTHER"); + default: + return ("unknown"); + } +} + + +/*! \brief Helper function for converting a firmware updater command acknowledge to a string +*/ +const char* ackToString(uint8_t command) +{ + switch (command) + { + case FWUP_READY: + return "FWUP_READY"; + case FWUP_OK: + return "FWUP_OK"; + case FWUP_CSERROR: + return "FWUP_CSERROR"; + case FWUP_CRITICAL: + return "FWUP_CRITICAL"; + case FWUP_PAGE: + return "FWUP_PAGE"; + case FWUP_PAGESLICE: + return "FWUP_PAGESLICE"; + case FWUP_STARTED: + return "FWUP_STARTED"; + case FWUP_OTHER: + return "FWUP_OTHER"; + default: + return "unknown"; + } +} + + +/*! \brief Read a uint32_t from the current position in the xff +*/ +uint32_t readUint32(FwUpdate* thisPtr) +{ + uint32_t result; + uint8_t buffer[4]; + uint32_t n = thisPtr->m_readXffData(buffer, thisPtr->m_readIndex, 4); + thisPtr->m_readIndex += n; + if (n == 4) + result = (uint32_t)buffer[0] << 24 | (uint32_t)buffer[1] << 16 | (uint32_t)buffer[2] << 8 | buffer[3]; + else + { + thisPtr->m_endOfFile = 1; + result = 0; + } + return result; +} + + +/*! \brief Read a uint16_t from the current position in the xff +*/ +uint16_t readUint16(FwUpdate* thisPtr) +{ + uint32_t result; + uint8_t buffer[2]; + uint32_t n = thisPtr->m_readXffData(buffer, thisPtr->m_readIndex, 2); + thisPtr->m_readIndex += n; + if (n == 2) + result = (uint32_t)buffer[0] << 8 | buffer[1]; + else + { + thisPtr->m_endOfFile = 1; + result = 0; + } + return (uint16_t)result; +} + + +/*! \brief Read a uint8_t from the current position in the xff +*/ +uint8_t readUint8(FwUpdate* thisPtr) +{ + uint8_t result; + uint8_t buffer[1]; + uint32_t n = thisPtr->m_readXffData(buffer, thisPtr->m_readIndex, 1); + thisPtr->m_readIndex += n; + if (n == 1) + result = buffer[0]; + else + { + thisPtr->m_endOfFile = 1; + result = 0; + } + return result; +} + + +/*! \brief Read an Xff header from the current position in the xff +*/ +static void readXffHeader(FwUpdate* thisPtr) +{ + LOG("Fwu: readXffHeader()\n"); + + uint32_t globalIdTemp = readUint32(thisPtr); + if (globalIdTemp != 0) + { + thisPtr->m_xffHeader.m_globalId = globalIdTemp; + thisPtr->m_xffHeader.m_sectionSize = readUint32(thisPtr); + thisPtr->m_xffHeader.m_firmwareRevision[0] = readUint8(thisPtr); + thisPtr->m_xffHeader.m_firmwareRevision[1] = readUint8(thisPtr); + thisPtr->m_xffHeader.m_firmwareRevision[2] = readUint8(thisPtr); + thisPtr->m_xffHeader.m_xffVersion[0] = readUint8(thisPtr); + thisPtr->m_xffHeader.m_xffVersion[1] = readUint8(thisPtr); + thisPtr->m_xffHeader.m_chipId = readUint8(thisPtr); + thisPtr->m_xffHeader.m_numberOfSections = readUint8(thisPtr); + thisPtr->m_xffHeader.m_addressLength = readUint8(thisPtr); + thisPtr->m_xffHeader.m_pageSize = readUint16(thisPtr); + if (thisPtr->m_xffHeader.m_xffVersion[0] >= 2) + thisPtr->m_xffHeader.m_sliceSize = readUint16(thisPtr); + else + thisPtr->m_xffHeader.m_sliceSize = FWUP_DEFAULT_SLICE_SIZE; + } + else + { + (void) readUint32(thisPtr); // headerSize + thisPtr->m_xffHeader.m_globalId = readUint32(thisPtr); + thisPtr->m_xffHeader.m_sectionSize = readUint32(thisPtr); + thisPtr->m_xffHeader.m_firmwareRevision[0] = readUint8(thisPtr); + thisPtr->m_xffHeader.m_firmwareRevision[1] = readUint8(thisPtr); + thisPtr->m_xffHeader.m_firmwareRevision[2] = readUint8(thisPtr); + thisPtr->m_xffHeader.m_xffVersion[0] = readUint8(thisPtr); + thisPtr->m_xffHeader.m_xffVersion[1] = readUint8(thisPtr); + thisPtr->m_xffHeader.m_chipId = readUint8(thisPtr); + thisPtr->m_xffHeader.m_numberOfSections = readUint8(thisPtr); + thisPtr->m_xffHeader.m_addressLength = readUint8(thisPtr); + thisPtr->m_xffHeader.m_pageSize = readUint16(thisPtr); + thisPtr->m_xffHeader.m_sliceSize = readUint16(thisPtr); + thisPtr->m_xffHeader.m_hardwareVersion[0] = readUint8(thisPtr); + thisPtr->m_xffHeader.m_hardwareVersion[1] = readUint8(thisPtr); + if (thisPtr->m_xffHeader.m_xffVersion[0] >= 4) + { + for (int n = 0; n < 20; n++) + thisPtr->m_xffHeader.m_productCode[n] = (char) readUint8(thisPtr); + thisPtr->m_xffHeader.m_productVariant = readUint32(thisPtr); + } + } +} + + +/*! \brief Send a FWUP_READY command +*/ +static void sendReady(FwUpdate* thisPtr) +{ + Xbus_message(thisPtr->m_txBuffer, 0xFF, XMID_FIRMWARE_UPDATE, 1); + Xbus_getPointerToPayload(thisPtr->m_txBuffer)[0] = FWUP_READY; + Xbus_insertChecksum(thisPtr->m_txBuffer); + thisPtr->m_sendXbusMessage(thisPtr->m_txBuffer); +} + + +/*! \brief Send a FWUP_HEADER command +*/ +static void sendHeader(FwUpdate* thisPtr) +{ + uint32_t n; + Xbus_message(thisPtr->m_txBuffer, 0xFF, XMID_FIRMWARE_UPDATE, 1 + thisPtr->m_xffHeader.m_addressLength + 2); + uint8_t* payload = Xbus_getPointerToPayload(thisPtr->m_txBuffer); + payload[0] = FWUP_HEADER; + n = thisPtr->m_readXffData(&payload[1], thisPtr->m_readIndex, thisPtr->m_xffHeader.m_addressLength); + thisPtr->m_readIndex += n; + if (n == thisPtr->m_xffHeader.m_addressLength) + { + memcpy(&payload[1 + thisPtr->m_xffHeader.m_addressLength], (uint8_t*)&thisPtr->m_nofSlicesPerPage, 2); + Xbus_insertChecksum(thisPtr->m_txBuffer); + thisPtr->m_sendXbusMessage(thisPtr->m_txBuffer); + } + else + thisPtr->m_endOfFile = 1; +} + + +/*! \brief Send a page slice +*/ +static void sendSlice(FwUpdate* thisPtr) +{ + uint32_t n; + + Xbus_message(thisPtr->m_txBuffer, 0xFF, XMID_FIRMWARE_UPDATE, 1 + thisPtr->m_xffHeader.m_sliceSize); + uint8_t* payload = Xbus_getPointerToPayload(thisPtr->m_txBuffer); + + payload[0] = FWUP_PAGESLICE; + n = thisPtr->m_readXffData(&payload[1], thisPtr->m_readIndex, thisPtr->m_xffHeader.m_sliceSize); + thisPtr->m_readIndex += n; + if (n == thisPtr->m_xffHeader.m_sliceSize) + { + Xbus_insertChecksum(thisPtr->m_txBuffer); + thisPtr->m_sendXbusMessage(thisPtr->m_txBuffer); + } + else + thisPtr->m_endOfFile = 1; +} + + +/*! \brief Send a FWUP_OTHER command +*/ +static void sendOther(FwUpdate* thisPtr) +{ + Xbus_message(thisPtr->m_txBuffer, 0xFF, XMID_FIRMWARE_UPDATE, 2); + uint8_t* payload = Xbus_getPointerToPayload(thisPtr->m_txBuffer); + payload[0] = FWUP_OTHER; + payload[1] = thisPtr->m_xffHeader.m_chipId; + LOG("Fwu: Send FWUP_OTHER\n"); + Xbus_insertChecksum(thisPtr->m_txBuffer); + thisPtr->m_sendXbusMessage(thisPtr->m_txBuffer); +} + + +/*! \brief Send a FWUP_FINISHED command +*/ +static void sendFinished(FwUpdate* thisPtr) +{ + Xbus_message(thisPtr->m_txBuffer, 0xFF, XMID_FIRMWARE_UPDATE, 1); + uint8_t* payload = Xbus_getPointerToPayload(thisPtr->m_txBuffer); + payload[0] = FWUP_FINISHED; + LOG("Fwu: Send FWUP_FINISHED\n"); + Xbus_insertChecksum(thisPtr->m_txBuffer); + thisPtr->m_sendXbusMessage(thisPtr->m_txBuffer); +} + + +/*! \brief Enter the next section of the xff file +*/ +static void enterNewSection(FwUpdate* thisPtr) +{ + LOG("\nFwu: enterNewSection()\n"); + readXffHeader(thisPtr); + thisPtr->m_nofPages = thisPtr->m_xffHeader.m_sectionSize / ((uint32_t)thisPtr->m_xffHeader.m_pageSize + (uint32_t)thisPtr->m_xffHeader.m_addressLength); + thisPtr->m_nofSlicesPerPage = thisPtr->m_xffHeader.m_pageSize / thisPtr->m_xffHeader.m_sliceSize; + thisPtr->m_pageCounter = 0; + thisPtr->m_sliceCounter = 0; + sendOther(thisPtr); +} + + +/*! \brief Initialize a FwUpdate instance +*/ +void FwUpdate_init(FwUpdate* thisPtr) +{ + LOG("Fwu: init()\n"); + thisPtr->m_state = STATE_Idle; +} + + +/*! \brief Start a firmware update +*/ +void FwUpdate_start(FwUpdate* thisPtr) +{ + if (thisPtr->m_state == STATE_Idle) + { + LOG("Fwu: start() --> Send FWUP_READY\n"); + thisPtr->m_readIndex = 0; + thisPtr->m_endOfFile = 0; + thisPtr->m_state = STATE_Start; + sendReady(thisPtr); + } + else + { + thisPtr->m_readyHandler(FWU_Failed); + LOG("Fwu: start() failed\n"); + } +} + + +/*! \brief Handle xbus message coming from the module + \param xbusMessage The xbus message from the module to be handled +*/ +void FwUpdate_handleXbus(FwUpdate* thisPtr, uint8_t const* xbusMessage) +{ + uint8_t ack; + + if (Xbus_getMessageId(xbusMessage) != XMID_FIRMWARE_UPDATE) + { + LOG("Fwu: Got unhandled xbus message 0x%02X (ignored)\n", Xbus_getMessageId(xbusMessage)); + return; + } + + ack = Xbus_getConstPointerToPayload(xbusMessage)[0]; + + switch (thisPtr->m_state) + { + case STATE_Idle: + { + LOG("Fwu: Got %s in STATE_Idle (ignored)\n", ackToString(ack)); + break; + } + + case STATE_Start: + { + if (ack == FWUP_READY) + { + LOG("Fwu: FWUP_READY in STATE_Start --> Enter new section\n"); + enterNewSection(thisPtr); + thisPtr->m_state = STATE_WaitReady; + } + else + LOG("Fwu: Got %s in STATE_Start (ignored)\n", ackToString(ack)); + break; + } + + case STATE_WaitReady: + { + if (ack == FWUP_READY) + { + LOG("Fwu: FWUP_READY in STATE_WaitReady --> Send header\n"); + sendHeader(thisPtr); + thisPtr->m_state = STATE_WaitHeaderResult; + } + else + { + LOG("Fwu: Got %s in STATE_WaitReady --> Failed\n", ackToString(ack)); + thisPtr->m_readyHandler(FWU_Failed); + thisPtr->m_state = STATE_Idle; + } + break; + } + + case STATE_WaitHeaderResult: + { + if (ack == FWUP_READY) + { + LOG("Fwu: FWUP_READY in STATE_WaitHeaderResult --> Send first slice\n"); + sendSlice(thisPtr); + thisPtr->m_sliceCounter = 1; + thisPtr->m_state = STATE_WaitSliceReady; + } + else + { + LOG("Fwu: Got %s in STATE_WaitHeaderResult --> Failed\n", ackToString(ack)); + thisPtr->m_readyHandler(FWU_Failed); + thisPtr->m_state = STATE_Idle; + } + break; + } + + case STATE_WaitSliceReady: + { + if (ack == FWUP_READY) + { + if (thisPtr->m_sliceCounter < thisPtr->m_nofSlicesPerPage) + { + LOG("Fwu: FWUP_READY in STATE_WaitSliceReady --> Send slice %d\n", thisPtr->m_sliceCounter); + sendSlice(thisPtr); + thisPtr->m_sliceCounter++; + } + else + { + LOG("Fwu: All slices sent --> STATE_WaitPageOk\n"); + thisPtr->m_state = STATE_WaitPageOk; + } + } + else + { + LOG("Fwu: Got %s in STATE_WaitSliceReady --> Failed\n", ackToString(ack)); + thisPtr->m_readyHandler(FWU_Failed); + thisPtr->m_state = STATE_Idle; + } + break; + } + + case STATE_WaitPageOk: + { + if (ack == FWUP_OK) + { + LOG("Fwu: FWUP_OK in STATE_WaitPageOk --> STATE_WaitPageReady\n"); + thisPtr->m_state = STATE_WaitPageReady; + } + else + { + LOG("Fwu: Got %s in STATE_WaitPageOk --> Failed\n", ackToString(ack)); + thisPtr->m_readyHandler(FWU_Failed); + thisPtr->m_state = STATE_Idle; + } + break; + } + + case STATE_WaitPageReady: + { + if (ack == FWUP_READY) + { + thisPtr->m_pageCounter++; + if (thisPtr->m_nofPages != 0 && thisPtr->m_pageCounter == thisPtr->m_nofPages) + { + LOG("Fwu: All pages sent --> Enter new section\n"); + enterNewSection(thisPtr); + thisPtr->m_state = STATE_WaitReady; + } + else + { + sendHeader(thisPtr); + if (thisPtr->m_endOfFile) + { + LOG("Fwu: End of file --> Firmware update done\n"); + sendFinished(thisPtr); + thisPtr->m_readyHandler(FWU_Success); + thisPtr->m_state = STATE_Idle; + } + else + { + LOG("Fwu: FWUP_READY in STATE_WaitPageReady --> Send header\n"); + thisPtr->m_state = STATE_WaitHeaderResult; + } + } + } + else + { + LOG("Fwu: Got %s in STATE_WaitPageReady --> Failed\n", ackToString(ack)); + thisPtr->m_readyHandler(FWU_Failed); + thisPtr->m_state = STATE_Idle; + } + break; + } + + default: + break; + } +} diff --git a/lib/xspublic/xscommon/fwupdate.h b/lib/xspublic/xscommon/fwupdate.h new file mode 100644 index 0000000..b019baa --- /dev/null +++ b/lib/xspublic/xscommon/fwupdate.h @@ -0,0 +1,172 @@ + +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions, and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions, and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the names of the copyright holders nor the names of their contributors +// may be used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY +// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL +// THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT +// OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY OR +// TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.THE LAWS OF THE NETHERLANDS +// SHALL BE EXCLUSIVELY APPLICABLE AND ANY DISPUTES SHALL BE FINALLY SETTLED UNDER THE RULES +// OF ARBITRATION OF THE INTERNATIONAL CHAMBER OF COMMERCE IN THE HAGUE BY ONE OR MORE +// ARBITRATORS APPOINTED IN ACCORDANCE WITH SAID RULES. +// + + +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions, and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions, and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the names of the copyright holders nor the names of their contributors +// may be used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY +// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL +// THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT +// OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY OR +// TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.THE LAWS OF THE NETHERLANDS +// SHALL BE EXCLUSIVELY APPLICABLE AND ANY DISPUTES SHALL BE FINALLY SETTLED UNDER THE RULES +// OF ARBITRATION OF THE INTERNATIONAL CHAMBER OF COMMERCE IN THE HAGUE BY ONE OR MORE +// ARBITRATORS APPOINTED IN ACCORDANCE WITH SAID RULES. +// + +#ifndef FWUPDATE_H +#define FWUPDATE_H + +#include "xbus.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#define FWU_REQUIRED_TXBUFFER_SIZE 300 + +/*! \brief Result value +*/ +typedef enum +{ + FWU_Success, + FWU_Failed +} FWU_Result; + + +/*! \brief Definition of a xff section header +*/ +typedef struct +{ + uint32_t m_globalId; /*!< globalId*/ + uint32_t m_sectionSize; /*!< Section size*/ + uint8_t m_firmwareRevision[3]; /*!< Firmware revision*/ + uint8_t m_xffVersion[2]; /*!< xff version*/ + uint8_t m_chipId; /*!< ChipId*/ + uint8_t m_numberOfSections; /*!< Number of sections in the xff file*/ + uint8_t m_addressLength; /*!< Address length*/ + uint16_t m_pageSize; /*!< Page size*/ + uint16_t m_sliceSize; /*!< Slice size*/ + uint8_t m_hardwareVersion[2]; /*!< Hardware version (only in xff version > 2)*/ + char m_productCode[20]; /*!< Product code (only in xff version > 3)*/ + uint32_t m_productVariant; /*!< Product variant (only in xff version > 3)*/ +} XffHeader; + + +/*! \brief Internal state +*/ +typedef enum +{ + STATE_Idle, + STATE_Start, + STATE_WaitReady, + STATE_WaitHeaderResult, + STATE_WaitSliceReady, + STATE_WaitPageOk, + STATE_WaitPageReady +} FWU_State; + + + +/*! \brief FwUpdate object definition +*/ +typedef struct +{ + /* External dependencies. Host should fill in these members */ + + /*! \brief Callback function by which FwUpdate requests for xff data + \param buffer Target buffer in which the xff data should be written by the host + \param offset Offset in the xff file where reading should start + \param length Number of bytes which is requested + \returns Number of bytes which is actually written to the buffer + */ + uint32_t (*m_readXffData)(uint8_t* buffer, uint32_t offset, uint32_t length); + + /*! \brief Callback function via which FwUpdate can send xbus messages to the module + \param xbusMessage Xbus message that should be send to the module + */ + void (*m_sendXbusMessage)(uint8_t const* xbusMessage); + + /*! \brief Callback function by which FwUpdate notifies the host that a firmware update has finished + \param result FWU_Success or FWU_Failed + */ + void (*m_readyHandler)(FWU_Result result); + + /*! \brief Memory needed by the FwUpdate. Host must allocate a block of memory + of size FWU_REQUIRED_TXBUFFER_SIZE + */ + uint8_t* m_txBuffer; + + /* State variables for internal use (the user must not touch these) */ + FWU_State m_state; /*!< Internal state member of FwUpdate*/ + XffHeader m_xffHeader; /*!< Internal state member of FwUpdate*/ + uint32_t m_nofPages; /*!< Internal state member of FwUpdate*/ + uint32_t m_nofSlicesPerPage; /*!< Internal state member of FwUpdate*/ + uint32_t m_pageCounter; /*!< Internal state member of FwUpdate*/ + uint32_t m_sliceCounter; /*!< Internal state member of FwUpdate*/ + uint32_t m_readIndex; /*!< Internal state member of FwUpdate*/ + uint8_t m_endOfFile; /*!< Internal state member of FwUpdate*/ +} FwUpdate; + + +void FwUpdate_init(FwUpdate* thisPtr); + +void FwUpdate_start(FwUpdate* thisPtr); + +void FwUpdate_handleXbus(FwUpdate* thisPtr, uint8_t const* xbusMessage); + + +#ifdef __cplusplus +} +#endif /* extern "C" */ + + +#endif diff --git a/lib/xspublic/xscommon/journalexception.cpp b/lib/xspublic/xscommon/journalexception.cpp index 397eab8..df30511 100644 --- a/lib/xspublic/xscommon/journalexception.cpp +++ b/lib/xspublic/xscommon/journalexception.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -66,12 +66,13 @@ #include "journalexception.h" /*! \brief A helper class that combines the StackWalker output into a std::string */ -class JournalExceptionStackWalker : public StackWalker { +class JournalExceptionStackWalker : public StackWalker +{ public: JournalExceptionStackWalker(JournalException* exc) : StackWalker() , m_exception(exc) - {} + {} JournalException* m_exception; @@ -109,9 +110,9 @@ const std::string& JournalException::stack() const } #ifdef XSENS_DEBUG -#define GOTODEBDEF true + #define GOTODEBDEF true #else -#define GOTODEBDEF false + #define GOTODEBDEF false #endif /*! When set to true, any caught exception at the C level will be passed on upward (ie to the debugger) diff --git a/lib/xspublic/xscommon/journalexception.h b/lib/xspublic/xscommon/journalexception.h index 4087047..255283a 100644 --- a/lib/xspublic/xscommon/journalexception.h +++ b/lib/xspublic/xscommon/journalexception.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -71,16 +71,17 @@ #include "journaller.h" #ifndef XSENS_WINDOWS -#include -#include + #include + #include #endif -class JournalException : public XsException { +class JournalException : public XsException +{ public: JournalException(std::string const& message); ~JournalException() throw() {} - const char *msg() const; + const char* msg() const; const std::string& stack() const; protected: @@ -99,64 +100,64 @@ extern bool gOnExceptionGotoDebugger; #endif #if JLDEF_BUILD > JLL_FATAL - #define JOURNALCRASHES_SIGNAL_FUNCTIONS - #define JOURNALCRASHES_BEGIN(journal) - #define JOURNALCRASHES_END(journal) +#define JOURNALCRASHES_SIGNAL_FUNCTIONS +#define JOURNALCRASHES_BEGIN(journal) +#define JOURNALCRASHES_END(journal) #else - #ifdef XSENS_WINDOWS - #define JOURNALCRASHES_SIGNAL_FUNCTIONS - #define JOURNALCRASHES_BEGIN(journal) \ - __try { - - #define JOURNALCRASHES_END(journal) \ - } __except (journallerExceptionFilter(GetExceptionInformation(), journal)) {} - - #else - #define JOURNALCRASHES_SIGNAL_FUNCTIONS \ - Journaller *gSCJ = 0;\ - extern "C" void signal_handler(int signal, siginfo_t *, void *)\ - {\ - if (gSCJ)\ - {\ - JLFATAL_NODEC(gSCJ, strsignal(signal));\ - gSCJ->writeCallstack(JLL_Fatal);\ - }\ - _exit(-1);\ - } - - #define JOURNALCRASHES_BEGIN(journal) \ - do { \ - gSCJ = journal;\ - struct sigaction act;\ - memset(&act, 0, sizeof(act));\ - act.sa_flags = SA_SIGINFO;\ - act.sa_sigaction = &signal_handler;\ - sigaction(SIGSEGV, &act, NULL);\ - sigaction(SIGILL, &act, NULL);\ - sigaction(SIGABRT, &act, NULL);\ - sigaction(SIGFPE, &act, NULL);\ - } while (0); - #define JOURNALCRASHES_END(journal) - #endif +#ifdef XSENS_WINDOWS +#define JOURNALCRASHES_SIGNAL_FUNCTIONS +#define JOURNALCRASHES_BEGIN(journal) \ + __try { + +#define JOURNALCRASHES_END(journal) \ + } __except (journallerExceptionFilter(GetExceptionInformation(), journal)) {} + +#else +#define JOURNALCRASHES_SIGNAL_FUNCTIONS \ + Journaller *gSCJ = 0;\ + extern "C" void signal_handler(int signal, siginfo_t *, void *)\ + {\ + if (gSCJ)\ + {\ + JLFATAL_NODEC(gSCJ, strsignal(signal));\ + gSCJ->writeCallstack(JLL_Fatal);\ + }\ + _exit(-1);\ + } + +#define JOURNALCRASHES_BEGIN(journal) \ + do { \ + gSCJ = journal;\ + struct sigaction act;\ + memset(&act, 0, sizeof(act));\ + act.sa_flags = SA_SIGINFO;\ + act.sa_sigaction = &signal_handler;\ + sigaction(SIGSEGV, &act, NULL);\ + sigaction(SIGILL, &act, NULL);\ + sigaction(SIGABRT, &act, NULL);\ + sigaction(SIGFPE, &act, NULL);\ + } while (0); +#define JOURNALCRASHES_END(journal) +#endif #endif #if JLDEF_BUILD > JLL_ERROR - #define JOURNALEXCEPTIONS_BEGIN(journal) try { - #define JOURNALEXCEPTIONS_END_NOTHROW(journal) } catch(...) { } - #define JOURNALEXCEPTIONS_END_RETHROW(journal) } catch(...) { throw; } +#define JOURNALEXCEPTIONS_BEGIN(journal) try { +#define JOURNALEXCEPTIONS_END_NOTHROW(journal) } catch(...) { } +#define JOURNALEXCEPTIONS_END_RETHROW(journal) } catch(...) { throw; } #else - #define JOURNALEXCEPTIONS_BEGIN(journal) \ - try { - - #define JOURNALEXCEPTIONS_END_NOTHROW(journal) \ - } catch (JournalException& e) { JLERROR(journal, e.msg()); JLERROR_NODEC(journal, e.stack()); }\ - catch (XsException& e) { JLERROR(journal, e.what()); }\ - catch (std::exception& e) { JLERROR(journal, e.what()); } - - #define JOURNALEXCEPTIONS_END_RETHROW(journal) \ - } catch (JournalException& e) { JLERROR(journal, e.msg()); JLERROR_NODEC(journal, e.stack()); throw; }\ - catch (XsException& e) { JLERROR(journal, e.what()); throw; }\ - catch (std::exception& e) { JLERROR(journal, e.what()); throw; } +#define JOURNALEXCEPTIONS_BEGIN(journal) \ + try { + +#define JOURNALEXCEPTIONS_END_NOTHROW(journal) \ + } catch (JournalException& e) { JLERROR(journal, e.msg()); JLERROR_NODEC(journal, e.stack()); }\ + catch (XsException& e) { JLERROR(journal, e.what()); }\ + catch (std::exception& e) { JLERROR(journal, e.what()); } + +#define JOURNALEXCEPTIONS_END_RETHROW(journal) \ + } catch (JournalException& e) { JLERROR(journal, e.msg()); JLERROR_NODEC(journal, e.stack()); throw; }\ + catch (XsException& e) { JLERROR(journal, e.what()); throw; }\ + catch (std::exception& e) { JLERROR(journal, e.what()); throw; } #endif #endif diff --git a/lib/xspublic/xscommon/journalfile.cpp b/lib/xspublic/xscommon/journalfile.cpp index 5732486..d69ad1a 100644 --- a/lib/xspublic/xscommon/journalfile.cpp +++ b/lib/xspublic/xscommon/journalfile.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -64,7 +64,7 @@ #include "journalfile.h" #ifdef _MSC_VER -#include + #include #endif /*! \class JournalFile @@ -108,7 +108,7 @@ JournalFile::~JournalFile() flush(); m_file.close(); } - catch(...) + catch (...) { } } diff --git a/lib/xspublic/xscommon/journalfile.h b/lib/xspublic/xscommon/journalfile.h index 05a6683..6cd8111 100644 --- a/lib/xspublic/xscommon/journalfile.h +++ b/lib/xspublic/xscommon/journalfile.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -70,7 +70,8 @@ #include #include -class JournalFile { +class JournalFile +{ public: JournalFile(const XsString& name, bool purge); ~JournalFile(); @@ -84,7 +85,10 @@ class JournalFile { XsString filename() const; JournalFile& operator<<(std::string const& msg); /*! \brief Return the internal XsFile object, only to be used directly when absolutely necessary */ - XsFile& xsFile() { return m_file; } + XsFile& xsFile() + { + return m_file; + } private: volatile std::atomic_int m_refCount; diff --git a/lib/xspublic/xscommon/journaller.cpp b/lib/xspublic/xscommon/journaller.cpp index 3ce5272..e022c59 100644 --- a/lib/xspublic/xscommon/journaller.cpp +++ b/lib/xspublic/xscommon/journaller.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -77,13 +77,14 @@ #include #ifndef XSENS_NO_AUTOLIB -#pragma comment(lib, "psapi.lib") + #pragma comment(lib, "psapi.lib") #endif //! \brief The optional additional logger AbstractAdditionalLogger* Journaller::m_additionalLogger = nullptr; -inline int threadId() { +inline int threadId() +{ #if JOURNALLER_WITH_THREAD_SUPPORT && !defined(__APPLE__) return (int) xsGetCurrentThreadId(); #else @@ -102,12 +103,12 @@ inline int threadId() { */ #if __GNUC__ >= 6 -// Ignore warning: nonnull argument ‘this’ compared to NULL [-Wnonnull-compare] -// This happens for the JL macro's which here have a this argument, which is always nonnull -// In the macros we compare this to NULL, which is now a warning in GCC6 -// On other locations, we do not use this for the macro, hence the warning does not show there -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wnonnull-compare" + // Ignore warning: nonnull argument ‘this’ compared to NULL [-Wnonnull-compare] + // This happens for the JL macro's which here have a this argument, which is always nonnull + // In the macros we compare this to NULL, which is now a warning in GCC6 + // On other locations, we do not use this for the macro, hence the warning does not show there + #pragma GCC diagnostic push + #pragma GCC diagnostic ignored "-Wnonnull-compare" #endif /*! \brief Constructor @@ -147,7 +148,7 @@ void Journaller::init(XsString const& pathfile, bool purge) need a tag set. \param fn The file name string */ -std::string Journaller::tagFromFilename(const std::string &fn) +std::string Journaller::tagFromFilename(const std::string& fn) { const size_t b = fn.find_last_of("/") + 1; const size_t l = fn.find_first_of("."); @@ -174,22 +175,22 @@ void Journaller::setAdditionalLogger(AbstractAdditionalLogger* additionallogger) } /*! \brief A list of strings representing the different log levels, use JournalLogLevel to index */ -static const char* gLogLevelString[] = { - "[TRACE] ", - "[DEBUG] ", - "[ALERT] ", - "[ERROR] ", - "[FATAL] ", - "[WRITE] "}; +static const char* gLogLevelString[] = +{ + "[TRACE] ", + "[DEBUG] ", + "[ALERT] ", + "[ERROR] ", + "[FATAL] ", + "[WRITE] " +}; /*! \brief Write a header for the log file including some meta-data about the journaller */ void Journaller::writeFileHeader(const std::string& appName) { m_appName = appName; - XsTimeStamp now = XsTimeStamp::now(); - JLWRITE(this, "Journaller logging to " << m_file->filename() << (appName.empty() ? XsString() : XsString(" for ") + appName) << " on " << now.toString()); - //JLWRITE(this, "Current log level is " << gLogLevelString[m_level]); + JLWRITE(this, "Journaller logging to " << m_file->filename() << (appName.empty() ? XsString() : XsString(" for ") + appName) << " on " << XsTimeStamp::now().toString()); } /*! \brief Write a log message to the file if \a level is at least equal to the current log level @@ -366,7 +367,7 @@ const XsString Journaller::filename() const /*! \brief Set a tag to be added before the log level tag in each log file */ -void Journaller::setTag(const std::string &tag) +void Journaller::setTag(const std::string& tag) { m_tag = tag; } @@ -401,6 +402,13 @@ void Journaller::moveLogFile(const XsString& pathfile, bool purge, bool eraseOld JournalFile* newFile = new JournalFile(pathfile, purge); + if (!newFile->xsFile().isOpen()) + { + JLERROR(this, "Could not switch to new log location " << pathfile); + delete newFile; + return; + } + XsString oldFileName; std::unique_ptr buffer; XsFilePos sz = 0; @@ -504,5 +512,5 @@ void jlTerminate(Journaller** gj) } #if __GNUC__ >= 6 -#pragma GCC diagnostic pop + #pragma GCC diagnostic pop #endif diff --git a/lib/xspublic/xscommon/journaller.h b/lib/xspublic/xscommon/journaller.h index eb3beef..ddb7c22 100644 --- a/lib/xspublic/xscommon/journaller.h +++ b/lib/xspublic/xscommon/journaller.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -69,7 +69,7 @@ #include "abstractadditionallogger.h" #include #ifndef JL_NOTEMPLATE -#include + #include #endif #include #include @@ -88,23 +88,35 @@ class Journaller void setLogLevel(JournalLogLevel level, bool writeLogLine = true); //! \returns The log level for logging to file - inline JournalLogLevel logLevel() const { return m_level; } + inline JournalLogLevel logLevel() const + { + return m_level; + } /*! \brief Compares a log/debug level with a set one \param level The log level to compare with \returns True if a set log/debug level is equal or higher than the compared one */ - inline bool logLevel(JournalLogLevel level) const { return level >= m_level || level >= m_debugLevel; } + inline bool logLevel(JournalLogLevel level) const + { + return level >= m_level || level >= m_debugLevel; + } void setDebugLevel(JournalLogLevel level, bool writeLogLine = true); //! \returns The log level for logging to debug output - inline JournalLogLevel debugLevel() const { return m_debugLevel; } + inline JournalLogLevel debugLevel() const + { + return m_debugLevel; + } void setFlushLevel(JournalLogLevel level, bool writeLogLine = true); //! \returns The flush level - inline JournalLogLevel flushLevel() const { return m_flushLevel; } + inline JournalLogLevel flushLevel() const + { + return m_flushLevel; + } void writeFileHeader(const std::string& appName); void setUseDateTime(bool yes); @@ -119,18 +131,24 @@ class Journaller const XsString filename() const; - void setTag(const std::string &tag); + void setTag(const std::string& tag); std::string tag() const; static void setAdditionalLogger(AbstractAdditionalLogger* additionalLogger); //! \returns True if it has an additional logger - inline static bool hasAdditionalLogger() { return m_additionalLogger != nullptr; } + inline static bool hasAdditionalLogger() + { + return m_additionalLogger != nullptr; + } //! \returns The additional logger - inline static AbstractAdditionalLogger* additionalLogger() { return m_additionalLogger; } + inline static AbstractAdditionalLogger* additionalLogger() + { + return m_additionalLogger; + } - static std::string tagFromFilename(const std::string &fn); + static std::string tagFromFilename(const std::string& fn); void moveLogFile(const XsString& pathfile, bool purge = true, bool eraseOld = true); void moveLogs(Journaller* target, bool eraseOld = true); @@ -158,11 +176,11 @@ class Journaller #if 1 && (defined(MSC_VER) || 1) // add exceptions to compilers here that do not (yet) support constexpr. These will fall back to the full path. If the first 1 is set to 0, no path stripping will be done in any case. inline static constexpr char const* jlStrippedPath(char const* a, char const* b) { - return (a[0] ? (a[0] == '/' || a[0] == '\\' ? jlStrippedPath(a+1, a+1) : jlStrippedPath(a+1, b) ) : b); + return (a[0] ? (a[0] == '/' || a[0] == '\\' ? jlStrippedPath(a + 1, a + 1) : jlStrippedPath(a + 1, b)) : b); } inline static constexpr char const* jlStrippedPathFile(char const* a) { - return jlStrippedPath(a,a); + return jlStrippedPath(a, a); } #define STRIPPEDFILE jlStrippedPathFile(__FILE__) #else @@ -170,9 +188,9 @@ inline static constexpr char const* jlStrippedPathFile(char const* a) #endif #if !defined(JLNOLINEINFO) -#define JLGENERIC_LINEINFO STRIPPEDFILE << "(" << __LINE__ << ") " + #define JLGENERIC_LINEINFO STRIPPEDFILE << "(" << __LINE__ << ") " #else -#define JLGENERIC_LINEINFO "" + #define JLGENERIC_LINEINFO "" #endif #define JLGENERIC(journal, level, msg)\ @@ -209,82 +227,87 @@ inline static constexpr char const* jlStrippedPathFile(char const* a) #if !defined(JLDEBUG) -#if JLDEF_BUILD > JLL_TRACE -#define JLTRACE(...) ((void)0) -#define JLTRACE_NODEC(...) ((void)0) -#else -#define JLTRACE(journal, msg) JLGENERIC(journal, JLL_Trace, msg) -#define JLTRACE_NODEC(journal, msg) JLGENERIC_NODEC(journal, JLL_Trace, msg) -#endif + #if JLDEF_BUILD > JLL_TRACE + #define JLTRACE(...) ((void)0) + #define JLTRACE_NODEC(...) ((void)0) + #else + #define JLTRACE(journal, msg) JLGENERIC(journal, JLL_Trace, msg) + #define JLTRACE_NODEC(journal, msg) JLGENERIC_NODEC(journal, JLL_Trace, msg) + #endif + + #if JLDEF_BUILD > JLL_DEBUG + #define JLDEBUG(...) ((void)0) + #define JLDEBUG_NODEC(...) ((void)0) + #else + #define JLDEBUG(journal, msg) JLGENERIC(journal, JLL_Debug, msg) + #define JLDEBUG_NODEC(journal, msg) JLGENERIC_NODEC(journal, JLL_Debug, msg) + #endif + + #if JLDEF_BUILD > JLL_ALERT + #define JLALERT(...) ((void)0) + #define JLALERT_NODEC(...) ((void)0) + #else + #define JLALERT(journal, msg) JLGENERIC(journal, JLL_Alert, msg) + #define JLALERT_NODEC(journal, msg) JLGENERIC_NODEC(journal, JLL_Alert, msg) + #endif + + #if JLDEF_BUILD > JLL_ERROR + #define JLERROR(...) ((void)0) + #define JLERROR_NODEC(...) ((void)0) + #else + #define JLERROR(journal, msg) JLGENERIC(journal, JLL_Error, msg) + #define JLERROR_NODEC(journal, msg) JLGENERIC_NODEC(journal, JLL_Error, msg) + #endif + + #if JLDEF_BUILD > JLL_FATAL + #define JLFATAL(...) ((void)0) + #define JLFATAL_NODEC(...) ((void)0) + #else + #define JLFATAL(journal, msg) JLGENERIC(journal, JLL_Fatal, msg) + #define JLFATAL_NODEC(journal, msg) JLGENERIC_NODEC(journal, JLL_Fatal, msg) + #endif + + #if JLDEF_BUILD > JLL_WRITE + #define JLWRITE(...) ((void)0) + #define JLWRITE_NODEC(...) ((void)0) + #else + #define JLWRITE(journal, msg) JLGENERIC(journal, JLL_Write, msg) + #define JLWRITE_NODEC(journal, msg) JLGENERIC_NODEC(journal, JLL_Write, msg) + #endif + + // some convenience macros, since we almost always use a global gJournal Journaller + #define JLTRACEG(msg) JLTRACE(gJournal, msg) + #define JLDEBUGG(msg) JLDEBUG(gJournal, msg) + #define JLALERTG(msg) JLALERT(gJournal, msg) + #define JLERRORG(msg) JLERROR(gJournal, msg) + #define JLFATALG(msg) JLFATAL(gJournal, msg) + #define JLWRITEG(msg) JLWRITE(gJournal, msg) + + // these can be used to log the final result of a value when leaving the function. + // use JLWRITEFINALG(myvar); or JLDEBUGFINALG(myvar); at the start of your function + #define JLFINALNAME(a) #a ": " + #define JLFINALVALUE(journal, level, a) JournalValueJanitor jlFinalValue ## a(journal, a, [](char const* fi, char const* fu, char const* va) { std::stringstream os; os << fi << " " << fu << " exit " << va; return os.str(); }(__FILE__, __FUNCTION__, JLFINALNAME(a)), level, true) + #define JLWRITEFINAL(journal, a) JLFINALVALUE(journal, JLL_Write, a) + #define JLWRITEFINALG(a) JLWRITEFINAL(gJournal, a) + #define JLDEBUGFINAL(journal, a) JLFINALVALUE(journal, JLL_Debug, a) + #define JLDEBUGFINALG(a) JLDEBUGFINAL(gJournal, a) -#if JLDEF_BUILD > JLL_DEBUG -#define JLDEBUG(...) ((void)0) -#define JLDEBUG_NODEC(...) ((void)0) -#else -#define JLDEBUG(journal, msg) JLGENERIC(journal, JLL_Debug, msg) -#define JLDEBUG_NODEC(journal, msg) JLGENERIC_NODEC(journal, JLL_Debug, msg) #endif -#if JLDEF_BUILD > JLL_ALERT -#define JLALERT(...) ((void)0) -#define JLALERT_NODEC(...) ((void)0) +#ifndef _lint //pclint defines this + #define JLIF(journal, level, todo) do { if (journal && journal->logLevel(level)) { todo; } } while(0) #else -#define JLALERT(journal, msg) JLGENERIC(journal, JLL_Alert, msg) -#define JLALERT_NODEC(journal, msg) JLGENERIC_NODEC(journal, JLL_Alert, msg) + #define JLIF(...) ((void)0) #endif -#if JLDEF_BUILD > JLL_ERROR -#define JLERROR(...) ((void)0) -#define JLERROR_NODEC(...) ((void)0) -#else -#define JLERROR(journal, msg) JLGENERIC(journal, JLL_Error, msg) -#define JLERROR_NODEC(journal, msg) JLGENERIC_NODEC(journal, JLL_Error, msg) -#endif - -#if JLDEF_BUILD > JLL_FATAL -#define JLFATAL(...) ((void)0) -#define JLFATAL_NODEC(...) ((void)0) -#else -#define JLFATAL(journal, msg) JLGENERIC(journal, JLL_Fatal, msg) -#define JLFATAL_NODEC(journal, msg) JLGENERIC_NODEC(journal, JLL_Fatal, msg) -#endif - -#if JLDEF_BUILD > JLL_WRITE -#define JLWRITE(...) ((void)0) -#define JLWRITE_NODEC(...) ((void)0) -#else -#define JLWRITE(journal, msg) JLGENERIC(journal, JLL_Write, msg) -#define JLWRITE_NODEC(journal, msg) JLGENERIC_NODEC(journal, JLL_Write, msg) -#endif - -// some convenience macros, since we almost always use a global gJournal Journaller -#define JLTRACEG(msg) JLTRACE(gJournal, msg) -#define JLDEBUGG(msg) JLDEBUG(gJournal, msg) -#define JLALERTG(msg) JLALERT(gJournal, msg) -#define JLERRORG(msg) JLERROR(gJournal, msg) -#define JLFATALG(msg) JLFATAL(gJournal, msg) -#define JLWRITEG(msg) JLWRITE(gJournal, msg) - -// these can be used to log the final result of a value when leaving the function. -// use JLWRITEFINALG(myvar); or JLDEBUGFINALG(myvar); at the start of your function -#define JLFINALNAME(a) #a ": " -#define JLFINALVALUE(journal, level, a) JournalValueJanitor jlFinalValue ## a(journal, a, [](char const* fi, char const* fu, char const* va) { std::stringstream os; os << fi << " " << fu << " exit " << va; return os.str(); }(__FILE__, __FUNCTION__, JLFINALNAME(a)), level, true) -#define JLWRITEFINAL(journal, a) JLFINALVALUE(journal, JLL_Write, a) -#define JLWRITEFINALG(a) JLWRITEFINAL(gJournal, a) -#define JLDEBUGFINAL(journal, a) JLFINALVALUE(journal, JLL_Debug, a) -#define JLDEBUGFINALG(a) JLDEBUGFINAL(gJournal, a) - -#endif - -#define JLIF(journal, level, todo) do { if(journal && journal->logLevel(level)) { todo; } } while(0) - #ifndef JL_NOTEMPLATE /*! \class JlHexLogger \brief A support class of journaller that is used for the logging of hex values */ template -class JlHexLogger { +class JlHexLogger +{ public: T m_value; //!< A hex value @@ -329,40 +352,46 @@ template <> std::ostream& operator << (std::ostream& os, JlHexLogger const #define JLENUMCASE(a) JLCASE(dbg, a) #define JLENUMCASE2(a, b) JLCASE2(dbg, a, b) #define JLENUMEXPFTR(...) JLDEFAULTCASE(dbg) } __VA_ARGS__ return dbg; } -/*! Use this macro to define enum expansion to a text stream. supply the type as parameter \a E and all +/*! Use this macro to define enum expansion to a text stream, items will show up as "EnumName (decimal value)". supply the type as parameter \a E and all enum values you want to expand as a sequence of JLENUMCASE(item) (no commas) */ #define JLENUMEXPANDER(E, items) JLENUMEXPHDR(E) items JLENUMEXPFTR() -/*! Use this macro to define enum expansion to a text stream. supply the type as parameter \a E and all +/*! Use this macro to define enum expansion to a text stream, items will show up as "EnumName (hexadecimal value)". supply the type as parameter \a E and all enum values you want to expand as a sequence of JLENUMCASE(item) (no commas) */ #define JLENUMEXPANDERHEX(E, items) JLENUMEXPHDR(E, dbg << std::hex << std::uppercase;) items JLENUMEXPFTR(dbg << std::dec << std::nouppercase;) +/*! Use this macro to define enum bit field expansion to a text stream, items will show up as "full hex=EnumName1(hex) | EnumName2(hex)". supply the type as parameter \a E and all + enum values you want to expand as a sequence of JLENUMCASEBITS(item) (no commas) */ +#define JLENUMEXPANDERBITS(E, items) /*! \brief Translate \a e into a text representation */ JLENUMEXPPROTO(E) { bool first = true; dbg << std::hex << std::uppercase << static_cast(e) << "="; items if (first) dbg << ""; dbg << std::dec << std::nouppercase; return dbg; } +#define JLENUMCASEBITS(a) if ((static_cast(e) & static_cast(a)) == static_cast(a)) { if (first) first = false; else dbg << " | "; dbg << #a << "(" << static_cast(a) << ")"; } +#define JLENUMCASEBITSNONE(a) if (first) { dbg << #a << "(0)"; first = false; } + #define JLQTDEBUGHANDLER \ -QtMessageHandler gOldQtMessageHandler = nullptr; \ -Journaller* qtJournal = nullptr; \ -QtMessageHandler qtChecker = nullptr; \ -void jlQtMessageHandler(QtMsgType type, const QMessageLogContext &context, const QString &msg) {\ - if (qtChecker) qtChecker(type, context, msg); \ - if (!qtJournal) return;\ - switch (type) {\ - case QtDebugMsg:\ - JLDEBUG(qtJournal, (const char *)msg.toLatin1());\ - break;\ - case QtWarningMsg:\ - JLALERT(qtJournal, (const char *)msg.toLatin1());\ - break;\ - case QtCriticalMsg:\ - JLERROR(qtJournal, (const char *)msg.toLatin1());\ - break;\ - case QtFatalMsg:\ - JLFATAL(qtJournal, (const char *)msg.toLatin1());\ - abort();\ - default:\ - break;\ - }\ - if (gOldQtMessageHandler)\ - gOldQtMessageHandler(type,context,msg);\ -} + QtMessageHandler gOldQtMessageHandler = nullptr; \ + Journaller* qtJournal = nullptr; \ + QtMessageHandler qtChecker = nullptr; \ + void jlQtMessageHandler(QtMsgType type, const QMessageLogContext &context, const QString &msg) {\ + if (qtChecker) qtChecker(type, context, msg); \ + if (!qtJournal) return;\ + switch (type) {\ + case QtDebugMsg:\ + JLDEBUG(qtJournal, (const char *)msg.toLatin1());\ + break;\ + case QtWarningMsg:\ + JLALERT(qtJournal, (const char *)msg.toLatin1());\ + break;\ + case QtCriticalMsg:\ + JLERROR(qtJournal, (const char *)msg.toLatin1());\ + break;\ + case QtFatalMsg:\ + JLFATAL(qtJournal, (const char *)msg.toLatin1());\ + abort();\ + default:\ + break;\ + }\ + if (gOldQtMessageHandler)\ + gOldQtMessageHandler(type,context,msg);\ + } #define JLINSTALLQTDEBUGHANDLER(journal) \ qtJournal = journal;\ @@ -383,7 +412,8 @@ void jlQtMessageHandler(QtMsgType type, const QMessageLogContext &context, const be defined. */ template -class JournalValueJanitor { +class JournalValueJanitor +{ private: const JournalValueJanitor& operator = (const JournalValueJanitor&); diff --git a/lib/xspublic/xscommon/journalloglevel.h b/lib/xspublic/xscommon/journalloglevel.h index 04f02c9..fb998c1 100644 --- a/lib/xspublic/xscommon/journalloglevel.h +++ b/lib/xspublic/xscommon/journalloglevel.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -85,21 +85,24 @@ #define JLL_DISABLE 6 #endif -enum JournalLogLevel { - JLL_Trace = JLL_TRACE //!< log all messages, including function entry/exit +enum JournalLogLevel +{ + JLL_Trace = JLL_TRACE //!< log all messages, including function entry/exit , JLL_Debug = JLL_DEBUG //!< log all messages, except function entry/exit (trace) , JLL_Alert = JLL_ALERT //!< only log fatal, error and alert messages , JLL_Error = JLL_ERROR //!< only log fatal and error messages , JLL_Fatal = JLL_FATAL //!< only log fatal messages , JLL_Write = JLL_WRITE //!< only log 'write' messages - , JLL_Diable= JLL_DISABLE //!< don't log any messages + , JLL_Disable = JLL_DISABLE //!< don't log any messages }; #ifdef NO_JOURNALLER - #ifdef JLDEF_BUILD - #undef JLDEF_BUILD - #endif + #undef JLDEF_BUILD + #undef JLDEF_FILE + #undef JLDEF_DEBUGGER #define JLDEF_BUILD JLL_DISABLE + #define JLDEF_FILE JLL_Disable + #define JLDEF_DEBUGGER JLL_Disable #endif #ifdef XSENS_DEBUG diff --git a/lib/xspublic/xscommon/journalstackwalker.h b/lib/xspublic/xscommon/journalstackwalker.h index 59312c9..121ed6e 100644 --- a/lib/xspublic/xscommon/journalstackwalker.h +++ b/lib/xspublic/xscommon/journalstackwalker.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -66,9 +66,9 @@ #define JOURNALSTACKWALKER_H #ifdef _MSC_VER -#include "stackwalker.h" + #include "stackwalker.h" #else -#include "stackwalker_linux.h" + #include "stackwalker_linux.h" #endif #include "journaller.h" diff --git a/lib/xspublic/xscommon/journalthreader.cpp b/lib/xspublic/xscommon/journalthreader.cpp index ddcf234..5ba2c8a 100644 --- a/lib/xspublic/xscommon/journalthreader.cpp +++ b/lib/xspublic/xscommon/journalthreader.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -66,7 +66,7 @@ #include "journalfile.h" #ifndef XSENS_WINDOWS -static void OutputDebugStringA(const char *msg) +static void OutputDebugStringA(const char* msg) { (void)fprintf(stderr, "%s", msg); } @@ -92,7 +92,7 @@ JournalThreader::~JournalThreader() */ void JournalThreader::flushAll(JournalFile* file) { - while(true) + while (true) { ThreadLine& line = nextLine(); if (line.m_line.empty()) diff --git a/lib/xspublic/xscommon/journalthreader.h b/lib/xspublic/xscommon/journalthreader.h index 156acec..0db7e9c 100644 --- a/lib/xspublic/xscommon/journalthreader.h +++ b/lib/xspublic/xscommon/journalthreader.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -69,14 +69,15 @@ #include #include "journalloglevel.h" #if JOURNALLER_WITH_THREAD_SUPPORT -#include -#include "xsens_mutex.h" + #include + #include "xsens_mutex.h" #endif class FileInfo; class JournalFile; -class JournalThreader { +class JournalThreader +{ public: JournalThreader(); ~JournalThreader(); @@ -91,7 +92,8 @@ class JournalThreader { private: /*! \brief Storage for logging queue of a specific thread */ - class ThreadLine { + class ThreadLine + { public: std::string m_line; //!< The contained text for this log line JournalLogLevel m_level; //!< The level of this log line diff --git a/lib/xspublic/xscommon/stackdumper.cpp b/lib/xspublic/xscommon/stackdumper.cpp index 1c7a3ac..e7114d4 100644 --- a/lib/xspublic/xscommon/stackdumper.cpp +++ b/lib/xspublic/xscommon/stackdumper.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -65,15 +65,16 @@ #include "stackdumper.h" #ifdef _MSC_VER -#include "stackwalker.h" + #include "stackwalker.h" #else -#include "stackwalker_linux.h" + #include "stackwalker_linux.h" #endif /*! \class DumpStackWalker \brief Helper class for the \ref getStackDump() function */ -class DumpStackWalker : public StackWalker { +class DumpStackWalker : public StackWalker +{ public: //! Constructor DumpStackWalker(bool includeModules) diff --git a/lib/xspublic/xscommon/stackdumper.h b/lib/xspublic/xscommon/stackdumper.h index 06a3c45..3b200c3 100644 --- a/lib/xspublic/xscommon/stackdumper.h +++ b/lib/xspublic/xscommon/stackdumper.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xscommon/stackwalker.cpp b/lib/xspublic/xscommon/stackwalker.cpp index 98b057a..acf46a0 100644 --- a/lib/xspublic/xscommon/stackwalker.cpp +++ b/lib/xspublic/xscommon/stackwalker.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -63,29 +63,29 @@ // /********************************************************************** - * - * StackWalker.cpp - * - * - * History: - * 2005-07-27 v1 - First public release on http://www.codeproject.com/ - * http://www.codeproject.com/threads/StackWalker.asp - * 2005-07-28 v2 - Changed the params of the constructor and ShowCallstack - * (to simplify the usage) - * 2005-08-01 v3 - Changed to use 'CONTEXT_FULL' instead of CONTEXT_ALL - * (should also be enough) - * - Changed to compile correctly with the PSDK of VC7.0 - * (GetFileVersionInfoSizeA and GetFileVersionInfoA is wrongly defined: - * it uses LPSTR instead of LPCSTR as first paremeter) - * - Added declarations to support VC5/6 without using 'dbghelp.h' - * - Added a 'pUserData' member to the ShowCallstack function and the - * PReadProcessMemoryRoutine declaration (to pass some user-defined data, - * which can be used in the readMemoryFunction-callback) - * 2005-08-02 v4 - OnSymInit now also outputs the OS-Version by default - * - Added example for doing an exception-callstack-walking in main.cpp - * (thanks to owillebo: http://www.codeproject.com/script/profile/whos_who.asp?id=536268) - * 2005-08-05 v5 - Removed most Lint (http://www.gimpel.com/) errors... thanks to Okko Willeboordse! - * + + StackWalker.cpp + + + History: + 2005-07-27 v1 - First public release on http://www.codeproject.com/ + http://www.codeproject.com/threads/StackWalker.asp + 2005-07-28 v2 - Changed the params of the constructor and ShowCallstack + (to simplify the usage) + 2005-08-01 v3 - Changed to use 'CONTEXT_FULL' instead of CONTEXT_ALL + (should also be enough) + - Changed to compile correctly with the PSDK of VC7.0 + (GetFileVersionInfoSizeA and GetFileVersionInfoA is wrongly defined: + it uses LPSTR instead of LPCSTR as first paremeter) + - Added declarations to support VC5/6 without using 'dbghelp.h' + - Added a 'pUserData' member to the ShowCallstack function and the + PReadProcessMemoryRoutine declaration (to pass some user-defined data, + which can be used in the readMemoryFunction-callback) + 2005-08-02 v4 - OnSymInit now also outputs the OS-Version by default + - Added example for doing an exception-callstack-walking in main.cpp + (thanks to owillebo: http://www.codeproject.com/script/profile/whos_who.asp?id=536268) + 2005-08-05 v5 - Removed most Lint (http://www.gimpel.com/) errors... thanks to Okko Willeboordse! + **********************************************************************/ #ifndef __GNUC__ @@ -196,29 +196,29 @@ typedef struct _tagSTACKFRAME64 KDHELP64 KdHelp; } STACKFRAME64, *LPSTACKFRAME64; -typedef BOOL(__stdcall *PREAD_PROCESS_MEMORY_ROUTINE64)( +typedef BOOL(__stdcall* PREAD_PROCESS_MEMORY_ROUTINE64)( HANDLE hProcess, DWORD64 qwBaseAddress, PVOID lpBuffer, DWORD nSize, LPDWORD lpNumberOfBytesRead - ); +); -typedef PVOID(__stdcall *PFUNCTION_TABLE_ACCESS_ROUTINE64)( +typedef PVOID(__stdcall* PFUNCTION_TABLE_ACCESS_ROUTINE64)( HANDLE hProcess, DWORD64 AddrBase - ); +); -typedef DWORD64(__stdcall *PGET_MODULE_BASE_ROUTINE64)( +typedef DWORD64(__stdcall* PGET_MODULE_BASE_ROUTINE64)( HANDLE hProcess, DWORD64 Address - ); +); -typedef DWORD64(__stdcall *PTRANSLATE_ADDRESS_ROUTINE64)( +typedef DWORD64(__stdcall* PTRANSLATE_ADDRESS_ROUTINE64)( HANDLE hProcess, HANDLE hThread, LPADDRESS64 lpaddr - ); +); #define SYMOPT_CASE_INSENSITIVE 0x00000001 #define SYMOPT_UNDNAME 0x00000002 @@ -246,15 +246,15 @@ typedef DWORD64(__stdcall *PTRANSLATE_ADDRESS_ROUTINE64)( // Some missing defines (for VC5/6): #ifndef INVALID_FILE_ATTRIBUTES -#define INVALID_FILE_ATTRIBUTES ((DWORD)-1) + #define INVALID_FILE_ATTRIBUTES ((DWORD)-1) #endif // secure-CRT_functions are only available starting with VC8 #if _MSC_VER < 1400 -#define strcpy_s strcpy -#define strcat_s(dst, len, src) strcat(dst, src) -#define _snprintf_s _snprintf -#define _tcscat_s _tcscat + #define strcpy_s strcpy + #define strcat_s(dst, len, src) strcat(dst, src) + #define _snprintf_s _snprintf + #define _tcscat_s _tcscat #endif // Normally it should be enough to use 'CONTEXT_FULL' (better would be 'CONTEXT_ALL') @@ -263,7 +263,7 @@ typedef DWORD64(__stdcall *PTRANSLATE_ADDRESS_ROUTINE64)( class StackWalkerInternal { public: - StackWalkerInternal(StackWalker *parent, HANDLE hProcess) + StackWalkerInternal(StackWalker* parent, HANDLE hProcess) { m_parent = parent; m_hDbhHelp = NULL; @@ -292,7 +292,7 @@ class StackWalkerInternal FreeLibrary(m_hDbhHelp); m_hDbhHelp = NULL; m_parent = NULL; - if(m_szSymPath != NULL) + if (m_szSymPath != NULL) free(m_szSymPath); m_szSymPath = NULL; } @@ -316,45 +316,41 @@ class StackWalkerInternal _tcscat_s(szTemp, _T("\\Debugging Tools for Windows\\dbghelp.dll")); // now check if the file exists: if (GetFileAttributes(szTemp) != INVALID_FILE_ATTRIBUTES) - { m_hDbhHelp = LoadLibrary(szTemp); - } } // Still not found? Then try to load the 64-Bit version: - if ( (m_hDbhHelp == NULL) && (GetEnvironmentVariable(_T("ProgramFiles"), szTemp, 4096) > 0) ) + if ((m_hDbhHelp == NULL) && (GetEnvironmentVariable(_T("ProgramFiles"), szTemp, 4096) > 0)) { _tcscat_s(szTemp, _T("\\Debugging Tools for Windows 64-Bit\\dbghelp.dll")); if (GetFileAttributes(szTemp) != INVALID_FILE_ATTRIBUTES) - { m_hDbhHelp = LoadLibrary(szTemp); - } } } } if (m_hDbhHelp == NULL) // if not already loaded, try to load a default-one - m_hDbhHelp = LoadLibrary( _T("dbghelp.dll") ); + m_hDbhHelp = LoadLibrary(_T("dbghelp.dll")); if (m_hDbhHelp == NULL) return FALSE; - pSI = (tSI) GetProcAddress(m_hDbhHelp, "SymInitialize" ); - pSC = (tSC) GetProcAddress(m_hDbhHelp, "SymCleanup" ); + pSI = (tSI) GetProcAddress(m_hDbhHelp, "SymInitialize"); + pSC = (tSC) GetProcAddress(m_hDbhHelp, "SymCleanup"); - pSW = (tSW) GetProcAddress(m_hDbhHelp, "StackWalk64" ); - pSGO = (tSGO) GetProcAddress(m_hDbhHelp, "SymGetOptions" ); - pSSO = (tSSO) GetProcAddress(m_hDbhHelp, "SymSetOptions" ); + pSW = (tSW) GetProcAddress(m_hDbhHelp, "StackWalk64"); + pSGO = (tSGO) GetProcAddress(m_hDbhHelp, "SymGetOptions"); + pSSO = (tSSO) GetProcAddress(m_hDbhHelp, "SymSetOptions"); - pSFTA = (tSFTA) GetProcAddress(m_hDbhHelp, "SymFunctionTableAccess64" ); - pSGLFA = (tSGLFA) GetProcAddress(m_hDbhHelp, "SymGetLineFromAddr64" ); - pSGMB = (tSGMB) GetProcAddress(m_hDbhHelp, "SymGetModuleBase64" ); - pSGMI = (tSGMI) GetProcAddress(m_hDbhHelp, "SymGetModuleInfo64" ); + pSFTA = (tSFTA) GetProcAddress(m_hDbhHelp, "SymFunctionTableAccess64"); + pSGLFA = (tSGLFA) GetProcAddress(m_hDbhHelp, "SymGetLineFromAddr64"); + pSGMB = (tSGMB) GetProcAddress(m_hDbhHelp, "SymGetModuleBase64"); + pSGMI = (tSGMI) GetProcAddress(m_hDbhHelp, "SymGetModuleInfo64"); //pSGMI_V3 = (tSGMI_V3) GetProcAddress(m_hDbhHelp, "SymGetModuleInfo64" ); - pSGSFA = (tSGSFA) GetProcAddress(m_hDbhHelp, "SymGetSymFromAddr64" ); - pUDSN = (tUDSN) GetProcAddress(m_hDbhHelp, "UnDecorateSymbolName" ); - pSLM = (tSLM) GetProcAddress(m_hDbhHelp, "SymLoadModule64" ); - pSGSP =(tSGSP) GetProcAddress(m_hDbhHelp, "SymGetSearchPath" ); + pSGSFA = (tSGSFA) GetProcAddress(m_hDbhHelp, "SymGetSymFromAddr64"); + pUDSN = (tUDSN) GetProcAddress(m_hDbhHelp, "UnDecorateSymbolName"); + pSLM = (tSLM) GetProcAddress(m_hDbhHelp, "SymLoadModule64"); + pSGSP = (tSGSP) GetProcAddress(m_hDbhHelp, "SymGetSearchPath"); - if ( pSC == NULL || pSFTA == NULL || pSGMB == NULL || pSGMI == NULL || + if (pSC == NULL || pSFTA == NULL || pSGMB == NULL || pSGMI == NULL || pSGO == NULL || pSGSFA == NULL || pSI == NULL || pSSO == NULL || - pSW == NULL || pUDSN == NULL || pSLM == NULL ) + pSW == NULL || pUDSN == NULL || pSLM == NULL) { FreeLibrary(m_hDbhHelp); m_hDbhHelp = NULL; @@ -389,129 +385,129 @@ class StackWalkerInternal return TRUE; } - StackWalker *m_parent; + StackWalker* m_parent; HMODULE m_hDbhHelp; HANDLE m_hProcess; LPSTR m_szSymPath; -/*typedef struct IMAGEHLP_MODULE64_V3 { - DWORD SizeOfStruct; // set to sizeof(IMAGEHLP_MODULE64) - DWORD64 BaseOfImage; // base load address of module - DWORD ImageSize; // virtual size of the loaded module - DWORD TimeDateStamp; // date/time stamp from pe header - DWORD CheckSum; // checksum from the pe header - DWORD NumSyms; // number of symbols in the symbol table - SYM_TYPE SymType; // type of symbols loaded - CHAR ModuleName[32]; // module name - CHAR ImageName[256]; // image name - // new elements: 07-Jun-2002 - CHAR LoadedImageName[256]; // symbol file name - CHAR LoadedPdbName[256]; // pdb file name - DWORD CVSig; // Signature of the CV record in the debug directories - CHAR CVData[MAX_PATH * 3]; // Contents of the CV record - DWORD PdbSig; // Signature of PDB - GUID PdbSig70; // Signature of PDB (VC 7 and up) - DWORD PdbAge; // DBI age of pdb - BOOL PdbUnmatched; // loaded an unmatched pdb - BOOL DbgUnmatched; // loaded an unmatched dbg - BOOL LineNumbers; // we have line number information - BOOL GlobalSymbols; // we have internal symbol information - BOOL TypeInfo; // we have type information - // new elements: 17-Dec-2003 - BOOL SourceIndexed; // pdb supports source server - BOOL Publics; // contains public symbols -}; -*/ -struct IMAGEHLP_MODULE64_V2 -{ - DWORD SizeOfStruct; // set to sizeof(IMAGEHLP_MODULE64) - DWORD64 BaseOfImage; // base load address of module - DWORD ImageSize; // virtual size of the loaded module - DWORD TimeDateStamp; // date/time stamp from pe header - DWORD CheckSum; // checksum from the pe header - DWORD NumSyms; // number of symbols in the symbol table - SYM_TYPE SymType; // type of symbols loaded - CHAR ModuleName[32]; // module name - CHAR ImageName[256]; // image name - CHAR LoadedImageName[256]; // symbol file name -}; + /* typedef struct IMAGEHLP_MODULE64_V3 { + DWORD SizeOfStruct; // set to sizeof(IMAGEHLP_MODULE64) + DWORD64 BaseOfImage; // base load address of module + DWORD ImageSize; // virtual size of the loaded module + DWORD TimeDateStamp; // date/time stamp from pe header + DWORD CheckSum; // checksum from the pe header + DWORD NumSyms; // number of symbols in the symbol table + SYM_TYPE SymType; // type of symbols loaded + CHAR ModuleName[32]; // module name + CHAR ImageName[256]; // image name + // new elements: 07-Jun-2002 + CHAR LoadedImageName[256]; // symbol file name + CHAR LoadedPdbName[256]; // pdb file name + DWORD CVSig; // Signature of the CV record in the debug directories + CHAR CVData[MAX_PATH * 3]; // Contents of the CV record + DWORD PdbSig; // Signature of PDB + GUID PdbSig70; // Signature of PDB (VC 7 and up) + DWORD PdbAge; // DBI age of pdb + BOOL PdbUnmatched; // loaded an unmatched pdb + BOOL DbgUnmatched; // loaded an unmatched dbg + BOOL LineNumbers; // we have line number information + BOOL GlobalSymbols; // we have internal symbol information + BOOL TypeInfo; // we have type information + // new elements: 17-Dec-2003 + BOOL SourceIndexed; // pdb supports source server + BOOL Publics; // contains public symbols + }; + */ + struct IMAGEHLP_MODULE64_V2 + { + DWORD SizeOfStruct; // set to sizeof(IMAGEHLP_MODULE64) + DWORD64 BaseOfImage; // base load address of module + DWORD ImageSize; // virtual size of the loaded module + DWORD TimeDateStamp; // date/time stamp from pe header + DWORD CheckSum; // checksum from the pe header + DWORD NumSyms; // number of symbols in the symbol table + SYM_TYPE SymType; // type of symbols loaded + CHAR ModuleName[32]; // module name + CHAR ImageName[256]; // image name + CHAR LoadedImageName[256]; // symbol file name + }; // SymCleanup() - typedef BOOL (__stdcall *tSC)( IN HANDLE hProcess ); + typedef BOOL (__stdcall* tSC)(IN HANDLE hProcess); tSC pSC; // SymFunctionTableAccess64() - typedef PVOID (__stdcall *tSFTA)( HANDLE hProcess, DWORD64 AddrBase ); + typedef PVOID(__stdcall* tSFTA)(HANDLE hProcess, DWORD64 AddrBase); tSFTA pSFTA; // SymGetLineFromAddr64() - typedef BOOL (__stdcall *tSGLFA)( IN HANDLE hProcess, IN DWORD64 dwAddr, - OUT PDWORD pdwDisplacement, OUT PIMAGEHLP_LINE64 Line ); + typedef BOOL (__stdcall* tSGLFA)(IN HANDLE hProcess, IN DWORD64 dwAddr, + OUT PDWORD pdwDisplacement, OUT PIMAGEHLP_LINE64 Line); tSGLFA pSGLFA; // SymGetModuleBase64() - typedef DWORD64 (__stdcall *tSGMB)( IN HANDLE hProcess, IN DWORD64 dwAddr ); + typedef DWORD64(__stdcall* tSGMB)(IN HANDLE hProcess, IN DWORD64 dwAddr); tSGMB pSGMB; // SymGetModuleInfo64() - typedef BOOL (__stdcall *tSGMI)( IN HANDLE hProcess, IN DWORD64 dwAddr, OUT IMAGEHLP_MODULE64_V2 *ModuleInfo ); + typedef BOOL (__stdcall* tSGMI)(IN HANDLE hProcess, IN DWORD64 dwAddr, OUT IMAGEHLP_MODULE64_V2* ModuleInfo); tSGMI pSGMI; -// // SymGetModuleInfo64() -// typedef BOOL (__stdcall *tSGMI_V3)( IN HANDLE hProcess, IN DWORD64 dwAddr, OUT IMAGEHLP_MODULE64_V3 *ModuleInfo ); -// tSGMI_V3 pSGMI_V3; + // // SymGetModuleInfo64() + // typedef BOOL (__stdcall *tSGMI_V3)( IN HANDLE hProcess, IN DWORD64 dwAddr, OUT IMAGEHLP_MODULE64_V3 *ModuleInfo ); + // tSGMI_V3 pSGMI_V3; // SymGetOptions() - typedef DWORD (__stdcall *tSGO)( VOID ); + typedef DWORD (__stdcall* tSGO)(VOID); tSGO pSGO; // SymGetSymFromAddr64() - typedef BOOL (__stdcall *tSGSFA)( IN HANDLE hProcess, IN DWORD64 dwAddr, - OUT PDWORD64 pdwDisplacement, OUT PIMAGEHLP_SYMBOL64 Symbol ); + typedef BOOL (__stdcall* tSGSFA)(IN HANDLE hProcess, IN DWORD64 dwAddr, + OUT PDWORD64 pdwDisplacement, OUT PIMAGEHLP_SYMBOL64 Symbol); tSGSFA pSGSFA; // SymInitialize() - typedef BOOL (__stdcall *tSI)( IN HANDLE hProcess, IN PSTR UserSearchPath, IN BOOL fInvadeProcess ); + typedef BOOL (__stdcall* tSI)(IN HANDLE hProcess, IN PSTR UserSearchPath, IN BOOL fInvadeProcess); tSI pSI; // SymLoadModule64() - typedef DWORD64 (__stdcall *tSLM)( IN HANDLE hProcess, IN HANDLE hFile, - IN PSTR ImageName, IN PSTR ModuleName, IN DWORD64 BaseOfDll, IN DWORD SizeOfDll ); + typedef DWORD64(__stdcall* tSLM)(IN HANDLE hProcess, IN HANDLE hFile, + IN PSTR ImageName, IN PSTR ModuleName, IN DWORD64 BaseOfDll, IN DWORD SizeOfDll); tSLM pSLM; // SymSetOptions() - typedef DWORD (__stdcall *tSSO)( IN DWORD SymOptions ); + typedef DWORD (__stdcall* tSSO)(IN DWORD SymOptions); tSSO pSSO; // StackWalk64() - typedef BOOL (__stdcall *tSW)( - DWORD MachineType, - HANDLE hProcess, - HANDLE hThread, - LPSTACKFRAME64 StackFrame, - PVOID ContextRecord, - PREAD_PROCESS_MEMORY_ROUTINE64 ReadMemoryRoutine, - PFUNCTION_TABLE_ACCESS_ROUTINE64 FunctionTableAccessRoutine, - PGET_MODULE_BASE_ROUTINE64 GetModuleBaseRoutine, - PTRANSLATE_ADDRESS_ROUTINE64 TranslateAddress ); + typedef BOOL (__stdcall* tSW)( + DWORD MachineType, + HANDLE hProcess, + HANDLE hThread, + LPSTACKFRAME64 StackFrame, + PVOID ContextRecord, + PREAD_PROCESS_MEMORY_ROUTINE64 ReadMemoryRoutine, + PFUNCTION_TABLE_ACCESS_ROUTINE64 FunctionTableAccessRoutine, + PGET_MODULE_BASE_ROUTINE64 GetModuleBaseRoutine, + PTRANSLATE_ADDRESS_ROUTINE64 TranslateAddress); tSW pSW; // UnDecorateSymbolName() - typedef DWORD (__stdcall *tUDSN)( PCSTR DecoratedName, PSTR UnDecoratedName, - DWORD UndecoratedLength, DWORD Flags ); + typedef DWORD (__stdcall* tUDSN)(PCSTR DecoratedName, PSTR UnDecoratedName, + DWORD UndecoratedLength, DWORD Flags); tUDSN pUDSN; - typedef BOOL (__stdcall *tSGSP)(HANDLE hProcess, PSTR SearchPath, DWORD SearchPathLength); + typedef BOOL (__stdcall* tSGSP)(HANDLE hProcess, PSTR SearchPath, DWORD SearchPathLength); tSGSP pSGSP; private: // **************************************** ToolHelp32 ************************ - #define MAX_MODULE_NAME32 255 - #define TH32CS_SNAPMODULE 0x00000008 - #pragma pack( push, 8 ) +#define MAX_MODULE_NAME32 255 +#define TH32CS_SNAPMODULE 0x00000008 +#pragma pack( push, 8 ) typedef struct tagMODULEENTRY32 { DWORD dwSize; @@ -519,28 +515,28 @@ struct IMAGEHLP_MODULE64_V2 DWORD th32ProcessID; // owning process DWORD GlblcntUsage; // Global usage count on the module DWORD ProccntUsage; // Module usage count in th32ProcessID's context - BYTE * modBaseAddr; // Base address of module in th32ProcessID's context + BYTE* modBaseAddr; // Base address of module in th32ProcessID's context DWORD modBaseSize; // Size in bytes of module starting at modBaseAddr HMODULE hModule; // The hModule of this module in th32ProcessID's context char szModule[MAX_MODULE_NAME32 + 1]; char szExePath[MAX_PATH]; } MODULEENTRY32; - typedef MODULEENTRY32 * PMODULEENTRY32; - typedef MODULEENTRY32 * LPMODULEENTRY32; - #pragma pack( pop ) + typedef MODULEENTRY32* PMODULEENTRY32; + typedef MODULEENTRY32* LPMODULEENTRY32; +#pragma pack( pop ) BOOL GetModuleListTH32(HANDLE hProcess, DWORD pid) { // CreateToolhelp32Snapshot() - typedef HANDLE (__stdcall *tCT32S)(DWORD dwFlags, DWORD th32ProcessID); + typedef HANDLE(__stdcall * tCT32S)(DWORD dwFlags, DWORD th32ProcessID); // Module32First() - typedef BOOL (__stdcall *tM32F)(HANDLE hSnapshot, LPMODULEENTRY32 lpme); + typedef BOOL (__stdcall * tM32F)(HANDLE hSnapshot, LPMODULEENTRY32 lpme); // Module32Next() - typedef BOOL (__stdcall *tM32N)(HANDLE hSnapshot, LPMODULEENTRY32 lpme); + typedef BOOL (__stdcall * tM32N)(HANDLE hSnapshot, LPMODULEENTRY32 lpme); // try both dlls... - const TCHAR *dllname[] = { _T("kernel32.dll"), _T("tlhelp32.dll") }; + const TCHAR* dllname[] = { _T("kernel32.dll"), _T("tlhelp32.dll") }; HINSTANCE hToolhelp = NULL; tCT32S pCT32S = NULL; tM32F pM32F = NULL; @@ -552,15 +548,15 @@ struct IMAGEHLP_MODULE64_V2 bool keepGoing; size_t i; - for (i = 0; i<(sizeof(dllname) / sizeof(dllname[0])); i++ ) + for (i = 0; i < (sizeof(dllname) / sizeof(dllname[0])); i++) { - hToolhelp = LoadLibrary( dllname[i] ); + hToolhelp = LoadLibrary(dllname[i]); if (hToolhelp == NULL) continue; pCT32S = (tCT32S) GetProcAddress(hToolhelp, "CreateToolhelp32Snapshot"); pM32F = (tM32F) GetProcAddress(hToolhelp, "Module32First"); pM32N = (tM32N) GetProcAddress(hToolhelp, "Module32Next"); - if ( (pCT32S != NULL) && (pM32F != NULL) && (pM32N != NULL) ) + if ((pCT32S != NULL) && (pM32F != NULL) && (pM32N != NULL)) break; // found the functions! FreeLibrary(hToolhelp); hToolhelp = NULL; @@ -569,17 +565,17 @@ struct IMAGEHLP_MODULE64_V2 if (hToolhelp == NULL) return FALSE; - hSnap = pCT32S( TH32CS_SNAPMODULE, pid ); - if (hSnap == (HANDLE) -1) + hSnap = pCT32S(TH32CS_SNAPMODULE, pid); + if (hSnap == (HANDLE) - 1) return FALSE; - keepGoing = !!pM32F( hSnap, &me ); + keepGoing = !!pM32F(hSnap, &me); int cnt = 0; while (keepGoing) { this->LoadModule(hProcess, me.szExePath, me.szModule, (DWORD64) me.modBaseAddr, me.modBaseSize); cnt++; - keepGoing = !!pM32N( hSnap, &me ); + keepGoing = !!pM32N(hSnap, &me); } CloseHandle(hSnap); FreeLibrary(hToolhelp); @@ -599,13 +595,13 @@ struct IMAGEHLP_MODULE64_V2 BOOL GetModuleListPSAPI(HANDLE hProcess) { // EnumProcessModules() - typedef BOOL (__stdcall *tEPM)(HANDLE hProcess, HMODULE *lphModule, DWORD cb, LPDWORD lpcbNeeded ); + typedef BOOL (__stdcall * tEPM)(HANDLE hProcess, HMODULE * lphModule, DWORD cb, LPDWORD lpcbNeeded); // GetModuleFileNameEx() - typedef DWORD (__stdcall *tGMFNE)(HANDLE hProcess, HMODULE hModule, LPSTR lpFilename, DWORD nSize ); + typedef DWORD (__stdcall * tGMFNE)(HANDLE hProcess, HMODULE hModule, LPSTR lpFilename, DWORD nSize); // GetModuleBaseName() - typedef DWORD (__stdcall *tGMBN)(HANDLE hProcess, HMODULE hModule, LPSTR lpFilename, DWORD nSize ); + typedef DWORD (__stdcall * tGMBN)(HANDLE hProcess, HMODULE hModule, LPSTR lpFilename, DWORD nSize); // GetModuleInformation() - typedef BOOL (__stdcall *tGMI)(HANDLE hProcess, HMODULE hModule, LPMODULEINFO pmi, DWORD nSize ); + typedef BOOL (__stdcall * tGMI)(HANDLE hProcess, HMODULE hModule, LPMODULEINFO pmi, DWORD nSize); HINSTANCE hPsapi; tEPM pEPM; @@ -617,21 +613,21 @@ struct IMAGEHLP_MODULE64_V2 //ModuleEntry e; DWORD cbNeeded; MODULEINFO mi; - HMODULE *hMods; - char *tt; - char *tt2; + HMODULE* hMods; + char* tt; + char* tt2; const SIZE_T TTBUFLEN = 8096; int cnt = 0; - hPsapi = LoadLibrary( _T("psapi.dll") ); + hPsapi = LoadLibrary(_T("psapi.dll")); if (hPsapi == NULL) return FALSE; - pEPM = (tEPM) GetProcAddress( hPsapi, "EnumProcessModules" ); - pGMFNE = (tGMFNE) GetProcAddress( hPsapi, "GetModuleFileNameExA" ); - pGMBN = (tGMFNE) GetProcAddress( hPsapi, "GetModuleBaseNameA" ); - pGMI = (tGMI) GetProcAddress( hPsapi, "GetModuleInformation" ); - if ( (pEPM == NULL) || (pGMFNE == NULL) || (pGMBN == NULL) || (pGMI == NULL) ) + pEPM = (tEPM) GetProcAddress(hPsapi, "EnumProcessModules"); + pGMFNE = (tGMFNE) GetProcAddress(hPsapi, "GetModuleFileNameExA"); + pGMBN = (tGMFNE) GetProcAddress(hPsapi, "GetModuleBaseNameA"); + pGMI = (tGMI) GetProcAddress(hPsapi, "GetModuleInformation"); + if ((pEPM == NULL) || (pGMFNE == NULL) || (pGMBN == NULL) || (pGMI == NULL)) { // we couldnt find all functions FreeLibrary(hPsapi); @@ -641,31 +637,31 @@ struct IMAGEHLP_MODULE64_V2 hMods = (HMODULE*) malloc(sizeof(HMODULE) * (TTBUFLEN / sizeof(HMODULE))); tt = (char*) malloc(sizeof(char) * TTBUFLEN); tt2 = (char*) malloc(sizeof(char) * TTBUFLEN); - if ( (hMods == NULL) || (tt == NULL) || (tt2 == NULL) ) + if ((hMods == NULL) || (tt == NULL) || (tt2 == NULL)) goto cleanup; - if ( ! pEPM( hProcess, hMods, TTBUFLEN, &cbNeeded ) ) + if (! pEPM(hProcess, hMods, TTBUFLEN, &cbNeeded)) { //_ftprintf(fLogFile, _T("%lu: EPM failed, GetLastError = %lu\n"), g_dwShowCount, gle ); goto cleanup; } - if ( cbNeeded > TTBUFLEN ) + if (cbNeeded > TTBUFLEN) { //_ftprintf(fLogFile, _T("%lu: More than %lu module handles. Huh?\n"), g_dwShowCount, lenof( hMods ) ); goto cleanup; } - for ( i = 0; i < cbNeeded / sizeof hMods[0]; i++ ) + for (i = 0; i < cbNeeded / sizeof hMods[0]; i++) { // base address, size - pGMI(hProcess, hMods[i], &mi, sizeof mi ); + pGMI(hProcess, hMods[i], &mi, sizeof mi); // image file name tt[0] = 0; - pGMFNE(hProcess, hMods[i], tt, TTBUFLEN ); + pGMFNE(hProcess, hMods[i], tt, TTBUFLEN); // module name tt2[0] = 0; - pGMBN(hProcess, hMods[i], tt2, TTBUFLEN ); + pGMBN(hProcess, hMods[i], tt2, TTBUFLEN); DWORD dwRes = this->LoadModule(hProcess, tt, tt2, (DWORD64) mi.lpBaseOfDll, mi.SizeOfImage); if (dwRes != ERROR_SUCCESS) @@ -673,31 +669,34 @@ struct IMAGEHLP_MODULE64_V2 cnt++; } - cleanup: +cleanup: FreeLibrary(hPsapi); - if (tt2 != NULL) free(tt2); - if (tt != NULL) free(tt); - if (hMods != NULL) free(hMods); + if (tt2 != NULL) + free(tt2); + if (tt != NULL) + free(tt); + if (hMods != NULL) + free(hMods); return (cnt != 0) ? TRUE : FALSE; } // GetModuleListPSAPI DWORD LoadModule(HANDLE hProcess, LPCSTR img, LPCSTR mod, DWORD64 baseAddr, DWORD size) { - CHAR *szImg = _strdup(img); - CHAR *szMod = _strdup(mod); + CHAR* szImg = _strdup(img); + CHAR* szMod = _strdup(mod); DWORD result = ERROR_SUCCESS; - if ( (szImg == NULL) || (szMod == NULL) ) + if ((szImg == NULL) || (szMod == NULL)) result = ERROR_NOT_ENOUGH_MEMORY; else if (pSLM(hProcess, 0, szImg, szMod, baseAddr, size) == 0) result = GetLastError(); ULONGLONG fileVersion = 0; - if ( (m_parent != NULL) && (szImg != NULL) ) + if ((m_parent != NULL) && (szImg != NULL)) { // try to retrive the file-version: - if ( (m_parent->m_options & (int) StackWalker::RetrieveFileVersion) != 0) + if ((m_parent->m_options & (int) StackWalker::RetrieveFileVersion) != 0) { - VS_FIXEDFILEINFO *fInfo = NULL; + VS_FIXEDFILEINFO* fInfo = NULL; DWORD dwHandle; DWORD dwSize = GetFileVersionInfoSizeA(szImg, &dwHandle); if (dwSize > 0) @@ -710,9 +709,7 @@ struct IMAGEHLP_MODULE64_V2 UINT len; TCHAR szSubBlock[] = _T("\\"); if (VerQueryValue(vData, szSubBlock, (LPVOID*) &fInfo, &len) != 0) - { fileVersion = ((ULONGLONG)fInfo->dwFileVersionLS) + ((ULONGLONG)fInfo->dwFileVersionMS << 32); - } } free(vData); } @@ -721,44 +718,46 @@ struct IMAGEHLP_MODULE64_V2 // Retrieve some additional-infos about the module IMAGEHLP_MODULE64_V2 Module; - const char *szSymType = "-unknown-"; + const char* szSymType = "-unknown-"; if (this->GetModuleInfo(hProcess, baseAddr, &Module) != FALSE) { - switch(Module.SymType) + switch (Module.SymType) { - case SymNone: - szSymType = "-nosymbols-"; - break; - case SymCoff: - szSymType = "COFF"; - break; - case SymCv: - szSymType = "CV"; - break; - case SymPdb: - szSymType = "PDB"; - break; - case SymExport: - szSymType = "-exported-"; - break; - case SymDeferred: - szSymType = "-deferred-"; - break; - case SymSym: - szSymType = "SYM"; - break; - case SymVirtual: - szSymType = "Virtual"; - break; - case SymDia: - szSymType = "DIA"; - break; + case SymNone: + szSymType = "-nosymbols-"; + break; + case SymCoff: + szSymType = "COFF"; + break; + case SymCv: + szSymType = "CV"; + break; + case SymPdb: + szSymType = "PDB"; + break; + case SymExport: + szSymType = "-exported-"; + break; + case SymDeferred: + szSymType = "-deferred-"; + break; + case SymSym: + szSymType = "SYM"; + break; + case SymVirtual: + szSymType = "Virtual"; + break; + case SymDia: + szSymType = "DIA"; + break; } } m_parent->OnLoadModule(img, mod, baseAddr, size, result, szSymType, Module.LoadedImageName, fileVersion); } - if (szImg != NULL) free(szImg); - if (szMod != NULL) free(szMod); + if (szImg != NULL) + free(szImg); + if (szMod != NULL) + free(szMod); return result; } public: @@ -772,9 +771,9 @@ struct IMAGEHLP_MODULE64_V2 } - BOOL GetModuleInfo(HANDLE hProcess, DWORD64 baseAddr, IMAGEHLP_MODULE64_V2 *pModuleInfo) + BOOL GetModuleInfo(HANDLE hProcess, DWORD64 baseAddr, IMAGEHLP_MODULE64_V2* pModuleInfo) { - if(pSGMI == NULL) + if (pSGMI == NULL) { SetLastError(ERROR_DLL_INIT_FAILED); return FALSE; @@ -792,7 +791,7 @@ struct IMAGEHLP_MODULE64_V2 // } // could not retrive the bigger structure, try with the smaller one (as defined in VC7.1)... pModuleInfo->SizeOfStruct = sizeof(IMAGEHLP_MODULE64_V2); - void *pData = malloc(4096); // reserve enough memory, so the bug in v6.3.5.1 does not lead to memory-overwrites... + void* pData = malloc(4096); // reserve enough memory, so the bug in v6.3.5.1 does not lead to memory-overwrites... if (pData == NULL) { SetLastError(ERROR_NOT_ENOUGH_MEMORY); @@ -860,8 +859,8 @@ BOOL StackWalker::LoadModules() return TRUE; // Build the sym-path: - char *szSymPath = NULL; - if ( (m_options & (int) SymBuildPath) != 0) + char* szSymPath = NULL; + if ((m_options & (int) SymBuildPath) != 0) { const size_t nSymPathLen = 4096; szSymPath = (char*) malloc(nSymPathLen); @@ -885,7 +884,7 @@ BOOL StackWalker::LoadModules() // Now add the current directory: if (GetCurrentDirectoryA(nTempLen, szTemp) > 0) { - szTemp[nTempLen-1] = 0; + szTemp[nTempLen - 1] = 0; strcat_s(szSymPath, nSymPathLen, szTemp); strcat_s(szSymPath, nSymPathLen, ";"); } @@ -893,11 +892,11 @@ BOOL StackWalker::LoadModules() // Now add the path for the main-module: if (GetModuleFileNameA(NULL, szTemp, nTempLen) > 0) { - szTemp[nTempLen-1] = 0; - for (char *p = (szTemp+strlen(szTemp)-1); p >= szTemp; --p) + szTemp[nTempLen - 1] = 0; + for (char* p = (szTemp + strlen(szTemp) - 1); p >= szTemp; --p) { // locate the rightmost path separator - if ( (*p == '\\') || (*p == '/') || (*p == ':') ) + if ((*p == '\\') || (*p == '/') || (*p == ':')) { *p = 0; break; @@ -911,19 +910,19 @@ BOOL StackWalker::LoadModules() } if (GetEnvironmentVariableA("_NT_SYMBOL_PATH", szTemp, nTempLen) > 0) { - szTemp[nTempLen-1] = 0; + szTemp[nTempLen - 1] = 0; strcat_s(szSymPath, nSymPathLen, szTemp); strcat_s(szSymPath, nSymPathLen, ";"); } if (GetEnvironmentVariableA("_NT_ALTERNATE_SYMBOL_PATH", szTemp, nTempLen) > 0) { - szTemp[nTempLen-1] = 0; + szTemp[nTempLen - 1] = 0; strcat_s(szSymPath, nSymPathLen, szTemp); strcat_s(szSymPath, nSymPathLen, ";"); } if (GetEnvironmentVariableA("SYSTEMROOT", szTemp, nTempLen) > 0) { - szTemp[nTempLen-1] = 0; + szTemp[nTempLen - 1] = 0; strcat_s(szSymPath, nSymPathLen, szTemp); strcat_s(szSymPath, nSymPathLen, ";"); // also add the "system32"-directory: @@ -932,20 +931,18 @@ BOOL StackWalker::LoadModules() strcat_s(szSymPath, nSymPathLen, ";"); } - if ( (m_options & (int) SymBuildPath) != 0) + if ((m_options & (int) SymBuildPath) != 0) { if (GetEnvironmentVariableA("SYSTEMDRIVE", szTemp, nTempLen) > 0) { - szTemp[nTempLen-1] = 0; + szTemp[nTempLen - 1] = 0; strcat_s(szSymPath, nSymPathLen, "SRV*"); strcat_s(szSymPath, nSymPathLen, szTemp); strcat_s(szSymPath, nSymPathLen, "\\websymbols"); strcat_s(szSymPath, nSymPathLen, "*http://msdl.microsoft.com/download/symbols;"); } else - { strcat_s(szSymPath, nSymPathLen, "SRV*c:\\websymbols*http://msdl.microsoft.com/download/symbols;"); - } } } @@ -978,14 +975,14 @@ static StackWalker::PReadProcessMemoryRoutine s_readMemoryFunction = NULL; static LPVOID s_readMemoryFunction_UserData = NULL; #ifdef _MSC_VER -#pragma warning(disable : 4748) + #pragma warning(disable : 4748) #endif -BOOL StackWalker::ShowCallstack(HANDLE hThread, const CONTEXT *context, PReadProcessMemoryRoutine readMemoryFunction, LPVOID pUserData) +BOOL StackWalker::ShowCallstack(HANDLE hThread, const CONTEXT* context, PReadProcessMemoryRoutine readMemoryFunction, LPVOID pUserData) { CONTEXT c;; CallstackEntry csEntry; - IMAGEHLP_SYMBOL64 *pSym = NULL; + IMAGEHLP_SYMBOL64* pSym = NULL; StackWalkerInternal::IMAGEHLP_MODULE64_V2 Module; IMAGEHLP_LINE64 Line; int frameNum; @@ -1010,9 +1007,7 @@ BOOL StackWalker::ShowCallstack(HANDLE hThread, const CONTEXT *context, PReadPro { // If no context is provided, capture the context if (hThread == GetCurrentThread()) - { GET_CURRENT_CONTEXT(c, USED_CONTEXT_FLAGS); - } else { SuspendThread(hThread); @@ -1060,11 +1055,12 @@ BOOL StackWalker::ShowCallstack(HANDLE hThread, const CONTEXT *context, PReadPro s.AddrStack.Offset = c.IntSp; s.AddrStack.Mode = AddrModeFlat; #else - #error "Platform not supported!" +#error "Platform not supported!" #endif - pSym = (IMAGEHLP_SYMBOL64 *) malloc(sizeof(IMAGEHLP_SYMBOL64) + STACKWALK_MAX_NAMELEN); - if (!pSym) goto cleanup; // not enough memory... + pSym = (IMAGEHLP_SYMBOL64*) malloc(sizeof(IMAGEHLP_SYMBOL64) + STACKWALK_MAX_NAMELEN); + if (!pSym) + goto cleanup; // not enough memory... memset(pSym, 0, sizeof(IMAGEHLP_SYMBOL64) + STACKWALK_MAX_NAMELEN); pSym->SizeOfStruct = sizeof(IMAGEHLP_SYMBOL64); pSym->MaxNameLength = STACKWALK_MAX_NAMELEN; @@ -1075,14 +1071,14 @@ BOOL StackWalker::ShowCallstack(HANDLE hThread, const CONTEXT *context, PReadPro memset(&Module, 0, sizeof(Module)); Module.SizeOfStruct = sizeof(Module); - for (frameNum = 0; ; ++frameNum ) + for (frameNum = 0; ; ++frameNum) { // get next stack frame (StackWalk64(), SymFunctionTableAccess64(), SymGetModuleBase64()) // if this returns ERROR_INVALID_ADDRESS (487) or ERROR_NOACCESS (998), you can // assume that either you are done, or that the stack is so hosed that the next // deeper frame could not be found. // CONTEXT need not to be suplied if imageTyp is IMAGE_FILE_MACHINE_I386! - if ( ! m_sw->pSW(imageType, m_hProcess, hThread, &s, &c, myReadProcMem, m_sw->pSFTA, m_sw->pSGMB, NULL) ) + if (! m_sw->pSW(imageType, m_hProcess, hThread, &s, &c, myReadProcMem, m_sw->pSFTA, m_sw->pSGMB, NULL)) { this->OnDbgHelpErr("StackWalk64", GetLastError(), s.AddrPC.Offset); break; @@ -1111,66 +1107,64 @@ BOOL StackWalker::ShowCallstack(HANDLE hThread, const CONTEXT *context, PReadPro { strcpy_s(csEntry.name, pSym->Name); // UnDecorateSymbolName() - m_sw->pUDSN( pSym->Name, csEntry.undName, STACKWALK_MAX_NAMELEN, UNDNAME_NAME_ONLY ); - m_sw->pUDSN( pSym->Name, csEntry.undFullName, STACKWALK_MAX_NAMELEN, UNDNAME_COMPLETE ); + m_sw->pUDSN(pSym->Name, csEntry.undName, STACKWALK_MAX_NAMELEN, UNDNAME_NAME_ONLY); + m_sw->pUDSN(pSym->Name, csEntry.undFullName, STACKWALK_MAX_NAMELEN, UNDNAME_COMPLETE); } else - { this->OnDbgHelpErr("SymGetSymFromAddr64", GetLastError(), s.AddrPC.Offset); - } // show line number info, NT5.0-method (SymGetLineFromAddr64()) - if (m_sw->pSGLFA != NULL ) - { // yes, we have SymGetLineFromAddr64() + if (m_sw->pSGLFA != NULL) + { + // yes, we have SymGetLineFromAddr64() if (m_sw->pSGLFA(m_hProcess, s.AddrPC.Offset, &(csEntry.offsetFromLine), &Line) != FALSE) { csEntry.lineNumber = Line.LineNumber; strcpy_s(csEntry.lineFileName, Line.FileName); } else - { this->OnDbgHelpErr("SymGetLineFromAddr64", GetLastError(), s.AddrPC.Offset); - } } // yes, we have SymGetLineFromAddr64() // show module info (SymGetModuleInfo64()) - if (m_sw->GetModuleInfo(m_hProcess, s.AddrPC.Offset, &Module ) != FALSE) - { // got module info OK - switch ( Module.SymType ) + if (m_sw->GetModuleInfo(m_hProcess, s.AddrPC.Offset, &Module) != FALSE) + { + // got module info OK + switch (Module.SymType) { - case SymNone: - csEntry.symTypeString = "-nosymbols-"; - break; - case SymCoff: - csEntry.symTypeString = "COFF"; - break; - case SymCv: - csEntry.symTypeString = "CV"; - break; - case SymPdb: - csEntry.symTypeString = "PDB"; - break; - case SymExport: - csEntry.symTypeString = "-exported-"; - break; - case SymDeferred: - csEntry.symTypeString = "-deferred-"; - break; - case SymSym: - csEntry.symTypeString = "SYM"; - break; + case SymNone: + csEntry.symTypeString = "-nosymbols-"; + break; + case SymCoff: + csEntry.symTypeString = "COFF"; + break; + case SymCv: + csEntry.symTypeString = "CV"; + break; + case SymPdb: + csEntry.symTypeString = "PDB"; + break; + case SymExport: + csEntry.symTypeString = "-exported-"; + break; + case SymDeferred: + csEntry.symTypeString = "-deferred-"; + break; + case SymSym: + csEntry.symTypeString = "SYM"; + break; #if API_VERSION_NUMBER >= 9 - case SymDia: - csEntry.symTypeString = "DIA"; - break; + case SymDia: + csEntry.symTypeString = "DIA"; + break; #endif - case SymVirtual: - csEntry.symTypeString = "Virtual"; - break; - default: - //_snprintf( ty, sizeof ty, "symtype=%ld", (long) Module.SymType ); - csEntry.symTypeString = NULL; - break; + case SymVirtual: + csEntry.symTypeString = "Virtual"; + break; + default: + //_snprintf( ty, sizeof ty, "symtype=%ld", (long) Module.SymType ); + csEntry.symTypeString = NULL; + break; } strcpy_s(csEntry.moduleName, Module.ModuleName); @@ -1178,9 +1172,7 @@ BOOL StackWalker::ShowCallstack(HANDLE hThread, const CONTEXT *context, PReadPro strcpy_s(csEntry.loadedImageName, Module.LoadedImageName); } // got module info OK else - { this->OnDbgHelpErr("SymGetModuleInfo64", GetLastError(), s.AddrPC.Offset); - } } // we seem to have a valid PC CallstackEntryType et = nextEntry; @@ -1196,8 +1188,9 @@ BOOL StackWalker::ShowCallstack(HANDLE hThread, const CONTEXT *context, PReadPro } } // for ( frameNum ) - cleanup: - if (pSym) free( pSym ); +cleanup: + if (pSym) + free(pSym); if (context == NULL) ResumeThread(hThread); @@ -1211,7 +1204,7 @@ BOOL __stdcall StackWalker::myReadProcMem( PVOID lpBuffer, DWORD nSize, LPDWORD lpNumberOfBytesRead - ) +) { if (s_readMemoryFunction == NULL) { @@ -1222,9 +1215,7 @@ BOOL __stdcall StackWalker::myReadProcMem( return bRet; } else - { return s_readMemoryFunction(hProcess, qwBaseAddress, lpBuffer, nSize, lpNumberOfBytesRead, s_readMemoryFunction_UserData); - } } void StackWalker::OnLoadModule(LPCSTR img, LPCSTR mod, DWORD64 baseAddr, DWORD size, DWORD result, LPCSTR symType, LPCSTR pdbName, ULONGLONG fileVersion) @@ -1235,18 +1226,18 @@ void StackWalker::OnLoadModule(LPCSTR img, LPCSTR mod, DWORD64 baseAddr, DWORD s else { DWORD v4 = (DWORD) fileVersion & 0xFFFF; - DWORD v3 = (DWORD) (fileVersion>>16) & 0xFFFF; - DWORD v2 = (DWORD) (fileVersion>>32) & 0xFFFF; - DWORD v1 = (DWORD) (fileVersion>>48) & 0xFFFF; + DWORD v3 = (DWORD)(fileVersion >> 16) & 0xFFFF; + DWORD v2 = (DWORD)(fileVersion >> 32) & 0xFFFF; + DWORD v1 = (DWORD)(fileVersion >> 48) & 0xFFFF; _snprintf_s(buffer, STACKWALK_MAX_NAMELEN, "%s:%s (%p), size: %d (result: %d), SymType: '%s', PDB: '%s', fileVersion: %d.%d.%d.%d", img, mod, (LPVOID) baseAddr, size, result, symType, pdbName, v1, v2, v3, v4); } OnOutput(buffer); } -void StackWalker::OnCallstackEntry(CallstackEntryType eType, CallstackEntry &entry) +void StackWalker::OnCallstackEntry(CallstackEntryType eType, CallstackEntry& entry) { CHAR buffer[STACKWALK_MAX_NAMELEN]; - if ( (eType != lastEntry) && (entry.offset != 0) ) + if ((eType != lastEntry) && (entry.offset != 0)) { if (entry.name[0] == 0) strcpy_s(entry.name, "(function-name not available)"); @@ -1296,7 +1287,7 @@ void StackWalker::OnSymInit(LPCSTR szSearchPath, DWORD symOptions, LPCSTR szUser ZeroMemory(&ver, sizeof(OSVERSIONINFOEXA)); ver.dwOSVersionInfoSize = sizeof(ver); #pragma warning(suppress: 4996) - if (GetVersionExA( (OSVERSIONINFOA*) &ver) != FALSE) + if (GetVersionExA((OSVERSIONINFOA*) &ver) != FALSE) { _snprintf_s(buffer, STACKWALK_MAX_NAMELEN, "OS-Version: %d.%d.%d (%s) 0x%x-0x%x", ver.dwMajorVersion, ver.dwMinorVersion, ver.dwBuildNumber, diff --git a/lib/xspublic/xscommon/stackwalker.h b/lib/xspublic/xscommon/stackwalker.h index 013f969..cbe48ad 100644 --- a/lib/xspublic/xscommon/stackwalker.h +++ b/lib/xspublic/xscommon/stackwalker.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -63,14 +63,14 @@ // /********************************************************************** - * - * StackWalker.h - * - * - * History: - * 2005-07-27 v1 - First public release on http://www.codeproject.com/ - * (for additional changes see History in 'StackWalker.cpp'! - * + + StackWalker.h + + + History: + 2005-07-27 v1 - First public release on http://www.codeproject.com/ + (for additional changes see History in 'StackWalker.cpp'! + **********************************************************************/ #ifndef STACKWALKER_H #define STACKWALKER_H @@ -85,17 +85,18 @@ // special defines for VC5/6 (if no actual PSDK is installed): #if _MSC_VER < 1300 -typedef unsigned __int64 DWORD64, *PDWORD64; -#if defined(_WIN64) -typedef unsigned __int64 SIZE_T, *PSIZE_T; -#else -typedef unsigned long SIZE_T, *PSIZE_T; -#endif + typedef unsigned __int64 DWORD64, *PDWORD64; + #if defined(_WIN64) + typedef unsigned __int64 SIZE_T, *PSIZE_T; + #else + typedef unsigned long SIZE_T, *PSIZE_T; + #endif #endif // _MSC_VER < 1300 class StackWalkerInternal; /*! \brief A class that can help with walking the stack for debugging purposes (Windows version) */ -class StackWalker { +class StackWalker +{ public: enum StackWalkOptions { @@ -132,28 +133,28 @@ class StackWalker { }; StackWalker(int options = (int) OptionsAll, // 'int' is by design, to combine the enum-flags - LPCSTR szSymPath = NULL, - DWORD dwProcessId = GetCurrentProcessId(), - HANDLE hProcess = GetCurrentProcess()); + LPCSTR szSymPath = NULL, + DWORD dwProcessId = GetCurrentProcessId(), + HANDLE hProcess = GetCurrentProcess()); StackWalker(DWORD dwProcessId, HANDLE hProcess); virtual ~StackWalker(); - typedef BOOL (__stdcall *PReadProcessMemoryRoutine)( - HANDLE hProcess, - DWORD64 qwBaseAddress, - PVOID lpBuffer, - DWORD nSize, - LPDWORD lpNumberOfBytesRead, - LPVOID pUserData // optional data, which was passed in "ShowCallstack" + typedef BOOL (__stdcall* PReadProcessMemoryRoutine)( + HANDLE hProcess, + DWORD64 qwBaseAddress, + PVOID lpBuffer, + DWORD nSize, + LPDWORD lpNumberOfBytesRead, + LPVOID pUserData // optional data, which was passed in "ShowCallstack" ); BOOL LoadModules(); BOOL ShowCallstack(HANDLE hThread = GetCurrentThread(), - const CONTEXT *context = NULL, - PReadProcessMemoryRoutine readMemoryFunction = NULL, - LPVOID pUserData = NULL // optional to identify some data in the 'readMemoryFunction'-callback - ); + const CONTEXT* context = NULL, + PReadProcessMemoryRoutine readMemoryFunction = NULL, + LPVOID pUserData = NULL // optional to identify some data in the 'readMemoryFunction'-callback + ); protected: enum { STACKWALK_MAX_NAMELEN = 1024 }; // max name length for found symbols @@ -180,11 +181,11 @@ class StackWalker { virtual void OnSymInit(LPCSTR szSearchPath, DWORD symOptions, LPCSTR szUserName); virtual void OnLoadModule(LPCSTR img, LPCSTR mod, DWORD64 baseAddr, DWORD size, DWORD result, LPCSTR symType, LPCSTR pdbName, ULONGLONG fileVersion); - virtual void OnCallstackEntry(CallstackEntryType eType, CallstackEntry &entry); + virtual void OnCallstackEntry(CallstackEntryType eType, CallstackEntry& entry); virtual void OnDbgHelpErr(LPCSTR szFuncName, DWORD gle, DWORD64 addr); virtual void OnOutput(LPCSTR szText); - StackWalkerInternal *m_sw; + StackWalkerInternal* m_sw; HANDLE m_hProcess; DWORD m_dwProcessId; BOOL m_modulesLoaded; @@ -215,39 +216,39 @@ class StackWalker { // The following is not a "perfect" implementation, // because the callstack is only valid in the "__except" block... #define GET_CURRENT_CONTEXT(c, contextFlags) \ - do { \ - memset(&c, 0, sizeof(CONTEXT)); \ - EXCEPTION_POINTERS *pExp = NULL; \ - __try { \ - throw 0; \ - } __except( ( (pExp = GetExceptionInformation()) ? EXCEPTION_EXECUTE_HANDLER : EXCEPTION_EXECUTE_HANDLER)) {} \ - if (pExp != NULL) \ - memcpy(&c, pExp->ContextRecord, sizeof(CONTEXT)); \ - c.ContextFlags = contextFlags; \ - } while(0); + do { \ + memset(&c, 0, sizeof(CONTEXT)); \ + EXCEPTION_POINTERS *pExp = NULL; \ + __try { \ + throw 0; \ + } __except( ( (pExp = GetExceptionInformation()) ? EXCEPTION_EXECUTE_HANDLER : EXCEPTION_EXECUTE_HANDLER)) {} \ + if (pExp != NULL) \ + memcpy(&c, pExp->ContextRecord, sizeof(CONTEXT)); \ + c.ContextFlags = contextFlags; \ + } while(0) #else // The following should be enough for walking the callstack... #define GET_CURRENT_CONTEXT(c, contextFlags) \ - do { \ - memset(&c, 0, sizeof(CONTEXT)); \ - c.ContextFlags = contextFlags; \ - __asm call x \ - __asm x: pop eax \ - __asm mov c.Eip, eax \ - __asm mov c.Ebp, ebp \ - __asm mov c.Esp, esp \ - } while(0); + do { \ + memset(&c, 0, sizeof(CONTEXT)); \ + c.ContextFlags = contextFlags; \ + __asm call x \ + __asm x: pop eax \ + __asm mov c.Eip, eax \ + __asm mov c.Ebp, ebp \ + __asm mov c.Esp, esp \ + } while(0) #endif #else // The following is defined for x86 (XP and higher), x64 and IA64: #define GET_CURRENT_CONTEXT(c, contextFlags) \ - do { \ - memset(&c, 0, sizeof(CONTEXT)); \ - c.ContextFlags = contextFlags; \ - RtlCaptureContext(&c); \ -} while(0); + do { \ + memset(&c, 0, sizeof(CONTEXT)); \ + c.ContextFlags = contextFlags; \ + RtlCaptureContext(&c); \ + } while(0) #endif #endif diff --git a/lib/xspublic/xscommon/stackwalker_linux.cpp b/lib/xspublic/xscommon/stackwalker_linux.cpp index eff016e..9c17217 100644 --- a/lib/xspublic/xscommon/stackwalker_linux.cpp +++ b/lib/xspublic/xscommon/stackwalker_linux.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -67,17 +67,17 @@ #include #ifdef ANDROID -#include + #include #elif defined(USING_UNWIND_LIB) -#define UNW_LOCAL_ONLY -#include -#include -#include + #define UNW_LOCAL_ONLY + #include + #include + #include #endif #ifdef __arm__ -#define UNW_PRINTF_POINTER "0x%x: " -#define UNW_PRINTF_NAME_AND_POINTER " (%s+0x%x)" + #define UNW_PRINTF_POINTER "0x%x: " + #define UNW_PRINTF_NAME_AND_POINTER " (%s+0x%x)" #else #include #if UINTPTR_MAX == 0xffffffff @@ -139,9 +139,7 @@ void StackWalker::ShowCallstack() std::free(demangled); } else - { std::snprintf(&logLine[offset], logLineSize - offset, " (unable to retrieve symbol name)"); - } OnOutput(logLine); } diff --git a/lib/xspublic/xscommon/stackwalker_linux.h b/lib/xspublic/xscommon/stackwalker_linux.h index 260c5bc..14331ae 100644 --- a/lib/xspublic/xscommon/stackwalker_linux.h +++ b/lib/xspublic/xscommon/stackwalker_linux.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xscommon/threading.cpp b/lib/xspublic/xscommon/threading.cpp index 7dd8a6a..261c353 100644 --- a/lib/xspublic/xscommon/threading.cpp +++ b/lib/xspublic/xscommon/threading.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -68,16 +68,18 @@ #include #ifdef __GNUC__ -#include -#include -#include -#include -#define _strdup strdup + #include + #include + #include + #include + #define _strdup strdup #endif -namespace xsens { +namespace xsens +{ #ifdef __GNUC__ -pthread_t xsStartThread(void *(func)(void *), void *param, void *pid) { +pthread_t xsStartThread(void* (func)(void*), void* param, void* pid) +{ return ::xsStartThread(func, param, pid); } #endif @@ -90,8 +92,8 @@ StandardThread::StandardThread() , m_stop(false) , m_yieldOnZeroSleep(true) #ifdef _WIN32 - , m_stopHandle(::CreateEvent(NULL,TRUE,FALSE,NULL)) - , m_running(::CreateEvent(NULL,TRUE,FALSE,NULL)) + , m_stopHandle(::CreateEvent(NULL, TRUE, FALSE, NULL)) + , m_running(::CreateEvent(NULL, TRUE, FALSE, NULL)) , m_threadId(0) #else , m_running(false) @@ -142,7 +144,7 @@ bool StandardThread::isAlive(void) volatile const noexcept #ifdef _WIN32 DWORD exitCode; - if (::GetExitCodeThread(m_thread,&exitCode)) + if (::GetExitCodeThread(m_thread, &exitCode)) { if (exitCode == STILL_ACTIVE) return true; @@ -160,15 +162,15 @@ bool StandardThread::isRunning(void) volatile const noexcept if (!isAlive()) return false; #ifdef _WIN32 - switch(::WaitForSingleObject(m_running,0)) + switch (::WaitForSingleObject(m_running, 0)) { - case WAIT_ABANDONED: - case WAIT_TIMEOUT: - return false; - case WAIT_OBJECT_0: - return true; - default: - return false; + case WAIT_ABANDONED: + case WAIT_TIMEOUT: + return false; + case WAIT_OBJECT_0: + return true; + default: + return false; } #else return m_running; @@ -207,60 +209,61 @@ bool StandardThread::setPriority(XsThreadPriority pri) rv = pthread_getschedparam(m_thread, &policy, ¶m); switch (rv) { - case ESRCH: - /* The value specified by thread does not refer to an existing thread. */ - return false; - default: - break; + case ESRCH: + /* The value specified by thread does not refer to an existing thread. */ + return false; + default: + break; } - switch (pri) { - case XS_THREAD_PRIORITY_HIGHEST: - param.sched_priority = sched_get_priority_max(policy); - break; + switch (pri) + { + case XS_THREAD_PRIORITY_HIGHEST: + param.sched_priority = sched_get_priority_max(policy); + break; - case XS_THREAD_PRIORITY_LOWEST: - param.sched_priority = sched_get_priority_max(policy); + case XS_THREAD_PRIORITY_LOWEST: + param.sched_priority = sched_get_priority_max(policy); // Fallthrough. - default: - /* we need to map the priority to the values used on this system */ - int32_t min_prio = sched_get_priority_min(policy); - int32_t max_prio = sched_get_priority_max(policy); + default: + /* we need to map the priority to the values used on this system */ + int32_t min_prio = sched_get_priority_min(policy); + int32_t max_prio = sched_get_priority_max(policy); - if (min_prio < 0 || max_prio < 0) - return false; + if (min_prio < 0 || max_prio < 0) + return false; - /* divide range up in amount of priority levels */ - float priostep = ((float)(max_prio - min_prio)) / ((int32_t)XS_THREAD_PRIORITY_HIGHEST + 1); + /* divide range up in amount of priority levels */ + float priostep = ((float)(max_prio - min_prio)) / ((int32_t)XS_THREAD_PRIORITY_HIGHEST + 1); - param.sched_priority = (int32_t) (min_prio + (pri * priostep)); - break; + param.sched_priority = (int32_t)(min_prio + (pri * priostep)); + break; } rv = pthread_setschedparam(m_thread, policy, ¶m); switch (rv) { - case ESRCH: + case ESRCH: /* The value specified by thread does not refer to an existing thread. */ - case EINVAL: - /* The value specified by policy or one of the scheduling parameters - * associated with the scheduling policy policy is invalid. - */ - case ENOTSUP: - /* An attempt was made to set the policy or scheduling parameters to an - * unsupported value. - * An attempt was made to dynamically change the scheduling policy to - * SCHED_SPORADIC, and the implementation does not support this change. - */ - case EPERM: - /* The caller does not have the appropriate permission to set either the - * scheduling parameters or the scheduling policy of the specified thread. - * The implementation does not allow the application to modify one of the - * parameters to the value specified. + case EINVAL: + /* The value specified by policy or one of the scheduling parameters + associated with the scheduling policy policy is invalid. + */ + case ENOTSUP: + /* An attempt was made to set the policy or scheduling parameters to an + unsupported value. + An attempt was made to dynamically change the scheduling policy to + SCHED_SPORADIC, and the implementation does not support this change. + */ + case EPERM: + /* The caller does not have the appropriate permission to set either the + scheduling parameters or the scheduling policy of the specified thread. + The implementation does not allow the application to modify one of the + parameters to the value specified. */ - return false; - default: - break; + return false; + default: + break; } return true; @@ -291,7 +294,7 @@ bool StandardThread::startThread(const char* name) m_stop = false; ::ResetEvent(m_stopHandle); ::SetEvent(m_running); - m_thread = xsStartThread(&threadInit,this,&m_threadId); + m_thread = xsStartThread(&threadInit, this, &m_threadId); if (m_thread == XSENS_INVALID_THREAD) { ::ResetEvent(m_running); @@ -387,14 +390,14 @@ void StandardThread::stopThread(void) noexcept { switch (errno) { - case EINVAL: + case EINVAL: /* no joinable thread found */ - case ESRCH: + case ESRCH: /* no thread fits thread id */ - case EDEADLK: + case EDEADLK: /* deadlock or trying to join self */ - default: - break; + default: + break; } } @@ -415,9 +418,9 @@ XSENS_THREAD_RETURN StandardThread::threadInit(void* obj) #ifndef _WIN32 /*! \brief Cleanup the thread by calling the exit function */ -void StandardThread::threadCleanup(void *obj) +void StandardThread::threadCleanup(void* obj) { - ((StandardThread *)obj)->exitFunction(); + ((StandardThread*)obj)->exitFunction(); } #endif @@ -435,7 +438,7 @@ void StandardThread::threadMain(void) while (!m_stop) { // sleep max 100ms at a time so we can terminate the thread quickly if necessary - int64_t timePassed = XsTimeStamp::nowMs()-sleepStart; + int64_t timePassed = XsTimeStamp::nowMs() - sleepStart; int32_t remaining = rv - (int32_t) timePassed; if (remaining > 100) XsTime::msleep(100); @@ -458,9 +461,9 @@ void StandardThread::threadMain(void) WatchDogThread::WatchDogThread(WatchDogFunction func, void* param) : m_thread(XSENS_INVALID_THREAD) #ifdef _WIN32 - , m_stop(::CreateEvent(NULL,TRUE,FALSE,NULL)) - , m_running(::CreateEvent(NULL,TRUE,FALSE,NULL)) - , m_reset(::CreateEvent(NULL,TRUE,FALSE,NULL)) + , m_stop(::CreateEvent(NULL, TRUE, FALSE, NULL)) + , m_running(::CreateEvent(NULL, TRUE, FALSE, NULL)) + , m_reset(::CreateEvent(NULL, TRUE, FALSE, NULL)) #else , m_running(false) , m_reset(false) @@ -500,7 +503,7 @@ bool WatchDogThread::isAlive(void) volatile const noexcept { #ifdef _WIN32 DWORD exitCode; - if (::GetExitCodeThread(m_thread,&exitCode)) + if (::GetExitCodeThread(m_thread, &exitCode)) return (exitCode == STILL_ACTIVE); #else if (m_thread == XSENS_INVALID_THREAD) @@ -516,24 +519,24 @@ bool WatchDogThread::isRunning(void) volatile const noexcept if (!isAlive()) return false; #ifdef _WIN32 - switch(::WaitForSingleObject(m_running,0)) + switch (::WaitForSingleObject(m_running, 0)) { - case WAIT_ABANDONED: - case WAIT_TIMEOUT: - return false; - case WAIT_OBJECT_0: - switch(::WaitForSingleObject(m_stop,0)) - { case WAIT_ABANDONED: case WAIT_TIMEOUT: - return true; - case WAIT_OBJECT_0: return false; + case WAIT_OBJECT_0: + switch (::WaitForSingleObject(m_stop, 0)) + { + case WAIT_ABANDONED: + case WAIT_TIMEOUT: + return true; + case WAIT_OBJECT_0: + return false; + default: + return false; + } default: return false; - } - default: - return false; } #else return m_running; @@ -564,7 +567,7 @@ bool WatchDogThread::startTimer(uint32_t timeout, const char* name) ::ResetEvent(m_stop); ::SetEvent(m_running); ::ResetEvent(m_reset); - m_thread = xsStartThread(&threadInit,this,&m_threadId); + m_thread = xsStartThread(&threadInit, this, &m_threadId); if (m_thread == XSENS_INVALID_THREAD) { ::ResetEvent(m_running); @@ -611,10 +614,12 @@ bool WatchDogThread::stopTimer(void) noexcept rv = pthread_getschedparam(m_thread, &policy, ¶m); if (rv) { - switch(errno) + switch (errno) { - case ESRCH: return false; - default: break; + case ESRCH: + return false; + default: + break; } } param.sched_priority = sched_get_priority_max(policy); @@ -624,23 +629,26 @@ bool WatchDogThread::stopTimer(void) noexcept { switch (errno) { - case ESRCH: - case EINVAL: - case ENOTSUP: - case EPERM: return false; - default: break; + case ESRCH: + case EINVAL: + case ENOTSUP: + case EPERM: + return false; + default: + break; } } rv = pthread_join(m_thread, NULL); if (rv) { - switch(errno) + switch (errno) { - case EINVAL: - case ESRCH: - case EDEADLK: - default: break; + case EINVAL: + case ESRCH: + case EDEADLK: + default: + break; } } m_running = false; @@ -662,7 +670,7 @@ XSENS_THREAD_RETURN WatchDogThread::threadInit(void* obj) void WatchDogThread::threadMain(void) { - XsTimeStamp toTime((int64_t) (XsTimeStamp::now().msTime() + m_timeout)); + XsTimeStamp toTime((int64_t)(XsTimeStamp::now().msTime() + m_timeout)); #ifdef _WIN32 HANDLE hlist[2]; @@ -671,28 +679,28 @@ void WatchDogThread::threadMain(void) while (go) { #ifdef _WIN32 - uint32_t timeout = (uint32_t) (toTime - XsTimeStamp::now()).msTime(); + uint32_t timeout = (uint32_t)(toTime - XsTimeStamp::now()).msTime(); if (timeout > m_timeout) break; hlist[0] = m_reset; hlist[1] = m_stop; - switch(::WaitForMultipleObjects(2,hlist,FALSE,timeout)) + switch (::WaitForMultipleObjects(2, hlist, FALSE, timeout)) { - case WAIT_OBJECT_0: // m_reset - // received an update for the timer - toTime = XsTimeStamp::now() + XsTimeStamp((int) m_timeout); - ::ResetEvent(m_reset); - continue; - - case WAIT_OBJECT_0+1: // m_stop - case WAIT_ABANDONED: - default: - go = false; - break; + case WAIT_OBJECT_0: // m_reset + // received an update for the timer + toTime = XsTimeStamp::now() + XsTimeStamp((int) m_timeout); + ::ResetEvent(m_reset); + continue; + + case WAIT_OBJECT_0+1: // m_stop + case WAIT_ABANDONED: + default: + go = false; + break; - case WAIT_TIMEOUT: - break; + case WAIT_TIMEOUT: + break; } #else Lock lock(&m_mutex, true); @@ -747,19 +755,19 @@ bool WatchDogThread::resetTimer(uint32_t timeout) } #ifdef _WIN32 -Semaphore::Semaphore(int32_t initVal, uint32_t nofOtherHandles, HANDLE *otherHandles) +Semaphore::Semaphore(int32_t initVal, uint32_t nofOtherHandles, HANDLE* otherHandles) { - m_nofHandles = nofOtherHandles+1; + m_nofHandles = nofOtherHandles + 1; m_handleList = new HANDLE[m_nofHandles]; - for (uint32_t i=0; i #ifndef __GNUC__ -#pragma warning(disable: 4127) + #pragma warning(disable: 4127) #endif -namespace xsens { - /*! \class StandardThread - \brief A class for a standard thread that has to perform the same action repeatedly. - \details The class has three virtual functions, of which the innerFunction is the most important. - innerFunction gets called repeatedly and is expected to return so that StandardThread can - check for thread termination. - */ - class StandardThread { - private: - XsThread m_thread; - XsThreadPriority m_priority; +namespace xsens +{ +/*! \class StandardThread + \brief A class for a standard thread that has to perform the same action repeatedly. + \details The class has three virtual functions, of which the innerFunction is the most important. + innerFunction gets called repeatedly and is expected to return so that StandardThread can + check for thread termination. +*/ +class StandardThread +{ +private: + XsThread m_thread; + XsThreadPriority m_priority; #ifdef _WIN32 - mutable xsens::Mutex m_mux; + mutable xsens::Mutex m_mux; #endif - protected: - volatile std::atomic_bool m_stop; //!< Indicates that the thread should stop. Derived classes should check isTerminating() instead of directly polling this value when checking if the thread should stop. However, there are some cases (tests, SignallingThread) where direct access from within the class is desired, which is why the vlaue is protected instead of private. - volatile std::atomic_bool m_yieldOnZeroSleep; //!< When true, a sleep value of 0 returned by innerFunction will trigger a thread yield operation. When false, the next cycle is started immediately. +protected: + volatile std::atomic_bool m_stop; //!< Indicates that the thread should stop. Derived classes should check isTerminating() instead of directly polling this value when checking if the thread should stop. However, there are some cases (tests, SignallingThread) where direct access from within the class is desired, which is why the vlaue is protected instead of private. + volatile std::atomic_bool m_yieldOnZeroSleep; //!< When true, a sleep value of 0 returned by innerFunction will trigger a thread yield operation. When false, the next cycle is started immediately. #ifdef _WIN32 - HANDLE m_stopHandle; //!< Duplicates m_stop functionality for external dependent classes such as Semaphore - HANDLE m_running; //!< Indicates that the thread is running - private: - XsThreadId m_threadId; + HANDLE m_stopHandle; //!< Duplicates m_stop functionality for external dependent classes such as Semaphore + HANDLE m_running; //!< Indicates that the thread is running +private: + XsThreadId m_threadId; #else - pthread_attr_t m_attr; //!< Duplicates m_stop functionality for external dependent classes such as Semaphore - bool m_running; //!< Indicates that the thread is running - private: + pthread_attr_t m_attr; //!< Duplicates m_stop functionality for external dependent classes such as Semaphore + bool m_running; //!< Indicates that the thread is running +private: #endif - char* m_name; - static XSENS_THREAD_RETURN threadInit(void *obj); + char* m_name; + static XSENS_THREAD_RETURN threadInit(void* obj); #ifndef _WIN32 - static void threadCleanup(void *obj); + static void threadCleanup(void* obj); #endif - void threadMain(void); - protected: - //! Virtual initialization function - virtual void initFunction(void) { } + void threadMain(void); +protected: + //! Virtual initialization function + virtual void initFunction(void) { } - //! Virtual exit function - virtual void exitFunction(void) { } + //! Virtual exit function + virtual void exitFunction(void) { } - //! Virtual inner function - virtual int32_t innerFunction(void) { return 0; } + //! Virtual inner function + virtual int32_t innerFunction(void) + { + return 0; + } - //! Return the thread handle - inline XsThread threadHandle() const { return m_thread; } - public: - StandardThread(); - virtual ~StandardThread(); + //! Return the thread handle + inline XsThread threadHandle() const + { + return m_thread; + } +public: + StandardThread(); + virtual ~StandardThread(); - bool startThread(const char* name=NULL); - virtual void signalStopThread(void); - void stopThread(void) noexcept; - bool isAlive(void) volatile const noexcept; - bool isRunning(void) volatile const noexcept; - bool setPriority(XsThreadPriority pri); - bool isTerminating() volatile const noexcept; + bool startThread(const char* name = NULL); + virtual void signalStopThread(void); + void stopThread(void) noexcept; + bool isAlive(void) volatile const noexcept; + bool isRunning(void) volatile const noexcept; + bool setPriority(XsThreadPriority pri); + bool isTerminating() volatile const noexcept; - //! \returns The thread ID - XsThreadId getThreadId(void) const - { + //! \returns The thread ID + XsThreadId getThreadId(void) const + { #ifdef _WIN32 - return m_threadId; + return m_threadId; #else - return m_thread; + return m_thread; #endif - } + } #ifdef _WIN32 - void terminateThread(); + void terminateThread(); #endif - }; - #define XSENS_THREAD_CHECK() if (isTerminating()) return 0 +}; +#define XSENS_THREAD_CHECK() if (isTerminating()) return 0 #ifdef ANDROID -#define CDECL_XS + #define CDECL_XS #else -#define CDECL_XS __cdecl + #define CDECL_XS __cdecl #endif #ifndef SWIG - typedef void (CDECL_XS *WatchDogFunction)(void*); +typedef void (CDECL_XS* WatchDogFunction)(void*); - /*! \class WatchDogThread - \brief A class that keeps an eye on a threads timer - */ - class WatchDogThread { - private: - XsThread m_thread; +/*! \class WatchDogThread + \brief A class that keeps an eye on a threads timer +*/ +class WatchDogThread +{ +private: + XsThread m_thread; #ifdef _WIN32 - HANDLE m_stop; - HANDLE m_running; - HANDLE m_reset; + HANDLE m_stop; + HANDLE m_running; + HANDLE m_reset; #else - pthread_attr_t m_attr; - Mutex m_mutex; - bool m_running; - bool m_reset; - bool m_stop; + pthread_attr_t m_attr; + Mutex m_mutex; + bool m_running; + bool m_reset; + bool m_stop; #endif - volatile std::atomic m_timeout; - WatchDogFunction m_func; - void* m_param; - char* m_name; - static XSENS_THREAD_RETURN threadInit(void* obj); + volatile std::atomic m_timeout; + WatchDogFunction m_func; + void* m_param; + char* m_name; + static XSENS_THREAD_RETURN threadInit(void* obj); + + XsThreadId m_threadId; + void threadMain(void); + bool isAlive(void) volatile const noexcept; + bool isRunning(void) volatile const noexcept; +public: - XsThreadId m_threadId; - void threadMain(void); - bool isAlive(void) volatile const noexcept; - bool isRunning(void) volatile const noexcept; - public: + /*! \brief Constructor + */ + WatchDogThread(WatchDogFunction func, void* param = NULL); - /*! \brief Constructor - */ - WatchDogThread(WatchDogFunction func, void* param = NULL); + /*! \brief Destructor + */ + ~WatchDogThread(); - /*! \brief Destructor - */ - ~WatchDogThread(); + bool resetTimer(uint32_t timeout = 0); + bool startTimer(uint32_t timeout = 10000, const char* name = NULL); + bool stopTimer(void) noexcept; - bool resetTimer(uint32_t timeout = 0); - bool startTimer(uint32_t timeout = 10000, const char* name=NULL); - bool stopTimer(void) noexcept; + //! \returns The thread ID + XsThreadId getThreadId(void) const + { + return m_threadId; + } +}; - //! \returns The thread ID - XsThreadId getThreadId(void) const { return m_threadId; } +/*! \class TaskThread + \brief Class for handling small tasks + \details Use this class if you have small tasks that need to be performed out of the main thread + The thread uses tasks supplied in a TaskType struct. +*/ +class TaskThread : public StandardThread +{ +public: + typedef void (CDECL_XS* TaskFunction)(void*); //!< A function prototype for a task +private: + struct TaskType + { + TaskFunction m_function; + void* m_param; }; - /*! \class TaskThread - \brief Class for handling small tasks - \details Use this class if you have small tasks that need to be performed out of the main thread - The thread uses tasks supplied in a TaskType struct. - */ - class TaskThread : public StandardThread { - public: - typedef void (CDECL_XS *TaskFunction)(void*); //!< A function prototype for a task - private: - struct TaskType { - TaskFunction m_function; - void* m_param; - }; - - std::deque m_queue; - Mutex m_safe; - bool m_inFunc; - protected: + std::deque m_queue; + Mutex m_safe; + bool m_inFunc; +protected: - /*! \brief The inner function of the task thread. - \details The function checks if there is a task in the queue and executes it. - Then it returns to the StandardThread main loop to check for termination. - If there are no tasks in the queue, the thread will terminate itself. - \returns Not 0 if successful - */ - virtual int32_t innerFunction(void) - { - Lock safety(&m_safe); - if (m_queue.size() > 0) - { - TaskType& task = m_queue.front(); - m_queue.pop_front(); - m_inFunc = true; - safety.unlock(); - task.m_function(task.m_param); - m_inFunc = false; - return 0; // no sleep - } - // notify thread to terminate when there are no more tasks - stopThread(); - return 0; - } - public: - TaskThread() : m_inFunc(false) {} - ~TaskThread() + /*! \brief The inner function of the task thread. + \details The function checks if there is a task in the queue and executes it. + Then it returns to the StandardThread main loop to check for termination. + If there are no tasks in the queue, the thread will terminate itself. + \returns Not 0 if successful + */ + virtual int32_t innerFunction(void) + { + Lock safety(&m_safe); + if (m_queue.size() > 0) { - while(isRunning() && getLength()) - XsTime::msleep(100); + TaskType& task = m_queue.front(); + m_queue.pop_front(); + m_inFunc = true; + safety.unlock(); + task.m_function(task.m_param); + m_inFunc = false; + return 0; // no sleep } + // notify thread to terminate when there are no more tasks + stopThread(); + return 0; + } +public: + TaskThread() : m_inFunc(false) {} + ~TaskThread() + { + while (isRunning() && getLength()) + XsTime::msleep(100); + } - /*! \brief Adds a task to a queue - \param func The function to add - \param param The parameters to add - */ - void addTask(TaskFunction func, void* param) - { - Lock safety(&m_safe); - TaskType tmp = {func,param}; - m_queue.push_back(tmp); - } + /*! \brief Adds a task to a queue + \param func The function to add + \param param The parameters to add + */ + void addTask(TaskFunction func, void* param) + { + Lock safety(&m_safe); + TaskType tmp = {func, param}; + m_queue.push_back(tmp); + } - //! \returns The length of a queue - int32_t getLength(void) noexcept - { - Lock safety(&m_safe); - return (int32_t) m_queue.size() + (m_inFunc?1:0); - } + //! \returns The length of a queue + int32_t getLength(void) noexcept + { + Lock safety(&m_safe); + return (int32_t) m_queue.size() + (m_inFunc ? 1 : 0); + } - //! \brief Clears a queue - void clear(void) - { - Lock safety(&m_safe); - m_queue.clear(); - } - }; + //! \brief Clears a queue + void clear(void) + { + Lock safety(&m_safe); + m_queue.clear(); + } +}; +#endif +#ifndef __APPLE__ +// the std::function is not properly available in all iOS versions +/*! \class ThreadedFunction + \brief A class that will run a single (lambda) function in a separate thread. +*/ +class ThreadedFunction : public StandardThread +{ + std::function m_func; +public: + /*! \brief Construct the object + \param f The desired (lambda) function to run + \param runNow Start the thread immediately (default = true) + */ + ThreadedFunction(std::function const& f, bool runNow = true) + : m_func(f) + { + if (runNow) + startThread(); + } + ~ThreadedFunction() + { + stopThread(); + } + + /*! \brief The internal thread function, runs the supplied function */ + int32_t innerFunction(void) override final + { + m_func(); + signalStopThread(); + return 0; + } +}; #endif } // namespace xsens #ifndef __GNUC__ -#pragma warning(default: 4127) + #pragma warning(default: 4127) #endif #endif diff --git a/lib/xspublic/xscommon/xbus.c b/lib/xspublic/xscommon/xbus.c new file mode 100644 index 0000000..2a606ff --- /dev/null +++ b/lib/xspublic/xscommon/xbus.c @@ -0,0 +1,198 @@ + +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions, and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions, and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the names of the copyright holders nor the names of their contributors +// may be used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY +// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL +// THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT +// OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY OR +// TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.THE LAWS OF THE NETHERLANDS +// SHALL BE EXCLUSIVELY APPLICABLE AND ANY DISPUTES SHALL BE FINALLY SETTLED UNDER THE RULES +// OF ARBITRATION OF THE INTERNATIONAL CHAMBER OF COMMERCE IN THE HAGUE BY ONE OR MORE +// ARBITRATORS APPOINTED IN ACCORDANCE WITH SAID RULES. +// + + +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions, and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions, and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the names of the copyright holders nor the names of their contributors +// may be used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY +// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL +// THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT +// OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY OR +// TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.THE LAWS OF THE NETHERLANDS +// SHALL BE EXCLUSIVELY APPLICABLE AND ANY DISPUTES SHALL BE FINALLY SETTLED UNDER THE RULES +// OF ARBITRATION OF THE INTERNATIONAL CHAMBER OF COMMERCE IN THE HAGUE BY ONE OR MORE +// ARBITRATORS APPOINTED IN ACCORDANCE WITH SAID RULES. +// + +#include "xbus.h" + + +/*! \brief Returns true if the preamble equeals 0xFA, false othersise +*/ +bool Xbus_checkPreamble(const uint8_t* xbusMessage) +{ + return xbusMessage[OFFSET_TO_PREAMBLE] == XBUS_PREAMBLE; +} + +/*! \brief Returns xbus Bus identifier +*/ +int Xbus_getBusId(const uint8_t* xbusMessage) +{ + return (xbusMessage[OFFSET_TO_BID] & 0xff); +} + +/*! \brief Sets xbus Bus identifier +*/ +void Xbus_setBusId(uint8_t* xbusMessage, uint8_t busId) +{ + xbusMessage[OFFSET_TO_BID] = busId & 0xff; +} + +/*! \brief Returns xbus Message identifier +*/ +int Xbus_getMessageId(const uint8_t* xbusMessage) +{ + return (xbusMessage[OFFSET_TO_MID] & 0xff); +} + +/*! \brief Sets xbus Message identifier +*/ +void Xbus_setMessageId(uint8_t* xbusMessage, uint8_t messageId) +{ + xbusMessage[OFFSET_TO_MID] = messageId & 0xff; +} + +/*! \brief Returns xbus message (payload) length +*/ +int Xbus_getPayloadLength(const uint8_t* xbusMessage) +{ + int length = xbusMessage[OFFSET_TO_LEN] & 0xff; + if (length != LENGTH_EXTENDER_BYTE) + return length; + else + { + int result = (xbusMessage[OFFSET_TO_LEN + 2] & 0xff); + result += (xbusMessage[OFFSET_TO_LEN + 1] & 0xff) << 8; + return result; + } +} + +/*! \brief Sets xbus message (payload) length +*/ +void Xbus_setPayloadLength(uint8_t* xbusMessage, uint16_t payloadLength) +{ + if (payloadLength < 255) + xbusMessage[OFFSET_TO_LEN] = payloadLength & 0xff; + else + { + xbusMessage[OFFSET_TO_LEN] = LENGTH_EXTENDER_BYTE; + xbusMessage[OFFSET_TO_LEN + 1] = (payloadLength >> 8) & 0xff; + xbusMessage[OFFSET_TO_LEN + 2] = payloadLength & 0xff; + } +} + +/*! \brief Initialize a xbus message with BID, MID and Length +*/ +void Xbus_message(uint8_t* xbusMessage, uint8_t bid, uint8_t mid, uint16_t len) +{ + xbusMessage[0] = 0xFA; + Xbus_setBusId(xbusMessage, bid); + Xbus_setMessageId(xbusMessage, mid); + Xbus_setPayloadLength(xbusMessage, len); +} + +/*! \brief Returns total length of xbus message (header + payload + checksum) +*/ +int Xbus_getRawLength(const uint8_t* xbusMessage) +{ + int rtrn = Xbus_getPayloadLength(xbusMessage); + + if ((xbusMessage[OFFSET_TO_LEN] & 0xff) == LENGTH_EXTENDER_BYTE) + rtrn += 7; + else + rtrn += 5; + return rtrn; +} + +/*! \brief Returns pointer to payload of an xbus message +*/ +uint8_t* Xbus_getPointerToPayload(uint8_t* xbusMessage) +{ + if ((xbusMessage[OFFSET_TO_LEN] & 0xff) == LENGTH_EXTENDER_BYTE) + return xbusMessage + OFFSET_TO_PAYLOAD_EXT; + else + return xbusMessage + OFFSET_TO_PAYLOAD; +} + +/*! \brief Returns a const pointer to payload of an xbus message +*/ +uint8_t const* Xbus_getConstPointerToPayload(uint8_t const* xbusMessage) +{ + return Xbus_getPointerToPayload((uint8_t*)xbusMessage); +} + + +/*! \brief Inserts the correct checksum in xbus message +*/ +void Xbus_insertChecksum(uint8_t* xbusMessage) +{ + int nBytes = Xbus_getRawLength(xbusMessage); + + uint8_t checksum = 0; + for (int i = 0; i < nBytes - 2; i++) + checksum -= xbusMessage[1 + i]; + + xbusMessage[nBytes - 1] = checksum; +} + +/*! \brief Verifies the checksum of aon xbus message +*/ +bool Xbus_verifyChecksum(const uint8_t* xbusMessage) +{ + int nBytes = Xbus_getRawLength(xbusMessage); + uint8_t checksum = 0; + for (int n = 1; n < nBytes; n++) + checksum += (xbusMessage[n] & 0xff); + checksum &= 0xff; + return (checksum == 0); +} + + diff --git a/lib/xspublic/xscommon/xbus.h b/lib/xspublic/xscommon/xbus.h new file mode 100644 index 0000000..cb012a8 --- /dev/null +++ b/lib/xspublic/xscommon/xbus.h @@ -0,0 +1,122 @@ + +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions, and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions, and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the names of the copyright holders nor the names of their contributors +// may be used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY +// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL +// THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT +// OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY OR +// TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.THE LAWS OF THE NETHERLANDS +// SHALL BE EXCLUSIVELY APPLICABLE AND ANY DISPUTES SHALL BE FINALLY SETTLED UNDER THE RULES +// OF ARBITRATION OF THE INTERNATIONAL CHAMBER OF COMMERCE IN THE HAGUE BY ONE OR MORE +// ARBITRATORS APPOINTED IN ACCORDANCE WITH SAID RULES. +// + + +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions, and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions, and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the names of the copyright holders nor the names of their contributors +// may be used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY +// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL +// THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT +// OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY OR +// TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.THE LAWS OF THE NETHERLANDS +// SHALL BE EXCLUSIVELY APPLICABLE AND ANY DISPUTES SHALL BE FINALLY SETTLED UNDER THE RULES +// OF ARBITRATION OF THE INTERNATIONAL CHAMBER OF COMMERCE IN THE HAGUE BY ONE OR MORE +// ARBITRATORS APPOINTED IN ACCORDANCE WITH SAID RULES. +// + +#ifndef XBUS_H +#define XBUS_H + +#include +#include +#include + + +#ifdef __cplusplus +extern "C" { +#endif + +#define OFFSET_TO_PREAMBLE 0 +#define OFFSET_TO_BID 1 +#define OFFSET_TO_MID 2 +#define OFFSET_TO_LEN 3 +#define OFFSET_TO_LEN_EXT_HI 4 +#define OFFSET_TO_LEN_EXT_LO 5 +#define OFFSET_TO_PAYLOAD 4 +#define OFFSET_TO_PAYLOAD_EXT 6 +#define XBUS_CHECKSUM_SIZE 1 +#define LENGTH_EXTENDER_BYTE 0xFF +#define XBUS_PREAMBLE 0xFA +#define XBUS_MASTERDEVICE 0xFF +#define XBUS_EXTENDED_LENGTH 0xFF + + + + +bool Xbus_checkPreamble(const uint8_t* xbusMessage); + +int Xbus_getBusId(const uint8_t* xbusMessage); +void Xbus_setBusId(uint8_t* xbusMessage, uint8_t busId); + +int Xbus_getMessageId(const uint8_t* xbusMessage); +void Xbus_setMessageId(uint8_t* xbusMessage, uint8_t messageId); + +int Xbus_getPayloadLength(const uint8_t* xbusMessage); +void Xbus_setPayloadLength(uint8_t* xbusMessage, uint16_t payloadLength); + +void Xbus_message(uint8_t* xbusMessage, uint8_t bid, uint8_t mid, uint16_t len); + +int Xbus_getRawLength(const uint8_t* xbusMessage); + +uint8_t* Xbus_getPointerToPayload(uint8_t* xbusMessage); +uint8_t const* Xbus_getConstPointerToPayload(uint8_t const* xbusMessage); + + +void Xbus_insertChecksum(uint8_t* xbusMessage); +bool Xbus_verifyChecksum(const uint8_t* xbusMessage); + +#ifdef __cplusplus +} +//} +#endif + + +#endif diff --git a/lib/xspublic/xscommon/xbusparser.c b/lib/xspublic/xscommon/xbusparser.c new file mode 100644 index 0000000..4b83695 --- /dev/null +++ b/lib/xspublic/xscommon/xbusparser.c @@ -0,0 +1,192 @@ + +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions, and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions, and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the names of the copyright holders nor the names of their contributors +// may be used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY +// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL +// THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT +// OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY OR +// TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.THE LAWS OF THE NETHERLANDS +// SHALL BE EXCLUSIVELY APPLICABLE AND ANY DISPUTES SHALL BE FINALLY SETTLED UNDER THE RULES +// OF ARBITRATION OF THE INTERNATIONAL CHAMBER OF COMMERCE IN THE HAGUE BY ONE OR MORE +// ARBITRATORS APPOINTED IN ACCORDANCE WITH SAID RULES. +// + + +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions, and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions, and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the names of the copyright holders nor the names of their contributors +// may be used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY +// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL +// THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT +// OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY OR +// TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.THE LAWS OF THE NETHERLANDS +// SHALL BE EXCLUSIVELY APPLICABLE AND ANY DISPUTES SHALL BE FINALLY SETTLED UNDER THE RULES +// OF ARBITRATION OF THE INTERNATIONAL CHAMBER OF COMMERCE IN THE HAGUE BY ONE OR MORE +// ARBITRATORS APPOINTED IN ACCORDANCE WITH SAID RULES. +// + +#include "xbusparser.h" +#include +#include +#include "xbus.h" + + +/*! \brief Initializes an XbusParser instance +*/ +void XbusParser_init(XbusParser* obj, unsigned char* buffer, int bufferSize) +{ + obj->m_rxBufferSize = bufferSize; + obj->m_rxBuffer = buffer; + obj->m_idxWrite = 0; + obj->m_state = haveNothing; + obj->m_lastError = RES_Ok; +} + + +/*! \brief Insert a new byte in the parser + \param obj The XbusParser instance + \param byte The byte to insert + \param messageSize Pointer to an integer in which to return the current message size +*/ +enum Result XbusParser_insertByte(XbusParser* obj, unsigned char byte, int* messageSize) +{ + enum Result result = RES_Ok; + bool incrementIdxWrite = true; + + if (obj->m_state != haveNothing) // if we have at least a preamble, + obj->m_rxBuffer[obj->m_idxWrite] = byte; // store the received byte in the buffer. + + switch (obj->m_state) + { + case haveNothing: + if (byte == XBUS_PREAMBLE) // if we have a preamble, + { + obj->m_idxWrite = 0; // reset the write index + obj->m_rxBuffer[obj->m_idxWrite] = byte; // store the byte + obj->m_state = havePreamble; // update our state + obj->m_totalLength = 1; // reset m_totalLength + obj->m_checksum = 0; // reset m_checksum + result = RES_FoundPreamble; + } + else + { + incrementIdxWrite = false; // don't increment m_idxWrite if we are not in a xbus message + result = RES_SpuriousByte; + } + break; + + case havePreamble: + obj->m_totalLength++; + obj->m_checksum += obj->m_rxBuffer[obj->m_idxWrite]; + if (obj->m_totalLength == OFFSET_TO_LEN + 1) + { + if (obj->m_rxBuffer[obj->m_idxWrite] != LENGTH_EXTENDER_BYTE) + { + obj->m_state = haveLength; + obj->m_payloadLength = obj->m_rxBuffer[obj->m_idxWrite]; + obj->m_offsetToPayload = OFFSET_TO_PAYLOAD; + } + else + { + obj->m_state = haveLengthExtenderByte; + obj->m_offsetToPayload = OFFSET_TO_PAYLOAD_EXT; + } + } + break; + + case haveLengthExtenderByte: + obj->m_totalLength++; + obj->m_checksum += obj->m_rxBuffer[obj->m_idxWrite]; + if (obj->m_totalLength == OFFSET_TO_LEN + 2) + obj->m_payloadLength = 256 * obj->m_rxBuffer[obj->m_idxWrite]; + if (obj->m_totalLength == OFFSET_TO_LEN + 3) + { + obj->m_payloadLength += obj->m_rxBuffer[obj->m_idxWrite]; + obj->m_state = haveLength; + } + break; + + case haveLength: + obj->m_totalLength++; + obj->m_checksum += obj->m_rxBuffer[obj->m_idxWrite]; + if (obj->m_totalLength == (obj->m_offsetToPayload + obj->m_payloadLength + 1)) // if the current byte is the checksum + { + if ((obj->m_checksum & 0x00FF) == 0) // if checksum ok + obj->m_state = haveMessage; + else // if checksum error, + { + obj->m_state = haveNothing; + return RES_CheckSumError; + } + } + break; + + case haveMessage: + break; + + case haveError: + return obj->m_lastError; + + default: + break; + } + + if (obj->m_state == haveMessage) + { + *messageSize = obj->m_totalLength; + return RES_MessageReceived; + } + + if (incrementIdxWrite) + { + if (++obj->m_idxWrite >= obj->m_rxBufferSize) + { + //Error condition if no packet received yet (report buffer overflow, restart ? Keep parser in error state) + //User must restart the parser to clear error state + obj->m_state = haveError; + obj->m_lastError = RES_BufferOverflow; + return obj->m_lastError; + } + } + + return result; +} + + diff --git a/lib/xspublic/xscommon/xbusparser.h b/lib/xspublic/xscommon/xbusparser.h new file mode 100644 index 0000000..563f973 --- /dev/null +++ b/lib/xspublic/xscommon/xbusparser.h @@ -0,0 +1,119 @@ + +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions, and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions, and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the names of the copyright holders nor the names of their contributors +// may be used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY +// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL +// THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT +// OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY OR +// TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.THE LAWS OF THE NETHERLANDS +// SHALL BE EXCLUSIVELY APPLICABLE AND ANY DISPUTES SHALL BE FINALLY SETTLED UNDER THE RULES +// OF ARBITRATION OF THE INTERNATIONAL CHAMBER OF COMMERCE IN THE HAGUE BY ONE OR MORE +// ARBITRATORS APPOINTED IN ACCORDANCE WITH SAID RULES. +// + + +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions, and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions, and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the names of the copyright holders nor the names of their contributors +// may be used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY +// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL +// THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT +// OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY OR +// TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.THE LAWS OF THE NETHERLANDS +// SHALL BE EXCLUSIVELY APPLICABLE AND ANY DISPUTES SHALL BE FINALLY SETTLED UNDER THE RULES +// OF ARBITRATION OF THE INTERNATIONAL CHAMBER OF COMMERCE IN THE HAGUE BY ONE OR MORE +// ARBITRATORS APPOINTED IN ACCORDANCE WITH SAID RULES. +// + +#ifndef XBUSPARSER_H +#define XBUSPARSER_H + +#ifdef __cplusplus +extern "C" { +#endif + +enum State +{ + haveNothing, + havePreamble, + haveLengthExtenderByte, + haveLength, + haveMessage, + haveError, +}; + +enum Result +{ + RES_Ok, + RES_MessageReceived, + RES_BufferOverflow, + RES_CheckSumError, + RES_InvalidLength, + RES_FoundPreamble, + RES_SpuriousByte, +}; + +/*! \brief XbusParser object definition +*/ +typedef struct +{ + unsigned char* m_rxBuffer; /*!< Receive buffer*/ + int m_rxBufferSize; /*!< Receive buffer size*/ + int m_idxWrite; /*!< Write index*/ + int m_totalLength; /*!< Total length*/ + int m_payloadLength; /*!< Payload length*/ + int m_offsetToPayload; /*!< Offset to payload*/ + unsigned short m_checksum; /*!< Running checksum*/ + enum Result m_lastError; /*!< Last error*/ + enum State m_state; /*!< State*/ +} XbusParser; + + +void XbusParser_init(XbusParser* obj, unsigned char* buffer, int bufferSize); + +enum Result XbusParser_insertByte(XbusParser* obj, unsigned char byte, int* messageSize); + + +#ifdef __cplusplus +} +#endif + + +#endif diff --git a/lib/xspublic/xscommon/xprintf.cpp b/lib/xspublic/xscommon/xprintf.cpp index edd1404..8ceb760 100644 --- a/lib/xspublic/xscommon/xprintf.cpp +++ b/lib/xspublic/xscommon/xprintf.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -65,26 +65,61 @@ #include "xprintf.h" #include #include -#include -#include -/*! \brief printf-style formatting for std::string. Use the xprintf macro instead of using this function directly +#define XPRINT_FAST_BUFFER_SIZE 256 + +/*! \brief printf-style formatting for std::string (internal). Use the xprintf macro instead of using this function directly \param fmt_str The printf-style format string - \param bSize The size of the temporary buffer. This is allocated on the heap so its default value of 65536 - as suppplied by the macro xprintf is usually ok. The bSize includes the terminating 0 character. + \param ap The variadic arguments, properly initialized with va_start. The function will call va_end on this argument before exiting. */ -std::string _xprintf_b(std::string const& fmt_str, int bSize, ...) +inline std::string _xprintf_ap(char const* fmt_str, va_list ap) { - assert(bSize > 0); - std::unique_ptr buffer(new char[bSize]); + std::string rv; - va_list ap; - va_start(ap, bSize); - int nResult = vsnprintf(static_cast(buffer.get()), (unsigned int) bSize, fmt_str.c_str(), ap); + // try using a fast fixed size buffer first + char fastBuffer[XPRINT_FAST_BUFFER_SIZE]; + va_list apc; + va_copy(apc, ap); + int nResult = vsnprintf(fastBuffer, XPRINT_FAST_BUFFER_SIZE, fmt_str, ap); va_end(ap); + if (nResult >= 0 && nResult < XPRINT_FAST_BUFFER_SIZE) + rv = fastBuffer; + else if (nResult < 0) + { + // fast buffer didn't work because of some parsing error, return the original format string + rv = fmt_str; + } + else + { + // fast buffer didn't work, but we now do know the size of our final string + std::unique_ptr buffer(new char[(size_t)((ptrdiff_t)nResult + 1)]); + nResult = vsnprintf(buffer.get(), (size_t)((ptrdiff_t)nResult + 1), fmt_str, apc); + + if (nResult < 0) + rv = fmt_str; + else + rv = buffer.get(); + } + va_end(apc); + return rv; +} - if (nResult < 0 || nResult >= bSize) - throw std::overflow_error("Unable to fit output into buffer"); +/*! \brief printf-style formatting for std::string. Use the xprintf macro instead of using this function directly + \param fmt_str The printf-style format string +*/ +std::string _xprintf_b(char const* fmt_str, int marker, ...) +{ + va_list ap; + va_start(ap, marker); + return _xprintf_ap(fmt_str, ap); +} - return std::string(buffer.get()); +/*! \brief printf-style formatting for std::string. Use the xprintf macro instead of using this function directly + \param fmt_str The printf-style format string +*/ +std::string _xprintf_b(std::string const& fmt_str, int marker, ...) +{ + va_list ap; + va_start(ap, marker); + return _xprintf_ap(fmt_str.c_str(), ap); } diff --git a/lib/xspublic/xscommon/xprintf.h b/lib/xspublic/xscommon/xprintf.h index 9800361..cd7aead 100644 --- a/lib/xspublic/xscommon/xprintf.h +++ b/lib/xspublic/xscommon/xprintf.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -65,11 +65,14 @@ #ifndef XPRINTF_H #define XPRINTF_H -#include +#ifdef __cplusplus + #include -std::string _xprintf_b(std::string const& fmt_str, int bSize, ...); -// We use 65500 instead of 65536 to prevent heap fragmentation. There should now be space around the block for -// heap management data. -#define xprintf(fmt_str, ...) _xprintf_b(fmt_str, 65500, ##__VA_ARGS__) + std::string _xprintf_b(char const* fmt_str, int marker, ...); + std::string _xprintf_b(std::string const& fmt_str, int marker, ...); + + #define xprintf(fmt_str, ...) _xprintf_b(fmt_str, 0, ##__VA_ARGS__) + +#endif #endif diff --git a/lib/xspublic/xscommon/xscommon_config.h b/lib/xspublic/xscommon/xscommon_config.h index 4fd798b..fa1652f 100644 --- a/lib/xspublic/xscommon/xscommon_config.h +++ b/lib/xspublic/xscommon/xscommon_config.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -69,10 +69,10 @@ #define CONFIG_H #ifndef UNICODE -#define UNICODE + #define UNICODE #endif #ifndef _UNICODE -#define _UNICODE + #define _UNICODE #endif #include diff --git a/lib/xspublic/xscommon/xsens_debugtools.h b/lib/xspublic/xscommon/xsens_debugtools.h index 2b5cede..929c848 100644 --- a/lib/xspublic/xscommon/xsens_debugtools.h +++ b/lib/xspublic/xscommon/xsens_debugtools.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -66,21 +66,21 @@ #define XSENS_DEBUGTOOLS_H #ifndef XSTRINGIFY -#define XSTRINGIFY2(x) #x -#define XSTRINGIFY(x) XSTRINGIFY2(x) + #define XSTRINGIFY2(x) #x + #define XSTRINGIFY(x) XSTRINGIFY2(x) #endif #ifdef __cplusplus -#ifdef _MSC_VER -#define XSENS_COMPILER_WARNING(n, w) __pragma(message(__FILE__ "(" XSTRINGIFY(__LINE__) "): warning " n ": " w)); + #ifdef _MSC_VER + #define XSENS_COMPILER_WARNING(n, w) __pragma(message(__FILE__ "(" XSTRINGIFY(__LINE__) "): warning " n ": " w)); + #else + // the warning needs to be made literal. String concatenation is unfortunately not performed before _Pragma + #define XCW_GCCWARNING(w) _Pragma(XSTRINGIFY(GCC warning #w)) + #define XSENS_COMPILER_WARNING(n, w) XCW_GCCWARNING(w (n)) + #endif #else -// the warning needs to be made literal. String concatenation is unfortunately not performed before _Pragma -#define XCW_GCCWARNING(w) _Pragma(XSTRINGIFY(GCC warning #w)) -#define XSENS_COMPILER_WARNING(n, w) XCW_GCCWARNING(w (n)) -#endif -#else -// old/plain C compilers don't support these keywords -#define XSENS_COMPILER_WARNING(n, w) + // old/plain C compilers don't support these keywords + #define XSENS_COMPILER_WARNING(n, w) #endif #define XSENS_COMPILER_WARNING0(w) XSENS_COMPILER_WARNING("X0000", w) diff --git a/lib/xspublic/xscommon/xsens_generic_matrix.h b/lib/xspublic/xscommon/xsens_generic_matrix.h index 8be38dc..638c12c 100644 --- a/lib/xspublic/xscommon/xsens_generic_matrix.h +++ b/lib/xspublic/xscommon/xsens_generic_matrix.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -68,32 +68,32 @@ #include "xsens_math_throw.h" #ifndef XSENS_MATH_FIRMWARE -# ifndef XSENS_EXCEPTION_H -# include -# endif + #ifndef XSENS_EXCEPTION_H + #include + #endif #endif #ifdef XSENS_GENERIC_MATRIX_RANGE_CHECKS -# ifdef _MSC_VER -# define XSENS_GENERIC_MATRIX_THROW throw(...) -# else -# define XSENS_GENERIC_MATRIX_THROW -# endif + #ifdef _MSC_VER + #define XSENS_GENERIC_MATRIX_THROW throw(...) + #else + #define XSENS_GENERIC_MATRIX_THROW + #endif #else -# define XSENS_GENERIC_MATRIX_THROW + #define XSENS_GENERIC_MATRIX_THROW #endif #ifndef XSENS_THROW_BAD_ALLOC -# ifdef XSENS_NO_EXCEPTIONS -# include -# define XSENS_THROW_BAD_ALLOC XM_THROW("Bad alloc") -# else -# define XSENS_THROW_BAD_ALLOC throw std::bad_alloc() -# endif + #ifdef XSENS_NO_EXCEPTIONS + #include + #define XSENS_THROW_BAD_ALLOC XM_THROW("Bad alloc") + #else + #define XSENS_THROW_BAD_ALLOC throw std::bad_alloc() + #endif #endif #ifndef _PSTDINT_H_INCLUDED -#include + #include #endif #include @@ -102,7 +102,8 @@ #ifdef __cplusplus namespace xsens { -namespace { +namespace +{ /*! \brief Swap the values of two objects of a basic type T through assignment */ template inline void swapb(T& a, T& b) @@ -132,108 +133,118 @@ class GenericMatrix int m_flags; //!< Flags for data management. public: - //! Standard constructor, creates a 1x1 matrix. + //! Standard constructor, creates a 1x1 matrix. GenericMatrix(); - //! Construct a matrix with a capacity of at least the given size. + //! Construct a matrix with a capacity of at least the given size. explicit GenericMatrix(const uint32_t newRows, const uint32_t newCols, bool zeroValues = true); - //! Construct a matrix as a direct copy of another one + //! Construct a matrix as a direct copy of another one GenericMatrix(const GenericMatrix& src); - //! Construct a matrix as a copy of a raw data buffer + //! Construct a matrix as a copy of a raw data buffer explicit GenericMatrix(const uint32_t newRows, const uint32_t newCols, const T* src); - //! Construct a matrix using the given buffer, useful when the matrix should be stack allocated. + //! Construct a matrix using the given buffer, useful when the matrix should be stack allocated. explicit GenericMatrix(const uint32_t newRows, const uint32_t newCols, T* buffer, XsDataFlags flags); - //! Destroy the matrix. This does NOT automatically delete items IN the list. + //! Destroy the matrix. This does NOT automatically delete items IN the list. virtual ~GenericMatrix(); - //! Sets the size of the matrix to the new dimensions. The contents should be considered garbage. If zeroValues is set to true a memset 0 is done on the newly created items + //! Sets the size of the matrix to the new dimensions. The contents should be considered garbage. If zeroValues is set to true a memset 0 is done on the newly created items void setSize(uint32_t newRows, uint32_t newCols, bool zeroValues = true); - //! Retrieves the item at the given row,col. An index beyond the end causes an exception. + //! Retrieves the item at the given row,col. An index beyond the end causes an exception. T& get(const uint32_t row, const uint32_t cols) const XSENS_GENERIC_MATRIX_THROW; - //! Retrieves a pointer to the first item of the given row. An index beyond the end causes an exception. - T* operator [] (const uint32_t row) const XSENS_GENERIC_MATRIX_THROW; - //! Returns the number of rows currently in the matrix. - uint32_t rows(void) const { return m_rows; } - //! Returns the number of cols currently in the matrix. - uint32_t cols(void) const { return m_cols; } - //! Returns the number of items currently in the matrix. - uint32_t count(void) const { return m_rows*m_cols; } - //! Returns the number of items currently in the matrix. - uint32_t size(void) const { return m_rows*m_cols; } - //! Copy a matrix into this one + //! Retrieves a pointer to the first item of the given row. An index beyond the end causes an exception. + T* operator [](const uint32_t row) const XSENS_GENERIC_MATRIX_THROW; + //! Returns the number of rows currently in the matrix. + uint32_t rows(void) const + { + return m_rows; + } + //! Returns the number of cols currently in the matrix. + uint32_t cols(void) const + { + return m_cols; + } + //! Returns the number of items currently in the matrix. + uint32_t count(void) const + { + return m_rows * m_cols; + } + //! Returns the number of items currently in the matrix. + uint32_t size(void) const + { + return m_rows * m_cols; + } + //! Copy a matrix into this one GenericMatrix& operator = (const GenericMatrix& src); - //! Swap a matrix with this one + //! Swap a matrix with this one void swap(GenericMatrix& other); - //! Swap a matrix with this one - friend void swap(GenericMatrix& first, GenericMatrix& second) { first.swap(second); } - //! Calls memset 0 on the contents of the matrix + //! Swap a matrix with this one + friend void swap(GenericMatrix& first, GenericMatrix& second) + { + first.swap(second); + } + //! Calls memset 0 on the contents of the matrix void zeroValues(); }; -template -GenericMatrix::GenericMatrix() +template GenericMatrix::GenericMatrix() : m_rows(1) , m_cols(1) , m_flags(XSDF_Managed) { - m_data = (T*)malloc(m_rows*m_cols * sizeof(T)); + m_data = (T*)malloc(m_rows * m_cols * sizeof(T)); if (!m_data) XSENS_THROW_BAD_ALLOC; - m_allocSize = m_rows*m_cols; + m_allocSize = m_rows * m_cols; } -template -GenericMatrix::GenericMatrix(uint32_t newRows, uint32_t newCols, bool zeroVals) +template GenericMatrix::GenericMatrix(uint32_t newRows, uint32_t newCols, bool zeroVals) : m_rows(newRows) , m_cols(newCols) , m_flags(XSDF_Managed) { - m_data = (T*)malloc(sizeof(T) * (size_t)m_rows*m_cols); + m_data = (T*)malloc(sizeof(T) * (size_t)m_rows * m_cols); if (!m_data) XSENS_THROW_BAD_ALLOC; - m_allocSize = m_rows*m_cols; + m_allocSize = m_rows * m_cols; if (zeroVals) zeroValues(); } -template -GenericMatrix::GenericMatrix(const GenericMatrix& src) +template GenericMatrix::GenericMatrix(const GenericMatrix& src) : m_rows(src.m_rows) , m_cols(src.m_cols) , m_flags(XSDF_Managed) { - m_data = (T*)malloc(sizeof(T) * (size_t)m_rows*m_cols); + m_data = (T*)malloc(sizeof(T) * (size_t)m_rows * m_cols); if (!m_data) XSENS_THROW_BAD_ALLOC; - m_allocSize = m_rows*m_cols; + m_allocSize = m_rows * m_cols; - memcpy(m_data, src.m_data, sizeof(T) * (size_t)m_rows*m_cols); + memcpy(m_data, src.m_data, sizeof(T) * (size_t)m_rows * m_cols); } -template -GenericMatrix::GenericMatrix(const uint32_t newRows, uint32_t newCols, const T* src) +template GenericMatrix::GenericMatrix(const uint32_t newRows, uint32_t newCols, const T* src) : m_rows(newRows) , m_cols(newCols) , m_flags(XSDF_Managed) { - m_data = (T*)malloc(sizeof(T) * (size_t)m_rows*m_cols); + m_data = (T*)malloc(sizeof(T) * (size_t)m_rows * m_cols); if (!m_data) XSENS_THROW_BAD_ALLOC; - m_allocSize = m_rows*m_cols; + m_allocSize = m_rows * m_cols; - memcpy(m_data, src, sizeof(T) * (size_t)m_rows*m_cols); + memcpy(m_data, src, sizeof(T) * (size_t)m_rows * m_cols); } -template -GenericMatrix::GenericMatrix(const uint32_t newRows, const uint32_t newCols, T* const buffer, XsDataFlags flags) +template GenericMatrix::GenericMatrix(const uint32_t newRows, const uint32_t newCols, T* const buffer, XsDataFlags flags) : m_rows(newRows) , m_cols(newCols) , m_flags(flags) @@ -247,19 +258,18 @@ GenericMatrix::GenericMatrix(const uint32_t newRows, const uint32_t newCols, } if (m_flags & XSDF_Managed) - m_data = (T*)malloc(sizeof(T) * (size_t)m_rows*m_cols); + m_data = (T*)malloc(sizeof(T) * (size_t)m_rows * m_cols); if (!m_data) XSENS_THROW_BAD_ALLOC; - m_allocSize = m_rows*m_cols; + m_allocSize = m_rows * m_cols; if (m_flags & XSDF_Managed) - memcpy(m_data, buffer, sizeof(T) * (size_t)m_rows*m_cols); + memcpy(m_data, buffer, sizeof(T) * (size_t)m_rows * m_cols); } -template -GenericMatrix::~GenericMatrix() +template GenericMatrix::~GenericMatrix() { if (m_flags & XSDF_Managed) free(m_data); @@ -313,9 +323,9 @@ void GenericMatrix::setSize(uint32_t newRows, uint32_t newCols, bool zeroVals if (newSize > 0) { - m_data = (T*)malloc(sizeof(T) * (size_t)newRows*newCols); + m_data = (T*)malloc(sizeof(T) * (size_t)newRows * newCols); m_flags = XSDF_Managed; - m_allocSize = newRows*newCols; + m_allocSize = newRows * newCols; } } @@ -323,9 +333,7 @@ void GenericMatrix::setSize(uint32_t newRows, uint32_t newCols, bool zeroVals m_cols = newCols; if (!m_data) - { XSENS_THROW_BAD_ALLOC; - } if (zeroVals) zeroValues(); @@ -338,23 +346,23 @@ T& GenericMatrix::get(uint32_t row, uint32_t col) const XSENS_GENERIC_MATRIX_ if (row >= m_rows || col >= m_cols) XM_THROW("index out of bounds"); #endif - return m_data[row*m_cols + col]; + return m_data[row * m_cols + col]; } template -T* GenericMatrix::operator [] (const uint32_t row) const XSENS_GENERIC_MATRIX_THROW +T* GenericMatrix::operator [](const uint32_t row) const XSENS_GENERIC_MATRIX_THROW { #ifdef XSENS_GENERIC_MATRIX_RANGE_CHECKS if (row >= m_rows) XM_THROW("index out of bounds"); #endif - return &m_data[row*m_cols]; + return &m_data[row * m_cols]; } template void GenericMatrix::zeroValues() { - memset(static_cast(m_data), 0, sizeof(T) * (size_t)m_rows*m_cols); + memset(static_cast(m_data), 0, sizeof(T) * (size_t)m_rows * m_cols); } template diff --git a/lib/xspublic/xscommon/xsens_janitors.h b/lib/xspublic/xscommon/xsens_janitors.h index 078f646..a10beca 100644 --- a/lib/xspublic/xscommon/xsens_janitors.h +++ b/lib/xspublic/xscommon/xsens_janitors.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -66,34 +66,35 @@ #define XSENS_JANITORS_H #ifndef __AVR32__ -#include + #include #endif // required for gnu c++ compiler versions due to difference in attribute declarations #if defined(__AVR32__) -# define __cdecl -# define __stdcall + #define __cdecl + #define __stdcall #elif defined(_ADI_COMPILER) -# define __cdecl -# define __stdcall + #define __cdecl + #define __stdcall #elif defined(__GNUC__) && !defined(HAVE_CDECL) -# if !defined(__cdecl) -# if defined(__x86_64__) -# define __cdecl -# else -# define __cdecl __attribute__((cdecl)) -# endif -# endif -# if !defined(__stdcall) -# if defined(__x86_64__) -# define __stdcall -# else -# define __stdcall __attribute__((stdcall)) -#endif -# endif + #if !defined(__cdecl) + #if defined(__x86_64__) + #define __cdecl + #else + #define __cdecl __attribute__((cdecl)) + #endif + #endif + #if !defined(__stdcall) + #if defined(__x86_64__) + #define __stdcall + #else + #define __stdcall __attribute__((stdcall)) + #endif + #endif #endif -namespace xsens { +namespace xsens +{ #ifndef NOT_FOR_PUBLIC_RELEASE ////////////////////////////////////////////////////////////////////////////////////////// @@ -102,7 +103,8 @@ namespace xsens { the janitor is created will be in it again when the janitor leaves scope. */ template -class JanitorRestore { +class JanitorRestore +{ private: T& m_control; T m_value; @@ -125,12 +127,16 @@ class JanitorRestore { /*! \brief Disables the value restoring */ void disable(void) - { m_enabled = false; } + { + m_enabled = false; + } /*! \brief Enables the value restoring */ void enable(void) - { m_enabled = true; } + { + m_enabled = true; + } }; ////////////////////////////////////////////////////////////////////////////////////////// @@ -139,7 +145,8 @@ class JanitorRestore { janitor leaves scope. */ template -class JanitorFree { +class JanitorFree +{ private: T* m_control; bool m_enabled; @@ -161,12 +168,16 @@ class JanitorFree { /*! \brief Disables the memory releasing */ void disable(void) - { m_enabled = false; } + { + m_enabled = false; + } /*! \brief Enables the memory releasing */ void enable(void) - { m_enabled = true; } + { + m_enabled = true; + } }; ////////////////////////////////////////////////////////////////////////////////////////// @@ -175,7 +186,8 @@ class JanitorFree { janitor leaves scope. */ template -class JanitorDelete { +class JanitorDelete +{ private: T* m_control; bool m_enabled; @@ -211,25 +223,37 @@ class JanitorDelete { \param control The control object */ void setControl(T* control) - { m_control = control; } + { + m_control = control; + } /*! \brief Disables the memory releasing */ void disable(void) - { m_enabled = false; } + { + m_enabled = false; + } /*! \brief Enables the memory releasing */ void enable(void) - { m_enabled = true; } + { + m_enabled = true; + } /*! \returns A const pointer to the data. This does not detach/copy the data. */ - const T *operator->() const { return m_control; } + const T* operator->() const + { + return m_control; + } /*! \returns A pointer to the data. This does not detach/copy the data. */ - T *operator->() { return m_control; } + T* operator->() + { + return m_control; + } }; ////////////////////////////////////////////////////////////////////////////////////////// @@ -238,7 +262,8 @@ class JanitorDelete { janitor leaves scope and the referenced pointer is set to NULL. */ template -class JanitorDeleteNull { +class JanitorDeleteNull +{ private: T*& m_control; bool m_enabled; @@ -267,20 +292,30 @@ class JanitorDeleteNull { /*! \brief Disables the memory releasing and nulling */ void disable(void) - { m_enabled = false; } + { + m_enabled = false; + } /*! \brief Enables the memory releasing and nulling */ void enable(void) - { m_enabled = true; } + { + m_enabled = true; + } /*! \returns A const pointer to the data. This does not detach/copy the data. */ - const T *operator->() const { return m_control; } + const T* operator->() const + { + return m_control; + } /*! \returns A pointer to the data. This does not detach/copy the data. */ - T *operator->() { return m_control; } + T* operator->() + { + return m_control; + } }; ////////////////////////////////////////////////////////////////////////////////////////// @@ -289,7 +324,8 @@ class JanitorDeleteNull { janitor leaves scope. */ template -class JanitorDeleteArray { +class JanitorDeleteArray +{ private: T* m_control; bool m_enabled; @@ -311,12 +347,16 @@ class JanitorDeleteArray { /*! \brief Disables the memory releasing */ void disable(void) - { m_enabled = false; } + { + m_enabled = false; + } /*! \brief Enables the memory releasing */ void enable(void) - { m_enabled = true; } + { + m_enabled = true; + } }; #endif @@ -326,9 +366,10 @@ class JanitorDeleteArray { janitor leaves scope. */ template -class JanitorClassFunc { +class JanitorClassFunc +{ public: - typedef R (T::*t_func_JanitorClassFunc)(void); //!< A function prototype for a janitor class + typedef R(T::*t_func_JanitorClassFunc)(void); //!< A function prototype for a janitor class private: const JanitorClassFunc& operator = (const JanitorClassFunc&); @@ -339,7 +380,7 @@ class JanitorClassFunc { /*! \brief Constructor */ - JanitorClassFunc(T& control, t_func_JanitorClassFunc func, bool enabl = true) : + JanitorClassFunc(T& control, t_func_JanitorClassFunc func, bool enabl = true) : m_control(control), m_funcJCF(func), m_enabled(enabl) { } @@ -355,12 +396,16 @@ class JanitorClassFunc { /*! \brief Disables the function calling */ void disable(void) - { m_enabled = false; } + { + m_enabled = false; + } /*! \brief Enables the function calling */ void enable(void) - { m_enabled = true; } + { + m_enabled = true; + } }; ////////////////////////////////////////////////////////////////////////////////////////// @@ -369,9 +414,10 @@ class JanitorClassFunc { janitor leaves scope. */ template -class JanitorClassFuncP1 { +class JanitorClassFuncP1 +{ public: - typedef R (T::*t_func_JanitorClassFunc)(P1); //!< A function prototype for a janitor class + typedef R(T::*t_func_JanitorClassFunc)(P1); //!< A function prototype for a janitor class private: const JanitorClassFuncP1& operator = (const JanitorClassFuncP1&); @@ -383,7 +429,7 @@ class JanitorClassFuncP1 { /*! \brief Constructor */ - JanitorClassFuncP1(T& control, P1 p1, t_func_JanitorClassFunc func, bool enabl = true) : + JanitorClassFuncP1(T& control, P1 p1, t_func_JanitorClassFunc func, bool enabl = true) : m_control(control), m_param1(p1), m_funcJCF(func), m_enabled(enabl) { } @@ -399,12 +445,16 @@ class JanitorClassFuncP1 { /*! \brief Disables the function calling */ void disable(void) - { m_enabled = false; } + { + m_enabled = false; + } /*! \brief Enable the function calling */ void enable(void) - { m_enabled = true; } + { + m_enabled = true; + } }; #ifndef NOT_FOR_PUBLIC_RELEASE @@ -415,9 +465,10 @@ class JanitorClassFuncP1 { is destroyed before the function unrolling begins. */ template -class JanitorFunc0 { +class JanitorFunc0 +{ public: - typedef ResultType (__cdecl * t_func_JanitorFunc)(void); //!< A function prototype for a janitor function + typedef ResultType(__cdecl* t_func_JanitorFunc)(void); //!< A function prototype for a janitor function private: const JanitorFunc0& operator = (const JanitorFunc0&); t_func_JanitorFunc m_funcJF; @@ -440,12 +491,16 @@ class JanitorFunc0 { /*! \brief Disables the function calling */ void disable(void) - { m_enabled = false; } + { + m_enabled = false; + } /*! \brief Enables the function calling */ void enable(void) - { m_enabled = true; } + { + m_enabled = true; + } }; #ifndef __AVR32__ @@ -456,9 +511,10 @@ class JanitorFunc0 { is destroyed before the function unrolling begins. */ template -class JanitorStdFunc0 { +class JanitorStdFunc0 +{ public: - typedef std::function t_func_JanitorFunc; //!< A function prototype for a janitor function + typedef std::function t_func_JanitorFunc; //!< A function prototype for a janitor function private: const JanitorStdFunc0& operator = (const JanitorStdFunc0&); t_func_JanitorFunc m_funcJF; @@ -481,12 +537,16 @@ class JanitorStdFunc0 { /*! \brief Disables the function calling */ void disable(void) - { m_enabled = false; } + { + m_enabled = false; + } /*! \brief Enables the function calling */ void enable(void) - { m_enabled = true; } + { + m_enabled = true; + } }; #endif @@ -497,9 +557,10 @@ class JanitorStdFunc0 { is destroyed before the function unrolling begins. */ template -class JanitorFunc1 { +class JanitorFunc1 +{ public: - typedef ResultType (__cdecl * t_func_JanitorFunc)(ParamType); //!< A function prototype for a janitor function + typedef ResultType(__cdecl* t_func_JanitorFunc)(ParamType); //!< A function prototype for a janitor function private: const JanitorFunc1& operator = (const JanitorFunc1&); ParamType m_control; @@ -509,7 +570,7 @@ class JanitorFunc1 { /*! \brief Constructor */ - JanitorFunc1(t_func_JanitorFunc func, ParamType control, bool enabl = true) : + JanitorFunc1(t_func_JanitorFunc func, ParamType control, bool enabl = true) : m_control(control), m_funcJF(func), m_enabled(enabl) {} /*! \brief Destructor @@ -523,12 +584,16 @@ class JanitorFunc1 { /*! \brief Disables the function calling */ void disable(void) - { m_enabled = false; } + { + m_enabled = false; + } /*! \brief Enables the function calling */ void enable(void) - { m_enabled = true; } + { + m_enabled = true; + } }; ////////////////////////////////////////////////////////////////////////////////////////// @@ -538,9 +603,10 @@ class JanitorFunc1 { is destroyed before the function unrolling begins. */ template -class JanitorFunc1R { +class JanitorFunc1R +{ public: - typedef ResultType (__cdecl * t_func_JanitorFunc)(ParamType); //!< A function prototype for a janitor function + typedef ResultType(__cdecl* t_func_JanitorFunc)(ParamType); //!< A function prototype for a janitor function private: const JanitorFunc1R& operator = (const JanitorFunc1R&); ParamType& m_control; @@ -550,7 +616,7 @@ class JanitorFunc1R { /*! \brief Constructor */ - JanitorFunc1R(t_func_JanitorFunc func, ParamType& control, bool enabl = true) : + JanitorFunc1R(t_func_JanitorFunc func, ParamType& control, bool enabl = true) : m_control(control), m_funcJF(func), m_enabled(enabl) {} /*! \brief Destructor @@ -564,12 +630,16 @@ class JanitorFunc1R { /*! \brief Disables the function calling */ void disable(void) - { m_enabled = false; } + { + m_enabled = false; + } /*! \brief Enables the function calling */ void enable(void) - { m_enabled = true; } + { + m_enabled = true; + } }; ////////////////////////////////////////////////////////////////////////////////////////// @@ -579,9 +649,10 @@ class JanitorFunc1R { is destroyed before the function unrolling begins. */ template -class JanitorFunc2 { +class JanitorFunc2 +{ public: - typedef ResultType (__cdecl * t_func_JanitorFunc)(Param1Type,Param2Type); //!< A function prototype for a janitor function + typedef ResultType(__cdecl* t_func_JanitorFunc)(Param1Type, Param2Type); //!< A function prototype for a janitor function private: const JanitorFunc2& operator = (const JanitorFunc2&); Param1Type m_control1; @@ -592,7 +663,7 @@ class JanitorFunc2 { /*! \brief Constructor */ - JanitorFunc2(t_func_JanitorFunc func, Param1Type control1, Param2Type control2, bool enabl = true) : + JanitorFunc2(t_func_JanitorFunc func, Param1Type control1, Param2Type control2, bool enabl = true) : m_funcJF(func), m_control1(control1), m_control2(control2), m_enabled(enabl) {} /*! \brief Destructor @@ -600,18 +671,22 @@ class JanitorFunc2 { ~JanitorFunc2() { if (m_enabled) - (*m_funcJF)(m_control1,m_control2); + (*m_funcJF)(m_control1, m_control2); } /*! \brief Disables the function calling */ void disable(void) - { m_enabled = false; } + { + m_enabled = false; + } /*! \brief Enables the function calling */ void enable(void) - { m_enabled = true; } + { + m_enabled = true; + } }; ////////////////////////////////////////////////////////////////////////////////////////// @@ -621,9 +696,10 @@ class JanitorFunc2 { is destroyed before the function unrolling begins. */ template -class JanitorFunc2R { +class JanitorFunc2R +{ public: - typedef ResultType (__cdecl * t_func_JanitorFunc)(Param1Type,Param2Type); //!< A function prototype for a janitor function + typedef ResultType(__cdecl* t_func_JanitorFunc)(Param1Type, Param2Type); //!< A function prototype for a janitor function private: const JanitorFunc2R& operator = (const JanitorFunc2R&); Param1Type& m_control1; @@ -634,7 +710,7 @@ class JanitorFunc2R { /*! \brief Constructor */ - JanitorFunc2R(t_func_JanitorFunc func, Param1Type& control1, Param2Type& control2, bool enabl = true) : + JanitorFunc2R(t_func_JanitorFunc func, Param1Type& control1, Param2Type& control2, bool enabl = true) : m_funcJF(func), m_control1(control1), m_control2(control2), m_enabled(enabl) {} /*! \brief Destructor @@ -642,18 +718,22 @@ class JanitorFunc2R { ~JanitorFunc2R() { if (m_enabled) - (*m_funcJF)(m_control1,m_control2); + (*m_funcJF)(m_control1, m_control2); } /*! \brief Disables the function calling */ void disable(void) - { m_enabled = false; } + { + m_enabled = false; + } /*! \brief Enables the function calling */ void enable(void) - { m_enabled = true; } + { + m_enabled = true; + } }; ////////////////////////////////////////////////////////////////////////////////////////// @@ -663,9 +743,10 @@ class JanitorFunc2R { is destroyed before the function unrolling begins. */ template -class JanitorLogFunc { +class JanitorLogFunc +{ public: - typedef R (__cdecl * t_func_JanitorLogFunc)(const char*, const char*, ...); //!< A function prototype for a janitor log function + typedef R(__cdecl* t_func_JanitorLogFunc)(const char*, const char*, ...); //!< A function prototype for a janitor log function private: const JanitorLogFunc& operator = (const JanitorLogFunc&); t_func_JanitorLogFunc m_funcJF; @@ -678,13 +759,13 @@ class JanitorLogFunc { /*! \brief Constructor with additional 'function name' parameter */ - JanitorLogFunc(t_func_JanitorLogFunc func, const char *filter, const char *str, const char *functionName, T& control, bool enable = true) : + JanitorLogFunc(t_func_JanitorLogFunc func, const char* filter, const char* str, const char* functionName, T& control, bool enable = true) : m_funcJF(func), m_filter(filter), m_str(str), m_functionName(functionName), m_control(control), m_enabled(enable) {} /*! \brief Constructor */ - JanitorLogFunc(t_func_JanitorLogFunc func, const char *filter, const char* str, T& control, bool enable = true) : + JanitorLogFunc(t_func_JanitorLogFunc func, const char* filter, const char* str, T& control, bool enable = true) : m_funcJF(func), m_filter(filter), m_str(str), m_functionName(0), m_control(control), m_enabled(enable) {} /*! \brief Destructor @@ -703,12 +784,16 @@ class JanitorLogFunc { /*! \brief Disables the log function calling */ void disable(void) - { m_enabled = false; } + { + m_enabled = false; + } /*! \brief Enables the log function calling */ void enable(void) - { m_enabled = true; } + { + m_enabled = true; + } }; ////////////////////////////////////////////////////////////////////////////////////////// @@ -717,9 +802,10 @@ class JanitorLogFunc { supplied string when the janitor leaves scope. */ template -class JanitorSimpleLogFunc { +class JanitorSimpleLogFunc +{ public: - typedef R (__cdecl * t_func_JanitorSimpleLogFunc)(const char*, const char*, ...); //!< A function prototype for a janitor simple log function + typedef R(__cdecl* t_func_JanitorSimpleLogFunc)(const char*, const char*, ...); //!< A function prototype for a janitor simple log function private: const JanitorSimpleLogFunc& operator = (const JanitorSimpleLogFunc&); t_func_JanitorSimpleLogFunc m_funcJF; @@ -731,13 +817,13 @@ class JanitorSimpleLogFunc { /*! \brief Constructor with additional 'function name' parameter */ - JanitorSimpleLogFunc(t_func_JanitorSimpleLogFunc func, const char *filter, const char *str, const char *functionName, bool enable = true) : + JanitorSimpleLogFunc(t_func_JanitorSimpleLogFunc func, const char* filter, const char* str, const char* functionName, bool enable = true) : m_funcJF(func), m_filter(filter), m_str(str), m_functionName(functionName), m_enabled(enable) {} /*! \brief Constructor */ - JanitorSimpleLogFunc(t_func_JanitorSimpleLogFunc func, const char *filter, const char* str, bool enable = true) : + JanitorSimpleLogFunc(t_func_JanitorSimpleLogFunc func, const char* filter, const char* str, bool enable = true) : m_funcJF(func), m_filter(filter), m_str(str), m_functionName(0), m_enabled(enable) {} /*! \brief Destructor @@ -756,12 +842,16 @@ class JanitorSimpleLogFunc { /*! \brief Disables the log function calling */ void disable(void) - { m_enabled = false; } + { + m_enabled = false; + } /*! \brief Enables the log function calling */ void enable(void) - { m_enabled = true; } + { + m_enabled = true; + } }; ////////////////////////////////////////////////////////////////////////////////////////// @@ -771,9 +861,14 @@ class JanitorSimpleLogFunc { is destroyed before the function unrolling begins. */ template -class JanitorFuncStdCall { +class JanitorFuncStdCall +{ public: - typedef ResultType (__stdcall * t_func_JanitorFuncStdCall)(ParamType); //!< A function prototype for a calling janitor function +#ifndef __ANDROID_API__ + typedef ResultType(__stdcall* t_func_JanitorFuncStdCall)(ParamType); //!< A function prototype for a calling janitor function +#else + typedef ResultType(*t_func_JanitorFuncStdCall)(ParamType); //!< A function prototype for a calling janitor function +#endif private: const JanitorFuncStdCall& operator = (const JanitorFuncStdCall&); ParamType& m_control; @@ -783,7 +878,7 @@ class JanitorFuncStdCall { /*! \brief Constructor */ - JanitorFuncStdCall(t_func_JanitorFuncStdCall func, ParamType& control, bool enabl = true) : + JanitorFuncStdCall(t_func_JanitorFuncStdCall func, ParamType& control, bool enabl = true) : m_funcJFSC(func), m_control(control), m_enabled(enabl) {} /*! \brief Destructor @@ -797,12 +892,16 @@ class JanitorFuncStdCall { /*! \brief Disables the function calling */ void disable(void) - { m_enabled = false; } + { + m_enabled = false; + } /*! \brief Enables the function calling */ void enable(void) - { m_enabled = true; } + { + m_enabled = true; + } }; ////////////////////////////////////////////////////////////////////////////////////////// @@ -811,7 +910,8 @@ class JanitorFuncStdCall { the janitor is created will be in it again when the janitor leaves scope. */ template -class JanitorSet { +class JanitorSet +{ private: const JanitorSet& operator = (const JanitorSet&); T& m_control; @@ -835,12 +935,16 @@ class JanitorSet { /*! \brief Disables the value restoring */ void disable(void) - { m_enabled = false; } + { + m_enabled = false; + } /*! \brief Enables the value restoring */ void enable(void) - { m_enabled = true; } + { + m_enabled = true; + } }; @@ -856,7 +960,10 @@ class JanitorRCDeallocationPolicy_Delete /*! \brief Deallocates (deletes) a pointer to an object \param p The pointer to an object to delete */ - static void Deallocate(T* p) {delete p;} + static void Deallocate(T* p) + { + delete p; + } protected: /*! \brief Destructor \note Always use non-virtual protected destructors in policies @@ -875,7 +982,10 @@ class JanitorRCDeallocationPolicy_Free /*! \brief Deallocates (frees) a pointer to an object \param p The pointer to an object to free */ - static void Deallocate(T* p) {free(p);} + static void Deallocate(T* p) + { + free(p); + } protected: /*! \brief Destructor \note Always use non-virtual protected destructors in policies @@ -888,7 +998,8 @@ class JanitorRCDeallocationPolicy_Free last janitor for it leaves scope. */ template class DeallocationPolicy> -class JanitorRCDeallocation { +class JanitorRCDeallocation +{ public: typedef JanitorRCDeallocation ThisType; //!< A type definition for this type typedef T ElementType; //!< A type definition for an element type @@ -896,7 +1007,7 @@ class JanitorRCDeallocation { /*! \brief Constructor with an element type */ - JanitorRCDeallocation(ElementType *target = 0, bool enabled=true) + JanitorRCDeallocation(ElementType* target = 0, bool enabled = true) : m_target(target) , m_refCounter(0) , m_enabled(enabled) @@ -906,7 +1017,7 @@ class JanitorRCDeallocation { /*! \brief Constructor with this type */ - JanitorRCDeallocation(const ThisType &j, bool enabled=true) + JanitorRCDeallocation(const ThisType& j, bool enabled = true) : m_target(j.m_target) , m_refCounter(j.m_refCounter) , m_enabled(enabled) @@ -916,15 +1027,19 @@ class JanitorRCDeallocation { /*! \brief Destructor */ - virtual ~JanitorRCDeallocation() { removeRef(); } + virtual ~JanitorRCDeallocation() + { + removeRef(); + } /*! \brief Copy the data from \a rhs \param rhs The janitor deallication class \returns this */ - JanitorRCDeallocation& operator=(const JanitorRCDeallocation &rhs) + JanitorRCDeallocation& operator=(const JanitorRCDeallocation& rhs) { - if (m_target != rhs.m_target) { + if (m_target != rhs.m_target) + { removeRef(); m_target = rhs.m_target; m_refCounter = rhs.m_refCounter; @@ -994,9 +1109,8 @@ class JanitorRCDeallocation { */ void reset(ElementType* p = 0) throw() { - if (m_target != p) { + if (m_target != p) ThisType(p).swap(*this); - } } private: @@ -1006,7 +1120,8 @@ class JanitorRCDeallocation { */ void addRef(void) { - if (m_target) { + if (m_target) + { if (!m_refCounter) m_refCounter = new int(0); (*m_refCounter)++; @@ -1018,18 +1133,20 @@ class JanitorRCDeallocation { */ void removeRef(void) { - if (m_refCounter) { + if (m_refCounter) + { if (*m_refCounter > 1) (*m_refCounter)--; - else { + else + { delete m_refCounter; DeallocationPolicy::Deallocate(m_target); } } } - ElementType *m_target; - int *m_refCounter; + ElementType* m_target; + int* m_refCounter; bool m_enabled; }; @@ -1042,7 +1159,7 @@ class JanitorRCFree : public JanitorRCDeallocation /*! \brief Constructor */ - JanitorRCFree(T *target = 0, bool enabled=true): JanitorRCDeallocation(target, enabled) {} + JanitorRCFree(T* target = 0, bool enabled = true): JanitorRCDeallocation(target, enabled) {} }; /*! \brief A reference counter memory releasing janitor class @@ -1054,7 +1171,7 @@ class JanitorRCDelete : public JanitorRCDeallocation /*! \brief Constructor */ - JanitorRCDelete(T *target = 0, bool enabled=true): JanitorRCDeallocation(target, enabled) {} + JanitorRCDelete(T* target = 0, bool enabled = true): JanitorRCDeallocation(target, enabled) {} }; #endif diff --git a/lib/xspublic/xscommon/xsens_math_throw.cpp b/lib/xspublic/xscommon/xsens_math_throw.cpp index 7a80494..e7d9099 100644 --- a/lib/xspublic/xscommon/xsens_math_throw.cpp +++ b/lib/xspublic/xscommon/xsens_math_throw.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -66,11 +66,11 @@ #include "xsens_mutex.h" #ifndef __STDC_VERSION__ -#define __STDC_VERSION__ 0 + #define __STDC_VERSION__ 0 #endif #if __STDC_VERSION__ >= 199901L -#include + #include #endif namespace xsens @@ -94,9 +94,9 @@ void mathThrowBreakFunc() void enableFloatingPointExceptions() { #ifdef _MSC_VER -// _clearfp(); // always call _clearfp before enabling/unmasking a FPU exception -// _controlfp(_EM_INVALID | _EM_DENORMAL | _EM_ZERODIVIDE | _EM_OVERFLOW | -// _EM_UNDERFLOW | _EM_INEXACT, _MCW_EM); + // _clearfp(); // always call _clearfp before enabling/unmasking a FPU exception + // _controlfp(_EM_INVALID | _EM_DENORMAL | _EM_ZERODIVIDE | _EM_OVERFLOW | + // _EM_UNDERFLOW | _EM_INEXACT, _MCW_EM); #elif __STDC_VERSION__ >= 199901L feenableexcept(FE_ALL_EXCEPT); #endif @@ -109,8 +109,8 @@ void enableFloatingPointExceptions() void disableFloatingPointExceptions() { #ifdef _MSC_VER -// _clearfp(); -// _controlfp(_CW_DEFAULT, ~0); + // _clearfp(); + // _controlfp(_CW_DEFAULT, ~0); #elif __STDC_VERSION__ >= 199901L feclearexcept(FE_ALL_EXCEPT); #endif diff --git a/lib/xspublic/xscommon/xsens_math_throw.h b/lib/xspublic/xscommon/xsens_math_throw.h index 7493f32..a5483cb 100644 --- a/lib/xspublic/xscommon/xsens_math_throw.h +++ b/lib/xspublic/xscommon/xsens_math_throw.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -67,11 +67,9 @@ #include -#ifndef XSENS_NO_EXCEPTIONS - #include -#endif - #ifdef XSENS_NO_EXCEPTIONS + #define XSNOEXCEPT + #define XSNOEXCEPT_ARG(a) // support for exceptions is disabled, just do whatever assert(0) does #ifdef XSENS_DEBUG #include @@ -82,7 +80,17 @@ #define XM_THROW_DEFINED 0 #endif #else + #include #define XM_THROW_DEFINED 1 + + #if __cplusplus >= 201103L + #define XSNOEXCEPT noexcept + #define XSNOEXCEPT_ARG(a) noexcept(#a) + #else + #define XSNOEXCEPT + #define XSNOEXCEPT_ARG(a) + #endif + #ifdef XSEXCEPTION_H #ifdef _MSC_VER #define XETHROW(a) throw XsException(XRV_ERROR, XsString(__FUNCTION__ " ") << XsString(a)) @@ -104,12 +112,12 @@ namespace xsens { - namespace DebugTools - { - void mathThrowBreakFunc(); - void enableFloatingPointExceptions(); - void disableFloatingPointExceptions(); - } // namespace DebugTools +namespace DebugTools +{ +void mathThrowBreakFunc(); +void enableFloatingPointExceptions(); +void disableFloatingPointExceptions(); +} // namespace DebugTools } // namespace xsens #endif diff --git a/lib/xspublic/xscommon/xsens_mutex.h b/lib/xspublic/xscommon/xsens_mutex.h index 8b7dffa..a0420ff 100644 --- a/lib/xspublic/xscommon/xsens_mutex.h +++ b/lib/xspublic/xscommon/xsens_mutex.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -69,276 +69,280 @@ #include #include #include - #include #ifndef __GNUC__ -#pragma warning(disable: 4127) + #pragma warning(disable: 4127) #endif -namespace xsens { +namespace xsens +{ // by setting this to 1 you will enable timeout checks on the locks, see xsens_math_throw.cpp for the implementations #if 0 && defined(XSENS_DEBUG) - namespace DebugTools +namespace DebugTools +{ +class TimeoutChecker +{ + XsTimeStamp m_tStart; + int m_timeout; + char m_category; +public: + void* m_lock; + + TimeoutChecker(char category = 'l', void* lock = nullptr); + ~TimeoutChecker() + { + stop(); + } + inline void start(void* lock) + { + if (lock && !m_lock) + m_lock = lock; + m_tStart = XsTimeStamp::now(); + } + inline void stop() { - class TimeoutChecker { - XsTimeStamp m_tStart; - int m_timeout; - char m_category; - public: - void* m_lock; + if (m_tStart.msTime() && (XsTimeStamp::now() - m_tStart).msTime() >= m_timeout) + warning((XsTimeStamp::now() - m_tStart).msTime()); + m_tStart = 0; + } + void warning(int64_t); +}; +} +#define CHECK_LOCK_DEFINE() mutable DebugTools::TimeoutChecker m_tcCheck +#define CHECK_LOCK_ACQUIRE() volatile DebugTools::TimeoutChecker tcAcq('a', this); m_tcCheck.start(this) +#define CHECK_LOCK_RELEASE() m_tcCheck.stop(); volatile DebugTools::TimeoutChecker tcRel('r', this) +#define TIMEOUTCHECKER_DEFINED 1 +#else +#define CHECK_LOCK_DEFINE() typedef void IgnoredLockDefine +#define CHECK_LOCK_ACQUIRE() ((void)0) +#define CHECK_LOCK_RELEASE() ((void)0) +#define TIMEOUTCHECKER_DEFINED 0 +#endif - TimeoutChecker(char category = 'l', void* lock = nullptr); - ~TimeoutChecker() - { - stop(); - } - inline void start(void* lock) - { - if (lock && !m_lock) - m_lock = lock; - m_tStart = XsTimeStamp::now(); - } - inline void stop() - { - if (m_tStart.msTime() && (XsTimeStamp::now() - m_tStart).msTime() >= m_timeout) - warning((XsTimeStamp::now() - m_tStart).msTime()); - m_tStart = 0; - } - void warning(int64_t); - }; +class Lock; +class LockReadWrite; +class LockSuspendable; +class WaitCondition; +enum LockState { LS_Unlocked, LS_Read, LS_Write, LS_SuspendedWrite }; + +/*! \class Mutex + \brief A base mutex class +*/ +class Mutex +{ +private: +#if !XSENS_USE_POSIX_LOCKING + CRITICAL_SECTION m_mutex; +#else + pthread_mutexattr_t m_attr; + pthread_mutex_t m_mutex; +#endif + XsThreadId m_lockedBy; + volatile std::atomic_int m_lockCount; + friend class Lock; + friend class WaitCondition; +public: + Mutex() + : m_lockedBy(0) + , m_lockCount(0) + { +#if !XSENS_USE_POSIX_LOCKING + ::InitializeCriticalSection(&m_mutex); +#else + // this is required to get the same behaviour we have in windows + pthread_mutexattr_init(&m_attr); + pthread_mutexattr_settype(&m_attr, PTHREAD_MUTEX_RECURSIVE); + pthread_mutex_init(&m_mutex, &m_attr); +#endif } - #define CHECK_LOCK_DEFINE() mutable DebugTools::TimeoutChecker m_tcCheck - #define CHECK_LOCK_ACQUIRE() volatile DebugTools::TimeoutChecker tcAcq('a', this); m_tcCheck.start(this) - #define CHECK_LOCK_RELEASE() m_tcCheck.stop(); volatile DebugTools::TimeoutChecker tcRel('r', this) - #define TIMEOUTCHECKER_DEFINED 1 + ~Mutex() + { + assert(m_lockCount == 0); + assert(m_lockedBy == 0); +#if !XSENS_USE_POSIX_LOCKING + ::DeleteCriticalSection(&m_mutex); #else - #define CHECK_LOCK_DEFINE() typedef void IgnoredLockDefine - #define CHECK_LOCK_ACQUIRE() ((void)0) - #define CHECK_LOCK_RELEASE() ((void)0) - #define TIMEOUTCHECKER_DEFINED 0 + pthread_mutex_destroy(&m_mutex); + pthread_mutexattr_destroy(&m_attr); #endif + } + Mutex(Mutex const&) = delete; + Mutex& operator=(Mutex const&) = delete; + Mutex(Mutex&&) = delete; + Mutex& operator=(Mutex&&) = delete; - class Lock; - class LockReadWrite; - class LockSuspendable; - class WaitCondition; - enum LockState { LS_Unlocked, LS_Read, LS_Write, LS_SuspendedWrite }; + /*! \brief Claims (locks) a mutex + \returns True if successful + */ + inline bool claimMutex() + { +#if !XSENS_USE_POSIX_LOCKING + ::EnterCriticalSection(&m_mutex); +#else + pthread_mutex_lock(&m_mutex); +#endif + ++m_lockCount; + m_lockedBy = xsGetCurrentThreadId(); + return true; + } - /*! \class Mutex - \brief A base mutex class + /*! \brief Releases (unlocks) a mutex + \returns True if successful */ - class Mutex { - private: + inline bool releaseMutex() + { + if (!--m_lockCount) + m_lockedBy = 0; #if !XSENS_USE_POSIX_LOCKING - CRITICAL_SECTION m_mutex; + ::LeaveCriticalSection(&m_mutex); #else - pthread_mutexattr_t m_attr; - pthread_mutex_t m_mutex; + pthread_mutex_unlock(&m_mutex); +#endif + return true; + } + + /*! \brief Tries to claim (lock) a mutex + \returns True if successful + */ + inline bool tryClaimMutex() + { + // *CriticalSection returns true on success, pthread_* returns 0 on success. +#if !XSENS_USE_POSIX_LOCKING + if (::TryEnterCriticalSection(&m_mutex)) +#else + if (pthread_mutex_trylock(&m_mutex) == 0) #endif - XsThreadId m_lockedBy; - volatile std::atomic_int m_lockCount; - friend class Lock; - friend class WaitCondition; - public: - Mutex() - : m_lockedBy(0) - , m_lockCount(0) - { - #if !XSENS_USE_POSIX_LOCKING - ::InitializeCriticalSection(&m_mutex); - #else - // this is required to get the same behaviour we have in windows - pthread_mutexattr_init(&m_attr); - pthread_mutexattr_settype(&m_attr, PTHREAD_MUTEX_RECURSIVE); - pthread_mutex_init(&m_mutex, &m_attr); - #endif - } - ~Mutex() - { - assert(m_lockCount == 0); - assert(m_lockedBy == 0); - #if !XSENS_USE_POSIX_LOCKING - ::DeleteCriticalSection(&m_mutex); - #else - pthread_mutex_destroy(&m_mutex); - pthread_mutexattr_destroy(&m_attr); - #endif - } - Mutex(Mutex const&) = delete; - Mutex& operator=(Mutex const&) = delete; - Mutex(Mutex &&) = delete; - Mutex& operator=(Mutex &&) = delete; - - /*! \brief Claims (locks) a mutex - \returns True if successful - */ - inline bool claimMutex() { - #if !XSENS_USE_POSIX_LOCKING - ::EnterCriticalSection(&m_mutex); - #else - pthread_mutex_lock(&m_mutex); - #endif ++m_lockCount; m_lockedBy = xsGetCurrentThreadId(); return true; } + return false; + } +#ifdef XSENS_DEBUG + // this function is only intended as a debug function for use in assert statements + inline bool haveLock() const + { + return m_lockedBy == xsGetCurrentThreadId(); + } +#endif - /*! \brief Releases (unlocks) a mutex - \returns True if successful - */ - inline bool releaseMutex() - { - if (!--m_lockCount) - m_lockedBy = 0; - #if !XSENS_USE_POSIX_LOCKING - ::LeaveCriticalSection(&m_mutex); - #else - pthread_mutex_unlock(&m_mutex); - #endif - return true; - } - - /*! \brief Tries to claim (lock) a mutex - \returns True if successful - */ - inline bool tryClaimMutex() - { - // *CriticalSection returns true on success, pthread_* returns 0 on success. - #if !XSENS_USE_POSIX_LOCKING - if (::TryEnterCriticalSection(&m_mutex)) - #else - if (pthread_mutex_trylock(&m_mutex) == 0) - #endif - { - ++m_lockCount; - m_lockedBy = xsGetCurrentThreadId(); - return true; - } - return false; - } - #ifdef XSENS_DEBUG - // this function is only intended as a debug function for use in assert statements - inline bool haveLock() const - { - return m_lockedBy == xsGetCurrentThreadId(); - } - #endif - - /*! \returns True if a mutex is locked - */ - inline bool isLocked() const volatile - { - return m_lockCount > 0; - } - }; - - - /*! \class MutexReadWrite - \brief A readers-writer mutex class. - \details Any number of readers can claim the mutex at the same time, which blocks a writer from - claiming it. If a writer claims the mutex, all subsequent read claims will be blocked until - no more read claims exist and the write claim has been completed (released). Recursive read claims - within a thread that already has a read or write claim for this mutex will be granted to prevent - deadlocks. - */ - class MutexReadWrite { - private: - Mutex m_access; - volatile std::atomic_int m_writeRef; - volatile std::atomic m_writeLocked; - volatile std::atomic_int m_writeLocksPending; - XsThreadId* m_readLocked; - - int m_readLockMax; - int m_readLockCount; - friend class LockReadWrite; - MutexReadWrite(MutexReadWrite const&) = delete; - MutexReadWrite& operator=(MutexReadWrite const&) = delete; - MutexReadWrite(MutexReadWrite &&) = delete; - MutexReadWrite& operator=(MutexReadWrite &&) = delete; - - public: - inline MutexReadWrite() - : m_writeRef(0) - , m_writeLocked(0) - , m_writeLocksPending(0) - { - m_readLockMax = 32; - m_readLocked = new XsThreadId[m_readLockMax]; - m_readLockCount = 0; - } - inline ~MutexReadWrite() + /*! \returns True if a mutex is locked + */ + inline bool isLocked() const volatile + { + return m_lockCount > 0; + } +}; + + +/*! \class MutexReadWrite + \brief A readers-writer mutex class. + \details Any number of readers can claim the mutex at the same time, which blocks a writer from + claiming it. If a writer claims the mutex, all subsequent read claims will be blocked until + no more read claims exist and the write claim has been completed (released). Recursive read claims + within a thread that already has a read or write claim for this mutex will be granted to prevent + deadlocks. +*/ +class MutexReadWrite +{ +private: + Mutex m_access; + volatile std::atomic_int m_writeRef; + volatile std::atomic m_writeLocked; + volatile std::atomic_int m_writeLocksPending; + XsThreadId* m_readLocked; + + int m_readLockMax; + int m_readLockCount; + friend class LockReadWrite; + MutexReadWrite(MutexReadWrite const&) = delete; + MutexReadWrite& operator=(MutexReadWrite const&) = delete; + MutexReadWrite(MutexReadWrite&&) = delete; + MutexReadWrite& operator=(MutexReadWrite&&) = delete; + +public: + inline MutexReadWrite() + : m_writeRef(0) + , m_writeLocked(0) + , m_writeLocksPending(0) + { + m_readLockMax = 32; + m_readLocked = new XsThreadId[m_readLockMax]; + m_readLockCount = 0; + } + inline ~MutexReadWrite() + { + delete[] m_readLocked; + } +private: + inline void addReadLock(XsThreadId cid) + { + if (m_readLockCount == m_readLockMax) { + m_readLockMax = m_readLockMax * 2; + XsThreadId* tmp = new XsThreadId[m_readLockMax]; + memcpy(tmp, m_readLocked, ((unsigned int) m_readLockCount)*sizeof(XsThreadId)); delete[] m_readLocked; + m_readLocked = tmp; } - private: - inline void addReadLock(XsThreadId cid) - { - if (m_readLockCount == m_readLockMax) - { - m_readLockMax = m_readLockMax*2; - XsThreadId* tmp = new XsThreadId[m_readLockMax]; - memcpy(tmp, m_readLocked, ((unsigned int) m_readLockCount)*sizeof(XsThreadId)); - delete[] m_readLocked; - m_readLocked = tmp; - } - m_readLocked[m_readLockCount++] = cid; - } + m_readLocked[m_readLockCount++] = cid; + } - inline void enterAtomic() - { - m_access.claimMutex(); - } + inline void enterAtomic() + { + m_access.claimMutex(); + } - inline void leaveAtomic() - { - m_access.releaseMutex(); - } + inline void leaveAtomic() + { + m_access.releaseMutex(); + } - inline bool claimMutex(bool write) + inline bool claimMutex(bool write) + { + XsThreadId cid = xsGetCurrentThreadId(); + // write lock + if (write) { - XsThreadId cid = xsGetCurrentThreadId(); - // write lock - if (write) + enterAtomic(); + ++m_writeLocksPending; + while (1) { - enterAtomic(); - ++m_writeLocksPending; - while(1) + bool readFail = false; + if (m_writeLocked == 0) { - bool readFail = false; - if (m_writeLocked == 0) + for (int i = 0; i < m_readLockCount; ++i) { - for (int i = 0; i < m_readLockCount; ++i) + if (m_readLocked[i] != cid) { - if (m_readLocked[i] != cid) - { - readFail = true; - break; - } + readFail = true; + break; } } + } - if (m_writeLocked != cid && (m_writeLocked != 0 || readFail)) - { - leaveAtomic(); - xsYield(); // wait for other locks to end - enterAtomic(); - } - else - { - ++m_writeRef; - --m_writeLocksPending; - m_writeLocked = cid; - leaveAtomic(); - return true; - } + if (m_writeLocked != cid && (m_writeLocked != 0 || readFail)) + { + leaveAtomic(); + xsYield(); // wait for other locks to end + enterAtomic(); + } + else + { + ++m_writeRef; + --m_writeLocksPending; + m_writeLocked = cid; + leaveAtomic(); + return true; } } - // read lock - else - while(1) + } + // read lock + else + { + while (true) { enterAtomic(); bool readGot = false; @@ -363,91 +367,303 @@ namespace xsens { xsYield(); } } + } + + /*! \brief Downgrade an existing write lock to a read lock */ + inline bool downgradeToRead() + { + XsThreadId cid = xsGetCurrentThreadId(); + enterAtomic(); + if (m_writeLocked == cid) + { + if (--m_writeRef == 0) + m_writeLocked = 0; + addReadLock(cid); + leaveAtomic(); + return true; + } + leaveAtomic(); + return false; + } - /*! \brief Downgrade an existing write lock to a read lock */ - inline bool downgradeToRead() + inline bool releaseMutex(bool write) + { + XsThreadId cid = xsGetCurrentThreadId(); + enterAtomic(); + if (write) { - XsThreadId cid = xsGetCurrentThreadId(); - enterAtomic(); if (m_writeLocked == cid) { if (--m_writeRef == 0) m_writeLocked = 0; - addReadLock(cid); + } + else + { leaveAtomic(); - return true; + return false; } - leaveAtomic(); - return false; } + else + { + if (m_readLockCount == 0) + { + leaveAtomic(); + return false; + } + for (int i = m_readLockCount - 1; i >= 0; --i) + { + if (m_readLocked[i] == cid) + { + for (int j = i + 1; j < m_readLockCount; ++j) + m_readLocked[j - 1] = m_readLocked[j]; + --m_readLockCount; + break; + } + } + } + leaveAtomic(); + return true; + } + + inline bool tryClaimMutex(bool write, uint32_t timeout) + { + XsThreadId cid = xsGetCurrentThreadId(); + long long timeToEnd = XsTimeStamp_now(0) + timeout; - inline bool releaseMutex(bool write) + // write lock + if (write) { - XsThreadId cid = xsGetCurrentThreadId(); enterAtomic(); - if (write) + ++m_writeLocksPending; + leaveAtomic(); + do { - if (m_writeLocked == cid) + enterAtomic(); + bool readFail = false; + if (m_writeLocked == 0) + { + for (int i = 0; i < m_readLockCount; ++i) + { + if (m_readLocked[i] != cid) + { + readFail = true; + break; + } + } + } + + if (m_writeLocked != cid && (m_writeLocked != 0 || readFail)) { - if (--m_writeRef == 0) - m_writeLocked = 0; + leaveAtomic(); + // wait for other locks to end + xsYield(); } else { + ++m_writeRef; + --m_writeLocksPending; + m_writeLocked = cid; leaveAtomic(); - return false; + return true; } - } + } while (XsTimeStamp_now(0) >= timeToEnd); + enterAtomic(); + --m_writeLocksPending; + leaveAtomic(); + return false; + } + // read lock + // else + do + { + enterAtomic(); + + bool readGot = false; + if ((m_writeLocked == 0 && m_writeLocksPending == 0) || m_writeLocked == cid) + readGot = true; else { - if (m_readLockCount == 0) + for (int i = 0; i < m_readLockCount; ++i) { - leaveAtomic(); - return false; - } - for (int i = m_readLockCount-1; i >= 0; --i) if (m_readLocked[i] == cid) { - for (int j = i+1; j < m_readLockCount; ++j) - m_readLocked[j-1] = m_readLocked[j]; - --m_readLockCount; + readGot = true; break; } + } + } + + if (readGot) + { + addReadLock(cid); + leaveAtomic(); + return true; } leaveAtomic(); - return true; + // wait for write lock to end + xsYield(); + } while (XsTimeStamp_now(0) <= timeToEnd); + return false; + } + +#ifdef XSENS_DEBUG +public: // these functions should only be used internally and in assert statements +#endif + inline bool haveWriteClaim() const + { + bool rv = false; + XsThreadId cid = xsGetCurrentThreadId(); + + const_cast(this)->enterAtomic(); + if (m_writeLocked == cid) + rv = true; + const_cast(this)->leaveAtomic(); + return rv; + } + // this function is only intended as a debug function for use in assert statements + inline bool haveReadClaim() const + { + bool rv = false; + XsThreadId cid = xsGetCurrentThreadId(); + + const_cast(this)->enterAtomic(); + if (m_writeLocked == cid) + rv = true; + else + { + for (int i = 0; i < m_readLockCount; ++i) + { + if (m_readLocked[i] == cid) + { + rv = true; + break; + } + } } + const_cast(this)->leaveAtomic(); + return rv; + } +}; + +/*! \brief An enum that hold mutex status flags +*/ +enum MutexStatusFlags : int +{ + MSF_HaveReadLock = 1, + MSF_HaveWriteLock = 2, + MSF_WriteLockSuspended = 4, + MSF_WriteLocked = 8, + MSF_ReadLocked = 16, + MSF_WriteLocksPending = 32, + MSF_GuardedLocked = 64, +}; + +/*! \class MutexReadWriteSuspendable + \brief A readers-writer mutex class that is able to be suspended +*/ +class MutexReadWriteSuspendable +{ +private: + Mutex m_access; + volatile std::atomic_int m_writeRef; + volatile std::atomic m_writeLocked; + volatile std::atomic_int m_writeLocksPending; + volatile std::atomic_int m_writeLockSuspendCount; + XsThreadId* m_readLocked; + + int m_readLockMax; + int m_readLockCount; + friend class LockSuspendable; + MutexReadWriteSuspendable(MutexReadWriteSuspendable const&) = delete; + MutexReadWriteSuspendable& operator=(MutexReadWriteSuspendable const&) = delete; + MutexReadWriteSuspendable(MutexReadWriteSuspendable&&) = delete; + MutexReadWriteSuspendable& operator=(MutexReadWriteSuspendable&&) = delete; +public: + + /*! \brief An enum class that holds a three different modes of a suspendable mutex + */ + enum class Mode + { + Read, + Write, + SuspendedWrite + }; + + inline MutexReadWriteSuspendable() + : m_writeRef(0) + , m_writeLocked(0) + , m_writeLocksPending(0) + , m_writeLockSuspendCount(0) + , m_readLockMax(32) + , m_readLockCount(0) + { + m_readLocked = new XsThreadId[m_readLockMax]; + } - inline bool tryClaimMutex(bool write, uint32_t timeout) + inline ~MutexReadWriteSuspendable() + { + delete[] m_readLocked; + } +private: + inline void addReadLock(XsThreadId cid) + { + if (m_readLockCount == m_readLockMax) { - XsThreadId cid = xsGetCurrentThreadId(); - long long timeToEnd = XsTimeStamp_now(0) + timeout; + m_readLockMax = m_readLockMax * 2; + XsThreadId* tmp = new XsThreadId[m_readLockMax]; + memcpy(tmp, m_readLocked, ((unsigned int)m_readLockCount)*sizeof(XsThreadId)); + delete[] m_readLocked; + m_readLocked = tmp; + } + m_readLocked[m_readLockCount++] = cid; + } - // write lock - if (write) - { + inline void enterAtomic() + { + m_access.claimMutex(); + } + + inline void leaveAtomic() + { + m_access.releaseMutex(); + } + +protected: + + /*! \brief Claims (locks) a mutex using a given mode + \param mode The mode to use + \returns True if successful + */ + inline bool claimMutex(Mode mode) + { + XsThreadId cid = xsGetCurrentThreadId(); + // write lock + switch (mode) + { + case Mode::Write: enterAtomic(); ++m_writeLocksPending; - leaveAtomic(); - do + while (true) { - enterAtomic(); bool readFail = false; - if (m_writeLocked == 0) + if (m_writeLocked == 0 || (m_writeLockSuspendCount && m_writeLockSuspendCount == m_writeRef)) { for (int i = 0; i < m_readLockCount; ++i) + { if (m_readLocked[i] != cid) { readFail = true; break; } + } } - if (m_writeLocked != cid && (m_writeLocked != 0 || readFail)) + if ((m_writeLocked != cid && (m_writeLocked != 0 || readFail)) + || (readFail && m_writeLockSuspendCount && m_writeLockSuspendCount == m_writeRef)) { leaveAtomic(); - // wait for other locks to end - xsYield(); + xsYield(); // wait for other locks to end + enterAtomic(); } else { @@ -457,221 +673,18 @@ namespace xsens { leaveAtomic(); return true; } - } while(XsTimeStamp_now(0) >= timeToEnd); - enterAtomic(); - --m_writeLocksPending; - leaveAtomic(); - return false; - } - // read lock - // else - do { - enterAtomic(); + } + break; - bool readGot = false; - if ((m_writeLocked == 0 && m_writeLocksPending == 0) || m_writeLocked == cid) - readGot = true; - else - { - for (int i = 0; i < m_readLockCount; ++i) - if (m_readLocked[i] == cid) - { - readGot = true; - break; - } - } - - if (readGot) - { - addReadLock(cid); - leaveAtomic(); - return true; - } - leaveAtomic(); - // wait for write lock to end - xsYield(); - } while (XsTimeStamp_now(0) <= timeToEnd); - return false; - } - -#ifdef XSENS_DEBUG - public: // these functions should only be used internally and in assert statements -#endif - inline bool haveWriteClaim() const - { - bool rv = false; - XsThreadId cid = xsGetCurrentThreadId(); - - const_cast(this)->enterAtomic(); - if (m_writeLocked == cid) - rv = true; - const_cast(this)->leaveAtomic(); - return rv; - } - // this function is only intended as a debug function for use in assert statements - inline bool haveReadClaim() const - { - bool rv = false; - XsThreadId cid = xsGetCurrentThreadId(); - - const_cast(this)->enterAtomic(); - if (m_writeLocked == cid) - rv = true; - else - { - for (int i = 0; i < m_readLockCount; ++i) - if (m_readLocked[i] == cid) - { - rv = true; - break; - } - } - const_cast(this)->leaveAtomic(); - return rv; - } - }; - - /*! \brief An enum that hold mutex status flags - */ - enum MutexStatusFlags : int { - MSF_HaveReadLock = 1, - MSF_HaveWriteLock = 2, - MSF_WriteLockSuspended = 4, - MSF_WriteLocked = 8, - MSF_ReadLocked = 16, - MSF_WriteLocksPending = 32, - MSF_GuardedLocked = 64, - }; - - /*! \class MutexReadWriteSuspendable - \brief A readers-writer mutex class that is able to be suspended - */ - class MutexReadWriteSuspendable - { - private: - Mutex m_access; - volatile std::atomic_int m_writeRef; - volatile std::atomic m_writeLocked; - volatile std::atomic_int m_writeLocksPending; - volatile std::atomic_int m_writeLockSuspendCount; - XsThreadId* m_readLocked; - - int m_readLockMax; - int m_readLockCount; - friend class LockSuspendable; - MutexReadWriteSuspendable(MutexReadWriteSuspendable const&) = delete; - MutexReadWriteSuspendable& operator=(MutexReadWriteSuspendable const&) = delete; - MutexReadWriteSuspendable(MutexReadWriteSuspendable &&) = delete; - MutexReadWriteSuspendable& operator=(MutexReadWriteSuspendable &&) = delete; - public: - - /*! \brief An enum class that holds a three different modes of a suspendable mutex - */ - enum class Mode - { - Read, - Write, - SuspendedWrite - }; - - inline MutexReadWriteSuspendable() - : m_writeRef(0) - , m_writeLocked(0) - , m_writeLocksPending(0) - , m_writeLockSuspendCount(0) - , m_readLockMax(32) - , m_readLockCount(0) - { - m_readLocked = new XsThreadId[m_readLockMax]; - } - - inline ~MutexReadWriteSuspendable() - { - delete[] m_readLocked; - } - private: - inline void addReadLock(XsThreadId cid) - { - if (m_readLockCount == m_readLockMax) - { - m_readLockMax = m_readLockMax*2; - XsThreadId* tmp = new XsThreadId[m_readLockMax]; - memcpy(tmp, m_readLocked, ((unsigned int)m_readLockCount)*sizeof(XsThreadId)); - delete[] m_readLocked; - m_readLocked = tmp; - } - m_readLocked[m_readLockCount++] = cid; - } - - inline void enterAtomic() - { - m_access.claimMutex(); - } - - inline void leaveAtomic() - { - m_access.releaseMutex(); - } - - protected: - - /*! \brief Claims (locks) a mutex using a given mode - \param mode The mode to use - \returns True if successful - */ - inline bool claimMutex(Mode mode) - { - XsThreadId cid = xsGetCurrentThreadId(); - // write lock - switch (mode) - { - case Mode::Write: - enterAtomic(); - ++m_writeLocksPending; - while(1) - { - bool readFail = false; - if (m_writeLocked == 0 || (m_writeLockSuspendCount && m_writeLockSuspendCount == m_writeRef)) - { - for (int i = 0; i < m_readLockCount; ++i) - { - if (m_readLocked[i] != cid) - { - readFail = true; - break; - } - } - } - - if ((m_writeLocked != cid && (m_writeLocked != 0 || readFail)) - || (readFail && m_writeLockSuspendCount && m_writeLockSuspendCount == m_writeRef)) - { - leaveAtomic(); - xsYield(); // wait for other locks to end - enterAtomic(); - } - else - { - ++m_writeRef; - --m_writeLocksPending; - m_writeLocked = cid; - leaveAtomic(); - return true; - } - } - break; - - case Mode::Read: - while (1) + case Mode::Read: + while (true) { enterAtomic(); bool readGot = false; if ((m_writeLocked == 0 && m_writeLocksPending == 0) || m_writeLocked == cid || (m_writeLockSuspendCount && m_writeLockSuspendCount == m_writeRef)) - { readGot = true; - } else { for (int i = 0; i < m_readLockCount; ++i) @@ -699,7 +712,7 @@ namespace xsens { case Mode::SuspendedWrite: enterAtomic(); ++m_writeLocksPending; - while(1) + while (1) { if (m_writeLocked != cid && m_writeLocked != 0) { @@ -721,630 +734,666 @@ namespace xsens { default: break; - } - - assert(0); - return false; } - /*! \brief Claims (locks) a mutex for read or write modes - \param write If set to true write mode is used, otherwise read mode - \returns True if successful - \note Made for backwards compatibility - */ - inline bool claimMutex(bool write) - { - return claimMutex(write ? Mode::Write : Mode::Read); - } + assert(0); + return false; + } + + /*! \brief Claims (locks) a mutex for read or write modes + \param write If set to true write mode is used, otherwise read mode + \returns True if successful + \note Made for backwards compatibility + */ + inline bool claimMutex(bool write) + { + return claimMutex(write ? Mode::Write : Mode::Read); + } - /*! \brief Downgrade an existing write lock to a read lock - */ - inline bool downgradeToRead(bool decreaseSuspend) + /*! \brief Downgrade an existing write lock to a read lock + */ + inline bool downgradeToRead(bool decreaseSuspend) + { + XsThreadId cid = xsGetCurrentThreadId(); + enterAtomic(); + if (m_writeLocked == cid) { - XsThreadId cid = xsGetCurrentThreadId(); - enterAtomic(); - if (m_writeLocked == cid) + if (decreaseSuspend) + --m_writeLockSuspendCount; + if (--m_writeRef == 0) { - if (decreaseSuspend) - --m_writeLockSuspendCount; - if (--m_writeRef == 0) - { - m_writeLocked = 0; - m_writeLockSuspendCount = 0; - } - addReadLock(cid); - leaveAtomic(); - return true; + m_writeLocked = 0; + m_writeLockSuspendCount = 0; } + addReadLock(cid); leaveAtomic(); - return false; + return true; } + leaveAtomic(); + return false; + } - /*! \brief Suspend an existing write lock so read locks are allowed, but do not allow other write locks - */ - inline bool suspendWriteLock() + /*! \brief Suspend an existing write lock so read locks are allowed, but do not allow other write locks + */ + inline bool suspendWriteLock() + { + XsThreadId cid = xsGetCurrentThreadId(); + enterAtomic(); + if (m_writeLocked == cid) { - XsThreadId cid = xsGetCurrentThreadId(); - enterAtomic(); - if (m_writeLocked == cid) - { - ++m_writeLockSuspendCount; - leaveAtomic(); - return true; - } + ++m_writeLockSuspendCount; leaveAtomic(); - return false; + return true; } + leaveAtomic(); + return false; + } - /*! \brief Resumes an existing write lock so read locks are no longer allowed - */ - inline bool resumeWriteLock() + /*! \brief Resumes an existing write lock so read locks are no longer allowed + */ + inline bool resumeWriteLock() + { + XsThreadId cid = xsGetCurrentThreadId(); + enterAtomic(); + if (m_writeLocked == cid) { - XsThreadId cid = xsGetCurrentThreadId(); - enterAtomic(); - if (m_writeLocked == cid) + if (m_writeLockSuspendCount-- == m_writeRef) { - if (m_writeLockSuspendCount-- == m_writeRef) + // wait for all read locks to end + while (true) { - // wait for all read locks to end - while(1) + bool stillReading = false; + for (int i = 0; i < m_readLockCount; ++i) { - bool stillReading = false; - for (int i = 0; i < m_readLockCount; ++i) + if (m_readLocked[i] != cid) { - if (m_readLocked[i] != cid) - { - stillReading = true; - break; - } + stillReading = true; + break; } + } - if (!stillReading) - { - leaveAtomic(); - return true; - } + if (!stillReading) + { leaveAtomic(); - xsYield(); - enterAtomic(); + return true; } + leaveAtomic(); + xsYield(); + enterAtomic(); } - leaveAtomic(); - return true; } leaveAtomic(); - return false; + return true; } + leaveAtomic(); + return false; + } - /*! \brief Releases (unlocks) a mutex for read or write modes - \param write If set to true, write mode is used, otherwise read mode - \param decreaseSuspendCount If set to true, it will decrease a suspend count - \returns True if successful - */ - inline bool releaseMutex(bool write, bool decreaseSuspendCount) + /*! \brief Releases (unlocks) a mutex for read or write modes + \param write If set to true, write mode is used, otherwise read mode + \param decreaseSuspendCount If set to true, it will decrease a suspend count + \returns True if successful + */ + inline bool releaseMutex(bool write, bool decreaseSuspendCount) + { + XsThreadId cid = xsGetCurrentThreadId(); + enterAtomic(); + if (write) { - XsThreadId cid = xsGetCurrentThreadId(); - enterAtomic(); - if (write) + if (m_writeLocked == cid) { - if (m_writeLocked == cid) - { - if (decreaseSuspendCount) - --m_writeLockSuspendCount; + if (decreaseSuspendCount) + --m_writeLockSuspendCount; - if (--m_writeRef == 0) - { - m_writeLockSuspendCount = 0; - m_writeLocked = 0; - } - } - else + if (--m_writeRef == 0) { - leaveAtomic(); - return false; + m_writeLockSuspendCount = 0; + m_writeLocked = 0; } } else { - if (m_readLockCount == 0) - { - leaveAtomic(); - return false; - } - for (int i = m_readLockCount-1; i >= 0; --i) - if (m_readLocked[i] == cid) - { - for (int j = i+1; j < m_readLockCount; ++j) - m_readLocked[j-1] = m_readLocked[j]; - --m_readLockCount; - break; - } + leaveAtomic(); + return false; } - leaveAtomic(); - return true; - } - - /*! \brief Releases (unlocks) a mutex using a given mode - \param mode The mode to use - \param decreaseSuspendCount If set to true, it will decrease a suspend count - \returns True if successful - */ - inline bool releaseMutex(Mode mode, bool decreaseSuspendCount) - { - return releaseMutex(mode != Mode::Read, decreaseSuspendCount); } - public: - - /*! \returns The status of the current thread - \sa MutexStatusFlags - */ - int status() const volatile + else { - int rv = 0; - XsThreadId cid = xsGetCurrentThreadId(); - const_cast(this)->enterAtomic(); - if (m_writeRef) + if (m_readLockCount == 0) { - rv |= MSF_WriteLocked; - if (m_writeLocked == cid) - rv |= MSF_HaveWriteLock; - if (m_writeLockSuspendCount == m_writeRef) - rv |= MSF_WriteLockSuspended; + leaveAtomic(); + return false; } - if (m_writeLocksPending) - rv |= MSF_WriteLocksPending; - if (m_readLockCount) - rv |= MSF_ReadLocked; - for (int i = 0; i < m_readLockCount; ++i) + for (int i = m_readLockCount - 1; i >= 0; --i) { if (m_readLocked[i] == cid) { - rv |= MSF_HaveReadLock; + for (int j = i + 1; j < m_readLockCount; ++j) + m_readLocked[j - 1] = m_readLocked[j]; + --m_readLockCount; break; } } - const_cast(this)->leaveAtomic(); - return rv; } + leaveAtomic(); + return true; + } -#ifndef XSENS_DEBUG - protected: - // these functions should only be used internally and in assert statements -#endif - /*! \returns True if is has an write lock - */ - inline bool haveWriteClaim() const volatile + /*! \brief Releases (unlocks) a mutex using a given mode + \param mode The mode to use + \param decreaseSuspendCount If set to true, it will decrease a suspend count + \returns True if successful + */ + inline bool releaseMutex(Mode mode, bool decreaseSuspendCount) + { + return releaseMutex(mode != Mode::Read, decreaseSuspendCount); + } +public: + + /*! \returns The status of the current thread + \sa MutexStatusFlags + */ + int status() const volatile + { + int rv = 0; + XsThreadId cid = xsGetCurrentThreadId(); + const_cast(this)->enterAtomic(); + if (m_writeRef) { - return (status() & MSF_HaveWriteLock) != 0; + rv |= MSF_WriteLocked; + if (m_writeLocked == cid) + rv |= MSF_HaveWriteLock; + if (m_writeLockSuspendCount == m_writeRef) + rv |= MSF_WriteLockSuspended; } - - /*! \returns True if is has a read lock - */ - inline bool haveReadClaim() const volatile + if (m_writeLocksPending) + rv |= MSF_WriteLocksPending; + if (m_readLockCount) + rv |= MSF_ReadLocked; + for (int i = 0; i < m_readLockCount; ++i) { - return (status() & (MSF_HaveWriteLock | MSF_HaveReadLock)) != 0; + if (m_readLocked[i] == cid) + { + rv |= MSF_HaveReadLock; + break; + } } - }; + const_cast(this)->leaveAtomic(); + return rv; + } - /*! \class Lock - \brief A base class for a ock +#ifndef XSENS_DEBUG +protected: + // these functions should only be used internally and in assert statements +#endif + /*! \returns True if is has an write lock */ - class Lock { - private: - Mutex* m_mutex; - volatile std::atomic_bool m_locked; - CHECK_LOCK_DEFINE(); + inline bool haveWriteClaim() const volatile + { + return (status() & MSF_HaveWriteLock) != 0; + } - Lock(Lock const&) = delete; - Lock& operator=(Lock const&) = delete; - Lock(Lock && rhs) - : m_mutex(rhs.m_mutex) - , m_locked(rhs.m_locked) + /*! \returns True if is has a read lock + */ + inline bool haveReadClaim() const volatile + { + return (status() & (MSF_HaveWriteLock | MSF_HaveReadLock)) != 0; + } +}; + +/*! \class Lock + \brief A base class for a Lock +*/ +class Lock +{ +private: + Mutex* m_mutex; + volatile std::atomic_bool m_locked; + CHECK_LOCK_DEFINE(); + + Lock(Lock const&) = delete; + Lock& operator=(Lock const&) = delete; + Lock(Lock&& rhs) + : m_mutex(rhs.m_mutex) + , m_locked(rhs.m_locked) + { + rhs.m_mutex = nullptr; + rhs.m_locked = false; + } + Lock& operator=(Lock&& rhs) + { + if (this != &rhs) { + m_mutex = rhs.m_mutex; + m_locked = rhs.m_locked; rhs.m_mutex = nullptr; rhs.m_locked = false; } - Lock& operator=(Lock && rhs) - { - if (this != &rhs) - { - m_mutex = rhs.m_mutex; - m_locked = rhs.m_locked; - rhs.m_mutex = nullptr; - rhs.m_locked = false; - } - return *this; - } + return *this; + } + +public: - public: + /*! \brief Constructs a lock a given mutex + \param mutex The pointer to a mutex to lock + */ + inline Lock(Mutex* mutex) : m_mutex(mutex), m_locked(false) + { + CHECK_LOCK_ACQUIRE(); + m_locked = m_mutex->claimMutex(); + } - /*! \brief Constructs a lock a given mutex - \param mutex The pointer to a mutex to lock - */ - inline Lock(Mutex* mutex) : m_mutex(mutex), m_locked(false) + /*! \brief Constructs a lock a given mutex + \param mutex The pointer to a mutex to lock + \param lockit If set to true then it will lock a mutex + */ + inline Lock(Mutex* mutex, bool lockit) : m_mutex(mutex), m_locked(false) + { + if (lockit) { CHECK_LOCK_ACQUIRE(); m_locked = m_mutex->claimMutex(); } + } - /*! \brief Constructs a lock a given mutex - \param mutex The pointer to a mutex to lock - \param lockit If set to true then it will lock a mutex - */ - inline Lock(Mutex* mutex, bool lockit) : m_mutex(mutex), m_locked(false) + /*! \brief Constructs a lock a given mutex + \param mutex The pointer to a mutex to lock + \param lockit If is not set to an unlocked state then it will lock it + */ + inline Lock(Mutex* mutex, LockState lockit) : m_mutex(mutex), m_locked(false) + { + if (lockit != LS_Unlocked) { - if (lockit) - { - CHECK_LOCK_ACQUIRE(); - m_locked = m_mutex->claimMutex(); - } + CHECK_LOCK_ACQUIRE(); + m_locked = m_mutex->claimMutex(); } + } + inline ~Lock() + { + unlock(); + } - /*! \brief Constructs a lock a given mutex - \param mutex The pointer to a mutex to lock - \param lockit If is not set to an unlocked state then it will lock it - */ - inline Lock(Mutex* mutex, LockState lockit) : m_mutex(mutex), m_locked(false) + /*! \brief Locks the unlocked mutex + \returns True if the mutex is successfully locked + */ + inline bool lock() + { + if (!m_locked) { - if (lockit != LS_Unlocked) - { - CHECK_LOCK_ACQUIRE(); - m_locked = m_mutex->claimMutex(); - } + CHECK_LOCK_ACQUIRE(); + return (m_locked = m_mutex->claimMutex()) != 0; } - inline ~Lock() + return true; + } + + /*! \brief Unlocks the locked mutex + \returns True if the mutex is successfully unlocked + */ + inline bool unlock() noexcept + { + if (m_locked) { - unlock(); + XSENS_MSC_WARNING_SUPPRESS(4706) + CHECK_LOCK_RELEASE(); + return !(m_locked = !m_mutex->releaseMutex()); } + return true; + } - /*! \brief Locks the unlocked mutex - \returns True if the mutex is successfully locked - */ - inline bool lock() - { - if (!m_locked) - { - CHECK_LOCK_ACQUIRE(); - return (m_locked = m_mutex->claimMutex()) != 0; - } - return true; - } + /*! \brief Locks the unlocked mutex + \param ls LockState, either LS_Unlocked to unlock or any other value to lock + \returns True if the mutex is successfully locked + */ + inline bool lock(LockState ls) + { + if (ls == LS_Unlocked) + return unlock(); + else + return lock(); + } - /*! \brief Unlocks the locked mutex - \returns True if the mutex is successfully unlocked - */ - inline bool unlock() noexcept + /*! \brief Tries to lock the unlocked mutex + \returns True if the mutex is successfully locked + */ + inline bool tryLock() + { + if (!m_locked) { - if (m_locked) - { -#ifndef __GNUC__ - #pragma warning(suppress: 4706) -#endif - CHECK_LOCK_RELEASE(); - return !(m_locked = !m_mutex->releaseMutex()); - } - return true; + CHECK_LOCK_ACQUIRE(); + return (m_locked = m_mutex->tryClaimMutex()); } + return true; + } - /*! \brief Tries to lock the unlocked mutex - \returns True if the mutex is successfully locked - */ - inline bool tryLock() - { - if (!m_locked) - { - CHECK_LOCK_ACQUIRE(); - return (m_locked = m_mutex->tryClaimMutex()); - } - return true; - } + /*! \brief Tries to lock the unlocked mutex + \param ls LockState, either LS_Unlocked to unlock or any other value to lock + \returns True if the mutex is successfully locked + */ + inline bool tryLock(LockState ls) + { + if (ls == LS_Unlocked) + return unlock(); + else + return tryLock(); + } - /*! \returns True if the mutex is locked*/ - inline bool isLocked() const - { - return m_locked; - } - }; + /*! \returns True if the mutex is locked*/ + inline bool isLocked() const + { + return m_locked; + } +}; + +/*! \class LockReadWrite + \brief A readers-writer lock class +*/ +class LockReadWrite +{ +private: + MutexReadWrite* m_mutex; + volatile std::atomic_bool m_lockedR; + volatile std::atomic_bool m_lockedW; + CHECK_LOCK_DEFINE(); + + LockReadWrite(LockReadWrite const&) = delete; + LockReadWrite& operator=(LockReadWrite const&) = delete; + LockReadWrite(LockReadWrite&&) = delete; + LockReadWrite& operator=(LockReadWrite&&) = delete; +public: + + /*! \brief Consturctor + */ + inline LockReadWrite(MutexReadWrite* mutex, LockState lockState = LS_Unlocked) : m_mutex(mutex), m_lockedR(false), m_lockedW(false) + { +#if defined(XSENS_DEBUG) + assert(m_mutex != NULL); +#endif - /*! \class LockReadWrite - \brief A readers-writer lock class - */ - class LockReadWrite { - private: - MutexReadWrite* m_mutex; - volatile std::atomic_bool m_lockedR; - volatile std::atomic_bool m_lockedW; - CHECK_LOCK_DEFINE(); - - LockReadWrite(LockReadWrite const&) = delete; - LockReadWrite& operator=(LockReadWrite const&) = delete; - LockReadWrite(LockReadWrite &&) = delete; - LockReadWrite& operator=(LockReadWrite &&) = delete; - public: - - /*! \brief Consturctor - */ - inline LockReadWrite(MutexReadWrite* mutex, LockState lockState = LS_Unlocked) : m_mutex(mutex), m_lockedR(false), m_lockedW(false) - { - #if defined(XSENS_DEBUG) - assert(m_mutex != NULL); - #endif - - if (lockState == LS_Read) - lock(false); - else if (lockState == LS_Write) - lock(true); - else - assert(lockState != LS_SuspendedWrite); - } - inline ~LockReadWrite() + if (lockState == LS_Read) + lock(false); + else if (lockState == LS_Write) + lock(true); + else + assert(lockState != LS_SuspendedWrite); + } + inline ~LockReadWrite() + { + unlock(); + } + + /*! \brief Make sure that the lock has exactly the given lock state + \details The lock will be upgraded/downgraded as required, possibly unlocking in between + \param write If set to true, write mode is used, otherwise read mode + \returns True if successfully locked + */ + inline bool lock(bool write) + { + CHECK_LOCK_ACQUIRE(); + if (write) { - unlock(); + if (m_lockedW) + return true; + if (m_lockedR) + unlock(); + m_lockedW = m_mutex->claimMutex(true); + assert(m_lockedW); + return m_lockedW; } - - /*! \brief Make sure that the lock has exactly the given lock state - \details The lock will be upgraded/downgraded as required, possibly unlocking in between - \param write If set to true, write mode is used, otherwise read mode - \returns True if successfully locked - */ - inline bool lock(bool write) + else { - CHECK_LOCK_ACQUIRE(); - if (write) - { - if (m_lockedW) - return true; - if (m_lockedR) - unlock(); - m_lockedW = m_mutex->claimMutex(true); - assert(m_lockedW); - return m_lockedW; - } - else + if (m_lockedW) { - if (m_lockedW) - { - m_lockedR = m_mutex->downgradeToRead(); - m_lockedW = false; - assert(m_lockedR); - } - if (!m_lockedR) - m_lockedR = m_mutex->claimMutex(false); + m_lockedR = m_mutex->downgradeToRead(); + m_lockedW = false; assert(m_lockedR); - return m_lockedR; } + if (!m_lockedR) + m_lockedR = m_mutex->claimMutex(false); + assert(m_lockedR); + return m_lockedR; } + } - /*! \brief Convenience function that accepts the generic LockState, forwards to lock(bool) - \param ls If set not to read lock state then it will lock the mutex - \returns True if successfully locked - */ - inline bool lock(LockState ls) + /*! \brief Convenience function that accepts the generic LockState, forwards to lock(bool) + \param ls If set not to read lock state then it will lock the mutex + \returns True if successfully locked + */ + inline bool lock(LockState ls) + { + assert(ls == LS_Read || ls == LS_Write); + return lock(ls != LS_Read); + } + /*! \brief Convenience function for lock(false); + \return True if successful + */ + inline bool lockRead() + { + return lock(false); + } + + /*! \brief Convenience function for lock(true); + \return True if successful + */ + inline bool lockWrite() + { + return lock(true); + } + + /*! \brief Unlocks the write or read locked mutex + \returns True if the mutex is successfully unlocked. False if it was not locked. + */ + inline bool unlock() noexcept + { + CHECK_LOCK_RELEASE(); + if (m_lockedW) { - assert(ls == LS_Read || ls == LS_Write); - return lock(ls != LS_Read); + assert(!m_lockedR); + m_lockedW = false; + return m_mutex->releaseMutex(true); } - /*! \brief Convenience function for lock(false); - \return True if successful - */ - inline bool lockRead() { return lock(false); } - - /*! \brief Convenience function for lock(true); - \return True if successful - */ - inline bool lockWrite() { return lock(true); } - - /*! \brief Unlocks the write or read locked mutex - \returns True if the mutex is successfully unlocked. False if it was not locked. - */ - inline bool unlock() noexcept + if (m_lockedR) + { + m_lockedR = false; + return m_mutex->releaseMutex(false); + } + return false; + } + + /*! \brief Tries to lock the write/read unlocked mutex before a given time runs out + \param write If set to true, write mode is used, otherwise read mode + \param timeout The timeout value in ms + \returns True if the mutex is successfully locked + */ + inline bool tryLock(bool write, uint32_t timeout = 0) + { + CHECK_LOCK_ACQUIRE(); + if (write) { - CHECK_LOCK_RELEASE(); if (m_lockedW) - { - assert(!m_lockedR); - m_lockedW = false; - return m_mutex->releaseMutex(true); - } + return true; if (m_lockedR) - { - m_lockedR = false; - return m_mutex->releaseMutex(false); - } - return false; + unlock(); + m_lockedW = m_mutex->tryClaimMutex(true, timeout); + assert(m_lockedW); + return m_lockedW; } - - /*! \brief Tries to lock the write/read unlocked mutex before a given time runs out - \param write If set to true, write mode is used, otherwise read mode - \param timeout The timeout value in ms - \returns True if the mutex is successfully locked - */ - inline bool tryLock(bool write, uint32_t timeout) + else { - CHECK_LOCK_ACQUIRE(); - if (write) - { - if (m_lockedW) - return true; - if (m_lockedR) - unlock(); - m_lockedW = m_mutex->tryClaimMutex(true, timeout); - assert(m_lockedW); - return m_lockedW; - } - else + if (m_lockedW) { - if (m_lockedW) - { - m_lockedR = m_mutex->downgradeToRead(); - m_lockedW = false; - assert(m_lockedR); - } - if (!m_lockedR) - m_lockedR = m_mutex->tryClaimMutex(false, timeout); + m_lockedR = m_mutex->downgradeToRead(); + m_lockedW = false; assert(m_lockedR); - return m_lockedR; } + if (!m_lockedR) + m_lockedR = m_mutex->tryClaimMutex(false, timeout); + assert(m_lockedR); + return m_lockedR; } + } - /*! \returns true if the lock has at least the given state - \param write If set to true, write mode is used, otherwise read mode - */ - inline bool isLocked(bool write) const volatile - { - return (!write && m_lockedR) || m_lockedW; - } - - /*! \returns True if the mutex has a write lock - \note Do not use this function internally! The mutex may have a write claim, while the lock does not, but this function will return true. - */ - inline bool haveWriteClaim() const volatile - { - return m_mutex->haveWriteClaim(); - } - }; - - /*! \brief A two-layer mutex, typically used for status+data protection - \details The class provides a main suspendable mutex, which can be used for all kinds of status updates and a - deeper guarded mutex, which will enforce the main mux to be locked before allowing itself to be locked. - - \note The main purpose of this is for multi-threaded data handling in which the mian mux guards the state of the device, - and the guarded mux guards data access. + /*! \returns true if the lock has at least the given state + \param write If set to true, write mode is used, otherwise read mode */ - class GuardedMutex : private MutexReadWriteSuspendable + inline bool isLocked(bool write) const volatile { - private: - Mutex m_guarded; - friend class LockGuarded; - friend class LockSuspendable; - - GuardedMutex(GuardedMutex const&) = delete; - GuardedMutex& operator=(GuardedMutex const&) = delete; - GuardedMutex(GuardedMutex &&) = delete; - GuardedMutex& operator=(GuardedMutex &&) = delete; - - public: + return (!write && m_lockedR) || m_lockedW; + } - /*! \brief Constructor - */ - inline GuardedMutex() - { - } + /*! \returns True if the mutex has a write lock + \note Do not use this function internally! The mutex may have a write claim, while the lock does not, but this function will return true. + */ + inline bool haveWriteClaim() const volatile + { + return m_mutex->haveWriteClaim(); + } +}; + +/*! \brief A two-layer mutex, typically used for status+data protection + \details The class provides a main suspendable mutex, which can be used for all kinds of status updates and a + deeper guarded mutex, which will enforce the main mux to be locked before allowing itself to be locked. + + \note The main purpose of this is for multi-threaded data handling in which the mian mux guards the state of the device, + and the guarded mux guards data access. +*/ +class GuardedMutex : private MutexReadWriteSuspendable +{ +private: + Mutex m_guarded; + friend class LockGuarded; + friend class LockSuspendable; + + GuardedMutex(GuardedMutex const&) = delete; + GuardedMutex& operator=(GuardedMutex const&) = delete; + GuardedMutex(GuardedMutex&&) = delete; + GuardedMutex& operator=(GuardedMutex&&) = delete; + +public: + + /*! \brief Constructor + */ + inline GuardedMutex() + { + } - /*! \brief Destructor - */ - inline ~GuardedMutex() - { - } + /*! \brief Destructor + */ + inline ~GuardedMutex() + { + } - /*! \returns true if the mutex is claimed successfully - */ - inline bool claimMutex() - { - MutexReadWriteSuspendable::claimMutex(Mode::Read); - return m_guarded.claimMutex(); - } + /*! \returns true if the mutex is claimed successfully + */ + inline bool claimMutex() + { + MutexReadWriteSuspendable::claimMutex(Mode::Read); + return m_guarded.claimMutex(); + } - /*! \returns true if the mutex is released successfully - */ - inline bool releaseMutex() - { - m_guarded.releaseMutex(); - MutexReadWriteSuspendable::releaseMutex(false, false); - return true; - } + /*! \returns true if the mutex is released successfully + */ + inline bool releaseMutex() + { + m_guarded.releaseMutex(); + MutexReadWriteSuspendable::releaseMutex(false, false); + return true; + } - /*! \returns The reference to this mutex - \note Give access to the base class for directly checking the state of the mux - */ - MutexReadWriteSuspendable& suspendable() { return *this; } + /*! \returns The reference to this mutex + \note Give access to the base class for directly checking the state of the mux + */ + MutexReadWriteSuspendable& suspendable() + { + return *this; + } - /*! \brief Check if the guarded mutex is equal to the supplied mutex - \param mutex The supplied mutex - \returns True if it is equal - */ - inline bool isUsing(Mutex const* mutex) const - { - return &m_guarded == mutex; - } + /*! \brief Check if the guarded mutex is equal to the supplied mutex + \param mutex The supplied mutex + \returns True if it is equal + */ + inline bool isUsing(Mutex const* mutex) const + { + return &m_guarded == mutex; + } - /*! \returns The status of this mutex*/ - int status() const volatile - { - return MutexReadWriteSuspendable::status() | (m_guarded.isLocked() ? MSF_GuardedLocked : 0); - } + /*! \returns The status of this mutex*/ + int status() const volatile + { + return MutexReadWriteSuspendable::status() | (m_guarded.isLocked() ? MSF_GuardedLocked : 0); + } #ifdef XSENS_DEBUG - // this function is only intended as a debug function for use in assert statements - inline bool haveGuardedLock() const - { - return m_guarded.haveLock(); - } + // this function is only intended as a debug function for use in assert statements + inline bool haveGuardedLock() const + { + return m_guarded.haveLock(); + } #endif #ifndef XSENS_DEBUG - protected: - // these functions should only be used internally and in assert statements +protected: + // these functions should only be used internally and in assert statements #endif - using MutexReadWriteSuspendable::haveWriteClaim; - using MutexReadWriteSuspendable::haveReadClaim; - }; - - /*! \class LockSuspendable - \brief A readers-writer lock class that is able to be suspended + using MutexReadWriteSuspendable::haveWriteClaim; + using MutexReadWriteSuspendable::haveReadClaim; +}; + +/*! \class LockSuspendable + \brief A readers-writer lock class that is able to be suspended +*/ +class LockSuspendable +{ +private: + MutexReadWriteSuspendable* m_mutex; + volatile std::atomic_bool m_lockedR; + volatile std::atomic_bool m_lockedW; + volatile std::atomic_bool m_iSuspended; // Read: I suspended the write lock, this does not mean that it actually IS suspended + + LockSuspendable(LockSuspendable const&) = delete; + LockSuspendable& operator=(LockSuspendable const&) = delete; + LockSuspendable(LockSuspendable&&) = delete; + LockSuspendable& operator=(LockSuspendable&&) = delete; + +public: + + /*! \brief Constructs a lock using a suspendable readers-writer mutex */ - class LockSuspendable { - private: - MutexReadWriteSuspendable* m_mutex; - volatile std::atomic_bool m_lockedR; - volatile std::atomic_bool m_lockedW; - volatile std::atomic_bool m_iSuspended; // Read: I suspended the write lock, this does not mean that it actually IS suspended - - LockSuspendable(LockSuspendable const&) = delete; - LockSuspendable& operator=(LockSuspendable const&) = delete; - LockSuspendable(LockSuspendable &&) = delete; - LockSuspendable& operator=(LockSuspendable &&) = delete; - - public: - - /*! \brief Constructs a lock using a suspendable readers-writer mutex - */ - inline LockSuspendable(MutexReadWriteSuspendable* mutex, LockState lockState) : m_mutex(mutex), m_lockedR(false), m_lockedW(false), m_iSuspended(false) - { - #if defined(XSENS_DEBUG) - assert(m_mutex != NULL); - #endif + inline LockSuspendable(MutexReadWriteSuspendable* mutex, LockState lockState) : m_mutex(mutex), m_lockedR(false), m_lockedW(false), m_iSuspended(false) + { +#if defined(XSENS_DEBUG) + assert(m_mutex != NULL); +#endif - if (lockState != LS_Unlocked) - lock(lockState); - } + if (lockState != LS_Unlocked) + lock(lockState); + } - /*! \brief Constructs a lock using a guarded mutex - */ - inline LockSuspendable(GuardedMutex* mutex, LockState lockState) : m_mutex(mutex), m_lockedR(false), m_lockedW(false), m_iSuspended(false) - { - #if defined(XSENS_DEBUG) - assert(m_mutex != NULL); - #endif + /*! \brief Constructs a lock using a guarded mutex + */ + inline LockSuspendable(GuardedMutex* mutex, LockState lockState) : m_mutex(mutex), m_lockedR(false), m_lockedW(false), m_iSuspended(false) + { +#if defined(XSENS_DEBUG) + assert(m_mutex != NULL); +#endif - if (lockState != LS_Unlocked) - lock(lockState); - } + if (lockState != LS_Unlocked) + lock(lockState); + } - inline ~LockSuspendable() - { - unlock(); - } + inline ~LockSuspendable() + { + unlock(); + } - /*! \brief Make sure that the lock has exactly the given lock state - \details The lock will be upgraded/downgraded as required, possibly unlocking in between - \param desiredLockState If set not to read lock state then it will lock the mutex - \returns True if successfully locked - */ - inline bool lock(LockState desiredLockState) + /*! \brief Make sure that the lock has exactly the given lock state + \details The lock will be upgraded/downgraded as required, possibly unlocking in between + \param desiredLockState If set not to read lock state then it will lock the mutex + \returns True if successfully locked + */ + inline bool lock(LockState desiredLockState) + { + switch (desiredLockState) { - switch (desiredLockState) - { case LS_Write: if (m_lockedW) return resume(); @@ -1383,431 +1432,460 @@ namespace xsens { default: assert(0); return false; - } - } - - /*! \brief Convenience function that accepts the boolean write value, forwards to lock(LockState) - \param write If set to true, write mode is used, otherwise read mode - \returns True if successfully locked - */ - inline bool lock(bool write) - { - return lock(write ? LS_Write : LS_Read); } + } - /*! \brief Convenience function for lock(LS_Read); - \return True if successful - */ - inline bool lockRead() { return lock(LS_Read); } - - /*! \brief Convenience function for lock(LS_Write); - \return True if successful - */ - inline bool lockWrite() { return lock(LS_Write); } - - /*! \brief Convenience function for lock(LS_SuspendedWrite); - \return True if successful - */ - inline bool lockSuspendedWrite() { return lock(LS_SuspendedWrite); } - - /*! \brief Suspend a write lock, allowing readers access, but preventing others from getting a write lock - \returns True if successful or if already suspended - */ - inline bool suspend() - { - assert(m_lockedW); - if (m_iSuspended) - return true; - m_iSuspended = true; - return m_mutex->suspendWriteLock(); - } + /*! \brief Convenience function that accepts the boolean write value, forwards to lock(LockState) + \param write If set to true, write mode is used, otherwise read mode + \returns True if successfully locked + */ + inline bool lock(bool write) + { + return lock(write ? LS_Write : LS_Read); + } - /*! \brief Resume a write lock - \return True if successful or if already resuming - */ - inline bool resume() - { - assert(m_lockedW); - if (!m_iSuspended) - return true; - m_iSuspended = false; - return m_mutex->resumeWriteLock(); - } + /*! \brief Convenience function for lock(LS_Read); + \return True if successful + */ + inline bool lockRead() + { + return lock(LS_Read); + } - /*! \returns True if mutex is suspended - */ - inline bool isSuspended() const - { - return m_iSuspended; - } + /*! \brief Convenience function for lock(LS_Write); + \return True if successful + */ + inline bool lockWrite() + { + return lock(LS_Write); + } - /*! \returns True if mutex is suspended - \note This function check the current status of the mutex to tell whether it is actually suspended - */ - inline bool mutexIsSuspended() const volatile - { - return (m_mutex->status() & (MSF_WriteLockSuspended | MSF_WriteLocked)) == (MSF_WriteLockSuspended | MSF_WriteLocked); - } + /*! \brief Convenience function for lock(LS_SuspendedWrite); + \return True if successful + */ + inline bool lockSuspendedWrite() + { + return lock(LS_SuspendedWrite); + } - /*! \brief Unlocks the write or read locked mutex - \returns True if the mutex is successfully unlocked. False if it was not locked. - */ - inline bool unlock() noexcept - { - if (m_lockedW) - { - assert(!m_lockedR); - m_lockedW = false; - bool s = m_iSuspended; - m_iSuspended = false; - return m_mutex->releaseMutex(true, s); - } - if (m_lockedR) - { - m_lockedR = false; - return m_mutex->releaseMutex(false, false); - } - return false; - } + /*! \brief Suspend a write lock, allowing readers access, but preventing others from getting a write lock + \returns True if successful or if already suspended + */ + inline bool suspend() + { + assert(m_lockedW); + if (m_iSuspended) + return true; + m_iSuspended = true; + return m_mutex->suspendWriteLock(); + } - /*! \returns True if the lock has at least the given state. A suspended write lock is treated as a read lock. - \param minimumState The minimum lock state to check with - */ - inline bool isLocked(LockState minimumState) const - { - bool sus = isSuspended(); - if (minimumState == LS_Write) - return m_lockedW && !sus; - if (minimumState == LS_SuspendedWrite) - return m_lockedW && sus; - if (minimumState == LS_Read) - return m_lockedW || m_lockedR; - assert(minimumState == LS_Unlocked); + /*! \brief Resume a write lock + \return True if successful or if already resuming + */ + inline bool resume() + { + assert(m_lockedW); + if (!m_iSuspended) return true; - } + m_iSuspended = false; + return m_mutex->resumeWriteLock(); + } + + /*! \returns True if mutex is suspended + */ + inline bool isSuspended() const + { + return m_iSuspended; + } - /*! \brief Check if the lock is using the supplied mutex - \param mutex The supplied suspendable readers-writer mutex - \returns True if it is equal - */ - bool isUsing(MutexReadWriteSuspendable const* mutex) const + /*! \returns True if mutex is suspended + \note This function check the current status of the mutex to tell whether it is actually suspended + */ + inline bool mutexIsSuspended() const volatile + { + return (m_mutex->status() & (MSF_WriteLockSuspended | MSF_WriteLocked)) == (MSF_WriteLockSuspended | MSF_WriteLocked); + } + + /*! \brief Unlocks the write or read locked mutex + \returns True if the mutex is successfully unlocked. False if it was not locked. + */ + inline bool unlock() noexcept + { + if (m_lockedW) { - return m_mutex == mutex; + assert(!m_lockedR); + m_lockedW = false; + bool s = m_iSuspended; + m_iSuspended = false; + return m_mutex->releaseMutex(true, s); } - - /*! \brief Check if the lock is using the supplied mutex - \param mutex The supplied guarded mutex - \returns True if it is equal - */ - bool isUsing(GuardedMutex const* mutex) const + if (m_lockedR) { - return m_mutex == mutex; + m_lockedR = false; + return m_mutex->releaseMutex(false, false); } - }; + return false; + } - /*! \class LockGuarded - \brief A guarded lock class + /*! \returns True if the lock has at least the given state. A suspended write lock is treated as a read lock. + \param minimumState The minimum lock state to check with */ - class LockGuarded { - private: - GuardedMutex* m_mutex; - volatile std::atomic_bool m_locked; + inline bool isLocked(LockState minimumState) const + { + bool sus = isSuspended(); + if (minimumState == LS_Write) + return m_lockedW && !sus; + if (minimumState == LS_SuspendedWrite) + return m_lockedW && sus; + if (minimumState == LS_Read) + return m_lockedW || m_lockedR; + assert(minimumState == LS_Unlocked); + return true; + } - LockGuarded(LockGuarded const&) = delete; - LockGuarded& operator=(LockGuarded const&) = delete; - LockGuarded(LockGuarded &&) = delete; - LockGuarded& operator=(LockGuarded &&) = delete; + /*! \brief Check if the lock is using the supplied mutex + \param mutex The supplied suspendable readers-writer mutex + \returns True if it is equal + */ + bool isUsing(MutexReadWriteSuspendable const* mutex) const + { + return m_mutex == mutex; + } - public: + /*! \brief Check if the lock is using the supplied mutex + \param mutex The supplied guarded mutex + \returns True if it is equal + */ + bool isUsing(GuardedMutex const* mutex) const + { + return m_mutex == mutex; + } +}; - /*! \brief Constructs a guarded lock using a guarded mutex - */ - inline LockGuarded(GuardedMutex* mutex, bool lockit = true) : m_mutex(mutex), m_locked(false) - { - if (lockit) - m_locked = m_mutex->claimMutex(); - } +/*! \class LockGuarded + \brief A guarded lock class +*/ +class LockGuarded +{ +private: + GuardedMutex* m_mutex; + volatile std::atomic_bool m_locked; - /*! \brief Constructs a guarded lock using a guarded mutex and a lock state - */ - inline LockGuarded(GuardedMutex* mutex, LockState lockit) : m_mutex(mutex), m_locked(false) - { - if (lockit != LS_Unlocked) - m_locked = m_mutex->claimMutex(); - } + LockGuarded(LockGuarded const&) = delete; + LockGuarded& operator=(LockGuarded const&) = delete; + LockGuarded(LockGuarded&&) = delete; + LockGuarded& operator=(LockGuarded&&) = delete; - /*! \brief Destructor - */ - inline ~LockGuarded() - { - unlock(); - } +public: - /*! \brief Locks the unlocked mutex - \returns True if the mutex is successfully locked - */ - inline bool lock() - { - if (!m_locked) - return (m_locked = m_mutex->claimMutex()) != 0; - return true; - } + /*! \brief Constructs a guarded lock using a guarded mutex + */ + inline LockGuarded(GuardedMutex* mutex, bool lockit = true) : m_mutex(mutex), m_locked(false) + { + if (lockit) + m_locked = m_mutex->claimMutex(); + } - /*! \brief Unlocks the locked mutex - \returns True if the mutex is successfully unlocked - */ - inline bool unlock() noexcept - { - if (m_locked) - { -#ifndef __GNUC__ - #pragma warning(suppress: 4706) -#endif - return !(m_locked = !m_mutex->releaseMutex()); - } - return true; - } + /*! \brief Constructs a guarded lock using a guarded mutex and a lock state + */ + inline LockGuarded(GuardedMutex* mutex, LockState lockit) : m_mutex(mutex), m_locked(false) + { + if (lockit != LS_Unlocked) + m_locked = m_mutex->claimMutex(); + } - /*! \returns True if the mutex is locked by this lock - */ - inline bool isLocked() const - { - return m_locked; - } + /*! \brief Destructor + */ + inline ~LockGuarded() + { + unlock(); + } - /*! \brief Check if the lock is using the supplied mutex - \param mutex The supplied guarded mutex - \returns True if it is equal - */ - bool isUsing(GuardedMutex const* mutex) const - { - return m_mutex == mutex; - } + /*! \brief Locks the unlocked mutex + \returns True if the mutex is successfully locked + */ + inline bool lock() + { + if (!m_locked) + return (m_locked = m_mutex->claimMutex()) != 0; + return true; + } - /*! \brief Check if the lock is using the supplied mutex - \param mutex The supplied suspendable readers-writer mutex - \returns True if it is equal - */ - bool isUsing(MutexReadWriteSuspendable const* mutex) const + /*! \brief Unlocks the locked mutex + \returns True if the mutex is successfully unlocked + */ + inline bool unlock() noexcept + { + if (m_locked) { - return m_mutex == mutex; + XSENS_MSC_WARNING_SUPPRESS(4706) + return !(m_locked = !m_mutex->releaseMutex()); } + return true; + } - /*! \brief Check if the lock is using the supplied mutex - \param mutex The supplied generic mutex - \returns True if it is equal - */ - bool isUsing(Mutex const* mutex) const - { - return m_mutex->isUsing(mutex); - } - }; + /*! \returns True if the mutex is locked by this lock + */ + inline bool isLocked() const + { + return m_locked; + } - /*! \class ProtectedValue - \brief A protected value class + /*! \brief Check if the lock is using the supplied mutex + \param mutex The supplied guarded mutex + \returns True if it is equal */ - template - class ProtectedValue { - private: - mutable LockReadWrite* m_lock; - public: + bool isUsing(GuardedMutex const* mutex) const + { + return m_mutex == mutex; + } - /*! \brief Constructs a protected value using a readers-writer mutex - */ - ProtectedValue(MutexReadWrite* mutex, bool writeLock = false) - { - m_lock = new LockReadWrite(mutex); - m_lock->lock(writeLock); - } + /*! \brief Check if the lock is using the supplied mutex + \param mutex The supplied suspendable readers-writer mutex + \returns True if it is equal + */ + bool isUsing(MutexReadWriteSuspendable const* mutex) const + { + return m_mutex == mutex; + } - /*! \brief Constructs a protected value using a reference of another protected value - */ - ProtectedValue(const ProtectedValue &pv) - { - m_lock = pv.m_lock; - m_value = pv.m_value; - pv.m_lock = NULL; - } + /*! \brief Check if the lock is using the supplied mutex + \param mutex The supplied generic mutex + \returns True if it is equal + */ + bool isUsing(Mutex const* mutex) const + { + return m_mutex->isUsing(mutex); + } +}; + +/*! \class ProtectedValue + \brief A protected value class +*/ +template +class ProtectedValue +{ +private: + mutable LockReadWrite* m_lock; +public: + + /*! \brief Constructs a protected value using a readers-writer mutex + */ + ProtectedValue(MutexReadWrite* mutex, bool writeLock = false) + { + m_lock = new LockReadWrite(mutex); + m_lock->lock(writeLock); + } - /*! \brief Destructor - */ - ~ProtectedValue() - { - if (m_lock) - delete m_lock; - } + /*! \brief Constructs a protected value using a reference of another protected value + */ + ProtectedValue(const ProtectedValue& pv) + { + m_lock = pv.m_lock; + m_value = pv.m_value; + pv.m_lock = NULL; + } - /*! \brief Assignment operator, copies contents from the \a pv protected value - \param pv : The protected value to copy from - */ - void operator = (const ProtectedValue& pv) - { - if (&pv == this) - return; + /*! \brief Destructor + */ + ~ProtectedValue() + { + if (m_lock) + delete m_lock; + } - assert(pv.m_lock); - m_lock = pv.m_lock; - m_value = pv.m_value; - pv.m_lock = NULL; - } + /*! \brief Assignment operator, copies contents from the \a pv protected value + \param pv : The protected value to copy from + */ + void operator = (const ProtectedValue& pv) + { + if (&pv == this) + return; - K m_value; //!< A type name of this class - }; + assert(pv.m_lock); + m_lock = pv.m_lock; + m_value = pv.m_value; + pv.m_lock = NULL; + } - /*! \class Semaphore - \brief A semaphore class - */ - class Semaphore { - protected: + K m_value; //!< A type name of this class +}; + +/*! \class Semaphore + \brief A semaphore class +*/ +class Semaphore +{ +protected: #ifdef _WIN32 - HANDLE *m_handleList; //!< The list of handles on the Semaphore + HANDLE* m_handleList; //!< The list of handles on the Semaphore #else - char *m_semname; //!< A name of the semaphore - sem_t *m_handle; //!< A semaphore's handle + char* m_semname; //!< A name of the semaphore + sem_t* m_handle; //!< A semaphore's handle #endif - uint32_t m_nofHandles; //!< A number of the semaphore's handles - - Semaphore(Semaphore const&) = delete; - Semaphore& operator=(Semaphore const&) = delete; - Semaphore(Semaphore &&) = delete; - Semaphore& operator=(Semaphore &&) = delete; - - public: - - /*! \brief Waits for an infinite time or until the semaphore is released - \returns True if the semaphore is released - */ - bool wait1(); - - /*! \brief Waits until time-out interval elapses or until the semaphore is released - \param timeout The timeout value to use - \returns True if the semaphore is released - */ - bool wait1(uint32_t timeout); - - /*! \brief Increases the count of the specified semaphore object by a specified amount - \param increment The amount by which the semaphore object's current count is to be increased - \returns The amount of the semaphore object's previous count - */ - int32_t post(int32_t increment = 1) noexcept; + uint32_t m_nofHandles; //!< A number of the semaphore's handles + + Semaphore(Semaphore const&) = delete; + Semaphore& operator=(Semaphore const&) = delete; + Semaphore(Semaphore&&) = delete; + Semaphore& operator=(Semaphore&&) = delete; + +public: + + /*! \brief Waits for an infinite time or until the semaphore is released + \returns True if the semaphore is released + */ + bool wait1(); + + /*! \brief Waits until time-out interval elapses or until the semaphore is released + \param timeout The timeout value to use + \returns True if the semaphore is released + */ + bool wait1(uint32_t timeout); + + /*! \brief Increases the count of the specified semaphore object by a specified amount + \param increment The amount by which the semaphore object's current count is to be increased + \returns The amount of the semaphore object's previous count + */ + int32_t post(int32_t increment = 1) noexcept; #ifdef _WIN32 - /*! \brief Constructor - */ - Semaphore(int32_t initVal = 0, uint32_t nofOtherHandles = 0, HANDLE *otherHandles = NULL); + /*! \brief Constructor + */ + Semaphore(int32_t initVal = 0, uint32_t nofOtherHandles = 0, HANDLE* otherHandles = NULL); #else - /*! \brief Constructor - */ - Semaphore(int32_t initVal = 0, uint32_t nofOtherHandles = 0, sem_t *otherHandles = NULL); + /*! \brief Constructor + */ + Semaphore(int32_t initVal = 0, uint32_t nofOtherHandles = 0, sem_t* otherHandles = NULL); #endif - /*! \brief Destructor - */ - ~Semaphore(); - }; + /*! \brief Destructor + */ + ~Semaphore(); +}; + +/*! \class WaitCondition + \brief A platform independent wait condition implementation + + Wait conditions are used to halt consumer threads until changes are made to data they operate on. + + Creating the wait condition: + \code + Mutex mut; + WaitCondition wc(&mut); + \endcode + + Consumer thread: + \code + void func() + { + Lock locker(&mut); - /*! \class WaitCondition - * \brief A platform independent wait condition implementation - * - * Wait conditions are used to halt consumer threads until changes are made to data they operate on. - * - * Creating the wait condition: - * \code - * Mutex mut; - * WaitCondition wc(&mut); - * \endcode - * - * Consumer thread: - * \code - * void func() - * { - * Lock locker(&mut); - * - * if (!wc.wait()) // unlocks the mutex and waits for a signal - * // the mutex is claimed - * return; - * - * // mutex is claimed - * // copy data - * lock.unlock(); - * // work with data - * } - * \endcode - * - * Feeder thread: - * \code - * void func() - * { - * Lock locker(&mut); - * - * // update data - * wc.signal(); // signal that changes have been made - * // alternatively, use wc.broadcast() to allow multiple consumers to handle the data - * } - * \endcode - */ - class WaitCondition - { - WaitCondition(WaitCondition const&) = delete; - WaitCondition& operator=(WaitCondition const&) = delete; - WaitCondition(WaitCondition &&) = delete; - WaitCondition& operator=(WaitCondition &&) = delete; - - public: - explicit WaitCondition(Mutex &m); - ~WaitCondition(); - - void signal(); - void broadcast(); - bool wait(); - bool wait(uint32_t timeout); - private: + if (!wc.wait()) // unlocks the mutex and waits for a signal + // the mutex is claimed + return; + + // mutex is claimed + // copy data + lock.unlock(); + // work with data + } + \endcode + + Feeder thread: + \code + void func() + { + Lock locker(&mut); + + // update data + wc.signal(); // signal that changes have been made + // alternatively, use wc.broadcast() to allow multiple consumers to handle the data + } + \endcode +*/ +class WaitCondition +{ + WaitCondition(WaitCondition const&) = delete; + WaitCondition& operator=(WaitCondition const&) = delete; + WaitCondition(WaitCondition&&) = delete; + WaitCondition& operator=(WaitCondition&&) = delete; + +public: + explicit WaitCondition(Mutex& m); + ~WaitCondition(); + + void signal(); + void broadcast(); + bool wait(); + bool wait(uint32_t timeout); +private: #ifdef _WIN32 - CONDITION_VARIABLE m_cond; + CONDITION_VARIABLE m_cond; #else - pthread_cond_t m_cond; - pthread_condattr_t m_condattr; + pthread_cond_t m_cond; + pthread_condattr_t m_condattr; #ifdef __APPLE__ - int m_clockId; + int m_clockId; #else - clockid_t m_clockId; + clockid_t m_clockId; #endif #endif - Mutex &m_mutex; - }; - - /*! \brief An event that can be set/reset and that can be waited for - \details The WaitEvent is intended for 1-1 thread synchronization, not 1-N thread synchronization - */ - class WaitEvent - { - WaitEvent(WaitEvent const&) = delete; - WaitEvent& operator=(WaitEvent const&) = delete; - WaitEvent(WaitEvent &&) = delete; - WaitEvent& operator=(WaitEvent &&) = delete; - - public: - WaitEvent(); - ~WaitEvent(); - - void terminate(); - bool wait(); - void set(); - void reset(); - - private: + Mutex& m_mutex; +}; + +/*! \brief An event that can be set/reset and that can be waited for + \details The WaitEvent is intended for 1-1 thread synchronization, not 1-N thread synchronization +*/ +class WaitEvent +{ + WaitEvent(WaitEvent const&) = delete; + WaitEvent& operator=(WaitEvent const&) = delete; + WaitEvent(WaitEvent&&) = delete; + WaitEvent& operator=(WaitEvent&&) = delete; + +public: + WaitEvent(); + ~WaitEvent(); + + void terminate(); + bool wait(); + void set(); + void reset(); + +private: #if defined(_WIN32) - HANDLE m_event; + HANDLE m_event; #else - pthread_mutex_t m_mutex; - pthread_cond_t m_cond; - bool m_triggered; + pthread_mutex_t m_mutex; + pthread_cond_t m_cond; + bool m_triggered; #endif - volatile std::atomic_int m_waiterCount; - volatile std::atomic_bool m_terminating; - }; + volatile std::atomic_int m_waiterCount; + volatile std::atomic_bool m_terminating; +}; + +/*! \brief Lock two mutex-locks in the order in which they appear, rolling back the locks if one fails. It repeats until they all succeed. + \param lock1 The first Lock to lock + \param lock2 The second Lock to lock + \param state1 desired LockState for lock1 + \param state2 desired LockState for lock2 +*/ +template +void multiLock(L1& lock1, L2& lock2, LockState state1 = LockState::LS_Write, LockState state2 = LockState::LS_Write) +{ + lock1.lock(state1); + while (!lock2.tryLock(state2)) + { + lock1.unlock(); + xsYield(); + lock1.lock(state1); + } +} + } // namespace xsens #ifndef __GNUC__ -#pragma warning(default: 4127) + #pragma warning(default: 4127) #endif #endif diff --git a/lib/xspublic/xscommon/xsens_threadpool.cpp b/lib/xspublic/xscommon/xsens_threadpool.cpp index 5df0014..9214f2e 100644 --- a/lib/xspublic/xscommon/xsens_threadpool.cpp +++ b/lib/xspublic/xscommon/xsens_threadpool.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -79,7 +79,8 @@ // Enable this if you want to disable multithreading //#define ADD_TASK_EXECUTE_NOW -namespace xsens { +namespace xsens +{ ///////////////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////////////// @@ -94,7 +95,7 @@ int processorCount() { #if defined(_WIN32) SYSTEM_INFO sysinfo; - ::GetSystemInfo( &sysinfo ); + ::GetSystemInfo(&sysinfo); return (int) sysinfo.dwNumberOfProcessors; #else return sysconf(_SC_NPROCESSORS_ONLN); @@ -126,7 +127,8 @@ unsigned int ThreadPoolTask::needToWaitFor() /*! \brief A class that contains a task and some administrative stuff */ -class PooledTask { +class PooledTask +{ public: ThreadPoolTask* m_task; //!< The task that is to be executed unsigned int m_id; //!< The id that was assigned to the task by the ThreadPool @@ -223,7 +225,7 @@ class PooledThread : public StandardThread unsigned int m_completed; //!< The number of tasks that this thread has completed so far, excluding incomplete tasks unsigned int m_failed; //!< The number of tasks that this thread has failed to complete so far due to an exception - virtual int32_t innerFunction(void); + int32_t innerFunction(void) override; }; /*! \brief Constructor */ @@ -272,12 +274,12 @@ int32_t PooledThread::innerFunction(void) } } #ifdef XSENS_DEBUG - catch (std::exception &e) + catch (std::exception& e) { - const std::type_info &et = typeid(e); + const std::type_info& et = typeid(e); #ifdef __GNUC__ int status; - char *realname = abi::__cxa_demangle(et.name(), 0, 0, &status); + char* realname = abi::__cxa_demangle(et.name(), 0, 0, &status); fprintf(stderr, "ThreadPool: Caught an unhandled %s(\"%s\")\n", realname, e.what()); free(realname); #else @@ -286,7 +288,7 @@ int32_t PooledThread::innerFunction(void) complete = true; } #endif - catch(...) + catch (...) { #ifdef XSENS_DEBUG fprintf(stderr, "ThreadPool: Caught an unhandled unknown exception\n"); @@ -382,7 +384,7 @@ ThreadPool::~ThreadPool() for (ThreadSet::iterator it = m_threads.begin(); it != m_threads.end(); ++it) delete *it; } - catch(...) + catch (...) { // nothing much we can do about this... } @@ -397,7 +399,7 @@ ThreadPool::~ThreadPool() ThreadPool::TaskId ThreadPool::addTask(ThreadPoolTask* task, ThreadPool::TaskId afterId) { assert(!m_terminating); - std::shared_ptr tmp(new PooledTask); + std::shared_ptr tmp = std::make_shared(); tmp->m_task = task; Lock safety(&m_safe); tmp->m_id = m_nextId; @@ -434,7 +436,7 @@ ThreadPool::TaskId ThreadPool::addTask(ThreadPoolTask* task, ThreadPool::TaskId unsigned int ThreadPool::count() { Lock safety(&m_safe); - return (unsigned int) (m_tasks.size() + m_delaying.size() + m_executing.size()); + return (unsigned int)(m_tasks.size() + m_delaying.size() + m_executing.size()); } /*! \brief Set the number of threads in the ThreadPool @@ -458,8 +460,13 @@ void ThreadPool::setPoolSize(unsigned int poolsize) { while (poolsize < m_threads.size()) { - delete *m_threads.begin(); - m_threads.erase(m_threads.begin()); + auto it = m_threads.begin(); + auto thread = *it; + safety.unlock(); + thread->stopThread(); + safety.lock(); + delete thread; + m_threads.erase(it); } } @@ -509,6 +516,10 @@ std::shared_ptr ThreadPool::findTask(ThreadPool::TaskId id) if (it != m_tasksSearch.end()) return it->second; + for (auto const& tsk : m_tasks) + if (tsk->m_id == id) + return tsk; + return std::shared_ptr(); } @@ -532,7 +543,7 @@ bool ThreadPool::doesTaskExist(ThreadPool::TaskId id) /*! \brief Remove the task with the supplied \a id if it exists, waits for the task to be finished */ -void ThreadPool::cancelTask(ThreadPool::TaskId id, bool wait) +void ThreadPool::cancelTask(ThreadPool::TaskId id, bool wait) noexcept { Lock safety(&m_safe); TaskSet::iterator it = m_executing.find(id); @@ -547,11 +558,27 @@ void ThreadPool::cancelTask(ThreadPool::TaskId id, bool wait) it = m_delaying.find(id); if (it != m_delaying.end()) + { + reportTaskComplete(it->second); m_delaying.erase(it); + } it = m_tasksSearch.find(id); if (it != m_tasksSearch.end()) + { + reportTaskComplete(it->second); m_tasksSearch.erase(it); + } + + for (auto tskIt = m_tasks.begin(); tskIt != m_tasks.end(); ++tskIt) + { + if ((*tskIt)->m_id == id) + { + reportTaskComplete(*tskIt); + m_tasks.erase(tskIt); + return; + } + } } /*! \brief Wait for the task with the given ID to complete @@ -707,12 +734,20 @@ ThreadPool* gPool = NULL; bool gManagePool = true; /*! \brief Return the global thread pool object, it will be created if it did not yet exist */ -ThreadPool* ThreadPool::instance() +ThreadPool* ThreadPool::instance() noexcept { if (gPool) return gPool; - gPool = new ThreadPool; - gManagePool = true; + try + { + gPool = new ThreadPool; + gManagePool = true; + } + catch (...) + { + gPool = nullptr; + gManagePool = false; + } return gPool; } diff --git a/lib/xspublic/xscommon/xsens_threadpool.h b/lib/xspublic/xscommon/xsens_threadpool.h index e679003..2067697 100644 --- a/lib/xspublic/xscommon/xsens_threadpool.h +++ b/lib/xspublic/xscommon/xsens_threadpool.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -73,12 +73,14 @@ #include #include -namespace xsens { +namespace xsens +{ int processorCount(); class PooledTask; -class ThreadPoolTask { +class ThreadPoolTask +{ public: virtual bool exec() = 0; //!< \returns True if the task completed or false to reschedule the task virtual unsigned int needToWaitFor(); @@ -127,7 +129,7 @@ class ThreadPool void setPoolSize(unsigned int poolsize); unsigned int poolSize() const; bool doesTaskExist(TaskId id); - void cancelTask(TaskId id, bool wait = true); + void cancelTask(TaskId id, bool wait = true) noexcept; void waitForCompletion(TaskId id); void suspend(bool wait = false) noexcept; void resume(); @@ -136,7 +138,7 @@ class ThreadPool unsigned int failedCount(unsigned int thread) const; XsThreadId taskThreadId(TaskId id); - static ThreadPool* instance(); + static ThreadPool* instance() noexcept; static void destroy(); static void setPool(ThreadPool* pool); }; diff --git a/lib/xspublic/xscontroller/broadcastdevice.cpp b/lib/xspublic/xscontroller/broadcastdevice.cpp index ef05da5..29ce1db 100644 --- a/lib/xspublic/xscontroller/broadcastdevice.cpp +++ b/lib/xspublic/xscontroller/broadcastdevice.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -84,7 +84,8 @@ using namespace xsens; /*! \brief Pure virtual base class for N-argument specific function forwarding \details The class provides the necessary locking to safely use XsControl. */ -class BroadcastForwardFunc { +class BroadcastForwardFunc +{ public: BroadcastForwardFunc(BroadcastDevice* bc) : m_broadcaster(bc) {} virtual ~BroadcastForwardFunc() {} @@ -96,7 +97,7 @@ class BroadcastForwardFunc { std::vector& ch = m_broadcaster->m_control->m_deviceList; bool ok = true; - XsResultValue res = (ok?XRV_OK:XRV_NOFILEORPORTOPEN); + XsResultValue res = (ok ? XRV_OK : XRV_NOFILEORPORTOPEN); for (std::vector::iterator it = ch.begin(); it != ch.end(); ++it) { if (!call(*it)) @@ -112,48 +113,64 @@ class BroadcastForwardFunc { }; //! \brief 0-argument const forwarding function class -class ForwardConstFunc : public BroadcastForwardFunc { +class ForwardConstFunc : public BroadcastForwardFunc +{ public: typedef bool (XsDevice::*FuncType)() const; - inline ForwardConstFunc (BroadcastDevice* bc, FuncType func) + inline ForwardConstFunc(BroadcastDevice* bc, FuncType func) : BroadcastForwardFunc(bc), m_func(func) {} - virtual bool call(XsDevice* device) { return (device->*m_func)(); } + virtual bool call(XsDevice* device) + { + return (device->*m_func)(); + } private: FuncType m_func; }; //! \brief 1-argument const forwarding function class template -class ForwardConstFunc1Arg : public BroadcastForwardFunc { +class ForwardConstFunc1Arg : public BroadcastForwardFunc +{ public: typedef bool (XsDevice::*FuncType)(Arg1) const; - inline ForwardConstFunc1Arg (BroadcastDevice* bc, FuncType func, Arg1 arg1) + inline ForwardConstFunc1Arg(BroadcastDevice* bc, FuncType func, Arg1 arg1) : BroadcastForwardFunc(bc), m_func(func), m_arg1(arg1) {} - virtual bool call(XsDevice* device) { return (device->*m_func)(m_arg1); } + virtual bool call(XsDevice* device) + { + return (device->*m_func)(m_arg1); + } private: FuncType m_func; Arg1 m_arg1; }; //! \brief 0-argument forwarding function class -class BroadcastForwardFunc0Arg : public BroadcastForwardFunc { +class BroadcastForwardFunc0Arg : public BroadcastForwardFunc +{ public: typedef bool (XsDevice::*FuncType)(); - inline BroadcastForwardFunc0Arg (BroadcastDevice* bc, FuncType func) + inline BroadcastForwardFunc0Arg(BroadcastDevice* bc, FuncType func) : BroadcastForwardFunc(bc), m_func(func) {} - virtual bool call(XsDevice* device) { return (device->*m_func)(); } + virtual bool call(XsDevice* device) + { + return (device->*m_func)(); + } private: FuncType m_func; }; //! \brief 1-argument forwarding function class template -class BroadcastForwardFunc1Arg : public BroadcastForwardFunc { +class BroadcastForwardFunc1Arg : public BroadcastForwardFunc +{ public: typedef bool (XsDevice::*FuncType)(Arg1); - inline BroadcastForwardFunc1Arg (BroadcastDevice* bc, FuncType func, Arg1 arg1) + inline BroadcastForwardFunc1Arg(BroadcastDevice* bc, FuncType func, Arg1 arg1) : BroadcastForwardFunc(bc), m_func(func), m_arg1(arg1) {} - virtual bool call(XsDevice* device) { return (device->*m_func)(m_arg1); } + virtual bool call(XsDevice* device) + { + return (device->*m_func)(m_arg1); + } private: FuncType m_func; Arg1 m_arg1; @@ -161,12 +178,16 @@ class BroadcastForwardFunc1Arg : public BroadcastForwardFunc { //! \brief 2-argument forwarding function class template -class BroadcastForwardFunc2Arg : public BroadcastForwardFunc { +class BroadcastForwardFunc2Arg : public BroadcastForwardFunc +{ public: typedef bool (XsDevice::*FuncType)(Arg1, Arg2); - inline BroadcastForwardFunc2Arg (BroadcastDevice* bc, FuncType func, Arg1 arg1, Arg2 arg2) + inline BroadcastForwardFunc2Arg(BroadcastDevice* bc, FuncType func, Arg1 arg1, Arg2 arg2) : BroadcastForwardFunc(bc), m_func(func), m_arg1(arg1), m_arg2(arg2) {} - virtual bool call(XsDevice* device) { return (device->*m_func)(m_arg1, m_arg2); } + virtual bool call(XsDevice* device) + { + return (device->*m_func)(m_arg1, m_arg2); + } private: FuncType m_func; Arg1 m_arg1; @@ -175,12 +196,16 @@ class BroadcastForwardFunc2Arg : public BroadcastForwardFunc { //! \brief 3-argument forwarding function class template -class BroadcastForwardFunc3Arg : public BroadcastForwardFunc { +class BroadcastForwardFunc3Arg : public BroadcastForwardFunc +{ public: typedef bool (XsDevice::*FuncType)(Arg1, Arg2, Arg3); - inline BroadcastForwardFunc3Arg (BroadcastDevice* bc, FuncType func, Arg1 arg1, Arg2 arg2, Arg3 arg3) + inline BroadcastForwardFunc3Arg(BroadcastDevice* bc, FuncType func, Arg1 arg1, Arg2 arg2, Arg3 arg3) : BroadcastForwardFunc(bc), m_func(func), m_arg1(arg1), m_arg2(arg2), m_arg3(arg3) {} - virtual bool call(XsDevice* device) { return (device->*m_func)(m_arg1, m_arg2, m_arg3); } + virtual bool call(XsDevice* device) + { + return (device->*m_func)(m_arg1, m_arg2, m_arg3); + } private: FuncType m_func; Arg1 m_arg1; @@ -189,10 +214,11 @@ class BroadcastForwardFunc3Arg : public BroadcastForwardFunc { }; //! \brief 0-argument forwarding function class, void return -class BroadcastForwardFunc0ArgVoid : public BroadcastForwardFunc { +class BroadcastForwardFunc0ArgVoid : public BroadcastForwardFunc +{ public: typedef void (XsDevice::*FuncType)(); - inline BroadcastForwardFunc0ArgVoid (BroadcastDevice* bc, FuncType func) + inline BroadcastForwardFunc0ArgVoid(BroadcastDevice* bc, FuncType func) : BroadcastForwardFunc(bc), m_func(func) {} virtual bool call(XsDevice* device) { @@ -205,10 +231,11 @@ class BroadcastForwardFunc0ArgVoid : public BroadcastForwardFunc { //! \brief 1-argument forwarding function class, void return template -class BroadcastForwardFunc1ArgVoid : public BroadcastForwardFunc { +class BroadcastForwardFunc1ArgVoid : public BroadcastForwardFunc +{ public: typedef void (XsDevice::*FuncType)(Arg1); - inline BroadcastForwardFunc1ArgVoid (BroadcastDevice* bc, FuncType func, Arg1 arg1) + inline BroadcastForwardFunc1ArgVoid(BroadcastDevice* bc, FuncType func, Arg1 arg1) : BroadcastForwardFunc(bc), m_func(func), m_arg1(arg1) {} virtual bool call(XsDevice* device) { @@ -222,10 +249,11 @@ class BroadcastForwardFunc1ArgVoid : public BroadcastForwardFunc { //! \brief 2-argument forwarding function class, void return template -class BroadcastForwardFunc2ArgVoid : public BroadcastForwardFunc { +class BroadcastForwardFunc2ArgVoid : public BroadcastForwardFunc +{ public: typedef void (XsDevice::*FuncType)(Arg1, Arg2); - inline BroadcastForwardFunc2ArgVoid (BroadcastDevice* bc, FuncType func, Arg1 arg1, Arg2 arg2) + inline BroadcastForwardFunc2ArgVoid(BroadcastDevice* bc, FuncType func, Arg1 arg1, Arg2 arg2) : BroadcastForwardFunc(bc), m_func(func), m_arg1(arg1), m_arg2(arg2) {} virtual bool call(XsDevice* device) { @@ -241,8 +269,8 @@ class BroadcastForwardFunc2ArgVoid : public BroadcastForwardFunc { /*! \brief Constructor, sets up the XsControl reference */ BroadcastDevice::BroadcastDevice(XsControl* control_) -: XsDevice(XsDeviceId(0)) -, m_control(control_) + : XsDevice(XsDeviceId(0)) + , m_control(control_) { } @@ -289,7 +317,7 @@ XsVersion BroadcastDevice::hardwareVersion() const \returns True if successful \sa objectAlignmentMatrix(), headingOffset(), setHeadingOffset(), lastResult() */ -bool BroadcastDevice::setObjectAlignment(const XsMatrix &matrix) +bool BroadcastDevice::setObjectAlignment(const XsMatrix& matrix) { return BroadcastForwardFunc1Arg(this, &XsDevice::setObjectAlignment, matrix)(); } @@ -373,7 +401,7 @@ bool BroadcastDevice::gotoConfig() std::vector& ch = m_control->m_deviceList; bool ok = true; - XsResultValue res = (ok?XRV_OK:XRV_NOFILEORPORTOPEN); + XsResultValue res = (ok ? XRV_OK : XRV_NOFILEORPORTOPEN); for (std::vector::reverse_iterator it = ch.rbegin(); it != ch.rend(); ++it) { if (!(*it)->gotoConfig()) diff --git a/lib/xspublic/xscontroller/broadcastdevice.h b/lib/xspublic/xscontroller/broadcastdevice.h index 2bda18d..86d49c9 100644 --- a/lib/xspublic/xscontroller/broadcastdevice.h +++ b/lib/xspublic/xscontroller/broadcastdevice.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -68,7 +68,8 @@ #include "xsdevice_def.h" struct XsControl; -class BroadcastDevice : public XsDevice { +class BroadcastDevice : public XsDevice +{ public: explicit BroadcastDevice(XsControl* control); ~BroadcastDevice() override; @@ -86,7 +87,7 @@ class BroadcastDevice : public XsDevice { bool isMeasuring() const override; bool isRecording() const override; - bool isReadingFromFile() const override; + bool isReadingFromFile() const override final; bool setSerialBaudRate(XsBaudRate baudrate) override; bool setSyncSettings(const XsSyncSettingArray& s) override; @@ -103,7 +104,7 @@ class BroadcastDevice : public XsDevice { bool updateCachedDeviceInformation() override; bool setHeadingOffset(double offset) override; bool setLocationId(int id) override; - bool setObjectAlignment(const XsMatrix &matrix) override; + bool setObjectAlignment(const XsMatrix& matrix) override; bool setGravityMagnitude(double mag) override; bool setXdaFilterProfile(int profileType) override; bool setXdaFilterProfile(XsString const& profileType) override; diff --git a/lib/xspublic/xscontroller/callbackmanagerxda.cpp b/lib/xspublic/xscontroller/callbackmanagerxda.cpp index 81d3690..b01b5fc 100644 --- a/lib/xspublic/xscontroller/callbackmanagerxda.cpp +++ b/lib/xspublic/xscontroller/callbackmanagerxda.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -69,14 +69,16 @@ using namespace xsens; /*! \brief Linked list item that contains a registered XsCallback handler for CallbackManagerXda */ -struct CallbackHandlerXdaItem { +struct CallbackHandlerXdaItem +{ XsCallbackPlainC* m_handler; //!< The callback handler CallbackHandlerXdaItem* m_next; //!< The next item in the list or NULL if this is the last item }; /*! \brief Linked list item that contains a chained CallbackManagerXda */ -struct CallbackManagerItem { +struct CallbackManagerItem +{ CallbackManagerXda* m_manager; //!< The callback managger CallbackManagerItem* m_next; //!< The next item in the list or NULL if this is the last item }; @@ -115,7 +117,7 @@ CallbackManagerXda::~CallbackManagerXda() clearCallbackHandlers(false); delete m_callbackMutex; } - catch(...) + catch (...) { } } @@ -447,7 +449,7 @@ int CallbackManagerXda::onWriteMessageToLogFile(XsDevice* dev, const XsMessage* rv = current->m_handler->m_onWriteMessageToLogFile(current->m_handler, dev, message) && rv; current = current->m_next; } - return rv?1:0; + return rv ? 1 : 0; } //! \brief The XsCallback::onBufferedDataAvailable() callback forwarding function @@ -516,7 +518,7 @@ void CallbackManagerXda::onError(XsDevice* dev, XsResultValue error) } //! \brief The Xscallback::onNonDataMessage() callback forwarding function -void CallbackManagerXda::onNonDataMessage(XsDevice* dev, XsMessage const * message) +void CallbackManagerXda::onNonDataMessage(XsDevice* dev, XsMessage const* message) { LockReadWrite locky(m_callbackMutex, LS_Read); CallbackHandlerXdaItem* current = m_handlerList; @@ -529,7 +531,7 @@ void CallbackManagerXda::onNonDataMessage(XsDevice* dev, XsMessage const * messa } //! \brief The Xscallback::onMessageReceivedFromDevice() callback forwarding function -void CallbackManagerXda::onMessageDetected(XsDevice* dev, XsProtocolType type, XsByteArray const * rawMessage) +void CallbackManagerXda::onMessageDetected(XsDevice* dev, XsProtocolType type, XsByteArray const* rawMessage) { LockReadWrite locky(m_callbackMutex, LS_Read); CallbackHandlerXdaItem* current = m_handlerList; @@ -542,7 +544,7 @@ void CallbackManagerXda::onMessageDetected(XsDevice* dev, XsProtocolType type, X } //! \brief The Xscallback::onMessageReceivedFromDevice() callback forwarding function -void CallbackManagerXda::onMessageReceivedFromDevice(XsDevice* dev, XsMessage const * message) +void CallbackManagerXda::onMessageReceivedFromDevice(XsDevice* dev, XsMessage const* message) { LockReadWrite locky(m_callbackMutex, LS_Read); CallbackHandlerXdaItem* current = m_handlerList; @@ -555,7 +557,7 @@ void CallbackManagerXda::onMessageReceivedFromDevice(XsDevice* dev, XsMessage co } //! \brief The Xscallback::onMessageSentToDevice() callback forwarding function -void CallbackManagerXda::onMessageSentToDevice(XsDevice* dev, XsMessage const * message) +void CallbackManagerXda::onMessageSentToDevice(XsDevice* dev, XsMessage const* message) { LockReadWrite locky(m_callbackMutex, LS_Read); CallbackHandlerXdaItem* current = m_handlerList; diff --git a/lib/xspublic/xscontroller/callbackmanagerxda.h b/lib/xspublic/xscontroller/callbackmanagerxda.h index 212c155..11c0895 100644 --- a/lib/xspublic/xscontroller/callbackmanagerxda.h +++ b/lib/xspublic/xscontroller/callbackmanagerxda.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -69,12 +69,14 @@ struct CallbackHandlerXdaItem; struct CallbackManagerItem; -namespace xsens { - class MutexReadWrite; +namespace xsens +{ +class MutexReadWrite; } struct MtwInfo; -class CallbackManagerXda : public XsCallback { +class CallbackManagerXda : public XsCallback +{ public: void onDeviceStateChanged(XsDevice* dev, XsDeviceState newState, XsDeviceState oldState) override; void onLiveDataAvailable(XsDevice* dev, const XsDataPacket* packet) override; @@ -89,10 +91,10 @@ class CallbackManagerXda : public XsCallback { void onConnectivityChanged(XsDevice* dev, XsConnectivityState newState) override; void onInfoResponse(XsDevice* dev, XsInfoRequest request) override; void onError(XsDevice* dev, XsResultValue error) override; - void onNonDataMessage(XsDevice* dev, XsMessage const * message) override; - void onMessageDetected(XsDevice* dev, XsProtocolType type, XsByteArray const * rawMessage) override; - void onMessageReceivedFromDevice(XsDevice* dev, XsMessage const * message) override; - void onMessageSentToDevice(XsDevice* dev, XsMessage const * message) override; + void onNonDataMessage(XsDevice* dev, XsMessage const* message) override; + void onMessageDetected(XsDevice* dev, XsProtocolType type, XsByteArray const* rawMessage) override; + void onMessageReceivedFromDevice(XsDevice* dev, XsMessage const* message) override; + void onMessageSentToDevice(XsDevice* dev, XsMessage const* message) override; void onDataAvailable(XsDevice* dev, const XsDataPacket* data) override; void onAllDataAvailable(XsDevicePtrArray* devs, const XsDataPacketPtrArray* packets) override; void onRecordedDataAvailable(XsDevice* dev, const XsDataPacket* data) override; diff --git a/lib/xspublic/xscontroller/clocksynccommand.h b/lib/xspublic/xscontroller/clocksynccommand.h index 1106bbf..848bb21 100644 --- a/lib/xspublic/xscontroller/clocksynccommand.h +++ b/lib/xspublic/xscontroller/clocksynccommand.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xscontroller/communicator.cpp b/lib/xspublic/xscontroller/communicator.cpp index a19747e..53ea97a 100644 --- a/lib/xspublic/xscontroller/communicator.cpp +++ b/lib/xspublic/xscontroller/communicator.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -123,7 +123,7 @@ void Communicator::prepareForDestruction() /*! \brief Sets a master device. \param masterDevice a master device. */ -void Communicator::setMasterDevice(XsDevice *masterDevice) +void Communicator::setMasterDevice(XsDevice* masterDevice) { JLDEBUGG(masterDevice); assert(m_masterInfo == nullptr); @@ -147,7 +147,7 @@ std::shared_ptr Communicator::protocolManager() const /*! \brief Sets a master device ID. \param id device ID. */ -void Communicator::setMasterDeviceId(const XsDeviceId &id) +void Communicator::setMasterDeviceId(const XsDeviceId& id) { m_masterDeviceId = id; } @@ -188,7 +188,7 @@ XsSize Communicator::childDeviceCount() const /*! \brief Handles a \a message */ -void Communicator::handleMessage(const XsMessage &message) +void Communicator::handleMessage(const XsMessage& message) { // handle one message at a time. This is only really necessary for dual-stream interfaces such as to the bodypack xsens::Lock locky(&m_handleMux); @@ -200,8 +200,8 @@ void Communicator::handleMessage(const XsMessage &message) XsSize sz = message.getTotalMessageSize(); const uint8_t* m = message.getMessageStart(); for (XsSize i = 0; i < sz; ++i) - sprintf(buffer+2*i, "%02X", (unsigned int) m[i]); - buffer[2*sz] = 0; + sprintf(buffer + 2 * i, "%02X", (unsigned int) m[i]); + buffer[2 * sz] = 0; JLALERTG("Error message received: " << buffer); JLIF(gJournal, JLL_Alert, m_replyMonitor->dumpObjectList(gJournal, JLL_Alert)); } @@ -211,7 +211,7 @@ void Communicator::handleMessage(const XsMessage &message) /*! \brief Write a message and await the reply */ -bool Communicator::doTransaction(const XsMessage &msg, uint32_t timeout) +bool Communicator::doTransaction(const XsMessage& msg, uint32_t timeout) { XsMessage rcv; return doTransaction(msg, rcv, timeout); @@ -219,14 +219,14 @@ bool Communicator::doTransaction(const XsMessage &msg, uint32_t timeout) /*! \brief Write a message and await the reply */ -bool Communicator::doTransaction(const XsMessage &msg) +bool Communicator::doTransaction(const XsMessage& msg) { return doTransaction(msg, defaultTimeout()); } /*! \brief Write a message and await the reply */ -bool Communicator::doTransaction(const XsMessage &msg, XsMessage &rcv) +bool Communicator::doTransaction(const XsMessage& msg, XsMessage& rcv) { return doTransaction(msg, rcv, defaultTimeout()); } @@ -259,7 +259,7 @@ XsResultValue Communicator::setAndReturnLastResult(XsResultValue res, XsString c Later more checks like odd message ID checks may be added here \returns true if check is successful */ -bool Communicator::sanityCheck(XsMessage const & msg) const +bool Communicator::sanityCheck(XsMessage const& msg) const { if (m_masterInfo) return m_masterInfo->messageLooksSane(msg); @@ -284,7 +284,7 @@ std::shared_ptr Communicator::addReplyObject(uint8_t mid) \param[in] data pointer to data to wait for (this object does not take ownership of the data) \returns a shared pointer to a reply object */ -std::shared_ptr Communicator::addReplyObject(uint8_t mid, XsSize offset, XsSize size, uint8_t const * data) +std::shared_ptr Communicator::addReplyObject(uint8_t mid, XsSize offset, XsSize size, uint8_t const* data) { assert(m_replyMonitor != nullptr); return m_replyMonitor->addReplyObject(new MidAndDataReplyObject(mid, offset, size, data)); diff --git a/lib/xspublic/xscontroller/communicator.h b/lib/xspublic/xscontroller/communicator.h index f457114..59f4015 100644 --- a/lib/xspublic/xscontroller/communicator.h +++ b/lib/xspublic/xscontroller/communicator.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -84,7 +84,8 @@ struct XsByteArray; struct XsString; struct XsMessage; struct XsDeviceConfiguration; -namespace xsens { +namespace xsens +{ class ReplyMonitor; } @@ -100,7 +101,11 @@ struct Communicator : public CallbackManagerXda struct Deleter { //! \brief A function that destroys communicator - void operator()(Communicator* c) {if (c != nullptr) c->destroy();} + void operator()(Communicator* c) + { + if (c != nullptr) + c->destroy(); + } }; //! \brief Initializes of UniquePtr @@ -123,27 +128,33 @@ struct Communicator : public CallbackManagerXda Communicator(void); - bool doTransaction(const XsMessage &message); - bool doTransaction(const XsMessage &message, uint32_t timeout); - bool doTransaction(const XsMessage &message, XsMessage &rcv); + bool doTransaction(const XsMessage& message); + bool doTransaction(const XsMessage& message, uint32_t timeout); + bool doTransaction(const XsMessage& message, XsMessage& rcv); //! \copybrief Communicator::doTransaction - virtual bool doTransaction(const XsMessage &message, XsMessage &rcv, uint32_t timeout) = 0; + virtual bool doTransaction(const XsMessage& message, XsMessage& rcv, uint32_t timeout) = 0; //! \brief Sets a default \a timeout - void setDefaultTimeout(uint32_t timeout) { m_defaultTimeout = timeout; } + void setDefaultTimeout(uint32_t timeout) + { + m_defaultTimeout = timeout; + } //! \returns a default timeout - uint32_t defaultTimeout() const { return m_defaultTimeout; } + uint32_t defaultTimeout() const + { + return m_defaultTimeout; + } XsResultValue lastResult() const; XsString lastResultText() const; - virtual void handleMessage(const XsMessage &message); + virtual void handleMessage(const XsMessage& message); XsSize childDeviceCount() const; XsDeviceId masterDeviceId() const; std::shared_ptr addReplyObject(uint8_t mid); - std::shared_ptr addReplyObject(uint8_t mid, XsSize offset, XsSize size, uint8_t const * data); + std::shared_ptr addReplyObject(uint8_t mid, XsSize offset, XsSize size, uint8_t const* data); std::shared_ptr addReplyObject(ReplyObject* obj); // live stuff @@ -173,7 +184,7 @@ struct Communicator : public CallbackManagerXda \param message a message \returns true on successful write, false otherwise. This doesn't guarantee proper delivery of the message. Use doTransaction for that */ - virtual bool writeMessage(const XsMessage &message) = 0; + virtual bool writeMessage(const XsMessage& message) = 0; /*! \brief Flushes all remaining data on the open port */ @@ -185,7 +196,10 @@ struct Communicator : public CallbackManagerXda /*! \brief Schedules to close the open port */ - virtual void scheduleClosePort() { closePort(); } + virtual void scheduleClosePort() + { + closePort(); + } /*! \returns true if the port is open */ @@ -201,7 +215,7 @@ struct Communicator : public CallbackManagerXda \param detectRs485 When set to true it will try to detect and use an RS485 interface \returns true if port is successfully open */ - virtual bool openPort(const XsPortInfo &portInfo, OpenPortStage stage = OPS_Full, bool detectRs485 = false) = 0; + virtual bool openPort(const XsPortInfo& portInfo, OpenPortStage stage = OPS_Full, bool detectRs485 = false) = 0; /*! \brief Reopens the port \param stage A openning stage of a communication port @@ -212,7 +226,7 @@ struct Communicator : public CallbackManagerXda /*! \brief Returns true if the \a other device is docked at this device */ - virtual bool isDockedAt(Communicator *other) const = 0; + virtual bool isDockedAt(Communicator* other) const = 0; /*! \brief Either disable or enable (default) the keep alive mechanism (if supported by the device) */ @@ -254,10 +268,10 @@ struct Communicator : public CallbackManagerXda virtual void abortLoadLogFile() = 0; /*! \brief Open the log file - ` \param filename a name of file to open + ` \param filename a name of file to open \returns true if the file was opened successfully */ - virtual bool openLogFile(const XsString &filename) = 0; + virtual bool openLogFile(const XsString& filename) = 0; /*! \returns The name of the logfile */ @@ -299,39 +313,42 @@ struct Communicator : public CallbackManagerXda \note SerialCommunicator & MTi: 5 (GotoConfig, ReqDeviceId, GotoConfig, Initbus, ReqConfiguration) \note BodyPack requires more because we also may have some other meta-data in the file, but base is: 7 (ReqDeviceId, RequestControl, SetDataPort, GotoConfig [Communicator & XsDevice], Initbus, ReqConfiguration) */ - static int configurationMessageSearchLimit() { return 16; } + static int configurationMessageSearchLimit() + { + return 20; + } virtual void setCredentials(XsString const& id, XsString const& key); - bool sanityCheck(XsMessage const & msg) const; + bool sanityCheck(XsMessage const& msg) const; /*! \brief Adds a protocol handler \param handler an \ref IProtocolHandler */ - virtual void addProtocolHandler(IProtocolHandler *handler); + virtual void addProtocolHandler(IProtocolHandler* handler); void removeProtocolHandler(XsProtocolType type); bool hasProtocol(XsProtocolType type) const; - void setMasterDevice(XsDevice *masterDevice); + void setMasterDevice(XsDevice* masterDevice); protected: virtual ~Communicator(); virtual void prepareForDestruction(); - XsDevice *masterDevice() const; + XsDevice* masterDevice() const; std::shared_ptr protocolManager() const; // these may be a bit surprising: void setLastResult(XsResultValue lastResult, XsString const& text = XsString()) const; XsResultValue setAndReturnLastResult(XsResultValue lastResult, XsString const& text = XsString()) const; - void setMasterDeviceId(const XsDeviceId &deviceId); + void setMasterDeviceId(const XsDeviceId& deviceId); //! \brief Prepared for destruction boolean variable bool m_preparedForDestruction; //! \brief A master device object - XsDevice *m_masterInfo; + XsDevice* m_masterInfo; //! \brief A shared pointer to protocl manager std::shared_ptr m_protocolManager; diff --git a/lib/xspublic/xscontroller/communicatorfactory.cpp b/lib/xspublic/xscontroller/communicatorfactory.cpp index 936eccd..8cf5b13 100644 --- a/lib/xspublic/xscontroller/communicatorfactory.cpp +++ b/lib/xspublic/xscontroller/communicatorfactory.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -85,7 +85,7 @@ bool CommunicatorFactory::registerType(CommunicatorTypeId typeId, CommunicatorCo /*! \brief Create a communicator based on a port info */ -Communicator *CommunicatorFactory::create(const XsPortInfo &portInfo) const +Communicator* CommunicatorFactory::create(const XsPortInfo& portInfo) const { CommunicatorTypeId id = portInfoToCommunicatorId(portInfo); return construct(id); @@ -93,7 +93,7 @@ Communicator *CommunicatorFactory::create(const XsPortInfo &portInfo) const /*! \brief Create a communicator based on a filename */ -Communicator *CommunicatorFactory::create(const XsString &filename) const +Communicator* CommunicatorFactory::create(const XsString& filename) const { CommunicatorTypeId id = filenameToCommunicatorId(filename); return construct(id); @@ -101,9 +101,9 @@ Communicator *CommunicatorFactory::create(const XsString &filename) const /*! \brief Construct a communicator based on \a typeId */ -Communicator *CommunicatorFactory::construct(CommunicatorTypeId typeId) const +Communicator* CommunicatorFactory::construct(CommunicatorTypeId typeId) const { - Communicator *c = nullptr; + Communicator* c = nullptr; if (typeId != CommunicatorType::INVALID) { ConstructorsMap::const_iterator i = m_constructors.find(typeId); diff --git a/lib/xspublic/xscontroller/communicatorfactory.h b/lib/xspublic/xscontroller/communicatorfactory.h index 0196bad..6c1b9b9 100644 --- a/lib/xspublic/xscontroller/communicatorfactory.h +++ b/lib/xspublic/xscontroller/communicatorfactory.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -101,10 +101,10 @@ class CommunicatorFactory typedef Communicator* (*CommunicatorConstructFunc)(); //! The typedef of the port info match function - typedef bool (*PortInfoMatchFunc)(const XsPortInfo &); + typedef bool (*PortInfoMatchFunc)(const XsPortInfo&); - Communicator *create(const XsPortInfo &portInfo) const; - Communicator *create(const XsString &filename) const; + Communicator* create(const XsPortInfo& portInfo) const; + Communicator* create(const XsString& filename) const; bool registerType(CommunicatorTypeId typeId, CommunicatorConstructFunc constructFunc, PortInfoMatchFunc matchFunc); @@ -112,13 +112,13 @@ class CommunicatorFactory \param filename A name of file \returns A communicator type ID */ - virtual CommunicatorTypeId filenameToCommunicatorId(const XsString &filename) const = 0; + virtual CommunicatorTypeId filenameToCommunicatorId(const XsString& filename) const = 0; /*! \brief Match a XsPortInfo to a communicator \param portInfo An information about the port \returns A communicator type ID */ - virtual CommunicatorTypeId portInfoToCommunicatorId(const XsPortInfo &portInfo) const = 0; + virtual CommunicatorTypeId portInfoToCommunicatorId(const XsPortInfo& portInfo) const = 0; /*! \brief Registrates communicator types */ virtual void registerCommunicatorTypes() {} @@ -128,15 +128,19 @@ class CommunicatorFactory typedef std::map> ConstructorsMap; //! \returns A constant reference to the constructors map - ConstructorsMap const& constructors() const { return m_constructors; } - virtual Communicator *construct(CommunicatorTypeId communicator) const; + ConstructorsMap const& constructors() const + { + return m_constructors; + } + virtual Communicator* construct(CommunicatorTypeId communicator) const; private: ConstructorsMap m_constructors; }; -namespace CommunicatorType { - static const CommunicatorFactory::CommunicatorTypeId INVALID = 0; +namespace CommunicatorType +{ +static const CommunicatorFactory::CommunicatorTypeId INVALID = 0; } #endif diff --git a/lib/xspublic/xscontroller/compat.h b/lib/xspublic/xscontroller/compat.h index 7bfeec9..663d383 100644 --- a/lib/xspublic/xscontroller/compat.h +++ b/lib/xspublic/xscontroller/compat.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -68,13 +68,13 @@ #ifdef __GNUC__ #if __cplusplus < 201402L /* - * RMO: keep this code until we update our GCC to a version which supports C++14 - */ + RMO: keep this code until we update our GCC to a version which supports C++14 +*/ #include namespace std { template -std::unique_ptr make_unique(Args&&... args) +std::unique_ptr make_unique(Args&& ... args) { return std::unique_ptr(new T(std::forward(args)...)); } diff --git a/lib/xspublic/xscontroller/datalogger.cpp b/lib/xspublic/xscontroller/datalogger.cpp index 8274f78..5b0805a 100644 --- a/lib/xspublic/xscontroller/datalogger.cpp +++ b/lib/xspublic/xscontroller/datalogger.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xscontroller/datalogger.h b/lib/xspublic/xscontroller/datalogger.h index 1da7e11..2eb67e5 100644 --- a/lib/xspublic/xscontroller/datalogger.h +++ b/lib/xspublic/xscontroller/datalogger.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -77,7 +77,7 @@ class DataLogger : public CallbackManagerXda virtual ~DataLogger(); //! \brief Write a message to the logging stream - virtual bool writeMessage(const XsMessage &message) = 0; + virtual bool writeMessage(const XsMessage& message) = 0; //! \brief Closes a file virtual void close() = 0; diff --git a/lib/xspublic/xscontroller/datapacketcache.cpp b/lib/xspublic/xscontroller/datapacketcache.cpp index aa0997b..31e2553 100644 --- a/lib/xspublic/xscontroller/datapacketcache.cpp +++ b/lib/xspublic/xscontroller/datapacketcache.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xscontroller/datapacketcache.h b/lib/xspublic/xscontroller/datapacketcache.h index 8546c96..461987c 100644 --- a/lib/xspublic/xscontroller/datapacketcache.h +++ b/lib/xspublic/xscontroller/datapacketcache.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xscontroller/dataparser.cpp b/lib/xspublic/xscontroller/dataparser.cpp index 2d459a8..fa09aaa 100644 --- a/lib/xspublic/xscontroller/dataparser.cpp +++ b/lib/xspublic/xscontroller/dataparser.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -84,7 +84,7 @@ DataParser::~DataParser() { terminate(); } - catch(...) + catch (...) { } } @@ -92,7 +92,7 @@ DataParser::~DataParser() /*! \brief Adds the raw data to an array \param arr The reference to a byte array to which the data will be added */ -void DataParser::addRawData(const XsByteArray &arr) +void DataParser::addRawData(const XsByteArray& arr) { xsens::Lock locky(&m_incomingMutex); m_incoming.push(arr); diff --git a/lib/xspublic/xscontroller/dataparser.h b/lib/xspublic/xscontroller/dataparser.h index 8dd4069..175134c 100644 --- a/lib/xspublic/xscontroller/dataparser.h +++ b/lib/xspublic/xscontroller/dataparser.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -93,14 +93,17 @@ class DataParser : protected xsens::StandardThread virtual XsResultValue processBufferedData(const XsByteArray& rawIn, std::deque& messages) = 0; //! \copybrief Communicator::handleMessage - virtual void handleMessage(const XsMessage &message) = 0; + virtual void handleMessage(const XsMessage& message) = 0; - void addRawData(const XsByteArray &arr); + void addRawData(const XsByteArray& arr); void clear(); void terminate(); //! \returns The parser type - virtual const char* parserType() const { return "DataParser"; } + virtual const char* parserType() const + { + return "DataParser"; + } protected: void initFunction() override; diff --git a/lib/xspublic/xscontroller/datapoller.cpp b/lib/xspublic/xscontroller/datapoller.cpp index 0f03175..f118b1f 100644 --- a/lib/xspublic/xscontroller/datapoller.cpp +++ b/lib/xspublic/xscontroller/datapoller.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -80,13 +80,13 @@ DataPoller::~DataPoller() { cleanup(); } - catch(...) + catch (...) { } } /*! \brief Conjure up the time to wait based on properties of the received data (like the length) */ -int32_t DataPoller::conjureUpWaitTime(const XsByteArray &bytes) const +int32_t DataPoller::conjureUpWaitTime(const XsByteArray& bytes) const { if (bytes.size() == 0) return 3; diff --git a/lib/xspublic/xscontroller/datapoller.h b/lib/xspublic/xscontroller/datapoller.h index e0b4e5e..1e3a1dd 100644 --- a/lib/xspublic/xscontroller/datapoller.h +++ b/lib/xspublic/xscontroller/datapoller.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -81,7 +81,7 @@ class DataPoller : public xsens::StandardThread explicit DataPoller(DataParser& parser); protected: - virtual int32_t conjureUpWaitTime(const XsByteArray &bytes) const; + virtual int32_t conjureUpWaitTime(const XsByteArray& bytes) const; void initFunction() override; int32_t innerFunction() override; diff --git a/lib/xspublic/xscontroller/devicecommunicator.cpp b/lib/xspublic/xscontroller/devicecommunicator.cpp index ca13806..797b48f 100644 --- a/lib/xspublic/xscontroller/devicecommunicator.cpp +++ b/lib/xspublic/xscontroller/devicecommunicator.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -71,7 +71,7 @@ #include "protocolhandler.h" #ifdef LOG_COMMUNICATOR_RX_TX -#include + #include #endif /*! \class DeviceCommunicator @@ -107,7 +107,7 @@ DeviceCommunicator::~DeviceCommunicator() \param timeout The timeout in ms \returns True if successful */ -bool DeviceCommunicator::doTransaction(const XsMessage &msg, XsMessage &rcv, uint32_t timeout) +bool DeviceCommunicator::doTransaction(const XsMessage& msg, XsMessage& rcv, uint32_t timeout) { XsXbusMessageId expected = static_cast(msg.getMessageId() + 1); @@ -174,11 +174,14 @@ XsResultValue DeviceCommunicator::getDeviceId() if (!doTransaction(snd, rcv_did)) return setAndReturnLastResult(XRV_COULDNOTREADSETTINGS); - uint64_t deviceId = 0; - if (rcv_did.getDataSize() == 4) - deviceId = rcv_did.getDataLong(); - else if (rcv_did.getDataSize() == 8) - deviceId = rcv_did.getDataLongLong(); + uint64_t deviceId = rcv_did.getDataLong(); + if (rcv_did.getDataSize() == 8) + { + XsDeviceId did1(deviceId); + XsDeviceId did2(rcv_did.getDataLong(4)); + if (!did1.isLegacyDeviceId() || !did2.isLegacyDeviceId())//a must-have workaround for Mk5s with FTT firmware + deviceId = rcv_did.getDataLongLong(); + } XsMessage rcv_pdc; XsString productCode; @@ -186,7 +189,7 @@ XsResultValue DeviceCommunicator::getDeviceId() if (doTransaction(snd, rcv_pdc)) { const char* pc = (const char*) rcv_pdc.getDataBuffer(); - std::string result(pc?pc:"", rcv_pdc.getDataSize()); + std::string result(pc ? pc : "", rcv_pdc.getDataSize()); std::string::size_type thingy = result.find(" "); if (thingy < rcv_pdc.getDataSize()) result.erase(result.begin() + (unsigned)thingy, result.end()); @@ -242,7 +245,7 @@ XsResultValue DeviceCommunicator::gotoMeasurement() \param message The message to write \returns true on successful write, false otherwise. This doesn't guarantee proper delivery of the message. Use doTransaction for that. */ -bool DeviceCommunicator::writeMessage(const XsMessage &message) +bool DeviceCommunicator::writeMessage(const XsMessage& message) { XsByteArray raw; if (ProtocolHandler::composeMessage(raw, message) < 0) @@ -284,7 +287,7 @@ DeviceCommunicator::RxChannelId DeviceCommunicator::addRxChannel() \param[in] channel the channel to extract from \returns XRV_OK on success, something else on failure */ -XsResultValue DeviceCommunicator::extractMessages(const XsByteArray &rawIn, std::deque& messages, RxChannelId channel) +XsResultValue DeviceCommunicator::extractMessages(const XsByteArray& rawIn, std::deque& messages, RxChannelId channel) { if (channel >= m_messageExtractors.size()) return XRV_ERROR; @@ -304,6 +307,13 @@ XsResultValue DeviceCommunicator::extractMessages(const XsByteArray &rawIn, std: return res; } +/*! \brief Returns the number of message extractor this device communicator has +*/ +XsSize DeviceCommunicator::messageExtractorCount() const +{ + return m_messageExtractors.size(); +} + /*! \brief Returns the message extractor for the given rx channel */ MessageExtractor& DeviceCommunicator::messageExtractor(RxChannelId channel) @@ -314,7 +324,7 @@ MessageExtractor& DeviceCommunicator::messageExtractor(RxChannelId channel) /*! \copybrief Communicator::handleMessage */ -void DeviceCommunicator::handleMessage(const XsMessage &message) +void DeviceCommunicator::handleMessage(const XsMessage& message) { #ifdef LOG_COMMUNICATOR_RX_TX logRxStream(message); @@ -357,7 +367,7 @@ void DeviceCommunicator::abortLoadLogFile() { } -bool DeviceCommunicator::openLogFile(const XsString &) +bool DeviceCommunicator::openLogFile(const XsString&) { return false; } @@ -414,7 +424,7 @@ void DeviceCommunicator::generateLogFiles() XsString extension("mtb"); #endif - auto generateFilename = [&](XsString const& stream) + auto generateFilename = [&](XsString const & stream) { std::string temp = xprintf("communicator_%s_%s_%s.%s", stream.c_str(), date.c_str(), time.c_str(), extension.c_str()); return XsString(temp); diff --git a/lib/xspublic/xscontroller/devicecommunicator.h b/lib/xspublic/xscontroller/devicecommunicator.h index 6c89168..e0c4138 100644 --- a/lib/xspublic/xscontroller/devicecommunicator.h +++ b/lib/xspublic/xscontroller/devicecommunicator.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -69,7 +69,7 @@ #include "messageextractor.h" #ifdef LOG_COMMUNICATOR_RX_TX -#include + #include #endif @@ -88,14 +88,17 @@ class DeviceCommunicator : public Communicator XsResultValue gotoMeasurement() override; //! \returns The timeout value for gotoConfig function - uint32_t gotoConfigTimeout() const { return m_gotoConfigTimeout; } + uint32_t gotoConfigTimeout() const + { + return m_gotoConfigTimeout; + } void setGotoConfigTimeout(uint32_t timeout) override; - bool writeMessage(const XsMessage &message) override; + bool writeMessage(const XsMessage& message) override; - void handleMessage(const XsMessage &message) override; + void handleMessage(const XsMessage& message) override; using Communicator::doTransaction; - virtual bool doTransaction(const XsMessage &msg, XsMessage &rcv, uint32_t timeout) override; + virtual bool doTransaction(const XsMessage& msg, XsMessage& rcv, uint32_t timeout) override; void setKeepAlive(bool enable) override; @@ -106,7 +109,7 @@ class DeviceCommunicator : public Communicator std::deque readMessagesFromStartOfFile(uint8_t msgId, int maxMsgs = 0) override; void loadLogFile(XsDevice* device) override; void abortLoadLogFile() override; - bool openLogFile(const XsString &filename) override; + bool openLogFile(const XsString& filename) override; XsString logFileName() const override; XsFilePos logFileSize() const override; XsTimeStamp logFileDate() const override; @@ -128,14 +131,15 @@ class DeviceCommunicator : public Communicator protected: ~DeviceCommunicator() override; - XsResultValue extractMessages(const XsByteArray &rawIn, std::deque &messages, RxChannelId channel = 0); + XsResultValue extractMessages(const XsByteArray& rawIn, std::deque& messages, RxChannelId channel = 0); /*! \brief Writes a raw data to a device \param data The raw data to write \return XRV_OK if successful */ - virtual XsResultValue writeRawData(const XsByteArray &data) = 0; + virtual XsResultValue writeRawData(const XsByteArray& data) = 0; RxChannelId addRxChannel(); + XsSize messageExtractorCount() const; MessageExtractor& messageExtractor(RxChannelId = 0); #ifdef LOG_COMMUNICATOR_RX_TX diff --git a/lib/xspublic/xscontroller/devicefactory.cpp b/lib/xspublic/xscontroller/devicefactory.cpp index 83110a0..577a962 100644 --- a/lib/xspublic/xscontroller/devicefactory.cpp +++ b/lib/xspublic/xscontroller/devicefactory.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -76,6 +76,7 @@ #include "mti3x0device.h" #include "mti6x0device.h" #include "mti8x0device.h" +#include "dotdevice.h" /*! \class DeviceFactory \brief A Factory for the devices @@ -147,14 +148,14 @@ XsDevice* DeviceFactory::constructDevice(DeviceTypeId deviceTypeId, Communicator return device; } - /*! \brief Creates and initializes a master device with a specified communicator. - The type of the new device is retrieved from the device id if the communicator. - After construction the device will be initialized. - \param[in] communicator The communicator, the function always takes ownership of this pointer - \param[in] doInitialize If false, the device will not be initialized - \returns the newly created device or null when the device was not created or could not be initialized - \note the initializeDevice method should be overridden in derived classes. - \note when a created device can not be initialized, it will be deleted again along with the supplied communicator +/*! \brief Creates and initializes a master device with a specified communicator. + The type of the new device is retrieved from the device id if the communicator. + After construction the device will be initialized. + \param[in] communicator The communicator, the function always takes ownership of this pointer + \param[in] doInitialize If false, the device will not be initialized + \returns the newly created device or null when the device was not created or could not be initialized + \note the initializeDevice method should be overridden in derived classes. + \note when a created device can not be initialized, it will be deleted again along with the supplied communicator */ XsDevice* DeviceFactory::createMasterDevice(Communicator* communicator, bool doInitialize) { @@ -167,7 +168,7 @@ XsDevice* DeviceFactory::createMasterDevice(Communicator* communicator, bool doI { #ifndef XSENS_DEBUG // this is support debug info, we don't want to spam the debugger windows - JLWRITEG("Created master device with id: " << constructedDevice->deviceId() << " and firmware version: "<< constructedDevice->firmwareVersion().toString()); + JLWRITEG("Created master device with id: " << constructedDevice->deviceId() << " and firmware version: " << constructedDevice->firmwareVersion().toString()); #endif return constructedDevice; } @@ -185,18 +186,28 @@ XsDevice* DeviceFactory::createMasterDevice(Communicator* communicator, bool doI \param[in] deviceId the deviceId \returns the DeviceTypeId */ -DeviceFactory::DeviceTypeId DeviceFactory::deviceToTypeId(XsDeviceId const & deviceId) const +DeviceFactory::DeviceTypeId DeviceFactory::deviceToTypeId(XsDeviceId const& deviceId) const { if (deviceId.isMti() || deviceId.isMtig()) { - if (deviceId.isMtig()) return DeviceType::MTIG; - if (deviceId.isMtiX00()) return DeviceType::MTI_X00; - if (deviceId.isMtiX0()) return DeviceType::MTI_X0; - if (deviceId.isMtiX() && deviceId.isGnss()) return DeviceType::MTI_7; - if (deviceId.isMtiX()) return DeviceType::MTI_X; - if (deviceId.isMti3X0()) return DeviceType::MTI_3X0; - if (deviceId.isMti6X0()) return DeviceType::MTI_6X0; - if (deviceId.isMti8X0()) return DeviceType::MTI_8X0; + if (deviceId.isMtig()) + return DeviceType::MTIG; + if (deviceId.isMtiX00()) + return DeviceType::MTI_X00; + if (deviceId.isMtiX0()) + return DeviceType::MTI_X0; + if (deviceId.isMtiX() && deviceId.isGnss()) + return DeviceType::MTI_7; + if (deviceId.isMtiX()) + return DeviceType::MTI_X; + if (deviceId.isMti3X0()) + return DeviceType::MTI_3X0; + if (deviceId.isMti6X0()) + return DeviceType::MTI_6X0; + if (deviceId.isMti8X0()) + return DeviceType::MTI_8X0; + if (deviceId.isDot()) + return DeviceType::DOT; } return DeviceType::INVALID; @@ -204,7 +215,7 @@ DeviceFactory::DeviceTypeId DeviceFactory::deviceToTypeId(XsDeviceId const & dev /*! \brief Tell our device manager to remove any devices matching \a deviceId */ -void DeviceFactory::removeExistingDevice(const XsDeviceId &deviceId) +void DeviceFactory::removeExistingDevice(const XsDeviceId& deviceId) { (void) deviceId; } @@ -213,7 +224,7 @@ void DeviceFactory::removeExistingDevice(const XsDeviceId &deviceId) Calls initialize(m_loadedScenarioFile) on the device \param[in] dev The device to initialize. \returns true if initialization is successful. - */ +*/ bool DeviceFactory::initializeDevice(XsDevice* dev) const { if (dev && dev->initialize()) @@ -242,4 +253,5 @@ void DeviceFactory::registerDevices() (void)registerStandaloneDeviceType(DeviceType::MTI_3X0, &Mti3X0Device::constructStandalone); (void)registerStandaloneDeviceType(DeviceType::MTI_6X0, &Mti6X0Device::constructStandalone); (void)registerStandaloneDeviceType(DeviceType::MTI_8X0, &Mti8X0Device::constructStandalone); + (void)registerStandaloneDeviceType(DeviceType::DOT, &DotDevice::constructStandalone); } diff --git a/lib/xspublic/xscontroller/devicefactory.h b/lib/xspublic/xscontroller/devicefactory.h index 3e1dac2..e785d76 100644 --- a/lib/xspublic/xscontroller/devicefactory.h +++ b/lib/xspublic/xscontroller/devicefactory.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -82,21 +82,21 @@ class DeviceFactory typedef unsigned int DeviceTypeId; //! \brief A function prototype that provides the device factory and communicator - typedef XsDevice* (*MasterConstructFunc)(DeviceFactory & deviceFactory, Communicator* comm); + typedef XsDevice* (*MasterConstructFunc)(DeviceFactory& deviceFactory, Communicator* comm); //! \brief A function prototype that provides the communicator typedef XsDevice* (*StandaloneConstructFunc)(Communicator* comm); virtual XsDevice* createMasterDevice(Communicator* communicator, bool doInitialize = true); - virtual DeviceTypeId deviceToTypeId(XsDeviceId const & deviceId) const; + virtual DeviceTypeId deviceToTypeId(XsDeviceId const& deviceId) const; bool registerStandaloneDeviceType(DeviceTypeId deviceTypeId, StandaloneConstructFunc constructFunc); bool registerMasterDeviceType(DeviceTypeId deviceTypeId, MasterConstructFunc constructFunc); bool initializeDevice(XsDevice& device) const; - virtual void removeExistingDevice(XsDeviceId const & deviceId); + virtual void removeExistingDevice(XsDeviceId const& deviceId); virtual void registerDevices(); protected: @@ -109,7 +109,8 @@ class DeviceFactory std::map m_standaloneConstructors; }; -namespace DeviceType { - static const DeviceFactory::DeviceTypeId INVALID = 0; +namespace DeviceType +{ +static const DeviceFactory::DeviceTypeId INVALID = 0; } #endif diff --git a/lib/xspublic/xscontroller/deviceredetector.cpp b/lib/xspublic/xscontroller/deviceredetector.cpp index 1b4b5b7..113ff06 100644 --- a/lib/xspublic/xscontroller/deviceredetector.cpp +++ b/lib/xspublic/xscontroller/deviceredetector.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -76,7 +76,7 @@ */ /*! \brief Construct an Device Redetector object */ -DeviceRedetector::DeviceRedetector(const XsPortInfo &portInfo) +DeviceRedetector::DeviceRedetector(const XsPortInfo& portInfo) { #ifdef XSENS_WINDOWS if (portInfo.isUsb()) @@ -115,7 +115,7 @@ DeviceRedetector::DeviceRedetector(const XsPortInfo &portInfo) \param skipDeviceIdCheck If set to true the it will skip device id check \returns True if successful */ -bool DeviceRedetector::redetect(const XsDeviceId &deviceId, XsPortInfo &portInfo, bool skipDeviceIdCheck) +bool DeviceRedetector::redetect(const XsDeviceId& deviceId, XsPortInfo& portInfo, bool skipDeviceIdCheck) { FunctionPointer currentFunction; currentFunction = m_detectFunctions[deviceId.deviceType(false)]; @@ -135,7 +135,7 @@ bool DeviceRedetector::redetect(const XsDeviceId &deviceId, XsPortInfo &portInfo \param portInfo Port info \param skipDeviceIdCheck Skip device ID check */ -bool DeviceRedetector::redetectNoScan(const XsDeviceId &deviceId, XsPortInfo &portInfo, bool skipDeviceIdCheck) +bool DeviceRedetector::redetectNoScan(const XsDeviceId& deviceId, XsPortInfo& portInfo, bool skipDeviceIdCheck) { (void)deviceId; (void)portInfo; @@ -187,7 +187,7 @@ bool DeviceRedetector::redetectScanPorts(const XsDeviceId& deviceId, XsPortInfo& \param skipDeviceIdCheck If set to true then it will skip device ID check \returns True if successful */ -bool DeviceRedetector::redetectEnumerateSerialPorts(const XsDeviceId &deviceId, XsPortInfo &portInfo, bool skipDeviceIdCheck) +bool DeviceRedetector::redetectEnumerateSerialPorts(const XsDeviceId& deviceId, XsPortInfo& portInfo, bool skipDeviceIdCheck) { JLDEBUGG("Redetecting device " << deviceId); @@ -223,7 +223,7 @@ bool DeviceRedetector::redetectEnumerateSerialPorts(const XsDeviceId &deviceId, \param skipDeviceIdCheck If set to true then it will skip device ID check \returns True if successful */ -bool DeviceRedetector::redetectOneComPort(const XsDeviceId &deviceId, XsPortInfo &portInfo, bool skipDeviceIdCheck) +bool DeviceRedetector::redetectOneComPort(const XsDeviceId& deviceId, XsPortInfo& portInfo, bool skipDeviceIdCheck) { JLDEBUGG("Redetecting device " << deviceId); @@ -276,7 +276,7 @@ bool DeviceRedetector::redetectOneComPort(const XsDeviceId &deviceId, XsPortInfo \param portInfo Port info \param skipDeviceIdCheck Skip device ID check */ -bool DeviceRedetector::redetectEnumerateNetworkDevices(const XsDeviceId &deviceId, XsPortInfo &portInfo, bool skipDeviceIdCheck) +bool DeviceRedetector::redetectEnumerateNetworkDevices(const XsDeviceId& deviceId, XsPortInfo& portInfo, bool skipDeviceIdCheck) { (void)deviceId; (void)portInfo; diff --git a/lib/xspublic/xscontroller/deviceredetector.h b/lib/xspublic/xscontroller/deviceredetector.h index 3f41c34..921c431 100644 --- a/lib/xspublic/xscontroller/deviceredetector.h +++ b/lib/xspublic/xscontroller/deviceredetector.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -73,16 +73,16 @@ struct XsPortInfo; class DeviceRedetector { public: - DeviceRedetector(const XsPortInfo &portInfo); + DeviceRedetector(const XsPortInfo& portInfo); virtual ~DeviceRedetector() {} - bool redetect(const XsDeviceId &deviceId, XsPortInfo &portInfo, bool skipDeviceIdCheck = false); + bool redetect(const XsDeviceId& deviceId, XsPortInfo& portInfo, bool skipDeviceIdCheck = false); - virtual bool redetectScanPorts(const XsDeviceId &deviceId, XsPortInfo &portInfo, bool skipDeviceIdCheck); - virtual bool redetectEnumerateSerialPorts(const XsDeviceId &deviceId, XsPortInfo &portInfo, bool skipDeviceIdCheck); - virtual bool redetectNoScan(const XsDeviceId &deviceId, XsPortInfo &portInfo, bool skipDeviceIdCheck); - virtual bool redetectOneComPort(const XsDeviceId &deviceId, XsPortInfo &portInfo, bool skipDeviceIdCheck); - virtual bool redetectEnumerateNetworkDevices(const XsDeviceId &deviceId, XsPortInfo &portInfo, bool skipDeviceIdCheck); + virtual bool redetectScanPorts(const XsDeviceId& deviceId, XsPortInfo& portInfo, bool skipDeviceIdCheck); + virtual bool redetectEnumerateSerialPorts(const XsDeviceId& deviceId, XsPortInfo& portInfo, bool skipDeviceIdCheck); + virtual bool redetectNoScan(const XsDeviceId& deviceId, XsPortInfo& portInfo, bool skipDeviceIdCheck); + virtual bool redetectOneComPort(const XsDeviceId& deviceId, XsPortInfo& portInfo, bool skipDeviceIdCheck); + virtual bool redetectEnumerateNetworkDevices(const XsDeviceId& deviceId, XsPortInfo& portInfo, bool skipDeviceIdCheck); protected: diff --git a/lib/xspublic/xscontroller/devicetypes.h b/lib/xspublic/xscontroller/devicetypes.h index 5a240bb..b42bb74 100644 --- a/lib/xspublic/xscontroller/devicetypes.h +++ b/lib/xspublic/xscontroller/devicetypes.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -67,27 +67,30 @@ #include "devicefactory.h" -namespace DeviceType { - static const DeviceFactory::DeviceTypeId BODYPACK = 5; - static const DeviceFactory::DeviceTypeId AWINDA2STATION = 6; - static const DeviceFactory::DeviceTypeId AWINDA2DONGLE = 7; - static const DeviceFactory::DeviceTypeId AWINDA2OEM = 8; - static const DeviceFactory::DeviceTypeId SYNCSTATION = 9; - static const DeviceFactory::DeviceTypeId MTI_X = 20; - static const DeviceFactory::DeviceTypeId MTI_X0 = 21; - static const DeviceFactory::DeviceTypeId MTI_X00 = 22; - static const DeviceFactory::DeviceTypeId MTIG = 23; - static const DeviceFactory::DeviceTypeId MTI_7 = 24; - static const DeviceFactory::DeviceTypeId MTI_6X0 = 25; - static const DeviceFactory::DeviceTypeId MTI_8X0 = 26; - static const DeviceFactory::DeviceTypeId MTI_3X0 = 27; - static const DeviceFactory::DeviceTypeId MTX2 = 30; - static const DeviceFactory::DeviceTypeId GLOVE = 50; - static const DeviceFactory::DeviceTypeId MTW2 = 80; - static const DeviceFactory::DeviceTypeId IMARIFOG = 100; - static const DeviceFactory::DeviceTypeId IMARFSAS = 101; - static const DeviceFactory::DeviceTypeId ABMCLOCKMASTER = 200; - static const DeviceFactory::DeviceTypeId HILDEVICE = 300; +namespace DeviceType +{ +static const DeviceFactory::DeviceTypeId BODYPACK = 5; +static const DeviceFactory::DeviceTypeId AWINDA2STATION = 6; +static const DeviceFactory::DeviceTypeId AWINDA2DONGLE = 7; +static const DeviceFactory::DeviceTypeId AWINDA2OEM = 8; +static const DeviceFactory::DeviceTypeId SYNCSTATION = 9; +static const DeviceFactory::DeviceTypeId MTI_X = 20; +static const DeviceFactory::DeviceTypeId MTI_X0 = 21; +static const DeviceFactory::DeviceTypeId MTI_X00 = 22; +static const DeviceFactory::DeviceTypeId MTIG = 23; +static const DeviceFactory::DeviceTypeId MTI_7 = 24; +static const DeviceFactory::DeviceTypeId MTI_6X0 = 25; +static const DeviceFactory::DeviceTypeId MTI_8X0 = 26; +static const DeviceFactory::DeviceTypeId MTI_3X0 = 27; +static const DeviceFactory::DeviceTypeId MTX2 = 30; +static const DeviceFactory::DeviceTypeId GLOVE = 50; +static const DeviceFactory::DeviceTypeId MTW2 = 80; +static const DeviceFactory::DeviceTypeId IMARIFOG = 100; +static const DeviceFactory::DeviceTypeId IMARFSAS = 101; +static const DeviceFactory::DeviceTypeId IMARIFOGUART = 102; +static const DeviceFactory::DeviceTypeId ABMCLOCKMASTER = 200; +static const DeviceFactory::DeviceTypeId HILDEVICE = 300; +static const DeviceFactory::DeviceTypeId DOT = 400; } #endif diff --git a/lib/xspublic/xscontroller/dotdevice.cpp b/lib/xspublic/xscontroller/dotdevice.cpp new file mode 100644 index 0000000..8c012a6 --- /dev/null +++ b/lib/xspublic/xscontroller/dotdevice.cpp @@ -0,0 +1,101 @@ + +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions, and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions, and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the names of the copyright holders nor the names of their contributors +// may be used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY +// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL +// THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT +// OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY OR +// TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.THE LAWS OF THE NETHERLANDS +// SHALL BE EXCLUSIVELY APPLICABLE AND ANY DISPUTES SHALL BE FINALLY SETTLED UNDER THE RULES +// OF ARBITRATION OF THE INTERNATIONAL CHAMBER OF COMMERCE IN THE HAGUE BY ONE OR MORE +// ARBITRATORS APPOINTED IN ACCORDANCE WITH SAID RULES. +// + + +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions, and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions, and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the names of the copyright holders nor the names of their contributors +// may be used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY +// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL +// THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT +// OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY OR +// TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.THE LAWS OF THE NETHERLANDS +// SHALL BE EXCLUSIVELY APPLICABLE AND ANY DISPUTES SHALL BE FINALLY SETTLED UNDER THE RULES +// OF ARBITRATION OF THE INTERNATIONAL CHAMBER OF COMMERCE IN THE HAGUE BY ONE OR MORE +// ARBITRATORS APPOINTED IN ACCORDANCE WITH SAID RULES. +// + +#include "dotdevice.h" + +#include +#include "replyobject.h" +#include "communicator.h" +#include "scenariomatchpred.h" +#include +#include +#include + +using namespace xsens; + +/*! \brief Constructs a device + \param comm The communicator to construct with +*/ +DotDevice::DotDevice(Communicator* comm) + : MtiBaseDeviceEx(comm) +{ +} + +/*! \brief Destroys a device +*/ +DotDevice::~DotDevice() +{ +} + +/*! \brief Returns the base update rate (Hz) corresponding to the dataType. Returns 0 if no update rate is available +*/ +MtiBaseDevice::BaseFrequencyResult DotDevice::getBaseFrequencyInternal(XsDataIdentifier dataType) const +{ + (void)dataType; + MtiBaseDevice::BaseFrequencyResult result; + result.m_frequency = 0; + result.m_divedable = true; + + return result; +} diff --git a/lib/xspublic/xscontroller/dotdevice.h b/lib/xspublic/xscontroller/dotdevice.h new file mode 100644 index 0000000..7f4c82d --- /dev/null +++ b/lib/xspublic/xscontroller/dotdevice.h @@ -0,0 +1,111 @@ + +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions, and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions, and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the names of the copyright holders nor the names of their contributors +// may be used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY +// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL +// THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT +// OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY OR +// TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.THE LAWS OF THE NETHERLANDS +// SHALL BE EXCLUSIVELY APPLICABLE AND ANY DISPUTES SHALL BE FINALLY SETTLED UNDER THE RULES +// OF ARBITRATION OF THE INTERNATIONAL CHAMBER OF COMMERCE IN THE HAGUE BY ONE OR MORE +// ARBITRATORS APPOINTED IN ACCORDANCE WITH SAID RULES. +// + + +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions, and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions, and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the names of the copyright holders nor the names of their contributors +// may be used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY +// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL +// THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT +// OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY OR +// TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.THE LAWS OF THE NETHERLANDS +// SHALL BE EXCLUSIVELY APPLICABLE AND ANY DISPUTES SHALL BE FINALLY SETTLED UNDER THE RULES +// OF ARBITRATION OF THE INTERNATIONAL CHAMBER OF COMMERCE IN THE HAGUE BY ONE OR MORE +// ARBITRATORS APPOINTED IN ACCORDANCE WITH SAID RULES. +// + +#ifndef XSDOTDEVICE_H +#define XSDOTDEVICE_H + +#include "mtibasedevice.h" + +/*! \class DotDevice + \brief The MTi device used for the Xsens DOT +*/ +class DotDevice : public MtiBaseDeviceEx +{ +public: + /*! \brief Constructs a standalone device using a provided communicator + \param comm The communicator to use + \return The constructed device + */ + static XsDevice* constructStandalone(Communicator* comm) + { + return new DotDevice(comm); + } + + explicit DotDevice(Communicator* comm); + + //! \brief An empty constructor for a master device + explicit DotDevice(XsDevice* master) : MtiBaseDeviceEx(master) {} + virtual ~DotDevice(); + +protected: + MtiBaseDevice::BaseFrequencyResult getBaseFrequencyInternal(XsDataIdentifier dataType = XDI_None) const override; +}; + +#ifndef XDA_PRIVATE_BUILD +/*! \class DotDeviceEx + \brief The internal base class for MTi-3X0 series devices +*/ +struct DotDeviceEx : public DotDevice +{ + //! \copybrief DotDevice::DotDevice + explicit DotDeviceEx(Communicator* comm) : DotDevice(comm) {}; + + //! \copybrief DotDevice::DotDevice + explicit DotDeviceEx(XsDevice* master) : DotDevice(master) {}; +}; +#else +#include "dotdeviceex.h" +#endif + +#endif diff --git a/lib/xspublic/xscontroller/dummy.cpp b/lib/xspublic/xscontroller/dummy.cpp index 7a888ad..86c7f03 100644 --- a/lib/xspublic/xscontroller/dummy.cpp +++ b/lib/xspublic/xscontroller/dummy.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -64,7 +64,7 @@ // This exists just so the library isn't empty for the VS build #ifdef _WIN32 -#include + #include #endif const int xsControllerDummy = 0; diff --git a/lib/xspublic/xscontroller/enumerateusbdevices.cpp b/lib/xspublic/xscontroller/enumerateusbdevices.cpp index 8027aad..9e36bf5 100644 --- a/lib/xspublic/xscontroller/enumerateusbdevices.cpp +++ b/lib/xspublic/xscontroller/enumerateusbdevices.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -66,26 +66,26 @@ #include "enumerateusbdevices.h" #ifdef _WIN32 -# include -# include -# include -# include -# include -# include -# include + #include + #include + #include + #include + #include + #include + #include #else -# include -# include -# include -# include "xslibusb.h" + #include + #include + #include + #include "xslibusb.h" #endif #ifdef _WIN32 /* return the device path for given windows device */ -static std::string getDevicePath(HDEVINFO hDevInfo, SP_DEVINFO_DATA *DeviceInfoData) +static std::string getDevicePath(HDEVINFO hDevInfo, SP_DEVINFO_DATA* DeviceInfoData) { char deviceInstanceID[MAX_DEVICE_ID_LEN]; - SetupDiGetDeviceInstanceIdA(hDevInfo, DeviceInfoData,deviceInstanceID, MAX_DEVICE_ID_LEN, NULL); + SetupDiGetDeviceInstanceIdA(hDevInfo, DeviceInfoData, deviceInstanceID, MAX_DEVICE_ID_LEN, NULL); return std::string(deviceInstanceID); } @@ -110,7 +110,7 @@ static inline uint16_t vidFromDevPath(std::string const& devpath) return 0; return static_cast(std::stoi(t1, nullptr, 16)); } - catch (std::invalid_argument &) + catch (std::invalid_argument&) { return 0; } @@ -126,7 +126,7 @@ static inline uint16_t pidFromDevPath(std::string const& devpath) return 0; return static_cast(std::stoi(t1, nullptr, 16)); } - catch (std::invalid_argument &) + catch (std::invalid_argument&) { return 0; } @@ -139,7 +139,7 @@ static inline uint16_t pidFromDevPath(std::string const& devpath) found by xsEnumerateSerialPorts(). \param[in,out] ports The list of serial ports to append to - \returns False if an error occured during scanning. + \returns False if an error occurred during scanning. True if zero or more device found or no scan could be done because both WINUSB and LIBUSB were not defined. */ bool xsEnumerateUsbDevices(XsPortInfoArray& ports) @@ -148,7 +148,7 @@ bool xsEnumerateUsbDevices(XsPortInfoArray& ports) #ifdef USE_WINUSB BOOL bResult = FALSE; ULONG length; - ULONG requiredLength=0; + ULONG requiredLength = 0; // {FD51225C-700A-47e5-9999-B2D9031B88ED} GUID guid = { 0xfd51225c, 0x700a, 0x47e5, { 0x99, 0x99, 0xb2, 0xd9, 0x3, 0x1b, 0x88, 0xed } }; @@ -161,10 +161,9 @@ bool xsEnumerateUsbDevices(XsPortInfoArray& ports) // Initialize variables. interfaceData.cbSize = sizeof(SP_INTERFACE_DEVICE_DATA); - int port = 0; - for (DWORD dwIndex = 0; port == 0; ++dwIndex) + for (DWORD dwIndex = 0; true; ++dwIndex) //lint !e774 !e440 !e506 { - BOOL bRet = SetupDiEnumDeviceInterfaces( deviceInfo, NULL, &guid, dwIndex, &interfaceData); + BOOL bRet = SetupDiEnumDeviceInterfaces(deviceInfo, NULL, &guid, dwIndex, &interfaceData); if (!bRet) { if (GetLastError() == ERROR_NO_MORE_ITEMS) @@ -204,21 +203,21 @@ bool xsEnumerateUsbDevices(XsPortInfoArray& ports) char* ptrEnd, *ptrStart = strchr(detailData->DevicePath, '#'); if (!ptrStart) continue; - ptrStart = strchr(ptrStart+1, '#'); + ptrStart = strchr(ptrStart + 1, '#'); if (!ptrStart) continue; - ptrEnd = strchr(ptrStart+1, '#'); + ptrEnd = strchr(ptrStart + 1, '#'); if (!ptrEnd) continue; - size_t ln = (size_t) ((ptrEnd-ptrStart)-1); - strncpy((char*)serialNumber, ptrStart+1, ln); + size_t ln = (size_t)((ptrEnd - ptrStart) - 1); + strncpy((char*)serialNumber, ptrStart + 1, ln); serialNumber[ln] = '\0'; current.setPortName(detailData->DevicePath); unsigned int id = 0; - sscanf((const char *)serialNumber, "%X", &id); + sscanf((const char*)serialNumber, "%X", &id); current.setDeviceId((uint32_t) id); std::string devpath = getDevicePath(deviceInfo, &DevInfoData); @@ -234,16 +233,16 @@ bool xsEnumerateUsbDevices(XsPortInfoArray& ports) return true; #elif defined(HAVE_LIBUSB) XsLibUsb libUsb; - libusb_context *context; + libusb_context* context; int result = libUsb.init(&context); if (result != LIBUSB_SUCCESS) return true; - libusb_device **deviceList; + libusb_device** deviceList; ssize_t deviceCount = libUsb.get_device_list(context, &deviceList); for (ssize_t i = 0; i < deviceCount; i++) { - libusb_device *device = deviceList[i]; + libusb_device* device = deviceList[i]; libusb_device_descriptor desc; result = libUsb.get_device_descriptor(device, &desc); if (result != LIBUSB_SUCCESS) @@ -252,7 +251,7 @@ bool xsEnumerateUsbDevices(XsPortInfoArray& ports) if (desc.idVendor != XSENS_VENDOR_ID) continue; - libusb_device_handle *handle; + libusb_device_handle* handle; result = libUsb.open(device, &handle); if (result != LIBUSB_SUCCESS) continue; @@ -260,7 +259,7 @@ bool xsEnumerateUsbDevices(XsPortInfoArray& ports) unsigned char serialNumber[256]; result = libUsb.get_string_descriptor_ascii(handle, desc.iSerialNumber, serialNumber, 256); - libusb_config_descriptor *configDesc; + libusb_config_descriptor* configDesc; result = libUsb.get_active_config_descriptor(device, &configDesc); if (result != LIBUSB_SUCCESS) { @@ -269,7 +268,8 @@ bool xsEnumerateUsbDevices(XsPortInfoArray& ports) } bool kernelActive = false; - for (uint8_t ifCount = 0; ifCount < configDesc->bNumInterfaces; ++ifCount) { + for (uint8_t ifCount = 0; ifCount < configDesc->bNumInterfaces; ++ifCount) + { int res = libUsb.kernel_driver_active(handle, ifCount); kernelActive |= (res == 1); } @@ -280,11 +280,11 @@ bool xsEnumerateUsbDevices(XsPortInfoArray& ports) { char name[256]; sprintf(name, "USB%03u:%03u", libUsb.get_bus_number(device), - libUsb.get_device_address(device)); + libUsb.get_device_address(device)); current.setPortName(name); int id = 0; - sscanf((const char *)serialNumber, "%X", &id); + sscanf((const char*)serialNumber, "%X", &id); current.setDeviceId((uint32_t) id); current.setVidPid(desc.idVendor, desc.idProduct); ports.push_back(current); @@ -293,7 +293,7 @@ bool xsEnumerateUsbDevices(XsPortInfoArray& ports) { JLDEBUGG("Kernel driver active on USB" << libUsb.get_bus_number(device) << ":" << libUsb.get_device_address(device) << - " device " << serialNumber); + " device " << serialNumber); } libUsb.close(handle); diff --git a/lib/xspublic/xscontroller/enumerateusbdevices.h b/lib/xspublic/xscontroller/enumerateusbdevices.h index 8550b64..00acfc9 100644 --- a/lib/xspublic/xscontroller/enumerateusbdevices.h +++ b/lib/xspublic/xscontroller/enumerateusbdevices.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xscontroller/enumexpanders.cpp b/lib/xspublic/xscontroller/enumexpanders.cpp index eb9c6a2..52f7f1a 100644 --- a/lib/xspublic/xscontroller/enumexpanders.cpp +++ b/lib/xspublic/xscontroller/enumexpanders.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -98,4 +98,5 @@ static void forceEnumExpanderInclusion() JLWRITEG("blah" << XDS_Initial << blah); JLWRITEG("blah" << XOP_Unknown << blah); JLWRITEG("blah" << XCS_File << blah); + (void)blah; } diff --git a/lib/xspublic/xscontroller/enumexpanders.h b/lib/xspublic/xscontroller/enumexpanders.h index e0e27b2..37877ac 100644 --- a/lib/xspublic/xscontroller/enumexpanders.h +++ b/lib/xspublic/xscontroller/enumexpanders.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xscontroller/fileloader.h b/lib/xspublic/xscontroller/fileloader.h index 66558d7..7188cf8 100644 --- a/lib/xspublic/xscontroller/fileloader.h +++ b/lib/xspublic/xscontroller/fileloader.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -79,7 +79,7 @@ class FileLoader \param device The device to read from \returns XRV_OK if successful */ - virtual XsResultValue readLogFile(XsDevice *device) = 0; + virtual XsResultValue readLogFile(XsDevice* device) = 0; /*! \brief Read a single packet from file \returns XRV_OK is successful diff --git a/lib/xspublic/xscontroller/gpsstatus.h b/lib/xspublic/xscontroller/gpsstatus.h index 1f60d7a..4c7335d 100644 --- a/lib/xspublic/xscontroller/gpsstatus.h +++ b/lib/xspublic/xscontroller/gpsstatus.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -70,15 +70,16 @@ #include #ifndef __cplusplus -#define XSGPSSATELLITEINFO_INITIALIZER { 0, 0, 0, 0 } -#define XSGPSSTATUS_INITIALIZER { 0, XSGPSSATELLITEINFO_INITIALIZER, XSGPSSATELLITEINFO_INITIALIZER, XSGPSSATELLITEINFO_INITIALIZER, XSGPSSATELLITEINFO_INITIALIZER, XSGPSSATELLITEINFO_INITIALIZER, XSGPSSATELLITEINFO_INITIALIZER, XSGPSSATELLITEINFO_INITIALIZER, XSGPSSATELLITEINFO_INITIALIZER, XSGPSSATELLITEINFO_INITIALIZER, XSGPSSATELLITEINFO_INITIALIZER, XSGPSSATELLITEINFO_INITIALIZER, XSGPSSATELLITEINFO_INITIALIZER, XSGPSSATELLITEINFO_INITIALIZER, XSGPSSATELLITEINFO_INITIALIZER, XSGPSSATELLITEINFO_INITIALIZER, XSGPSSATELLITEINFO_INITIALIZER }; + #define XSGPSSATELLITEINFO_INITIALIZER { 0, 0, 0, 0 } + #define XSGPSSTATUS_INITIALIZER { 0, XSGPSSATELLITEINFO_INITIALIZER, XSGPSSATELLITEINFO_INITIALIZER, XSGPSSATELLITEINFO_INITIALIZER, XSGPSSATELLITEINFO_INITIALIZER, XSGPSSATELLITEINFO_INITIALIZER, XSGPSSATELLITEINFO_INITIALIZER, XSGPSSATELLITEINFO_INITIALIZER, XSGPSSATELLITEINFO_INITIALIZER, XSGPSSATELLITEINFO_INITIALIZER, XSGPSSATELLITEINFO_INITIALIZER, XSGPSSATELLITEINFO_INITIALIZER, XSGPSSATELLITEINFO_INITIALIZER, XSGPSSATELLITEINFO_INITIALIZER, XSGPSSATELLITEINFO_INITIALIZER, XSGPSSATELLITEINFO_INITIALIZER, XSGPSSATELLITEINFO_INITIALIZER }; #endif #define XS_MAX_SVINFO 16 /*! \brief Storage for status of one GPS satellite, used by MTi-G only */ -struct XsGpsSatelliteInfo { +struct XsGpsSatelliteInfo +{ uint8_t m_id; //!< Satellite ID uint8_t m_navigationStatus; //!< Navigation status of the Satellite (see low-level documentation for interpretation) uint8_t m_signalQuality; //!< Signal quality @@ -88,7 +89,8 @@ typedef struct XsGpsSatelliteInfo XsGpsSatelliteInfo; /*! \brief Storage for all GPS satellites' information, used by MTi-G only */ -struct GpsStatus { +struct GpsStatus +{ XsDeviceId m_deviceId; //!< The device that reported the GPS status XsGpsSatelliteInfo m_svInfo[XS_MAX_SVINFO]; //!< An array of the GPS satellite infos }; diff --git a/lib/xspublic/xscontroller/idfetchhelpers.h b/lib/xspublic/xscontroller/idfetchhelpers.h index 0f12109..e4cb69c 100644 --- a/lib/xspublic/xscontroller/idfetchhelpers.h +++ b/lib/xspublic/xscontroller/idfetchhelpers.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -103,7 +103,7 @@ static inline uint16_t vidFromString(std::string const& string) return 0; return static_cast(std::stoi(t1, nullptr, 16)); } - catch (std::invalid_argument &) + catch (std::invalid_argument&) { return 0; } @@ -121,7 +121,7 @@ static inline uint16_t pidFromString(std::string const& string) return 0; return static_cast(std::stoi(t1, nullptr, 16)); } - catch (std::invalid_argument &) + catch (std::invalid_argument&) { return 0; } diff --git a/lib/xspublic/xscontroller/iointerface.cpp b/lib/xspublic/xscontroller/iointerface.cpp index f541c20..8ea9acb 100644 --- a/lib/xspublic/xscontroller/iointerface.cpp +++ b/lib/xspublic/xscontroller/iointerface.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -66,22 +66,22 @@ #include "iointerface.h" #include #ifndef _WIN32 -# include // close -# include // ioctl -# include // open, O_RDWR -# include // strcpy -# include -# include + #include // close + #include // ioctl + #include // open, O_RDWR + #include // strcpy + #include + #include #else -# include -# include + #include + #include #endif #ifndef _CRT_SECURE_NO_DEPRECATE -# define _CRT_SECURE_NO_DEPRECATE -# ifdef _WIN32 -# pragma warning(disable:4996) -# endif + #define _CRT_SECURE_NO_DEPRECATE + #ifdef _WIN32 + #pragma warning(disable:4996) + #endif #endif /*! \class IoInterface @@ -93,18 +93,18 @@ // IOInterface functions /*! \copydoc SerialInterface::open(const XsPortInfo&, XsFilePos, XsFilePos, PortOptions) */ -XsResultValue IoInterface::open ( const XsPortInfo&, XsFilePos, XsFilePos, PortOptions) +XsResultValue IoInterface::open(const XsPortInfo&, XsFilePos, XsFilePos, PortOptions) { return XRV_INVALIDOPERATION; } /*! \copydoc SerialInterface::setTimeout(uint32_t) */ -XsResultValue IoInterface::setTimeout (uint32_t ms) +XsResultValue IoInterface::setTimeout(uint32_t ms) { (void) ms; return XRV_INVALIDOPERATION; } /*! \copydoc SerialInterface::waitForData(XsFilePos, XsByteArray&) */ -XsResultValue IoInterface::waitForData (XsFilePos maxLength, XsByteArray& data) +XsResultValue IoInterface::waitForData(XsFilePos maxLength, XsByteArray& data) { (void) maxLength; (void) data; diff --git a/lib/xspublic/xscontroller/iointerface.h b/lib/xspublic/xscontroller/iointerface.h index b4250fd..533c68b 100644 --- a/lib/xspublic/xscontroller/iointerface.h +++ b/lib/xspublic/xscontroller/iointerface.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -83,7 +83,8 @@ struct XsPortInfo; -struct IoInterface { +struct IoInterface +{ public: /*! \brief Destructor */ virtual ~IoInterface() {} @@ -101,7 +102,7 @@ struct IoInterface { /*! \brief Returns true if the object has a connection to a device \returns true if the object has a connection to a device */ - virtual bool isOpen (void) const = 0; + virtual bool isOpen(void) const = 0; /*! \brief Returns the last result value produced by this interface \returns The last result value produced by this interface. @@ -126,19 +127,20 @@ struct IoInterface { virtual XsResultValue readData(XsFilePos maxLength, XsByteArray& data) = 0; //! Options for flow control and stopbits which must be used when opening a port - enum PortOptions { + enum PortOptions + { PO_NoFlowControl = 0, PO_RtsCtsFlowControl = (1 << 0), PO_DtrDsrFlowControl = (1 << 1), PO_XonXoffFlowControl = (1 << 2), PO_OneStopBit = 0, PO_TwoStopBits = (1 << 3), - PO_XsensDefaults = (PO_NoFlowControl|PO_TwoStopBits) + PO_XsensDefaults = (PO_NoFlowControl | PO_TwoStopBits) }; // SerialInterface overridable functions virtual XsResultValue open(const XsPortInfo& portInfo, XsFilePos readBufSize = XS_DEFAULT_READ_BUFFER_SIZE, XsFilePos writeBufSize = XS_DEFAULT_WRITE_BUFFER_SIZE, PortOptions options = PO_XsensDefaults); - virtual XsResultValue setTimeout (uint32_t ms); - virtual XsResultValue waitForData (XsFilePos maxLength, XsByteArray& data); + virtual XsResultValue setTimeout(uint32_t ms); + virtual XsResultValue waitForData(XsFilePos maxLength, XsByteArray& data); virtual void cancelIo(void) const; // IOInterfaceFile overridable functions @@ -162,6 +164,6 @@ struct IoInterface { IoInterface() {} -XSENS_DISABLE_COPY(IoInterface); + XSENS_DISABLE_COPY(IoInterface); }; #endif diff --git a/lib/xspublic/xscontroller/iointerfacefile.cpp b/lib/xspublic/xscontroller/iointerfacefile.cpp index 6631e86..7aa62c5 100644 --- a/lib/xspublic/xscontroller/iointerfacefile.cpp +++ b/lib/xspublic/xscontroller/iointerfacefile.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -66,26 +66,26 @@ #include #ifndef _WIN32 -# include // close -# include // ioctl -# include // open, O_RDWR -# include // strcpy -# include -# include -# include + #include // close + #include // ioctl + #include // open, O_RDWR + #include // strcpy + #include + #include + #include #else -# include -# include -# include + #include + #include + #include #endif #include #ifndef _CRT_SECURE_NO_DEPRECATE -# define _CRT_SECURE_NO_DEPRECATE -# ifdef _WIN32 -# pragma warning(disable:4996) -# endif + #define _CRT_SECURE_NO_DEPRECATE + #ifdef _WIN32 + #pragma warning(disable:4996) + #endif #endif /*! Default constructor, initializes all members to their default values. @@ -109,7 +109,7 @@ IoInterfaceFile::~IoInterfaceFile() { closeFile(); } - catch(...) + catch (...) { } } @@ -290,9 +290,7 @@ XsResultValue IoInterfaceFile::deleteData(XsFilePos start, XsFilePos length) m_fileSize -= length; } else - { m_fileSize = start; - } XsResultValue truncateResult = m_handle->truncate(m_fileSize); @@ -330,7 +328,7 @@ XsResultValue IoInterfaceFile::find(const XsByteArray& needleV, XsFilePos& pos) XsFilePos bufferPos, needlePos = 0; XsFilePos readBytes; if (m_readPos & 0x1FF) // read a block of data - readBytes = m_handle->read(buffer, 1, (m_fileBlockSize-(m_readPos & (m_fileBlockSize-1)))); + readBytes = m_handle->read(buffer, 1, (m_fileBlockSize - (m_readPos & (m_fileBlockSize - 1)))); else readBytes = m_handle->read(buffer, 1, m_fileBlockSize); // read a block of data @@ -392,14 +390,14 @@ XsTimeStamp IoInterfaceFile::getFileDate() const if (stat(m_filename.c_str(), &stats) == 0) #endif { - XsTimeStamp t = XsTimeStamp( (int64_t)stats.st_mtime * 1000); + XsTimeStamp t = XsTimeStamp((int64_t)stats.st_mtime * 1000); return t; } return XsTimeStamp(); } /*! \copydoc IoInterface::flushData */ -XsResultValue IoInterfaceFile::flushData () +XsResultValue IoInterfaceFile::flushData() { m_handle->flush(); @@ -471,11 +469,11 @@ XsResultValue IoInterfaceFile::insertData(XsFilePos start, const XsByteArray& da XsFilePos read1, read2; XsFilePos remaining = m_fileSize - start; XsFilePos bsize = (length > m_fileBlockSize) ? length : m_fileBlockSize; - char* bufferRoot = (char*) malloc((XsSize) (bsize*2)); + char* bufferRoot = (char*) malloc((XsSize)(bsize * 2)); if (!bufferRoot) return XRV_OUTOFMEMORY; char* buffer1 = bufferRoot; - char* buffer2 = bufferRoot+bsize; + char* buffer2 = bufferRoot + bsize; char* btemp; // copy data @@ -492,11 +490,13 @@ XsResultValue IoInterfaceFile::insertData(XsFilePos start, const XsByteArray& da remaining -= read1; rPos += read1; - while(remaining > 0) + while (remaining > 0) { // move data to correct buffer read2 = read1; - btemp = buffer1; buffer1 = buffer2; buffer2 = btemp; + btemp = buffer1; + buffer1 = buffer2; + buffer2 = btemp; // read next block if (remaining >= bsize) @@ -546,9 +546,7 @@ XsResultValue IoInterfaceFile::open(const XsString& filename, bool createNew, bo if (localResult != XRV_OK) { if (createNew) - { localResult = m_handle->create(filename, false); - } else { // final attempt: open it forced readonly @@ -566,11 +564,11 @@ XsResultValue IoInterfaceFile::open(const XsString& filename, bool createNew, bo bool fail = false; #ifdef _WIN32 wchar_t fullpath[XS_MAX_FILENAME_LENGTH]; - if (_wfullpath(fullpath,filename.toStdWString().c_str(),XS_MAX_FILENAME_LENGTH) == NULL) + if (_wfullpath(fullpath, filename.toStdWString().c_str(), XS_MAX_FILENAME_LENGTH) == NULL) fail = true; #else // use the same trick again. - char fullpath[XS_MAX_FILENAME_LENGTH*2]; + char fullpath[XS_MAX_FILENAME_LENGTH * 2]; if (realpath(filename.c_str(), fullpath) == NULL) fail = true; #endif @@ -622,7 +620,7 @@ XsResultValue IoInterfaceFile::readData(XsFilePos maxLength, XsByteArray& data) m_readPos += length; if (length < maxLength) - data.pop_back((XsSize) (maxLength - length)); + data.pop_back((XsSize)(maxLength - length)); return m_lastResult = XRV_OK; } @@ -635,7 +633,7 @@ XsResultValue IoInterfaceFile::readData(XsFilePos maxLength, XsByteArray& data) */ XsResultValue IoInterfaceFile::readDataBlocks(XsFilePos blockCount, XsByteArray& data) { - XsFilePos realign = (m_readPos & (m_fileBlockSize-1)); + XsFilePos realign = (m_readPos & (m_fileBlockSize - 1)); if (realign) blockCount = m_fileBlockSize * blockCount + m_fileBlockSize - realign; else @@ -667,7 +665,7 @@ XsResultValue IoInterfaceFile::readTerminatedData(XsFilePos maxLength, unsigned } bdata.setSize((XsSize) maxLength); - char *data = (char *) bdata.data(); + char* data = (char*) bdata.data(); XsFilePos length; int readChar; @@ -687,11 +685,11 @@ XsResultValue IoInterfaceFile::readTerminatedData(XsFilePos maxLength, unsigned return m_lastResult = XRV_OK; if ((unsigned char) readChar == terminator) { - bdata.pop_back((XsSize) (maxLength - length)); + bdata.pop_back((XsSize)(maxLength - length)); return m_lastResult = XRV_OK; } } - bdata.pop_back((XsSize) (maxLength - length)); + bdata.pop_back((XsSize)(maxLength - length)); return m_lastResult = XRV_ENDOFFILE; } @@ -750,7 +748,7 @@ XsResultValue IoInterfaceFile::setWritePosition(XsFilePos pos) /*! \copydoc IoInterface::writeData \note The function writes the given data to the file at the current write position. */ -XsResultValue IoInterfaceFile::writeData(const XsByteArray& data, XsFilePos *written) +XsResultValue IoInterfaceFile::writeData(const XsByteArray& data, XsFilePos* written) { if (!m_handle) return m_lastResult = XRV_NOFILEOPEN; @@ -768,10 +766,14 @@ XsResultValue IoInterfaceFile::writeData(const XsByteArray& data, XsFilePos *wri int err = errno; switch (err) { - case 0: break; - case ENOSPC: return m_lastResult = XRV_INSUFFICIENTSPACE; - case ENOMEM: return m_lastResult = XRV_OUTOFMEMORY; - default: return m_lastResult = XRV_ERROR; + case 0: + break; + case ENOSPC: + return m_lastResult = XRV_INSUFFICIENTSPACE; + case ENOMEM: + return m_lastResult = XRV_OUTOFMEMORY; + default: + return m_lastResult = XRV_ERROR; } } m_writePos += writeRes; diff --git a/lib/xspublic/xscontroller/iointerfacefile.h b/lib/xspublic/xscontroller/iointerfacefile.h index c80cf0c..067da9b 100644 --- a/lib/xspublic/xscontroller/iointerfacefile.h +++ b/lib/xspublic/xscontroller/iointerfacefile.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -74,17 +74,17 @@ class IoInterfaceFile : public IoInterface { protected: - //! The file handle, also indicates if the file is open or not. + //! The file handle, also indicates if the file is open or not. XsFile* m_handle; - //! Contains the size of the file + //! Contains the size of the file XsFilePos m_fileSize; - //! The last read position in the file + //! The last read position in the file XsFilePos m_readPos; - //! The last write position in the file + //! The last write position in the file XsFilePos m_writePos; - //! The last result of an operation + //! The last result of an operation mutable XsResultValue m_lastResult; - //! Contains the name of the file that was last successfully opened. + //! Contains the name of the file that was last successfully opened. XsString m_filename; /*! \brief Indicates whether the last operation was a read or write operation. @@ -92,7 +92,7 @@ class IoInterfaceFile : public IoInterface requested read or write operation. */ bool m_reading; - //! Indicates if the file was opened in read-only mode + //! Indicates if the file was opened in read-only mode bool m_readOnly; void gotoRead(); @@ -107,7 +107,7 @@ class IoInterfaceFile : public IoInterface XsResultValue flushData() override; bool isOpen() const override; XsResultValue getLastResult() const override; - XsResultValue writeData(const XsByteArray& data, XsFilePos *written = nullptr) override; + XsResultValue writeData(const XsByteArray& data, XsFilePos* written = nullptr) override; XsResultValue readData(XsFilePos maxLength, XsByteArray& data) override; XsResultValue readDataBlocks(XsFilePos blockCount, XsByteArray& data); XsResultValue readTerminatedData(XsFilePos maxLength, unsigned char terminator, XsByteArray& bdata); @@ -135,7 +135,7 @@ class IoInterfaceFile : public IoInterface //! \brief The default file block size static const XsFilePos m_fileBlockSize = 4096; -XSENS_DISABLE_COPY(IoInterfaceFile); + XSENS_DISABLE_COPY(IoInterfaceFile); }; #endif diff --git a/lib/xspublic/xscontroller/iprotocolhandler.h b/lib/xspublic/xscontroller/iprotocolhandler.h index cf9eb63..a4bcdfc 100644 --- a/lib/xspublic/xscontroller/iprotocolhandler.h +++ b/lib/xspublic/xscontroller/iprotocolhandler.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xscontroller/iprotocolmanager.h b/lib/xspublic/xscontroller/iprotocolmanager.h index effecb6..450b689 100644 --- a/lib/xspublic/xscontroller/iprotocolmanager.h +++ b/lib/xspublic/xscontroller/iprotocolmanager.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -70,7 +70,7 @@ #include "xsprotocoltype.h" /*! \brief Interface class for protocol manager -\details Describes the interfaces of a manager of different protocols \sa ProtocolHandler + \details Describes the interfaces of a manager of different protocols \sa ProtocolHandler */ class IProtocolManager { @@ -98,7 +98,7 @@ class IProtocolManager \param msg: The message to check \returns true if the message passes the protocol managers sanity checks */ - virtual bool validateMessage(XsMessage const &msg) const = 0; + virtual bool validateMessage(XsMessage const& msg) const = 0; }; #endif diff --git a/lib/xspublic/xscontroller/lastresultmanager.h b/lib/xspublic/xscontroller/lastresultmanager.h index d466059..9f1c412 100644 --- a/lib/xspublic/xscontroller/lastresultmanager.h +++ b/lib/xspublic/xscontroller/lastresultmanager.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -72,7 +72,8 @@ \brief This class manages a result code with optional additional text \details It can be treated like a simple XsResultValue, but gives extra options */ -class LastResultManager { +class LastResultManager +{ XsResultValue m_lastResult; XsString m_lastResultText; public: @@ -90,7 +91,10 @@ class LastResultManager { /*! \returns The last result value */ - XsResultValue lastResult() const { return m_lastResult; } + XsResultValue lastResult() const + { + return m_lastResult; + } /*! \returns The last result text */ @@ -114,7 +118,10 @@ class LastResultManager { /*! \returns The last result value */ - operator XsResultValue() const { return m_lastResult; } + operator XsResultValue() const + { + return m_lastResult; + } }; #endif diff --git a/lib/xspublic/xscontroller/messageextractor.cpp b/lib/xspublic/xscontroller/messageextractor.cpp index bfecb6f..76fe677 100644 --- a/lib/xspublic/xscontroller/messageextractor.cpp +++ b/lib/xspublic/xscontroller/messageextractor.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -95,7 +95,7 @@ MessageExtractor::MessageExtractor(std::shared_ptr const& prot \param messages: Newly extracted messages are stored in this vector. This vector will be cleared upon function entry \returns XRV_OK if one or more messages were successfully extracted. Something else if not */ -XsResultValue MessageExtractor::processNewData(XsDevice* devicePtr, XsByteArray const& newData, std::deque &messages) +XsResultValue MessageExtractor::processNewData(XsDevice* devicePtr, XsByteArray const& newData, std::deque& messages) { if (!m_protocolManager) return XRV_ERROR; @@ -133,7 +133,7 @@ XsResultValue MessageExtractor::processNewData(XsDevice* devicePtr, XsByteArray if (location.isValid()) { - XsByteArray detectedMessage(&raw[(XsSize) (ptrdiff_t) location.m_startPos], (XsSize) (ptrdiff_t) location.m_size, XSDF_None); + XsByteArray detectedMessage(&raw[(XsSize)(ptrdiff_t) location.m_startPos], (XsSize)(ptrdiff_t) location.m_size, XSDF_None); if (devicePtr != nullptr) devicePtr->onMessageDetected2(type, detectedMessage); @@ -157,7 +157,7 @@ XsResultValue MessageExtractor::processNewData(XsDevice* devicePtr, XsByteArray if (location.m_incompletePos > 0) { JLALERTG("Skipping " << location.m_incompletePos << " bytes from the input buffer"); - popped += (XsSize) (ptrdiff_t) location.m_incompletePos; + popped += (XsSize)(ptrdiff_t) location.m_incompletePos; } return retval(); @@ -190,21 +190,19 @@ XsResultValue MessageExtractor::processNewData(XsDevice* devicePtr, XsByteArray } // message is valid, remove data from cache - popped += (XsSize) (ptrdiff_t) (location.m_size + location.m_startPos); + popped += (XsSize)(ptrdiff_t)(location.m_size + location.m_startPos); messages.push_back(message); } else { if (type == XPT_Nmea) - popped += (XsSize) (ptrdiff_t) (location.m_size + location.m_startPos); + popped += (XsSize)(ptrdiff_t)(location.m_size + location.m_startPos); else return retval(); } } else - { return retval(); - } } } diff --git a/lib/xspublic/xscontroller/messageextractor.h b/lib/xspublic/xscontroller/messageextractor.h index 53644af..aae76cc 100644 --- a/lib/xspublic/xscontroller/messageextractor.h +++ b/lib/xspublic/xscontroller/messageextractor.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -77,7 +77,7 @@ class MessageExtractor public: MessageExtractor(std::shared_ptr const& protocolManager); - XsResultValue processNewData(XsDevice* devicePtr, XsByteArray const& newData, std::deque &messages); + XsResultValue processNewData(XsDevice* devicePtr, XsByteArray const& newData, std::deque& messages); void clearBuffer(); int setMaxIncompleteRetryCount(int max); diff --git a/lib/xspublic/xscontroller/messagelocation.h b/lib/xspublic/xscontroller/messagelocation.h index ecafe0c..1afc8f7 100644 --- a/lib/xspublic/xscontroller/messagelocation.h +++ b/lib/xspublic/xscontroller/messagelocation.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -67,7 +67,8 @@ /*! \brief Stores the location of a message in a buffer using a start position and a size */ -class MessageLocation { +class MessageLocation +{ public: int m_startPos; //!< The offset of the first byte of the message or -1 if no message int m_size; //!< The size of the message, when less than 0 it indicates the expected message size diff --git a/lib/xspublic/xscontroller/messageserializer.cpp b/lib/xspublic/xscontroller/messageserializer.cpp index 1e6b93f..81f2a95 100644 --- a/lib/xspublic/xscontroller/messageserializer.cpp +++ b/lib/xspublic/xscontroller/messageserializer.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -68,6 +68,7 @@ #include #include #include +#include /*! \class MessageSerializer @@ -76,9 +77,9 @@ /*! \brief Default constructor */ -MessageSerializer::MessageSerializer(XsMessage &msg, XsSize offset) : - m_message(msg), - m_index(offset) +MessageSerializer::MessageSerializer(XsMessage& msg, XsSize offset) + : m_message(msg) + , m_index(offset) { } @@ -91,7 +92,7 @@ MessageSerializer::~MessageSerializer() { finalize(); } - catch(...) + catch (...) { } } @@ -100,7 +101,7 @@ MessageSerializer::~MessageSerializer() \param id The data identifier \returns A reference to this object */ -MessageSerializer &MessageSerializer::operator<<(XsDataIdentifier id) +MessageSerializer& MessageSerializer::operator<<(XsDataIdentifier id) { return operator<<((uint16_t)id); } @@ -109,34 +110,37 @@ MessageSerializer &MessageSerializer::operator<<(XsDataIdentifier id) \param id The CAN data identifier \returns A reference to this object */ -MessageSerializer &MessageSerializer::operator<<(XsCanDataIdentifier id) +MessageSerializer& MessageSerializer::operator<<(XsCanDataIdentifier id) { return operator<<((uint8_t)id); } /*! \brief Output stream operator that adds a XsCanIdLenght to the stream - \param idl The CAN Id lenght enum value + \param idl The CAN Id length enum value \returns A reference to this object */ -MessageSerializer &MessageSerializer::operator<<(XsCanFrameFormat idl) +MessageSerializer& MessageSerializer::operator<<(XsCanFrameFormat idl) { - return operator<<((uint8_t)((idl==XCFF_11Bit_Identifier)?0:1)); + return operator<<((uint8_t)((idl == XCFF_11Bit_Identifier) ? 0 : 1)); } /*! \brief Output stream operator that adds a XsDeviceId to the stream \param id The device ID \returns A reference to this object */ -MessageSerializer &MessageSerializer::operator<<(const XsDeviceId &id) +MessageSerializer& MessageSerializer::operator<<(const XsDeviceId& id) { - return operator<<(id.legacyDeviceId()); + operator<<(id.legacyDeviceId()); + if (!id.isLegacyDeviceId()) + operator<<((uint32_t)(id.toInt() >> 32)); + return *this; } /*! \brief Output stream operator that adds a uint8_t to the stream \param value The value to add to the stream \returns A reference to this object */ -MessageSerializer &MessageSerializer::operator<<(uint8_t value) +MessageSerializer& MessageSerializer::operator<<(uint8_t value) { m_message.setDataByte(value, m_index); m_index += sizeof(value); @@ -147,7 +151,7 @@ MessageSerializer &MessageSerializer::operator<<(uint8_t value) \param value The value to add to the stream \returns A reference to this object */ -MessageSerializer &MessageSerializer::operator<<(uint16_t value) +MessageSerializer& MessageSerializer::operator<<(uint16_t value) { m_message.setDataShort(value, m_index); m_index += sizeof(value); @@ -158,7 +162,7 @@ MessageSerializer &MessageSerializer::operator<<(uint16_t value) \param value The value to add to the stream \returns A reference to this object */ -MessageSerializer &MessageSerializer::operator<<(uint32_t value) +MessageSerializer& MessageSerializer::operator<<(uint32_t value) { m_message.setDataLong(value, m_index); m_index += sizeof(value); @@ -169,7 +173,7 @@ MessageSerializer &MessageSerializer::operator<<(uint32_t value) \param value The value to add to the stream \returns A reference to this object */ -MessageSerializer &MessageSerializer::operator<<(uint64_t value) +MessageSerializer& MessageSerializer::operator<<(uint64_t value) { m_message.setDataLongLong(value, m_index); m_index += sizeof(value); @@ -180,12 +184,12 @@ MessageSerializer &MessageSerializer::operator<<(uint64_t value) \param config The output configuration array \returns A reference to this object */ -MessageSerializer &MessageSerializer::operator<<(const XsOutputConfigurationArray &config) +MessageSerializer& MessageSerializer::operator<<(const XsOutputConfigurationArray& config) { if (config.size() == 0) return (*this << (XsOutputConfiguration(XDI_None, 0))); - for (auto &cfg : config) + for (auto& cfg : config) *this << cfg; return *this; } @@ -194,7 +198,7 @@ MessageSerializer &MessageSerializer::operator<<(const XsOutputConfigurationArra \param cfg The output configuration \returns A reference to this object */ -MessageSerializer &MessageSerializer::operator<<(const XsOutputConfiguration &cfg) +MessageSerializer& MessageSerializer::operator<<(const XsOutputConfiguration& cfg) { return (*this << cfg.m_dataIdentifier << cfg.m_frequency); } @@ -203,12 +207,12 @@ MessageSerializer &MessageSerializer::operator<<(const XsOutputConfiguration &cf \param config The output configuration array \returns A reference to this object */ -MessageSerializer &MessageSerializer::operator<<(const XsCanOutputConfigurationArray &config) +MessageSerializer& MessageSerializer::operator<<(const XsCanOutputConfigurationArray& config) { if (config.size() == 0) return (*this << (XsCanOutputConfiguration(XCFF_11Bit_Identifier, XCDI_Invalid, 0, 0))); - for (auto &cfg : config) + for (auto& cfg : config) *this << cfg; return *this; } @@ -217,7 +221,7 @@ MessageSerializer &MessageSerializer::operator<<(const XsCanOutputConfigurationA \param cfg The output configuration \returns A reference to this object */ -MessageSerializer &MessageSerializer::operator<<(const XsCanOutputConfiguration &cfg) +MessageSerializer& MessageSerializer::operator<<(const XsCanOutputConfiguration& cfg) { return (*this << cfg.m_dataIdentifier << cfg.m_frameFormat << cfg.m_id << cfg.m_frequency); } @@ -227,7 +231,7 @@ MessageSerializer &MessageSerializer::operator<<(const XsCanOutputConfiguration \param data The value to add to the message \param size The size of this value */ -void MessageSerializer::append(const uint8_t *data, XsSize size) +void MessageSerializer::append(const uint8_t* data, XsSize size) { m_message.setDataBuffer(data, size, m_index); m_index += size; @@ -247,11 +251,10 @@ void MessageSerializer::finalize() /*! \brief Default constructor */ -MessageDeserializer::MessageDeserializer(const XsMessage &msg, XsSize offset) : - m_message(msg), - m_index(offset) +MessageDeserializer::MessageDeserializer(const XsMessage& msg, XsSize offset) + : m_message(msg) + , m_index(offset) { - } /*! \brief Destructor @@ -264,7 +267,7 @@ MessageDeserializer::~MessageDeserializer() \param value Reference in which the data identifier is stored \returns A reference to this object */ -MessageDeserializer &MessageDeserializer::operator>>(XsDataIdentifier &value) +MessageDeserializer& MessageDeserializer::operator>>(XsDataIdentifier& value) { uint16_t v; operator>>(v); @@ -276,7 +279,7 @@ MessageDeserializer &MessageDeserializer::operator>>(XsDataIdentifier &value) \param value Reference in which the CAN data identifier is stored \returns A reference to this object */ -MessageDeserializer &MessageDeserializer::operator>>(XsCanDataIdentifier &value) +MessageDeserializer& MessageDeserializer::operator>>(XsCanDataIdentifier& value) { uint8_t v; operator>>(v); @@ -288,7 +291,7 @@ MessageDeserializer &MessageDeserializer::operator>>(XsCanDataIdentifier &value) \param value Reference in which the CAN ID length is stored \returns A reference to this object */ -MessageDeserializer &MessageDeserializer::operator>>(XsCanFrameFormat &value) +MessageDeserializer& MessageDeserializer::operator>>(XsCanFrameFormat& value) { uint8_t v; operator>>(v); @@ -300,11 +303,19 @@ MessageDeserializer &MessageDeserializer::operator>>(XsCanFrameFormat &value) \param value Reference in which the device ID is stored \returns A reference to this object */ -MessageDeserializer &MessageDeserializer::operator>>(XsDeviceId &value) +MessageDeserializer& MessageDeserializer::operator>>(XsDeviceId& value) { uint32_t v; operator>>(v); - value = XsDeviceId(v); + if (v & XS_DID64_BIT) + { + uint32_t u; + operator>>(u); + uint64_t w = (((uint64_t) u) << 32) | (uint64_t) v; + value = XsDeviceId(w); + } + else + value = XsDeviceId(v); return *this; } @@ -312,7 +323,7 @@ MessageDeserializer &MessageDeserializer::operator>>(XsDeviceId &value) \param value Reference in which the read value is stored \returns A reference to this object */ -MessageDeserializer &MessageDeserializer::operator>>(uint8_t &value) +MessageDeserializer& MessageDeserializer::operator>>(uint8_t& value) { value = m_message.getDataByte(m_index); m_index += sizeof(value); @@ -323,7 +334,7 @@ MessageDeserializer &MessageDeserializer::operator>>(uint8_t &value) \param value Reference in which the read value is stored \returns A reference to this object */ -MessageDeserializer &MessageDeserializer::operator>>(int8_t &value) +MessageDeserializer& MessageDeserializer::operator>>(int8_t& value) { value = (int8_t)m_message.getDataByte(m_index); m_index += sizeof(value); @@ -334,7 +345,7 @@ MessageDeserializer &MessageDeserializer::operator>>(int8_t &value) \param value Reference in which the read value is stored \returns A reference to this object */ -MessageDeserializer &MessageDeserializer::operator>>(uint16_t &value) +MessageDeserializer& MessageDeserializer::operator>>(uint16_t& value) { value = m_message.getDataShort(m_index); m_index += sizeof(value); @@ -345,7 +356,7 @@ MessageDeserializer &MessageDeserializer::operator>>(uint16_t &value) \param value Reference in which the read value is stored \returns A reference to this object */ -MessageDeserializer &MessageDeserializer::operator>>(uint32_t &value) +MessageDeserializer& MessageDeserializer::operator>>(uint32_t& value) { value = m_message.getDataLong(m_index); m_index += sizeof(value); @@ -356,7 +367,7 @@ MessageDeserializer &MessageDeserializer::operator>>(uint32_t &value) \param value Reference in which the read value is stored \returns A reference to this object */ -MessageDeserializer &MessageDeserializer::operator>>(uint64_t &value) +MessageDeserializer& MessageDeserializer::operator>>(uint64_t& value) { value = m_message.getDataLongLong(m_index); m_index += sizeof(value); @@ -367,7 +378,7 @@ MessageDeserializer &MessageDeserializer::operator>>(uint64_t &value) \param config Reference in which the output configuration array is stored \returns A reference to this object */ -MessageDeserializer &MessageDeserializer::operator>>(XsOutputConfigurationArray &config) +MessageDeserializer& MessageDeserializer::operator>>(XsOutputConfigurationArray& config) { config.clear(); while (!atEnd()) @@ -383,7 +394,7 @@ MessageDeserializer &MessageDeserializer::operator>>(XsOutputConfigurationArray \param cfg Reference in which the output configuration is stored \returns A reference to this object */ -MessageDeserializer &MessageDeserializer::operator>>(XsOutputConfiguration &cfg) +MessageDeserializer& MessageDeserializer::operator>>(XsOutputConfiguration& cfg) { return (*this >> cfg.m_dataIdentifier >> cfg.m_frequency); } @@ -392,7 +403,7 @@ MessageDeserializer &MessageDeserializer::operator>>(XsOutputConfiguration &cfg) \param config Reference in which the CAN output configuration array is stored \returns A reference to this object */ -MessageDeserializer &MessageDeserializer::operator>>(XsCanOutputConfigurationArray &config) +MessageDeserializer& MessageDeserializer::operator>>(XsCanOutputConfigurationArray& config) { config.clear(); while (!atEnd()) @@ -408,7 +419,7 @@ MessageDeserializer &MessageDeserializer::operator>>(XsCanOutputConfigurationArr \param cfg Reference in which the CAN output configuration is stored \returns A reference to this object */ -MessageDeserializer &MessageDeserializer::operator>>(XsCanOutputConfiguration &cfg) +MessageDeserializer& MessageDeserializer::operator>>(XsCanOutputConfiguration& cfg) { return (*this >> cfg.m_dataIdentifier >> cfg.m_frameFormat >> cfg.m_id >> cfg.m_frequency); } diff --git a/lib/xspublic/xscontroller/messageserializer.h b/lib/xspublic/xscontroller/messageserializer.h index 44dec56..8757733 100644 --- a/lib/xspublic/xscontroller/messageserializer.h +++ b/lib/xspublic/xscontroller/messageserializer.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -79,23 +79,23 @@ struct XsCanOutputConfiguration; class MessageSerializer { public: - MessageSerializer(XsMessage &message, XsSize offset = 0); + MessageSerializer(XsMessage& message, XsSize offset = 0); virtual ~MessageSerializer(); - MessageSerializer &operator<<(XsDataIdentifier value); - MessageSerializer &operator<<(XsCanDataIdentifier value); - MessageSerializer &operator<<(XsCanFrameFormat value); - MessageSerializer &operator<<(const XsDeviceId &id); - MessageSerializer &operator<<(uint8_t value); - MessageSerializer &operator<<(uint16_t value); - MessageSerializer &operator<<(uint32_t value); - MessageSerializer &operator<<(uint64_t value); + MessageSerializer& operator<<(XsDataIdentifier value); + MessageSerializer& operator<<(XsCanDataIdentifier value); + MessageSerializer& operator<<(XsCanFrameFormat value); + MessageSerializer& operator<<(const XsDeviceId& id); + MessageSerializer& operator<<(uint8_t value); + MessageSerializer& operator<<(uint16_t value); + MessageSerializer& operator<<(uint32_t value); + MessageSerializer& operator<<(uint64_t value); /*! \brief Output stream operator that adds a int64_t to the stream \param value The value to add to the stream \returns A reference to this object */ - inline MessageSerializer &operator<<(int64_t value) + inline MessageSerializer& operator<<(int64_t value) { return (operator<<((uint64_t)value)); } @@ -104,7 +104,7 @@ class MessageSerializer \param value The value to add to the stream \returns A reference to this object */ - inline MessageSerializer &operator<<(int32_t value) + inline MessageSerializer& operator<<(int32_t value) { return (operator<<((uint32_t)value)); } @@ -113,7 +113,7 @@ class MessageSerializer \param value The value to add to the stream \returns A reference to this object */ - inline MessageSerializer &operator<<(int16_t value) + inline MessageSerializer& operator<<(int16_t value) { return (operator<<((uint16_t)value)); } @@ -122,48 +122,51 @@ class MessageSerializer \param value The value to add to the stream \returns A reference to this object */ - inline MessageSerializer &operator<<(int8_t value) + inline MessageSerializer& operator<<(int8_t value) { return (operator<<((uint8_t)value)); } - MessageSerializer &operator<<(const XsOutputConfigurationArray &config); - MessageSerializer &operator<<(const XsOutputConfiguration &cfg); + MessageSerializer& operator<<(const XsOutputConfigurationArray& config); + MessageSerializer& operator<<(const XsOutputConfiguration& cfg); - MessageSerializer &operator<<(const XsCanOutputConfigurationArray &config); - MessageSerializer &operator<<(const XsCanOutputConfiguration &cfg); + MessageSerializer& operator<<(const XsCanOutputConfigurationArray& config); + MessageSerializer& operator<<(const XsCanOutputConfiguration& cfg); /*! \returns The current index */ - inline XsSize index() const { return m_index; } + inline XsSize index() const + { + return m_index; + } - void append(const uint8_t *data, XsSize size); + void append(const uint8_t* data, XsSize size); void finalize(); private: - XsMessage &m_message; + XsMessage& m_message; XsSize m_index; }; class MessageDeserializer { public: - MessageDeserializer(const XsMessage &message, XsSize offset = 0); + MessageDeserializer(const XsMessage& message, XsSize offset = 0); virtual ~MessageDeserializer(); - MessageDeserializer &operator>>(XsDataIdentifier &value); - MessageDeserializer &operator>>(XsCanDataIdentifier &value); - MessageDeserializer &operator>>(XsCanFrameFormat &value); - MessageDeserializer &operator>>(XsDeviceId &id); - MessageDeserializer &operator>>(uint8_t &value); - MessageDeserializer &operator>>(int8_t &value); - MessageDeserializer &operator>>(uint16_t &value); - MessageDeserializer &operator>>(uint32_t &value); - MessageDeserializer &operator>>(uint64_t &value); + MessageDeserializer& operator>>(XsDataIdentifier& value); + MessageDeserializer& operator>>(XsCanDataIdentifier& value); + MessageDeserializer& operator>>(XsCanFrameFormat& value); + MessageDeserializer& operator>>(XsDeviceId& id); + MessageDeserializer& operator>>(uint8_t& value); + MessageDeserializer& operator>>(int8_t& value); + MessageDeserializer& operator>>(uint16_t& value); + MessageDeserializer& operator>>(uint32_t& value); + MessageDeserializer& operator>>(uint64_t& value); /*! \brief Input stream operator that takes a int64_t from the stream \param value Reference in which the read value is stored \returns A reference to this object */ - inline MessageDeserializer &operator>>(int64_t &value) + inline MessageDeserializer& operator>>(int64_t& value) { return (operator>>((uint64_t&)value)); } @@ -172,7 +175,7 @@ class MessageDeserializer \param value Reference in which the read value is stored \returns A reference to this object */ - inline MessageDeserializer &operator>>(int32_t &value) + inline MessageDeserializer& operator>>(int32_t& value) { return (operator>>((uint32_t&)value)); } @@ -181,27 +184,33 @@ class MessageDeserializer \param value Reference in which the read value is stored \returns A reference to this object */ - inline MessageDeserializer &operator>>(int16_t &value) + inline MessageDeserializer& operator>>(int16_t& value) { return (operator>>((uint16_t&)value)); } bool atEnd() const; - MessageDeserializer &operator>>(XsOutputConfigurationArray &config); - MessageDeserializer &operator>>(XsOutputConfiguration &cfg); + MessageDeserializer& operator>>(XsOutputConfigurationArray& config); + MessageDeserializer& operator>>(XsOutputConfiguration& cfg); - MessageDeserializer &operator>>(XsCanOutputConfigurationArray &config); - MessageDeserializer &operator>>(XsCanOutputConfiguration &cfg); + MessageDeserializer& operator>>(XsCanOutputConfigurationArray& config); + MessageDeserializer& operator>>(XsCanOutputConfiguration& cfg); //! \returns The current message - const XsMessage& message() const { return m_message; } + const XsMessage& message() const + { + return m_message; + } //! \returns The current index - inline XsSize index() const { return m_index; } + inline XsSize index() const + { + return m_index; + } private: - const XsMessage &m_message; + const XsMessage& m_message; XsSize m_index; }; diff --git a/lib/xspublic/xscontroller/mtbdatalogger.cpp b/lib/xspublic/xscontroller/mtbdatalogger.cpp index 444bd52..f2fa912 100644 --- a/lib/xspublic/xscontroller/mtbdatalogger.cpp +++ b/lib/xspublic/xscontroller/mtbdatalogger.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -66,7 +66,6 @@ #include "iointerfacefile.h" #include "protocolhandler.h" - /*! \class MtbDataLogger \brief A class for logging the mtb data */ @@ -84,7 +83,7 @@ MtbDataLogger::~MtbDataLogger() { close(false); } - catch(...) + catch (...) { } } @@ -96,7 +95,7 @@ MtbDataLogger::~MtbDataLogger() \returns True if successful \see close */ -bool MtbDataLogger::create(const XsString &filename) +bool MtbDataLogger::create(const XsString& filename) { if (m_ioInterfaceFile) { @@ -120,7 +119,7 @@ bool MtbDataLogger::create(const XsString &filename) m_lastResult = m_ioInterfaceFile->writeData(test, nullptr); if (m_lastResult == XRV_OK) - m_lastResult = m_ioInterfaceFile->deleteData(0,5); + m_lastResult = m_ioInterfaceFile->deleteData(0, 5); if (m_lastResult != XRV_OK) { m_ioInterfaceFile->close(); @@ -154,7 +153,7 @@ void MtbDataLogger::close(bool deleteFile) /*! \brief Overloadable function to allow easier testing */ -bool MtbDataLogger::writeMessage(const XsMessage &message) +bool MtbDataLogger::writeMessage(const XsMessage& message) { if (!m_ioInterfaceFile) { @@ -171,6 +170,19 @@ bool MtbDataLogger::writeMessage(const XsMessage &message) return m_lastResult == XRV_OK; } +/*! \brief Write precomposed raw data to the file stream */ +bool MtbDataLogger::writeRaw(const XsByteArray& raw) +{ + if (!m_ioInterfaceFile) + { + m_lastResult = XRV_NOFILEOPEN; + return false; + } + + m_lastResult = m_ioInterfaceFile->writeData(raw, nullptr); + return m_lastResult == XRV_OK; +} + /*! \returns the filename of the file that we're logging to (if any) */ XsString MtbDataLogger::filename() const diff --git a/lib/xspublic/xscontroller/mtbdatalogger.h b/lib/xspublic/xscontroller/mtbdatalogger.h index a9ffbfb..f590250 100644 --- a/lib/xspublic/xscontroller/mtbdatalogger.h +++ b/lib/xspublic/xscontroller/mtbdatalogger.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -74,15 +74,16 @@ class MtbDataLogger : public DataLogger { public: MtbDataLogger(); - virtual ~MtbDataLogger(); + ~MtbDataLogger() override; - virtual bool writeMessage(const XsMessage &message); + bool writeMessage(const XsMessage& message) override; + bool writeRaw(const XsByteArray& message); - bool create(const XsString &filename); - virtual void close(); + bool create(const XsString& filename); + void close() override; void close(bool deleteFile); XsString filename() const; -protected: + private: XsResultValue m_lastResult; std::shared_ptr m_ioInterfaceFile; diff --git a/lib/xspublic/xscontroller/mtbfilecommunicator.cpp b/lib/xspublic/xscontroller/mtbfilecommunicator.cpp index 0688ed3..76cee4a 100644 --- a/lib/xspublic/xscontroller/mtbfilecommunicator.cpp +++ b/lib/xspublic/xscontroller/mtbfilecommunicator.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -88,7 +88,7 @@ FileLoader::~FileLoader() /*! \brief Constructs new MtbFileCommunicator */ -Communicator *MtbFileCommunicator::construct() +Communicator* MtbFileCommunicator::construct() { return new MtbFileCommunicator; } @@ -143,13 +143,13 @@ uint32_t MtbFileCommunicator::timeoutToMaxMessages(uint32_t timeout) const \param timeout The timeout in ms \details This one is nowhere near finished, but it does the trick for simple systems if a reply is available. \returns True if successful - */ -bool MtbFileCommunicator::doTransaction(const XsMessage &msg, XsMessage &rcv, uint32_t timeout) +*/ +bool MtbFileCommunicator::doTransaction(const XsMessage& msg, XsMessage& rcv, uint32_t timeout) { XsXbusMessageId expected = static_cast(msg.getMessageId() + 1); std::deque messages = readMessagesFromStartOfFile(expected, (int) timeoutToMaxMessages(timeout)); rcv.clear(); - for (const XsMessage &r : messages) + for (const XsMessage& r : messages) { if (r.getBusId() != msg.getBusId()) continue; @@ -192,7 +192,7 @@ void MtbFileCommunicator::waitForLastTaskCompletion() completeAllThreadedWork(); } -/* \brief Closes the log file +/* \brief Closes the log file */ void MtbFileCommunicator::closeLogFile() { @@ -344,6 +344,9 @@ class Xs4FileTask : public ThreadPoolTask */ bool exec() override { + // short sleep to prevent thread starvation + XsTime::msleep(1); + if (m_thread.m_done) return true; @@ -352,7 +355,9 @@ class Xs4FileTask : public ThreadPoolTask JLDEBUGG("Starting dedicated read thread"); m_thread.startThread(); } - return false; // reschedule + + // reschedule + return false; } /*! \brief Destroy this process task. */ @@ -444,7 +449,7 @@ XsResultValue MtbFileCommunicator::readLogFile(XsDevice* device) { res = readSinglePacketFromFile(); } - catch(...) + catch (...) { // Simply ignore the error and continue. continue; @@ -503,7 +508,7 @@ void MtbFileCommunicator::abortLoadLogFile() \see closeLogFile \returns True if successful */ -bool MtbFileCommunicator::openLogFile(const XsString &filename) +bool MtbFileCommunicator::openLogFile(const XsString& filename) { if (m_ioInterfaceFile) { @@ -512,7 +517,10 @@ bool MtbFileCommunicator::openLogFile(const XsString &filename) return true; return false; } - m_ioInterfaceFile = std::shared_ptr(new IoInterfaceFile, [](IoInterfaceFile *f) { f->close(); }); + m_ioInterfaceFile = std::shared_ptr(new IoInterfaceFile, [](IoInterfaceFile * f) + { + f->close(); + }); setLastResult(m_ioInterfaceFile->open(filename, false, true)); if (lastResult() != XRV_OK) @@ -618,7 +626,7 @@ XsFilePos MtbFileCommunicator::logFileReadPosition() const if (m_extractedMessages->empty()) return pos; - return pos > 0 ? pos-1 : 0; + return pos > 0 ? pos - 1 : 0; } /*! \brief Restart reading from the start of the open log file. @@ -664,8 +672,7 @@ XsMessage MtbFileCommunicator::readMessage(uint8_t msgId) do { msg = readNextMessage(); - } - while (!msg.empty() && msgId != 0 && msg.getMessageId() != msgId); + } while (!msg.empty() && msgId != 0 && msg.getMessageId() != msgId); if (msgId == 0 || msg.getMessageId() == msgId) return msg; @@ -675,14 +682,15 @@ XsMessage MtbFileCommunicator::readMessage(uint8_t msgId) } /*! \brief Read the next message from the open file. - * \returns The message that was read or an empty message if no message was found (end-of-file for example). - */ + \returns The message that was read or an empty message if no message was found (end-of-file for example). +*/ XsMessage MtbFileCommunicator::readNextMessage() { while (m_extractedMessages->empty()) { XsByteArray raw; - XsResultValue res = m_ioInterfaceFile->readDataBlocks(1, raw); (void)res; + XsResultValue res = m_ioInterfaceFile->readDataBlocks(1, raw); + (void)res; if (raw.empty()) { // end of file really reached @@ -702,13 +710,13 @@ XsMessage MtbFileCommunicator::readNextMessage() */ bool MtbFileCommunicator::isLoadLogFileInProgress() const { - return ThreadPool::instance()->doesTaskExist(m_loadFileTaskId); + return ThreadPool::instance()->doesTaskExist(m_loadFileTaskId); } /*! \brief Add the protocol handler -\param handler : The protocol hanlder to add + \param handler : The protocol hanlder to add */ -void MtbFileCommunicator::addProtocolHandler(IProtocolHandler *handler) +void MtbFileCommunicator::addProtocolHandler(IProtocolHandler* handler) { handler->ignoreMaximumMessageSize(true); Communicator::addProtocolHandler(handler); @@ -738,7 +746,7 @@ void MtbFileCommunicator::setGotoConfigTimeout(uint32_t timeout) (void)timeout; } -bool MtbFileCommunicator::writeMessage(const XsMessage &message) +bool MtbFileCommunicator::writeMessage(const XsMessage& message) { (void)message; return false; @@ -762,7 +770,7 @@ XsPortInfo MtbFileCommunicator::portInfo() const return XsPortInfo(); } -bool MtbFileCommunicator::openPort(const XsPortInfo &portInfo, OpenPortStage stage, bool detectRs485) +bool MtbFileCommunicator::openPort(const XsPortInfo& portInfo, OpenPortStage stage, bool detectRs485) { (void)portInfo; (void)stage; @@ -777,7 +785,7 @@ bool MtbFileCommunicator::reopenPort(OpenPortStage stage, bool skipDeviceIdCheck return false; } -bool MtbFileCommunicator::isDockedAt(Communicator *other) const +bool MtbFileCommunicator::isDockedAt(Communicator* other) const { (void)other; return false; diff --git a/lib/xspublic/xscontroller/mtbfilecommunicator.h b/lib/xspublic/xscontroller/mtbfilecommunicator.h index 425cde6..4da854a 100644 --- a/lib/xspublic/xscontroller/mtbfilecommunicator.h +++ b/lib/xspublic/xscontroller/mtbfilecommunicator.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -79,7 +79,7 @@ class MessageExtractor; class MtbFileCommunicator : public Communicator, protected FileLoader { public: - static Communicator *construct(); + static Communicator* construct(); MtbFileCommunicator(); void closeLogFile() override; @@ -88,7 +88,7 @@ class MtbFileCommunicator : public Communicator, protected FileLoader std::deque readMessagesFromStartOfFile(uint8_t msgId, int maxMsgs = 0) override; void loadLogFile(XsDevice* device) override; void abortLoadLogFile() override; - bool openLogFile(const XsString &filename) override; + bool openLogFile(const XsString& filename) override; XsString logFileName() const override; XsFilePos logFileSize() const override; XsTimeStamp logFileDate() const override; @@ -97,22 +97,22 @@ class MtbFileCommunicator : public Communicator, protected FileLoader bool isReadingFromFile() const override; bool isLoadLogFileInProgress() const override; - bool doTransaction(const XsMessage &msg, XsMessage &rcv, uint32_t timeout) override; + bool doTransaction(const XsMessage& msg, XsMessage& rcv, uint32_t timeout) override; XsResultValue gotoConfig(bool detectRs485 = false) override; XsResultValue gotoMeasurement() override; XsResultValue getDeviceId() override; void setGotoConfigTimeout(uint32_t timeout) override; - bool writeMessage(const XsMessage &message) override; + bool writeMessage(const XsMessage& message) override; void flushPort() override; void closePort() override; bool isPortOpen() const override; XsPortInfo portInfo() const override; - bool openPort(const XsPortInfo &portInfo, OpenPortStage stage = OPS_Full, bool detectRs485 = false) override; + bool openPort(const XsPortInfo& portInfo, OpenPortStage stage = OPS_Full, bool detectRs485 = false) override; bool reopenPort(OpenPortStage stage = OPS_Full, bool skipDeviceIdCheck = false) override; - bool isDockedAt(Communicator *other) const override; + bool isDockedAt(Communicator* other) const override; void setKeepAlive(bool enable) override; - void addProtocolHandler(IProtocolHandler *handler) override; + void addProtocolHandler(IProtocolHandler* handler) override; protected: MtbFileCommunicator(std::shared_ptr const& ioInterfaceFile); diff --git a/lib/xspublic/xscontroller/mtdevice.cpp b/lib/xspublic/xscontroller/mtdevice.cpp index fa1adc9..cba549c 100644 --- a/lib/xspublic/xscontroller/mtdevice.cpp +++ b/lib/xspublic/xscontroller/mtdevice.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -69,9 +69,22 @@ #include #include "xsselftestresult.h" #include +#include + +// Undef the windows min macro that conflict with e.g. std::numeric_limits<..>::min +#ifdef min + #undef min +#endif using namespace xsens; +/*! \brief Constructs a standalone MtDevice with device Id \a id +*/ +MtDevice::MtDevice(XsDeviceId const& id) + : XsDeviceEx(id) +{ +} + /*! \brief Constructs a standalone MtDevice based on \a comm */ MtDevice::MtDevice(Communicator* comm) @@ -81,7 +94,7 @@ MtDevice::MtDevice(Communicator* comm) /*! \brief Constructs a standalone MtDevice based on \a master and \a childDeviceId */ -MtDevice::MtDevice(XsDevice * master, const XsDeviceId &childDeviceId) +MtDevice::MtDevice(XsDevice* master, const XsDeviceId& childDeviceId) : XsDeviceEx(master, childDeviceId) { } @@ -90,15 +103,13 @@ MtDevice::MtDevice(XsDevice * master, const XsDeviceId &childDeviceId) */ MtDevice::~MtDevice() { - JLTRACEG("entry"); - XSEXITLOGN(gJournal); } /*! \brief Checks for the sanity of a message \param msg A message to check \returns True if successful */ -bool MtDevice::messageLooksSane(const XsMessage &msg) const +bool MtDevice::messageLooksSane(const XsMessage& msg) const { return msg.getBusId() == 1 || XsDevice::messageLooksSane(msg); } @@ -133,12 +144,12 @@ void MtDevice::updateFilterProfiles() if (info.m_filterProfile != 0) { m_hardwareFilterProfile = XsFilterProfile(info.m_filterProfile & 0xFF - , info.m_filterProfile >> 8 - , m_hardwareFilterProfile.kind() - , m_hardwareFilterProfile.label() - , info.m_filterType - , info.m_filterMajor - , info.m_filterMinor); + , info.m_filterProfile >> 8 + , m_hardwareFilterProfile.kind() + , m_hardwareFilterProfile.label() + , info.m_filterType + , info.m_filterMajor + , info.m_filterMinor); } for (auto i = m_hardwareFilterProfiles.begin(); i != m_hardwareFilterProfiles.end(); ++i) @@ -234,16 +245,16 @@ bool MtDevice::canDoOrientationResetInFirmware(XsResetMethod method) { switch (method) { - case XRM_DefaultAlignment: - case XRM_DefaultHeading: - case XRM_DefaultInclination: - return true; + case XRM_DefaultAlignment: + case XRM_DefaultHeading: + case XRM_DefaultInclination: + return true; - case XRM_None: - return false; + case XRM_None: + return false; - default: - break; + default: + break; } return updateRateForDataIdentifier(XDI_OrientationGroup) > 0; @@ -254,36 +265,36 @@ bool MtDevice::scheduleOrientationReset(XsResetMethod method) { switch (deviceState()) { - case XDS_Measurement: - case XDS_Recording: - case XDS_WaitingForRecordingStart: - case XDS_FlushingData: - if (method == XRM_StoreAlignmentMatrix) - return false; - - if (canDoOrientationResetInFirmware(method)) - if (!XsDevice::scheduleOrientationReset(method)) + case XDS_Measurement: + case XDS_Recording: + case XDS_WaitingForRecordingStart: + case XDS_FlushingData: + if (method == XRM_StoreAlignmentMatrix) return false; - break; + if (canDoOrientationResetInFirmware(method)) + if (!XsDevice::scheduleOrientationReset(method)) + return false; - case XDS_Config: - if (method != XRM_StoreAlignmentMatrix) - return false; + break; - if (canDoOrientationResetInFirmware(method)) - { - if (!storeAlignmentMatrix()) + case XDS_Config: + if (method != XRM_StoreAlignmentMatrix) return false; - // read stored value from emts by reinitializing - return reinitialize(); - } - return true; - case XDS_Initial: - case XDS_Destructing: - default: - return false; + if (canDoOrientationResetInFirmware(method)) + { + if (!storeAlignmentMatrix()) + return false; + // read stored value from emts by reinitializing + return reinitialize(); + } + return true; + + case XDS_Initial: + case XDS_Destructing: + default: + return false; } return true; } @@ -393,10 +404,10 @@ XsFilterProfileArray MtDevice::readFilterProfilesFromDevice() const result.resize(nofScenarios); for (XsSize i = 0; i < nofScenarios; ++i) { - uint8_t type = rcv.getDataByte(i*(1+1+XS_LEN_FILTERPROFILELABEL)); + uint8_t type = rcv.getDataByte(i * (1 + 1 + XS_LEN_FILTERPROFILELABEL)); result[i].setType(type); - result[i].setVersion(rcv.getDataByte(1 + i*(1+1+XS_LEN_FILTERPROFILELABEL))); - result[i].setLabel((const char*) rcv.getDataBuffer(2 + i*(1+1+XS_LEN_FILTERPROFILELABEL))); + result[i].setVersion(rcv.getDataByte(1 + i * (1 + 1 + XS_LEN_FILTERPROFILELABEL))); + result[i].setLabel((const char*) rcv.getDataBuffer(2 + i * (1 + 1 + XS_LEN_FILTERPROFILELABEL))); result[i].setFilterType(filterType); XsString kind; if (type == XFPK_Base) @@ -417,14 +428,14 @@ void MtDevice::fetchAvailableHardwareScenarios() m_hardwareFilterProfiles.clear(); m_hardwareFilterProfiles = readFilterProfilesFromDevice(); std::sort(m_hardwareFilterProfiles.begin(), m_hardwareFilterProfiles.end(), - [](XsFilterProfile const& left, XsFilterProfile const& right) - { - if (left.type() == right.type()) - return strcmp(left.label(), right.label()) < 0; - else - return left.type() < right.type(); - } - ); + [](XsFilterProfile const & left, XsFilterProfile const & right) + { + if (left.type() == right.type()) + return strcmp(left.label(), right.label()) < 0; + else + return left.type() < right.type(); + } + ); } /*! \copybrief XsDevice::productCode @@ -437,7 +448,7 @@ XsString MtDevice::productCode() const const char* pc = (const char*) rcv.getDataBuffer(); assert(pc); - std::string result(pc?pc:" ", 20); + std::string result(pc ? pc : " ", 20); std::string::difference_type thingy = (std::string::difference_type) result.find(" "); if (thingy < 20) result.erase(result.begin() + thingy, result.end()); @@ -470,7 +481,64 @@ bool MtDevice::restoreFactoryDefaults() */ XsFilterProfile MtDevice::onboardFilterProfile() const { - return m_hardwareFilterProfile; + XsMessage snd(XMID_ReqFilterProfile); + snd.setBusId(busId()); + XsMessage rcv; + + //tries to get the current filter profile from the device + if (!doTransaction(snd, rcv)) + return m_hardwareFilterProfile;//in case the transaction with the device fails or if the dev is in measurement + + auto profilesFromDevice = readFilterProfilesFromDevice(); + std::string fullMessageData((const char*)rcv.getDataBuffer()); + if (fullMessageData.empty())//for older devices where the profiles are numbers + { + auto numericFilter = rcv.getDataShort(); + //looks-up the full profile data among all the profiles available for the device + for (auto currentProfile : profilesFromDevice) + { + if (currentProfile.type() == numericFilter) + return currentProfile; + } + } + else//for newer devices where the profiles are strings + { + //removes the checksum at the end of the message and keeps only the name of the profile + // Stop when we see the first space or at message size, whichever is the lowest. + fullMessageData = fullMessageData.substr(0, std::min(rcv.getDataSize(), fullMessageData.find(' '))); + + //looks up the full profile structure from the ones available on the device, by using the label + for (auto currentProfile : profilesFromDevice) + { + if (currentProfile.label() == fullMessageData) + return currentProfile; + } + + //some strings have the names of two profiles separated by a '/', so the above look-up will not work + //thus two separate profiles need to be found and "combined" + auto splitterPosition = fullMessageData.find('/'); + if (splitterPosition != std::string::npos) + { + std::string combinedName; + auto firstProfileName = fullMessageData.substr(0, splitterPosition); + auto secondProfileName = fullMessageData.substr(splitterPosition + 1, fullMessageData.size());//the '/' is not taken into consideration + XsFilterProfile firstProfile, secondProfile, combinedProfile; + for (auto currentProfile : profilesFromDevice) + { + if (currentProfile.label() == firstProfileName) + firstProfile = currentProfile; + else if (currentProfile.label() == secondProfileName) + secondProfile = currentProfile; + } + //sets all the data from the first profile + combinedProfile = firstProfile; + //and changes the name to a combination from the first and second profile + combinedName.append(firstProfile.label()).append("/").append(secondProfile.label()); + combinedProfile.setLabel(combinedName.c_str()); + return combinedProfile; + } + } + return m_hardwareFilterProfile;//in case everything above fails } /*! \copybrief XsDevice::setOnboardFilterProfile @@ -481,7 +549,7 @@ bool MtDevice::setOnboardFilterProfile(int profileType) return false; XsFilterProfileArray::iterator item = std::find_if(m_hardwareFilterProfiles.begin(), m_hardwareFilterProfiles.end(), - [profileType](XsFilterProfile const& p) + [profileType](XsFilterProfile const & p) { return p.type() == profileType; }); @@ -513,10 +581,10 @@ bool MtDevice::setOnboardFilterProfile(XsString const& profile) for (auto currentProfile : profileList) { item[i++] = std::find_if(m_hardwareFilterProfiles.begin(), m_hardwareFilterProfiles.end(), - [currentProfile](XsFilterProfile const& p) - { - return currentProfile == p.label(); - }); + [currentProfile](XsFilterProfile const & p) + { + return currentProfile == p.label(); + }); if (i == 2) break; } @@ -616,14 +684,14 @@ XsVector MtDevice::initialPositionLLA() const */ uint32_t MtDevice::syncTicksToUs(uint32_t ticks) const { - return ((uint32_t) (((double) ticks) * XS_SYNC_CLOCK_TICKS_TO_US + 0.5)); + return ((uint32_t)(((double) ticks) * XS_SYNC_CLOCK_TICKS_TO_US + 0.5)); } /*! \brief Convert microseconds to mt sync ticks */ uint32_t MtDevice::usToSyncTicks(uint32_t us) const { - return ((uint32_t) (((double) us) * XS_SYNC_CLOCK_US_TO_TICKS + 0.5)); + return ((uint32_t)(((double) us) * XS_SYNC_CLOCK_US_TO_TICKS + 0.5)); } /*! \returns the error mode of the device. @@ -776,7 +844,7 @@ XsString MtDevice::stripProductCode(const XsString& code) if (hwtype.empty()) return code; - int offset = code.findSubStr(hwtype); + ptrdiff_t offset = code.findSubStr(hwtype); while (offset >= 0 && code[(unsigned int)offset] != '-') --offset; @@ -785,3 +853,11 @@ XsString MtDevice::stripProductCode(const XsString& code) return code.mid(0, (unsigned int)offset); } + +/*! \brief Returns the base update rate (Hz) corresponding to the \a dataType. Returns 0 if no update rate is available +*/ +int MtDevice::getBaseFrequency(XsDataIdentifier dataType) const +{ + (void) dataType; + return 0; +} diff --git a/lib/xspublic/xscontroller/mtdevice.h b/lib/xspublic/xscontroller/mtdevice.h index 01864bd..3a4d2bd 100644 --- a/lib/xspublic/xscontroller/mtdevice.h +++ b/lib/xspublic/xscontroller/mtdevice.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -71,8 +71,9 @@ struct XsFilterProfile; -namespace xsens { - class Emts5Public; +namespace xsens +{ +class Emts5Public; } /*! \class MtDevice @@ -82,13 +83,13 @@ namespace xsens { class MtDevice : public XsDeviceEx { public: - MtDevice(); virtual ~MtDevice(); bool initialize() override; bool isMotionTracker() const override; int updateRateForDataIdentifier(XsDataIdentifier dataType) const override; + virtual int getBaseFrequency(XsDataIdentifier dataType = XDI_None) const; uint16_t stringOutputType() const override; uint16_t stringSamplePeriod() const override; @@ -150,19 +151,20 @@ class MtDevice : public XsDeviceEx static int calcFrequency(int baseFrequency, uint16_t skipFactor); - bool messageLooksSane(const XsMessage &msg) const; + bool messageLooksSane(const XsMessage& msg) const; uint32_t supportedStatusFlags() const override; protected: + explicit MtDevice(XsDeviceId const& id); explicit MtDevice(Communicator* comm); - explicit MtDevice(XsDevice*, const XsDeviceId &); + explicit MtDevice(XsDevice*, const XsDeviceId&); virtual void updateFilterProfiles(); XsFilterProfileArray readFilterProfilesFromDevice() const; virtual void fetchAvailableHardwareScenarios(); - static XsString stripProductCode(const XsString &code); + static XsString stripProductCode(const XsString& code); uint32_t syncTicksToUs(uint32_t ticks) const; uint32_t usToSyncTicks(uint32_t us) const; @@ -187,7 +189,7 @@ struct MtDeviceEx : public MtDevice explicit MtDeviceEx(Communicator* comm) : MtDevice(comm) {} //! Construct a device with device id \a childDeviceId for master \a master - explicit MtDeviceEx(XsDevice *master, const XsDeviceId &childDeviceId) : MtDevice(master, childDeviceId) {} + explicit MtDeviceEx(XsDevice* master, const XsDeviceId& childDeviceId) : MtDevice(master, childDeviceId) {} }; #else #include "mtdeviceex.h" diff --git a/lib/xspublic/xscontroller/mti3x0device.cpp b/lib/xspublic/xscontroller/mti3x0device.cpp index 6415d10..2cb8ec0 100644 --- a/lib/xspublic/xscontroller/mti3x0device.cpp +++ b/lib/xspublic/xscontroller/mti3x0device.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -117,20 +117,33 @@ MtiBaseDevice::BaseFrequencyResult Mti3X0Device::getBaseFrequencyInternal(XsData { switch (dataType & XDI_TypeMask) { - case XDI_None: return 100; - case XDI_TimestampGroup: return XDI_MAX_FREQUENCY_VAL; - case XDI_StatusGroup: return 100; - case XDI_TemperatureGroup: return 100; - case XDI_OrientationGroup: return deviceId().isImu() ? 0 : 100; - case XDI_AccelerationGroup: return 100; - case XDI_AngularVelocityGroup: return 100; - case XDI_MagneticGroup: return 100; - - case XDI_GnssGroup: return deviceId().isGnss() ? 4 : 0; - case XDI_PressureGroup: return deviceId().isGnss() ? 50 : 0; - case XDI_PositionGroup: return deviceId().isGnss() ? 100 : 0; - case XDI_VelocityGroup: return deviceId().isGnss() ? 100 : 0; - default: return 0; + case XDI_None: + return 100; + case XDI_TimestampGroup: + return XDI_MAX_FREQUENCY_VAL; + case XDI_StatusGroup: + return 100; + case XDI_TemperatureGroup: + return 100; + case XDI_OrientationGroup: + return deviceId().isImu() ? 0 : 100; + case XDI_AccelerationGroup: + return 100; + case XDI_AngularVelocityGroup: + return 100; + case XDI_MagneticGroup: + return 100; + + case XDI_GnssGroup: + return deviceId().isGnss() ? 4 : 0; + case XDI_PressureGroup: + return deviceId().isGnss() ? 50 : 0; + case XDI_PositionGroup: + return deviceId().isGnss() ? 100 : 0; + case XDI_VelocityGroup: + return deviceId().isGnss() ? 100 : 0; + default: + return 0; } }; result.m_frequency = baseFreq(dataType); @@ -150,27 +163,27 @@ bool Mti3X0Device::hasIccSupport() const uint32_t Mti3X0Device::supportedStatusFlags() const { - return (uint32_t) (XSF_ExternalClockSynced - | (deviceId().isImu() ? 0 : XSF_OrientationValid - |XSF_NoRotationMask - |XSF_RepresentativeMotion + return (uint32_t)(XSF_ExternalClockSynced + | (deviceId().isImu() ? 0 : XSF_OrientationValid + | XSF_NoRotationMask + | XSF_RepresentativeMotion ) - |XSF_ClipAccX - |XSF_ClipAccY - |XSF_ClipAccZ - |XSF_ClipGyrX - |XSF_ClipGyrY - |XSF_ClipGyrZ - |XSF_ClipMagX - |XSF_ClipMagY - |XSF_ClipMagZ - //|XSF_Retransmitted - |XSF_ClippingDetected - //|XSF_Interpolated - |XSF_SyncIn - |XSF_SyncOut - //|XSF_FilterMode - //|XSF_HaveGnssTimePulse + | XSF_ClipAccX + | XSF_ClipAccY + | XSF_ClipAccZ + | XSF_ClipGyrX + | XSF_ClipGyrY + | XSF_ClipGyrZ + | XSF_ClipMagX + | XSF_ClipMagY + | XSF_ClipMagZ + //|XSF_Retransmitted + | XSF_ClippingDetected + //|XSF_Interpolated + | XSF_SyncIn + | XSF_SyncOut + //|XSF_FilterMode + //|XSF_HaveGnssTimePulse ); } diff --git a/lib/xspublic/xscontroller/mti3x0device.h b/lib/xspublic/xscontroller/mti3x0device.h index d1a990f..9c376e9 100644 --- a/lib/xspublic/xscontroller/mti3x0device.h +++ b/lib/xspublic/xscontroller/mti3x0device.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xscontroller/mti6x0device.cpp b/lib/xspublic/xscontroller/mti6x0device.cpp index ecf65b6..9aff716 100644 --- a/lib/xspublic/xscontroller/mti6x0device.cpp +++ b/lib/xspublic/xscontroller/mti6x0device.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -103,8 +103,9 @@ MtiBaseDevice::BaseFrequencyResult Mti6X0Device::getBaseFrequencyInternal(XsData result.m_divedable = true; if ((dataType == XDI_FreeAcceleration && deviceId().isImu()) || - ((dataType & XDI_FullTypeMask) == XDI_LocationId) || - ((dataType & XDI_FullTypeMask) == XDI_DeviceId)) + ((dataType & XDI_FullTypeMask) == XDI_LocationId) || + ((dataType & XDI_FullTypeMask) == XDI_DeviceId) || + ((dataType & XDI_FullTypeMask) == XDI_RawGyroTemp)) return result; if ((dataType & XDI_FullTypeMask) == XDI_AccelerationHR) @@ -123,20 +124,35 @@ MtiBaseDevice::BaseFrequencyResult Mti6X0Device::getBaseFrequencyInternal(XsData { switch (dataType & XDI_TypeMask) { - case XDI_None: return 400; - case XDI_TimestampGroup: return XDI_MAX_FREQUENCY_VAL; - case XDI_StatusGroup: return 400; - case XDI_TemperatureGroup: return 400; - case XDI_OrientationGroup: return deviceId().isImu() ? 0 : 400; - case XDI_AccelerationGroup: return 400; - case XDI_AngularVelocityGroup: return 400; - case XDI_MagneticGroup: return 100; - - case XDI_GnssGroup: return deviceId().isGnss() ? 4 : 0; - case XDI_PressureGroup: return 100; - case XDI_PositionGroup: return deviceId().isGnss() ? 400 : 0; - case XDI_VelocityGroup: return deviceId().isGnss() ? 400 : 0; - default: return 0; + case XDI_None: + return 400; + case XDI_TimestampGroup: + return XDI_MAX_FREQUENCY_VAL; + case XDI_StatusGroup: + return 400; + case XDI_TemperatureGroup: + return 400; + case XDI_OrientationGroup: + return deviceId().isImu() ? 0 : 400; + case XDI_AccelerationGroup: + return 400; + case XDI_AngularVelocityGroup: + return 400; + case XDI_MagneticGroup: + return 100; + case XDI_RawSensorGroup: + return 200; + + case XDI_GnssGroup: + return deviceId().isGnss() ? 4 : 0; + case XDI_PressureGroup: + return 100; + case XDI_PositionGroup: + return deviceId().isGnss() ? 400 : 0; + case XDI_VelocityGroup: + return deviceId().isGnss() ? 400 : 0; + default: + return 0; } }; result.m_frequency = baseFreq(dataType); @@ -208,31 +224,31 @@ bool Mti6X0Device::setStringOutputMode6x0(uint32_t type, uint16_t frequency) uint32_t Mti6X0Device::supportedStatusFlags() const { - return (uint32_t) ( - XSF_ExternalClockSynced - | (deviceId().isImu() ? 0 : XSF_OrientationValid - |XSF_NoRotationMask - |XSF_RepresentativeMotion - ) - | (deviceId().isGnss() ? XSF_GpsValid : 0) - |XSF_ClipAccX - |XSF_ClipAccY - |XSF_ClipAccZ - |XSF_ClipGyrX - |XSF_ClipGyrY - |XSF_ClipGyrZ - |XSF_ClipMagX - |XSF_ClipMagY - |XSF_ClipMagZ - //|XSF_Retransmitted - |XSF_ClippingDetected - //|XSF_Interpolated - |XSF_SyncIn - |XSF_SyncOut - | (deviceId().isGnss() ? XSF_FilterMode : 0) - | (deviceId().isGnss() ? XSF_HaveGnssTimePulse : 0) - | (deviceId().isRtk() ? XSF_RtkStatus : 0) - ); + return (uint32_t)( + XSF_ExternalClockSynced + | (deviceId().isImu() ? 0 : XSF_OrientationValid + | XSF_NoRotationMask + | XSF_RepresentativeMotion + ) + | (deviceId().isGnss() ? XSF_GpsValid : 0) + | XSF_ClipAccX + | XSF_ClipAccY + | XSF_ClipAccZ + | XSF_ClipGyrX + | XSF_ClipGyrY + | XSF_ClipGyrZ + | XSF_ClipMagX + | XSF_ClipMagY + | XSF_ClipMagZ + //|XSF_Retransmitted + | XSF_ClippingDetected + //|XSF_Interpolated + | XSF_SyncIn + | XSF_SyncOut + | (deviceId().isGnss() ? XSF_FilterMode : 0) + | (deviceId().isGnss() ? XSF_HaveGnssTimePulse : 0) + | (deviceId().isRtk() ? XSF_RtkStatus : 0) + ); } /*! \copybrief XsDevice::shortProductCode diff --git a/lib/xspublic/xscontroller/mti6x0device.h b/lib/xspublic/xscontroller/mti6x0device.h index 791cf1b..2138565 100644 --- a/lib/xspublic/xscontroller/mti6x0device.h +++ b/lib/xspublic/xscontroller/mti6x0device.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xscontroller/mti7device.cpp b/lib/xspublic/xscontroller/mti7device.cpp index 0e5f551..44354fb 100644 --- a/lib/xspublic/xscontroller/mti7device.cpp +++ b/lib/xspublic/xscontroller/mti7device.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -83,32 +83,44 @@ Mti7Device::~Mti7Device() namespace { - //! \brief Returns the default frequency of the supplied \a dataType - int baseFreq(XsDataIdentifier dataType) +//! \brief Returns the default frequency of the supplied \a dataType +int baseFreq(XsDataIdentifier dataType) +{ + switch (dataType & XDI_TypeMask) { - switch (dataType & XDI_TypeMask) - { - case XDI_None: return 100; - case XDI_TimestampGroup: return XDI_MAX_FREQUENCY_VAL; - case XDI_StatusGroup: return 100; - case XDI_TemperatureGroup: return 100; - case XDI_PositionGroup: return 100; - case XDI_VelocityGroup: return 100; - case XDI_OrientationGroup: return 100; - case XDI_AccelerationGroup: return 100; - case XDI_AngularVelocityGroup: return 100; - case XDI_MagneticGroup: return 100; - case XDI_PressureGroup: return 50; + case XDI_None: + return 100; + case XDI_TimestampGroup: + return XDI_MAX_FREQUENCY_VAL; + case XDI_StatusGroup: + return 100; + case XDI_TemperatureGroup: + return 100; + case XDI_PositionGroup: + return 100; + case XDI_VelocityGroup: + return 100; + case XDI_OrientationGroup: + return 100; + case XDI_AccelerationGroup: + return 100; + case XDI_AngularVelocityGroup: + return 100; + case XDI_MagneticGroup: + return 100; + case XDI_PressureGroup: + return 50; case XDI_GnssGroup: { if ((dataType & XDI_FullTypeMask) == XDI_GnssPvtData) return 4; return 0; } - default: return 0; - } + default: + return 0; } } +} /*! \brief Returns the base update rate (hz) corresponding to the dataType */ @@ -123,8 +135,10 @@ MtiBaseDevice::BaseFrequencyResult Mti7Device::getBaseFrequencyInternal(XsDataId if ((dataType & XDI_FullTypeMask) == XDI_AccelerationHR || (dataType & XDI_FullTypeMask) == XDI_RateOfTurnHR) { - result.m_frequency = 800; - result.m_divedable = true; + bool isMtMk4_1_v1 = hardwareVersion().major() == 1; + bool isMtMk4_1_v2 = hardwareVersion().major() == 2; + result.m_frequency = isMtMk4_1_v2 ? 800 : 1000; + result.m_divedable = isMtMk4_1_v1 ? false : true; return result; } @@ -144,29 +158,29 @@ bool Mti7Device::hasIccSupport() const uint32_t Mti7Device::supportedStatusFlags() const { - return (uint32_t) ( - //|XSF_SelfTestOk - XSF_OrientationValid - |XSF_GpsValid - |XSF_NoRotationMask - |XSF_RepresentativeMotion - |XSF_ExternalClockSynced - |XSF_ClipAccX - |XSF_ClipAccY - |XSF_ClipAccZ - |XSF_ClipGyrX - |XSF_ClipGyrY - |XSF_ClipGyrZ - |XSF_ClipMagX - |XSF_ClipMagY - |XSF_ClipMagZ - //|XSF_Retransmitted - |XSF_ClippingDetected - //|XSF_Interpolated - //|XSF_SyncIn - //|XSF_SyncOut - |XSF_FilterMode - |XSF_HaveGnssTimePulse + return (uint32_t)( + //|XSF_SelfTestOk + XSF_OrientationValid + | XSF_GpsValid + | XSF_NoRotationMask + | XSF_RepresentativeMotion + | XSF_ExternalClockSynced + | XSF_ClipAccX + | XSF_ClipAccY + | XSF_ClipAccZ + | XSF_ClipGyrX + | XSF_ClipGyrY + | XSF_ClipGyrZ + | XSF_ClipMagX + | XSF_ClipMagY + | XSF_ClipMagZ + //|XSF_Retransmitted + | XSF_ClippingDetected + //|XSF_Interpolated + //|XSF_SyncIn + //|XSF_SyncOut + | XSF_FilterMode + | XSF_HaveGnssTimePulse ); } diff --git a/lib/xspublic/xscontroller/mti7device.h b/lib/xspublic/xscontroller/mti7device.h index b8fbfba..b321d3c 100644 --- a/lib/xspublic/xscontroller/mti7device.h +++ b/lib/xspublic/xscontroller/mti7device.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xscontroller/mti8x0device.cpp b/lib/xspublic/xscontroller/mti8x0device.cpp index 1782a87..d0503b6 100644 --- a/lib/xspublic/xscontroller/mti8x0device.cpp +++ b/lib/xspublic/xscontroller/mti8x0device.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -123,20 +123,33 @@ MtiBaseDevice::BaseFrequencyResult Mti8X0Device::getBaseFrequencyInternal(XsData { switch (dataType & XDI_TypeMask) { - case XDI_None: return 400; - case XDI_TimestampGroup: return XDI_MAX_FREQUENCY_VAL; - case XDI_StatusGroup: return 400; - case XDI_TemperatureGroup: return 400; - case XDI_OrientationGroup: return deviceId().isImu() ? 0 : 400; - case XDI_AccelerationGroup: return 400; - case XDI_AngularVelocityGroup: return 400; - case XDI_MagneticGroup: return 100; - - case XDI_GnssGroup: return deviceId().isGnss() ? 4 : 0; - case XDI_PressureGroup: return 100; - case XDI_PositionGroup: return deviceId().isGnss() ? 400 : 0; - case XDI_VelocityGroup: return deviceId().isGnss() ? 400 : 0; - default: return 0; + case XDI_None: + return 400; + case XDI_TimestampGroup: + return XDI_MAX_FREQUENCY_VAL; + case XDI_StatusGroup: + return 400; + case XDI_TemperatureGroup: + return 400; + case XDI_OrientationGroup: + return deviceId().isImu() ? 0 : 400; + case XDI_AccelerationGroup: + return 400; + case XDI_AngularVelocityGroup: + return 400; + case XDI_MagneticGroup: + return 100; + + case XDI_GnssGroup: + return deviceId().isGnss() ? 4 : 0; + case XDI_PressureGroup: + return 100; + case XDI_PositionGroup: + return deviceId().isGnss() ? 400 : 0; + case XDI_VelocityGroup: + return deviceId().isGnss() ? 400 : 0; + default: + return 0; } }; result.m_frequency = baseFreq(dataType); @@ -216,29 +229,29 @@ bool Mti8X0Device::setStringOutputMode6x0(uint32_t type, uint16_t frequency) uint32_t Mti8X0Device::supportedStatusFlags() const { - return (uint32_t) ( - XSF_ExternalClockSynced - | (deviceId().isImu() ? 0 : XSF_OrientationValid - |XSF_NoRotationMask - |XSF_RepresentativeMotion + return (uint32_t)( + XSF_ExternalClockSynced + | (deviceId().isImu() ? 0 : XSF_OrientationValid + | XSF_NoRotationMask + | XSF_RepresentativeMotion ) - | (deviceId().isGnss() ? XSF_GpsValid : 0) - |XSF_ClipAccX - |XSF_ClipAccY - |XSF_ClipAccZ - |XSF_ClipGyrX - |XSF_ClipGyrY - |XSF_ClipGyrZ - |XSF_ClipMagX - |XSF_ClipMagY - |XSF_ClipMagZ - //|XSF_Retransmitted - |XSF_ClippingDetected - //|XSF_Interpolated - |XSF_SyncIn - |XSF_SyncOut - | (deviceId().isGnss() ? XSF_FilterMode : 0) - | (deviceId().isGnss() ? XSF_HaveGnssTimePulse : 0) + | (deviceId().isGnss() ? XSF_GpsValid : 0) + | XSF_ClipAccX + | XSF_ClipAccY + | XSF_ClipAccZ + | XSF_ClipGyrX + | XSF_ClipGyrY + | XSF_ClipGyrZ + | XSF_ClipMagX + | XSF_ClipMagY + | XSF_ClipMagZ + //|XSF_Retransmitted + | XSF_ClippingDetected + //|XSF_Interpolated + | XSF_SyncIn + | XSF_SyncOut + | (deviceId().isGnss() ? XSF_FilterMode : 0) + | (deviceId().isGnss() ? XSF_HaveGnssTimePulse : 0) ); } @@ -304,7 +317,7 @@ XsIntArray Mti8X0Device::portConfiguration() const /*! \copydoc XsDevice::setPortConfiguration */ -bool Mti8X0Device::setPortConfiguration(XsIntArray &config) +bool Mti8X0Device::setPortConfiguration(XsIntArray& config) { XsIntArray currentConfig = portConfiguration(); diff --git a/lib/xspublic/xscontroller/mti8x0device.h b/lib/xspublic/xscontroller/mti8x0device.h index b9db10c..9e4f41d 100644 --- a/lib/xspublic/xscontroller/mti8x0device.h +++ b/lib/xspublic/xscontroller/mti8x0device.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xscontroller/mtibasedevice.cpp b/lib/xspublic/xscontroller/mtibasedevice.cpp index 4244029..e0b48ed 100644 --- a/lib/xspublic/xscontroller/mtibasedevice.cpp +++ b/lib/xspublic/xscontroller/mtibasedevice.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -110,6 +110,9 @@ bool MtiBaseDevice::setHeadingOffset(double) */ XsOutputConfigurationArray MtiBaseDevice::outputConfiguration() const { + if (deviceState() == XDS_Measurement || deviceState() == XDS_Recording) + return m_outputConfiguration; + XsMessage snd(XMID_ReqOutputConfiguration), rcv; if (!doTransaction(snd, rcv)) return XsOutputConfigurationArray(); @@ -129,14 +132,14 @@ XsOutputConfigurationArray MtiBaseDevice::outputConfiguration() const /*! \brief Set the output configuration for this device \param config The desired output configuration - \returns true if the output configuration was successfully updated + \returns XRV_OK on success \sa XsResultValue */ -bool MtiBaseDevice::setOutputConfiguration(XsOutputConfigurationArray& config) +XsResultValue MtiBaseDevice::setOutputConfigurationInternal(XsOutputConfigurationArray& config) { if (!deviceId().isMti6X0()) setStringOutputMode(0, 0, 0); - return MtDeviceEx::setOutputConfiguration(config); + return MtDeviceEx::setOutputConfigurationInternal(config); } /*! \copybrief XsDevice::setAlignmentRotationMatrix @@ -147,7 +150,7 @@ bool MtiBaseDevice::setAlignmentRotationQuaternion(XsAlignmentFrame frame, const snd.setBusId(busId()); snd.setDataByte((uint8_t)frame); for (XsSize i = 0; i < 4; ++i) - snd.setDataFloat((float)quat[i], (uint16_t) (1 + i * sizeof(float))); + snd.setDataFloat((float)quat[i], (uint16_t)(1 + i * sizeof(float))); return doTransaction(snd); } @@ -184,14 +187,14 @@ XsMatrix MtiBaseDevice::alignmentRotationMatrix(XsAlignmentFrame frame) const /*! \copybrief XsDevice::setSyncSettings */ -bool MtiBaseDevice::setSyncSettings(const XsSyncSettingArray &s) -{ - /* Mk4 sync lines: - * XSL_ClockIn => 0 external clock sync (SMCU) - * XSL_GnssClockIn => 1 GNSS clock sync (DSP?) - * XSL_In1 => 2 send data line (IMCU) - * XSL_Bi1In => 3 SMCU - * XSL_Bi1Out => 4 SMCU +bool MtiBaseDevice::setSyncSettings(const XsSyncSettingArray& s) +{ + /* Mk4 sync lines: + XSL_ClockIn => 0 external clock sync (SMCU) + XSL_GnssClockIn => 1 GNSS clock sync (DSP?) + XSL_In1 => 2 send data line (IMCU) + XSL_Bi1In => 3 SMCU + XSL_Bi1Out => 4 SMCU */ unsigned int timeResolution = XsDevice::syncSettingsTimeResolutionInMicroSeconds(deviceId()); @@ -200,10 +203,10 @@ bool MtiBaseDevice::setSyncSettings(const XsSyncSettingArray &s) return false; //if no item, create the length of 1 item with zero values - XsMessage snd(XMID_SetSyncConfiguration, (s.size() == 0) ? 12 : s.size()*12); + XsMessage snd(XMID_SetSyncConfiguration, (s.size() == 0) ? 12 : s.size() * 12); snd.setBusId(busId()); - /* create message but abort if we encounter anything out of the ordinary + /* create message but abort if we encounter anything out of the ordinary each sync setting is: 0: event / action (function) 1: line @@ -217,26 +220,24 @@ bool MtiBaseDevice::setSyncSettings(const XsSyncSettingArray &s) for (size_t i = 0; i < s.size(); ++i) { const XsSyncSetting& setting = s[i]; - XsSize offset = i*12; + XsSize offset = i * 12; snd.setDataByte(setting.m_function, offset); const uint8_t line = syncLine(setting); - snd.setDataByte(line, offset+1); + snd.setDataByte(line, offset + 1); assert(setting.m_polarity != XSP_None); - snd.setDataByte(setting.m_polarity, offset+2); - snd.setDataByte(setting.m_triggerOnce?1:0, offset+3); - snd.setDataShort(setting.m_skipFirst, offset+4); - snd.setDataShort(setting.m_skipFactor, offset+6); - snd.setDataShort((uint16_t) (setting.m_pulseWidth / timeResolution), offset+8); + snd.setDataByte(setting.m_polarity, offset + 2); + snd.setDataByte(setting.m_triggerOnce ? 1 : 0, offset + 3); + snd.setDataShort(setting.m_skipFirst, offset + 4); + snd.setDataShort(setting.m_skipFactor, offset + 6); + snd.setDataShort((uint16_t)(setting.m_pulseWidth / timeResolution), offset + 8); uint16_t value = 0; if (setting.m_function == XSF_ClockBiasEstimation || setting.m_function == XSF_SampleAndSend) - { - snd.setDataShort(setting.m_clockPeriod, offset+10); - } + snd.setDataShort(setting.m_clockPeriod, offset + 10); else { - value = (uint16_t) (int16_t) (setting.m_offset / (int) timeResolution); - snd.setDataShort(value, offset+10); + value = (uint16_t)(int16_t)(setting.m_offset / (int) timeResolution); + snd.setDataShort(value, offset + 10); } } @@ -264,25 +265,25 @@ XsSyncSettingArray MtiBaseDevice::syncSettingsFromBuffer(const uint8_t* buffer) for (XsSize i = 0; i < 10; ++i) { uint16_t tmp_pulseWidth; - XsSize offset = i*12; + XsSize offset = i * 12; XsSyncSetting ss; ss.m_function = (XsSyncFunction) buffer[offset]; - ss.m_polarity = (XsSyncPolarity) buffer[offset+2]; + ss.m_polarity = (XsSyncPolarity) buffer[offset + 2]; if (ss.m_polarity == XSP_None) break; ss.m_line = syncSettingsLine(buffer, offset); - ss.m_triggerOnce = buffer[offset+3]; - memcpy((void*) &ss.m_skipFirst, (void const*) &buffer[offset+4], sizeof(uint16_t)); - memcpy((void*) &ss.m_skipFactor, (void const*) &buffer[offset+6], sizeof(uint16_t)); - memcpy((void*) &tmp_pulseWidth, (void const*) &buffer[offset+8], sizeof(uint16_t)); + ss.m_triggerOnce = buffer[offset + 3]; + memcpy((void*) &ss.m_skipFirst, (void const*) &buffer[offset + 4], sizeof(uint16_t)); + memcpy((void*) &ss.m_skipFactor, (void const*) &buffer[offset + 6], sizeof(uint16_t)); + memcpy((void*) &tmp_pulseWidth, (void const*) &buffer[offset + 8], sizeof(uint16_t)); ss.m_pulseWidth = (uint32_t) tmp_pulseWidth * (uint32_t) timeResolution; if (ss.m_function == XSF_ClockBiasEstimation || ss.m_function == XSF_SampleAndSend) - memcpy((void*) &ss.m_clockPeriod, (void const*) &buffer[offset+10], sizeof(uint16_t)); + memcpy((void*) &ss.m_clockPeriod, (void const*) &buffer[offset + 10], sizeof(uint16_t)); else { int16_t tmp_offset; - memcpy((void*) &tmp_offset, (void const*) &buffer[offset+10], sizeof(int16_t)); + memcpy((void*) &tmp_offset, (void const*) &buffer[offset + 10], sizeof(int16_t)); ss.m_offset = (int32_t) tmp_offset * (int32_t) timeResolution; } @@ -360,19 +361,11 @@ std::vector MtiBaseDevice::supportedUpdateRates(XsDataIdentifier dataType) return updateRates; } - std::set unsupportedUpdateRates; - unsupportedUpdateRates.insert(500); - unsupportedUpdateRates.insert(250); - unsupportedUpdateRates.insert(125); - for (int skip = 0; skip <= baseFreq.m_frequency; ++skip) { int freq = calcFrequency(baseFreq.m_frequency, (uint16_t) skip); - if (freq * (skip+1) == baseFreq.m_frequency) - { - if (unsupportedUpdateRates.count(freq) == 0) - updateRates.push_back(freq); - } + if (freq * (skip + 1) == baseFreq.m_frequency) + updateRates.push_back(freq); } return updateRates; @@ -393,7 +386,7 @@ bool MtiBaseDevice::setInitialPositionLLA(const XsVector& lla) if (bid == XS_BID_INVALID || bid == XS_BID_BROADCAST || lla.size() != 3) return false; - XsMessage snd(XMID_SetLatLonAlt, 3*sizeof(double)); + XsMessage snd(XMID_SetLatLonAlt, 3 * sizeof(double)); snd.setDataDouble(lla[0], 0); snd.setDataDouble(lla[1], 8); snd.setDataDouble(lla[2], 16); @@ -525,9 +518,7 @@ uint8_t MtiBaseDevice::syncLine(const XsSyncSetting& setting) const SyncLineMk4 mk4Line = xslToXsl4(setting.m_line); assert(mk4Line != XSL4_Invalid); if (mk4Line == XSL4_ClockIn || mk4Line == XSL4_GnssClockIn) - { assert(setting.m_function == XSF_ClockBiasEstimation); - } return static_cast(mk4Line); } else @@ -535,14 +526,12 @@ uint8_t MtiBaseDevice::syncLine(const XsSyncSetting& setting) const SyncLineGmt gmtLine = xslToXslgmt(setting.m_line); assert(gmtLine != XSLGMT_Invalid); if (gmtLine == XSLGMT_ClockIn) - { assert(setting.m_function == XSF_SampleAndSend); - } return static_cast(gmtLine); } } -bool MtiBaseDevice::messageLooksSane(const XsMessage &msg) const +bool MtiBaseDevice::messageLooksSane(const XsMessage& msg) const { (void)msg; return true; diff --git a/lib/xspublic/xscontroller/mtibasedevice.h b/lib/xspublic/xscontroller/mtibasedevice.h index 082263d..5855c3b 100644 --- a/lib/xspublic/xscontroller/mtibasedevice.h +++ b/lib/xspublic/xscontroller/mtibasedevice.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -89,9 +89,9 @@ class MtiBaseDevice : public MtDeviceEx XsOutputConfigurationArray outputConfiguration() const; - bool setOutputConfiguration(XsOutputConfigurationArray& o) override; + XsResultValue setOutputConfigurationInternal(XsOutputConfigurationArray& o) override; - int getBaseFrequency(XsDataIdentifier dataType = XDI_None) const; + int getBaseFrequency(XsDataIdentifier dataType = XDI_None) const override; std::vector supportedUpdateRates(XsDataIdentifier dataType = XDI_None) const override; bool setAlignmentRotationMatrix(XsAlignmentFrame frame, const XsMatrix& matrix) override; @@ -102,7 +102,7 @@ class MtiBaseDevice : public MtDeviceEx bool setHeadingOffset(double offset); XsSyncSettingArray syncSettings() const override; - bool setSyncSettings(const XsSyncSettingArray &s) override; + bool setSyncSettings(const XsSyncSettingArray& s) override; bool setNoRotation(uint16_t duration); @@ -115,7 +115,7 @@ class MtiBaseDevice : public MtDeviceEx uint16_t rs485TransmissionDelay() const; bool setRs485TransmissionDelay(uint16_t delay); - bool messageLooksSane(const XsMessage &msg) const override; + bool messageLooksSane(const XsMessage& msg) const override; bool startRepresentativeMotion() override; bool representativeMotionState() override; @@ -144,7 +144,11 @@ class MtiBaseDevice : public MtDeviceEx \param dataType The Data identifier to use \returns The base frequency result */ - virtual BaseFrequencyResult getBaseFrequencyInternal(XsDataIdentifier dataType = XDI_None) const { (void) dataType; return BaseFrequencyResult(); }; + virtual BaseFrequencyResult getBaseFrequencyInternal(XsDataIdentifier dataType = XDI_None) const + { + (void) dataType; + return BaseFrequencyResult(); + }; virtual bool hasIccSupport() const; virtual bool deviceUsesOnBoardFiltering(); diff --git a/lib/xspublic/xscontroller/mtigdevice.cpp b/lib/xspublic/xscontroller/mtigdevice.cpp index a345733..e3121f2 100644 --- a/lib/xspublic/xscontroller/mtigdevice.cpp +++ b/lib/xspublic/xscontroller/mtigdevice.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -139,24 +139,39 @@ MtiBaseDevice::BaseFrequencyResult MtigDevice::getBaseFrequencyInternal(XsDataId switch (dataType & XDI_TypeMask) { - case XDI_None: return 2000; - case XDI_TimestampGroup: return XDI_MAX_FREQUENCY_VAL; - - case XDI_RawSensorGroup: return 2000; - case XDI_AnalogInGroup: return 2000; - case XDI_StatusGroup: return 2000; - - case XDI_TemperatureGroup: return 400; - case XDI_PositionGroup: return 400; - case XDI_VelocityGroup: return 400; - case XDI_OrientationGroup: return 400; - case XDI_AccelerationGroup: return 400; - case XDI_AngularVelocityGroup: return 400; - case XDI_MagneticGroup: return 100; - case XDI_PressureGroup: return 50; - - case XDI_GnssGroup: return isLegacyFirmware ? 0 : 4; - default: return 0; + case XDI_None: + return 2000; + case XDI_TimestampGroup: + return XDI_MAX_FREQUENCY_VAL; + + case XDI_RawSensorGroup: + return 2000; + case XDI_AnalogInGroup: + return 2000; + case XDI_StatusGroup: + return 2000; + + case XDI_TemperatureGroup: + return 400; + case XDI_PositionGroup: + return 400; + case XDI_VelocityGroup: + return 400; + case XDI_OrientationGroup: + return 400; + case XDI_AccelerationGroup: + return 400; + case XDI_AngularVelocityGroup: + return 400; + case XDI_MagneticGroup: + return 100; + case XDI_PressureGroup: + return 50; + + case XDI_GnssGroup: + return isLegacyFirmware ? 0 : 4; + default: + return 0; } }; @@ -170,28 +185,28 @@ MtiBaseDevice::BaseFrequencyResult MtigDevice::getBaseFrequencyInternal(XsDataId uint32_t MtigDevice::supportedStatusFlags() const { - return (uint32_t) ( - //|XSF_SelfTestOk - XSF_OrientationValid - |XSF_GpsValid - |XSF_NoRotationMask - |XSF_RepresentativeMotion - |XSF_ExternalClockSynced - |XSF_ClipAccX - |XSF_ClipAccY - |XSF_ClipAccZ - |XSF_ClipGyrX - |XSF_ClipGyrY - |XSF_ClipGyrZ - |XSF_ClipMagX - |XSF_ClipMagY - |XSF_ClipMagZ - //|XSF_Retransmitted - |XSF_ClippingDetected - //|XSF_Interpolated - |XSF_SyncIn - |XSF_SyncOut - |XSF_FilterMode - |XSF_HaveGnssTimePulse + return (uint32_t)( + //|XSF_SelfTestOk + XSF_OrientationValid + | XSF_GpsValid + | XSF_NoRotationMask + | XSF_RepresentativeMotion + | XSF_ExternalClockSynced + | XSF_ClipAccX + | XSF_ClipAccY + | XSF_ClipAccZ + | XSF_ClipGyrX + | XSF_ClipGyrY + | XSF_ClipGyrZ + | XSF_ClipMagX + | XSF_ClipMagY + | XSF_ClipMagZ + //|XSF_Retransmitted + | XSF_ClippingDetected + //|XSF_Interpolated + | XSF_SyncIn + | XSF_SyncOut + | XSF_FilterMode + | XSF_HaveGnssTimePulse ); } diff --git a/lib/xspublic/xscontroller/mtigdevice.h b/lib/xspublic/xscontroller/mtigdevice.h index 4a257f2..530549e 100644 --- a/lib/xspublic/xscontroller/mtigdevice.h +++ b/lib/xspublic/xscontroller/mtigdevice.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xscontroller/mtix00device.cpp b/lib/xspublic/xscontroller/mtix00device.cpp index 9642eff..1e8ae69 100644 --- a/lib/xspublic/xscontroller/mtix00device.cpp +++ b/lib/xspublic/xscontroller/mtix00device.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -141,24 +141,39 @@ MtiBaseDevice::BaseFrequencyResult MtiX00Device::getBaseFrequencyInternal(XsData { switch (dataType & XDI_TypeMask) { - case XDI_None: return 2000; - case XDI_TimestampGroup: return XDI_MAX_FREQUENCY_VAL; - - case XDI_RawSensorGroup: return 2000; - case XDI_AnalogInGroup: return 2000; - case XDI_StatusGroup: return 2000; - - case XDI_TemperatureGroup: return 400; - case XDI_PositionGroup: return deviceId().isGnss() ? 400 : 0; - case XDI_VelocityGroup: return deviceId().isGnss() ? 400 : 0; - case XDI_OrientationGroup: return deviceId().isImu() ? 0 : 400; - case XDI_AccelerationGroup: return 400; - case XDI_AngularVelocityGroup: return 400; - case XDI_MagneticGroup: return 100; - case XDI_PressureGroup: return 50; - - case XDI_GnssGroup: return deviceId().isGnss() ? 4 : 0; - default: return 0; + case XDI_None: + return 2000; + case XDI_TimestampGroup: + return XDI_MAX_FREQUENCY_VAL; + + case XDI_RawSensorGroup: + return 2000; + case XDI_AnalogInGroup: + return 2000; + case XDI_StatusGroup: + return 2000; + + case XDI_TemperatureGroup: + return 400; + case XDI_PositionGroup: + return deviceId().isGnss() ? 400 : 0; + case XDI_VelocityGroup: + return deviceId().isGnss() ? 400 : 0; + case XDI_OrientationGroup: + return deviceId().isImu() ? 0 : 400; + case XDI_AccelerationGroup: + return 400; + case XDI_AngularVelocityGroup: + return 400; + case XDI_MagneticGroup: + return 100; + case XDI_PressureGroup: + return 50; + + case XDI_GnssGroup: + return deviceId().isGnss() ? 4 : 0; + default: + return 0; } }; result.m_frequency = baseFreq(dataType); @@ -171,26 +186,26 @@ MtiBaseDevice::BaseFrequencyResult MtiX00Device::getBaseFrequencyInternal(XsData uint32_t MtiX00Device::supportedStatusFlags() const { - return (uint32_t) (XSF_ExternalClockSynced - | (deviceId().isImu() ? 0 : XSF_OrientationValid - |XSF_NoRotationMask - |XSF_RepresentativeMotion + return (uint32_t)(XSF_ExternalClockSynced + | (deviceId().isImu() ? 0 : XSF_OrientationValid + | XSF_NoRotationMask + | XSF_RepresentativeMotion ) - |XSF_ClipAccX - |XSF_ClipAccY - |XSF_ClipAccZ - |XSF_ClipGyrX - |XSF_ClipGyrY - |XSF_ClipGyrZ - |XSF_ClipMagX - |XSF_ClipMagY - |XSF_ClipMagZ - //|XSF_Retransmitted - |XSF_ClippingDetected - //|XSF_Interpolated - |XSF_SyncIn - |XSF_SyncOut - //|XSF_FilterMode - //|XSF_HaveGnssTimePulse + | XSF_ClipAccX + | XSF_ClipAccY + | XSF_ClipAccZ + | XSF_ClipGyrX + | XSF_ClipGyrY + | XSF_ClipGyrZ + | XSF_ClipMagX + | XSF_ClipMagY + | XSF_ClipMagZ + //|XSF_Retransmitted + | XSF_ClippingDetected + //|XSF_Interpolated + | XSF_SyncIn + | XSF_SyncOut + //|XSF_FilterMode + //|XSF_HaveGnssTimePulse ); } diff --git a/lib/xspublic/xscontroller/mtix00device.h b/lib/xspublic/xscontroller/mtix00device.h index 3f9cba4..d3eddbf 100644 --- a/lib/xspublic/xscontroller/mtix00device.h +++ b/lib/xspublic/xscontroller/mtix00device.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xscontroller/mtix0device.cpp b/lib/xspublic/xscontroller/mtix0device.cpp index 8ddf094..7036fa8 100644 --- a/lib/xspublic/xscontroller/mtix0device.cpp +++ b/lib/xspublic/xscontroller/mtix0device.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -141,19 +141,30 @@ MtiBaseDevice::BaseFrequencyResult MtiX0Device::getBaseFrequencyInternal(XsDataI { switch (dataType & XDI_TypeMask) { - case XDI_None: return 2000; - case XDI_TimestampGroup: return XDI_MAX_FREQUENCY_VAL; - - case XDI_RawSensorGroup: return 2000; - case XDI_AnalogInGroup: return 2000; - case XDI_StatusGroup: return 2000; - - case XDI_TemperatureGroup: return 400; - case XDI_OrientationGroup: return deviceId().isImu() ? 0 : 400; - case XDI_AccelerationGroup: return 400; - case XDI_AngularVelocityGroup: return 400; - case XDI_MagneticGroup: return 100; - default: return 0; + case XDI_None: + return 2000; + case XDI_TimestampGroup: + return XDI_MAX_FREQUENCY_VAL; + + case XDI_RawSensorGroup: + return 2000; + case XDI_AnalogInGroup: + return 2000; + case XDI_StatusGroup: + return 2000; + + case XDI_TemperatureGroup: + return 400; + case XDI_OrientationGroup: + return deviceId().isImu() ? 0 : 400; + case XDI_AccelerationGroup: + return 400; + case XDI_AngularVelocityGroup: + return 400; + case XDI_MagneticGroup: + return 100; + default: + return 0; } }; result.m_frequency = baseFreq(dataType); @@ -166,26 +177,26 @@ MtiBaseDevice::BaseFrequencyResult MtiX0Device::getBaseFrequencyInternal(XsDataI uint32_t MtiX0Device::supportedStatusFlags() const { - return (uint32_t) (XSF_ExternalClockSynced - | (deviceId().isImu() ? 0 : XSF_OrientationValid - |XSF_NoRotationMask - |XSF_RepresentativeMotion + return (uint32_t)(XSF_ExternalClockSynced + | (deviceId().isImu() ? 0 : XSF_OrientationValid + | XSF_NoRotationMask + | XSF_RepresentativeMotion ) - |XSF_ClipAccX - |XSF_ClipAccY - |XSF_ClipAccZ - |XSF_ClipGyrX - |XSF_ClipGyrY - |XSF_ClipGyrZ - |XSF_ClipMagX - |XSF_ClipMagY - |XSF_ClipMagZ - //|XSF_Retransmitted - |XSF_ClippingDetected - //|XSF_Interpolated - |XSF_SyncIn - |XSF_SyncOut - //|XSF_FilterMode - //|XSF_HaveGnssTimePulse + | XSF_ClipAccX + | XSF_ClipAccY + | XSF_ClipAccZ + | XSF_ClipGyrX + | XSF_ClipGyrY + | XSF_ClipGyrZ + | XSF_ClipMagX + | XSF_ClipMagY + | XSF_ClipMagZ + //|XSF_Retransmitted + | XSF_ClippingDetected + //|XSF_Interpolated + | XSF_SyncIn + | XSF_SyncOut + //|XSF_FilterMode + //|XSF_HaveGnssTimePulse ); } diff --git a/lib/xspublic/xscontroller/mtix0device.h b/lib/xspublic/xscontroller/mtix0device.h index 0a6989e..3480e14 100644 --- a/lib/xspublic/xscontroller/mtix0device.h +++ b/lib/xspublic/xscontroller/mtix0device.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xscontroller/mtixdevice.cpp b/lib/xspublic/xscontroller/mtixdevice.cpp index ebd03e4..3993979 100644 --- a/lib/xspublic/xscontroller/mtixdevice.cpp +++ b/lib/xspublic/xscontroller/mtixdevice.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -107,9 +107,10 @@ MtiBaseDevice::BaseFrequencyResult MtiXDevice::getBaseFrequencyInternal(XsDataId if ((dataType & XDI_FullTypeMask) == XDI_AccelerationHR || (dataType & XDI_FullTypeMask) == XDI_RateOfTurnHR) { + bool isMtMk4_1_v1 = hardwareVersion().major() == 1; bool isMtMk4_1_v2 = hardwareVersion().major() == 2; result.m_frequency = isMtMk4_1_v2 ? 800 : 1000; - result.m_divedable = isMtMk4_1_v2 ? true : false; + result.m_divedable = isMtMk4_1_v1 ? false : true; return result; } @@ -118,20 +119,33 @@ MtiBaseDevice::BaseFrequencyResult MtiXDevice::getBaseFrequencyInternal(XsDataId { switch (dataType & XDI_TypeMask) { - case XDI_None: return 100; - case XDI_TimestampGroup: return XDI_MAX_FREQUENCY_VAL; - case XDI_StatusGroup: return 100; - case XDI_TemperatureGroup: return 100; - case XDI_OrientationGroup: return deviceId().isImu() ? 0 : 100; - case XDI_AccelerationGroup: return 100; - case XDI_AngularVelocityGroup: return 100; - case XDI_MagneticGroup: return 100; - - case XDI_GnssGroup: return deviceId().isGnss() ? 4 : 0; - case XDI_PressureGroup: return deviceId().isGnss() ? 50 : 0; - case XDI_PositionGroup: return deviceId().isGnss() ? 100 : 0; - case XDI_VelocityGroup: return deviceId().isGnss() ? 100 : 0; - default: return 0; + case XDI_None: + return 100; + case XDI_TimestampGroup: + return XDI_MAX_FREQUENCY_VAL; + case XDI_StatusGroup: + return 100; + case XDI_TemperatureGroup: + return 100; + case XDI_OrientationGroup: + return deviceId().isImu() ? 0 : 100; + case XDI_AccelerationGroup: + return 100; + case XDI_AngularVelocityGroup: + return 100; + case XDI_MagneticGroup: + return 100; + + case XDI_GnssGroup: + return deviceId().isGnss() ? 4 : 0; + case XDI_PressureGroup: + return deviceId().isGnss() ? 50 : 0; + case XDI_PositionGroup: + return deviceId().isGnss() ? 100 : 0; + case XDI_VelocityGroup: + return deviceId().isGnss() ? 100 : 0; + default: + return 0; } }; result.m_frequency = baseFreq(dataType); @@ -151,27 +165,27 @@ bool MtiXDevice::hasIccSupport() const uint32_t MtiXDevice::supportedStatusFlags() const { - return (uint32_t) (XSF_ExternalClockSynced - | (deviceId().isImu() ? 0 : XSF_OrientationValid - |XSF_NoRotationMask - |XSF_RepresentativeMotion + return (uint32_t)(XSF_ExternalClockSynced + | (deviceId().isImu() ? 0 : XSF_OrientationValid + | XSF_NoRotationMask + | XSF_RepresentativeMotion ) - |XSF_ClipAccX - |XSF_ClipAccY - |XSF_ClipAccZ - |XSF_ClipGyrX - |XSF_ClipGyrY - |XSF_ClipGyrZ - |XSF_ClipMagX - |XSF_ClipMagY - |XSF_ClipMagZ - //|XSF_Retransmitted - |XSF_ClippingDetected - //|XSF_Interpolated - |XSF_SyncIn - |XSF_SyncOut - //|XSF_FilterMode - //|XSF_HaveGnssTimePulse + | XSF_ClipAccX + | XSF_ClipAccY + | XSF_ClipAccZ + | XSF_ClipGyrX + | XSF_ClipGyrY + | XSF_ClipGyrZ + | XSF_ClipMagX + | XSF_ClipMagY + | XSF_ClipMagZ + //|XSF_Retransmitted + | XSF_ClippingDetected + //|XSF_Interpolated + | XSF_SyncIn + | XSF_SyncOut + //|XSF_FilterMode + //|XSF_HaveGnssTimePulse ); } diff --git a/lib/xspublic/xscontroller/mtixdevice.h b/lib/xspublic/xscontroller/mtixdevice.h index 77e381a..6adb024 100644 --- a/lib/xspublic/xscontroller/mtixdevice.h +++ b/lib/xspublic/xscontroller/mtixdevice.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xscontroller/mtsyncsettings.h b/lib/xspublic/xscontroller/mtsyncsettings.h index 455d3c2..24c4e5a 100644 --- a/lib/xspublic/xscontroller/mtsyncsettings.h +++ b/lib/xspublic/xscontroller/mtsyncsettings.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -69,11 +69,13 @@ /*! \brief The sync settings of MT */ -struct MtSyncSettings { +struct MtSyncSettings +{ /*! \brief Sync in settings */ - struct { + struct + { uint16_t m_mode; uint16_t m_skipFactor; uint32_t m_offset; @@ -81,7 +83,8 @@ struct MtSyncSettings { /*! \brief Sync out settings */ - struct { + struct + { uint16_t m_mode; uint16_t m_skipFactor; uint32_t m_offset; diff --git a/lib/xspublic/xscontroller/mtthread.cpp b/lib/xspublic/xscontroller/mtthread.cpp index fc582ea..6c14b61 100644 --- a/lib/xspublic/xscontroller/mtthread.cpp +++ b/lib/xspublic/xscontroller/mtthread.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -96,7 +96,7 @@ MtThread::~MtThread(void) if (m_gotoConfigPlus) delete m_gotoConfigPlus; } - catch(...) + catch (...) { } } @@ -104,7 +104,7 @@ MtThread::~MtThread(void) /*! \brief Set whether we should send gotoconfig here */ void MtThread::setDoGotoConfig(bool doit) { - srand( (unsigned int)XsTime_timeStampNow(0)); + srand((unsigned int)XsTime_timeStampNow(0)); m_doGotoConfig = doit; } @@ -120,7 +120,7 @@ int32_t MtThread::innerFunction(void) if (m_communicator->writeRawData(*m_gotoConfigPlus) != XRV_OK) JLALERTG("Send gotoConfig failed"); - XsTime_msleep((uint32_t) ((rand() * 10)/RAND_MAX+5)); // if we sent a goto config, wait a bit for the result + XsTime_msleep((uint32_t)(((unsigned)rand()) / (RAND_MAX / 10) + 5)); // if we sent a goto config, wait a bit for the result } return DataPoller::innerFunction(); diff --git a/lib/xspublic/xscontroller/mtthread.h b/lib/xspublic/xscontroller/mtthread.h index 095628a..fdd65ef 100644 --- a/lib/xspublic/xscontroller/mtthread.h +++ b/lib/xspublic/xscontroller/mtthread.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xscontroller/nmea_protocolhandler.cpp b/lib/xspublic/xscontroller/nmea_protocolhandler.cpp index c1eec1f..09fb2b5 100644 --- a/lib/xspublic/xscontroller/nmea_protocolhandler.cpp +++ b/lib/xspublic/xscontroller/nmea_protocolhandler.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -133,9 +133,7 @@ MessageLocation ProtocolHandler::findMessage(XsProtocolType& type, const XsByteA end = i + 9; } else - { foundPreamble = false; - } } } if (foundPreamble && foundEnd) diff --git a/lib/xspublic/xscontroller/nmea_protocolhandler.h b/lib/xspublic/xscontroller/nmea_protocolhandler.h index ea7e1da..436cb35 100644 --- a/lib/xspublic/xscontroller/nmea_protocolhandler.h +++ b/lib/xspublic/xscontroller/nmea_protocolhandler.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xscontroller/openportstage.h b/lib/xspublic/xscontroller/openportstage.h index dc64049..7fba5d3 100644 --- a/lib/xspublic/xscontroller/openportstage.h +++ b/lib/xspublic/xscontroller/openportstage.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -73,7 +73,8 @@ performed or at least not all at the same time. For these cases, this enumeration supplies the option of selecting which stages should be performed. */ -enum OpenPortStage { +enum OpenPortStage +{ OPS_OpenPort = 1, //OPS_StartThread = 2, // obsolete OPS_InitDevice = 4, @@ -84,4 +85,4 @@ enum OpenPortStage { /*! @} */ typedef enum OpenPortStage OpenPortStage; -#endif // file guard +#endif diff --git a/lib/xspublic/xscontroller/packeterrorrateestimator.cpp b/lib/xspublic/xscontroller/packeterrorrateestimator.cpp index 2c3db37..93428f8 100644 --- a/lib/xspublic/xscontroller/packeterrorrateestimator.cpp +++ b/lib/xspublic/xscontroller/packeterrorrateestimator.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -148,7 +148,7 @@ int32_t PacketErrorRateEstimator::innerFunction(void) packetDeliveryRate = 100.0f; m_packetErrorRate = (uint8_t)(100 - (int)floor(packetDeliveryRate)); - JLDEBUGG("Received " << m_receivedPacketCount << " packets in " << msSinceLastUpdate << " ms. Delivery rate: " << (int) (packetDeliveryRate) << "%. Error rate: " << (int)m_packetErrorRate << "%."); + JLDEBUGG("Received " << m_receivedPacketCount << " packets in " << msSinceLastUpdate << " ms. Delivery rate: " << (int)(packetDeliveryRate) << "%. Error rate: " << (int)m_packetErrorRate << "%."); } else { diff --git a/lib/xspublic/xscontroller/packeterrorrateestimator.h b/lib/xspublic/xscontroller/packeterrorrateestimator.h index f655bbd..8554807 100644 --- a/lib/xspublic/xscontroller/packeterrorrateestimator.h +++ b/lib/xspublic/xscontroller/packeterrorrateestimator.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xscontroller/packetstamper.cpp b/lib/xspublic/xscontroller/packetstamper.cpp index a76634e..52383e7 100644 --- a/lib/xspublic/xscontroller/packetstamper.cpp +++ b/lib/xspublic/xscontroller/packetstamper.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -88,7 +88,7 @@ PacketStamper::PacketStamper() //! \brief Reset the Time Of Sampling estimation parameters void PacketStamper::resetTosEstimation() { - m_latest = DataPair{-1,0}; + m_latest = DataPair{-1, 0}; m_rejectionCountdown = 0; m_dataPoints.clear(); m_rate = 0; @@ -157,7 +157,7 @@ int64_t PacketStamper::stampPacket(XsDataPacket& pack, XsDataPacket const& highe else newCounter = lastCounter + 1; -// JLDEBUGG("XsensDeviceAPI", "%s [%08x] old = %I64d new = %I64d diff = %I64d", __FUNCTION__, did, lastCounter, newCounter, (newCounter-lastCounter)); + // JLDEBUGG("XsensDeviceAPI", "%s [%08x] old = %I64d new = %I64d diff = %I64d", __FUNCTION__, did, lastCounter, newCounter, (newCounter-lastCounter)); pack.setPacketId(newCounter); estimateTos(pack); @@ -179,9 +179,9 @@ int64_t PacketStamper::calculateLargeSampleTime(int64_t frameTime, int64_t lastT int64_t low = lastTime % 864000000; int64_t dt = frameTime - low; - if (dt < (-864000000/2)) + if (dt < (-864000000 / 2)) return lastTime + dt + 864000000; // positive wraparound - if (dt < (864000000/2)) + if (dt < (864000000 / 2)) return lastTime + dt; // normal increment return lastTime + dt - 864000000; // negative wraparound @@ -225,8 +225,8 @@ void PacketStamper::estimateClockParameters() { double dpid = d.m_pid - avgPid; double dtoa = d.m_toa - avgToa; - fracTop += dpid*dtoa; - fracBot += dpid*dpid; + fracTop += dpid * dtoa; + fracBot += dpid * dpid; } m_rate = fracTop / fracBot; m_toa0 = avgToa - m_rate * avgPid; @@ -288,7 +288,7 @@ int64_t PacketStamper::estimateTosInternal(int64_t pid, int64_t toa) if (m_dataPoints.empty()) { m_linearize = DataPair{pid, toa}; - m_dataPoints.push_back(DataPair{0,0}); + m_dataPoints.push_back(DataPair{0, 0}); m_toa0 = 0; m_rate = 0; m_rejectionCountdown = 0; @@ -298,7 +298,7 @@ int64_t PacketStamper::estimateTosInternal(int64_t pid, int64_t toa) DataPair last = {pid - m_linearize.m_pid, toa - m_linearize.m_toa}; auto first = *m_dataPoints.begin(); m_toa0 = 0; - m_rate = (double) (last.m_toa - first.m_toa) / (double) (last.m_pid - first.m_pid); + m_rate = (double)(last.m_toa - first.m_toa) / (double)(last.m_pid - first.m_pid); m_dataPoints.push_back(last); } m_latest = DataPair{pid, toa}; // non-linearized values! @@ -314,7 +314,7 @@ int64_t PacketStamper::estimateTosInternal(int64_t pid, int64_t toa) if (enough) { double toaPred = (pid - m_linearize.m_pid) * m_rate + m_toa0; - if ((double) (toa - m_linearize.m_toa) - toaPred >= 2.0*m_rate) + if ((double)(toa - m_linearize.m_toa) - toaPred >= 2.0 * m_rate) { // ignore this point, it doesn't match known information well enough // also ignore the next few points as they're likely also not very reliable @@ -331,7 +331,7 @@ int64_t PacketStamper::estimateTosInternal(int64_t pid, int64_t toa) // add data point to list m_dataPoints.push_back(DataPair {pid - m_linearize.m_pid, toa - m_linearize.m_toa}); - /* filter list, we remove any points that can't define the rate because they're above the + /* filter list, we remove any points that can't define the rate because they're above the toa line spanned by the neighbouring points */ if (enough) @@ -341,7 +341,7 @@ int64_t PacketStamper::estimateTosInternal(int64_t pid, int64_t toa) auto it = next++; while (next != m_dataPoints.end()) { - double rate = (double) (next->m_toa - prev->m_toa) / (double) (next->m_pid - prev->m_pid); + double rate = (double)(next->m_toa - prev->m_toa) / (double)(next->m_pid - prev->m_pid); double itoa = (it->m_pid - prev->m_pid) * rate + prev->m_toa; if ((double) it->m_toa >= itoa) { diff --git a/lib/xspublic/xscontroller/packetstamper.h b/lib/xspublic/xscontroller/packetstamper.h index 5b97937..41236b5 100644 --- a/lib/xspublic/xscontroller/packetstamper.h +++ b/lib/xspublic/xscontroller/packetstamper.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -86,7 +86,8 @@ class PacketStamper protected: /*! Holds a data point for the clock estimation algorithm */ - struct DataPair { + struct DataPair + { int64_t m_pid; //!< Packet ID of data item int64_t m_toa; //!< Time Of Arrival of data item diff --git a/lib/xspublic/xscontroller/protocolhandler.cpp b/lib/xspublic/xscontroller/protocolhandler.cpp index 5852870..82bc5fa 100644 --- a/lib/xspublic/xscontroller/protocolhandler.cpp +++ b/lib/xspublic/xscontroller/protocolhandler.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -68,8 +68,8 @@ #include #define DUMP_BUFFER_ON_ERROR 512 // this define doubles as the maximum buffer dump size, set to 0 to remove limit #ifdef DUMP_BUFFER_ON_ERROR -#include -#include + #include + #include #endif /*! \class ProtocolHandler @@ -107,9 +107,9 @@ static int expectedMessageSize(const unsigned char* buffer, int sz) if (sz < 6) return XS_EXTLENCODE + XS_LEN_MSGEXTHEADERCS; // typical minimum size at which point extended size is needed - return XS_LEN_MSGEXTHEADERCS + (int) (((uint32_t) hdr->m_datlen.m_extended.m_length.m_high * 256 + (uint32_t) hdr->m_datlen.m_extended.m_length.m_low)); + return XS_LEN_MSGEXTHEADERCS + (int)(((uint32_t) hdr->m_datlen.m_extended.m_length.m_high * 256 + (uint32_t) hdr->m_datlen.m_extended.m_length.m_low)); } - return XS_LEN_MSGHEADERCS + (int) (hdr->m_length); + return XS_LEN_MSGHEADERCS + (int)(hdr->m_length); } /*! \brief Write the contents of a uint8 buffer to string as hex characters */ @@ -211,9 +211,9 @@ MessageLocation ProtocolHandler::findMessage(XsProtocolType& type, const XsByteA // skip current preamble if (rv.m_size == 0) { - /* only report an error if we didn't already find a valid header - in this case, we're probably parsing data within a message, so we don't want to - skip data unless we're sure we have a valid message + /* only report an error if we didn't already find a valid header + in this case, we're probably parsing data within a message, so we don't want to + skip data unless we're sure we have a valid message */ JLALERTG("Invalid message length: " << target); //JLDEBUGG("Buffer: " << dumpBuffer(buffer, bufferSize)); @@ -266,7 +266,7 @@ MessageLocation ProtocolHandler::findMessage(XsProtocolType& type, const XsByteA rv.m_incompletePos = -1; JLALERTG( "Invalid checksum for msg at offset " << pre << " bufferSize = " << bufferSize - << " buffer at offset: " << dumpBuffer(raw.data()+pre, raw.size()-pre)); + << " buffer at offset: " << dumpBuffer(raw.data() + pre, raw.size() - pre)); } } } @@ -307,7 +307,7 @@ int ProtocolHandler::minimumMessageSize() const /*! \brief Returns the maximum size of a valid message of this protocol including preambles and checksums */ int ProtocolHandler::maximumMessageSize() const { - return XS_LEN_MSGEXTHEADERCS+XS_MAXDATALEN; // maximum size of xsens xbus protocol message + return XS_LEN_MSGEXTHEADERCS + XS_MAXDATALEN; // maximum size of xsens xbus protocol message } /*! \brief Compose a message for transmission diff --git a/lib/xspublic/xscontroller/protocolhandler.h b/lib/xspublic/xscontroller/protocolhandler.h index 128f631..9cb8c94 100644 --- a/lib/xspublic/xscontroller/protocolhandler.h +++ b/lib/xspublic/xscontroller/protocolhandler.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xscontroller/protocolmanager.cpp b/lib/xspublic/xscontroller/protocolmanager.cpp index 01c6b75..41c084a 100644 --- a/lib/xspublic/xscontroller/protocolmanager.cpp +++ b/lib/xspublic/xscontroller/protocolmanager.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -80,8 +80,8 @@ /*! \brief Default constructor */ -ProtocolManager::ProtocolManager(Communicator const & communicator) -: m_communicator(communicator) +ProtocolManager::ProtocolManager(Communicator const& communicator) + : m_communicator(communicator) { } @@ -117,7 +117,7 @@ MessageLocation ProtocolManager::findMessage(XsProtocolType& type, const XsByteA for (container_type::iterator i = m_protocolHandlers.begin(); i != m_protocolHandlers.end(); ++i) { assert((*i).operator->() != 0); - IProtocolHandler const & handler = **i; + IProtocolHandler const& handler = **i; XsProtocolType currentProtocolType = static_cast(handler.type()); MessageLocation currentMessageLocation = handler.findMessage(type, raw); @@ -134,18 +134,16 @@ MessageLocation ProtocolManager::findMessage(XsProtocolType& type, const XsByteA // Stop searching if the location is as good as it gets if (bestMessageLocation.m_startPos == 0) - { break; - } } else { // No valid location/message produced. Continue searching. // location may still contain useful information if ((currentMessageLocation.m_startPos >= 0 && currentMessageLocation.m_size < 0 && - (!bestMessageLocation.isValid() || bestMessageLocation.m_startPos > currentMessageLocation.m_startPos)) || + (!bestMessageLocation.isValid() || bestMessageLocation.m_startPos > currentMessageLocation.m_startPos)) || (currentMessageLocation.m_incompletePos >= 0 && currentMessageLocation.m_incompleteSize > 0 && - (!bestMessageLocation.isValid() || bestMessageLocation.m_startPos > currentMessageLocation.m_incompletePos))) + (!bestMessageLocation.isValid() || bestMessageLocation.m_startPos > currentMessageLocation.m_incompletePos))) { bestMessageLocation = currentMessageLocation; bestProtocolType = currentProtocolType; @@ -189,8 +187,10 @@ bool ProtocolManager::remove(XsProtocolType type) { bool result = false; container_type::iterator i = m_protocolHandlers.begin(); - while (i != m_protocolHandlers.end()) { - if ((*i)->type() == type) { + while (i != m_protocolHandlers.end()) + { + if ((*i)->type() == type) + { // Increment the incrementing iterator before removing the pointed to element to // prevent icrementing an invalidated iterator container_type::iterator toErase = i; @@ -198,9 +198,8 @@ bool ProtocolManager::remove(XsProtocolType type) m_protocolHandlers.erase(toErase); result = true; } - else { + else ++i; - } } return result; } @@ -212,10 +211,10 @@ bool ProtocolManager::remove(XsProtocolType type) bool ProtocolManager::hasProtocol(XsProtocolType type) const { bool result = false; - for (container_type::const_iterator i = m_protocolHandlers.begin(); i != m_protocolHandlers.end(); ++i) { - if ((*i)->type() == type) { + for (container_type::const_iterator i = m_protocolHandlers.begin(); i != m_protocolHandlers.end(); ++i) + { + if ((*i)->type() == type) result = true; - } } return result; } @@ -245,7 +244,7 @@ void ProtocolManager::clear() \param[in] message The message to check \returns True if valid */ -bool ProtocolManager::validateMessage(XsMessage const & message) const +bool ProtocolManager::validateMessage(XsMessage const& message) const { return m_communicator.sanityCheck(message); } diff --git a/lib/xspublic/xscontroller/protocolmanager.h b/lib/xspublic/xscontroller/protocolmanager.h index fea47e0..3da83d8 100644 --- a/lib/xspublic/xscontroller/protocolmanager.h +++ b/lib/xspublic/xscontroller/protocolmanager.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -91,7 +91,7 @@ class ProtocolManager : public IProtocolManager //! \brief A typedef for a const iterator typedef container_type::const_iterator const_iterator; - explicit ProtocolManager(Communicator const &); + explicit ProtocolManager(Communicator const&); virtual ~ProtocolManager() throw(); int likelyMinimumMessageSize() const; @@ -100,19 +100,19 @@ class ProtocolManager : public IProtocolManager const_iterator end() const; MessageLocation findMessage(XsProtocolType& type, const XsByteArray& raw) override; XsMessage convertToMessage(XsProtocolType& type, MessageLocation& location, const XsByteArray& raw) override; - bool validateMessage(XsMessage const & message) const override; + bool validateMessage(XsMessage const& message) const override; /*! \brief Adds the protocol handler \param[in] handler The protocol handler to add \returns The value type */ - virtual value_type add(IProtocolHandler *handler); + virtual value_type add(IProtocolHandler* handler); virtual bool remove(XsProtocolType type); virtual bool hasProtocol(XsProtocolType type) const; virtual void clear(); private: - Communicator const & m_communicator; + Communicator const& m_communicator; // mutable because the order of elements is optimized during findMessage to speedup future searches // but the findMessage method is conceptually const diff --git a/lib/xspublic/xscontroller/proxycommunicator.cpp b/lib/xspublic/xscontroller/proxycommunicator.cpp index f42f352..734f1b8 100644 --- a/lib/xspublic/xscontroller/proxycommunicator.cpp +++ b/lib/xspublic/xscontroller/proxycommunicator.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -83,7 +83,7 @@ \param channelLatency The latency of channel */ ProxyCommunicator::ProxyCommunicator(int channelId, uint32_t channelLatency) - : m_channelId (channelId) + : m_channelId(channelId) , m_channelLatency(channelLatency) { } @@ -122,7 +122,7 @@ bool ProxyCommunicator::isPortOpen() const \details If successful information on the connected device is available in the port info \sa portInfo \returns True if successful */ -bool ProxyCommunicator::openPort(const XsPortInfo &portInfo, OpenPortStage stage, bool detectRs485) +bool ProxyCommunicator::openPort(const XsPortInfo& portInfo, OpenPortStage stage, bool detectRs485) { auto result = [this]() { @@ -175,14 +175,14 @@ bool ProxyCommunicator::reopenPort(OpenPortStage stage, bool skipDeviceIdCheck) /*! \brief Has no effect for the ProxyCommunicator. Always returns false */ -bool ProxyCommunicator::isDockedAt(Communicator *) const +bool ProxyCommunicator::isDockedAt(Communicator*) const { return false; } /*! \brief Writes raw data to the communication channel */ -XsResultValue ProxyCommunicator::writeRawData(const XsByteArray &data) +XsResultValue ProxyCommunicator::writeRawData(const XsByteArray& data) { onTransmissionRequest(m_channelId, &data); return XRV_OK; @@ -196,14 +196,14 @@ void ProxyCommunicator::handleReceivedData(const XsByteArray& data) } /*! \brief Write a message and await the reply */ -bool ProxyCommunicator::doTransaction(const XsMessage &msg, XsMessage &rcv, uint32_t timeout) +bool ProxyCommunicator::doTransaction(const XsMessage& msg, XsMessage& rcv, uint32_t timeout) { return SerialCommunicator::doTransaction(msg, rcv, timeout + m_channelLatency); } /*! \brief Has no effect for the ProxyCommunicator */ -XsResultValue ProxyCommunicator::readDataToBuffer(XsByteArray& ) +XsResultValue ProxyCommunicator::readDataToBuffer(XsByteArray&) { //No action required as data is pushed (no polling required) in the buffer by the handleReceivedData function return XRV_OK; @@ -211,7 +211,7 @@ XsResultValue ProxyCommunicator::readDataToBuffer(XsByteArray& ) /*! \brief Handles a message received on the communication channel */ -void ProxyCommunicator::handleMessage(const XsMessage &msg) +void ProxyCommunicator::handleMessage(const XsMessage& msg) { DeviceCommunicator::handleMessage(msg); } diff --git a/lib/xspublic/xscontroller/proxycommunicator.h b/lib/xspublic/xscontroller/proxycommunicator.h index 0e6b465..c605c33 100644 --- a/lib/xspublic/xscontroller/proxycommunicator.h +++ b/lib/xspublic/xscontroller/proxycommunicator.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -78,24 +78,31 @@ class ProxyCommunicator : public SerialCommunicator XsResultValue readDataToBuffer(XsByteArray& raw) override; XsResultValue processBufferedData(const XsByteArray& rawIn, std::deque& messages) override; - void handleMessage(const XsMessage &message) override; + void handleMessage(const XsMessage& message) override; void flushPort() override; void closePort() override; bool isPortOpen() const override; - bool openPort(const XsPortInfo &portInfo, OpenPortStage stage = OPS_Full, bool detectRs485 = false) override; + bool openPort(const XsPortInfo& portInfo, OpenPortStage stage = OPS_Full, bool detectRs485 = false) override; bool reopenPort(OpenPortStage stage = OPS_Full, bool skipDeviceIdCheck = false) override; - bool isDockedAt(Communicator *other) const override; - bool allowReprocessing() const override { return false; } + bool isDockedAt(Communicator* other) const override; + bool allowReprocessing() const override + { + return false; + } void handleReceivedData(const XsByteArray& data); - bool doTransaction(const XsMessage &msg, XsMessage &rcv, uint32_t timeout) override; + bool doTransaction(const XsMessage& msg, XsMessage& rcv, uint32_t timeout) override; static XsPortInfo createPortInfo(int channelId); protected: - XsResultValue writeRawData(const XsByteArray &data) override; - std::shared_ptr createStreamInterface(const XsPortInfo &pi) override { (void)pi; return std::shared_ptr(); } + XsResultValue writeRawData(const XsByteArray& data) override; + std::shared_ptr createStreamInterface(const XsPortInfo& pi) override + { + (void)pi; + return std::shared_ptr(); + } uint32_t defaultInterfaceTimeout() const override; diff --git a/lib/xspublic/xscontroller/rangequeue.h b/lib/xspublic/xscontroller/rangequeue.h index 39e3c27..be79e0c 100644 --- a/lib/xspublic/xscontroller/rangequeue.h +++ b/lib/xspublic/xscontroller/rangequeue.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -127,22 +127,16 @@ class RangeQueue for (auto r = m_queue.rbegin(); r != m_queue.rend(); ++r) { - if ( (index > r->m_end) || (index < r->m_start) ) + if ((index > r->m_end) || (index < r->m_start)) continue; --m_count; if (r->m_start == r->m_end) - { m_queue.erase(--(r.base())); - } else if (r->m_start == index) - { r->m_start++; - } else if (r->m_end == index) - { r->m_end--; - } else { Range split(r->m_start, index - 1); @@ -229,16 +223,16 @@ class RangeQueue } /*! \returns a range limit. - * \param index: start or end limit to return. - * - * Concatenating the start and end points for each range will generate a list of limits, - * this function gives access to such a list. - * For example: - * - index 0: start of the first range - * - index 1: end of the first range - * - index 2: start of the second range - * - ... - */ + \param index: start or end limit to return. + + Concatenating the start and end points for each range will generate a list of limits, + this function gives access to such a list. + For example: + - index 0: start of the first range + - index 1: end of the first range + - index 2: start of the second range + - ... + */ const T operator[](std::size_t index) const { xsens::Lock locky(&m_mutex); @@ -267,7 +261,7 @@ class RangeQueue xsens::Lock locky(&m_mutex); for (auto i = m_queue.begin(); i != m_queue.end(); ++i) { - if ( (index >= i->m_start) && (index <= i->m_end) ) + if ((index >= i->m_start) && (index <= i->m_end)) return true; } return false; @@ -287,9 +281,7 @@ class RangeQueue xsens::Lock locky(&m_mutex); m_count = 0; for (auto i = m_queue.begin(); i != m_queue.end(); ++i) - { m_count += (i->m_end - i->m_start) + 1; - } return m_count; } @@ -304,14 +296,14 @@ class RangeQueue */ static const T illegalIndex() { - return (T)-1; + return (T) - 1; } /*! - * \brief Copy all ranges within the given limits [start, end]. - * If the limit falls in a certain range, that range is split and included - * from the start until the limit. - */ + \brief Copy all ranges within the given limits [start, end]. + If the limit falls in a certain range, that range is split and included + from the start until the limit. + */ void copy(RangeQueue& destination, T start, T end) { xsens::Lock locky(&m_mutex); @@ -319,13 +311,9 @@ class RangeQueue for (auto i = m_queue.begin(); i != m_queue.end(); ++i) { if ((i->m_start >= start && start <= i->m_end) && (i->m_start <= end)) - { destination.pushBack(i->m_start, std::min(i->m_end, end)); - } else if (i->m_start < start && i->m_start < end) - { destination.pushBack(start, std::min(i->m_end, end)); - } } } diff --git a/lib/xspublic/xscontroller/replymonitor.cpp b/lib/xspublic/xscontroller/replymonitor.cpp index 7b650f9..ab869f5 100644 --- a/lib/xspublic/xscontroller/replymonitor.cpp +++ b/lib/xspublic/xscontroller/replymonitor.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -66,7 +66,8 @@ #include "replyobject.h" #include -namespace xsens { +namespace xsens +{ /*! \class ReplyObjectDeleter \brief A class that deletes a reply object and removes it from the given monitor @@ -75,21 +76,21 @@ class ReplyObjectDeleter { public: // create a reply object deleter - ReplyObjectDeleter(ReplyObjectRemover *monitor = NULL) : + ReplyObjectDeleter(ReplyObjectRemover* monitor = NULL) : m_monitor(monitor) { } // delete the ReplyObject and remove it from m_monitor // not necessarily in that order - void operator()(ReplyObject *p) const + void operator()(ReplyObject* p) const { if (m_monitor) m_monitor->removeObject(p); delete p; } private: - ReplyObjectRemover *m_monitor; + ReplyObjectRemover* m_monitor; }; @@ -131,13 +132,13 @@ void ReplyMonitor::removeObject(std::shared_ptr const& obj) \param obj The object to remove \note This does not delete the object */ -void ReplyMonitor::removeObject(ReplyObject *obj) +void ReplyMonitor::removeObject(ReplyObject* obj) { xsens::Lock locky(&m_mutex); if (m_objectList.empty()) return; - std::vector::iterator it = std::find(m_objectList.begin(), m_objectList.end(), obj); + std::vector::iterator it = std::find(m_objectList.begin(), m_objectList.end(), obj); if (it == m_objectList.end()) return; @@ -157,7 +158,7 @@ bool ReplyMonitor::addReply(const XsMessage& message) if (m_objectList[i]->isReplyFor(message)) { ReplyObject* tmp = m_objectList[i]; - m_objectList.erase(m_objectList.begin()+(ptrdiff_t) i); + m_objectList.erase(m_objectList.begin() + (ptrdiff_t) i); tmp->setMessage(message); return true; } @@ -173,9 +174,7 @@ void ReplyMonitor::dumpObjectList(Journaller* journal, JournalLogLevel level) co size_t numElements = m_objectList.size(); JLGENERIC(journal, level, "Waiting for " << numElements << " objects"); for (size_t i = 0; i < numElements; i++) - { JLGENERIC(journal, level, i << ": msg ID = " << JLHEXLOG((int) m_objectList[i]->msgId())); - } } } // namespace xsens diff --git a/lib/xspublic/xscontroller/replymonitor.h b/lib/xspublic/xscontroller/replymonitor.h index e7b89c6..2b69c0f 100644 --- a/lib/xspublic/xscontroller/replymonitor.h +++ b/lib/xspublic/xscontroller/replymonitor.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -74,7 +74,8 @@ struct XsMessage; class ReplyObject; -namespace xsens { +namespace xsens +{ /*! \class ReplyObjectRemover \brief A class that removes a reply object from the given monitor @@ -83,7 +84,7 @@ class ReplyObjectRemover { public: //! \copybrief ReplyMonitor::removeObject - virtual void removeObject(ReplyObject *obj) = 0; + virtual void removeObject(ReplyObject* obj) = 0; virtual ~ReplyObjectRemover() {} }; @@ -93,13 +94,13 @@ class ReplyMonitor : private ReplyObjectRemover ReplyMonitor(); virtual ~ReplyMonitor(); - std::shared_ptr addReplyObject(ReplyObject *replyObject); // Posts the semaphore + std::shared_ptr addReplyObject(ReplyObject* replyObject); // Posts the semaphore bool addReply(const XsMessage& message); void removeObject(std::shared_ptr const& obj); void dumpObjectList(Journaller* journal, JournalLogLevel level) const; private: - void removeObject(ReplyObject *obj); + void removeObject(ReplyObject* obj); std::vector m_objectList; mutable Mutex m_mutex; diff --git a/lib/xspublic/xscontroller/replyobject.cpp b/lib/xspublic/xscontroller/replyobject.cpp index e603c66..0088d80 100644 --- a/lib/xspublic/xscontroller/replyobject.cpp +++ b/lib/xspublic/xscontroller/replyobject.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -84,7 +84,7 @@ ReplyObject::~ReplyObject() delete m_waitCondition; delete m_mutex; } - catch(...) + catch (...) { } } @@ -139,7 +139,7 @@ uint8_t MidReplyObject::msgId() const /*! \returns True when a message is a valid reply message for this reply object \param[in] msg the message to check */ -bool MidReplyObject::isReplyFor(XsMessage const & msg) +bool MidReplyObject::isReplyFor(XsMessage const& msg) { if (m_messageId == msg.getMessageId()) return true; @@ -158,7 +158,7 @@ bool MidReplyObject::isReplyFor(XsMessage const & msg) \param[in] size the size of the data in the data part of the message \param[in] data pointer to data to wait for (this object does not take ownership of the data) */ -MidAndDataReplyObject::MidAndDataReplyObject(uint8_t messageId, XsSize offset, XsSize size, uint8_t const * data) +MidAndDataReplyObject::MidAndDataReplyObject(uint8_t messageId, XsSize offset, XsSize size, uint8_t const* data) : ReplyObject() , m_messageId(messageId) , m_dataOffset(offset) @@ -193,7 +193,7 @@ uint8_t MidAndDataReplyObject::msgId() const /*! \returns true when a message is a valid reply message for this reply object \param[in] msg the message to check */ -bool MidAndDataReplyObject::isReplyFor(XsMessage const & msg) +bool MidAndDataReplyObject::isReplyFor(XsMessage const& msg) { if (msg.getMessageId() == XMID_Error) return true; @@ -216,7 +216,7 @@ void MidAndDataReplyObject::freeData() /*! \brief Copies data from 'data' into this object. This is the data to wait for. \param[in] data the data to copy into this object and wait for */ -void MidAndDataReplyObject::setData(uint8_t const * data) +void MidAndDataReplyObject::setData(uint8_t const* data) { freeData(); if (data != 0) diff --git a/lib/xspublic/xscontroller/replyobject.h b/lib/xspublic/xscontroller/replyobject.h index 2eac6bb..a423d84 100644 --- a/lib/xspublic/xscontroller/replyobject.h +++ b/lib/xspublic/xscontroller/replyobject.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -90,14 +90,14 @@ class ReplyObject /*! \returns True when a message is a valid reply message for this reply object \param[in] message The message to check */ - virtual bool isReplyFor(XsMessage const & message) = 0; + virtual bool isReplyFor(XsMessage const& message) = 0; //! \returns The message ID that this reply object is waiting for virtual uint8_t msgId() const = 0; private: xsens::Mutex* m_mutex; - xsens::WaitCondition *m_waitCondition; + xsens::WaitCondition* m_waitCondition; XsMessage m_message; bool m_delivered; }; @@ -111,7 +111,7 @@ class MidReplyObject : public ReplyObject MidReplyObject(uint8_t messageId); ~MidReplyObject(void); - virtual bool isReplyFor(XsMessage const & message); + virtual bool isReplyFor(XsMessage const& message); virtual uint8_t msgId() const; private: uint8_t m_messageId; @@ -123,17 +123,17 @@ class MidReplyObject : public ReplyObject class MidAndDataReplyObject : public ReplyObject { public: - MidAndDataReplyObject(uint8_t messageId, XsSize offset, XsSize size, uint8_t const * data); + MidAndDataReplyObject(uint8_t messageId, XsSize offset, XsSize size, uint8_t const* data); ~MidAndDataReplyObject(); - void setData(uint8_t const * data); - virtual bool isReplyFor(XsMessage const & message); + void setData(uint8_t const* data); + virtual bool isReplyFor(XsMessage const& message); virtual uint8_t msgId() const; private: void freeData(); uint8_t m_messageId; XsSize m_dataOffset; XsSize m_dataSize; - uint8_t * m_data; + uint8_t* m_data; }; #endif diff --git a/lib/xspublic/xscontroller/restorecommunication.cpp b/lib/xspublic/xscontroller/restorecommunication.cpp index 31377a8..0b3d0bc 100644 --- a/lib/xspublic/xscontroller/restorecommunication.cpp +++ b/lib/xspublic/xscontroller/restorecommunication.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -105,7 +105,7 @@ RestoreCommunication::~RestoreCommunication() \param portName A string with a port name. \returns XRV_OK if the procedure started successfully */ -XsResultValue RestoreCommunication::start(const XsString &portName) +XsResultValue RestoreCommunication::start(const XsString& portName) { m_portName = portName; XsPortInfo portInfo(portName, XBR_115k2); @@ -182,9 +182,7 @@ int32_t RestoreCommunication::innerFunction(void) m_control->onRestoreCommunication(&m_portName, result); } else - { m_control->onRestoreCommunication(&m_portName, XRV_RESTORE_COMMUNICATION_STOPPED); - } m_isRestoring = false; stopThread(); @@ -196,7 +194,7 @@ int32_t RestoreCommunication::innerFunction(void) \param portInfo A port info to open. \return XRV_OK if was successful. */ -XsResultValue RestoreCommunication::openComPort(const XsPortInfo &portInfo) +XsResultValue RestoreCommunication::openComPort(const XsPortInfo& portInfo) { XsResultValue result = m_serialInterface->open(portInfo); if (result == XRV_ALREADYOPEN) diff --git a/lib/xspublic/xscontroller/restorecommunication.h b/lib/xspublic/xscontroller/restorecommunication.h index 69b8a4b..572c56a 100644 --- a/lib/xspublic/xscontroller/restorecommunication.h +++ b/lib/xspublic/xscontroller/restorecommunication.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -78,14 +78,14 @@ class RestoreCommunication : public CallbackManagerXda, protected xsens::Standar RestoreCommunication(XsControl* control); ~RestoreCommunication(); - XsResultValue start(const XsString &portName); + XsResultValue start(const XsString& portName); void stop(); protected: int32_t innerFunction(void) override; private: - XsResultValue openComPort(const XsPortInfo &portInfo); + XsResultValue openComPort(const XsPortInfo& portInfo); XsControl* m_control; SerialInterface* m_serialInterface; diff --git a/lib/xspublic/xscontroller/rx_tx_log.h b/lib/xspublic/xscontroller/rx_tx_log.h index e420411..3fe872b 100644 --- a/lib/xspublic/xscontroller/rx_tx_log.h +++ b/lib/xspublic/xscontroller/rx_tx_log.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -101,30 +101,30 @@ inline static void checkStateRx(char* state, int length, XsByteArray const& data { // find preamble int idx = 0; - while (idx < length-4) + while (idx < length - 4) { - if (data[idx] == 0xFA && (data[idx+1] == 0xFF || data[idx+1] == 0) && data[idx+3] == 0) + if (data[idx] == 0xFA && (data[idx + 1] == 0xFF || data[idx + 1] == 0) && data[idx + 3] == 0) { - switch (data[idx+2]) + switch (data[idx + 2]) { - case XMID_GotoConfigAck: - strcpy(state, "_Config"); - break; - case XMID_GotoMeasurementAck: - strcpy(state, "_Measurement"); - break; - case XMID_GotoOperationalAck: - strcpy(state, "_Operational"); - break; - case XMID_StartRecordingAck: - strcpy(state, "_Recording"); - break; - case XMID_StopRecordingAck: - strcpy(state, "_Flushing"); - break; - default: - ++idx; - continue; + case XMID_GotoConfigAck: + strcpy(state, "_Config"); + break; + case XMID_GotoMeasurementAck: + strcpy(state, "_Measurement"); + break; + case XMID_GotoOperationalAck: + strcpy(state, "_Operational"); + break; + case XMID_StartRecordingAck: + strcpy(state, "_Recording"); + break; + case XMID_StopRecordingAck: + strcpy(state, "_Flushing"); + break; + default: + ++idx; + continue; } break; } @@ -154,30 +154,30 @@ inline static void checkStateTx(char* state, int length, XsByteArray const& data { // find preamble int idx = 0; - while (idx < length-4) + while (idx < length - 4) { - if (data[idx] == 0xFA && (data[idx+1] == 0xFF || data[idx+1] == 0) && data[idx+3] == 0) + if (data[idx] == 0xFA && (data[idx + 1] == 0xFF || data[idx + 1] == 0) && data[idx + 3] == 0) { - switch (data[idx+2]) + switch (data[idx + 2]) { - case XMID_GotoConfig: - strcpy(state, "_Config"); - break; - case XMID_GotoMeasurement: - strcpy(state, "_Measurement"); - break; - case XMID_GotoOperational: - strcpy(state, "_Operational"); - break; - case XMID_StartRecording: - strcpy(state, "_Recording"); - break; - case XMID_StopRecording: - strcpy(state, "_Flushing"); - break; - default: - ++idx; - continue; + case XMID_GotoConfig: + strcpy(state, "_Config"); + break; + case XMID_GotoMeasurement: + strcpy(state, "_Measurement"); + break; + case XMID_GotoOperational: + strcpy(state, "_Operational"); + break; + case XMID_StartRecording: + strcpy(state, "_Recording"); + break; + case XMID_StopRecording: + strcpy(state, "_Flushing"); + break; + default: + ++idx; + continue; } break; } @@ -200,9 +200,9 @@ inline static void checkStateTx(char* state, int length, void const* data, XsFil checkStateTx(state, (int) (length), (data), (logfile)) #else - static const char state[1] = ""; - #define CHECK_STATE_RX(...) ((void)0) - #define CHECK_STATE_TX(...) ((void)0) +static const char state[1] = ""; +#define CHECK_STATE_RX(...) ((void)0) +#define CHECK_STATE_TX(...) ((void)0) #endif #endif diff --git a/lib/xspublic/xscontroller/scanner.cpp b/lib/xspublic/xscontroller/scanner.cpp index f0b75d4..a93476e 100644 --- a/lib/xspublic/xscontroller/scanner.cpp +++ b/lib/xspublic/xscontroller/scanner.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -65,19 +65,19 @@ #include "scanner.h" #ifdef _WIN32 -# include -# include -# include -# include -# include -# include -# include "idfetchhelpers.h" + #include + #include + #include + #include + #include + #include + #include "idfetchhelpers.h" #else -# include -# include -# include -# include "xslibusb.h" -# include "udev.h" + #include + #include + #include + #include "xslibusb.h" + #include "udev.h" #endif #include @@ -93,10 +93,11 @@ #include "enumerateusbdevices.h" #include -namespace XsScannerNamespace { - volatile std::atomic_bool abortPortScan{false}; - Scanner* gScanner = nullptr; - XsScanLogCallbackFunc gScanLogCallback = nullptr; +namespace XsScannerNamespace +{ +volatile std::atomic_bool abortPortScan{false}; +Scanner* gScanner = nullptr; +XsScanLogCallbackFunc gScanLogCallback = nullptr; } using namespace XsScannerNamespace; @@ -140,12 +141,12 @@ void Scanner::setScanLogCallback(XsScanLogCallbackFunc cb) \returns XRV_OK if successful */ -XsResultValue Scanner::fetchBasicInfo(XsPortInfo &portInfo, uint32_t singleScanTimeout, bool detectRs485) +XsResultValue Scanner::fetchBasicInfo(XsPortInfo& portInfo, uint32_t singleScanTimeout, bool detectRs485) { auto serial = Communicator::createUniquePtr(); auto usb = Communicator::createUniquePtr(); - SerialPortCommunicator *port = (portInfo.isUsb() ? usb.get() : serial.get()); + SerialPortCommunicator* port = (portInfo.isUsb() ? usb.get() : serial.get()); port->setGotoConfigTimeout(singleScanTimeout); LOGXSSCAN("Opening port " << portInfo.portName() << " (" << portInfo.portNumber() << ") @ " << portInfo.baudrate() << " baud, expected device " << portInfo.deviceId()); @@ -256,30 +257,38 @@ bool Scanner::xsScanPort(XsPortInfo& portInfo, XsBaudRate baud, uint32_t singleS LOGXSSCAN("Failed to find device"); return false; } - switch(baudrate) + switch (baudrate) { - default: - case XBR_115k2: - baudrate = XBR_2000k; break; - case XBR_2000k: - baudrate = XBR_921k6; break; - case XBR_921k6: - baudrate = XBR_460k8; break; - case XBR_460k8: - baudrate = XBR_230k4; break; - case XBR_230k4: - // On some systems a delay of about 100ms seems necessary to successfully perform the scan. - XsTime::msleep(100); - baudrate = XBR_57k6; break; - case XBR_57k6: - baudrate = XBR_38k4; break; - case XBR_38k4: - baudrate = XBR_19k2; break; - case XBR_19k2: - baudrate = XBR_9600; break; - case XBR_9600: - LOGXSSCAN("No more available baudrates, failed to find device"); - return false; // could not detect Xsens device, return false + default: + case XBR_115k2: + baudrate = XBR_2000k; + break; + case XBR_2000k: + baudrate = XBR_921k6; + break; + case XBR_921k6: + baudrate = XBR_460k8; + break; + case XBR_460k8: + baudrate = XBR_230k4; + break; + case XBR_230k4: + // On some systems a delay of about 100ms seems necessary to successfully perform the scan. + XsTime::msleep(100); + baudrate = XBR_57k6; + break; + case XBR_57k6: + baudrate = XBR_38k4; + break; + case XBR_38k4: + baudrate = XBR_19k2; + break; + case XBR_19k2: + baudrate = XBR_9600; + break; + case XBR_9600: + LOGXSSCAN("No more available baudrates, failed to find device"); + return false; // could not detect Xsens device, return false } LOGXSSCAN("Checking next baudrate: " << baudrate); } @@ -359,10 +368,10 @@ bool Scanner::xsFilterResponsiveDevices(XsPortInfoArray& ports, XsBaudRate baudr \param[in] hDevInfo The refernce to a device information \param[in] DeviceInfoData The pointer to a device information data */ -std::string Scanner::getDevicePath(HDEVINFO hDevInfo, SP_DEVINFO_DATA *DeviceInfoData) +std::string Scanner::getDevicePath(HDEVINFO hDevInfo, SP_DEVINFO_DATA* DeviceInfoData) { char deviceInstanceID[MAX_DEVICE_ID_LEN]; - SetupDiGetDeviceInstanceIdA(hDevInfo, DeviceInfoData,deviceInstanceID, MAX_DEVICE_ID_LEN, NULL); + SetupDiGetDeviceInstanceIdA(hDevInfo, DeviceInfoData, deviceInstanceID, MAX_DEVICE_ID_LEN, NULL); return std::string(deviceInstanceID); } #endif @@ -375,17 +384,17 @@ bool Scanner::isXsensUsbDevice(uint16_t vid, uint16_t pid) { switch (vid) { - // Xsens - case XSENS_VENDOR_ID: - // ignore the body pack serial port - return (pid != 0x0100); + // Xsens + case XSENS_VENDOR_ID: + // ignore the body pack serial port + return (pid != 0x0100); - // FTDI reserved PIDs - case FTDI_VENDOR_ID: - return (pid >= 0xd388 && pid <= 0xd38f); + // FTDI reserved PIDs + case FTDI_VENDOR_ID: + return (pid >= 0xd388 && pid <= 0xd38f); - default: - return false; + default: + return false; } } @@ -415,7 +424,7 @@ bool Scanner::xsEnumerateSerialPorts(XsPortInfoArray& ports, bool ignoreNonXsens // Enumerate through all devices in Set. DeviceInfoData.cbSize = sizeof(SP_DEVINFO_DATA); - for (i=0;!abortPortScan && SetupDiEnumDeviceInfo(hDevInfo,i,&DeviceInfoData);++i) + for (i = 0; !abortPortScan && SetupDiEnumDeviceInfo(hDevInfo, i, &DeviceInfoData); ++i) { // Get the registry key which stores the ports settings HKEY hDeviceKey = SetupDiOpenDevRegKey(hDevInfo, &DeviceInfoData, DICS_FLAG_GLOBAL, 0, DIREG_DEV, KEY_QUERY_VALUE); @@ -471,25 +480,25 @@ bool Scanner::xsEnumerateSerialPorts(XsPortInfoArray& ports, bool ignoreNonXsens #else // !_WIN32 Udev xsudev; - struct udev *udev = xsudev.unew(); + struct udev* udev = xsudev.unew(); if (udev) { - struct udev_enumerate *enumerate = xsudev.enumerate_new(udev); + struct udev_enumerate* enumerate = xsudev.enumerate_new(udev); xsudev.enumerate_add_match_subsystem(enumerate, "tty"); xsudev.enumerate_scan_devices(enumerate); - struct udev_list_entry *devices, *dev_list_entry; + struct udev_list_entry* devices, *dev_list_entry; devices = xsudev.enumerate_get_list_entry(enumerate); for (dev_list_entry = devices; dev_list_entry != nullptr; dev_list_entry = xsudev.list_entry_get_next(dev_list_entry)) { - const char *path = xsudev.list_entry_get_name(dev_list_entry); - auto devcleaner = [&](struct udev_device *d) + const char* path = xsudev.list_entry_get_name(dev_list_entry); + auto devcleaner = [&](struct udev_device * d) { xsudev.device_unref(d); }; std::unique_ptr - device(xsudev.device_new_from_syspath(udev, path), devcleaner); + device(xsudev.device_new_from_syspath(udev, path), devcleaner); if (!device) continue; @@ -497,12 +506,12 @@ bool Scanner::xsEnumerateSerialPorts(XsPortInfoArray& ports, bool ignoreNonXsens // this is probably a console, definitely not a physical serial port device continue; - struct udev_device *usbParentDevice = xsudev.device_get_parent_with_subsystem_devtype(device.get(), "usb", "usb_device"); + struct udev_device* usbParentDevice = xsudev.device_get_parent_with_subsystem_devtype(device.get(), "usb", "usb_device"); unsigned int vid = 0, pid = 0; if (usbParentDevice) { - const char *vendor = xsudev.device_get_sysattr_value(usbParentDevice, "idVendor"); - const char *product = xsudev.device_get_sysattr_value(usbParentDevice, "idProduct"); + const char* vendor = xsudev.device_get_sysattr_value(usbParentDevice, "idVendor"); + const char* product = xsudev.device_get_sysattr_value(usbParentDevice, "idProduct"); if (vendor && product) { @@ -524,7 +533,7 @@ bool Scanner::xsEnumerateSerialPorts(XsPortInfoArray& ports, bool ignoreNonXsens XsPortInfo portInfo; - const char *devnode = xsudev.device_get_devnode(device.get()); + const char* devnode = xsudev.device_get_devnode(device.get()); if (strlen(devnode) > 255 || strncmp(devnode, "/dev/ttyS", 9) == 0) continue; portInfo.setPortName(devnode); @@ -532,7 +541,7 @@ bool Scanner::xsEnumerateSerialPorts(XsPortInfoArray& ports, bool ignoreNonXsens if (usbParentDevice) { - const char *deviceidstring; + const char* deviceidstring; deviceidstring = xsudev.device_get_sysattr_value(usbParentDevice, "serial"); if (deviceidstring) { @@ -550,8 +559,8 @@ bool Scanner::xsEnumerateSerialPorts(XsPortInfoArray& ports, bool ignoreNonXsens else { (void)ignoreNonXsensDevices; - DIR *dir; - struct dirent *entry; + DIR* dir; + struct dirent* entry; if ((dir = opendir("/dev/")) == NULL) return false; @@ -573,11 +582,21 @@ bool Scanner::xsEnumerateSerialPorts(XsPortInfoArray& ports, bool ignoreNonXsens return true; } -/*! \brief Enaumerates a network device +/*! \brief Enumerates a network device + \param ports The port info array + \returns false +*/ +bool Scanner::xsEnumerateNetworkDevices(XsPortInfoArray& ports) +{ + (void)ports; + return false; +} + +/*! \brief Enumerates a bluetooth device \param ports The port info array \returns false */ -bool Scanner::xsEnumerateNetworkDevices(XsPortInfoArray & ports) +bool Scanner::xsEnumerateBluetoothDevices(XsPortInfoArray& ports) { (void)ports; return false; @@ -596,7 +615,7 @@ bool Scanner::xsEnumerateNetworkDevices(XsPortInfoArray & ports) \param[in] deviceInfoData The pointer to a device information data \returns Non-zero if successful */ -int Scanner::xsScanGetHubNumber(HDEVINFO hDevInfo, SP_DEVINFO_DATA *deviceInfoData) +int Scanner::xsScanGetHubNumber(HDEVINFO hDevInfo, SP_DEVINFO_DATA* deviceInfoData) { DWORD DataT; char buffer[256]; @@ -611,11 +630,9 @@ int Scanner::xsScanGetHubNumber(HDEVINFO hDevInfo, SP_DEVINFO_DATA *deviceInfoDa NULL)) { LOGXSSCAN("Registry access successful: \"" << buffer << "\""); - char const * hubString = strstr((char const *)buffer, HUB_SEARCH_STRING); + char const* hubString = strstr((char const*)buffer, HUB_SEARCH_STRING); if (hubString) - { result = strtol(hubString + strlen(HUB_SEARCH_STRING), 0, 10); - } } else LOGXSSCAN("Get Hub Number failed with error " << GetLastError()); @@ -637,10 +654,10 @@ XsPortInfo Scanner::xsScanPortByHubId(const char* id) // Get device interface info set handle for all devices attached to system HDEVINFO hDevInfo = SetupDiGetClassDevs( - &GUID_DEVCLASS_PORTS, /* CONST GUID * ClassGuid - USB class GUID */ - NULL, /* PCTSTR Enumerator */ - NULL, /* HWND hwndParent */ - DIGCF_PRESENT | DIGCF_DEVICEINTERFACE /* DWORD Flags */ + &GUID_DEVCLASS_PORTS, /* CONST GUID * ClassGuid - USB class GUID */ + NULL, /* PCTSTR Enumerator */ + NULL, /* HWND hwndParent */ + DIGCF_PRESENT | DIGCF_DEVICEINTERFACE /* DWORD Flags */ ); if (hDevInfo == INVALID_HANDLE_VALUE) @@ -657,14 +674,14 @@ XsPortInfo Scanner::xsScanPortByHubId(const char* id) devInterfaceData.cbSize = sizeof(SP_DEVICE_INTERFACE_DATA); XsPortInfo portInfo; int port = 0; - for (DWORD dwIndex = 0; port == 0; ++dwIndex) + for (DWORD dwIndex = 0; port == 0; ++dwIndex) //lint !e440 funky condition is ok { BOOL bRet = SetupDiEnumDeviceInterfaces( - hDevInfo, /* HDEVINFO DeviceInfoSet */ - NULL, /* PSP_DEVINFO_DATA DeviceInfoData */ - &GUID_DEVINTERFACE_SERENUM_BUS_ENUMERATOR, /* CONST GUID * InterfaceClassGuid */ - dwIndex, - &devInterfaceData /* PSP_DEVICE_INTERFACE_DATA DeviceInterfaceData */ + hDevInfo, /* HDEVINFO DeviceInfoSet */ + NULL, /* PSP_DEVINFO_DATA DeviceInfoData */ + &GUID_DEVINTERFACE_SERENUM_BUS_ENUMERATOR, /* CONST GUID * InterfaceClassGuid */ + dwIndex, + &devInterfaceData /* PSP_DEVICE_INTERFACE_DATA DeviceInterfaceData */ ); if (!bRet) { @@ -673,7 +690,7 @@ XsPortInfo Scanner::xsScanPortByHubId(const char* id) } else { - size_t buffer[1024/sizeof(size_t)]; // we use size_t instead of char to ensure proper data alignment + size_t buffer[1024 / sizeof(size_t)]; // we use size_t instead of char to ensure proper data alignment SP_DEVINFO_DATA diData; diData.cbSize = sizeof(SP_DEVINFO_DATA); @@ -748,10 +765,10 @@ bool Scanner::xsScanXsensUsbHubs(XsIntArray& hubs, XsPortInfoArray& ports) // Get device interface info set handle for all devices attached to system HDEVINFO hDevInfo = SetupDiGetClassDevs( - &GUID_DEVINTERFACE_USB_DEVICE, /* CONST GUID * ClassGuid - USB class GUID */ - NULL, /* PCTSTR Enumerator */ - NULL, /* HWND hwndParent */ - DIGCF_PRESENT | DIGCF_DEVICEINTERFACE /* DWORD Flags */ + &GUID_DEVINTERFACE_USB_DEVICE, /* CONST GUID * ClassGuid - USB class GUID */ + NULL, /* PCTSTR Enumerator */ + NULL, /* HWND hwndParent */ + DIGCF_PRESENT | DIGCF_DEVICEINTERFACE /* DWORD Flags */ ); if (hDevInfo == INVALID_HANDLE_VALUE) @@ -769,11 +786,11 @@ bool Scanner::xsScanXsensUsbHubs(XsIntArray& hubs, XsPortInfoArray& ports) for (DWORD dwIndex = 0; ; ++dwIndex) { BOOL bRet = SetupDiEnumDeviceInterfaces( - hDevInfo, /* HDEVINFO DeviceInfoSet */ - NULL, /* PSP_DEVINFO_DATA DeviceInfoData */ - &GUID_DEVINTERFACE_USB_DEVICE, /* CONST GUID * InterfaceClassGuid */ - dwIndex, - &devInterfaceData /* PSP_DEVICE_INTERFACE_DATA DeviceInterfaceData */ + hDevInfo, /* HDEVINFO DeviceInfoSet */ + NULL, /* PSP_DEVINFO_DATA DeviceInfoData */ + &GUID_DEVINTERFACE_USB_DEVICE, /* CONST GUID * InterfaceClassGuid */ + dwIndex, + &devInterfaceData /* PSP_DEVICE_INTERFACE_DATA DeviceInterfaceData */ ); if (!bRet) { @@ -782,8 +799,8 @@ bool Scanner::xsScanXsensUsbHubs(XsIntArray& hubs, XsPortInfoArray& ports) continue; } - size_t tmp1[1024/sizeof(size_t)]; - size_t tmp2[1024/sizeof(size_t)]; + size_t tmp1[1024 / sizeof(size_t)]; + size_t tmp2[1024 / sizeof(size_t)]; char* buffer = (char*) tmp1; SP_DEVINFO_DATA diData; diData.cbSize = sizeof(SP_DEVINFO_DATA); @@ -798,7 +815,7 @@ bool Scanner::xsScanXsensUsbHubs(XsIntArray& hubs, XsPortInfoArray& ports) DWORD dataT; if (SetupDiGetDeviceRegistryPropertyA(hDevInfo, &diData, SPDRP_MFG, &dataT, (PBYTE)buffer, 256, NULL)) { - if (_strnicmp(buffer,"xsens",5)) // if this is NOT an xsens device, ignore it + if (_strnicmp(buffer, "xsens", 5)) // if this is NOT an xsens device, ignore it break; } else @@ -884,7 +901,7 @@ XsUsbHubInfo Scanner::xsScanUsbHub(const XsPortInfo& portInfo) // Enumerate through all devices in Set. devInfoData.cbSize = sizeof(SP_DEVINFO_DATA); - for (DWORD i=0; behindXsensHub == ILLEGAL_HUB && SetupDiEnumDeviceInfo(hDevInfo,i,&devInfoData); ++i) + for (DWORD i = 0; behindXsensHub == ILLEGAL_HUB && SetupDiEnumDeviceInfo(hDevInfo, i, &devInfoData); ++i) { DWORD dataT; char buffer[256]; @@ -896,12 +913,12 @@ XsUsbHubInfo Scanner::xsScanUsbHub(const XsPortInfo& portInfo) // if (!SetupDiGetDeviceRegistryPropertyA(hDevInfo, - &devInfoData, - SPDRP_MFG, - &dataT, - (PBYTE)buffer, - 256, - NULL)) + &devInfoData, + SPDRP_MFG, + &dataT, + (PBYTE)buffer, + 256, + NULL)) continue; // Get the registry key which stores the ports settings @@ -957,33 +974,34 @@ XsUsbHubInfo Scanner::xsScanUsbHub(const XsPortInfo& portInfo) Udev xsudev; - udev *udevInstance = xsudev.unew(); - if (udevInstance == NULL) { + udev* udevInstance = xsudev.unew(); + if (udevInstance == NULL) + { fprintf(stderr, "Unable to create udev object\n"); return XsUsbHubInfo(); } - udev_enumerate *enumerate = xsudev.enumerate_new(udevInstance); + udev_enumerate* enumerate = xsudev.enumerate_new(udevInstance); xsudev.enumerate_scan_devices(enumerate); - udev_list_entry *devices = xsudev.enumerate_get_list_entry(enumerate); - udev_list_entry *dev; + udev_list_entry* devices = xsudev.enumerate_get_list_entry(enumerate); + udev_list_entry* dev; for (dev = devices; dev != NULL; dev = xsudev.list_entry_get_next(dev)) { - const char *path = xsudev.list_entry_get_name(dev); - udev_device *device = xsudev.device_new_from_syspath(udevInstance, path); + const char* path = xsudev.list_entry_get_name(dev); + udev_device* device = xsudev.device_new_from_syspath(udevInstance, path); if (!device) return XsUsbHubInfo(); - const char *devnode = xsudev.device_get_devnode(device); + const char* devnode = xsudev.device_get_devnode(device); if (!devnode || strcmp(devnode, portInfo.portName_c_str()) != 0) continue; - udev_device *parent = xsudev.device_get_parent_with_subsystem_devtype(device, "usb", "usb_device"); + udev_device* parent = xsudev.device_get_parent_with_subsystem_devtype(device, "usb", "usb_device"); if (!parent) break; - const char *devpath = xsudev.device_get_sysattr_value(parent, "devpath"); + const char* devpath = xsudev.device_get_sysattr_value(parent, "devpath"); if (!devpath) break; diff --git a/lib/xspublic/xscontroller/scanner.h b/lib/xspublic/xscontroller/scanner.h index 423b290..42e4f44 100644 --- a/lib/xspublic/xscontroller/scanner.h +++ b/lib/xspublic/xscontroller/scanner.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -77,7 +77,7 @@ struct XsIntArray; #ifdef _WIN32 -# include + #include #endif class Scanner @@ -91,20 +91,21 @@ class Scanner Scanner& scanner() const; }; - XsResultValue fetchBasicInfo(XsPortInfo &portInfo, uint32_t singleScanTimeout, bool detectRs485); + XsResultValue fetchBasicInfo(XsPortInfo& portInfo, uint32_t singleScanTimeout, bool detectRs485); bool xsScanPort(XsPortInfo& portInfo, XsBaudRate baud, uint32_t singleScanTimeout, bool detectRs485); virtual bool xsScanPorts(XsPortInfoArray& ports, XsBaudRate baudrate, uint32_t singleScanTimeout, bool ignoreNonXsensDevices, bool detectRs485); bool xsFilterResponsiveDevices(XsPortInfoArray& ports, XsBaudRate baudrate, uint32_t singleScanTimeout, bool detectRs485); #ifdef _WIN32 - static std::string getDevicePath(HDEVINFO hDevInfo, SP_DEVINFO_DATA *DeviceInfoData); - int xsScanGetHubNumber(HDEVINFO hDevInfo, SP_DEVINFO_DATA *deviceInfoData); + static std::string getDevicePath(HDEVINFO hDevInfo, SP_DEVINFO_DATA* DeviceInfoData); + int xsScanGetHubNumber(HDEVINFO hDevInfo, SP_DEVINFO_DATA* deviceInfoData); XsPortInfo xsScanPortByHubId(const char* id); bool xsScanXsensUsbHubs(XsIntArray& hubs, XsPortInfoArray& ports); #endif static bool isXsensUsbDevice(uint16_t vid, uint16_t pid); bool xsEnumerateSerialPorts(XsPortInfoArray& ports, bool ignoreNonXsensDevices); virtual bool xsEnumerateNetworkDevices(XsPortInfoArray& ports); + virtual bool xsEnumerateBluetoothDevices(XsPortInfoArray& ports); XsUsbHubInfo xsScanUsbHub(const XsPortInfo& portInfo); @@ -113,9 +114,9 @@ class Scanner namespace XsScannerNamespace { - extern volatile std::atomic_bool abortPortScan; - extern Scanner* gScanner; - extern XsScanLogCallbackFunc gScanLogCallback; +extern volatile std::atomic_bool abortPortScan; +extern Scanner* gScanner; +extern XsScanLogCallbackFunc gScanLogCallback; } #if 1 diff --git a/lib/xspublic/xscontroller/scenariomatchpred.h b/lib/xspublic/xscontroller/scenariomatchpred.h index 9c149e6..ecfa780 100644 --- a/lib/xspublic/xscontroller/scenariomatchpred.h +++ b/lib/xspublic/xscontroller/scenariomatchpred.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -71,14 +71,14 @@ /*! \brief Returns true if \a lhs has the same type as \a rhs */ -inline bool scenarioTypeMatches(const XsFilterProfile &lhs, const XsFilterProfile &rhs) +inline bool scenarioTypeMatches(const XsFilterProfile& lhs, const XsFilterProfile& rhs) { return lhs.type() == rhs.type(); } /*! \brief Returns true if \a lhs has the same type as \a rhs */ -inline bool operator ==(const XsFilterProfile &lhs, const XsFilterProfile &rhs) +inline bool operator ==(const XsFilterProfile& lhs, const XsFilterProfile& rhs) { return scenarioTypeMatches(lhs, rhs); } @@ -86,12 +86,13 @@ inline bool operator ==(const XsFilterProfile &lhs, const XsFilterProfile &rhs) /*! \class ScenarioMatchPred \brief The class that compares two scenarios for matching */ -class ScenarioMatchPred { +class ScenarioMatchPred +{ public: /*! \brief Default constructor */ - ScenarioMatchPred(const XsFilterProfile &scenario) + ScenarioMatchPred(const XsFilterProfile& scenario) : m_scenarioType(scenario.type()) { } @@ -107,7 +108,7 @@ class ScenarioMatchPred { /*! \param s : The filter profile to compare \returns true if \a s matches */ - bool operator()(const XsFilterProfile &s) const + bool operator()(const XsFilterProfile& s) const { return s.type() == m_scenarioType; } @@ -115,7 +116,7 @@ class ScenarioMatchPred { /*! \param s : The filter profile to compare \returns true if \a s matches */ - bool operator()(const XsFilterProfile *s) const + bool operator()(const XsFilterProfile* s) const { return s->type() == m_scenarioType; } diff --git a/lib/xspublic/xscontroller/serialcommunicator.cpp b/lib/xspublic/xscontroller/serialcommunicator.cpp index 5dbc447..ca24917 100644 --- a/lib/xspublic/xscontroller/serialcommunicator.cpp +++ b/lib/xspublic/xscontroller/serialcommunicator.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -85,8 +85,8 @@ */ SerialCommunicator::SerialCommunicator() : m_thread(*this, *this) - , m_firmwareRevision(0,0,0) - , m_hardwareRevision(0,0) + , m_firmwareRevision(0, 0, 0) + , m_hardwareRevision(0, 0) { messageExtractor().clearBuffer(); startPollThread(); @@ -177,7 +177,7 @@ XsResultValue SerialCommunicator::gotoConfig(bool detectRs485) /*! \brief Write raw data to the open COM or USB port */ -XsResultValue SerialCommunicator::writeRawData(const XsByteArray &data) +XsResultValue SerialCommunicator::writeRawData(const XsByteArray& data) { if (!isPortOpen()) return XRV_NOPORTOPEN; @@ -225,7 +225,7 @@ void SerialCommunicator::closePort() } /*! \returns whether the communicator has an open COM or USB port -\*/ + \*/ bool SerialCommunicator::isPortOpen() const { return m_streamInterface && m_streamInterface->isOpen(); @@ -240,7 +240,7 @@ XsPortInfo SerialCommunicator::portInfo() const /*! \brief Open a serial port and return the main device connected to it */ -bool SerialCommunicator::openPort(const XsPortInfo &portInfo, OpenPortStage stage, bool detectRs485) +bool SerialCommunicator::openPort(const XsPortInfo& portInfo, OpenPortStage stage, bool detectRs485) { JLDEBUGG("Opening " << portInfo << " stage " << stage << " configtimeout " << gotoConfigTimeout()); XSEXITLOGN(gJournal); @@ -310,9 +310,7 @@ bool SerialCommunicator::openPort(const XsPortInfo &portInfo, OpenPortStage stag return true; } else - { return false; - } } @@ -375,7 +373,7 @@ void SerialCommunicator::setDoGotoConfig(bool doit) \param other The communicator of an other device \note This is true if the USB hub matches. */ -bool SerialCommunicator::isDockedAt(Communicator *other) const +bool SerialCommunicator::isDockedAt(Communicator* other) const { XsUsbHubInfo thisHub = XsScanner::scanUsbHub(XsPortInfo(portInfo().portName())); XsUsbHubInfo otherHub = XsScanner::scanUsbHub(XsPortInfo(other->portInfo().portName())); @@ -403,17 +401,17 @@ XsResultValue SerialCommunicator::readDataToBuffer(XsByteArray& raw) switch (res) { - // all intended fall-throughs - case XRV_UNEXPECTED_DISCONNECT: - if (masterDevice() != nullptr) - masterDevice()->onConnectionLost(); + // all intended fall-throughs + case XRV_UNEXPECTED_DISCONNECT: + if (masterDevice() != nullptr) + masterDevice()->onConnectionLost(); //lint -fallthrough - case XRV_NOFILEORPORTOPEN: - closePort(); - break; + case XRV_NOFILEORPORTOPEN: + closePort(); + break; - default: - break; + default: + break; } return res; @@ -423,7 +421,7 @@ XsResultValue SerialCommunicator::readDataToBuffer(XsByteArray& raw) \note Overridden here for implementation of DataParser::handleMessage \param msg The XsMessage to handle */ -void SerialCommunicator::handleMessage(const XsMessage &msg) +void SerialCommunicator::handleMessage(const XsMessage& msg) { DeviceCommunicator::handleMessage(msg); } @@ -434,7 +432,7 @@ void SerialCommunicator::handleMessage(const XsMessage &msg) \details This function will read all present messages in the read buffer. In order for this function to work, you need to call readDataToBuffer() first. \returns The messages that were read. - */ +*/ XsResultValue SerialCommunicator::processBufferedData(const XsByteArray& rawIn, std::deque& messages) { return extractMessages(rawIn, messages); diff --git a/lib/xspublic/xscontroller/serialcommunicator.h b/lib/xspublic/xscontroller/serialcommunicator.h index 8ac2901..758db50 100644 --- a/lib/xspublic/xscontroller/serialcommunicator.h +++ b/lib/xspublic/xscontroller/serialcommunicator.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -80,17 +80,17 @@ class SerialCommunicator : public DeviceCommunicator, public DataParser XsResultValue gotoConfig(bool detectRs485 = false) override; XsResultValue gotoMeasurement() override; - void handleMessage(const XsMessage &msg) override; + void handleMessage(const XsMessage& msg) override; void flushPort() override; void closePort() override; bool isPortOpen() const override; XsPortInfo portInfo() const override; - bool openPort(const XsPortInfo &portInfo, OpenPortStage stage = OPS_Full, bool detectRs485 = false) override; + bool openPort(const XsPortInfo& portInfo, OpenPortStage stage = OPS_Full, bool detectRs485 = false) override; bool reopenPort(OpenPortStage stage = OPS_Full, bool skipDeviceIdCheck = false) override; - bool isDockedAt(Communicator *other) const override; + bool isDockedAt(Communicator* other) const override; - XsResultValue writeRawData(const XsByteArray &data) override; + XsResultValue writeRawData(const XsByteArray& data) override; XsVersion firmwareRevision(); XsVersion hardwareRevision(); @@ -102,7 +102,7 @@ class SerialCommunicator : public DeviceCommunicator, public DataParser \param pi The port information to use \returns The shared pointer to a stream interface */ - virtual std::shared_ptr createStreamInterface(const XsPortInfo &pi) = 0; + virtual std::shared_ptr createStreamInterface(const XsPortInfo& pi) = 0; XsResultValue readDataToBuffer(XsByteArray& raw) override; XsResultValue processBufferedData(const XsByteArray& rawIn, std::deque& messages) override; @@ -116,7 +116,10 @@ class SerialCommunicator : public DeviceCommunicator, public DataParser /*! \returns The default interface timeout */ - virtual uint32_t defaultInterfaceTimeout() const { return 0; } + virtual uint32_t defaultInterfaceTimeout() const + { + return 0; + } XsPortInfo m_activePortInfo; //!< The information about the port this communicator is currently connected to diff --git a/lib/xspublic/xscontroller/serialinterface.cpp b/lib/xspublic/xscontroller/serialinterface.cpp index 370a229..9c90159 100644 --- a/lib/xspublic/xscontroller/serialinterface.cpp +++ b/lib/xspublic/xscontroller/serialinterface.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -69,26 +69,26 @@ #include #ifndef _WIN32 -# include // close -# include // ioctl -# include // open, O_RDWR -# include // strcpy -# include -# include -# include -#if !defined(__APPLE__) && !defined(ANDROID) -# include -#endif + #include // close + #include // ioctl + #include // open, O_RDWR + #include // strcpy + #include + #include + #include + #if !defined(__APPLE__) && !defined(ANDROID) + #include + #endif #else -# include -# include + #include + #include #endif #ifndef _CRT_SECURE_NO_DEPRECATE -# define _CRT_SECURE_NO_DEPRECATE -# ifdef _WIN32 -# pragma warning(disable:4996) -# endif + #define _CRT_SECURE_NO_DEPRECATE + #ifdef _WIN32 + #pragma warning(disable:4996) + #endif #endif ////////////////////////////////////////////////////////////////////////////////////////// @@ -119,7 +119,7 @@ SerialInterface::~SerialInterface() { closeLive(); } - catch(...) + catch (...) { } } @@ -147,16 +147,17 @@ XsResultValue SerialInterface::closeLive(void) // read all data before closing the handle, a Flush is not enough for FTDI devices unfortunately // we first need to set the COMM timeouts to instantly return when no more data is available COMMTIMEOUTS cto; - if (::GetCommTimeouts(m_handle,&cto)) + if (::GetCommTimeouts(m_handle, &cto)) { cto.ReadIntervalTimeout = MAXDWORD; cto.ReadTotalTimeoutConstant = 0; cto.ReadTotalTimeoutMultiplier = 0; - if (::SetCommTimeouts(m_handle,&cto)) + if (::SetCommTimeouts(m_handle, &cto)) { char buffer[1024]; DWORD length; - do { + do + { if (!::ReadFile(m_handle, buffer, 1024, &length, NULL)) break; } while (length > 0); @@ -189,7 +190,7 @@ XsResultValue SerialInterface::closeLive(void) \param state Contains the new state of the control lines. \returns XRV_OK if the function succeeded */ -XsResultValue SerialInterface::escape (const XsControlLine mask, const XsControlLine state) +XsResultValue SerialInterface::escape(const XsControlLine mask, const XsControlLine state) { if (!isOpen()) return (m_lastResult = XRV_NOPORTOPEN); @@ -198,17 +199,17 @@ XsResultValue SerialInterface::escape (const XsControlLine mask, const XsControl if (mask & XCL_DTR) { if (state & XCL_DTR) - rv = EscapeCommFunction(m_handle,SETDTR); + rv = EscapeCommFunction(m_handle, SETDTR); else - rv = EscapeCommFunction(m_handle,CLRDTR); + rv = EscapeCommFunction(m_handle, CLRDTR); } if (mask & XCL_RTS) { if (state & XCL_RTS) - rv = EscapeCommFunction(m_handle,SETRTS); + rv = EscapeCommFunction(m_handle, SETRTS); else - rv = EscapeCommFunction(m_handle,CLRRTS); + rv = EscapeCommFunction(m_handle, CLRRTS); } if (rv) return m_lastResult = XRV_OK; @@ -254,7 +255,7 @@ XsResultValue SerialInterface::escape (const XsControlLine mask, const XsControl \note This function tries to send and receive any remaining data immediately and does not return until the buffers are empty. */ -XsResultValue SerialInterface::flushData (void) +XsResultValue SerialInterface::flushData(void) { m_lastResult = XRV_OK; #ifdef _WIN32 @@ -283,7 +284,7 @@ XsIoHandle SerialInterface::getHandle(void) const } //! Retrieve the port number that was last successfully opened. -uint16_t SerialInterface::getPortNumber (void) const +uint16_t SerialInterface::getPortNumber(void) const { return m_port; } @@ -301,13 +302,13 @@ XsResultValue SerialInterface::getLastResult(void) const } //! Return the current timeout value -uint32_t SerialInterface::getTimeout (void) const +uint32_t SerialInterface::getTimeout(void) const { return m_timeout; } //! Return whether the communication port is open or not. -bool SerialInterface::isOpen (void) const +bool SerialInterface::isOpen(void) const { #ifdef _WIN32 return m_handle != INVALID_HANDLE_VALUE; @@ -339,9 +340,9 @@ static T setBitsEnabled(T field, T bits, bool cond) \returns XRV_OK if the device was opened successfully */ XsResultValue SerialInterface::open(const XsPortInfo& portInfo, - XsFilePos readBufSize, - XsFilePos writeBufSize, - PortOptions options) + XsFilePos readBufSize, + XsFilePos writeBufSize, + PortOptions options) { m_endTime = 0; @@ -385,16 +386,16 @@ XsResultValue SerialInterface::open(const XsPortInfo& portInfo, commState.BaudRate = (DWORD) portInfo.baudrate(); // Setup the baud rate commState.Parity = NOPARITY; // Setup the Parity commState.ByteSize = 8; // Setup the data bits - commState.StopBits = (options&PO_TwoStopBits)?TWOSTOPBITS:ONESTOPBIT; + commState.StopBits = (options & PO_TwoStopBits) ? TWOSTOPBITS : ONESTOPBIT; // Setup flow control - commState.fDsrSensitivity = (options&PO_DtrDsrFlowControl)?TRUE:FALSE; - commState.fOutxDsrFlow = (options&PO_DtrDsrFlowControl)?DTR_CONTROL_HANDSHAKE:DTR_CONTROL_DISABLE; + commState.fDsrSensitivity = (options & PO_DtrDsrFlowControl) ? TRUE : FALSE; + commState.fOutxDsrFlow = (options & PO_DtrDsrFlowControl) ? DTR_CONTROL_HANDSHAKE : DTR_CONTROL_DISABLE; commState.fOutxCtsFlow = (options & PO_RtsCtsFlowControl || (portInfo.linesOptions() & XPLO_RtsCtsFlowControl)) ? TRUE : FALSE; commState.fRtsControl = (options & PO_RtsCtsFlowControl || (portInfo.linesOptions() & XPLO_RtsCtsFlowControl)) ? RTS_CONTROL_HANDSHAKE : RTS_CONTROL_ENABLE; - commState.fOutX = (options&PO_XonXoffFlowControl)?TRUE:FALSE; + commState.fOutX = (options & PO_XonXoffFlowControl) ? TRUE : FALSE; commState.fInX = commState.fOutX; if (!SetCommState(m_handle, (LPDCB)&commState)) // Set new state @@ -443,7 +444,8 @@ XsResultValue SerialInterface::open(const XsPortInfo& portInfo, // O_NOCTTY: Raw input, no "controlling terminal" // O_NDELAY: Don't care about DCD signal - if (m_handle < 0) { + if (m_handle < 0) + { // Port not open return m_lastResult = XRV_INPUTCANNOTBEOPENED; } @@ -470,29 +472,29 @@ XsResultValue SerialInterface::open(const XsPortInfo& portInfo, // Enable the receiver and set local mode m_commState.c_cflag |= (CLOCAL | CREAD); // Set character size to data bits and set no parity Mask the characte size bits - m_commState.c_cflag &= ~(CSIZE|PARENB|PARODD); + m_commState.c_cflag &= ~(CSIZE | PARENB | PARODD); m_commState.c_cflag |= CS8; // Select 8 data bits - m_commState.c_cflag = setBitsEnabled(m_commState.c_cflag, (tcflag_t)CSTOPB, (options&PO_TwoStopBits) == PO_TwoStopBits); + m_commState.c_cflag = setBitsEnabled(m_commState.c_cflag, (tcflag_t)CSTOPB, (options & PO_TwoStopBits) == PO_TwoStopBits); // Hardware flow control - m_commState.c_cflag = setBitsEnabled(m_commState.c_cflag, (tcflag_t)CRTSCTS, (options&PO_RtsCtsFlowControl) == PO_RtsCtsFlowControl); + m_commState.c_cflag = setBitsEnabled(m_commState.c_cflag, (tcflag_t)CRTSCTS, (options & PO_RtsCtsFlowControl) == PO_RtsCtsFlowControl); #ifdef CDTRDSR - m_commState.c_cflag = setBitsEnabled(m_commState.c_cflag, (tcflag_t)CDTRDSR, (options&PO_DtrDsrFlowControl) == PO_DtrDsrFlowControl); + m_commState.c_cflag = setBitsEnabled(m_commState.c_cflag, (tcflag_t)CDTRDSR, (options & PO_DtrDsrFlowControl) == PO_DtrDsrFlowControl); #endif - m_commState.c_lflag &= ~(ECHO|ECHOE|ECHOK|ECHONL|ICANON|ISIG|IEXTEN); + m_commState.c_lflag &= ~(ECHO | ECHOE | ECHOK | ECHONL | ICANON | ISIG | IEXTEN); // Software flow control - m_commState.c_iflag &= ~(IGNBRK|BRKINT|PARMRK|INPCK|ISTRIP|INLCR|IGNCR|ICRNL); - m_commState.c_iflag = setBitsEnabled(m_commState.c_iflag, (tcflag_t)(IXON|IXOFF), options&PO_XonXoffFlowControl); + m_commState.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | INPCK | ISTRIP | INLCR | IGNCR | ICRNL); + m_commState.c_iflag = setBitsEnabled(m_commState.c_iflag, (tcflag_t)(IXON | IXOFF), options & PO_XonXoffFlowControl); // Set Raw output m_commState.c_oflag &= ~OPOST; // Timeout 0.001 sec for first byte, read minimum of 0 bytes m_commState.c_cc[VMIN] = 0; - m_commState.c_cc[VTIME] = (m_timeout+99)/100; // 1 + m_commState.c_cc[VTIME] = (m_timeout + 99) / 100; // 1 // Set the new options for the port - if (tcsetattr(m_handle,TCSANOW, &m_commState) != 0) + if (tcsetattr(m_handle, TCSANOW, &m_commState) != 0) return XRV_INPUTCANNOTBEOPENED; #if defined(JLDEF_BUILD) && JLDEF_BUILD <= JLL_ALERT @@ -506,19 +508,19 @@ XsResultValue SerialInterface::open(const XsPortInfo& portInfo, if (cfgetospeed(&checkCommState) != portInfo.baudrate()) JLALERTG("Set baudrate doesn't match requested baudrate"); - if (options&PO_RtsCtsFlowControl && !(checkCommState.c_cflag&CRTSCTS)) + if (options & PO_RtsCtsFlowControl && !(checkCommState.c_cflag & CRTSCTS)) JLALERTG("Requested RTS/CTS flow control, but could not be set."); - if (options&PO_DtrDsrFlowControl && + if (options & PO_DtrDsrFlowControl && #ifdef CDTRDSR - !(checkCommState.c_cflag&CDTRDSR) + !(checkCommState.c_cflag & CDTRDSR) #else false #endif - ) + ) JLALERTG("Requested DTR/DSR flow control, but could not be set."); - if (options&PO_XonXoffFlowControl && !((checkCommState.c_iflag&(IXON|IXOFF)) == (IXON|IXOFF))) + if (options & PO_XonXoffFlowControl && !((checkCommState.c_iflag & (IXON | IXOFF)) == (IXON | IXOFF))) JLALERTG("Requested Xon/Xoff flow control, but could not be set."); #endif // JLDEF_BUILD < JLL_ALERT @@ -547,17 +549,13 @@ XsResultValue SerialInterface::open(const XsPortInfo& portInfo, // setting RTS and DTR; int cmbits; if (ioctl(m_handle, TIOCMGET, &cmbits) < 0) - { JLDEBUGG("TIOCMGET failed, which is OK for USB connected MkIV devices"); - } // Port Lines Options applyHwControlLinesOptions(options, portInfo.linesOptions(), cmbits); if (ioctl(m_handle, TIOCMSET, &cmbits) < 0) - { JLDEBUGG("TIOCMSET failed, which is OK for USB connected MkIV devices"); - } #endif // !_WIN32 JLDEBUGG("Port " << portInfo.portName().toStdString() << " opened"); @@ -581,7 +579,7 @@ XsResultValue SerialInterface::readData(XsFilePos maxLength, XsByteArray& data) DWORD length; data.setSize((XsSize) maxLength); BOOL rres = ::ReadFile(m_handle, data.data(), (DWORD) maxLength, &length, NULL); - data.pop_back((XsSize) (maxLength-length)); + data.pop_back((XsSize)(maxLength - length)); JLTRACEG("ReadFile result " << rres << ", length " << length); if (!rres) @@ -606,7 +604,7 @@ XsResultValue SerialInterface::readData(XsFilePos maxLength, XsByteArray& data) FD_SET(m_handle, &fd); FD_SET(m_handle, &err); - timeout.tv_sec = m_timeout/1000; + timeout.tv_sec = m_timeout / 1000; timeout.tv_usec = (m_timeout - (timeout.tv_sec * 1000)) * 1000; int res = select(FD_SETSIZE, &fd, NULL, &err, &timeout); @@ -614,7 +612,9 @@ XsResultValue SerialInterface::readData(XsFilePos maxLength, XsByteArray& data) { data.clear(); return (m_lastResult = XRV_ERROR); - } else if (res == 0) { + } + else if (res == 0) + { data.clear(); return (m_lastResult = XRV_TIMEOUT); } @@ -622,9 +622,7 @@ XsResultValue SerialInterface::readData(XsFilePos maxLength, XsByteArray& data) data.setSize(maxLength); int length = read(m_handle, (void*)data.data(), maxLength); if (length > 0) - { data.pop_back(maxLength - length); - } else { int err = errno; @@ -632,17 +630,17 @@ XsResultValue SerialInterface::readData(XsFilePos maxLength, XsByteArray& data) data.clear(); switch (err) { - case EAGAIN: + case EAGAIN: #if defined(EWOULDBLOCK) && EWOULDBLOCK != EAGAIN - case EWOULDBLOCK: + case EWOULDBLOCK: #endif - return XRV_TIMEOUT; + return XRV_TIMEOUT; - case EIO: - return XRV_UNEXPECTED_DISCONNECT; + case EIO: + return XRV_UNEXPECTED_DISCONNECT; - default: - break; + default: + break; } } #if defined(JLDEF_BUILD) && JLDEF_BUILD <= JLL_TRACE && !defined(ANDROID) @@ -670,7 +668,7 @@ XsResultValue SerialInterface::readData(XsFilePos maxLength, XsByteArray& data) #ifdef _WIN32 sprintf(fname, "rx_%03d_%d.log", (int32_t) m_port, m_baudrate); #else - char *devname = strrchr(m_portname, '/'); + char* devname = strrchr(m_portname, '/'); sprintf(fname, "rx_%s_%d.log", devname + 1, XsBaud::rateToNumeric(m_baudrate)); #endif makeFilenameUnique(fname, state); @@ -696,7 +694,7 @@ XsResultValue SerialInterface::readData(XsFilePos maxLength, XsByteArray& data) \param ms The new timeout in milliseconds \returns XRV_OK if the function succeeded */ -XsResultValue SerialInterface::setTimeout (const uint32_t ms) +XsResultValue SerialInterface::setTimeout(const uint32_t ms) { JLDEBUGG("Setting timeout to " << ms << " ms"); @@ -705,7 +703,7 @@ XsResultValue SerialInterface::setTimeout (const uint32_t ms) // Set COM timeouts COMMTIMEOUTS commTimeouts; - if (!GetCommTimeouts(m_handle,&commTimeouts)) // Fill CommTimeouts structure + if (!GetCommTimeouts(m_handle, &commTimeouts)) // Fill CommTimeouts structure return m_lastResult = XRV_ERROR; // immediate return if data is available, wait 1ms otherwise @@ -719,7 +717,7 @@ XsResultValue SerialInterface::setTimeout (const uint32_t ms) } else { - // immediate return whether data is available or not + // immediate return whether data is available or not commTimeouts.ReadIntervalTimeout = MAXDWORD; commTimeouts.ReadTotalTimeoutConstant = 0; commTimeouts.ReadTotalTimeoutMultiplier = 0; @@ -732,11 +730,11 @@ XsResultValue SerialInterface::setTimeout (const uint32_t ms) #else // Timeout 0.1 sec for first byte, read minimum of 0 bytes m_commState.c_cc[VMIN] = 0; - m_commState.c_cc[VTIME] = (m_timeout+99)/100; // ds time + m_commState.c_cc[VTIME] = (m_timeout + 99) / 100; // ds time // Set the new options for the port if it is open if (isOpen()) - tcsetattr(m_handle,TCSANOW, &m_commState); + tcsetattr(m_handle, TCSANOW, &m_commState); #endif return (m_lastResult = XRV_OK); } @@ -760,7 +758,7 @@ XsResultValue SerialInterface::waitForData(XsFilePos maxLength, XsByteArray& dat uint32_t timeout = m_timeout; uint32_t eTime = XsTime_getTimeOfDay(NULL, NULL) + timeout; -// uint32_t newLength = 0; + // uint32_t newLength = 0; while (((XsFilePos) data.size() < maxLength) && (XsTime_getTimeOfDay(NULL, NULL) <= eTime)) { @@ -812,18 +810,18 @@ XsResultValue SerialInterface::writeData(const XsByteArray& data, XsFilePos* wri *written = 0; switch (err) { - case EAGAIN: + case EAGAIN: #if defined(EWOULDBLOCK) && EWOULDBLOCK != EAGAIN - case EWOULDBLOCK: + case EWOULDBLOCK: #endif - return XRV_TIMEOUT; + return XRV_TIMEOUT; - case EIO: - return XRV_UNEXPECTED_DISCONNECT; + case EIO: + return XRV_UNEXPECTED_DISCONNECT; - /* we don't expect any other errors to actually occur */ - default: - break; + /* we don't expect any other errors to actually occur */ + default: + break; } } @@ -843,8 +841,8 @@ XsResultValue SerialInterface::writeData(const XsByteArray& data, XsFilePos* wri #ifdef _WIN32 sprintf(fname, "tx_%03d_%d.log", (int32_t) m_port, m_baudrate); #else - char *devname = strrchr(m_portname, '/'); - sprintf(fname,"tx_%s_%d.log", devname + 1, XsBaud::rateToNumeric(m_baudrate)); + char* devname = strrchr(m_portname, '/'); + sprintf(fname, "tx_%s_%d.log", devname + 1, XsBaud::rateToNumeric(m_baudrate)); #endif makeFilenameUnique(fname, state); @@ -864,8 +862,8 @@ XsResultValue SerialInterface::writeData(const XsByteArray& data, XsFilePos* wri void SerialInterface::cancelIo() const { #ifdef _WIN32 - /* This function is only available on Windows Vista and higher. - When a read action hangs, this function can cancel IO operations from another thread. + /* This function is only available on Windows Vista and higher. + When a read action hangs, this function can cancel IO operations from another thread. */ //CancelIoEx(m_handle, NULL); #endif @@ -901,7 +899,7 @@ void SerialInterface::applyHwControlLinesOptions(PortOptions options, int portLi #ifdef _WIN32 // DTR Line Options - if (!(options&PO_DtrDsrFlowControl)) + if (!(options & PO_DtrDsrFlowControl)) { // Flow Control is disabled if ((pLinesOpts == XPLO_Invalid) || (pLinesOpts & XPLO_DTR_Ignore)) @@ -937,7 +935,7 @@ void SerialInterface::applyHwControlLinesOptions(PortOptions options, int portLi } // RTS Line Options - if (!(options&PO_RtsCtsFlowControl)) + if (!(options & PO_RtsCtsFlowControl)) { // Flow Control is disabled if ((pLinesOpts != XPLO_Invalid) && !(pLinesOpts & XPLO_RTS_Ignore)) @@ -967,7 +965,7 @@ void SerialInterface::applyHwControlLinesOptions(PortOptions options, int portLi #else // DTR Line - if (!(options&PO_DtrDsrFlowControl)) + if (!(options & PO_DtrDsrFlowControl)) { // Flow control is disabled if ((pLinesOpts != XPLO_Invalid) && !(pLinesOpts & XPLO_DTR_Ignore)) @@ -986,7 +984,7 @@ void SerialInterface::applyHwControlLinesOptions(PortOptions options, int portLi } // RTS Line - if (!(options&PO_RtsCtsFlowControl)) + if (!(options & PO_RtsCtsFlowControl)) { if ((pLinesOpts != XPLO_Invalid) && !(pLinesOpts & XPLO_RTS_Ignore)) { diff --git a/lib/xspublic/xscontroller/serialinterface.h b/lib/xspublic/xscontroller/serialinterface.h index 533fe61..888cc74 100644 --- a/lib/xspublic/xscontroller/serialinterface.h +++ b/lib/xspublic/xscontroller/serialinterface.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -79,7 +79,8 @@ struct XsPortInfo; /*! \brief The low-level serial communication class. */ -class SerialInterface : public StreamInterface { +class SerialInterface : public StreamInterface +{ private: void applyHwControlLinesOptions(PortOptions options, int portLinesOptions, int& p); @@ -87,38 +88,38 @@ class SerialInterface : public StreamInterface { XsFile rx_log; XsFile tx_log; protected: - //! The baudrate that was last set to be used by the port + //! The baudrate that was last set to be used by the port XsBaudRate m_baudrate; - //! The time at which an operation will end in ms, used by several functions. + //! The time at which an operation will end in ms, used by several functions. uint32_t m_endTime; - //! The last result of an operation + //! The last result of an operation mutable XsResultValue m_lastResult; - //! The opened COM port nr + //! The opened COM port nr uint16_t m_port; - //! The name of the open serial port + //! The name of the open serial port char m_portname[32]; /*! The default timeout value to use during blocking operations. A value of 0 means that all operations become non-blocking. */ uint32_t m_timeout; - #ifdef _WIN32 - XsIoHandle m_handle; //!< The serial port handle, also indicates if the port is open or not. - #else - termios m_commState; //!< Stored settings about the serial port - int32_t m_handle; //!< The serial port handle, also indicates if the port is open or not. - #endif +#ifdef _WIN32 + XsIoHandle m_handle; //!< The serial port handle, also indicates if the port is open or not. +#else + termios m_commState; //!< Stored settings about the serial port + int32_t m_handle; //!< The serial port handle, also indicates if the port is open or not. +#endif public: SerialInterface(); virtual ~SerialInterface(); // Function overrides - XsResultValue close (void) override; + XsResultValue close(void) override; XsResultValue closeLive(void); - XsResultValue flushData (void) override; - bool isOpen (void) const; + XsResultValue flushData(void) override; + bool isOpen(void) const; XsResultValue getLastResult(void) const; - XsResultValue writeData (const XsByteArray& data, XsFilePos* written = 0) override; + XsResultValue writeData(const XsByteArray& data, XsFilePos* written = 0) override; XsResultValue readData(XsFilePos maxLength, XsByteArray& data) override; void cancelIo(void) const; @@ -127,15 +128,15 @@ class SerialInterface : public StreamInterface { XsResultValue escape(XsControlLine mask, XsControlLine state); XsBaudRate getBaudrate(void) const; XsIoHandle getHandle(void) const; - uint16_t getPortNumber (void) const; + uint16_t getPortNumber(void) const; void getPortName(XsString& portname) const; - uint32_t getTimeout (void) const; + uint32_t getTimeout(void) const; - XsResultValue open ( const XsPortInfo& portInfo, XsFilePos readBufSize = XS_DEFAULT_READ_BUFFER_SIZE, XsFilePos writeBufSize = XS_DEFAULT_WRITE_BUFFER_SIZE, PortOptions options = PO_XsensDefaults) override; - XsResultValue setTimeout (uint32_t ms); - XsResultValue waitForData (XsFilePos maxLength, XsByteArray& data) override; + XsResultValue open(const XsPortInfo& portInfo, XsFilePos readBufSize = XS_DEFAULT_READ_BUFFER_SIZE, XsFilePos writeBufSize = XS_DEFAULT_WRITE_BUFFER_SIZE, PortOptions options = PO_XsensDefaults) override; + XsResultValue setTimeout(uint32_t ms); + XsResultValue waitForData(XsFilePos maxLength, XsByteArray& data) override; -XSENS_DISABLE_COPY(SerialInterface); + XSENS_DISABLE_COPY(SerialInterface); }; SerialInterface::PortOptions operator|(SerialInterface::PortOptions lhs, SerialInterface::PortOptions rhs); diff --git a/lib/xspublic/xscontroller/serialportcommunicator.cpp b/lib/xspublic/xscontroller/serialportcommunicator.cpp index d33167a..6d753c3 100644 --- a/lib/xspublic/xscontroller/serialportcommunicator.cpp +++ b/lib/xspublic/xscontroller/serialportcommunicator.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -66,12 +66,12 @@ #include /*! \class SerialPortCommunicator -\brief A class that uses serial port to communicate + \brief A class that uses serial port to communicate */ /*! \brief Constructs new SerialPortCommunicator */ -Communicator *SerialPortCommunicator::construct() +Communicator* SerialPortCommunicator::construct() { return new SerialPortCommunicator; } @@ -90,14 +90,14 @@ SerialPortCommunicator::~SerialPortCommunicator() \param pi The port to use \returns The shared pointer to a stream interface */ -std::shared_ptr SerialPortCommunicator::createStreamInterface(const XsPortInfo &pi) +std::shared_ptr SerialPortCommunicator::createStreamInterface(const XsPortInfo& pi) { assert(!pi.isUsb() && !pi.isNetwork()); std::shared_ptr stream(new SerialInterface()); SerialInterface::PortOptions options = SerialInterface::PO_XsensDefaults; if (pi.deviceId() == XS_DID_ABMCLOCKMASTER) - options = SerialInterface::PO_RtsCtsFlowControl|SerialInterface::PO_OneStopBit; + options = SerialInterface::PO_RtsCtsFlowControl | SerialInterface::PO_OneStopBit; setLastResult(stream->open(pi, 65536, 65536, options)); return stream; diff --git a/lib/xspublic/xscontroller/serialportcommunicator.h b/lib/xspublic/xscontroller/serialportcommunicator.h index 37a2fdb..1eb7091 100644 --- a/lib/xspublic/xscontroller/serialportcommunicator.h +++ b/lib/xspublic/xscontroller/serialportcommunicator.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -70,12 +70,12 @@ class SerialPortCommunicator : public SerialCommunicator { public: - static Communicator *construct(); + static Communicator* construct(); SerialPortCommunicator(); protected: ~SerialPortCommunicator(); - std::shared_ptr createStreamInterface(const XsPortInfo &pi) override; + std::shared_ptr createStreamInterface(const XsPortInfo& pi) override; }; #endif diff --git a/lib/xspublic/xscontroller/simpleprotocolmanager.h b/lib/xspublic/xscontroller/simpleprotocolmanager.h index 569c0a6..00bb7a6 100644 --- a/lib/xspublic/xscontroller/simpleprotocolmanager.h +++ b/lib/xspublic/xscontroller/simpleprotocolmanager.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xscontroller/streaminterface.cpp b/lib/xspublic/xscontroller/streaminterface.cpp index cb00e6a..778253d 100644 --- a/lib/xspublic/xscontroller/streaminterface.cpp +++ b/lib/xspublic/xscontroller/streaminterface.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xscontroller/streaminterface.h b/lib/xspublic/xscontroller/streaminterface.h index 5fecc69..25ff392 100644 --- a/lib/xspublic/xscontroller/streaminterface.h +++ b/lib/xspublic/xscontroller/streaminterface.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xscontroller/supportedsyncsettings.cpp b/lib/xspublic/xscontroller/supportedsyncsettings.cpp index c3752ed..1e8808c 100644 --- a/lib/xspublic/xscontroller/supportedsyncsettings.cpp +++ b/lib/xspublic/xscontroller/supportedsyncsettings.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -79,66 +79,36 @@ XsSyncSettingArray supportedSyncSettings(XsDeviceId const& deviceId) { if (deviceId.isAwindaXDongle() || deviceId.isBodyPack()) - { return XsSyncSettingArray(); - } if (deviceId.isAwindaX()) - { return supportedSyncSettingsForAwindaBaseStation(); - } else if (deviceId.isSyncStationX()) - { return supportedSyncSettingsForAwindaBaseStation(); - } else if (deviceId.isMtMark5() && deviceId.isMtigX10()) - { return supportedSyncSettingsForMark5MtigX10Device(); - } else if (deviceId.isMtig()) - { return supportedSyncSettingsForMtigDevice(); - } else if (deviceId.isMtiX() && deviceId.isGnss()) - { return supportedSyncSettingsForMti7Device(); - } - else if (deviceId.isMti6X0() && deviceId.isRtk()) - { - return supportedSyncSettingsForMti680Device(); - } + else if (deviceId.isMti6X0() && deviceId.isGnss() && deviceId.hasInternalGnss()) + return supportedSyncSettingsForMt6x0IntGnssDevice(); else if (deviceId.isMti6X0() && deviceId.isGnss()) - { - return supportedSyncSettingsForMti670Device(); - } + return supportedSyncSettingsForMt6x0GnssDevice(); else if (deviceId.isMti6X0()) - { return supportedSyncSettingsForMt6x0Device(); - } else if (deviceId.isMtiX() || deviceId.isMti3X0()) - { return supportedSyncSettingsForMtiXDevice(); - } else if (deviceId.isMti() || deviceId.isMtig()) - { return supportedSyncSettingsForMtiDevice(); - } else if (deviceId.isGnss()) - { return supportedSyncSettingsForGnssDevice(); - } else if (deviceId.isMtx2()) - { return supportedSyncSettingsForMtx2Device(); - } else if (deviceId.isMtw2()) - { return supportedSyncSettingsForMtx2Device(); - } else - { return XsSyncSettingArray(); - } } /*! \returns true if the specified device id supports sync settings, false otherwise @@ -153,7 +123,7 @@ bool supportsSyncSettings(XsDeviceId const& deviceId) /*! \brief Return true if \a setting1 is compatible with \a setting2 for a device with \a deviceId */ -bool isCompatibleSyncSetting(XsDeviceId const& deviceId, XsSyncSetting const & setting1, XsSyncSetting const & setting2) +bool isCompatibleSyncSetting(XsDeviceId const& deviceId, XsSyncSetting const& setting1, XsSyncSetting const& setting2) { if (deviceId.isAwindaX()) return isAwindaSettingCompatible(setting1, setting2); @@ -334,8 +304,8 @@ XsSyncSettingArray supportedSyncSettingsForMt6x0Device() return settings; } -/*! \brief get list of supported synchronizations settings for an Mti670Device */ -XsSyncSettingArray supportedSyncSettingsForMti670Device() +/*! \brief get list of supported synchronizations settings for any MTi-6x0, non-rugged devices with external GNSS support, such as the 670 */ +XsSyncSettingArray supportedSyncSettingsForMt6x0GnssDevice() { XsSyncSettingArray settings = supportedSyncSettingsForMt6x0Device(); @@ -356,8 +326,8 @@ XsSyncSettingArray supportedSyncSettingsForMti670Device() return settings; } -/*! \brief get list of supported synchronizations settings for an Mti680Device */ -XsSyncSettingArray supportedSyncSettingsForMti680Device() +/*! \brief get list of supported synchronizations settings for any MTi-6x0G, rugged devices with internal GNSS, such as the 680G */ +XsSyncSettingArray supportedSyncSettingsForMt6x0IntGnssDevice() { XsSyncSetting s; XsSyncSettingArray settings; @@ -407,13 +377,6 @@ XsSyncSettingArray supportedSyncSettingsForMti680Device() s.m_skipFirst = 0; s.m_triggerOnce = 0; - s.m_function = XSF_ClockBiasEstimation; - s.m_offset = 0; - s.m_pulseWidth = 0; - s.m_clockPeriod = 1; - s.m_line = XSL_In3; - settings.push_back(s); - s.m_function = XSF_StartSampling; s.m_skipFactor = 0; s.m_clockPeriod = 0; @@ -463,7 +426,7 @@ XsSyncSettingArray supportedSyncSettingsForMtigDevice() for (auto it = settings.begin(); it != settings.end(); ++it) { //insert a copy of existing XSF_ClockBiasEstimation with gps line setting - if(it->m_function == XSF_ClockBiasEstimation) + if (it->m_function == XSF_ClockBiasEstimation) { XsSyncSetting s = *it; s.m_line = xsl4ToXsl(XSL4_GnssClockIn); @@ -622,7 +585,7 @@ XsSyncSettingArray supportedSyncSettingsForAwindaBaseStation() inputSyncFunctions.insert(XSF_ResetTimer); inputSyncFunctions.insert(XSF_TriggerIndication); - for (XsSyncFunction const & syncFunction : inputSyncFunctions) + for (XsSyncFunction const& syncFunction : inputSyncFunctions) { s.m_line = XSL_In1; s.m_polarity = XSP_RisingEdge; @@ -656,7 +619,7 @@ XsSyncSettingArray supportedSyncSettingsForAwindaBaseStation() outputSyncFunctions.insert(XSF_GotoOperational); outputSyncFunctions.insert(XSF_IntervalTransitionMeasurement); outputSyncFunctions.insert(XSF_IntervalTransitionRecording); - for (XsSyncFunction const & syncFunction : outputSyncFunctions) + for (XsSyncFunction const& syncFunction : outputSyncFunctions) { s.m_line = XSL_Out1; s.m_polarity = XSP_RisingEdge; @@ -691,7 +654,7 @@ XsSyncSettingArray supportedSyncSettingsForAwindaBaseStation() Basically this checks that if a specific function is configured for In1 and In2 (or Out1 and Out2), then the other settings of both should match. This is because this get combined into one function with the line: both setting */ -bool isAwindaSettingCompatible(XsSyncSetting const & setting1, XsSyncSetting const & setting2) +bool isAwindaSettingCompatible(XsSyncSetting const& setting1, XsSyncSetting const& setting2) { bool isCompatible = true; if (setting1.m_function == setting2.m_function) @@ -709,9 +672,7 @@ bool isAwindaSettingCompatible(XsSyncSetting const & setting1, XsSyncSetting con setting1.m_skipFactor != setting2.m_skipFactor || setting1.m_clockPeriod != setting2.m_clockPeriod || setting1.m_triggerOnce != setting2.m_triggerOnce) - { isCompatible = false; - } } } } @@ -722,7 +683,7 @@ bool isAwindaSettingCompatible(XsSyncSetting const & setting1, XsSyncSetting con \note This *very* specific for a sync station \sa isAwindaSettingCompatible */ -bool isSyncStationSettingCompatible(XsSyncSetting const & setting1, XsSyncSetting const & setting2) +bool isSyncStationSettingCompatible(XsSyncSetting const& setting1, XsSyncSetting const& setting2) { return isAwindaSettingCompatible(setting1, setting2); } diff --git a/lib/xspublic/xscontroller/supportedsyncsettings.h b/lib/xspublic/xscontroller/supportedsyncsettings.h index 636ad79..2bf0b18 100644 --- a/lib/xspublic/xscontroller/supportedsyncsettings.h +++ b/lib/xspublic/xscontroller/supportedsyncsettings.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -73,32 +73,32 @@ struct XsSyncSettingArray; namespace Synchronization { - bool supportsSyncSettings(XsDeviceId const& deviceId); - XsSyncSettingArray supportedSyncSettings(XsDeviceId const& deviceId); +bool supportsSyncSettings(XsDeviceId const& deviceId); +XsSyncSettingArray supportedSyncSettings(XsDeviceId const& deviceId); - XsSyncSettingArray supportedSyncSettingsForMtx2Device(); - XsSyncSettingArray supportedSyncSettingsForMtiDevice(); - XsSyncSettingArray supportedSyncSettingsForMti7Device(); - XsSyncSettingArray supportedSyncSettingsForMti670Device(); - XsSyncSettingArray supportedSyncSettingsForMti680Device(); - XsSyncSettingArray supportedSyncSettingsForMtigDevice(); - XsSyncSettingArray supportedSyncSettingsForMark5MtigX10Device(); - XsSyncSettingArray supportedSyncSettingsForMtDevice(); - XsSyncSettingArray supportedSyncSettingsForMt6x0Device(); - XsSyncSettingArray supportedSyncSettingsForGnssDevice(); - XsSyncSettingArray supportedSyncSettingsForAwindaBaseStation(); - XsSyncSettingArray supportedSyncSettingsForMtiXDevice(); +XsSyncSettingArray supportedSyncSettingsForMtx2Device(); +XsSyncSettingArray supportedSyncSettingsForMtiDevice(); +XsSyncSettingArray supportedSyncSettingsForMti7Device(); +XsSyncSettingArray supportedSyncSettingsForMtigDevice(); +XsSyncSettingArray supportedSyncSettingsForMark5MtigX10Device(); +XsSyncSettingArray supportedSyncSettingsForMtDevice(); +XsSyncSettingArray supportedSyncSettingsForMt6x0Device(); +XsSyncSettingArray supportedSyncSettingsForMt6x0GnssDevice(); +XsSyncSettingArray supportedSyncSettingsForMt6x0IntGnssDevice(); +XsSyncSettingArray supportedSyncSettingsForGnssDevice(); +XsSyncSettingArray supportedSyncSettingsForAwindaBaseStation(); +XsSyncSettingArray supportedSyncSettingsForMtiXDevice(); - bool isCompatibleSyncSetting(XsDeviceId const& deviceId, XsSyncSetting const & setting1, XsSyncSetting const & setting2); - unsigned int timeResolutionInMicroseconds(XsDeviceId const& deviceId); +bool isCompatibleSyncSetting(XsDeviceId const& deviceId, XsSyncSetting const& setting1, XsSyncSetting const& setting2); +unsigned int timeResolutionInMicroseconds(XsDeviceId const& deviceId); - bool isAwindaSettingCompatible(XsSyncSetting const & setting1, XsSyncSetting const & setting2); - bool isSyncStationSettingCompatible(XsSyncSetting const & setting1, XsSyncSetting const & setting2); +bool isAwindaSettingCompatible(XsSyncSetting const& setting1, XsSyncSetting const& setting2); +bool isSyncStationSettingCompatible(XsSyncSetting const& setting1, XsSyncSetting const& setting2); - unsigned int awindaTimeResolutionInMicroseconds(); - unsigned int syncStationTimeResolutionInMicroseconds(); - unsigned int mtiTimeResolutionInMicroseconds(); - unsigned int mtx2TimeResolutionInMicroseconds(); +unsigned int awindaTimeResolutionInMicroseconds(); +unsigned int syncStationTimeResolutionInMicroseconds(); +unsigned int mtiTimeResolutionInMicroseconds(); +unsigned int mtx2TimeResolutionInMicroseconds(); } #endif diff --git a/lib/xspublic/xscontroller/synclinegmt.c b/lib/xspublic/xscontroller/synclinegmt.c index a310201..8d115a0 100644 --- a/lib/xspublic/xscontroller/synclinegmt.c +++ b/lib/xspublic/xscontroller/synclinegmt.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -73,20 +73,30 @@ XsSyncLine xslgmtToXsl(SyncLineGmt mk4Line) { switch (mk4Line) { - case XSLGMT_ClockIn: return XSL_ClockIn; - case XSLGMT_GnssClockIn: return XSL_GnssClockIn; - case XSLGMT_ReqData: return XSL_ReqData; - case XSLGMT_In1: return XSL_In1; - case XSLGMT_Out: return XSL_Out1; - case XSLGMT_BiIn: return XSL_Bi1In; - case XSLGMT_BiOut: return XSL_Bi1Out; - case XSLGMT_Gnss1Pps: return XSL_Gnss1Pps; - case XSLGMT_In2: return XSL_In2; - case XSLGMT_In3: return XSL_In3; + case XSLGMT_ClockIn: + return XSL_ClockIn; + case XSLGMT_GnssClockIn: + return XSL_GnssClockIn; + case XSLGMT_ReqData: + return XSL_ReqData; + case XSLGMT_In1: + return XSL_In1; + case XSLGMT_Out: + return XSL_Out1; + case XSLGMT_BiIn: + return XSL_Bi1In; + case XSLGMT_BiOut: + return XSL_Bi1Out; + case XSLGMT_Gnss1Pps: + return XSL_Gnss1Pps; + case XSLGMT_In2: + return XSL_In2; + case XSLGMT_In3: + return XSL_In3; - case XSLGMT_Invalid: - default: - return XSL_Invalid; + case XSLGMT_Invalid: + default: + return XSL_Invalid; } } @@ -95,24 +105,34 @@ SyncLineGmt xslToXslgmt(XsSyncLine line) { switch (line) { - case XSL_ClockIn: return XSLGMT_ClockIn; - case XSL_GnssClockIn: return XSLGMT_GnssClockIn; - case XSL_In1: return XSLGMT_In1; - case XSL_Out1: return XSLGMT_Out; - case XSL_ReqData: return XSLGMT_ReqData; - case XSL_Bi1In: return XSLGMT_BiIn; - case XSL_Bi1Out: return XSLGMT_BiOut; - case XSL_Gnss1Pps: return XSLGMT_Gnss1Pps; - case XSL_In2: return XSLGMT_In2; - case XSL_In3: return XSLGMT_In3; + case XSL_ClockIn: + return XSLGMT_ClockIn; + case XSL_GnssClockIn: + return XSLGMT_GnssClockIn; + case XSL_In1: + return XSLGMT_In1; + case XSL_Out1: + return XSLGMT_Out; + case XSL_ReqData: + return XSLGMT_ReqData; + case XSL_Bi1In: + return XSLGMT_BiIn; + case XSL_Bi1Out: + return XSLGMT_BiOut; + case XSL_Gnss1Pps: + return XSLGMT_Gnss1Pps; + case XSL_In2: + return XSLGMT_In2; + case XSL_In3: + return XSLGMT_In3; - case XSL_CtsIn: - case XSL_Invalid: - case XSL_ExtTimepulseIn: - case XSL_Out2: - case XSL_RtsOut: - default: - return XSLGMT_Invalid; + case XSL_CtsIn: + case XSL_Invalid: + case XSL_ExtTimepulseIn: + case XSL_Out2: + case XSL_RtsOut: + default: + return XSLGMT_Invalid; } } diff --git a/lib/xspublic/xscontroller/synclinegmt.h b/lib/xspublic/xscontroller/synclinegmt.h index 893922f..0798e66 100644 --- a/lib/xspublic/xscontroller/synclinegmt.h +++ b/lib/xspublic/xscontroller/synclinegmt.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -72,9 +72,9 @@ */ /*! \brief Synchronization line identifiers for the generic motion tracker (GMT) devices, only to be used directly in Xbus messages Applies to these devices: - * MTx2 - * MTw2 - * MTi-1 series + MTx2 + MTw2 + MTi-1 series */ enum SyncLineGmt { diff --git a/lib/xspublic/xscontroller/synclinemk4.c b/lib/xspublic/xscontroller/synclinemk4.c index 2a0b377..1aa6589 100644 --- a/lib/xspublic/xscontroller/synclinemk4.c +++ b/lib/xspublic/xscontroller/synclinemk4.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -73,18 +73,26 @@ XsSyncLine xsl4ToXsl(SyncLineMk4 mk4Line) { switch (mk4Line) { - case XSL4_ClockIn: return XSL_ClockIn; - case XSL4_GnssClockIn: return XSL_GnssClockIn; - case XSL4_ReqData: return XSL_ReqData; - case XSL4_In: return XSL_In1; - case XSL4_BiIn: return XSL_Bi1In; - case XSL4_BiOut: return XSL_Bi1Out; - case XSL4_ExtTimepulseIn: return XSL_ExtTimepulseIn; - case XSL4_Gnss1Pps: return XSL_Gnss1Pps; + case XSL4_ClockIn: + return XSL_ClockIn; + case XSL4_GnssClockIn: + return XSL_GnssClockIn; + case XSL4_ReqData: + return XSL_ReqData; + case XSL4_In: + return XSL_In1; + case XSL4_BiIn: + return XSL_Bi1In; + case XSL4_BiOut: + return XSL_Bi1Out; + case XSL4_ExtTimepulseIn: + return XSL_ExtTimepulseIn; + case XSL4_Gnss1Pps: + return XSL_Gnss1Pps; - case XSL4_Invalid: - default: - return XSL_Invalid; + case XSL4_Invalid: + default: + return XSL_Invalid; } } @@ -93,24 +101,32 @@ SyncLineMk4 xslToXsl4(XsSyncLine line) { switch (line) { - case XSL_ClockIn: return XSL4_ClockIn; - case XSL_GnssClockIn: return XSL4_GnssClockIn; - case XSL_In1: return XSL4_In; - case XSL_ReqData: return XSL4_ReqData; - case XSL_Bi1In: return XSL4_BiIn; - case XSL_Bi1Out: return XSL4_BiOut; - case XSL_ExtTimepulseIn: return XSL4_ExtTimepulseIn; - case XSL_Gnss1Pps: return XSL4_Gnss1Pps; + case XSL_ClockIn: + return XSL4_ClockIn; + case XSL_GnssClockIn: + return XSL4_GnssClockIn; + case XSL_In1: + return XSL4_In; + case XSL_ReqData: + return XSL4_ReqData; + case XSL_Bi1In: + return XSL4_BiIn; + case XSL_Bi1Out: + return XSL4_BiOut; + case XSL_ExtTimepulseIn: + return XSL4_ExtTimepulseIn; + case XSL_Gnss1Pps: + return XSL4_Gnss1Pps; - case XSL_In2: - case XSL_In3: - case XSL_CtsIn: - case XSL_Invalid: - case XSL_Out1: - case XSL_Out2: - case XSL_RtsOut: - default: - return XSL4_Invalid; + case XSL_In2: + case XSL_In3: + case XSL_CtsIn: + case XSL_Invalid: + case XSL_Out1: + case XSL_Out2: + case XSL_RtsOut: + default: + return XSL4_Invalid; } } diff --git a/lib/xspublic/xscontroller/synclinemk4.h b/lib/xspublic/xscontroller/synclinemk4.h index 3213cac..33a35bf 100644 --- a/lib/xspublic/xscontroller/synclinemk4.h +++ b/lib/xspublic/xscontroller/synclinemk4.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xscontroller/udev.cpp b/lib/xspublic/xscontroller/udev.cpp index c2874a7..5df75cf 100644 --- a/lib/xspublic/xscontroller/udev.cpp +++ b/lib/xspublic/xscontroller/udev.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -114,7 +114,7 @@ void Udev::initLibrary() \returns a new udev library context */ -udev *Udev::unew(void) +udev* Udev::unew(void) { if (m_uDev.unew) return m_uDev.unew(); @@ -128,7 +128,7 @@ udev *Udev::unew(void) If the refcount reaches zero, the resources of the context will be released. */ -udev *Udev::unref(struct udev *udev) +udev* Udev::unref(struct udev* udev) { if (m_uDev.unref) return m_uDev.unref(udev); @@ -143,7 +143,7 @@ udev *Udev::unref(struct udev *udev) \param udev_device udev device \return NULL */ -udev_device *Udev::device_unref(struct udev_device *udev_device) +udev_device* Udev::device_unref(struct udev_device* udev_device) { if (m_uDev.device_unref) return m_uDev.device_unref(udev_device); @@ -156,7 +156,7 @@ udev_device *Udev::device_unref(struct udev_device *udev_device) \return an enumeration context. */ -udev_enumerate *Udev::enumerate_new(struct udev *udev) +udev_enumerate* Udev::enumerate_new(struct udev* udev) { if (m_uDev.enumerate_new) return m_uDev.enumerate_new(udev); @@ -169,7 +169,7 @@ udev_enumerate *Udev::enumerate_new(struct udev *udev) \param subsystem filter for a subsystem of the device to include in the list \return: 0 on success, otherwise a negative error value. */ -int Udev::enumerate_add_match_subsystem(struct udev_enumerate *udev_enumerate, const char *subsystem) +int Udev::enumerate_add_match_subsystem(struct udev_enumerate* udev_enumerate, const char* subsystem) { if (m_uDev.enumerate_add_match_subsystem) return m_uDev.enumerate_add_match_subsystem(udev_enumerate, subsystem); @@ -181,7 +181,7 @@ int Udev::enumerate_add_match_subsystem(struct udev_enumerate *udev_enumerate, c \param udev_enumerate udev enumeration context \return 0 on success, otherwise a negative error value. */ -int Udev::enumerate_scan_devices(struct udev_enumerate *udev_enumerate) +int Udev::enumerate_scan_devices(struct udev_enumerate* udev_enumerate) { if (m_uDev.enumerate_scan_devices) return m_uDev.enumerate_scan_devices(udev_enumerate); @@ -194,7 +194,7 @@ int Udev::enumerate_scan_devices(struct udev_enumerate *udev_enumerate) \param list_entry current entry \return udev_list_entry, NULL if no more entries are available. */ -udev_list_entry *Udev::list_entry_get_next(struct udev_list_entry *list_entry) +udev_list_entry* Udev::list_entry_get_next(struct udev_list_entry* list_entry) { if (m_uDev.list_entry_get_next) return m_uDev.list_entry_get_next(list_entry); @@ -206,7 +206,7 @@ udev_list_entry *Udev::list_entry_get_next(struct udev_list_entry *list_entry) \param udev_enumerate context \return a udev_list_entry. */ -udev_list_entry *Udev::enumerate_get_list_entry(struct udev_enumerate *udev_enumerate) +udev_list_entry* Udev::enumerate_get_list_entry(struct udev_enumerate* udev_enumerate) { if (m_uDev.enumerate_get_list_entry) return m_uDev.enumerate_get_list_entry(udev_enumerate); @@ -222,7 +222,7 @@ udev_list_entry *Udev::enumerate_get_list_entry(struct udev_enumerate *udev_enum \return: NULL */ -udev_enumerate *Udev::enumerate_unref(struct udev_enumerate *udev_enumerate) +udev_enumerate* Udev::enumerate_unref(struct udev_enumerate* udev_enumerate) { if (m_uDev.enumerate_unref) return m_uDev.enumerate_unref(udev_enumerate); @@ -234,7 +234,7 @@ udev_enumerate *Udev::enumerate_unref(struct udev_enumerate *udev_enumerate) \param list_entry: current entry \return the name string of this entry. */ -const char *Udev::list_entry_get_name(struct udev_list_entry *list_entry) +const char* Udev::list_entry_get_name(struct udev_list_entry* list_entry) { if (m_uDev.list_entry_get_name) return m_uDev.list_entry_get_name(list_entry); @@ -250,7 +250,7 @@ const char *Udev::list_entry_get_name(struct udev_list_entry *list_entry) \param syspath sys device path including sys directory \return a new udev device, or NULL, if it does not exist */ -udev_device *Udev::device_new_from_syspath(struct udev *udev, const char *syspath) +udev_device* Udev::device_new_from_syspath(struct udev* udev, const char* syspath) { if (m_uDev.device_new_from_syspath) return m_uDev.device_new_from_syspath(udev, syspath); @@ -268,7 +268,7 @@ udev_device *Udev::device_new_from_syspath(struct udev *udev, const char *syspat \param udev_device: the device to start searching from \return a new udev device, or NULL, if it no parent exist. */ -udev_device *Udev::device_get_parent(struct udev_device *udev_device) +udev_device* Udev::device_get_parent(struct udev_device* udev_device) { if (m_uDev.device_get_parent) return m_uDev.device_get_parent(udev_device); @@ -283,7 +283,7 @@ udev_device *Udev::device_get_parent(struct udev_device *udev_device) \param udev_device udev device \return the device node file name of the udev device, or NULL if no device node exists */ -const char *Udev::device_get_devnode(struct udev_device *udev_device) +const char* Udev::device_get_devnode(struct udev_device* udev_device) { if (m_uDev.device_get_devnode) return m_uDev.device_get_devnode(udev_device); @@ -304,7 +304,7 @@ const char *Udev::device_get_devnode(struct udev_device *udev_device) \param devtype the type (DEVTYPE) of the device \return a new udev device, or NULL if no matching parent exists. */ -udev_device *Udev::device_get_parent_with_subsystem_devtype(struct udev_device *udev_device, const char *subsystem, const char *devtype) +udev_device* Udev::device_get_parent_with_subsystem_devtype(struct udev_device* udev_device, const char* subsystem, const char* devtype) { if (m_uDev.device_get_parent_with_subsystem_devtype) return m_uDev.device_get_parent_with_subsystem_devtype(udev_device, subsystem, devtype); @@ -321,7 +321,7 @@ udev_device *Udev::device_get_parent_with_subsystem_devtype(struct udev_device * \return the content of a sys attribute file, or NULL if there is no sys attribute value. */ -const char *Udev::device_get_sysattr_value(struct udev_device *udev_device, const char *sysattr) +const char* Udev::device_get_sysattr_value(struct udev_device* udev_device, const char* sysattr) { if (m_uDev.device_get_sysattr_value) return m_uDev.device_get_sysattr_value(udev_device, sysattr); diff --git a/lib/xspublic/xscontroller/udev.h b/lib/xspublic/xscontroller/udev.h index 5a11694..8f4f528 100644 --- a/lib/xspublic/xscontroller/udev.h +++ b/lib/xspublic/xscontroller/udev.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -72,21 +72,21 @@ struct udev_list_entry; struct XsLibraryLoader; -typedef struct udev *uDEV_new(void); -typedef struct udev *uDEV_unref(struct udev *udev); -typedef struct udev_device *uDEV_device_unref(struct udev_device *udev_device); -typedef struct udev_enumerate *uDEV_enumerate_new(struct udev *udev); -typedef int uDEV_enumerate_add_match_subsystem(struct udev_enumerate *udev_enumerate, const char *subsystem); -typedef int uDEV_enumerate_scan_devices(struct udev_enumerate *udev_enumerate); -typedef struct udev_list_entry *uDEV_enumerate_get_list_entry(struct udev_enumerate *udev_enumerate); -typedef struct udev_enumerate *uDEV_enumerate_unref(struct udev_enumerate *udev_enumerate); -typedef struct udev_list_entry *uDEV_list_entry_get_next(struct udev_list_entry *list_entry); -typedef const char *uDEV_list_entry_get_name(struct udev_list_entry *list_entry); -typedef struct udev_device *uDEV_device_new_from_syspath(struct udev *udev, const char *syspath); -typedef struct udev_device *uDEV_device_get_parent(struct udev_device *udev_device); -typedef const char *uDEV_device_get_devnode(struct udev_device *udev_device); -typedef struct udev_device *uDEV_device_get_parent_with_subsystem_devtype(struct udev_device *udev_device, const char *subsystem, const char *devtype); -typedef const char *uDEV_device_get_sysattr_value(struct udev_device *udev_device, const char *sysattr); +typedef struct udev* uDEV_new(void); +typedef struct udev* uDEV_unref(struct udev* udev); +typedef struct udev_device* uDEV_device_unref(struct udev_device* udev_device); +typedef struct udev_enumerate* uDEV_enumerate_new(struct udev* udev); +typedef int uDEV_enumerate_add_match_subsystem(struct udev_enumerate* udev_enumerate, const char* subsystem); +typedef int uDEV_enumerate_scan_devices(struct udev_enumerate* udev_enumerate); +typedef struct udev_list_entry* uDEV_enumerate_get_list_entry(struct udev_enumerate* udev_enumerate); +typedef struct udev_enumerate* uDEV_enumerate_unref(struct udev_enumerate* udev_enumerate); +typedef struct udev_list_entry* uDEV_list_entry_get_next(struct udev_list_entry* list_entry); +typedef const char* uDEV_list_entry_get_name(struct udev_list_entry* list_entry); +typedef struct udev_device* uDEV_device_new_from_syspath(struct udev* udev, const char* syspath); +typedef struct udev_device* uDEV_device_get_parent(struct udev_device* udev_device); +typedef const char* uDEV_device_get_devnode(struct udev_device* udev_device); +typedef struct udev_device* uDEV_device_get_parent_with_subsystem_devtype(struct udev_device* udev_device, const char* subsystem, const char* devtype); +typedef const char* uDEV_device_get_sysattr_value(struct udev_device* udev_device, const char* sysattr); #ifdef __cplusplus class Udev @@ -115,21 +115,21 @@ class Udev struct UDEV_API { - uDEV_new (* unew); - uDEV_unref (* unref); - uDEV_device_unref (* device_unref); - uDEV_enumerate_new (* enumerate_new); - uDEV_enumerate_add_match_subsystem (* enumerate_add_match_subsystem); - uDEV_enumerate_scan_devices (* enumerate_scan_devices); - uDEV_enumerate_get_list_entry (* enumerate_get_list_entry); - uDEV_enumerate_unref (* enumerate_unref); - uDEV_list_entry_get_next (* list_entry_get_next); - uDEV_list_entry_get_name (* list_entry_get_name); - uDEV_device_new_from_syspath (* device_new_from_syspath); - uDEV_device_get_parent (* device_get_parent); - uDEV_device_get_devnode (* device_get_devnode); - uDEV_device_get_parent_with_subsystem_devtype (* device_get_parent_with_subsystem_devtype); - uDEV_device_get_sysattr_value (* device_get_sysattr_value); + uDEV_new* unew; + uDEV_unref* unref; + uDEV_device_unref* device_unref; + uDEV_enumerate_new* enumerate_new; + uDEV_enumerate_add_match_subsystem* enumerate_add_match_subsystem; + uDEV_enumerate_scan_devices* enumerate_scan_devices; + uDEV_enumerate_get_list_entry* enumerate_get_list_entry; + uDEV_enumerate_unref* enumerate_unref; + uDEV_list_entry_get_next* list_entry_get_next; + uDEV_list_entry_get_name* list_entry_get_name; + uDEV_device_new_from_syspath* device_new_from_syspath; + uDEV_device_get_parent* device_get_parent; + uDEV_device_get_devnode* device_get_devnode; + uDEV_device_get_parent_with_subsystem_devtype* device_get_parent_with_subsystem_devtype; + uDEV_device_get_sysattr_value* device_get_sysattr_value; } m_uDev; XsLibraryLoader* m_libraryLoader; diff --git a/lib/xspublic/xscontroller/usbcommunicator.cpp b/lib/xspublic/xscontroller/usbcommunicator.cpp index 9355d4c..1699f8e 100644 --- a/lib/xspublic/xscontroller/usbcommunicator.cpp +++ b/lib/xspublic/xscontroller/usbcommunicator.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -72,7 +72,7 @@ /*! \brief Construct and returns new USB communicator \returns The new USB communicator */ -Communicator *UsbCommunicator::construct() +Communicator* UsbCommunicator::construct() { return new UsbCommunicator; } @@ -119,17 +119,17 @@ XsResultValue UsbCommunicator::gotoMeasurement() \param pi The port to use \returns The shared pointer to a stream interface */ -std::shared_ptr UsbCommunicator::createStreamInterface(const XsPortInfo &pi) +std::shared_ptr UsbCommunicator::createStreamInterface(const XsPortInfo& pi) { assert(pi.isUsb()); m_usbInterface = new UsbInterface(); std::shared_ptr stream(m_usbInterface, - [this](StreamInterface *intf) - { - m_usbInterface = nullptr; - delete intf; - } - ); + [this](StreamInterface * intf) + { + m_usbInterface = nullptr; + delete intf; + } + ); setLastResult(m_usbInterface->open(pi)); diff --git a/lib/xspublic/xscontroller/usbcommunicator.h b/lib/xspublic/xscontroller/usbcommunicator.h index c2d44f4..844fb1d 100644 --- a/lib/xspublic/xscontroller/usbcommunicator.h +++ b/lib/xspublic/xscontroller/usbcommunicator.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -72,7 +72,7 @@ class UsbInterface; class UsbCommunicator : public SerialPortCommunicator { public: - static Communicator *construct(); + static Communicator* construct(); UsbCommunicator(); virtual XsResultValue gotoConfig(bool detectRs485) override; @@ -80,10 +80,10 @@ class UsbCommunicator : public SerialPortCommunicator protected: virtual ~UsbCommunicator(); - std::shared_ptr createStreamInterface(const XsPortInfo &pi) override; + std::shared_ptr createStreamInterface(const XsPortInfo& pi) override; private: - UsbInterface *m_usbInterface; + UsbInterface* m_usbInterface; }; #endif diff --git a/lib/xspublic/xscontroller/usbinterface.cpp b/lib/xspublic/xscontroller/usbinterface.cpp index c0da27a..58c1bf2 100644 --- a/lib/xspublic/xscontroller/usbinterface.cpp +++ b/lib/xspublic/xscontroller/usbinterface.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -72,32 +72,32 @@ #include "rx_tx_log.h" #ifndef _WIN32 -# include // strcpy + #include // strcpy #else -# include -# include + #include + #include #endif #include #ifndef _CRT_SECURE_NO_DEPRECATE -# define _CRT_SECURE_NO_DEPRECATE -# ifdef _WIN32 -# pragma warning(disable:4996) -# endif + #define _CRT_SECURE_NO_DEPRECATE + #ifdef _WIN32 + #pragma warning(disable:4996) + #endif #endif /*! \brief Private object for UsbInterface */ class UsbInterfacePrivate { public: - #ifdef LOG_RX_TX - XsFile rx_log; - XsFile tx_log; - #endif +#ifdef LOG_RX_TX + XsFile rx_log; + XsFile tx_log; +#endif - //! The time at which an operation will end in ms, used by several functions. + //! The time at which an operation will end in ms, used by several functions. uint32_t m_endTime; - //! The last result of an operation + //! The last result of an operation XsResultValue m_lastResult; /*! The default timeout value to use during blocking operations. A value of 0 means that all operations become non-blocking. @@ -117,7 +117,7 @@ class UsbInterfacePrivate XsThreadId m_threadId; XsThread m_threadHandle; - static const int m_oCount = MAXIMUM_WAIT_OBJECTS-1; + static const int m_oCount = MAXIMUM_WAIT_OBJECTS - 1; OVERLAPPED m_overlapped[m_oCount]; XsByteArray m_varBuffer; static const int m_fixedBufferSize = 8192; @@ -136,12 +136,13 @@ class UsbInterfacePrivate int m_interfaceCount; int m_interface; - libusb_device_handle *m_deviceHandle; + libusb_device_handle* m_deviceHandle; /*! \brief A context manager for libusb - For predictable operation with libusb, it is recommended to use at least one context per library. + For predictable operation with libusb, it is recommended to use at least one context per library. */ - class UsbContext { + class UsbContext + { public: /*! \brief Create the USB context */ @@ -160,7 +161,7 @@ class UsbInterfacePrivate if (!m_claimedContexts) m_libUsb.exit(m_usbContext); } - static libusb_context *m_usbContext; // needed for proper use of libusb + static libusb_context* m_usbContext; // needed for proper use of libusb XsLibUsb m_libUsb; private: static int m_claimedContexts; @@ -174,102 +175,104 @@ class UsbInterfacePrivate */ XsResultValue libusbErrorToXrv(int libusbError, XsResultValue hint = XRV_ERROR) { - switch (libusbError) { - case LIBUSB_SUCCESS: - return XRV_OK; + switch (libusbError) + { + case LIBUSB_SUCCESS: + return XRV_OK; - case LIBUSB_ERROR_IO: - return hint; + case LIBUSB_ERROR_IO: + return hint; - case LIBUSB_ERROR_INVALID_PARAM: - return XRV_INVALIDPARAM; + case LIBUSB_ERROR_INVALID_PARAM: + return XRV_INVALIDPARAM; - case LIBUSB_ERROR_ACCESS: - return hint; + case LIBUSB_ERROR_ACCESS: + return hint; - case LIBUSB_ERROR_NO_DEVICE: - return XRV_UNEXPECTED_DISCONNECT; + case LIBUSB_ERROR_NO_DEVICE: + return XRV_UNEXPECTED_DISCONNECT; - case LIBUSB_ERROR_NOT_FOUND: - return XRV_NOTFOUND; + case LIBUSB_ERROR_NOT_FOUND: + return XRV_NOTFOUND; - case LIBUSB_ERROR_BUSY: - return XRV_INVALIDOPERATION; + case LIBUSB_ERROR_BUSY: + return XRV_INVALIDOPERATION; - case LIBUSB_ERROR_TIMEOUT: - return XRV_TIMEOUT; + case LIBUSB_ERROR_TIMEOUT: + return XRV_TIMEOUT; - case LIBUSB_ERROR_OVERFLOW: - return hint; + case LIBUSB_ERROR_OVERFLOW: + return hint; - case LIBUSB_ERROR_PIPE: - return hint; + case LIBUSB_ERROR_PIPE: + return hint; - case LIBUSB_ERROR_INTERRUPTED: - return XRV_UNEXPECTEDMSG; + case LIBUSB_ERROR_INTERRUPTED: + return XRV_UNEXPECTEDMSG; - case LIBUSB_ERROR_NO_MEM: - return XRV_OUTOFMEMORY; + case LIBUSB_ERROR_NO_MEM: + return XRV_OUTOFMEMORY; - case LIBUSB_ERROR_NOT_SUPPORTED: - return XRV_NOTIMPLEMENTED; + case LIBUSB_ERROR_NOT_SUPPORTED: + return XRV_NOTIMPLEMENTED; - case LIBUSB_ERROR_OTHER: - return hint; + case LIBUSB_ERROR_OTHER: + return hint; } return hint; } /*! \brief Convert a libusb error to a human-readable string */ - const char *libusbErrorToString(int libusbError) + const char* libusbErrorToString(int libusbError) { - switch (libusbError) { - case LIBUSB_SUCCESS: - return "LIBUSB_SUCCESS"; + switch (libusbError) + { + case LIBUSB_SUCCESS: + return "LIBUSB_SUCCESS"; - case LIBUSB_ERROR_IO: - return "LIBUSB_ERROR_IO"; + case LIBUSB_ERROR_IO: + return "LIBUSB_ERROR_IO"; - case LIBUSB_ERROR_INVALID_PARAM: - return "LIBUSB_ERROR_INVALID_PARAM"; + case LIBUSB_ERROR_INVALID_PARAM: + return "LIBUSB_ERROR_INVALID_PARAM"; - case LIBUSB_ERROR_ACCESS: - return "LIBUSB_ERROR_ACCESS"; + case LIBUSB_ERROR_ACCESS: + return "LIBUSB_ERROR_ACCESS"; - case LIBUSB_ERROR_NO_DEVICE: - return "LIBUSB_ERROR_NO_DEVICE"; + case LIBUSB_ERROR_NO_DEVICE: + return "LIBUSB_ERROR_NO_DEVICE"; - case LIBUSB_ERROR_NOT_FOUND: - return "LIBUSB_ERROR_NOT_FOUND"; + case LIBUSB_ERROR_NOT_FOUND: + return "LIBUSB_ERROR_NOT_FOUND"; - case LIBUSB_ERROR_BUSY: - return "LIBUSB_ERROR_BUSY"; + case LIBUSB_ERROR_BUSY: + return "LIBUSB_ERROR_BUSY"; - case LIBUSB_ERROR_TIMEOUT: - return "LIBUSB_ERROR_TIMEOUT"; + case LIBUSB_ERROR_TIMEOUT: + return "LIBUSB_ERROR_TIMEOUT"; - case LIBUSB_ERROR_OVERFLOW: - return "LIBUSB_ERROR_OVERFLOW"; + case LIBUSB_ERROR_OVERFLOW: + return "LIBUSB_ERROR_OVERFLOW"; - case LIBUSB_ERROR_PIPE: - return "LIBUSB_ERROR_PIPE"; + case LIBUSB_ERROR_PIPE: + return "LIBUSB_ERROR_PIPE"; - case LIBUSB_ERROR_INTERRUPTED: - return "LIBUSB_ERROR_INTERRUPTED"; + case LIBUSB_ERROR_INTERRUPTED: + return "LIBUSB_ERROR_INTERRUPTED"; - case LIBUSB_ERROR_NO_MEM: - return "LIBUSB_ERROR_NO_MEM"; + case LIBUSB_ERROR_NO_MEM: + return "LIBUSB_ERROR_NO_MEM"; - case LIBUSB_ERROR_NOT_SUPPORTED: - return "LIBUSB_ERROR_NOT_SUPPORTED"; + case LIBUSB_ERROR_NOT_SUPPORTED: + return "LIBUSB_ERROR_NOT_SUPPORTED"; - case LIBUSB_ERROR_OTHER: - return "LIBUSB_ERROR_OTHER"; + case LIBUSB_ERROR_OTHER: + return "LIBUSB_ERROR_OTHER"; } return "Unknown"; } #else - void *m_deviceHandle; + void* m_deviceHandle; #endif }; @@ -284,7 +287,7 @@ static DWORD usbReadThreadFunc(void* obj) { d->threadFunc(); } - catch(...) + catch (...) { #ifdef XSENS_DEBUG assert(0); @@ -296,15 +299,15 @@ static DWORD usbReadThreadFunc(void* obj) void UsbInterfacePrivate::threadFunc() { - HANDLE handles[1+m_oCount]; + HANDLE handles[1 + m_oCount]; handles[0] = m_quitEvent; - handles[m_oCount] = m_waitEvents[m_oCount-1]; + handles[m_oCount] = m_waitEvents[m_oCount - 1]; //= { m_quitEvent, m_waitEvents[0], m_waitEvents[1] }; // start first read operation - for (m_readIdx = 0 ; m_readIdx < (m_oCount-1); ++m_readIdx) + for (m_readIdx = 0 ; m_readIdx < (m_oCount - 1); ++m_readIdx) { - handles[m_readIdx+1] = m_waitEvents[m_readIdx]; + handles[m_readIdx + 1] = m_waitEvents[m_readIdx]; m_overlapped[m_readIdx] = OVERLAPPED(); ::ResetEvent(m_waitEvents[m_readIdx]); m_overlapped[m_readIdx].hEvent = m_waitEvents[m_readIdx]; @@ -319,72 +322,73 @@ void UsbInterfacePrivate::threadFunc() m_overlapped[m_readIdx].hEvent = m_waitEvents[m_readIdx]; m_winUsb.ReadPipe(m_usbHandle[1], m_bulkInPipe, m_fixedBuffer[m_readIdx], (ULONG)m_fixedBufferSize, 0, &m_overlapped[m_readIdx]); m_readIdx = (m_readIdx + 1) % m_oCount; - DWORD waitResult = ::WaitForMultipleObjects(1+m_oCount, handles, FALSE, INFINITE); + DWORD waitResult = ::WaitForMultipleObjects(1 + m_oCount, handles, FALSE, INFINITE); // handle data switch (waitResult) { - case WAIT_TIMEOUT: - case WAIT_FAILED: - case WAIT_OBJECT_0: - run = false; - break; - - default: - if (waitResult >= WAIT_ABANDONED_0) - { - JLDEBUGG("WFMO abandoned: " << (waitResult - WAIT_OBJECT_0)); + case WAIT_TIMEOUT: + case WAIT_FAILED: + case WAIT_OBJECT_0: + run = false; break; - } + + default: + if (waitResult >= WAIT_ABANDONED_0) + { + JLDEBUGG("WFMO abandoned: " << (waitResult - WAIT_OBJECT_0)); + break; + } #ifndef XSENS_RELEASE - JLDEBUGG("WFMO trigger: " << (waitResult - WAIT_OBJECT_0)); + JLDEBUGG("WFMO trigger: " << (waitResult - WAIT_OBJECT_0)); #endif - { - // put data into buffer - int idx = m_readIdx; - DWORD dataRead = 0; - if (!m_winUsb.GetOverlappedResult(m_usbHandle[0], &m_overlapped[idx], &dataRead, FALSE)) { - // error - DWORD err = ::GetLastError(); - switch (err) + // put data into buffer + int idx = m_readIdx; + DWORD dataRead = 0; + if (!m_winUsb.GetOverlappedResult(m_usbHandle[0], &m_overlapped[idx], &dataRead, FALSE)) { - case ERROR_SEM_TIMEOUT: - case ERROR_IO_INCOMPLETE: - //JLDEBUGG("m_winUsb.GetOverlappedResult resulted in acceptable windows error " << err); - break; - - case ERROR_GEN_FAILURE: - JLDEBUGG("We seem to have lost contact with the usb device"); - m_threadedResult = XRV_UNEXPECTED_DISCONNECT; - run = false; - break; - - default: - JLALERTG("m_winUsb.GetOverlappedResult resulted in windows error " << err); - run = false; - break; + // error + DWORD err = ::GetLastError(); + switch (err) + { + case ERROR_SEM_TIMEOUT: + case ERROR_IO_INCOMPLETE: + //JLDEBUGG("m_winUsb.GetOverlappedResult resulted in acceptable windows error " << err); + break; + + case ERROR_GEN_FAILURE: + JLDEBUGG("We seem to have lost contact with the usb device"); + m_threadedResult = XRV_UNEXPECTED_DISCONNECT; + run = false; + break; + + default: + JLALERTG("m_winUsb.GetOverlappedResult resulted in windows error " << err); + run = false; + break; + } + //assert (err == ERROR_IO_INCOMPLETE); + } + else + { + // append unread data to var buffer + JLTRACEG("m_winUsb.GetOverlappedResult resulted in " << dataRead << " bytes being read"); + XsByteArray ref(&m_fixedBuffer[idx][0], dataRead, XSDF_None); + ::EnterCriticalSection(&m_mutex); + m_varBuffer.append(ref); + ::LeaveCriticalSection(&m_mutex); } - //assert (err == ERROR_IO_INCOMPLETE); - } - else - { - // append unread data to var buffer - JLTRACEG("m_winUsb.GetOverlappedResult resulted in " << dataRead << " bytes being read"); - XsByteArray ref(&m_fixedBuffer[idx][0], dataRead, XSDF_None); - ::EnterCriticalSection(&m_mutex); - m_varBuffer.append(ref); - ::LeaveCriticalSection(&m_mutex); } - } break; + break; } } } #elif defined(HAVE_LIBUSB) int UsbInterfacePrivate::UsbContext::m_claimedContexts = 0; -libusb_context *UsbInterfacePrivate::UsbContext::m_usbContext = NULL; +libusb_context* UsbInterfacePrivate::UsbContext::m_usbContext = NULL; #endif /*! \class UsbInterface @@ -435,7 +439,7 @@ UsbInterface::~UsbInterface() #endif delete d; } - catch(...) + catch (...) { } } @@ -493,7 +497,7 @@ XsResultValue UsbInterface::closeUsb(void) } #elif defined(HAVE_LIBUSB) flushData(); - libusb_device *dev = d->m_contextManager.m_libUsb.get_device(d->m_deviceHandle); + libusb_device* dev = d->m_contextManager.m_libUsb.get_device(d->m_deviceHandle); for (int i = 0; i < d->m_interfaceCount; i++) { int result = LIBUSB_ERROR_OTHER; @@ -543,9 +547,10 @@ XsResultValue UsbInterface::flushData(void) #elif defined(HAVE_LIBUSB) unsigned char flushBuffer[256]; int actual; - for (int i = 0; i < 64; ++i) { - if (d->m_contextManager.m_libUsb.bulk_transfer(d->m_deviceHandle, d->m_dataInEndPoint|LIBUSB_ENDPOINT_IN, - flushBuffer, sizeof(flushBuffer), &actual, 1) != LIBUSB_SUCCESS) + for (int i = 0; i < 64; ++i) + { + if (d->m_contextManager.m_libUsb.bulk_transfer(d->m_deviceHandle, d->m_dataInEndPoint | LIBUSB_ENDPOINT_IN, + flushBuffer, sizeof(flushBuffer), &actual, 1) != LIBUSB_SUCCESS) break; if (actual == 0) break; @@ -564,19 +569,19 @@ XsResultValue UsbInterface::getLastResult(void) const } //! Return the current timeout value -uint32_t UsbInterface::getTimeout (void) const +uint32_t UsbInterface::getTimeout(void) const { return d->m_timeout; } //! Return whether the USB communication port is open or not. -bool UsbInterface::isOpen (void) const +bool UsbInterface::isOpen(void) const { return d->m_deviceHandle != NULL; } /*! \brief Open a communication channel to the given USB port name. */ -XsResultValue UsbInterface::open(const XsPortInfo &portInfo, XsFilePos, XsFilePos, PortOptions) +XsResultValue UsbInterface::open(const XsPortInfo& portInfo, XsFilePos, XsFilePos, PortOptions) { d->m_endTime = 0; @@ -604,12 +609,12 @@ XsResultValue UsbInterface::open(const XsPortInfo &portInfo, XsFilePos, XsFilePo BOOL result; UCHAR speed = 0; ULONG length = 0; - USB_INTERFACE_DESCRIPTOR interfaceDescriptor = {0,0,0,0,0,0,0,0,0}; + USB_INTERFACE_DESCRIPTOR interfaceDescriptor = {0, 0, 0, 0, 0, 0, 0, 0, 0}; WINUSB_PIPE_INFORMATION pipeInfo; result = d->m_winUsb.Initialize(d->m_deviceHandle, &d->m_usbHandle[0]); if (result) - result = d->m_winUsb.GetAssociatedInterface(d->m_usbHandle[0],0,&d->m_usbHandle[1]); + result = d->m_winUsb.GetAssociatedInterface(d->m_usbHandle[0], 0, &d->m_usbHandle[1]); else { #ifdef XSENS_DEBUG @@ -619,7 +624,7 @@ XsResultValue UsbInterface::open(const XsPortInfo &portInfo, XsFilePos, XsFilePo return (d->m_lastResult = XRV_ERROR); } - for (int k = 0; k<2;k++) + for (int k = 0; k < 2; k++) { if (result) { @@ -635,7 +640,7 @@ XsResultValue UsbInterface::open(const XsPortInfo &portInfo, XsFilePos, XsFilePo } if (result) { - for(int i=0;im_winUsb.QueryPipe(d->m_usbHandle[k], 0, (UCHAR) i, &pipeInfo); @@ -646,13 +651,9 @@ XsResultValue UsbInterface::open(const XsPortInfo &portInfo, XsFilePos, XsFilePo pipeInfo.MaximumPacketSize; } else if (pipeInfo.PipeType == UsbdPipeTypeBulk && USB_ENDPOINT_DIRECTION_OUT(pipeInfo.PipeId)) - { d->m_bulkOutPipe = pipeInfo.PipeId; - } else if (pipeInfo.PipeType == UsbdPipeTypeInterrupt) - { d->m_interruptPipe = pipeInfo.PipeId; - } else { result = FALSE; @@ -667,7 +668,7 @@ XsResultValue UsbInterface::open(const XsPortInfo &portInfo, XsFilePos, XsFilePo sprintf(d->m_portname, "%s", portInfo.portName_c_str()); -// d->m_offset = 0; + // d->m_offset = 0; ::ResetEvent(&d->m_quitEvent); d->m_threadHandle = xsStartThread(usbReadThreadFunc, d, &d->m_threadId); if (d->m_threadHandle == XSENS_INVALID_THREAD) @@ -679,7 +680,7 @@ XsResultValue UsbInterface::open(const XsPortInfo &portInfo, XsFilePos, XsFilePo } #elif defined(HAVE_LIBUSB) - libusb_device **deviceList; + libusb_device** deviceList; ssize_t listLength = d->m_contextManager.m_libUsb.get_device_list(d->m_contextManager.m_usbContext, &deviceList); if (listLength < 0) return d->m_lastResult = d->libusbErrorToXrv((int)listLength); @@ -690,10 +691,10 @@ XsResultValue UsbInterface::open(const XsPortInfo &portInfo, XsFilePos, XsFilePo XsResultValue xrv = XRV_OK; int result; - libusb_device *device = NULL; + libusb_device* device = NULL; for (int i = 0; i < listLength && device == NULL; ++i) { - libusb_device *dev = deviceList[i]; + libusb_device* dev = deviceList[i]; if (d->m_contextManager.m_libUsb.get_bus_number(dev) != bus || d->m_contextManager.m_libUsb.get_device_address(dev) != address) continue; @@ -702,7 +703,7 @@ XsResultValue UsbInterface::open(const XsPortInfo &portInfo, XsFilePos, XsFilePo if (result != LIBUSB_SUCCESS) break; - libusb_config_descriptor *configDesc; + libusb_config_descriptor* configDesc; result = d->m_contextManager.m_libUsb.get_active_config_descriptor(dev, &configDesc); if (result != LIBUSB_SUCCESS) break; @@ -714,22 +715,22 @@ XsResultValue UsbInterface::open(const XsPortInfo &portInfo, XsFilePos, XsFilePo { for (uint8_t altsettingCount = 0; altsettingCount < configDesc->interface[ifCount].num_altsetting; altsettingCount++) { - const libusb_endpoint_descriptor *endpoints = configDesc->interface[ifCount].altsetting[altsettingCount].endpoint; + const libusb_endpoint_descriptor* endpoints = configDesc->interface[ifCount].altsetting[altsettingCount].endpoint; int inEndpoint = -1, outEndpoint = -1; for (uint8_t i = 0; i < configDesc->interface[ifCount].altsetting[altsettingCount].bNumEndpoints; i++) { - if ((endpoints[i].bmAttributes&LIBUSB_TRANSFER_TYPE_MASK) != LIBUSB_TRANSFER_TYPE_BULK) + if ((endpoints[i].bmAttributes & LIBUSB_TRANSFER_TYPE_MASK) != LIBUSB_TRANSFER_TYPE_BULK) continue; - switch (endpoints[i].bEndpointAddress&LIBUSB_ENDPOINT_DIR_MASK) + switch (endpoints[i].bEndpointAddress & LIBUSB_ENDPOINT_DIR_MASK) { - case LIBUSB_ENDPOINT_IN: - inEndpoint = endpoints[i].bEndpointAddress&LIBUSB_ENDPOINT_ADDRESS_MASK; - break; + case LIBUSB_ENDPOINT_IN: + inEndpoint = endpoints[i].bEndpointAddress & LIBUSB_ENDPOINT_ADDRESS_MASK; + break; - case LIBUSB_ENDPOINT_OUT: - outEndpoint = endpoints[i].bEndpointAddress&LIBUSB_ENDPOINT_ADDRESS_MASK; - break; + case LIBUSB_ENDPOINT_OUT: + outEndpoint = endpoints[i].bEndpointAddress & LIBUSB_ENDPOINT_ADDRESS_MASK; + break; } } @@ -767,7 +768,7 @@ XsResultValue UsbInterface::open(const XsPortInfo &portInfo, XsFilePos, XsFilePo return d->m_lastResult = xrv; } - libusb_device_handle *handle; + libusb_device_handle* handle; result = d->m_contextManager.m_libUsb.open(device, &handle); if (result != LIBUSB_SUCCESS) { @@ -805,6 +806,7 @@ XsResultValue UsbInterface::open(const XsPortInfo &portInfo, XsFilePos, XsFilePo flushData(); #else // HAVE_LIBUSB + (void)portInfo; d->m_lastResult = XRV_NOTIMPLEMENTED; #endif JLDEBUGG("USB Port opened"); @@ -824,7 +826,7 @@ XsResultValue UsbInterface::readData(XsFilePos maxLength, XsByteArray& data) XsFilePos length = 0; data.setSize((XsSize) maxLength); XsResultValue res = readData(maxLength, data.data(), &length); - data.pop_back((XsSize) (maxLength - length)); + data.pop_back((XsSize)(maxLength - length)); return res; } @@ -837,7 +839,7 @@ XsResultValue UsbInterface::readData(XsFilePos maxLength, XsByteArray& data) \returns XRV_OK if no error occurred. It can be that no data is available and XRV_OK will be returned. Check *length for the number of bytes that were read. */ -XsResultValue UsbInterface::readData(XsFilePos maxLength, void *data, XsFilePos* length) +XsResultValue UsbInterface::readData(XsFilePos maxLength, void* data, XsFilePos* length) { JLTRACEG("maxLength=" << maxLength << ", data=" << JLHEXLOG(data) << ", length=" << JLHEXLOG(length)); XsFilePos ln; @@ -869,7 +871,7 @@ XsResultValue UsbInterface::readData(XsFilePos maxLength, void *data, XsFilePos* #elif defined(HAVE_LIBUSB) int actual = 0; JLTRACEG("starting bulk read, timeout = " << d->m_timeout); - int res = d->m_contextManager.m_libUsb.bulk_transfer(d->m_deviceHandle, (d->m_dataInEndPoint|LIBUSB_ENDPOINT_IN), (unsigned char *)data, maxLength, &actual, d->m_timeout); + int res = d->m_contextManager.m_libUsb.bulk_transfer(d->m_deviceHandle, (d->m_dataInEndPoint | LIBUSB_ENDPOINT_IN), (unsigned char*)data, maxLength, &actual, d->m_timeout); JLTRACEG("bulk read returned: " << d->libusbErrorToString(res) << ". " << actual << " bytes received"); if ((res != LIBUSB_SUCCESS && res != LIBUSB_ERROR_TIMEOUT) || (res == LIBUSB_ERROR_TIMEOUT && actual <= 0)) return d->m_lastResult = d->libusbErrorToXrv(res); @@ -888,10 +890,10 @@ XsResultValue UsbInterface::readData(XsFilePos maxLength, void *data, XsFilePos* if (!d->rx_log.isOpen()) { char fname[XS_MAX_FILENAME_LENGTH]; - sprintf(fname,"rx_USB%03u_%03u.log", usbBus(), usbAddress()); + sprintf(fname, "rx_USB%03u_%03u.log", usbBus(), usbAddress()); d->rx_log.create(XsString(fname), true); } - d->rx_log.write(data,1,*length); + d->rx_log.write(data, 1, *length); #ifdef LOG_RX_TX_FLUSH d->rx_log.flush(); #endif @@ -978,7 +980,7 @@ XsResultValue UsbInterface::waitForData(XsFilePos maxLength, void* data, XsFileP JLTRACEG("timeout=" << d->m_timeout << ", data=" << data << ", length=" << length); uint32_t timeout = d->m_timeout; - char *bdata = (char *)data; + char* bdata = (char*)data; XsFilePos ln; if (length == NULL) @@ -1023,7 +1025,7 @@ XsResultValue UsbInterface::writeData(const XsByteArray& data, XsFilePos* writte \returns XRV_OK if the data was successfully written \sa writeData(const XsByteArray&, XsFilePos*) */ -XsResultValue UsbInterface::writeData(XsFilePos length, const void *data, XsFilePos* written) +XsResultValue UsbInterface::writeData(XsFilePos length, const void* data, XsFilePos* written) { XsFilePos bytes; if (written == NULL) @@ -1044,7 +1046,7 @@ XsResultValue UsbInterface::writeData(XsFilePos length, const void *data, XsFile while (*written < length) { int actual; - int result = d->m_contextManager.m_libUsb.bulk_transfer(d->m_deviceHandle, (d->m_dataOutEndPoint|LIBUSB_ENDPOINT_OUT), (unsigned char *)data, length, &actual, 0); + int result = d->m_contextManager.m_libUsb.bulk_transfer(d->m_deviceHandle, (d->m_dataOutEndPoint | LIBUSB_ENDPOINT_OUT), (unsigned char*)data, length, &actual, 0); *written += actual; if (result != LIBUSB_SUCCESS) { @@ -1065,10 +1067,10 @@ XsResultValue UsbInterface::writeData(XsFilePos length, const void *data, XsFile if (!d->tx_log.isOpen()) { char fname[XS_MAX_FILENAME_LENGTH]; - sprintf(fname,"tx_USB%03u_%03u.log", usbBus(), usbAddress()); + sprintf(fname, "tx_USB%03u_%03u.log", usbBus(), usbAddress()); d->tx_log.create(XsString(fname), true); } - d->tx_log.write(data,1,*written); + d->tx_log.write(data, 1, *written); #ifdef LOG_RX_TX_FLUSH d->tx_log.flush(); #endif @@ -1085,7 +1087,7 @@ uint8_t UsbInterface::usbBus() const if (!d->m_deviceHandle) return 0; - libusb_device *dev = d->m_contextManager.m_libUsb.get_device(d->m_deviceHandle); + libusb_device* dev = d->m_contextManager.m_libUsb.get_device(d->m_deviceHandle); return d->m_contextManager.m_libUsb.get_bus_number(dev); #else return 0; @@ -1099,7 +1101,7 @@ uint8_t UsbInterface::usbAddress() const if (!d->m_deviceHandle) return 0; - libusb_device *dev = d->m_contextManager.m_libUsb.get_device(d->m_deviceHandle); + libusb_device* dev = d->m_contextManager.m_libUsb.get_device(d->m_deviceHandle); return d->m_contextManager.m_libUsb.get_device_address(dev); #else return 0; diff --git a/lib/xspublic/xscontroller/usbinterface.h b/lib/xspublic/xscontroller/usbinterface.h index 46ef520..3d35bbc 100644 --- a/lib/xspublic/xscontroller/usbinterface.h +++ b/lib/xspublic/xscontroller/usbinterface.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -70,13 +70,13 @@ #include #include #ifdef _WIN32 -# include -//# include + #include + //# include #else -# include -// these are not required by level 1, but to keep the higher levels platform-independent they are put here -# include -# include + #include + // these are not required by level 1, but to keep the higher levels platform-independent they are put here + #include + #include #endif #include "streaminterface.h" @@ -86,26 +86,27 @@ struct XsPortInfo; class UsbInterfacePrivate; -class UsbInterface : public StreamInterface { - UsbInterfacePrivate *d; +class UsbInterface : public StreamInterface +{ + UsbInterfacePrivate* d; public: UsbInterface(); ~UsbInterface(); - XsResultValue open(const XsPortInfo &portInfo, XsFilePos readBufSize = 0, XsFilePos writeBufSize = 0, PortOptions = PO_XsensDefaults) override; + XsResultValue open(const XsPortInfo& portInfo, XsFilePos readBufSize = 0, XsFilePos writeBufSize = 0, PortOptions = PO_XsensDefaults) override; XsResultValue close(void); XsResultValue closeUsb(void); - XsResultValue flushData (void); + XsResultValue flushData(void); - bool isOpen (void) const; + bool isOpen(void) const; uint8_t usbBus() const; uint8_t usbAddress() const; XsResultValue getLastResult(void) const; - XsResultValue setTimeout (uint32_t ms); - uint32_t getTimeout (void) const; + XsResultValue setTimeout(uint32_t ms); + uint32_t getTimeout(void) const; void setRawIo(bool enable); bool getRawIo(void); @@ -114,13 +115,13 @@ class UsbInterface : public StreamInterface { XsResultValue readData(XsFilePos maxLength, XsByteArray& data) override; using IoInterface::waitForData; - XsResultValue writeData(XsFilePos length, const void *data, XsFilePos* written = NULL); - XsResultValue readData(XsFilePos maxLength, void *data, XsFilePos* length = NULL); - XsResultValue waitForData(XsFilePos maxLength, void *data, XsFilePos* length = NULL); + XsResultValue writeData(XsFilePos length, const void* data, XsFilePos* written = NULL); + XsResultValue readData(XsFilePos maxLength, void* data, XsFilePos* length = NULL); + XsResultValue waitForData(XsFilePos maxLength, void* data, XsFilePos* length = NULL); void getPortName(XsString& portname) const; -XSENS_DISABLE_COPY(UsbInterface); + XSENS_DISABLE_COPY(UsbInterface); }; #endif diff --git a/lib/xspublic/xscontroller/xdacommunicatorfactory.cpp b/lib/xspublic/xscontroller/xdacommunicatorfactory.cpp index 7619e4a..9c3e51f 100644 --- a/lib/xspublic/xscontroller/xdacommunicatorfactory.cpp +++ b/lib/xspublic/xscontroller/xdacommunicatorfactory.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -67,11 +67,12 @@ #include "serialportcommunicator.h" #include "usbcommunicator.h" -namespace CommunicatorType { - static const CommunicatorFactory::CommunicatorTypeId UNKNOWN = 0; - static const CommunicatorFactory::CommunicatorTypeId MTBFILE = 1; - static const CommunicatorFactory::CommunicatorTypeId USB = 2; - static const CommunicatorFactory::CommunicatorTypeId SERIALPORT = 3; +namespace CommunicatorType +{ +static const CommunicatorFactory::CommunicatorTypeId UNKNOWN = 0; +static const CommunicatorFactory::CommunicatorTypeId MTBFILE = 1; +static const CommunicatorFactory::CommunicatorTypeId USB = 2; +static const CommunicatorFactory::CommunicatorTypeId SERIALPORT = 3; } /*! \class XdaCommunicatorFactory @@ -84,18 +85,18 @@ XdaCommunicatorFactory::XdaCommunicatorFactory() } /*! \copydoc CommunicatorFactory::filenameToCommunicatorId */ -XdaCommunicatorFactory::CommunicatorTypeId XdaCommunicatorFactory::filenameToCommunicatorId(const XsString &) const +XdaCommunicatorFactory::CommunicatorTypeId XdaCommunicatorFactory::filenameToCommunicatorId(const XsString&) const { - /* Everything is expected to be an mtb file for now. - It is very wel possible that we're going to have to check for - other weird stuff as well, such as COM1 or /dev/ttyUSB0, - which would state a case for just a string-based approach. + /* Everything is expected to be an mtb file for now. + It is very wel possible that we're going to have to check for + other weird stuff as well, such as COM1 or /dev/ttyUSB0, + which would state a case for just a string-based approach. */ return CommunicatorType::MTBFILE; } /*! \copydoc CommunicatorFactory::portInfoToCommunicatorId */ -XdaCommunicatorFactory::CommunicatorTypeId XdaCommunicatorFactory::portInfoToCommunicatorId(const XsPortInfo &portInfo) const +XdaCommunicatorFactory::CommunicatorTypeId XdaCommunicatorFactory::portInfoToCommunicatorId(const XsPortInfo& portInfo) const { for (auto it = constructors().begin(); it != constructors().end(); ++it) if (it->second.second && it->second.second(portInfo)) @@ -104,22 +105,23 @@ XdaCommunicatorFactory::CommunicatorTypeId XdaCommunicatorFactory::portInfoToCom return CommunicatorType::UNKNOWN; } -namespace { - /*! \returns True if a \a portInfo is USB port - \param portInfo The port info to check - */ - bool isUsb(const XsPortInfo &portInfo) - { - return portInfo.isUsb(); - } +namespace +{ +/*! \returns True if a \a portInfo is USB port + \param portInfo The port info to check +*/ +bool isUsb(const XsPortInfo& portInfo) +{ + return portInfo.isUsb(); +} - /*! \returns True if a \a portInfo is a serial port - \param portInfo The port info to check - */ - bool isSerialPort(const XsPortInfo &portInfo) - { - return !portInfo.isUsb() && !portInfo.isNetwork(); - } +/*! \returns True if a \a portInfo is a serial port + \param portInfo The port info to check +*/ +bool isSerialPort(const XsPortInfo& portInfo) +{ + return !portInfo.isUsb() && !portInfo.isNetwork() && !portInfo.isBluetooth(); +} } /*! \brief Register the communicator types */ diff --git a/lib/xspublic/xscontroller/xdacommunicatorfactory.h b/lib/xspublic/xscontroller/xdacommunicatorfactory.h index 68f5148..810fc53 100644 --- a/lib/xspublic/xscontroller/xdacommunicatorfactory.h +++ b/lib/xspublic/xscontroller/xdacommunicatorfactory.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -76,8 +76,8 @@ class XdaCommunicatorFactory : public CommunicatorFactory void registerCommunicatorTypes() override; protected: - virtual CommunicatorTypeId filenameToCommunicatorId(const XsString &filename) const; - virtual CommunicatorTypeId portInfoToCommunicatorId(const XsPortInfo &portInfo) const; + virtual CommunicatorTypeId filenameToCommunicatorId(const XsString& filename) const; + virtual CommunicatorTypeId portInfoToCommunicatorId(const XsPortInfo& portInfo) const; }; diff --git a/lib/xspublic/xscontroller/xsaccesscontrolmode.h b/lib/xspublic/xscontroller/xsaccesscontrolmode.h index 7dad784..fe0ba97 100644 --- a/lib/xspublic/xscontroller/xsaccesscontrolmode.h +++ b/lib/xspublic/xscontroller/xsaccesscontrolmode.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -71,10 +71,11 @@ //AUTO namespace xscontroller { //! Device access control modes (XsDevice::setAccessControlMode()) -enum XsAccessControlMode { - XACM_None = -1 //!< No access control mode defined, do not use in 'set' function. Devices that support Access Control are always in either BlackList or WhiteList mode and will return an error if you try to set the Access Control Mode to XACM_None - ,XACM_BlackList = 0 //!< Accept all device connections, explicit rejection through blacklist - ,XACM_WhiteList = 1 //!< Decline all device connections, explicit acceptance through whitelist +enum XsAccessControlMode +{ + XACM_None = -1 //!< No access control mode defined, do not use in 'set' function. Devices that support Access Control are always in either BlackList or WhiteList mode and will return an error if you try to set the Access Control Mode to XACM_None + , XACM_BlackList = 0 //!< Accept all device connections, explicit rejection through blacklist + , XACM_WhiteList = 1 //!< Decline all device connections, explicit acceptance through whitelist }; /*! @} */ typedef enum XsAccessControlMode XsAccessControlMode; diff --git a/lib/xspublic/xscontroller/xsalignmentframe.h b/lib/xspublic/xscontroller/xsalignmentframe.h index d516cf7..3b78694 100644 --- a/lib/xspublic/xscontroller/xsalignmentframe.h +++ b/lib/xspublic/xscontroller/xsalignmentframe.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -71,7 +71,8 @@ //AUTO namespace xscontroller { /*! \brief Alignment frame. */ -enum XsAlignmentFrame { +enum XsAlignmentFrame +{ XAF_Sensor, //!< Sensor alignment frame XAF_Local //!< Local alignment frame }; @@ -80,4 +81,4 @@ typedef enum XsAlignmentFrame XsAlignmentFrame; //AUTO } -#endif // file guard +#endif diff --git a/lib/xspublic/xscontroller/xscalibrateddatamode.h b/lib/xspublic/xscontroller/xscalibrateddatamode.h index 1dda1ab..af49ee9 100644 --- a/lib/xspublic/xscontroller/xscalibrateddatamode.h +++ b/lib/xspublic/xscontroller/xscalibrateddatamode.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -69,17 +69,18 @@ @{ */ /*! \brief Legacy calibrated data output selection flags */ -enum XsCalibratedDataMode { +enum XsCalibratedDataMode +{ XCDM_None = 0, XCDM_Acceleration = (1 << 0), XCDM_GyroscopeData = (1 << 1), XCDM_MagneticField = (1 << 2), - XCDM_AccGyr = XCDM_Acceleration|XCDM_GyroscopeData, - XCDM_AccMag = XCDM_Acceleration|XCDM_MagneticField, - XCDM_GyrMag = XCDM_GyroscopeData|XCDM_MagneticField, + XCDM_AccGyr = XCDM_Acceleration | XCDM_GyroscopeData, + XCDM_AccMag = XCDM_Acceleration | XCDM_MagneticField, + XCDM_GyrMag = XCDM_GyroscopeData | XCDM_MagneticField, - XCDM_All = XCDM_Acceleration|XCDM_GyroscopeData|XCDM_MagneticField + XCDM_All = XCDM_Acceleration | XCDM_GyroscopeData | XCDM_MagneticField }; /*! @} */ typedef enum XsCalibratedDataMode XsCalibratedDataMode; diff --git a/lib/xspublic/xscontroller/xscallback.h b/lib/xspublic/xscontroller/xscallback.h index 3509858..151879a 100644 --- a/lib/xspublic/xscontroller/xscallback.h +++ b/lib/xspublic/xscontroller/xscallback.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -120,109 +120,275 @@ class XsCallback : public XsCallbackPlainC */ virtual ~XsCallback() {} -// Swig needs these functions to be protected, not private, otherwise they are ignored. + // Swig needs these functions to be protected, not private, otherwise they are ignored. protected: -/*! \protectedsection - \addtogroup Callbacks - @{ -*/ + /*! \protectedsection + \addtogroup Callbacks + @{ + */ //! \copybrief m_onDeviceStateChanged virtual void onDeviceStateChanged(XsDevice* dev, XsDeviceState newState, XsDeviceState oldState) - { (void) dev; (void) newState; (void) oldState; m_onDeviceStateChanged = 0; } + { + (void) dev; + (void) newState; + (void) oldState; + m_onDeviceStateChanged = 0; + } //! \copybrief m_onLiveDataAvailable virtual void onLiveDataAvailable(XsDevice* dev, const XsDataPacket* packet) - { (void) dev; (void) packet; m_onLiveDataAvailable = 0; } + { + (void) dev; + (void) packet; + m_onLiveDataAvailable = 0; + } //! \copybrief m_onMissedPackets virtual void onMissedPackets(XsDevice* dev, int count, int first, int last) - { (void) dev; (void)count; (void) first; (void) last; m_onMissedPackets = 0; } + { + (void) dev; + (void)count; + (void) first; + (void) last; + m_onMissedPackets = 0; + } //! \copybrief m_onWakeupReceived virtual void onWakeupReceived(XsDevice* dev) - { (void) dev; m_onWakeupReceived = 0; } + { + (void) dev; + m_onWakeupReceived = 0; + } //! \copybrief m_onProgressUpdated virtual void onProgressUpdated(XsDevice* dev, int current, int total, const XsString* identifier) - { (void) dev; (void) current; (void) total; (void)identifier; m_onProgressUpdated = 0; } + { + (void) dev; + (void) current; + (void) total; + (void)identifier; + m_onProgressUpdated = 0; + } //! \copydoc m_onWriteMessageToLogFile virtual int onWriteMessageToLogFile(XsDevice* dev, const XsMessage* message) - { (void) dev; (void) message; m_onWriteMessageToLogFile = 0; return 1; } + { + (void) dev; + (void) message; + m_onWriteMessageToLogFile = 0; + return 1; + } //! \copydoc m_onBufferedDataAvailable virtual void onBufferedDataAvailable(XsDevice* dev, const XsDataPacket* packet) - { (void)dev; (void)packet; m_onBufferedDataAvailable = 0; } + { + (void)dev; + (void)packet; + m_onBufferedDataAvailable = 0; + } //! \copydoc m_onConnectivityChanged virtual void onConnectivityChanged(XsDevice* dev, XsConnectivityState newState) - { (void) dev; (void) newState; m_onConnectivityChanged = 0; } + { + (void) dev; + (void) newState; + m_onConnectivityChanged = 0; + } //! \copydoc m_onInfoResponse virtual void onInfoResponse(XsDevice* dev, XsInfoRequest request) - { (void) dev; (void) request; m_onInfoResponse = 0; } + { + (void) dev; + (void) request; + m_onInfoResponse = 0; + } //! \copydoc m_onError virtual void onError(XsDevice* dev, XsResultValue error) - { (void) dev; (void) error; m_onError = 0; } + { + (void) dev; + (void) error; + m_onError = 0; + } //! \copydoc m_onNonDataMessage - virtual void onNonDataMessage(XsDevice* dev, XsMessage const * message) - { (void) dev; (void) message; m_onNonDataMessage = 0; } + virtual void onNonDataMessage(XsDevice* dev, XsMessage const* message) + { + (void) dev; + (void) message; + m_onNonDataMessage = 0; + } //! \copydoc m_onMessageDetected - virtual void onMessageDetected(XsDevice* dev, XsProtocolType type, XsByteArray const * rawMessage) - { (void)dev; (void)type; (void)rawMessage; m_onMessageDetected = 0; } + virtual void onMessageDetected(XsDevice* dev, XsProtocolType type, XsByteArray const* rawMessage) + { + (void)dev; + (void)type; + (void)rawMessage; + m_onMessageDetected = 0; + } //! \copydoc m_onMessageReceivedFromDevice - virtual void onMessageReceivedFromDevice(XsDevice* dev, XsMessage const * message) - { (void) dev; (void) message; m_onMessageReceivedFromDevice = 0; } + virtual void onMessageReceivedFromDevice(XsDevice* dev, XsMessage const* message) + { + (void) dev; + (void) message; + m_onMessageReceivedFromDevice = 0; + } //! \copydoc m_onMessageSentToDevice - virtual void onMessageSentToDevice(XsDevice* dev, XsMessage const * message) - { (void) dev; (void) message; m_onMessageSentToDevice = 0; } + virtual void onMessageSentToDevice(XsDevice* dev, XsMessage const* message) + { + (void) dev; + (void) message; + m_onMessageSentToDevice = 0; + } //! \copydoc m_onAllLiveDataAvailable virtual void onAllLiveDataAvailable(XsDevicePtrArray* devs, const XsDataPacketPtrArray* packets) - { (void) devs; (void) packets; m_onAllLiveDataAvailable = 0; } + { + (void) devs; + (void) packets; + m_onAllLiveDataAvailable = 0; + } //! \copydoc m_onAllBufferedDataAvailable virtual void onAllBufferedDataAvailable(XsDevicePtrArray* devs, const XsDataPacketPtrArray* packets) - { (void)devs; (void)packets; m_onAllBufferedDataAvailable = 0; } + { + (void)devs; + (void)packets; + m_onAllBufferedDataAvailable = 0; + } //! \copybrief m_onDataUnavailable virtual void onDataUnavailable(XsDevice* dev, int64_t packetId) - { (void) dev; (void)packetId; m_onMissedPackets = 0; } + { + (void) dev; + (void)packetId; + m_onMissedPackets = 0; + } //! \copydoc m_onDataAvailable virtual void onDataAvailable(XsDevice* dev, const XsDataPacket* packet) - { (void) dev; (void) packet; m_onDataAvailable = 0; } + { + (void) dev; + (void) packet; + m_onDataAvailable = 0; + } //! \copydoc m_onAllDataAvailable virtual void onAllDataAvailable(XsDevicePtrArray* devs, const XsDataPacketPtrArray* packets) - { (void) devs; (void) packets; m_onAllDataAvailable = 0; } + { + (void) devs; + (void) packets; + m_onAllDataAvailable = 0; + } //! \copydoc m_onRecordedDataAvailable virtual void onRecordedDataAvailable(XsDevice* dev, const XsDataPacket* packet) - { (void) dev; (void) packet; m_onRecordedDataAvailable = 0; } + { + (void) dev; + (void) packet; + m_onRecordedDataAvailable = 0; + } //! \copydoc m_onAllRecordedDataAvailable virtual void onAllRecordedDataAvailable(XsDevicePtrArray* devs, const XsDataPacketPtrArray* packets) - { (void) devs; (void) packets; m_onAllRecordedDataAvailable = 0; } + { + (void) devs; + (void) packets; + m_onAllRecordedDataAvailable = 0; + } //! \copydoc m_onTransmissionRequest virtual void onTransmissionRequest(int channelId, const XsByteArray* data) - { (void) channelId; (void)data; } + { + (void) channelId; + (void)data; + } //! \copydoc m_onRestoreCommunication virtual void onRestoreCommunication(const XsString* portName, XsResultValue result) - { (void)portName; (void)result; m_onRestoreCommunication = 0; } + { + (void)portName; + (void)result; + m_onRestoreCommunication = 0; + } -//! @} + //! @} private: -/*! \privatesection */ - static void sonDeviceStateChanged(XsCallbackPlainC* cb, XsDevice* dev, XsDeviceState newState, XsDeviceState oldState) { ((XsCallback*)cb)->onDeviceStateChanged(dev, newState, oldState); } - static void sonLiveDataAvailable(XsCallbackPlainC* cb, XsDevice* dev, const XsDataPacket* packet) { ((XsCallback*)cb)->onLiveDataAvailable(dev, packet); } - static void sonMissedPackets(XsCallbackPlainC* cb, XsDevice* dev, int count, int first, int last) { ((XsCallback*)cb)->onMissedPackets(dev, count, first, last); } - static void sonWakeupReceived(XsCallbackPlainC* cb, XsDevice* dev) { ((XsCallback*)cb)->onWakeupReceived(dev); } - static void sonProgressUpdated(XsCallbackPlainC* cb, XsDevice* dev, int current, int total, const XsString* identifier) { ((XsCallback*)cb)->onProgressUpdated(dev, current, total, identifier); } - static int sonWriteMessageToLogFile(XsCallbackPlainC* cb, XsDevice* dev, const XsMessage* message) { return ((XsCallback*)cb)->onWriteMessageToLogFile(dev, message); } - static void sonBufferedDataAvailable(XsCallbackPlainC* cb, XsDevice* dev, const XsDataPacket* packet) { ((XsCallback*)cb)->onBufferedDataAvailable(dev, packet); } - static void sonConnectivityChanged(XsCallbackPlainC* cb, XsDevice* dev, XsConnectivityState newState) { ((XsCallback*)cb)->onConnectivityChanged(dev, newState); } - static void sonInfoResponse(XsCallbackPlainC* cb, XsDevice* dev, XsInfoRequest request) { ((XsCallback*)cb)->onInfoResponse(dev, request); } - static void sonError(XsCallbackPlainC* cb, XsDevice* dev, XsResultValue error) { ((XsCallback*)cb)->onError(dev, error); } - static void sonNonDataMessage(XsCallbackPlainC* cb, XsDevice* dev, XsMessage const * message) { ((XsCallback*)cb)->onNonDataMessage(dev, message); } - static void sonMessageDetected(XsCallbackPlainC* cb, XsDevice* dev, XsProtocolType type, XsByteArray const * rawMessage) { ((XsCallback*)cb)->onMessageDetected(dev, type, rawMessage); } - static void sonMessageReceivedFromDevice(XsCallbackPlainC* cb, XsDevice* dev, XsMessage const * message) { ((XsCallback*)cb)->onMessageReceivedFromDevice(dev, message); } - static void sonMessageSentToDevice(XsCallbackPlainC* cb, XsDevice* dev, XsMessage const * message) { ((XsCallback*)cb)->onMessageSentToDevice(dev, message); } - static void sonAllLiveDataAvailable(XsCallbackPlainC* cb, XsDevicePtrArray* devs, const XsDataPacketPtrArray* packets) { ((XsCallback*)cb)->onAllLiveDataAvailable(devs, packets); } - static void sonAllBufferedDataAvailable(XsCallbackPlainC* cb, XsDevicePtrArray* devs, const XsDataPacketPtrArray* packets) { ((XsCallback*)cb)->onAllBufferedDataAvailable(devs, packets); } - static void sonDataUnavailable(XsCallbackPlainC* cb, XsDevice* dev, int64_t packetId) { ((XsCallback*)cb)->onDataUnavailable(dev, packetId); } - static void sonDataAvailable(XsCallbackPlainC* cb, XsDevice* dev, const XsDataPacket* packet) { ((XsCallback*)cb)->onDataAvailable(dev, packet); } - static void sonAllDataAvailable(XsCallbackPlainC* cb, XsDevicePtrArray* devs, const XsDataPacketPtrArray* packets) { ((XsCallback*)cb)->onAllDataAvailable(devs, packets); } - static void sonRecordedDataAvailable(XsCallbackPlainC* cb, XsDevice* dev, const XsDataPacket* packet) { ((XsCallback*)cb)->onRecordedDataAvailable(dev, packet); } - static void sonAllRecordedDataAvailable(XsCallbackPlainC* cb, XsDevicePtrArray* devs, const XsDataPacketPtrArray* packets) { ((XsCallback*)cb)->onAllRecordedDataAvailable(devs, packets); } - static void sonTransmissionRequest(XsCallbackPlainC* cb, int channelId, const XsByteArray* data) { ((XsCallback*)cb)->onTransmissionRequest(channelId, data); } - static void sonRestoreCommunication(XsCallbackPlainC* cb, const XsString* portName, XsResultValue result) { ((XsCallback*)cb)->onRestoreCommunication(portName, result); } + /*! \privatesection */ + static void sonDeviceStateChanged(XsCallbackPlainC* cb, XsDevice* dev, XsDeviceState newState, XsDeviceState oldState) + { + ((XsCallback*)cb)->onDeviceStateChanged(dev, newState, oldState); + } + static void sonLiveDataAvailable(XsCallbackPlainC* cb, XsDevice* dev, const XsDataPacket* packet) + { + ((XsCallback*)cb)->onLiveDataAvailable(dev, packet); + } + static void sonMissedPackets(XsCallbackPlainC* cb, XsDevice* dev, int count, int first, int last) + { + ((XsCallback*)cb)->onMissedPackets(dev, count, first, last); + } + static void sonWakeupReceived(XsCallbackPlainC* cb, XsDevice* dev) + { + ((XsCallback*)cb)->onWakeupReceived(dev); + } + static void sonProgressUpdated(XsCallbackPlainC* cb, XsDevice* dev, int current, int total, const XsString* identifier) + { + ((XsCallback*)cb)->onProgressUpdated(dev, current, total, identifier); + } + static int sonWriteMessageToLogFile(XsCallbackPlainC* cb, XsDevice* dev, const XsMessage* message) + { + return ((XsCallback*)cb)->onWriteMessageToLogFile(dev, message); + } + static void sonBufferedDataAvailable(XsCallbackPlainC* cb, XsDevice* dev, const XsDataPacket* packet) + { + ((XsCallback*)cb)->onBufferedDataAvailable(dev, packet); + } + static void sonConnectivityChanged(XsCallbackPlainC* cb, XsDevice* dev, XsConnectivityState newState) + { + ((XsCallback*)cb)->onConnectivityChanged(dev, newState); + } + static void sonInfoResponse(XsCallbackPlainC* cb, XsDevice* dev, XsInfoRequest request) + { + ((XsCallback*)cb)->onInfoResponse(dev, request); + } + static void sonError(XsCallbackPlainC* cb, XsDevice* dev, XsResultValue error) + { + ((XsCallback*)cb)->onError(dev, error); + } + static void sonNonDataMessage(XsCallbackPlainC* cb, XsDevice* dev, XsMessage const* message) + { + ((XsCallback*)cb)->onNonDataMessage(dev, message); + } + static void sonMessageDetected(XsCallbackPlainC* cb, XsDevice* dev, XsProtocolType type, XsByteArray const* rawMessage) + { + ((XsCallback*)cb)->onMessageDetected(dev, type, rawMessage); + } + static void sonMessageReceivedFromDevice(XsCallbackPlainC* cb, XsDevice* dev, XsMessage const* message) + { + ((XsCallback*)cb)->onMessageReceivedFromDevice(dev, message); + } + static void sonMessageSentToDevice(XsCallbackPlainC* cb, XsDevice* dev, XsMessage const* message) + { + ((XsCallback*)cb)->onMessageSentToDevice(dev, message); + } + static void sonAllLiveDataAvailable(XsCallbackPlainC* cb, XsDevicePtrArray* devs, const XsDataPacketPtrArray* packets) + { + ((XsCallback*)cb)->onAllLiveDataAvailable(devs, packets); + } + static void sonAllBufferedDataAvailable(XsCallbackPlainC* cb, XsDevicePtrArray* devs, const XsDataPacketPtrArray* packets) + { + ((XsCallback*)cb)->onAllBufferedDataAvailable(devs, packets); + } + static void sonDataUnavailable(XsCallbackPlainC* cb, XsDevice* dev, int64_t packetId) + { + ((XsCallback*)cb)->onDataUnavailable(dev, packetId); + } + static void sonDataAvailable(XsCallbackPlainC* cb, XsDevice* dev, const XsDataPacket* packet) + { + ((XsCallback*)cb)->onDataAvailable(dev, packet); + } + static void sonAllDataAvailable(XsCallbackPlainC* cb, XsDevicePtrArray* devs, const XsDataPacketPtrArray* packets) + { + ((XsCallback*)cb)->onAllDataAvailable(devs, packets); + } + static void sonRecordedDataAvailable(XsCallbackPlainC* cb, XsDevice* dev, const XsDataPacket* packet) + { + ((XsCallback*)cb)->onRecordedDataAvailable(dev, packet); + } + static void sonAllRecordedDataAvailable(XsCallbackPlainC* cb, XsDevicePtrArray* devs, const XsDataPacketPtrArray* packets) + { + ((XsCallback*)cb)->onAllRecordedDataAvailable(devs, packets); + } + static void sonTransmissionRequest(XsCallbackPlainC* cb, int channelId, const XsByteArray* data) + { + ((XsCallback*)cb)->onTransmissionRequest(channelId, data); + } + static void sonRestoreCommunication(XsCallbackPlainC* cb, const XsString* portName, XsResultValue result) + { + ((XsCallback*)cb)->onRestoreCommunication(portName, result); + } }; #endif diff --git a/lib/xspublic/xscontroller/xscallbackplainc.h b/lib/xspublic/xscontroller/xscallbackplainc.h index 1f48d44..acd09d6 100644 --- a/lib/xspublic/xscontroller/xscallbackplainc.h +++ b/lib/xspublic/xscontroller/xscallbackplainc.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -73,7 +73,7 @@ #include "xsprotocoltype.h" #ifndef __cplusplus -#define XSCALLBACK_INITIALIZER { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 } + #define XSCALLBACK_INITIALIZER { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 } #endif struct XsDevice; @@ -98,10 +98,10 @@ struct XsByteArray; */ typedef struct XsCallbackPlainC { -/*! \defgroup Callbacks Callback functions. - \addtogroup Callbacks - @{ -*/ + /*! \defgroup Callbacks Callback functions. + \addtogroup Callbacks + @{ + */ /*! \brief Called when a device's state has changed (ie config mode, measurement mode, recording mode) \param dev The device that initiated the callback. This may be 0 in some cases. \param newState The new device state @@ -145,7 +145,7 @@ typedef struct XsCallbackPlainC \returns true if the write to file should be allowed. Note that if ANY callback decides that the write is not allowed, it will be disallowed. \sa m_onWriteDataToLogFile */ - int (*m_onWriteMessageToLogFile)(struct XsCallbackPlainC* thisPtr, struct XsDevice* dev, const struct XsMessage* message); + int (*m_onWriteMessageToLogFile)(struct XsCallbackPlainC* thisPtr, struct XsDevice* dev, const struct XsMessage* message); /*! \brief Called when XDA has a data packet that could be written to a log file. \details This callback is for the Buffered stream, which will attempt to retransmit missed data when in Recording mode. So there should be no gaps (when recording), but the data arrival may be delayed a bit. When not recording, the behaviour is identical to the Live stream. @@ -181,7 +181,7 @@ typedef struct XsCallbackPlainC \param dev The device that generated the error message \param message The message that has been received */ - void (*m_onNonDataMessage)(struct XsCallbackPlainC* thisPtr, struct XsDevice* dev, struct XsMessage const * message); + void (*m_onNonDataMessage)(struct XsCallbackPlainC* thisPtr, struct XsDevice* dev, struct XsMessage const* message); /*! \brief Called just after a message is detected in raw data from the device. \param dev The device that sent the message @@ -189,19 +189,19 @@ typedef struct XsCallbackPlainC \param rawMessage The raw message that has been detected \note This message can be invalid, since it wasn't checked for sanity */ - void(*m_onMessageDetected)(struct XsCallbackPlainC* thisPtr, struct XsDevice* dev, XsProtocolType type, struct XsByteArray const * rawMessage); + void(*m_onMessageDetected)(struct XsCallbackPlainC* thisPtr, struct XsDevice* dev, XsProtocolType type, struct XsByteArray const* rawMessage); /*! \brief Called just after a valid message (after parsing) is received from the device. \param dev The device that sent the message \param message The message that has been received */ - void (*m_onMessageReceivedFromDevice)(struct XsCallbackPlainC* thisPtr, struct XsDevice* dev, struct XsMessage const * message); + void (*m_onMessageReceivedFromDevice)(struct XsCallbackPlainC* thisPtr, struct XsDevice* dev, struct XsMessage const* message); /*! \brief Called just after a message is sent to the device. \param dev The device that will receive the message \param message The message that will be sent */ - void (*m_onMessageSentToDevice)(struct XsCallbackPlainC* thisPtr, struct XsDevice* dev, struct XsMessage const * message); + void (*m_onMessageSentToDevice)(struct XsCallbackPlainC* thisPtr, struct XsDevice* dev, struct XsMessage const* message); /*! \brief Called when new data has been received for devices connected to the same main device. When processing on PC is enabled, this callback occurs after processing has been done and so the packet will contain the processing output. \details This callback is for the Live stream, so there may be gaps in the data, but it will always contain the latest data. This stream will interpolate missing data to provide the fastest data output. @@ -267,13 +267,13 @@ typedef struct XsCallbackPlainC */ void (*m_onTransmissionRequest)(struct XsCallbackPlainC* thisPtr, int channelId, const struct XsByteArray* data); - /*! \brief Called when restore communication is completed, stopped or an error occured. + /*! \brief Called when restore communication is completed, stopped or an error occurred. \param portName A name of port to which device is attached. \param result The result code. */ void (*m_onRestoreCommunication)(struct XsCallbackPlainC* thisPtr, const struct XsString* portName, XsResultValue result); -//! @} + //! @} #ifdef __cplusplus // Make sure that this struct is not used in C++ (except as base class for XsCallback) friend class XsCallback; @@ -305,8 +305,8 @@ typedef struct XsCallbackPlainC {} ~XsCallbackPlainC() throw() {} private: - XsCallbackPlainC(XsCallbackPlainC const &); - XsCallbackPlainC& operator = (XsCallbackPlainC const &); + XsCallbackPlainC(XsCallbackPlainC const&); + XsCallbackPlainC& operator = (XsCallbackPlainC const&); #endif } XsCallbackPlainC; diff --git a/lib/xspublic/xscontroller/xsconnectivitystate.c b/lib/xspublic/xscontroller/xsconnectivitystate.c index 4d78780..c4e7d59 100644 --- a/lib/xspublic/xscontroller/xsconnectivitystate.c +++ b/lib/xspublic/xscontroller/xsconnectivitystate.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -70,18 +70,19 @@ */ /*! \brief Convert the device state to a human readable string */ -const char *XsConnectivityState_toString(XsConnectivityState s) +const char* XsConnectivityState_toString(XsConnectivityState s) { - switch (s) { - XS_ENUM_TO_STR_CASE(XCS_Disconnected); - XS_ENUM_TO_STR_CASE(XCS_Rejected); - XS_ENUM_TO_STR_CASE(XCS_PluggedIn); - XS_ENUM_TO_STR_CASE(XCS_Wireless); - XS_ENUM_TO_STR_CASE(XCS_WirelessOutOfRange); - XS_ENUM_TO_STR_CASE(XCS_File); - XS_ENUM_TO_STR_CASE(XCS_Unknown); - default: - ; + switch (s) + { + XS_ENUM_TO_STR_CASE(XCS_Disconnected); + XS_ENUM_TO_STR_CASE(XCS_Rejected); + XS_ENUM_TO_STR_CASE(XCS_PluggedIn); + XS_ENUM_TO_STR_CASE(XCS_Wireless); + XS_ENUM_TO_STR_CASE(XCS_WirelessOutOfRange); + XS_ENUM_TO_STR_CASE(XCS_File); + XS_ENUM_TO_STR_CASE(XCS_Unknown); + default: + ; } return "UnknownState"; } diff --git a/lib/xspublic/xscontroller/xsconnectivitystate.h b/lib/xspublic/xscontroller/xsconnectivitystate.h index 2715b7a..02bb170 100644 --- a/lib/xspublic/xscontroller/xsconnectivitystate.h +++ b/lib/xspublic/xscontroller/xsconnectivitystate.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -65,10 +65,8 @@ #ifndef XSCONNECTIVITYSTATE_H #define XSCONNECTIVITYSTATE_H -#include "xscontrollerconfig.h" - #ifdef __cplusplus -#include + #include #endif /*! \addtogroup enums Global enumerations @@ -77,7 +75,8 @@ //AUTO namespace xscontroller { /*! \brief XsDevice connectivity state identifiers */ -enum XsConnectivityState { +enum XsConnectivityState +{ XCS_Disconnected, /*!< Device has disconnected, only limited informational functionality is available. */ XCS_Rejected, /*!< Device has been rejected and is disconnected, only limited informational functionality is available. */ XCS_PluggedIn, /*!< Device is connected through a cable. */ @@ -96,7 +95,7 @@ extern "C" { #endif /*! \brief Convert the device state to a human readable string */ -const char *XsConnectivityState_toString(XsConnectivityState s); +XDA_DLL_API const char* XsConnectivityState_toString(XsConnectivityState s); #ifdef __cplusplus } // extern "C" @@ -104,11 +103,11 @@ const char *XsConnectivityState_toString(XsConnectivityState s); #ifndef XSENS_NO_STL namespace std { - template - basic_ostream<_CharT, _Traits>& operator<<(basic_ostream<_CharT, _Traits>& o, XsConnectivityState const& xs) - { - return (o << XsConnectivityState_toString(xs)); - } +template +basic_ostream<_CharT, _Traits>& operator<<(basic_ostream<_CharT, _Traits>& o, XsConnectivityState const& xs) +{ + return (o << XsConnectivityState_toString(xs)); +} } #endif #endif diff --git a/lib/xspublic/xscontroller/xscontrol_def.cpp b/lib/xspublic/xscontroller/xscontrol_def.cpp index 485342d..59b214d 100644 --- a/lib/xspublic/xscontroller/xscontrol_def.cpp +++ b/lib/xspublic/xscontroller/xscontrol_def.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -139,7 +139,6 @@ using namespace XsMath; */ XsControl::XsControl() : m_useFakeMessages(true) - , m_synchronousDataReport(false) , m_lastHwError(XRV_OK) , m_lastHwErrorDeviceId(0) , m_recording(false) @@ -165,11 +164,13 @@ XsControl::XsControl() */ XsControl::~XsControl() { - try { + try + { close(); delete m_broadcaster; delete m_restoreCommunication; - } catch(...) + } + catch (...) {} delete m_deviceFactory; @@ -196,9 +197,7 @@ void XsControl::close() XSEXITLOGD(gJournal); if (!m_broadcaster->isReadingFromFile()) - { m_broadcaster->gotoConfig(); - } std::vector localList = m_deviceList; for (std::vector::iterator it = localList.begin(); it != localList.end(); ++it) @@ -237,12 +236,12 @@ void XsControl::closePort(int portNr) \param device the XsDevice obtained with the device() function \sa device() */ -void XsControl::closePort(XsDevice *device) +void XsControl::closePort(XsDevice* device) { JLDEBUGG(device); XSEXITLOGD(gJournal); - for (uint16_t i = 0;i < m_deviceList.size();++i) + for (uint16_t i = 0; i < m_deviceList.size(); ++i) { if (device == m_deviceList[i]) { @@ -267,7 +266,7 @@ void XsControl::closePort(const XsString& portname) LockReadWrite portLock(&m_portMutex); portLock.lock(true); - for (uint16_t i = 0;i < m_deviceList.size();++i) + for (uint16_t i = 0; i < m_deviceList.size(); ++i) { if (portname == m_deviceList[i]->portName()) closePort(m_deviceList[i]); @@ -307,7 +306,7 @@ void XsControl::flushInputBuffers() for (uint32_t i = 0; i < m_deviceList.size(); i++) // loop over all devices, { - XsDevice *dev = m_deviceList[i]; + XsDevice* dev = m_deviceList[i]; dev->flushInputBuffers(); // flush data buffer of each device. } } @@ -320,7 +319,7 @@ int XsControl::deviceCount() const int count = 0; for (uint32_t i = 0; i < m_deviceList.size(); i++) { - XsDevice *dev = m_deviceList[i]; + XsDevice* dev = m_deviceList[i]; count += 1 + (int) dev->childCount(); } m_lastResult = XRV_OK; @@ -366,7 +365,7 @@ XsDevice* XsControl::getDeviceFromLocationId(uint16_t locationId) const XSEXITLOGD(gJournal); for (uint16_t i = 0; i < m_deviceList.size(); ++i) { - XsDevice *d = m_deviceList[i]->getDeviceFromLocationId(locationId); + XsDevice* d = m_deviceList[i]->getDeviceFromLocationId(locationId); if (!d) continue; m_lastResult = XRV_OK; @@ -432,7 +431,7 @@ XsString XsControl::lastResultText() const */ int XsControl::mainDeviceCount() const { -// m_lastResult = XRV_OK; + // m_lastResult = XRV_OK; return (int) m_deviceList.size(); } @@ -449,7 +448,7 @@ int XsControl::mtCount() const int count = 0; for (uint32_t i = 0; i < m_deviceList.size(); i++) { - XsDevice *dev = m_deviceList[i]; + XsDevice* dev = m_deviceList[i]; if (dev->isMotionTracker()) count++; } @@ -491,9 +490,9 @@ std::vector XsControl::mtDeviceIds() const portLock.lock(false); std::vector result; - for (uint32_t i = 0;i < m_deviceList.size();++i) + for (uint32_t i = 0; i < m_deviceList.size(); ++i) { - XsDevice const *main = m_deviceList.at(i); + XsDevice const* main = m_deviceList.at(i); if (main->isMotionTracker()) result.push_back(main->deviceId()); } @@ -502,11 +501,11 @@ std::vector XsControl::mtDeviceIds() const /*! \brief Place all sensors connected through a serial port into Configuration Mode. - This function is called before close() in the destructor of the class. - /sa close() + This function is called before close() in the destructor of the class. + /sa close() - \internal - The function places the sensors in configuration mode in the appropriate order as they are sorted by sortBySync. + \internal + The function places the sensors in configuration mode in the appropriate order as they are sorted by sortBySync. */ void XsControl::gotoConfig(void) { @@ -536,7 +535,7 @@ void XsControl::gotoMeasurement() \param portName the name of port to which device is connected. \returns XRV_OK if restore communication procedure was successful. */ -XsResultValue XsControl::startRestoreCommunication(const XsString & portName) +XsResultValue XsControl::startRestoreCommunication(const XsString& portName) { return m_restoreCommunication->start(portName); } @@ -599,13 +598,13 @@ XsDevice* XsControl::addMasterDevice(Communicator* communicator) /*! \endcond */ /*! \brief Open the log file with the given \a filename. - \returns True is the file was opened successfully. False if an error was encountered. + \returns True is the file was opened successfully. False if an error was encountered. \param filename the name of the file to open \returns true on success, false on failure - \sa lastResult(), loadLogFile(), logFileName() + \sa lastResult(), loadLogFile(), logFileName() */ bool XsControl::openLogFile(const XsString& filename) { @@ -629,7 +628,10 @@ bool XsControl::openLogFile(const XsString& filename) // note that addMasterDevice ALWAYS takes ownership of communicator XsDevice* dev = addMasterDevice(object.release()); if (!dev) + { + m_lastResult = XRV_DEVICEERROR; return false; + } dev->resetLogFileReadPosition(); // this will and should call reinitializeProcessors(); @@ -679,7 +681,7 @@ bool XsControl::openPort(int portNr, XsBaudRate baudrate, uint32_t timeout, bool \sa openPort(int, XsBaudRate, uint32_t, bool) */ -bool XsControl::openPort(const XsString &portname, XsBaudRate baudrate, uint32_t timeout, bool detectRs485) +bool XsControl::openPort(const XsString& portname, XsBaudRate baudrate, uint32_t timeout, bool detectRs485) { XsPortInfo localPortInfo = XsPortInfo(portname, baudrate); uint16_t vid = vidFromString(portname.toStdString()); @@ -691,9 +693,9 @@ bool XsControl::openPort(const XsString &portname, XsBaudRate baudrate, uint32_t /*! \cond XS_INTERNAL */ /*! \brief Finalize opening the port - Takes ownership of the passed Communicator. + Takes ownership of the passed Communicator. */ -bool XsControl::finalizeOpenPort(Communicator *communicator, XsPortInfo &portinfo, uint32_t timeout, bool detectRs485) +bool XsControl::finalizeOpenPort(Communicator* communicator, XsPortInfo& portinfo, uint32_t timeout, bool detectRs485) { XSEXITLOGD(gJournal); if (!communicator) @@ -739,7 +741,7 @@ bool XsControl::openPort(XsPortInfo& portinfo, uint32_t timeout, bool detectRs48 JLDEBUGG("port " << portinfo << " timeout " << timeout << " detectRs485 " << (int32_t)detectRs485); XSEXITLOGD(gJournal); - Communicator *xs3info = findXbusInterface(portinfo); + Communicator* xs3info = findXbusInterface(portinfo); if (xs3info) { if (xs3info->masterDeviceId().isValid() && !portinfo.deviceId().isValid()) @@ -765,7 +767,7 @@ bool XsControl::openPortWithCredentials(XsPortInfo& portinfo, XsString const& id JLDEBUGG("port " << portinfo << " id " << id << " key " << key << " timeout " << timeout); XSEXITLOGD(gJournal); - Communicator *xs3info = findXbusInterface(portinfo); + Communicator* xs3info = findXbusInterface(portinfo); if (xs3info) { if (xs3info->masterDeviceId().isValid()) @@ -875,7 +877,7 @@ void XsControl::transmissionReceived(int channelId, const XsByteArray& data) \sa openPort(int, XsBaudRate, bool) */ -bool XsControl::openImarPort_internal(const XsString &, XsBaudRate , int , uint32_t ) +bool XsControl::openImarPort_internal(const XsString&, XsBaudRate, int, uint32_t) { return false; } @@ -904,8 +906,9 @@ XsDeviceId XsControl::dockDeviceId(const XsDeviceId& deviceId) const if (!deviceId.isMtw()) return XsDeviceId(); - XsDevice *dev = findDevice(deviceId); - if (!dev) { + XsDevice* dev = findDevice(deviceId); + if (!dev) + { m_lastResult = XRV_INVALIDID; return XsDeviceId(); } @@ -962,7 +965,7 @@ XsDevicePtrArray XsControl::mainDevices() const they support it) \returns An XsDevice pointer representing the broadcast device */ - XsDevice* XsControl::broadcast() const +XsDevice* XsControl::broadcast() const { return m_broadcaster; } @@ -1000,7 +1003,7 @@ XsDevice* XsControl::findDevice(const XsDeviceId& deviceId) const for (size_t i = 0; i < m_deviceList.size(); i++) { - XsDevice *dev = m_deviceList[i]; + XsDevice* dev = m_deviceList[i]; if (!dev) break; @@ -1020,9 +1023,9 @@ XsDevice* XsControl::findDevice(const XsDeviceId& deviceId) const /*! \brief Close an existing device. Because it was detected somewhere else. \param deviceId the device ID to look for */ -void XsControl::removeExistingDevice(XsDeviceId const & deviceId) +void XsControl::removeExistingDevice(XsDeviceId const& deviceId) { - XsDevice *dev = findDevice(deviceId); + XsDevice* dev = findDevice(deviceId); if (!dev) return; @@ -1034,9 +1037,9 @@ void XsControl::removeExistingDevice(XsDeviceId const & deviceId) \param deviceId The ID of device object to search in \returns The found communication interface */ -Communicator* XsControl::findXbusInterface(const XsDeviceId &deviceId) const +Communicator* XsControl::findXbusInterface(const XsDeviceId& deviceId) const { - XsDevice *dev = findDevice(deviceId); + XsDevice* dev = findDevice(deviceId); if (!dev) return NULL; @@ -1047,7 +1050,7 @@ Communicator* XsControl::findXbusInterface(const XsDeviceId &deviceId) const \param portInfo The port information object to search in \returns The found communication interface */ -Communicator* XsControl::findXbusInterface(const XsPortInfo &portInfo) const +Communicator* XsControl::findXbusInterface(const XsPortInfo& portInfo) const { return findXbusInterface(portInfo.portName()); } @@ -1056,7 +1059,7 @@ Communicator* XsControl::findXbusInterface(const XsPortInfo &portInfo) const \param portName The name of port to search in \returns The found communication interface */ -Communicator* XsControl::findXbusInterface(const XsString &portName) const +Communicator* XsControl::findXbusInterface(const XsString& portName) const { for (size_t i = 0; i < m_deviceList.size(); ++i) { @@ -1080,7 +1083,7 @@ Communicator* XsControl::findXbusInterface(const XsString &portName) const \returns true on success, false on failure */ -bool XsControl::loadFilterProfiles(const XsString& ) +bool XsControl::loadFilterProfiles(const XsString&) { return false; } @@ -1092,9 +1095,9 @@ bool XsControl::loadFilterProfiles(const XsString& ) void XsControl::closePortByIndex(uint32_t i) { JLDEBUGG(i); -// XSEXITLOGD(gJournal); + // XSEXITLOGD(gJournal); - XsDevice *dev = m_deviceList[i]; + XsDevice* dev = m_deviceList[i]; closePort(dev); } /*! \endcond */ @@ -1154,7 +1157,7 @@ void XsControl::setOptions(XsOption enable, XsOption disable) void XsControl::setOptionsForce(XsOption enabled) { m_optionsEnable = XsOption_purify(enabled); - m_optionsDisable = ((~m_optionsEnable) & XSO_All); + m_optionsDisable = (XsOption)((~m_optionsEnable) & XSO_All); m_broadcaster->setOptions(m_optionsEnable, m_optionsDisable); } @@ -1173,12 +1176,11 @@ void XsControl::setPersistentSettings(XsDevice* dev) /*! \endcond */ #ifndef XDA_PRIVATE_BUILD -#include "xscontrol_public.h" + #include "xscontrol_public.h" #else -#include "xscontrolex.h" + #include "xscontrolex.h" #endif XsControl* XsControl::construct() { return new XsControlEx; } -#include "xscontrol_def.h" diff --git a/lib/xspublic/xscontroller/xscontrol_def.h b/lib/xspublic/xscontroller/xscontrol_def.h index 348824d..661251c 100644 --- a/lib/xspublic/xscontroller/xscontrol_def.h +++ b/lib/xspublic/xscontroller/xscontrol_def.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -123,8 +123,10 @@ struct XsDeviceConfiguration; struct XsDevicePtrArray; //AUTO } -#define XsensThreadReturn XSENS_THREAD_RETURN // for generator -#define XsensThreadParam XSENS_THREAD_PARAM // for generator +#ifndef XsensThreadReturn + #define XsensThreadReturn XSENS_THREAD_RETURN // for generator + #define XsensThreadParam XSENS_THREAD_PARAM // for generator +#endif struct XsControl : public CallbackManagerXda { @@ -152,7 +154,7 @@ struct XsControl : public CallbackManagerXda void closePort(const XsPortInfo& portinfo); void closeCustomPort(int channelId); #ifndef XSENS_NO_PORT_NUMBERS - XSNOLINUXEXPORT void closePort (int portNr); + XSNOLINUXEXPORT void closePort(int portNr); #endif void closePort(XsDevice* device); @@ -201,20 +203,22 @@ struct XsControl : public CallbackManagerXda // these are only required to allow using the lib the same way as to using the dll static XSNOEXPORT XsControl* construct(); XSNOEXPORT void destruct() - { delete this; } + { + delete this; + } - virtual bool XSNOEXPORT finalizeOpenPort(Communicator *communicator, XsPortInfo &portinfo, uint32_t timeout, bool detectRs485); + virtual bool XSNOEXPORT finalizeOpenPort(Communicator* communicator, XsPortInfo& portinfo, uint32_t timeout, bool detectRs485); void gotoConfig(); void gotoMeasurement(); - XsResultValue startRestoreCommunication(const XsString &portName); + XsResultValue startRestoreCommunication(const XsString& portName); void stopRestoreCommunication(); protected: virtual XsDevice* XSNOCOMEXPORT addMasterDevice(Communicator* communicator); #ifndef DOXYGEN - XsControl(const XsControl& ref); + XSNOEXPORT XsControl(const XsControl&) = delete; #endif //void gotoOperational(const XsDeviceId& stationId = XsDeviceId()); @@ -222,9 +226,6 @@ struct XsControl : public CallbackManagerXda //! Boolean variable for enabling/disabling the use of fake messages bool m_useFakeMessages; - //! \see setSynchronousDataReport - bool m_synchronousDataReport; - //! This list contains device-information and cached data per device. std::vector m_deviceList; @@ -250,23 +251,16 @@ struct XsControl : public CallbackManagerXda //! AwindaStationIndication of threads started or not volatile std::atomic_bool m_recording; -#ifndef SWIG - //! The start of the thread - XSNOEXPORT static XsensThreadReturn XSENS_THREAD_TYPE threadInit(XsensThreadParam); - //! The start of the treadEx thread - XSNOEXPORT static XsensThreadReturn XSENS_THREAD_TYPE threadExInit(XsensThreadParam); -#endif - void updateRecordingState(); - XsDevice *findDevice(const XsDeviceId &deviceId) const; + XsDevice* findDevice(const XsDeviceId& deviceId) const; - virtual void removeExistingDevice(XsDeviceId const & deviceId); + virtual void removeExistingDevice(XsDeviceId const& deviceId); //! Find the xs3 info of the given id - Communicator* findXbusInterface(const XsDeviceId &deviceId) const; - Communicator* findXbusInterface(const XsPortInfo &portInfo) const; - Communicator* findXbusInterface(const XsString &portName) const; + Communicator* findXbusInterface(const XsDeviceId& deviceId) const; + Communicator* findXbusInterface(const XsPortInfo& portInfo) const; + Communicator* findXbusInterface(const XsString& portName) const; void closePortByIndex(uint32_t index); @@ -285,18 +279,18 @@ struct XsControl : public CallbackManagerXda void setPersistentSettings(XsDevice* dev); //! The device factory object - DeviceFactory *m_deviceFactory; + DeviceFactory* m_deviceFactory; //! The communicator factory object - XdaCommunicatorFactory *m_communicatorFactory; + XdaCommunicatorFactory* m_communicatorFactory; //! The restore communication object - RestoreCommunication *m_restoreCommunication; + RestoreCommunication* m_restoreCommunication; -/*! \cond XS_INTERNAL */ + /*! \cond XS_INTERNAL */ friend class BroadcastDevice; friend class BroadcastForwardFunc; -/*! \endcond */ // XS_INTERNAL + /*! \endcond */ // XS_INTERNAL }; #endif diff --git a/lib/xspublic/xscontroller/xscontrol_public.h b/lib/xspublic/xscontroller/xscontrol_public.h index 6f36eb6..895bcfc 100644 --- a/lib/xspublic/xscontroller/xscontrol_public.h +++ b/lib/xspublic/xscontroller/xscontrol_public.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xscontroller/xscontrollerconfig.h b/lib/xspublic/xscontroller/xscontrollerconfig.h index 0b0848b..98d2ec1 100644 --- a/lib/xspublic/xscontroller/xscontrollerconfig.h +++ b/lib/xspublic/xscontroller/xscontrollerconfig.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -69,7 +69,7 @@ // define to build with Journaller #ifndef HAVE_JOURNALLER -//#define HAVE_JOURNALLER + //#define HAVE_JOURNALLER #endif ////////////////////////////////////////////////// @@ -77,109 +77,111 @@ // make sure both _WIN32 and WIN32 are defined if either of them is. #if defined(_WIN32) || defined(_M_IX86) -# ifndef WIN32 -# define WIN32 -# endif -# define XSENS_WINDOWS + #ifndef WIN32 + #define WIN32 + #endif + #ifndef XSENS_WINDOWS + #define XSENS_WINDOWS + #endif #endif #ifdef WIN32 -# ifndef _WIN32 -# define _WIN32 -# define XSENS_WINDOWS -# endif + #ifndef _WIN32 + #define _WIN32 + #define XSENS_WINDOWS + #endif #endif #ifdef _WIN32 -# define USE_WINUSB + #define USE_WINUSB #else -# define XSENS_NO_PORT_NUMBERS + #define XSENS_NO_PORT_NUMBERS #endif // make things as secure as possible without modifying the code... #ifndef _CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES -#define _CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES 1 + #define _CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES 1 #endif #ifndef _CRT_SECURE_NO_WARNINGS -#define _CRT_SECURE_NO_WARNINGS + #define _CRT_SECURE_NO_WARNINGS #endif #ifdef __GNUC__ -#include -#if __WORDSIZE == 64 -# define XSENS_64BIT -#else -# define XSENS_32BIT -#endif + #include + #if __WORDSIZE == 64 + #define XSENS_64BIT + #else + #define XSENS_32BIT + #endif #endif #if defined(_WIN64) || defined(_M_X64) || defined(_M_IA64) -# ifndef XSENS_64BIT -# define XSENS_64BIT -# endif -# ifndef XSENS_WINDOWS -# define XSENS_WINDOWS -# endif -# ifndef WIN64 -# define WIN64 -# endif + #ifndef XSENS_64BIT + #define XSENS_64BIT + #endif + #ifndef XSENS_WINDOWS + #define XSENS_WINDOWS + #endif + #ifndef WIN64 + #define WIN64 + #endif #else -# ifndef XSENS_32BIT -# define XSENS_32BIT -# endif + #ifndef XSENS_32BIT + #define XSENS_32BIT + #endif #endif // all xsens libraries should use unicode #ifndef UNICODE -#define UNICODE + #define UNICODE #endif // use XSENS_32BIT and XSENS_64BIT to check for 32/64 bit builds in your application // on non-windows systems these should be defined in this file #ifndef XDA_DLL_API -# ifdef XDA_DLL_EXPORT -# ifdef _WIN32 -//# pragma message("XDA_DLL_API export in xscontrollerconfig.h") -# define XDA_DLL_API __declspec(dllexport) -# else -//# pragma message("XDA_DLL_API linux export in xscontrollerconfig.h") -# define XDA_DLL_API __attribute__((visibility("default"))) -# endif -# else // ifdef XDA_DLL_EXPORT -# ifdef XDA_STATIC_LIB -//# pragma message("XDA_DLL_API static in xscontrollerconfig.h") -# define XDA_DLL_API -# else -# ifdef _WIN32 -//# pragma message("XDA_DLL_API import in xscontrollerconfig.h") -# define XDA_DLL_API __declspec(dllimport) -# else -//# pragma message("XDA_DLL_API import/static for linux in xscontrollerconfig.h") -# define XDA_DLL_API -# endif -# endif -# endif // ifdef XDA_DLL_EXPORT - else + #ifdef XDA_DLL_EXPORT + #ifdef _WIN32 + //# pragma message("XDA_DLL_API export in xscontrollerconfig.h") + #define XDA_DLL_API __declspec(dllexport) + #else + //# pragma message("XDA_DLL_API linux export in xscontrollerconfig.h") + #define XDA_DLL_API __attribute__((visibility("default"))) + #endif + #else // ifdef XDA_DLL_EXPORT + #ifdef XDA_STATIC_LIB + //# pragma message("XDA_DLL_API static in xscontrollerconfig.h") + #define XDA_DLL_API + #else + #ifdef _WIN32 + //# pragma message("XDA_DLL_API import in xscontrollerconfig.h") + #define XDA_DLL_API __declspec(dllimport) + #else + //# pragma message("XDA_DLL_API import/static for linux in xscontrollerconfig.h") + #define XDA_DLL_API + #endif + #endif + #endif // ifdef XDA_DLL_EXPORT - else #endif // ifndef XDA_DLL_API /* -Configuration | Runtime | DebInfo | Defines ---------------+--------------------------------------- -Debug | MDd | Yes | XSENS_DEBUG;_DEBUG -RelWithDeb | MD | Yes | XSENS_DEBUG;XSENS_RELEASE;_DEBUG -Release | MD | No | XSENS_RELEASE;NDEBUG - -The common way to setup configuration-dependent defines: -#if defined(XSENS_DEBUG) + Configuration | Runtime | DebInfo | Defines + --------------+--------------------------------------- + Debug | MDd | Yes | XSENS_DEBUG;_DEBUG + RelWithDeb | MD | Yes | XSENS_DEBUG;XSENS_RELEASE;_DEBUG + Release | MD | No | XSENS_RELEASE;NDEBUG + + The common way to setup configuration-dependent defines: + #if defined(XSENS_DEBUG) //// Debug or RelWithDeb build #if defined(XSENS_RELEASE) //// RelWithDeb build #else //// Debug build #endif -#else + #else //// Release build -#endif + #endif */ ////////////////////////////////////////////////// @@ -187,64 +189,68 @@ The common way to setup configuration-dependent defines: #ifndef XSENS_DISABLE_COPY //! Add this macro to the start of a class definition to prevent automatic creation of copy functions #define XSENS_DISABLE_COPY(className) \ -private: \ + private: \ className(className const &) = delete; \ className &operator = (className const &) = delete #endif #ifdef __cplusplus -#ifdef HAVE_JOURNALLER -#include -#else -class Journaller; -#endif -extern Journaller* gJournal; + #ifdef HAVE_JOURNALLER + #include + #else + class Journaller; + #endif + extern Journaller* gJournal; #endif #if defined(HAVE_JOURNALLER) && (defined(XSENS_DEBUG) || defined(XS_LOG_ALWAYS)) -# define XSEXITLOGC(j) JournalValueJanitor _xsExitLogC(j, m_lastResult, std::string(__FUNCTION__) + " exit, lastResult = ") -# define XSEXITLOGD(j) XSEXITLOGC(j) //JournalValueJanitor _xsExitLogD(j, d->m_lastResult, std::string(__FUNCTION__) + " exit, lastResult = ") -# define XSEXITLOGN(j) JournalValueJanitor _xsExitLogN(j, std::string(), std::string(__FUNCTION__) + " exit") -# ifndef DIDLOG -# define DIDLOG(d) JLHEXLOG_BARE(d) -# endif + #define XSEXITLOGC(j) JournalValueJanitor _xsExitLogC(j, m_lastResult, std::string(__FUNCTION__) + " exit, lastResult = ") + #define XSEXITLOGD(j) XSEXITLOGC(j) //JournalValueJanitor _xsExitLogD(j, d->m_lastResult, std::string(__FUNCTION__) + " exit, lastResult = ") + #define XSEXITLOGN(j) JournalValueJanitor _xsExitLogN(j, std::string(), std::string(__FUNCTION__) + " exit") + #ifndef DIDLOG + #define DIDLOG(d) JLHEXLOG_BARE(d) + #endif #else -# define XSEXITLOGC(j) ((void) 0) -# define XSEXITLOGD(j) ((void) 0) -# define XSEXITLOGN(j) ((void) 0) -# ifndef DIDLOG -# define DIDLOG(d) "" -# endif - -#if !defined(HAVE_JOURNALLER) && !defined(JLDEBUG) -#define JLTRACE(...) ((void)0) -#define JLTRACE_NODEC(...) ((void)0) -#define JLTRACEG(...) ((void)0) -#define JLDEBUG(...) ((void)0) -#define JLDEBUG_NODEC(...) ((void)0) -#define JLDEBUGG(...) ((void)0) -#define JLALERT(...) ((void)0) -#define JLALERT_NODEC(...) ((void)0) -#define JLALERTG(...) ((void)0) -#define JLERROR(...) ((void)0) -#define JLERROR_NODEC(...) ((void)0) -#define JLERRORG(...) ((void)0) -#define JLFATAL(...) ((void)0) -#define JLFATAL_NODEC(...) ((void)0) -#define JLFATALG(...) ((void)0) -#define JLWRITE(...) ((void)0) -#define JLWRITE_NODEC(...) ((void)0) -#define JLWRITEG(...) ((void)0) + #define XSEXITLOGC(j) ((void) 0) + #define XSEXITLOGD(j) ((void) 0) + #define XSEXITLOGN(j) ((void) 0) + #ifndef DIDLOG + #define DIDLOG(d) "" + #endif -#endif + #if !defined(HAVE_JOURNALLER) && !defined(JLDEBUG) + #define JLTRACE(...) ((void)0) + #define JLTRACE_NODEC(...) ((void)0) + #define JLTRACEG(...) ((void)0) + #define JLDEBUG(...) ((void)0) + #define JLDEBUG_NODEC(...) ((void)0) + #define JLDEBUGG(...) ((void)0) + #define JLALERT(...) ((void)0) + #define JLALERT_NODEC(...) ((void)0) + #define JLALERTG(...) ((void)0) + #define JLERROR(...) ((void)0) + #define JLERROR_NODEC(...) ((void)0) + #define JLERRORG(...) ((void)0) + #define JLFATAL(...) ((void)0) + #define JLFATAL_NODEC(...) ((void)0) + #define JLFATALG(...) ((void)0) + #define JLWRITE(...) ((void)0) + #define JLWRITE_NODEC(...) ((void)0) + #define JLWRITEG(...) ((void)0) + #define JLFINALVALUE(...) ((void)0) + #define JLWRITEFINAL(...) ((void)0) + #define JLWRITEFINALG(...) ((void)0) + #define JLDEBUGFINAL(...) ((void)0) + #define JLDEBUGFINALG(...) ((void)0) + #endif #endif #ifndef XSNOEXPORT -#define XSNOEXPORT + #define XSNOEXPORT #endif #ifndef XSNOLINUXEXPORT -#define XSNOLINUXEXPORT + #define XSNOLINUXEXPORT #endif #endif diff --git a/lib/xspublic/xscontroller/xscoordinatesystem.h b/lib/xspublic/xscontroller/xscoordinatesystem.h index 00e57ff..45abb76 100644 --- a/lib/xspublic/xscontroller/xscoordinatesystem.h +++ b/lib/xspublic/xscontroller/xscoordinatesystem.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -69,7 +69,8 @@ @{ */ /*! \brief Coordinate system definition flags */ -enum XsCoordinateSystem { +enum XsCoordinateSystem +{ CS_NorthWestUp, CS_NorthEastDown, }; diff --git a/lib/xspublic/xscontroller/xsdef.cpp b/lib/xspublic/xscontroller/xsdef.cpp index 716f309..b457831 100644 --- a/lib/xspublic/xscontroller/xsdef.cpp +++ b/lib/xspublic/xscontroller/xsdef.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -68,5 +68,5 @@ //*! \brief Scale the battery level to a percentage */ int xsScaleBatteryLevel(int batteryLevel) { - return (int) std::min(batteryLevel/2.0, 100.0); + return (int) std::min(batteryLevel / 2.0, 100.0); } diff --git a/lib/xspublic/xscontroller/xsdef.h b/lib/xspublic/xscontroller/xsdef.h index 74cad94..8b8be8b 100644 --- a/lib/xspublic/xscontroller/xsdef.h +++ b/lib/xspublic/xscontroller/xsdef.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -81,19 +81,19 @@ ////////////////////////////////////////////////////////////////////////////////////////// // Field message indices #define XS_IND_PREAMBLE 0 -#define XS_IND_BID 1 -#define XS_IND_MID 2 -#define XS_IND_LEN 3 -#define XS_IND_DATA0 4 -#define XS_IND_LENEXTH 4 -#define XS_IND_LENEXTL 5 +#define XS_IND_BID 1 +#define XS_IND_MID 2 +#define XS_IND_LEN 3 +#define XS_IND_DATA0 4 +#define XS_IND_LENEXTH 4 +#define XS_IND_LENEXTL 5 #define XS_IND_DATAEXT0 6 -#define XS_SELFTEST_OK 0x1FF +#define XS_SELFTEST_OK 0x1FF // message data lengths #define XS_LEN_TRANSPORTMODE 1 -#define XS_LEN_DEVICEID 4 +#define XS_LEN_DEVICEID 4 #define XS_LEN_INITBUSRESULTS 4 #define XS_LEN_PERIOD 2 #define XS_LEN_BUSPWR 2 @@ -102,17 +102,17 @@ #define XS_LEN_FIRMWAREREV 3 #define XS_LEN_BTDISABLE 1 #define XS_LEN_OPMODE 1 -#define XS_LEN_BAUDRATE 1 -#define XS_LEN_SYNCMODE 1 +#define XS_LEN_BAUDRATE 1 +#define XS_LEN_SYNCMODE 1 #define XS_LEN_PRODUCTCODE 20 #define XS_LEN_PROCESSINGFLAGS 2 -#define XS_LEN_XMPWROFF 0 -#define XS_LEN_OUTPUTMODE 2 -#define XS_LEN_OUTPUTSETTINGS 4 -#define XS_LEN_OUTPUTSKIPFACTOR 2 +#define XS_LEN_XMPWROFF 0 +#define XS_LEN_OUTPUTMODE 2 +#define XS_LEN_OUTPUTSETTINGS 4 +#define XS_LEN_OUTPUTSKIPFACTOR 2 #define XS_LEN_SYNCINMODE 2 -#define XS_LEN_SYNCINSKIPFACTOR 2 -#define XS_LEN_SYNCINOFFSET 4 +#define XS_LEN_SYNCINSKIPFACTOR 2 +#define XS_LEN_SYNCINOFFSET 4 #define XS_LEN_SYNCOUTMODE 2 #define XS_LEN_SYNCOUTSKIPFACTOR 2 #define XS_LEN_SYNCOUTOFFSET 4 @@ -123,8 +123,8 @@ #define XS_LEN_ALIGNMENTROTATION (4*4) #define XS_LEN_XMERRORMODE 2 #define XS_LEN_BUFFERSIZE 2 -#define XS_LEN_HEADING 4 -#define XS_LEN_MAGNETICFIELD 12 +#define XS_LEN_HEADING 4 +#define XS_LEN_MAGNETICFIELD 12 #define XS_LEN_LOCATIONID 2 #define XS_LEN_EXTOUTPUTMODE 2 #define XS_LEN_INITTRACKMODE 2 @@ -135,40 +135,40 @@ #define XS_LEN_AVAILABLEFILTERPROFILES (XS_MAX_FILTERPROFILES_IN_MT*XS_LEN_FILTERPROFILEFULL) #define XS_LEN_REQFILTERPROFILEACK 2 #define XS_LEN_SETFILTERPROFILE 2 -#define XS_LEN_GRAVITYMAGNITUDE 4 +#define XS_LEN_GRAVITYMAGNITUDE 4 #define XS_LEN_GPSLEVERARM 12 #define XS_LEN_LATLONALT 18 #define XS_LEN_SETNOROTATION 2 #define XS_LEN_FILTERSETTINGS 4 #define XS_LEN_AMD 2 -#define XS_LEN_RESETORIENTATION 2 +#define XS_LEN_RESETORIENTATION 2 #define XS_LEN_GPSSTATUS (1+5*16) #define XS_LEN_CLIENTUSAGE 1 #define XS_LEN_CLIENTPRIORITY 1 #define XS_LEN_WIRELESSCONFIG 4 #define XS_LEN_INFOREQUEST 1 -#define XS_LEN_SETOUTPUTTRIGGER 10 +#define XS_LEN_SETOUTPUTTRIGGER 10 #define XS_LEN_SETINPUTTRIGGER 10 // MTData defines // Length of data blocks in bytes -#define XS_LEN_RAWDATA 20 -#define XS_LEN_CALIBDATA 36 -#define XS_LEN_CALIB_ACCDATA 12 -#define XS_LEN_CALIB_GYRDATA 12 -#define XS_LEN_CALIB_MAGDATA 12 -#define XS_LEN_ORIENT_QUATDATA 16 +#define XS_LEN_RAWDATA 20 +#define XS_LEN_CALIBDATA 36 +#define XS_LEN_CALIB_ACCDATA 12 +#define XS_LEN_CALIB_GYRDATA 12 +#define XS_LEN_CALIB_MAGDATA 12 +#define XS_LEN_ORIENT_QUATDATA 16 #define XS_LEN_ORIENT_EULERDATA 12 #define XS_LEN_ORIENT_MATRIXSTA 36 -#define XS_LEN_SAMPLECNT 2 +#define XS_LEN_SAMPLECNT 2 #define XS_LEN_TEMPDATA 4 // Length of data blocks in floats #define XS_LEN_CALIBDATA_FLT 9 -#define XS_LEN_TEMPDATA_FLT 1 +#define XS_LEN_TEMPDATA_FLT 1 #define XS_LEN_ORIENT_QUATDATA_FLT 4 -#define XS_LEN_ORIENT_EULERDATA_FLT 3 -#define XS_LEN_ORIENT_MATRIXSTA_FLT 9 +#define XS_LEN_ORIENT_EULERDATA_FLT 3 +#define XS_LEN_ORIENT_MATRIXSTA_FLT 9 #define XS_INVALIDSETTINGVALUE 0xFFFFFFFF @@ -178,14 +178,14 @@ #define XS_CONF_MASTERDID 0 #define XS_CONF_PERIOD 4 #define XS_CONF_OUTPUTSKIPFACTOR 6 -#define XS_CONF_SYNCIN_MODE 8 +#define XS_CONF_SYNCIN_MODE 8 #define XS_CONF_SYNCIN_SKIPFACTOR 10 #define XS_CONF_SYNCIN_OFFSET 12 #define XS_CONF_DATE 16 #define XS_CONF_TIME 24 #define XS_CONF_NUMDEVICES 96 // Configuration sensor block properties -#define XS_CONF_DID 98 +#define XS_CONF_DID 98 #define XS_CONF_DATALENGTH 102 #define XS_CONF_OUTPUTMODE 104 #define XS_CONF_OUTPUTSETTINGS 106 @@ -194,12 +194,12 @@ // CONF_OUTPUTMODE + 1*CONF_BLOCKLEN #define XS_CONF_MASTERDIDLEN 4 #define XS_CONF_PERIODLEN 2 -#define XS_CONF_OUTPUTSKIPFACTORLEN 2 +#define XS_CONF_OUTPUTSKIPFACTORLEN 2 #define XS_CONF_SYNCIN_MODELEN 2 #define XS_CONF_SYNCIN_SKIPFACTORLEN 2 #define XS_CONF_SYNCIN_OFFSETLEN 4 -#define XS_CONF_DATELEN 8 -#define XS_CONF_TIMELEN 8 +#define XS_CONF_DATELEN 8 +#define XS_CONF_TIMELEN 8 #define XS_CONF_RESERVED_CLIENTLEN 32 #define XS_CONF_RESERVED_HOSTLEN 32 #define XS_CONF_NUMDEVICESLEN 2 @@ -225,27 +225,27 @@ #define XS_SYNCIN_DISABLED 0x0000 #define XS_SYNCIN_EDGE_RISING 0x0001 #define XS_SYNCIN_EDGE_FALLING 0x0002 -#define XS_SYNCIN_EDGE_BOTH 0x0003 -#define XS_SYNCIN_EDGE_MASK 0x0003 +#define XS_SYNCIN_EDGE_BOTH 0x0003 +#define XS_SYNCIN_EDGE_MASK 0x0003 #define XS_SYNCIN_TYPE_DOSAMPLING 0x0000 -#define XS_SYNCIN_TYPE_SENDLASTDATA 0x0004 +#define XS_SYNCIN_TYPE_SENDLASTDATA 0x0004 #define XS_SYNCIN_TYPE_CLOCK 0x0010 -#define XS_SYNCIN_TYPE_MASK 0x001C +#define XS_SYNCIN_TYPE_MASK 0x001C // SyncOut params #define XS_PARAM_SYNCOUT_MODE 0x00 -#define XS_PARAM_SYNCOUT_SKIPFACTOR 0x01 -#define XS_PARAM_SYNCOUT_OFFSET 0x02 -#define XS_PARAM_SYNCOUT_PULSEWIDTH 0x03 +#define XS_PARAM_SYNCOUT_SKIPFACTOR 0x01 +#define XS_PARAM_SYNCOUT_OFFSET 0x02 +#define XS_PARAM_SYNCOUT_PULSEWIDTH 0x03 // SyncOut mode -#define XS_SYNCOUT_DISABLED 0x0000 +#define XS_SYNCOUT_DISABLED 0x0000 #define XS_SYNCOUT_TYPE_TOGGLE 0x0001 #define XS_SYNCOUT_TYPE_PULSE 0x0002 #define XS_SYNCOUT_POL_NEG 0x0000 #define XS_SYNCOUT_POL_POS 0x0010 #define XS_SYNCOUT_TYPE_MASK 0x000F -#define XS_SYNCOUT_POL_MASK 0x0010 +#define XS_SYNCOUT_POL_MASK 0x0010 // Initial tracking mode (SetInitTrackMode) #define XS_INITTRACKMODE_DISABLED 0x0000 @@ -253,7 +253,7 @@ // Filter settings params #define XS_PARAM_FILTER_GAIN 0x00 -#define XS_PARAM_FILTER_RHO 0x01 +#define XS_PARAM_FILTER_RHO 0x01 #define XS_DONOTSTORE 0x00 #define XS_STORE 0x01 @@ -272,7 +272,7 @@ // Timeouts #define XS_TO_DEFAULT 500 #define XS_TO_INIT 250 -#define XS_TO_RETRY 50 +#define XS_TO_RETRY 50 #define XS_PERIOD_10HZ 11520 // invalid with gps pulse time correction #define XS_PERIOD_12HZ 9600 @@ -320,37 +320,37 @@ #define XS_PERIOD_512HZ 225 #ifndef NOT_FOR_PUBLIC_RELEASE -#define XS_PERIOD_576HZ 200 -#define XS_PERIOD_600HZ 192 -#define XS_PERIOD_640HZ 180 -#define XS_PERIOD_720HZ 160 -#define XS_PERIOD_768HZ 150 -#define XS_PERIOD_800HZ 144 -#define XS_PERIOD_900HZ 128 -#define XS_PERIOD_960HZ 120 -#define XS_PERIOD_1152HZ 100 -#define XS_PERIOD_1200HZ 96 -#define XS_PERIOD_1280HZ 90 -#define XS_PERIOD_1440HZ 80 -#define XS_PERIOD_1536HZ 75 -#define XS_PERIOD_1600HZ 72 -#define XS_PERIOD_1800HZ 64 -#define XS_PERIOD_1920HZ 60 -#define XS_PERIOD_2304HZ 50 -#define XS_PERIOD_2400HZ 48 -#define XS_PERIOD_2560HZ 45 -#define XS_PERIOD_2880HZ 40 -#define XS_PERIOD_3200HZ 36 -#define XS_PERIOD_3600HZ 32 -#define XS_PERIOD_3840HZ 30 -#define XS_PERIOD_4608HZ 25 -#define XS_PERIOD_4800HZ 24 -#define XS_PERIOD_5760HZ 20 -#define XS_PERIOD_6400HZ 18 -#define XS_PERIOD_7200HZ 16 -#define XS_PERIOD_7680HZ 15 -#define XS_PERIOD_9600HZ 12 -#define XS_PERIOD_11520HZ 10 + #define XS_PERIOD_576HZ 200 + #define XS_PERIOD_600HZ 192 + #define XS_PERIOD_640HZ 180 + #define XS_PERIOD_720HZ 160 + #define XS_PERIOD_768HZ 150 + #define XS_PERIOD_800HZ 144 + #define XS_PERIOD_900HZ 128 + #define XS_PERIOD_960HZ 120 + #define XS_PERIOD_1152HZ 100 + #define XS_PERIOD_1200HZ 96 + #define XS_PERIOD_1280HZ 90 + #define XS_PERIOD_1440HZ 80 + #define XS_PERIOD_1536HZ 75 + #define XS_PERIOD_1600HZ 72 + #define XS_PERIOD_1800HZ 64 + #define XS_PERIOD_1920HZ 60 + #define XS_PERIOD_2304HZ 50 + #define XS_PERIOD_2400HZ 48 + #define XS_PERIOD_2560HZ 45 + #define XS_PERIOD_2880HZ 40 + #define XS_PERIOD_3200HZ 36 + #define XS_PERIOD_3600HZ 32 + #define XS_PERIOD_3840HZ 30 + #define XS_PERIOD_4608HZ 25 + #define XS_PERIOD_4800HZ 24 + #define XS_PERIOD_5760HZ 20 + #define XS_PERIOD_6400HZ 18 + #define XS_PERIOD_7200HZ 16 + #define XS_PERIOD_7680HZ 15 + #define XS_PERIOD_9600HZ 12 + #define XS_PERIOD_11520HZ 10 #endif /*! \cond XS_INTERNAL */ diff --git a/lib/xspublic/xscontroller/xsdevice_def.cpp b/lib/xspublic/xscontroller/xsdevice_def.cpp index c55e64c..d7b3e73 100644 --- a/lib/xspublic/xscontroller/xsdevice_def.cpp +++ b/lib/xspublic/xscontroller/xsdevice_def.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -94,6 +94,7 @@ #include #include "xsdef.h" #include "xsiccrepmotionresult.h" +#include //! \cond DOXYGEN_SHOULD_SKIP_THIS using namespace xsens; @@ -109,9 +110,16 @@ using namespace XsTime; #define TOADUMP 0 // set to 0 to disable #if TOADUMP -#define CREATETOADUMPFILE() if (this == master()) do { if (m_toaDumpFile != nullptr) { fflush(m_toaDumpFile); fclose(m_toaDumpFile); } m_toaDumpFile = fopen(xprintf("toadump_%08X_%p_%llu.csv", deviceId().toInt(), this, XsTimeStamp::nowMs()).c_str(), "wt"); } while(0) + #define CREATETOADUMPFILE() if (this == master()) do { if (m_toaDumpFile != nullptr) { fflush(m_toaDumpFile); fclose(m_toaDumpFile); } m_toaDumpFile = fopen(xprintf("toadump_%08X_%p_%llu.csv", deviceId().toInt(), this, XsTimeStamp::nowMs()).c_str(), "wt"); } while(0) #else -#define CREATETOADUMPFILE() ((void)0) + #define CREATETOADUMPFILE() ((void)0) +#endif + +// set to 1 to log all messages sent and received through the doTransaction functions +#ifndef XSENS_RELEASE + #define LOGTRANSACTIONS 1 // enable in full debug build +#else + #define LOGTRANSACTIONS 0 // disable in release and rd builds #endif /*! \fn XsDevice::clearCallbackHandlers(bool chain = true) @@ -135,6 +143,22 @@ using namespace XsTime; chaining is still done. */ +#if LOGTRANSACTIONS +std::string msgToString(XsMessage const& msg) +{ + std::string rv; + XsSize sz = msg.getTotalMessageSize(); + uint8_t const* buffy = msg.getMessageStart(); + rv.reserve((sz + 1) * 2); + for (XsSize i = 0; i < sz; ++i) + rv.append(xprintf("%02X", (unsigned int)buffy[i])); + return rv; +} +#define TRANSACTIONLOG(...) JLDEBUGG(__VA_ARGS__) +#else +#define TRANSACTIONLOG(...) ((void) 0) +#endif + /*! \brief Construct an empty device with device id \a id \param id The device ID to construct with */ @@ -163,7 +187,7 @@ XsDevice::XsDevice(XsDeviceId const& id) , m_toaDumpFile(nullptr) { CREATETOADUMPFILE(); - JLDEBUGG("Created device " << deviceId()); + JLDEBUGG(this << " Created device " << deviceId()); } /*! \brief Construct a device using \a inf for communication @@ -210,7 +234,7 @@ XsDevice::XsDevice(Communicator* comm) m_connectivity = XCS_File; } CREATETOADUMPFILE(); - JLDEBUGG("Created device " << deviceId()); + JLDEBUGG(this << " Created device " << deviceId()); } /*! \brief Construct a device with device id \a childDeviceId for master \a masterDevice @@ -218,7 +242,7 @@ XsDevice::XsDevice(Communicator* comm) \param childDeviceId The child device ID to construct with \details Communication uses \a masterDevice's channel */ -XsDevice::XsDevice(XsDevice *masterDevice, const XsDeviceId &childDeviceId) +XsDevice::XsDevice(XsDevice* masterDevice, const XsDeviceId& childDeviceId) : m_latestLivePacket(new XsDataPacket) , m_latestBufferedPacket(new XsDataPacket) , m_unavailableDataBoundary(-1) @@ -243,6 +267,7 @@ XsDevice::XsDevice(XsDevice *masterDevice, const XsDeviceId &childDeviceId) , m_toaDumpFile(nullptr) { (void)masterDevice; + JLDEBUGG(this << " Created device " << deviceId() << " child of " << masterDevice << " " << (masterDevice ? masterDevice->deviceId() : XsDeviceId())); } /*! \brief Destroy the device */ @@ -286,7 +311,7 @@ XsDevice::~XsDevice() delete m_logFileInterface; } - JLDEBUGG("object: " << this << " done"); + JLDEBUGG(this << " destroyed"); if (m_toaDumpFile) { @@ -295,7 +320,7 @@ XsDevice::~XsDevice() m_toaDumpFile = nullptr; } } - catch(...) + catch (...) { } } @@ -304,7 +329,7 @@ XsDevice::~XsDevice() \details This function returns the master device of the current device. This may be the device itself \returns The master device of this device */ -XsDevice *XsDevice::master() const +XsDevice* XsDevice::master() const { return m_master; } @@ -360,90 +385,90 @@ void XsDevice::updateDeviceState(XsDeviceState newState) // some special case handling switch (newState) { - case XDS_FlushingData: // to - switch (oldState) - { - case XDS_Measurement: // from - return; + case XDS_FlushingData: // to + switch (oldState) + { + case XDS_Measurement: // from + return; - case XDS_Recording: // from - if (m_stopRecordingPacketId == -1 && isMasterDevice()) - m_stopRecordingPacketId = latestLivePacketId(); - m_stoppedRecordingPacketId = m_stopRecordingPacketId; - ONLYFIRSTMTX2 - JLDEBUGG(this << " " << deviceId() << " m_startRecordingPacketId = " << m_startRecordingPacketId << " m_stopRecordingPacketId = " << m_stopRecordingPacketId << " m_stoppedRecordingPacketId = " << m_stoppedRecordingPacketId); - break; + case XDS_Recording: // from + if (m_stopRecordingPacketId == -1 && isMasterDevice()) + m_stopRecordingPacketId = latestLivePacketId(); + m_stoppedRecordingPacketId = m_stopRecordingPacketId; + ONLYFIRSTMTX2 + JLDEBUGG(this << " " << deviceId() << " m_startRecordingPacketId = " << m_startRecordingPacketId << " m_stopRecordingPacketId = " << m_stopRecordingPacketId << " m_stoppedRecordingPacketId = " << m_stoppedRecordingPacketId); + break; - case XDS_WaitingForRecordingStart: // from - updateDeviceState(XDS_Measurement); - return; + case XDS_WaitingForRecordingStart: // from + updateDeviceState(XDS_Measurement); + return; - default: + default: + break; + } break; - } - break; - case XDS_Recording: // to - switch (oldState) - { - case XDS_Measurement: // from - m_stopRecordingPacketId = -1; - m_stoppedRecordingPacketId = -1; - if (m_startRecordingPacketId == -1 && isMasterDevice() && latestLivePacketId() >= 0) - m_startRecordingPacketId = latestLivePacketId()+1; - ONLYFIRSTMTX2 - JLDEBUGG(this << " " << deviceId() << " m_startRecordingPacketId = " << m_startRecordingPacketId << " m_stopRecordingPacketId = " << m_stopRecordingPacketId << " m_stoppedRecordingPacketId = " << m_stoppedRecordingPacketId); - break; - default: + case XDS_Recording: // to + switch (oldState) + { + case XDS_Measurement: // from + m_stopRecordingPacketId = -1; + m_stoppedRecordingPacketId = -1; + if (m_startRecordingPacketId == -1 && isMasterDevice() && latestLivePacketId() >= 0) + m_startRecordingPacketId = latestLivePacketId() + 1; + ONLYFIRSTMTX2 + JLDEBUGG(this << " " << deviceId() << " m_startRecordingPacketId = " << m_startRecordingPacketId << " m_stopRecordingPacketId = " << m_stopRecordingPacketId << " m_stoppedRecordingPacketId = " << m_stoppedRecordingPacketId); + break; + default: + break; + } break; - } - break; - case XDS_WaitingForRecordingStart: // to - switch (oldState) - { - case XDS_Measurement: // from - m_stopRecordingPacketId = -1; - m_startRecordingPacketId = -1; - m_stoppedRecordingPacketId = -1; - ONLYFIRSTMTX2 - JLDEBUGG(this << " " << deviceId() << " m_startRecordingPacketId = " << m_startRecordingPacketId << " m_stopRecordingPacketId = " << m_stopRecordingPacketId << " m_stoppedRecordingPacketId = " << m_stoppedRecordingPacketId); - break; - default: + case XDS_WaitingForRecordingStart: // to + switch (oldState) + { + case XDS_Measurement: // from + m_stopRecordingPacketId = -1; + m_startRecordingPacketId = -1; + m_stoppedRecordingPacketId = -1; + ONLYFIRSTMTX2 + JLDEBUGG(this << " " << deviceId() << " m_startRecordingPacketId = " << m_startRecordingPacketId << " m_stopRecordingPacketId = " << m_stopRecordingPacketId << " m_stoppedRecordingPacketId = " << m_stoppedRecordingPacketId); + break; + default: + break; + } break; - } - break; - case XDS_Measurement: // to - switch (oldState) - { - case XDS_Recording: // from - case XDS_FlushingData: // from - m_stoppedRecordingPacketId = m_stopRecordingPacketId; - if (m_stoppedRecordingPacketId == -1 && isMasterDevice()) - m_stoppedRecordingPacketId = latestLivePacketId(); - m_stopRecordingPacketId = -1; - m_startRecordingPacketId = -1; - //m_latestBufferedPacket->clear(); - ONLYFIRSTMTX2 - JLDEBUGG(this << " " << deviceId() << " m_startRecordingPacketId = " << m_startRecordingPacketId << " m_stopRecordingPacketId = " << m_stopRecordingPacketId << " m_stoppedRecordingPacketId = " << m_stoppedRecordingPacketId); + case XDS_Measurement: // to + switch (oldState) + { + case XDS_Recording: // from + case XDS_FlushingData: // from + m_stoppedRecordingPacketId = m_stopRecordingPacketId; + if (m_stoppedRecordingPacketId == -1 && isMasterDevice()) + m_stoppedRecordingPacketId = latestLivePacketId(); + m_stopRecordingPacketId = -1; + m_startRecordingPacketId = -1; + //m_latestBufferedPacket->clear(); + ONLYFIRSTMTX2 + JLDEBUGG(this << " " << deviceId() << " m_startRecordingPacketId = " << m_startRecordingPacketId << " m_stopRecordingPacketId = " << m_stopRecordingPacketId << " m_stoppedRecordingPacketId = " << m_stoppedRecordingPacketId); + break; + + case XDS_Config: + default: + m_stopRecordingPacketId = -1; + m_startRecordingPacketId = -1; + m_stoppedRecordingPacketId = -1; + ONLYFIRSTMTX2 + JLDEBUGG(this << " " << deviceId() << " m_startRecordingPacketId = " << m_startRecordingPacketId << " m_stopRecordingPacketId = " << m_stopRecordingPacketId << " m_stoppedRecordingPacketId = " << m_stoppedRecordingPacketId); + resetPacketStamping(); + reinitializeProcessors(); + break; + } break; - case XDS_Config: default: - m_stopRecordingPacketId = -1; - m_startRecordingPacketId = -1; - m_stoppedRecordingPacketId = -1; - ONLYFIRSTMTX2 - JLDEBUGG(this << " " << deviceId() << " m_startRecordingPacketId = " << m_startRecordingPacketId << " m_stopRecordingPacketId = " << m_stopRecordingPacketId << " m_stoppedRecordingPacketId = " << m_stoppedRecordingPacketId); - resetPacketStamping(); - reinitializeProcessors(); break; - } - break; - - default: - break; } m_state = newState; @@ -472,10 +497,10 @@ bool XsDevice::isSoftwareCalibrationEnabled() const \param deviceid The device ID to search for \returns A pointer to the found %XsDevice or 0 if the device could not be found */ -XsDevice *XsDevice::findDevice(XsDeviceId const& deviceid) +XsDevice* XsDevice::findDevice(XsDeviceId const& deviceid) const { if (deviceid == m_deviceId) - return this; + return const_cast(this); return nullptr; } @@ -484,7 +509,7 @@ XsDevice *XsDevice::findDevice(XsDeviceId const& deviceid) \param deviceid The device ID to search for \returns A pointer to the found %XsDevice or 0 if the device could not be found */ -XsDevice const *XsDevice::findDeviceConst(XsDeviceId const& deviceid) const +XsDevice const* XsDevice::findDeviceConst(XsDeviceId const& deviceid) const { return const_cast(const_cast(this)->findDevice(deviceid)); } @@ -515,7 +540,7 @@ int XsDevice::batteryLevel() const /*! \brief Get the legacy update rate of the device \details This function is only valid for devices in legacy mode. \returns The legacy update rate of the device - */ +*/ int XsDevice::updateRate() const { return 0; @@ -541,7 +566,7 @@ XsDeviceOptionFlag XsDevice::deviceOptionFlags() const /*! \cond XS_INTERNAL */ /*! \brief Find the output configuration for \a dataType - */ +*/ XsOutputConfiguration XsDevice::findConfiguration(XsDataIdentifier dataType) const { XsDataIdentifier mask; @@ -552,12 +577,12 @@ XsOutputConfiguration XsDevice::findConfiguration(XsDataIdentifier dataType) con XsOutputConfigurationArray cfg = outputConfiguration(); auto item = std::find_if(cfg.begin(), cfg.end(), - [&](const XsOutputConfiguration &cfg) - { - return (cfg.m_dataIdentifier & mask) == dataType; - } + [&](const XsOutputConfiguration & cfg) + { + return (cfg.m_dataIdentifier & mask) == dataType; + } - ); + ); if (item == cfg.end()) return XsOutputConfiguration(); return *item; @@ -611,7 +636,7 @@ bool XsDevice::hasDataEnabled(XsDataIdentifier dataType) const /*! \cond XS_INTERNAL */ /*! \brief Returns true if the supplied \a configurations contains \a dataType */ -bool XsDevice::checkDataEnabled(XsDataIdentifier dataType, XsOutputConfigurationArray const & configurations) +bool XsDevice::checkDataEnabled(XsDataIdentifier dataType, XsOutputConfigurationArray const& configurations) { XsDataIdentifier mask; if ((dataType & XDI_TypeMask) == dataType) @@ -637,7 +662,7 @@ bool XsDevice::checkDataEnabled(XsDataIdentifier dataType, XsOutputConfiguration \returns true if \a dataType is configured for output \sa hasDataEnabled \sa processedOutputConfiguration \sa updateRateForDataIdentifier */ -bool XsDevice::hasProcessedDataEnabled(XsDataIdentifier ) const +bool XsDevice::hasProcessedDataEnabled(XsDataIdentifier) const { return false; } @@ -655,7 +680,7 @@ XsVersion XsDevice::firmwareVersion() const /*! \cond XS_INTERNAL */ /*! \brief Set the cached firmware version of the device */ -void XsDevice::setFirmwareVersion(const XsVersion &version) +void XsDevice::setFirmwareVersion(const XsVersion& version) { if (m_firmwareVersion != version) #ifdef XSENS_DEBUG @@ -667,8 +692,8 @@ void XsDevice::setFirmwareVersion(const XsVersion &version) } /*! \brief Extracts the firmware version of the device. - * \param message: XMID_FirmwareRevision message. - */ + \param message: XMID_FirmwareRevision message. +*/ void XsDevice::extractFirmwareVersion(XsMessage const& message) { XsVersion old = m_firmwareVersion; @@ -703,7 +728,7 @@ DataLogger* XsDevice::logFileInterface(std::unique_ptr& myLock) con /*! \brief Set the device id of this device */ -void XsDevice::setDeviceId(const XsDeviceId &deviceid) +void XsDevice::setDeviceId(const XsDeviceId& deviceid) { m_deviceId = deviceid; } @@ -739,7 +764,7 @@ bool XsDevice::isSyncSlave() const \param busid The busid to serach for \returns The XsDevice corresponding to the supplied \a busid */ -XsDevice *XsDevice::deviceAtBusId(int busid) +XsDevice* XsDevice::deviceAtBusId(int busid) { if (isMasterDevice() && (busid == XS_BID_MASTER || busid == 1)) return this; @@ -750,7 +775,7 @@ XsDevice *XsDevice::deviceAtBusId(int busid) \param busid The busid to serach for \returns The const XsDevice corresponding to the supplied \a busid */ -const XsDevice *XsDevice::deviceAtBusIdConst(int busid) const +const XsDevice* XsDevice::deviceAtBusIdConst(int busid) const { // prevent code duplication, go through the non-const implementation return const_cast(const_cast(this)->deviceAtBusId(busid)); @@ -902,7 +927,7 @@ XsIntArray XsDevice::portConfiguration() const \returns true if the port configuration was successfully updated \sa portConfiguration, XsBaudCode */ -bool XsDevice::setPortConfiguration(XsIntArray &config) +bool XsDevice::setPortConfiguration(XsIntArray& config) { (void)config; return false; @@ -1111,6 +1136,18 @@ bool XsDevice::setDeviceOptionFlags(XsDeviceOptionFlag setFlags, XsDeviceOptionF \returns true if the output configuration was successfully updated */ bool XsDevice::setOutputConfiguration(XsOutputConfigurationArray& config) +{ + auto const rv = setOutputConfigurationInternal(config); + if (rv != XRV_OK) + return false; + return true; +} + +/*! \cond XS_INTERNAL */ +/*! \copydoc XsDevice::setOutputConfiguration + \returns XRV_OK on success, other XsResultValue on failure +*/ +XsResultValue XsDevice::setOutputConfigurationInternal(XsOutputConfigurationArray& config) { XsMessage snd(XMID_SetOutputConfiguration, 4); snd.setBusId(busId()); @@ -1119,13 +1156,21 @@ bool XsDevice::setOutputConfiguration(XsOutputConfigurationArray& config) XsMessage rcv; if (!doTransaction(snd, rcv)) - return false; + { + // deal w/ possible race condition on m_lastResult + auto const rv = rcv.toResultValue(); + m_lastResult = rv; + return rv; + } MessageDeserializer(rcv) >> config; if (wasEmpty && config.size() == 1 && config[0] == XsOutputConfiguration(XDI_None, 0)) config.clear(); - return true; + + m_outputConfiguration = config; + return XRV_OK; } +/* \endcond */ /*! \brief Set the CAN output configuration for this device \details When the function exits with a true value \a config will contain the actual configuration in the @@ -1147,8 +1192,8 @@ bool XsDevice::setCanOutputConfiguration(XsCanOutputConfigurationArray& config) } /*! \brief Set the CAN configuration for this device - \param config Should consist of 8 bytes baudcode and 1 bit to enable CAN - \returns true if the CAN output configuration was successfully updated + \param config Should consist of 8 bytes baudcode and 1 bit to enable CAN + \returns true if the CAN output configuration was successfully updated */ bool XsDevice::setCanConfiguration(uint32_t config) { @@ -1266,7 +1311,10 @@ bool XsDevice::sendCustomMessage(const XsMessage& messageSend, bool waitForResul bool XsDevice::sendRawMessage(const XsMessage& message) { Communicator* comm = communicator(); - return comm ? comm->writeMessage(message) : false; + if (!comm) + return false; + TRANSACTIONLOG(m_deviceId << " SND: " << msgToString(message)); + return comm->writeMessage(message); } /*! \cond XS_INTERNAL */ @@ -1293,6 +1341,7 @@ bool XsDevice::sendCustomMessage(const XsMessage& messageSend, bool waitForResul if (!comm) return false; + TRANSACTIONLOG(m_deviceId << " SND: " << msgToString(messageSend)); std::shared_ptr reply = comm->addReplyObject(messageId); if (!comm->writeMessage(messageSend)) return false; @@ -1302,6 +1351,7 @@ bool XsDevice::sendCustomMessage(const XsMessage& messageSend, bool waitForResul if (!timeout) timeout = (int)(int32_t) comm->defaultTimeout(); messageReceive = reply->message((uint32_t)(unsigned int)timeout); + TRANSACTIONLOG(m_deviceId << " RCV: " << msgToString(messageReceive)); if (messageReceive.getMessageId() != messageId) return false; } @@ -1315,7 +1365,7 @@ bool XsDevice::sendCustomMessage(const XsMessage& messageSend, bool waitForResul \param timeout Optional timeout in ms. When 0 is supplied (the default), the default timeout is used. \returns true if the correct reply has been received */ -bool XsDevice::waitForCustomMessage(XsXbusMessageId messageId, XsMessage &messageReceive, int timeout) +bool XsDevice::waitForCustomMessage(XsXbusMessageId messageId, XsMessage& messageReceive, int timeout) { Communicator* comm = communicator(); if (!comm) @@ -1339,7 +1389,7 @@ bool XsDevice::waitForCustomMessage(XsXbusMessageId messageId, XsMessage &messag \param timeout Optional timeout in ms. When 0 is supplied (the default), the default timeout is used. \returns true if the correct reply has been received */ -bool XsDevice::waitForCustomMessage(std::shared_ptr reply, XsMessage &messageReceive, int timeout) +bool XsDevice::waitForCustomMessage(std::shared_ptr reply, XsMessage& messageReceive, int timeout) { if (!timeout && communicator()) timeout = (int)(int32_t) communicator()->defaultTimeout(); @@ -1371,7 +1421,7 @@ std::shared_ptr XsDevice::addReplyObject(XsXbusMessageId messageId, The function is only supplied XMID_Data and XMID_Data2 messages. If this needs to change, make sure to update the existing implementations accordingly. */ -bool XsDevice::shouldDataMsgBeRecorded(const XsMessage &msg) const +bool XsDevice::shouldDataMsgBeRecorded(const XsMessage& msg) const { (void)msg; return (deviceState() == XDS_Recording); @@ -1379,53 +1429,54 @@ bool XsDevice::shouldDataMsgBeRecorded(const XsMessage &msg) const /*! \brief Handle an XbusMessage */ -void XsDevice::handleMessage(const XsMessage &msg) +void XsDevice::handleMessage(const XsMessage& msg) { if (isMasterDevice()) writeMessageToLogFile(msg); switch (msg.getMessageId()) { - case XMID_MtData2: - { - XsDataPacket packet(&msg); - packet.setDeviceId(deviceId()); - handleDataPacket(packet); - } break; - - case XMID_Error: - handleErrorMessage(msg); + case XMID_MtData2: + { + XsDataPacket packet(&msg); + packet.setDeviceId(deviceId()); + handleDataPacket(packet); + } break; - case XMID_Warning: - handleWarningMessage(msg); - break; + case XMID_Error: + handleErrorMessage(msg); + break; - case XMID_MasterIndication: - handleMasterIndication(msg); - break; + case XMID_Warning: + handleWarningMessage(msg); + break; - case XMID_Wakeup: - handleWakeupMessage(msg); - break; + case XMID_MasterIndication: + handleMasterIndication(msg); + break; - default: - JLDEBUGG("Handling non-data msg " << msg.getMessageId() << " bid " << JLHEXLOG((int)msg.getBusId())); - handleNonDataMessage(msg); - break; + case XMID_Wakeup: + handleWakeupMessage(msg); + break; + + default: + JLDEBUGG("Handling non-data msg " << msg.getMessageId() << " bid " << JLHEXLOG((int)msg.getBusId())); + handleNonDataMessage(msg); + break; } } /*! \brief Process a message which is not a data message */ -void XsDevice::handleNonDataMessage(const XsMessage &msg) +void XsDevice::handleNonDataMessage(const XsMessage& msg) { onNonDataMessage(this, &msg); } /*! \brief Process an error message */ -void XsDevice::handleErrorMessage(const XsMessage &msg) +void XsDevice::handleErrorMessage(const XsMessage& msg) { uint8_t errorCode = msg.getDataByte(0); XsResultValue xsResultValue = static_cast(errorCode); @@ -1446,6 +1497,12 @@ void XsDevice::handleWakeupMessage(const XsMessage& msg) { (void)msg; onWakeupReceived(this); + + //"reset" some of the states after a wake up of a device + resetPacketStamping(); + setDeviceState(XDS_Config); + if ((deviceOptionFlags() & XDOF_DisableAutoMeasurement) != XDOF_DisableAutoMeasurement) + setDeviceState(XDS_Measurement); } /*! \brief Inserts the packet ID and data packet into the data cache @@ -1466,9 +1523,7 @@ void XsDevice::insertIntoDataCache(int64_t pid, XsDataPacket* pack) else { if (it->first != pid) - { m_dataCache.insert(it, std::make_pair(pid, pack)); - } else { it->second->merge(*pack, true); @@ -1526,12 +1581,12 @@ bool XsDevice::shouldDoRecordedCallback(XsDataPacket const& p) const switch (deviceState()) { - case XDS_Recording: - case XDS_FlushingData: - break; + case XDS_Recording: + case XDS_FlushingData: + break; - default: - return false; + default: + return false; } if (isStandaloneDevice()) @@ -1550,7 +1605,7 @@ bool XsDevice::shouldDoRecordedCallback(XsDataPacket const& p) const /*! \brief Handle an XbusDataPacket \param packet The data packet to handle */ -void XsDevice::handleDataPacket(const XsDataPacket &packet) +void XsDevice::handleDataPacket(const XsDataPacket& packet) { LockGuarded locky(&m_deviceMutex); if (m_terminationPrepared) @@ -1571,12 +1626,12 @@ void XsDevice::handleDataPacket(const XsDataPacket &packet) #if 0 ONLYFIRSTMTX2 JLWRITEG(this << " [TOALOG] pid: " << current << - " did: " << deviceId() << - " awindaframenr: " << packet.awindaSnapshot().m_frameNumber << - " packetTOA: " << pack->timeOfArrival().msTime() << - " fastest: " << fastest << - " slowest: " << slowest << - " etos " << pack->estimatedTimeOfSampling().msTime()); + " did: " << deviceId() << + " awindaframenr: " << packet.awindaSnapshot().m_frameNumber << + " packetTOA: " << pack->timeOfArrival().msTime() << + " fastest: " << fastest << + " slowest: " << slowest << + " etos " << pack->estimatedTimeOfSampling().msTime()); JLDEBUGG("stamped: " << current << " new latestlive: " << latestLivePacketConst().packetId()); #endif @@ -1590,8 +1645,8 @@ void XsDevice::handleDataPacket(const XsDataPacket &packet) int64_t firstMissed = fastest + 1; int64_t lastMissed = current - 1; ONLYFIRSTMTX2 - JLDEBUGG("Detected " << (dpc-1) << " packets have been missed by device " << deviceId() << ", last was " << fastest << " (" << (uint16_t) fastest << ") current is " << current << " (" << (uint16_t) current << ")"); - onMissedPackets(this, (int) dpc-1, (int) firstMissed, (int) lastMissed); + JLDEBUGG("Detected " << (dpc - 1) << " packets have been missed by device " << deviceId() << ", last was " << fastest << " (" << (uint16_t) fastest << ") current is " << current << " (" << (uint16_t) current << ")"); + onMissedPackets(this, (int) dpc - 1, (int) firstMissed, (int) lastMissed); for (int64_t i = firstMissed; i <= lastMissed; ++i) { @@ -1618,11 +1673,11 @@ void XsDevice::handleDataPacket(const XsDataPacket &packet) { // when this returns true, the packet has been processed properly already so we should return if (interpolateMissingData(*copy, latestLivePacketConst(), - [this](XsDataPacket* ppp) - { - handleDataPacket(*ppp); - delete ppp; - })) + [this](XsDataPacket * ppp) + { + handleDataPacket(*ppp); + delete ppp; + })) { return; } @@ -1687,17 +1742,17 @@ bool XsDevice::isMeasuring() const { switch (deviceState()) { - case XDS_Measurement: - case XDS_WaitingForRecordingStart: - case XDS_Recording: - case XDS_FlushingData: - return true; + case XDS_Measurement: + case XDS_WaitingForRecordingStart: + case XDS_Recording: + case XDS_FlushingData: + return true; - case XDS_Initial: - case XDS_Config: - case XDS_Destructing: - default: - return false; + case XDS_Initial: + case XDS_Config: + case XDS_Destructing: + default: + return false; } } @@ -1706,18 +1761,19 @@ bool XsDevice::isMeasuring() const */ bool XsDevice::isRecording() const { - switch (deviceState()) { - case XDS_WaitingForRecordingStart: - case XDS_Recording: - case XDS_FlushingData: - return true; + switch (deviceState()) + { + case XDS_WaitingForRecordingStart: + case XDS_Recording: + case XDS_FlushingData: + return true; - case XDS_Measurement: - case XDS_Initial: - case XDS_Config: - case XDS_Destructing: - default: - return false; + case XDS_Measurement: + case XDS_Initial: + case XDS_Config: + case XDS_Destructing: + default: + return false; } } @@ -1735,13 +1791,13 @@ bool XsDevice::isReadingFromFile() const /*! \cond XS_INTERNAL */ /*! \brief Process a XsDataPacket (calibrate and filter) and put the results in the appropriate cache */ -void XsDevice::processLivePacket(XsDataPacket &) +void XsDevice::processLivePacket(XsDataPacket&) { } /*! \brief Process a XsDataPacket (calibrate and filter) and put the results in the appropriate cache */ -void XsDevice::processBufferedPacket(XsDataPacket &) +void XsDevice::processBufferedPacket(XsDataPacket&) { } /*! \endcond */ @@ -1789,7 +1845,7 @@ bool XsDevice::readDeviceConfiguration() return false; m_config.readFromMessage(rcv); - rcv = comm->readMessageFromStartOfFile(XMID_FirmwareRevision, Communicator::configurationMessageSearchLimit() * (busId() == XS_BID_MASTER ? 2 : (1+busId()))); + rcv = comm->readMessageFromStartOfFile(XMID_FirmwareRevision, Communicator::configurationMessageSearchLimit() * (busId() == XS_BID_MASTER ? 2 : (1 + busId()))); if (comm->lastResult() != XRV_OK) { // We cannot determine the firmware version of the master, use the first sensor (fallback) @@ -1833,6 +1889,8 @@ bool XsDevice::readDeviceConfiguration() if (doTransaction(snd, rcv) && rcv.getMessageId() == XMID_FirmwareRevision) extractFirmwareVersion(rcv); + + m_outputConfiguration = outputConfiguration(); } return true; } @@ -1850,7 +1908,7 @@ bool XsDevice::readDeviceConfiguration() #endif return true; } - catch (XsDeviceConfigurationException &) + catch (XsDeviceConfigurationException&) { return false; } @@ -1900,7 +1958,7 @@ bool XsDevice::restoreFactoryDefaults() XsMessage snd(XMID_RestoreFactoryDef); snd.setBusId(busId()); - if (!doTransaction(snd,2000)) + if (!doTransaction(snd, 2000)) return false; return true; @@ -1936,6 +1994,7 @@ bool XsDevice::reset(bool skipDeviceIdCheck) XsMessage snd(XMID_Reset); snd.setBusId(XS_BID_MASTER); + TRANSACTIONLOG(m_deviceId << " SND: " << msgToString(snd)); std::shared_ptr wakeup = comm->addReplyObject(XMID_Wakeup); comm->writeMessage(snd); @@ -1976,7 +2035,7 @@ void XsDevice::writeDeviceSettingsToFile() \param filename The desired path and filename of the log file \returns Result value indicating success (XRV_OK) or failure */ -XsResultValue XsDevice::createLogFile(const XsString &filename) +XsResultValue XsDevice::createLogFile(const XsString& filename) { JLDEBUGG(filename); Communicator* comm = communicator(); @@ -2020,7 +2079,7 @@ XsResultValue XsDevice::createLogFile(const XsString &filename) \param[in] filename The desired path and filename of the config file \returns Result value indicating success (XRV_OK) or failure */ -XsResultValue XsDevice::createConfigFile(const XsString& ) +XsResultValue XsDevice::createConfigFile(const XsString&) { return XRV_NOTIMPLEMENTED; } @@ -2080,7 +2139,7 @@ XsMatrix XsDevice::alignmentRotationMatrix(XsAlignmentFrame frame) const \param[in] filename The desired path and filename of the config file \returns Result value indicating success (XRV_OK) or failure */ -XsResultValue XsDevice::applyConfigFile(const XsString& ) +XsResultValue XsDevice::applyConfigFile(const XsString&) { return XRV_NOTIMPLEMENTED; } @@ -2217,7 +2276,7 @@ void XsDevice::checkDataCache() if (m_options & XSO_ExpectNoRetransmissionsInFile) return true; if ((m_stopRecordingPacketId >= 0 && rFirst > m_stopRecordingPacketId) || - (rLast-1 <= m_unavailableDataBoundary)) + (rLast - 1 <= m_unavailableDataBoundary)) { // notify unavailable data in recording states // when not recording, unavailable data is expected and should not be reported @@ -2276,8 +2335,8 @@ void XsDevice::checkDataCache() // store result latestBufferedPacket().swap(*it->second); -// ONLYFIRSTMTX2 -// JLDEBUGG("latestBufferedPacket is now " << latestBufferedPacket().packetId() << " old: " << it->second->packetId()); + // ONLYFIRSTMTX2 + // JLDEBUGG("latestBufferedPacket is now " << latestBufferedPacket().packetId() << " old: " << it->second->packetId()); delete it->second; m_dataCache.erase(it); @@ -2315,7 +2374,7 @@ void XsDevice::checkDataCache() } /*! \brief Store a message in the log file if a log file is open */ -void XsDevice::writeMessageToLogFile(const XsMessage &message) +void XsDevice::writeMessageToLogFile(const XsMessage& message) { if (communicator()->isReadingFromFile()) return; @@ -2461,10 +2520,10 @@ int XsDevice::locationId() const } /*! \brief Get the device given \a locId - * - * \param locId the location ID of the device we're looking for - * \returns a pointer to the device if found, nullptr otherwise. - */ + + \param locId the location ID of the device we're looking for + \returns a pointer to the device if found, nullptr otherwise. +*/ XsDevice* XsDevice::getDeviceFromLocationId(uint16_t locId) { if (locationId() == locId) @@ -2492,7 +2551,7 @@ XsMatrix XsDevice::objectAlignment() const MT Mk4 devices it is suggested to use setAlignmentRotationQuaternion or setAlignmentRotationMatrix instead. \sa objectAlignmentMatrix \sa headingOffset \sa setHeadingOffset \sa setAlignmentRotationQuaternion \sa setAlignmentRotationMatrix */ -bool XsDevice::setObjectAlignment(const XsMatrix &matrix) +bool XsDevice::setObjectAlignment(const XsMatrix& matrix) { (void)matrix; return false; @@ -2716,7 +2775,7 @@ XsIccRepMotionResult XsDevice::stopRepresentativeMotion() } /*! \brief Store the onboard ICC results for use by the device - \returns true if the store was successful + \returns true if the store was successful */ bool XsDevice::storeIccResults() { @@ -3030,7 +3089,7 @@ XsString XsDevice::logFileName() const return XsString(); } - Communicator *object = communicator(); + Communicator* object = communicator(); if (!object) return XsString(); return object->logFileName(); @@ -3063,7 +3122,7 @@ int XsDevice::recordingQueueLength() const if (m_dataCache.empty()) return 0; - return (int) (m_dataCache.rbegin()->first - latestBufferedPacketId()); + return (int)(m_dataCache.rbegin()->first - latestBufferedPacketId()); } /*! \brief Get the number of items currently in the slow data cache for the device @@ -3226,14 +3285,14 @@ bool XsDevice::enableProtocol(XsProtocolType protocol) switch (protocol) { - case XPT_Xbus: - comm->addProtocolHandler(new ProtocolHandler()); - return true; - case XPT_Nmea: - comm->addProtocolHandler(new nmea::ProtocolHandler()); - return true; - default: - return false; // unknown type + case XPT_Xbus: + comm->addProtocolHandler(new ProtocolHandler()); + return true; + case XPT_Nmea: + comm->addProtocolHandler(new nmea::ProtocolHandler()); + return true; + default: + return false; // unknown type } } @@ -3250,14 +3309,14 @@ bool XsDevice::disableProtocol(XsProtocolType protocol) switch (protocol) { - case XPT_Xbus: - comm->removeProtocolHandler(XPT_Xbus); - return true; - case XPT_Nmea: - comm->removeProtocolHandler(XPT_Nmea); - return true; - default: - return false; // unknown type + case XPT_Xbus: + comm->removeProtocolHandler(XPT_Xbus); + return true; + case XPT_Nmea: + comm->removeProtocolHandler(XPT_Nmea); + return true; + default: + return false; // unknown type } } @@ -3332,7 +3391,7 @@ XsCanOutputConfigurationArray XsDevice::canOutputConfiguration() const } /*! \brief Returns the currently configured CAN configuration of the device - \return The can configuration of the device + \return The can configuration of the device */ uint32_t XsDevice::canConfiguration() const { @@ -3351,13 +3410,13 @@ uint32_t XsDevice::canConfiguration() const */ void XsDevice::setOptions(XsOption enable, XsOption disable) { - if (!communicator()->allowReprocessing()) + if (communicator() && !communicator()->allowReprocessing()) { enable = enable & ~(XSO_Calibrate | XSO_Orientation); disable = disable | (XSO_Calibrate | XSO_Orientation); } - XsOption upd = XsOption_purify((XsOption) ((m_options & ~disable) | enable)); + XsOption upd = XsOption_purify((XsOption)((m_options & ~disable) | enable)); static const XsOption mask = XSO_KeepLastLiveData | XSO_RetainLiveData | XSO_RetainBufferedData; // this has to be done in this order to prevent timing issues @@ -3465,7 +3524,7 @@ int XsDevice::radioQualityIndication() const /*! \brief Handles master indication message. \param message The message to handle. */ -void XsDevice::handleMasterIndication(const XsMessage &message) +void XsDevice::handleMasterIndication(const XsMessage& message) { onNonDataMessage(this, &message); } @@ -3540,7 +3599,7 @@ void XsDevice::updateConnectivityState(XsConnectivityState newState) } /*! \brief The default connectivity state for newly created devices - */ +*/ XsConnectivityState XsDevice::defaultChildConnectivityState() const { return m_connectivity; @@ -3570,7 +3629,7 @@ int16_t XsDevice::lastKnownRssi() const /*! \cond XS_INTERNAL */ /*! \brief Set the packet error rate for the device. \param per The packet error rate of the device expressed as a percentage. - */ +*/ void XsDevice::setPacketErrorRate(int per) { (void)per; @@ -3578,19 +3637,19 @@ void XsDevice::setPacketErrorRate(int per) /*! \endcond */ /*! - * \brief Returns the packet error rate for the for the device. - * - * \details The packet error rate indicates the proportion of data packets from - * the device that are lost or corrupted in some manner over some time window. - * Depending on the device the packet error rate may be updated actively or - * passively, and the time window may vary, so packet error rates cannot be - * compared directly between different types of device. - * - * \note Not all devices support packet error rate estimation. Those that don't - * will always report a 0% packet error rate. - * - * \returns The packet error rate as a percentage. - */ + \brief Returns the packet error rate for the for the device. + + \details The packet error rate indicates the proportion of data packets from + the device that are lost or corrupted in some manner over some time window. + Depending on the device the packet error rate may be updated actively or + passively, and the time window may vary, so packet error rates cannot be + compared directly between different types of device. + + \note Not all devices support packet error rate estimation. Those that don't + will always report a 0% packet error rate. + + \returns The packet error rate as a percentage. +*/ int XsDevice::packetErrorRate() const { return 0; @@ -3611,7 +3670,7 @@ XsConnectivityState XsDevice::connectivityState() const \param dev The device to check \return true if the device is docked in this device */ -bool XsDevice::deviceIsDocked(XsDevice *dev) const +bool XsDevice::deviceIsDocked(XsDevice* dev) const { (void)dev; return false; @@ -3821,22 +3880,22 @@ void XsDevice::removeIfNoRefs() } /*! \brief Return true if the message looks sane */ -bool XsDevice::messageLooksSane(const XsMessage &msg) const +bool XsDevice::messageLooksSane(const XsMessage& msg) const { return msg.getBusId() == XS_BID_MASTER; } -void XsDevice::onMessageSent(const XsMessage &msg) +void XsDevice::onMessageSent(const XsMessage& msg) { onMessageSentToDevice(this, &msg); } -void XsDevice::onMessageReceived(const XsMessage &msg) +void XsDevice::onMessageReceived(const XsMessage& msg) { onMessageReceivedFromDevice(this, &msg); } -void XsDevice::onMessageDetected2(XsProtocolType type, const XsByteArray &rawMessage) +void XsDevice::onMessageDetected2(XsProtocolType type, const XsByteArray& rawMessage) { CallbackManagerXda::onMessageDetected(this, type, &rawMessage); } @@ -3881,7 +3940,7 @@ int64_t XsDevice::latestBufferedPacketId() const \details We have some cases where we don't want every child device to write its messages to log file (XM/AbmClockMaster) \returns True if successful */ -bool XsDevice::shouldWriteMessageToLogFile(const XsMessage &msg) const +bool XsDevice::shouldWriteMessageToLogFile(const XsMessage& msg) const { return shouldWriteMessageToLogFile(this, msg); } @@ -3892,7 +3951,7 @@ bool XsDevice::shouldWriteMessageToLogFile(const XsMessage &msg) const \details We have some cases where we don't want every child device to write its messages to log file (XM/AbmClockMaster) \returns True if successful */ -bool XsDevice::shouldWriteMessageToLogFile(const XsDevice *dev, const XsMessage &message) const +bool XsDevice::shouldWriteMessageToLogFile(const XsDevice* dev, const XsMessage& message) const { (void)dev; (void)message; @@ -3904,9 +3963,12 @@ bool XsDevice::shouldWriteMessageToLogFile(const XsDevice *dev, const XsMessage \param snd the message to send \returns true if the message was sent and acknowledged, false otherwise */ -bool XsDevice::doTransaction(const XsMessage &snd) const +bool XsDevice::doTransaction(const XsMessage& snd) const { - return communicator() && communicator()->doTransaction(snd); + TRANSACTIONLOG(m_deviceId << " SND: " << msgToString(snd)); + bool rv = communicator() && communicator()->doTransaction(snd); + TRANSACTIONLOG(m_deviceId << " RCV rv: " << rv); + return rv; } /*! \brief Send a message and wait for its reply @@ -3915,9 +3977,12 @@ bool XsDevice::doTransaction(const XsMessage &snd) const \param timeout the timeout to use \returns true if the message was sent and acknowledged, false otherwise */ -bool XsDevice::doTransaction(const XsMessage &snd, uint32_t timeout) const +bool XsDevice::doTransaction(const XsMessage& snd, uint32_t timeout) const { - return communicator() && communicator()->doTransaction(snd, timeout); + TRANSACTIONLOG(m_deviceId << " SND: " << msgToString(snd) << " timeout: " << timeout); + bool rv = communicator() && communicator()->doTransaction(snd, timeout); + TRANSACTIONLOG(m_deviceId << " RCV rv: " << rv); + return rv; } /*! \brief Send a message and wait for its reply @@ -3926,9 +3991,12 @@ bool XsDevice::doTransaction(const XsMessage &snd, uint32_t timeout) const \param rcv a pointer to a receive message. The contents are only valid if true was returned. \returns true if the message was sent and acknowledged, false otherwise */ -bool XsDevice::doTransaction(const XsMessage &snd, XsMessage &rcv) const +bool XsDevice::doTransaction(const XsMessage& snd, XsMessage& rcv) const { - return communicator() && communicator()->doTransaction(snd, rcv); + TRANSACTIONLOG(m_deviceId << " SND: " << msgToString(snd)); + bool rv = communicator() && communicator()->doTransaction(snd, rcv); + TRANSACTIONLOG(m_deviceId << " RCV: " << msgToString(rcv)); + return rv; } /*! \brief Send a message and wait for its reply @@ -3938,9 +4006,12 @@ bool XsDevice::doTransaction(const XsMessage &snd, XsMessage &rcv) const \param timeout the timeout to use \returns true if the message was sent and acknowledged, false otherwise */ -bool XsDevice::doTransaction(const XsMessage &snd, XsMessage &rcv, uint32_t timeout) const +bool XsDevice::doTransaction(const XsMessage& snd, XsMessage& rcv, uint32_t timeout) const { - return communicator() && communicator()->doTransaction(snd, rcv, timeout); + TRANSACTIONLOG(m_deviceId << " SND: " << msgToString(snd) << " timeout: " << timeout); + bool rv = communicator() && communicator()->doTransaction(snd, rcv, timeout); + TRANSACTIONLOG(m_deviceId << " RCV: " << msgToString(rcv)); + return rv; } /*! \endcond */ @@ -3973,7 +4044,7 @@ XsSyncSettingArray XsDevice::supportedSyncSettings(XsDeviceId const& deviceId) { XsSyncSettingArray settings = Synchronization::supportedSyncSettings(deviceId); XsSyncSettingArray result; - for (XsSyncSetting const & setting : settings) + for (XsSyncSetting const& setting : settings) result.push_back(setting); return result; @@ -3994,7 +4065,7 @@ bool XsDevice::supportsSyncSettings(XsDeviceId const& deviceId) \param[in] setting2 Setting 2 \returns true when \a setting1 is compatible with \a setting2 for deviceId \a deviceId */ -bool XsDevice::isCompatibleSyncSetting(XsDeviceId const& deviceId, XsSyncSetting const & setting1, XsSyncSetting const & setting2) +bool XsDevice::isCompatibleSyncSetting(XsDeviceId const& deviceId, XsSyncSetting const& setting1, XsSyncSetting const& setting2) { return Synchronization::isCompatibleSyncSetting(deviceId, setting1, setting2); } @@ -4092,7 +4163,7 @@ void XsDevice::updateLastAvailableLiveDataCache(XsDataPacket const& pack) /*! \brief Add the supplied \a pack to the linear packet cache \param pack The packet to retain in the packet cache - */ +*/ void XsDevice::retainPacket(XsDataPacket const& pack) { LockGuarded lockG(&m_deviceMutex); @@ -4253,7 +4324,7 @@ uint32_t XsDevice::deviceBufferSize() /*! \brief Request the device to set it's internal buffer to the specified size \param frames: buffer size in frames \return True if the setting was successfully updated - */ +*/ bool XsDevice::setDeviceBufferSize(uint32_t frames) { (void)frames; @@ -4432,7 +4503,7 @@ void XsDevice::reinitializeProcessors() } /*! \brief This function indicates if the device supports retransmissions (at all) - \details This is required for the handleDataMessage to deal with missed data properly. + \details This is required for the handleDataPacket to deal with missed data properly. \param packetId The ID of the packet that we want to check \return true if a retransmission is expected for the given packet \note Devices that return true are expected to handle their retransmissions properly themselves! @@ -4471,7 +4542,7 @@ XsDevice const* XsDevice::firstChild() const \return true if interpolation was successful, false if it was not \note The default implementation does nothing */ -bool XsDevice::interpolateMissingData(XsDataPacket const & pack, XsDataPacket const & prev, std::function packetHandler) +bool XsDevice::interpolateMissingData(XsDataPacket const& pack, XsDataPacket const& prev, std::function packetHandler) { (void) pack; (void) prev; @@ -4514,3 +4585,34 @@ XsResultValue XsDevice::updatePortInfo(XsPortInfo const& newInfo) (void) newInfo; return XRV_NOTIMPLEMENTED; } + +/*! \brief Returns the sub-device at index \a index + \details This function returns a software-only sub device of a larger device. It could for example return an XsDevice representing a single finger segment of an Xsens Glove. + These devices have only limited functionality and are typically only used for data extraction as all communication goes through the main device. + \param subDeviceId The sub device id of the device to return. Which ids are valid depends on the type of device, 0 always indicates the main device. + \returns A pointer to the found %XsDevice or 0 if the device could not be found or the device does not have sub-devices. +*/ +XsDevice* XsDevice::subDevice(int subDeviceId) const +{ + if (subDeviceId) + return nullptr; + return const_cast(this); +} + +/*! \brief Returns the number of sub-devices of this device + \details This function returns the number of software-only sub devices of a larger device. + \returns The number of available sub-devices or 0 if none are available + \sa subDevice +*/ +int XsDevice::subDeviceCount() const +{ + return 0; +} + +/*! \brief Returns internal meta-data about the recording that some devices store in the mtb logfile + \returns Internal meta-data about the recording that some devices store in the mtb logfile +*/ +XsByteArray XsDevice::readMetaDataFromLogFile() +{ + return XsByteArray(); +} diff --git a/lib/xspublic/xscontroller/xsdevice_def.h b/lib/xspublic/xscontroller/xsdevice_def.h index 7ffec3b..d5a88bf 100644 --- a/lib/xspublic/xscontroller/xsdevice_def.h +++ b/lib/xspublic/xscontroller/xsdevice_def.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -79,6 +79,7 @@ #include "xsaccesscontrolmode.h" #include "datapacketcache.h" #include +#include #include "lastresultmanager.h" #include "xsgnssplatform.h" #include @@ -160,7 +161,8 @@ struct XsFilterProfile; //AUTO struct XdaConfig; -struct XsDevice : public CallbackManagerXda { +struct XsDevice : public CallbackManagerXda +{ public: XSNOEXPORT virtual ~XsDevice(); @@ -168,10 +170,12 @@ struct XsDevice : public CallbackManagerXda { virtual void removeRef(); XsSize refCounter() const; - virtual XsDevice *master() const; + virtual XsDevice* master() const; - virtual XsDevice *findDevice(XsDeviceId const& deviceid); - XsDevice const *findDeviceConst(XsDeviceId const& deviceid) const; + virtual XsDevice* findDevice(XsDeviceId const& deviceid) const; + XsDevice const* findDeviceConst(XsDeviceId const& deviceid) const; + virtual XsDevice* subDevice(int subDeviceId) const; + virtual int subDeviceCount() const; virtual int busId() const; XsDeviceId const& deviceId() const; virtual XsVersion firmwareVersion() const; @@ -180,16 +184,16 @@ struct XsDevice : public CallbackManagerXda { bool isInitialized() const; bool isStandaloneDevice() const; - const XsDevice *deviceAtBusIdConst(int busid) const; - virtual XsDevice *deviceAtBusId(int busid); + const XsDevice* deviceAtBusIdConst(int busid) const; + virtual XsDevice* deviceAtBusId(int busid); - Communicator XSNOEXPORT *communicator() const; - DataLogger XSNOEXPORT *logFileInterface(std::unique_ptr& myLock) const; + Communicator XSNOEXPORT* communicator() const; + virtual DataLogger XSNOEXPORT* logFileInterface(std::unique_ptr& myLock) const; virtual XsResultValue updatePortInfo(XsPortInfo const& newInfo); void setGotoConfigOnClose(bool gotoConfigOnClose); - XsResultValue createLogFile(const XsString &filename); + XsResultValue createLogFile(const XsString& filename); virtual bool closeLogFile(); virtual bool isMeasuring() const; @@ -208,7 +212,7 @@ struct XsDevice : public CallbackManagerXda { virtual XsDeviceState deviceState() const; static bool supportsSyncSettings(XsDeviceId const& deviceId); - static bool isCompatibleSyncSetting(XsDeviceId const& deviceId, XsSyncSetting const & setting1, XsSyncSetting const & setting2); + static bool isCompatibleSyncSetting(XsDeviceId const& deviceId, XsSyncSetting const& setting1, XsSyncSetting const& setting2); static unsigned int syncSettingsTimeResolutionInMicroSeconds(XsDeviceId const& deviceId); #ifdef DOXYGEN @@ -220,16 +224,24 @@ struct XsDevice : public CallbackManagerXda { //! \brief Compare device ID with that of \a dev \param dev Device to compare against \returns true if \a dev has a higher device ID \sa deviceId() bool operator < (const XsDevice& dev) const - { return m_deviceId.toInt() < dev.m_deviceId.toInt(); } + { + return m_deviceId.toInt() < dev.m_deviceId.toInt(); + } //! \brief Compare device ID with that of \a dev \param dev Device to compare against \returns true if \a dev has the same device ID \sa deviceId() bool operator == (const XsDevice& dev) const - { return m_deviceId.toInt() == dev.m_deviceId.toInt(); } + { + return m_deviceId.toInt() == dev.m_deviceId.toInt(); + } //! \brief Compare device ID with \a devId \param devId DeviceId to compare against \returns true if \a devId is higher than the contained device ID \sa deviceId() bool operator < (XsDeviceId const& devId) const - { return m_deviceId.toInt() < devId.toInt(); } + { + return m_deviceId.toInt() < devId.toInt(); + } //! \brief Compare device ID with \a devId \param devId DeviceId to compare against \returns true if \a devId is the same as the contained device ID \sa deviceId() bool operator == (XsDeviceId const& devId) const - { return m_deviceId.toInt() == devId.toInt(); } + { + return m_deviceId.toInt() == devId.toInt(); + } XsDeviceConfiguration deviceConfiguration() const; XsDeviceConfiguration& XSNOEXPORT deviceConfigurationRef(); @@ -237,7 +249,9 @@ struct XsDevice : public CallbackManagerXda { //! \internal XSNOEXPORT template T* toType() - { return dynamic_cast(this); } + { + return dynamic_cast(this); + } virtual bool XSNOEXPORT initialize(); virtual bool XSNOEXPORT initializeSoftwareCalibration(); @@ -275,14 +289,14 @@ struct XsDevice : public CallbackManagerXda { XSNOEXPORT virtual bool sendCustomMessage(const XsMessage& messageSend, bool waitForResult, XsXbusMessageId messageId, XsMessage& messageReceive, int timeout = 0); XSNOEXPORT virtual bool waitForCustomMessage(XsXbusMessageId messageId, XsMessage& messageReceive, int timeout = 0); - XSNOEXPORT virtual bool waitForCustomMessage(std::shared_ptr reply, XsMessage &messageReceive, int timeout); + XSNOEXPORT virtual bool waitForCustomMessage(std::shared_ptr reply, XsMessage& messageReceive, int timeout); virtual std::shared_ptr addReplyObject(XsXbusMessageId messageId, uint8_t data); - XSNOEXPORT virtual void handleMessage(const XsMessage &msg); + XSNOEXPORT virtual void handleMessage(const XsMessage& msg); XSNOEXPORT virtual void handleDataPacket(const XsDataPacket& packet); - XSNOEXPORT virtual void handleNonDataMessage(const XsMessage &msg); - XSNOEXPORT virtual void handleErrorMessage(const XsMessage &msg); - XSNOEXPORT virtual void handleWarningMessage(const XsMessage &msg); + XSNOEXPORT virtual void handleNonDataMessage(const XsMessage& msg); + XSNOEXPORT virtual void handleErrorMessage(const XsMessage& msg); + XSNOEXPORT virtual void handleWarningMessage(const XsMessage& msg); XSNOEXPORT virtual void handleWakeupMessage(const XsMessage& msg); virtual bool setSerialBaudRate(XsBaudRate baudrate); @@ -303,7 +317,7 @@ struct XsDevice : public CallbackManagerXda { virtual XsOutputConfigurationArray outputConfiguration() const; virtual XsOutputConfigurationArray processedOutputConfiguration() const; - virtual bool setOutputConfiguration(XsOutputConfigurationArray& config); + bool setOutputConfiguration(XsOutputConfigurationArray& config); virtual bool isInStringOutputMode() const; virtual XsCanOutputConfigurationArray canOutputConfiguration() const; virtual bool setCanOutputConfiguration(XsCanOutputConfigurationArray& config); @@ -381,7 +395,7 @@ struct XsDevice : public CallbackManagerXda { virtual void discardRetransmissions(int64_t firstNewPacketId); //virtual int radioQualityIndication() const; - XSNOEXPORT virtual void handleMasterIndication(const XsMessage &message); + XSNOEXPORT virtual void handleMasterIndication(const XsMessage& message); virtual bool abortFlushing(); virtual bool setDeviceAccepted(const XsDeviceId& deviceId); virtual bool setDeviceRejected(const XsDeviceId& deviceId); @@ -425,7 +439,7 @@ struct XsDevice : public CallbackManagerXda { virtual int locationId() const; virtual XsDevice* getDeviceFromLocationId(uint16_t locId); virtual XsMatrix objectAlignment() const; - virtual bool setObjectAlignment(const XsMatrix &matrix); + virtual bool setObjectAlignment(const XsMatrix& matrix); virtual double gravityMagnitude() const; virtual bool setGravityMagnitude(double mag); virtual XsVector initialPositionLLA() const; @@ -477,14 +491,17 @@ struct XsDevice : public CallbackManagerXda { virtual XsQuaternion alignmentRotationQuaternion(XsAlignmentFrame frame) const; //! \returns The device mutex. - xsens::GuardedMutex* mutex() const { return &m_deviceMutex; } + xsens::GuardedMutex* mutex() const + { + return &m_deviceMutex; + } - virtual bool deviceIsDocked(XsDevice *dev) const; + virtual bool deviceIsDocked(XsDevice* dev) const; bool isLoadLogFileInProgress() const; void waitForLoadLogFileDone() const; - XSNOEXPORT virtual bool messageLooksSane(const XsMessage &msg) const; + XSNOEXPORT virtual bool messageLooksSane(const XsMessage& msg) const; XSNOEXPORT virtual void prepareForTermination(); // MTi with RTK support @@ -501,6 +518,7 @@ struct XsDevice : public CallbackManagerXda { XSNOEXPORT virtual bool readEmtsAndDeviceConfiguration(); virtual uint32_t supportedStatusFlags() const; + virtual XsByteArray readMetaDataFromLogFile(); protected: virtual XsDevice const* firstChild() const; @@ -515,7 +533,7 @@ struct XsDevice : public CallbackManagerXda { friend struct XsDeviceEx; /*! \private @{ */ - void setFirmwareVersion(const XsVersion &version); + void setFirmwareVersion(const XsVersion& version); void extractFirmwareVersion(XsMessage const& message); friend class MtContainer; @@ -528,47 +546,53 @@ struct XsDevice : public CallbackManagerXda { explicit XsDevice(XsDeviceId const& id); explicit XsDevice(Communicator* comm); - explicit XsDevice(XsDevice *master, const XsDeviceId &childDeviceId); + explicit XsDevice(XsDevice* master, const XsDeviceId& childDeviceId); /*! \return A const reference to the cached device configuration */ inline const XsDeviceConfiguration& deviceConfig() const - { return m_config; } + { + return m_config; + } - void setDeviceId(const XsDeviceId &deviceId); + void setDeviceId(const XsDeviceId& deviceId); XsOutputConfiguration findConfiguration(XsDataIdentifier dataType) const; - virtual void writeMessageToLogFile(const XsMessage &message); + virtual void writeMessageToLogFile(const XsMessage& message); virtual void writeFilterStateToFile(); virtual void processLivePacket(XsDataPacket& pack); virtual void processBufferedPacket(XsDataPacket& pack); - bool readDeviceConfiguration(); + virtual bool readDeviceConfiguration(); /*! \return A reference to the cached latest received packet */ inline XsDataPacket& latestLivePacket() { - assert(m_latestLivePacket); assert(m_deviceMutex.haveGuardedLock()); + assert(m_latestLivePacket); + assert(m_deviceMutex.haveGuardedLock()); return *m_latestLivePacket; } /*! \return A reference to the cached latest buffered packet */ inline XsDataPacket& latestBufferedPacket() { - assert(m_latestBufferedPacket); assert(m_deviceMutex.haveGuardedLock()); + assert(m_latestBufferedPacket); + assert(m_deviceMutex.haveGuardedLock()); return *m_latestBufferedPacket; } /*! \return A const reference to the cached latest received packet */ inline XsDataPacket const& latestLivePacketConst() const { - assert(m_latestLivePacket); assert(m_deviceMutex.haveGuardedLock()); + assert(m_latestLivePacket); + assert(m_deviceMutex.haveGuardedLock()); return *m_latestLivePacket; } /*! \return A const reference to the cached latest buffered packet */ inline XsDataPacket const& latestBufferedPacketConst() const { - assert(m_latestBufferedPacket); assert(m_deviceMutex.haveGuardedLock()); + assert(m_latestBufferedPacket); + assert(m_deviceMutex.haveGuardedLock()); return *m_latestBufferedPacket; } virtual int64_t latestLivePacketId() const; @@ -580,18 +604,23 @@ struct XsDevice : public CallbackManagerXda { virtual XsConnectivityState defaultChildConnectivityState() const; //! \brief Set the initialized state to \a initialized - void setInitialized(bool initialized) { m_isInitialized = initialized; } + void setInitialized(bool initialized) + { + m_isInitialized = initialized; + } //! \brief Set the "termination prepared" state to \a prepared void setTerminationPrepared(bool prepared) - NOEXCEPT { m_terminationPrepared = prepared; } + NOEXCEPT { m_terminationPrepared = prepared; } + + virtual bool shouldWriteMessageToLogFile(const XsMessage& msg) const; + virtual bool shouldWriteMessageToLogFile(const XsDevice* dev, const XsMessage& message) const; - virtual bool shouldWriteMessageToLogFile(const XsMessage &msg) const; - virtual bool shouldWriteMessageToLogFile(const XsDevice *dev, const XsMessage &message) const; + virtual XsResultValue setOutputConfigurationInternal(XsOutputConfigurationArray& config); public: - XSNOEXPORT virtual void onMessageSent(const XsMessage &message); - XSNOEXPORT virtual void onMessageReceived(const XsMessage &message); - XSNOEXPORT virtual void onMessageDetected2(XsProtocolType type, const XsByteArray &rawMessage); + XSNOEXPORT virtual void onMessageSent(const XsMessage& message); + XSNOEXPORT virtual void onMessageReceived(const XsMessage& message); + XSNOEXPORT virtual void onMessageDetected2(XsProtocolType type, const XsByteArray& rawMessage); XSNOEXPORT virtual void onSessionRestarted(); XSNOEXPORT virtual void onConnectionLost(); @@ -600,13 +629,16 @@ struct XsDevice : public CallbackManagerXda { XSNOEXPORT virtual int64_t deviceRecordingBufferItemCount(int64_t& lastCompletePacketId) const; protected: - bool doTransaction(const XsMessage &snd) const; - bool doTransaction(const XsMessage &snd, XsMessage &rcv) const; - bool doTransaction(const XsMessage &snd, XsMessage &rcv, uint32_t timeout) const; - bool doTransaction(const XsMessage &snd, uint32_t timeout) const; + bool doTransaction(const XsMessage& snd) const; + bool doTransaction(const XsMessage& snd, XsMessage& rcv) const; + bool doTransaction(const XsMessage& snd, XsMessage& rcv, uint32_t timeout) const; + bool doTransaction(const XsMessage& snd, uint32_t timeout) const; //! \return The value of the m_justWriteSetting flag, which is used in file-based processing - bool justWriteSetting() const { return m_justWriteSetting; } + bool justWriteSetting() const + { + return m_justWriteSetting; + } virtual void clearProcessors(); virtual void clearDataCache(); @@ -642,11 +674,14 @@ struct XsDevice : public CallbackManagerXda { void setCommunicator(Communicator* comm); //! \returns True if skip EMTS read on initialization is set to true - bool skipEmtsReadOnInit() const { return m_skipEmtsReadOnInit; } - static bool checkDataEnabled(XsDataIdentifier dataType, XsOutputConfigurationArray const & configurations); - virtual bool shouldDataMsgBeRecorded(const XsMessage &msg) const; + bool skipEmtsReadOnInit() const + { + return m_skipEmtsReadOnInit; + } + static bool checkDataEnabled(XsDataIdentifier dataType, XsOutputConfigurationArray const& configurations); + virtual bool shouldDataMsgBeRecorded(const XsMessage& msg) const; virtual bool shouldDoRecordedCallback(XsDataPacket const& packet) const; - virtual bool interpolateMissingData(XsDataPacket const & pack, XsDataPacket const & prev, std::function packetHandler); + virtual bool interpolateMissingData(XsDataPacket const& pack, XsDataPacket const& prev, std::function packetHandler); //! \brief A data cache DataPacketCache m_dataCache; @@ -672,6 +707,9 @@ struct XsDevice : public CallbackManagerXda { //! \brief A device configuration XsDeviceConfiguration m_config; + //! \brief A devices output configuration + XsOutputConfigurationArray m_outputConfiguration; + //! \brief A firmware version XsVersion m_firmwareVersion; @@ -749,9 +787,9 @@ struct XsDevice : public CallbackManagerXda { }; #ifndef XDA_PRIVATE_BUILD -#include "xsdevice_public.h" + #include "xsdevice_public.h" #else -#include "xsdeviceex.h" + #include "xsdeviceex.h" #endif #endif diff --git a/lib/xspublic/xscontroller/xsdevice_public.h b/lib/xspublic/xscontroller/xsdevice_public.h index 9b9a8a9..f60cd12 100644 --- a/lib/xspublic/xscontroller/xsdevice_public.h +++ b/lib/xspublic/xscontroller/xsdevice_public.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -67,8 +67,9 @@ #include "xsdevice_def.h" -namespace xsens { - class XSNOEXPORT SettingsFile; +namespace xsens +{ +class XSNOEXPORT SettingsFile; } /*! \class XsDeviceEx @@ -84,7 +85,7 @@ struct XsDeviceEx : public XsDevice explicit XsDeviceEx(XsDeviceId const& id) : XsDevice(id) {} //! Construct a device using a device id \a childDeviceId for master \a masterDevice - explicit XsDeviceEx(XsDevice *master, const XsDeviceId &childDeviceId) : XsDevice(master, childDeviceId) {} + explicit XsDeviceEx(XsDevice* master, const XsDeviceId& childDeviceId) : XsDevice(master, childDeviceId) {} //! Destroy the device virtual ~XsDeviceEx() {} diff --git a/lib/xspublic/xscontroller/xsdeviceconfiguration.c b/lib/xspublic/xscontroller/xsdeviceconfiguration.c index d006741..ad4155e 100644 --- a/lib/xspublic/xscontroller/xsdeviceconfiguration.c +++ b/lib/xspublic/xscontroller/xsdeviceconfiguration.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -101,7 +101,7 @@ void XsDeviceConfiguration_assign(XsDeviceConfiguration* thisPtr, XsSize numberO if (numberOfDevices) { // init to size - *((XsMtDeviceConfiguration**) &thisPtr->m_deviceInfo) = (XsMtDeviceConfiguration*) malloc(numberOfDevices*sizeof(XsMtDeviceConfiguration)); + *((XsMtDeviceConfiguration**) &thisPtr->m_deviceInfo) = (XsMtDeviceConfiguration*) malloc(numberOfDevices * sizeof(XsMtDeviceConfiguration)); INC_ALLOC; } } @@ -113,7 +113,7 @@ void XsDeviceConfiguration_assign(XsDeviceConfiguration* thisPtr, XsSize numberO { memcpy(&thisPtr->m_masterInfo, &src->m_masterInfo, sizeof(XsMasterDeviceConfiguration)); if (numberOfDevices) - memcpy(thisPtr->m_deviceInfo, src->m_deviceInfo, numberOfDevices*sizeof(XsMtDeviceConfiguration)); + memcpy(thisPtr->m_deviceInfo, src->m_deviceInfo, numberOfDevices * sizeof(XsMtDeviceConfiguration)); } } @@ -163,13 +163,11 @@ void XsDeviceConfiguration_readFromMessage(XsDeviceConfiguration* thisPtr, const nDevs = XsMessage_getDataShort(msg, 96); if (nDevs != thisPtr->m_numberOfDevices) - { XsDeviceConfiguration_assign(thisPtr, nDevs, 0); - } thisPtr->m_masterInfo.m_masterDeviceId = XsMessage_getDataLong(msg, 0); { - XsDeviceId deviceId = { thisPtr->m_masterInfo.m_masterDeviceId, XSDEVICEID_PRODUCT_CODE_INIT, 0, 0}; + XsDeviceId deviceId = { thisPtr->m_masterInfo.m_masterDeviceId, XSDEVICEID_PRODUCT_CODE_INIT, 0, 0, 0}; if (XsDeviceId_isLegacyDeviceId(&deviceId)) { thisPtr->m_masterInfo.m_samplingPeriod = XsMessage_getDataShort(msg, 4); @@ -189,26 +187,26 @@ void XsDeviceConfiguration_readFromMessage(XsDeviceConfiguration* thisPtr, const for (i = 0; i < nDevs; ++i) { - thisPtr->m_deviceInfo[i].m_deviceId = XsMessage_getDataLong(msg, 98+i*20); - XsDeviceId deviceId = {thisPtr->m_deviceInfo[i].m_deviceId, XSDEVICEID_PRODUCT_CODE_INIT, 0, 0}; + thisPtr->m_deviceInfo[i].m_deviceId = XsMessage_getDataLong(msg, 98 + i * 20); + XsDeviceId deviceId = {thisPtr->m_deviceInfo[i].m_deviceId, XSDEVICEID_PRODUCT_CODE_INIT, 0, 0, 0}; if (XsDeviceId_isLegacyDeviceId(&deviceId)) { - thisPtr->m_deviceInfo[i].m_deviceId = (uint32_t)XsMessage_getDataLong(msg, 98+i*20); + thisPtr->m_deviceInfo[i].m_deviceId = (uint32_t)XsMessage_getDataLong(msg, 98 + i * 20); memcpy(thisPtr->m_deviceInfo[i].m_reserved, XsMessage_getDataBuffer(msg, 102 + i * 20), 8); } else { - thisPtr->m_deviceInfo[i].m_deviceId = XsMessage_getDataLong(msg, 98+i*20); - thisPtr->m_deviceInfo[i].m_deviceId |= ((uint64_t)XsMessage_getDataLong(msg, 102+i*20) << 32); + thisPtr->m_deviceInfo[i].m_deviceId = XsMessage_getDataLong(msg, 98 + i * 20); + thisPtr->m_deviceInfo[i].m_deviceId |= ((uint64_t)XsMessage_getDataLong(msg, 102 + i * 20) << 32); memcpy(thisPtr->m_deviceInfo[i].m_reserved, XsMessage_getDataBuffer(msg, 106 + i * 20), 4); } - thisPtr->m_deviceInfo[i].m_filterProfile = XsMessage_getDataShort(msg, 110+i*20); - thisPtr->m_deviceInfo[i].m_fwRevMajor = XsMessage_getDataByte(msg, 112+i*20); - thisPtr->m_deviceInfo[i].m_fwRevMinor = XsMessage_getDataByte(msg, 113+i*20); - thisPtr->m_deviceInfo[i].m_fwRevRevision = XsMessage_getDataByte(msg, 114+i*20); - thisPtr->m_deviceInfo[i].m_filterType = (char)XsMessage_getDataByte(msg, 115+i*20); - thisPtr->m_deviceInfo[i].m_filterMajor = XsMessage_getDataByte(msg, 116+i*20); - thisPtr->m_deviceInfo[i].m_filterMinor = XsMessage_getDataByte(msg, 117+i*20); + thisPtr->m_deviceInfo[i].m_filterProfile = XsMessage_getDataShort(msg, 110 + i * 20); + thisPtr->m_deviceInfo[i].m_fwRevMajor = XsMessage_getDataByte(msg, 112 + i * 20); + thisPtr->m_deviceInfo[i].m_fwRevMinor = XsMessage_getDataByte(msg, 113 + i * 20); + thisPtr->m_deviceInfo[i].m_fwRevRevision = XsMessage_getDataByte(msg, 114 + i * 20); + thisPtr->m_deviceInfo[i].m_filterType = (char)XsMessage_getDataByte(msg, 115 + i * 20); + thisPtr->m_deviceInfo[i].m_filterMajor = XsMessage_getDataByte(msg, 116 + i * 20); + thisPtr->m_deviceInfo[i].m_filterMinor = XsMessage_getDataByte(msg, 117 + i * 20); } } @@ -228,17 +226,17 @@ void XsDeviceConfiguration_writeToMessage(const XsDeviceConfiguration* thisPtr, msgHeader->m_busId = XS_BID_MASTER; { - XsDeviceId deviceId = {thisPtr->m_masterInfo.m_masterDeviceId, XSDEVICEID_PRODUCT_CODE_INIT, 0, 0}; + XsDeviceId deviceId = {thisPtr->m_masterInfo.m_masterDeviceId, XSDEVICEID_PRODUCT_CODE_INIT, 0, 0, 0}; if (XsDeviceId_isLegacyDeviceId(&deviceId)) { - XsMessage_setDataLong (msg, (uint32_t)thisPtr->m_masterInfo.m_masterDeviceId, 0); - XsMessage_setDataShort (msg, thisPtr->m_masterInfo.m_samplingPeriod, 4); - XsMessage_setDataShort (msg, thisPtr->m_masterInfo.m_outputSkipFactor, 6); + XsMessage_setDataLong(msg, (uint32_t)thisPtr->m_masterInfo.m_masterDeviceId, 0); + XsMessage_setDataShort(msg, thisPtr->m_masterInfo.m_samplingPeriod, 4); + XsMessage_setDataShort(msg, thisPtr->m_masterInfo.m_outputSkipFactor, 6); } else { - XsMessage_setDataLong (msg, (uint32_t)thisPtr->m_masterInfo.m_masterDeviceId & 0x00000000FFFFFFFF, 0); - XsMessage_setDataLong (msg, (uint32_t)((thisPtr->m_masterInfo.m_masterDeviceId & 0xFFFFFFFF00000000) >> 32), 4); + XsMessage_setDataLong(msg, (uint32_t)thisPtr->m_masterInfo.m_masterDeviceId & 0x00000000FFFFFFFF, 0); + XsMessage_setDataLong(msg, (uint32_t)((thisPtr->m_masterInfo.m_masterDeviceId & 0xFFFFFFFF00000000) >> 32), 4); } } @@ -251,25 +249,25 @@ void XsDeviceConfiguration_writeToMessage(const XsDeviceConfiguration* thisPtr, for (i = 0; i < thisPtr->m_numberOfDevices; ++i) { - XsDeviceId deviceId = {thisPtr->m_deviceInfo[i].m_deviceId, XSDEVICEID_PRODUCT_CODE_INIT, 0, 0}; + XsDeviceId deviceId = {thisPtr->m_deviceInfo[i].m_deviceId, XSDEVICEID_PRODUCT_CODE_INIT, 0, 0, 0}; if (XsDeviceId_isLegacyDeviceId(&deviceId)) { - XsMessage_setDataLong (msg, (uint32_t)thisPtr->m_deviceInfo[i].m_deviceId, 98+i*20); - XsMessage_setDataBuffer(msg, thisPtr->m_masterInfo.m_reserved1, 8, 102+i*20); + XsMessage_setDataLong(msg, (uint32_t)thisPtr->m_deviceInfo[i].m_deviceId, 98 + i * 20); + XsMessage_setDataBuffer(msg, thisPtr->m_masterInfo.m_reserved1, 8, 102 + i * 20); } else { - XsMessage_setDataLong (msg, (uint32_t)thisPtr->m_deviceInfo[i].m_deviceId & 0x00000000FFFFFFFF, 98+i*20); - XsMessage_setDataLong (msg, (uint32_t)((thisPtr->m_deviceInfo[i].m_deviceId & 0xFFFFFFFF00000000) >> 32), 102+i*20); - XsMessage_setDataBuffer(msg, thisPtr->m_masterInfo.m_reserved1, 4, 106+i*20); + XsMessage_setDataLong(msg, (uint32_t)thisPtr->m_deviceInfo[i].m_deviceId & 0x00000000FFFFFFFF, 98 + i * 20); + XsMessage_setDataLong(msg, (uint32_t)((thisPtr->m_deviceInfo[i].m_deviceId & 0xFFFFFFFF00000000) >> 32), 102 + i * 20); + XsMessage_setDataBuffer(msg, thisPtr->m_masterInfo.m_reserved1, 4, 106 + i * 20); } - XsMessage_setDataShort(msg, thisPtr->m_deviceInfo[i].m_filterProfile, 110+i*20); - XsMessage_setDataByte (msg, thisPtr->m_deviceInfo[i].m_fwRevMajor, 112+i*20); - XsMessage_setDataByte (msg, thisPtr->m_deviceInfo[i].m_fwRevMinor, 113+i*20); - XsMessage_setDataByte (msg, thisPtr->m_deviceInfo[i].m_fwRevRevision, 114+i*20); - XsMessage_setDataByte (msg, (uint8_t) thisPtr->m_deviceInfo[i].m_filterType, 115+i*20); - XsMessage_setDataByte (msg, thisPtr->m_deviceInfo[i].m_filterMajor, 116+i*20); - XsMessage_setDataByte (msg, thisPtr->m_deviceInfo[i].m_filterMinor, 117+i*20); + XsMessage_setDataShort(msg, thisPtr->m_deviceInfo[i].m_filterProfile, 110 + i * 20); + XsMessage_setDataByte(msg, thisPtr->m_deviceInfo[i].m_fwRevMajor, 112 + i * 20); + XsMessage_setDataByte(msg, thisPtr->m_deviceInfo[i].m_fwRevMinor, 113 + i * 20); + XsMessage_setDataByte(msg, thisPtr->m_deviceInfo[i].m_fwRevRevision, 114 + i * 20); + XsMessage_setDataByte(msg, (uint8_t) thisPtr->m_deviceInfo[i].m_filterType, 115 + i * 20); + XsMessage_setDataByte(msg, thisPtr->m_deviceInfo[i].m_filterMajor, 116 + i * 20); + XsMessage_setDataByte(msg, thisPtr->m_deviceInfo[i].m_filterMinor, 117 + i * 20); } XsMessage_recomputeChecksum(msg); } @@ -292,4 +290,4 @@ XsSize XsDeviceConfiguration_findDevice(const XsDeviceConfiguration* thisPtr, co } return 0; } - /*! @} */ +/*! @} */ diff --git a/lib/xspublic/xscontroller/xsdeviceconfiguration.h b/lib/xspublic/xscontroller/xsdeviceconfiguration.h index 7f87757..0c51971 100644 --- a/lib/xspublic/xscontroller/xsdeviceconfiguration.h +++ b/lib/xspublic/xscontroller/xsdeviceconfiguration.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -70,7 +70,7 @@ #include #include #ifdef __cplusplus -#include + #include #endif struct MtwInfo; @@ -99,7 +99,8 @@ XDA_DLL_API XsSize XsDeviceConfiguration_findDevice(const struct XsDeviceConfigu #endif /*! \brief %Device information for MT devices in an XsDeviceConfiguration. */ -struct XsMtDeviceConfiguration { +struct XsMtDeviceConfiguration +{ uint64_t m_deviceId; /*!< \brief This device ID */ uint8_t m_reserved[8]; /*!< \brief Reserved space */ uint16_t m_filterProfile; /*!< \brief The currently chosen filter profile */ @@ -114,7 +115,8 @@ struct XsMtDeviceConfiguration { typedef struct XsMtDeviceConfiguration XsMtDeviceConfiguration; /*! \brief Device information for the main device in an XsDeviceConfiguration. */ -struct XsMasterDeviceConfiguration { +struct XsMasterDeviceConfiguration +{ uint64_t m_masterDeviceId; /*!< \brief The master device ID */ uint16_t m_samplingPeriod; /*!< \brief The sampling period */ uint16_t m_outputSkipFactor; /*!< \brief The output skip factor */ @@ -128,16 +130,18 @@ typedef struct XsMasterDeviceConfiguration XsMasterDeviceConfiguration; #ifdef __cplusplus /*! \class XsDeviceConfigurationException - Exception class thrown when an exception occured inside the XsDeviceConfiguration + Exception class thrown when an exception occurred inside the XsDeviceConfiguration */ -class XsDeviceConfigurationException : public XsException { +class XsDeviceConfigurationException : public XsException +{ public: XsDeviceConfigurationException() : XsException("Invalid device configuration") {} }; #endif /*! \brief Structure containing a full device configuration as returned by the ReqConfig message. */ -struct XsDeviceConfiguration { +struct XsDeviceConfiguration +{ #ifdef __cplusplus /*! \brief Constructor @@ -157,7 +161,7 @@ struct XsDeviceConfiguration { /*! \brief Copy constructor \param other the object to copy \sa XsDeviceConfiguration_copy - */ + */ XsDeviceConfiguration(const XsDeviceConfiguration& other) : m_numberOfDevices(0) , m_deviceInfo(0) @@ -172,7 +176,7 @@ struct XsDeviceConfiguration { \returns a const reference to this object \sa XsDeviceConfiguration_copy - */ + */ inline const XsDeviceConfiguration& operator = (const XsDeviceConfiguration& other) { XsDeviceConfiguration_copy(this, &other); @@ -204,8 +208,8 @@ struct XsDeviceConfiguration { \param msg the message to read the device configuration from \sa XsDeviceConfiguration_readFromMessage - */ - inline void readFromMessage(const XsMessage &msg) + */ + inline void readFromMessage(const XsMessage& msg) { XsDeviceConfiguration_readFromMessage(this, &msg); } @@ -215,7 +219,7 @@ struct XsDeviceConfiguration { \param msg the message to write the device configuration to \sa XsDeviceConfiguration_writeToMessage - */ + */ inline void writeToMessage(XsMessage& msg) const { XsDeviceConfiguration_writeToMessage(this, &msg); @@ -274,7 +278,7 @@ struct XsDeviceConfiguration { if (busId > m_numberOfDevices) throw XsDeviceConfigurationException(); - return m_deviceInfo[busId-1]; + return m_deviceInfo[busId - 1]; } /*! \brief The device info for the device at \a busId @@ -294,7 +298,7 @@ struct XsDeviceConfiguration { if (busId > m_numberOfDevices) throw XsDeviceConfigurationException(); - return m_deviceInfo[busId-1]; + return m_deviceInfo[busId - 1]; } @@ -318,7 +322,7 @@ struct XsDeviceConfiguration { /*! \brief Set the number of devices to \a count \param count the new number of devices to allocate for - */ + */ inline void setNumberOfDevices(XsSize count) { XsDeviceConfiguration_assign(this, count, 0); @@ -326,7 +330,7 @@ struct XsDeviceConfiguration { /*! \brief The current number of devices \returns the number of devices - */ + */ inline XsSize numberOfDevices() const { return (XsSize) m_numberOfDevices; @@ -341,7 +345,7 @@ struct XsDeviceConfiguration { } private: -//! \protectedsection + //! \protectedsection #endif XsMasterDeviceConfiguration m_masterInfo; //!< \brief The master info const uint16_t m_numberOfDevices; //!< \brief The currently allocated number of devices diff --git a/lib/xspublic/xscontroller/xsdeviceparameter.h b/lib/xspublic/xscontroller/xsdeviceparameter.h index 1f12298..0488aa8 100644 --- a/lib/xspublic/xscontroller/xsdeviceparameter.h +++ b/lib/xspublic/xscontroller/xsdeviceparameter.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -75,9 +75,9 @@ struct XsDeviceParameter; #endif /*! - * \class XsDeviceParameter - * \brief Class to set and retrieve parameters from a XsDevice object. - */ + \class XsDeviceParameter + \brief Class to set and retrieve parameters from a XsDevice object. +*/ struct XsDeviceParameter { #ifdef __cplusplus @@ -102,17 +102,17 @@ struct XsDeviceParameter } /*! - * \brief Returns the stored parameter value. - * \returns The parameter value in the desired type. - */ + \brief Returns the stored parameter value. + \returns The parameter value in the desired type. + */ template T getValue() const; /*! - * \brief Sets the parameter value. - * \param value: the desired parameter value. - */ + \brief Sets the parameter value. + \param value: the desired parameter value. + */ template void setValue(T value); @@ -163,7 +163,7 @@ inline int XsDeviceParameter::getValue() const template<> inline void XsDeviceParameter::setValue(bool value) { - m_value = value ? 1 :0; + m_value = value ? 1 : 0; } //! \copydoc XsDeviceParameter::setValue diff --git a/lib/xspublic/xscontroller/xsdeviceparameteridentifier.h b/lib/xspublic/xscontroller/xsdeviceparameteridentifier.h index a00a58b..03cdaa5 100644 --- a/lib/xspublic/xscontroller/xsdeviceparameteridentifier.h +++ b/lib/xspublic/xscontroller/xsdeviceparameteridentifier.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -67,7 +67,7 @@ ////////////////////////////////////////////////////////////////////////////////////////// /*! \addtogroup enums Global enumerations -@{ + @{ */ /*! \enum XsDeviceParameterIdentifier diff --git a/lib/xspublic/xscontroller/xsdeviceptr.h b/lib/xspublic/xscontroller/xsdeviceptr.h index 8a7725e..3bd0bd8 100644 --- a/lib/xspublic/xscontroller/xsdeviceptr.h +++ b/lib/xspublic/xscontroller/xsdeviceptr.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xscontroller/xsdeviceptrarray.c b/lib/xspublic/xscontroller/xsdeviceptrarray.c index 5facd3b..537d414 100644 --- a/lib/xspublic/xscontroller/xsdeviceptrarray.c +++ b/lib/xspublic/xscontroller/xsdeviceptrarray.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -106,7 +106,8 @@ void zeroDevicePtr(XsDevicePtr* a) } //! \brief Descriptor for XsDevicePtrArray -XsArrayDescriptor const g_xsDevicePtrArrayDescriptor = { +XsArrayDescriptor const g_xsDevicePtrArrayDescriptor = +{ sizeof(XsDevicePtr), XSEXPCASTITEMSWAP swapDevicePtr, // swap XSEXPCASTITEMMAKE zeroDevicePtr, // construct diff --git a/lib/xspublic/xscontroller/xsdeviceptrarray.h b/lib/xspublic/xscontroller/xsdeviceptrarray.h index 60cbb9b..907d2ee 100644 --- a/lib/xspublic/xscontroller/xsdeviceptrarray.h +++ b/lib/xspublic/xscontroller/xsdeviceptrarray.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -87,33 +87,34 @@ XDA_DLL_API void XsDevicePtrArray_construct(XsDevicePtrArray* thisPtr, XsSize co #endif #ifdef __cplusplus -struct XsDevicePtrArray : public XsArrayImpl { - //! \brief Constructs an XsDevicePtrArray - inline explicit XsDevicePtrArray(XsSize sz = 0, XsDevicePtr const* src = 0) - : ArrayImpl(sz, src) + struct XsDevicePtrArray : public XsArrayImpl { - } + //! \brief Constructs an XsDevicePtrArray + inline explicit XsDevicePtrArray(XsSize sz = 0, XsDevicePtr const* src = 0) + : ArrayImpl(sz, src) + { + } - //! \brief Constructs an XsDevicePtrArray as a copy of \a other - inline XsDevicePtrArray(XsDevicePtrArray const& other) - : ArrayImpl(other) - { - } + //! \brief Constructs an XsDevicePtrArray as a copy of \a other + inline XsDevicePtrArray(XsDevicePtrArray const& other) + : ArrayImpl(other) + { + } - //! \brief Constructs an XsDevicePtrArray that references the data supplied in \a ref - inline explicit XsDevicePtrArray(XsDevicePtr* ref, XsSize sz, XsDataFlags flags /* = XSDF_None */) - : ArrayImpl(ref, sz, flags) - { - } + //! \brief Constructs an XsDevicePtrArray that references the data supplied in \a ref + inline explicit XsDevicePtrArray(XsDevicePtr* ref, XsSize sz, XsDataFlags flags /* = XSDF_None */) + : ArrayImpl(ref, sz, flags) + { + } #ifndef XSENS_NOITERATOR - //! \brief Constructs an XsDevicePtrArray with the array bound by the supplied iterators \a beginIt and \a endIt - template - inline XsDevicePtrArray(Iterator beginIt, Iterator endIt) - : ArrayImpl(beginIt, endIt) - { - } + //! \brief Constructs an XsDevicePtrArray with the array bound by the supplied iterators \a beginIt and \a endIt + template + inline XsDevicePtrArray(Iterator beginIt, Iterator endIt) + : ArrayImpl(beginIt, endIt) + { + } #endif -}; + }; #endif #endif diff --git a/lib/xspublic/xscontroller/xsdevicestate.c b/lib/xspublic/xscontroller/xsdevicestate.c index 6b75c69..f65b7a1 100644 --- a/lib/xspublic/xscontroller/xsdevicestate.c +++ b/lib/xspublic/xscontroller/xsdevicestate.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -71,20 +71,21 @@ */ /*! \brief Convert the device state to a human readable string */ -const char *XsDeviceState_toString(XsDeviceState s) +const char* XsDeviceState_toString(XsDeviceState s) { static char unknown[64]; - switch (s) { - XS_ENUM_TO_STR_CASE(XDS_Initial); - XS_ENUM_TO_STR_CASE(XDS_Config); - XS_ENUM_TO_STR_CASE(XDS_Measurement); - XS_ENUM_TO_STR_CASE(XDS_WaitingForRecordingStart); - XS_ENUM_TO_STR_CASE(XDS_Recording); - XS_ENUM_TO_STR_CASE(XDS_FlushingData); - XS_ENUM_TO_STR_CASE(XDS_Destructing); - default: - sprintf(unknown, "XDS Unknown(%d)", (int) s); - return unknown; + switch (s) + { + XS_ENUM_TO_STR_CASE(XDS_Initial); + XS_ENUM_TO_STR_CASE(XDS_Config); + XS_ENUM_TO_STR_CASE(XDS_Measurement); + XS_ENUM_TO_STR_CASE(XDS_WaitingForRecordingStart); + XS_ENUM_TO_STR_CASE(XDS_Recording); + XS_ENUM_TO_STR_CASE(XDS_FlushingData); + XS_ENUM_TO_STR_CASE(XDS_Destructing); + default: + sprintf(unknown, "XDS Unknown(%d)", (int) s); + return unknown; } } diff --git a/lib/xspublic/xscontroller/xsdevicestate.h b/lib/xspublic/xscontroller/xsdevicestate.h index a20e6a5..5a37a38 100644 --- a/lib/xspublic/xscontroller/xsdevicestate.h +++ b/lib/xspublic/xscontroller/xsdevicestate.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -74,7 +74,8 @@ //AUTO namespace xscontroller { /*! \brief XsDevice state identifiers */ -enum XsDeviceState { +enum XsDeviceState +{ XDS_Initial, /*!< Initial unknown state */ XDS_Config, /*!< Configuration mode. */ XDS_Measurement, /*!< Measurement mode, devices are transmitting data. */ @@ -92,7 +93,7 @@ extern "C" { #endif /*! \brief Convert the device state to a human readable string */ -const char *XsDeviceState_toString(XsDeviceState s); +XDA_DLL_API const char* XsDeviceState_toString(XsDeviceState s); #ifdef __cplusplus } // extern "C" @@ -107,4 +108,4 @@ std::basic_ostream<_CharT, _Traits>& operator<<(std::basic_ostream<_CharT, _Trai #endif //__cplusplus -#endif // file guard +#endif diff --git a/lib/xspublic/xscontroller/xserrormode.h b/lib/xspublic/xscontroller/xserrormode.h index ec50d7e..549a65c 100644 --- a/lib/xspublic/xscontroller/xserrormode.h +++ b/lib/xspublic/xscontroller/xserrormode.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -71,12 +71,13 @@ /*! \brief Error modes for use in XsDevice::setErrorMode \sa XsDevice::setErrorMode \sa XsDevice::errorMode */ -enum XsErrorMode { - XEM_Ignore = 0x0000 //!< Ignore all errors without warning - ,XEM_IncreasePacketCounter = 0x0001 //!< Increase packet counter when an error occurs, resulting in gaps in the packet counter - ,XEM_IncreasePacketCounterAndSendError = 0x0002 //!< Increase packet counter when an error occurs, resulting in gaps in the packet counter and send an explicit error message - ,XEM_SendErrorAndGoToConfig = 0x0003 //!< Abort measuring and send an error message - ,XEM_Invalid //!< No error mode available +enum XsErrorMode +{ + XEM_Ignore = 0x0000 //!< Ignore all errors without warning + , XEM_IncreasePacketCounter = 0x0001 //!< Increase packet counter when an error occurs, resulting in gaps in the packet counter + , XEM_IncreasePacketCounterAndSendError = 0x0002 //!< Increase packet counter when an error occurs, resulting in gaps in the packet counter and send an explicit error message + , XEM_SendErrorAndGoToConfig = 0x0003 //!< Abort measuring and send an error message + , XEM_Invalid //!< No error mode available }; /*! @} */ typedef enum XsErrorMode XsErrorMode; diff --git a/lib/xspublic/xscontroller/xsfloatformat.h b/lib/xspublic/xscontroller/xsfloatformat.h index f593e54..e3ea574 100644 --- a/lib/xspublic/xscontroller/xsfloatformat.h +++ b/lib/xspublic/xscontroller/xsfloatformat.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -69,7 +69,8 @@ @{ */ /*! \brief Legacy floating point / fixed point format options */ -enum XsFloatFormat { +enum XsFloatFormat +{ FF_IEEE754Float, //!< Single precision floating point (IEEE 754) FF_FixedPoint1220, //!< Fixed point 12.20 format (12 signed bits before decimal, 20 after) FF_FixedPoint1632 //!< Fixed point 16.32 format (16 signed bits before decimal, 32 after) diff --git a/lib/xspublic/xscontroller/xsgnssplatform.h b/lib/xspublic/xscontroller/xsgnssplatform.h index 5dad3ee..a4c8097 100644 --- a/lib/xspublic/xscontroller/xsgnssplatform.h +++ b/lib/xspublic/xscontroller/xsgnssplatform.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xscontroller/xsgnssstatus.h b/lib/xspublic/xscontroller/xsgnssstatus.h new file mode 100644 index 0000000..c2d5acc --- /dev/null +++ b/lib/xspublic/xscontroller/xsgnssstatus.h @@ -0,0 +1,149 @@ + +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions, and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions, and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the names of the copyright holders nor the names of their contributors +// may be used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY +// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL +// THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT +// OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY OR +// TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.THE LAWS OF THE NETHERLANDS +// SHALL BE EXCLUSIVELY APPLICABLE AND ANY DISPUTES SHALL BE FINALLY SETTLED UNDER THE RULES +// OF ARBITRATION OF THE INTERNATIONAL CHAMBER OF COMMERCE IN THE HAGUE BY ONE OR MORE +// ARBITRATORS APPOINTED IN ACCORDANCE WITH SAID RULES. +// + + +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions, and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions, and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the names of the copyright holders nor the names of their contributors +// may be used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY +// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL +// THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT +// OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY OR +// TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.THE LAWS OF THE NETHERLANDS +// SHALL BE EXCLUSIVELY APPLICABLE AND ANY DISPUTES SHALL BE FINALLY SETTLED UNDER THE RULES +// OF ARBITRATION OF THE INTERNATIONAL CHAMBER OF COMMERCE IN THE HAGUE BY ONE OR MORE +// ARBITRATORS APPOINTED IN ACCORDANCE WITH SAID RULES. +// + +#ifndef XSGNSSSTATUS_H +#define XSGNSSSTATUS_H + +#include + +/*! \addtogroup enums Global enumerations + @{ +*/ +/*! \brief GNSS status flags + \details The Flags contain bits the indicate the operational status of the GNSS module +*/ +enum XsGnssStatusFlag +{ + XGSF_GnssReceiverInitialized = 0x01 //! GNSS receiver has been initialized succesfully + , XGSF_GnssReceiverDataOk = 0x02 //! GNSS receiver has correctly received Data + , XGSF_AntennaDetectionMask = 0x70 //! 3-Bit field indicating the status of the GNSS antenna +}; + +/*! \brief GNSS status flags offsets + \details Sometimes (rarely) it is necessary to know the bit offset instead of the bit mask (ie when + shifting to only keep a subset of flags) for the status flags. This enumeration provides these + offsets. + \sa XsGnssStatusFlag +*/ +enum XsGnssStatusFlagOffset +{ + XGSFO_OffsetGnssReceiverInitialized = 0 + , XGSFO_OffsetGnssReceiverDataOk = 1 + , XGSFO_OffsetAntennaDetection = 4 +}; + +/*! \brief Contains flags indicating the status of the GNSS antenna +*/ +enum XsGnssAntennaStatus +{ + XGAS_OpenCircuit = 0, //! No antenna is connected to the antenne port + XGAS_ShortCircuit = 1, //! The antennaconnector seems to be shorted with Ground + XGAS_OK = 2, //! Everything seems ok, antenna connected + XGAS_DontKnow = 3, //! Unknown Status + XGAS_Requesting = 4, //! Busy requesting the current antenna status from the GNSS receiver + XGAS_Init = 5 //! GNSS receiver is initalizing, requesting antenna status not available yet +}; +/*! @} */ +typedef enum XsGnssStatusFlag XsGnssStatusFlag; +typedef enum XsGnssStatusFlagOffset XsGnssStatusFlagOffset; +typedef enum XsGnssAntennaStatus XsGnssAntennaStatus; + +/*! \brief Contains flags indicating the opperational status of the GNSS module +*/ +struct XsGnssStatus +{ + uint8_t m_flags; //!< Flags that specify which tests have passed + +#ifdef __cplusplus + //! \brief Create a new %XsGnssStatus from supplied flags + static inline XsGnssStatus create(uint8_t resultFlags) + { + XsGnssStatus tmp = { resultFlags }; + return tmp; + } + + //! \brief Returns whether the gnss Receiver is initialized correctly + inline bool gnssReceiverInitialized() const + { + return (m_flags & XGSF_GnssReceiverInitialized) != 0; + } + + //! \brief Returns whether the the data from the GNSS receiver is OK + inline bool gnssReceiverDataOk() const + { + return (m_flags & XGSF_GnssReceiverDataOk) != 0; + } + + //! \brief Returns the antenna status + XsGnssAntennaStatus gnssAntennaStatus() const + { + return ((XsGnssAntennaStatus)((m_flags & XGSF_AntennaDetectionMask) >> XGSFO_OffsetAntennaDetection)); + } + +#endif +}; + +typedef struct XsGnssStatus XsGnssStatus; + +#endif diff --git a/lib/xspublic/xscontroller/xsicccommand.h b/lib/xspublic/xscontroller/xsicccommand.h index d19b082..2bdf1ea 100644 --- a/lib/xspublic/xscontroller/xsicccommand.h +++ b/lib/xspublic/xscontroller/xsicccommand.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xscontroller/xsiccrepmotionresult.h b/lib/xspublic/xscontroller/xsiccrepmotionresult.h index 67ae7be..cbb07df 100644 --- a/lib/xspublic/xscontroller/xsiccrepmotionresult.h +++ b/lib/xspublic/xscontroller/xsiccrepmotionresult.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xscontroller/xslibusb.cpp b/lib/xspublic/xscontroller/xslibusb.cpp index 1b5f5d0..5d5afa2 100644 --- a/lib/xspublic/xscontroller/xslibusb.cpp +++ b/lib/xspublic/xscontroller/xslibusb.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -134,7 +134,7 @@ void XsLibUsb::initLibrary() Only valid on return code 0. \returns 0 on success, or a LIBUSB_ERROR code on failure */ -int XsLibUsb::init(libusb_context **ctx) +int XsLibUsb::init(libusb_context** ctx) { if (m_libUsb.init) return m_libUsb.init(ctx); @@ -145,7 +145,7 @@ int XsLibUsb::init(libusb_context **ctx) /*! \brief Deinitialize libusb. Should be called after closing all open devices and before your application terminates. \param ctx the context to deinitialize, or NULL for the default context */ -void XsLibUsb::exit(libusb_context *ctx) +void XsLibUsb::exit(libusb_context* ctx) { if (m_libUsb.exit) m_libUsb.exit(ctx); @@ -167,7 +167,7 @@ void XsLibUsb::exit(libusb_context *ctx) \returns LIBUSB_ERROR_NO_DEVICE if the device has been disconnected \returns another LIBUSB_ERROR code on other failure */ -int XsLibUsb::open(libusb_device *dev, libusb_device_handle **handle) +int XsLibUsb::open(libusb_device* dev, libusb_device_handle** handle) { if (m_libUsb.open) return m_libUsb.open(dev, handle); @@ -181,7 +181,7 @@ int XsLibUsb::open(libusb_device *dev, libusb_device_handle **handle) This is a non-blocking function; no requests are sent over the bus. \param dev_handle the handle to close */ -void XsLibUsb::close(libusb_device_handle *dev_handle) +void XsLibUsb::close(libusb_device_handle* dev_handle) { if (m_libUsb.close) m_libUsb.close(dev_handle); @@ -202,7 +202,7 @@ void XsLibUsb::close(libusb_device_handle *dev_handle) \returns another LIBUSB_ERROR code on other failure \see libusb_detach_kernel_driver() */ -int XsLibUsb::kernel_driver_active(libusb_device_handle *dev,int interface_number) +int XsLibUsb::kernel_driver_active(libusb_device_handle* dev, int interface_number) { if (m_libUsb.kernel_driver_active) return m_libUsb.kernel_driver_active(dev, interface_number); @@ -227,7 +227,7 @@ int XsLibUsb::kernel_driver_active(libusb_device_handle *dev,int interface_numbe \returns another LIBUSB_ERROR code on other failure \see libusb_kernel_driver_active() */ -int XsLibUsb::attach_kernel_driver(libusb_device_handle *dev,int interface_number) +int XsLibUsb::attach_kernel_driver(libusb_device_handle* dev, int interface_number) { if (m_libUsb.attach_kernel_driver) return m_libUsb.attach_kernel_driver(dev, interface_number); @@ -250,7 +250,7 @@ int XsLibUsb::attach_kernel_driver(libusb_device_handle *dev,int interface_numbe \returns another LIBUSB_ERROR code on other failure \see libusb_kernel_driver_active() */ -int XsLibUsb::detach_kernel_driver(libusb_device_handle *dev,int interface_number) +int XsLibUsb::detach_kernel_driver(libusb_device_handle* dev, int interface_number) { if (m_libUsb.detach_kernel_driver) return m_libUsb.detach_kernel_driver(dev, interface_number); @@ -262,7 +262,7 @@ int XsLibUsb::detach_kernel_driver(libusb_device_handle *dev,int interface_numbe \param dev the device to reference \returns the same device */ -libusb_device * XsLibUsb::ref_device(libusb_device *dev) +libusb_device* XsLibUsb::ref_device(libusb_device* dev) { if (m_libUsb.ref_device) return m_libUsb.ref_device(dev); @@ -274,7 +274,7 @@ libusb_device * XsLibUsb::ref_device(libusb_device *dev) If the decrement operation causes the reference count to reach zero, the device shall be destroyed. \param dev the device to unreference */ -void XsLibUsb::unref_device(libusb_device *dev) +void XsLibUsb::unref_device(libusb_device* dev) { if (m_libUsb.unref_device) m_libUsb.unref_device(dev); @@ -301,7 +301,7 @@ void XsLibUsb::unref_device(libusb_device *dev) \returns LIBUSB_ERROR_NO_DEVICE if the device has been disconnected \returns a LIBUSB_ERROR code on other failure */ -int XsLibUsb::claim_interface(libusb_device_handle *dev,int interface_number) +int XsLibUsb::claim_interface(libusb_device_handle* dev, int interface_number) { if (m_libUsb.claim_interface) return m_libUsb.claim_interface(dev, interface_number); @@ -322,7 +322,7 @@ int XsLibUsb::claim_interface(libusb_device_handle *dev,int interface_number) \returns LIBUSB_ERROR_NO_DEVICE if the device has been disconnected \returns another LIBUSB_ERROR code on other failure */ -int XsLibUsb::release_interface(libusb_device_handle *dev, int interface_number) +int XsLibUsb::release_interface(libusb_device_handle* dev, int interface_number) { if (m_libUsb.release_interface) return m_libUsb.release_interface(dev, interface_number); @@ -343,7 +343,7 @@ int XsLibUsb::release_interface(libusb_device_handle *dev, int interface_number) \returns another LIBUSB_ERROR code on error \see libusb_get_config_descriptor */ -int XsLibUsb::get_active_config_descriptor(libusb_device *dev, struct libusb_config_descriptor **config) +int XsLibUsb::get_active_config_descriptor(libusb_device* dev, struct libusb_config_descriptor** config) { if (m_libUsb.get_active_config_descriptor) return m_libUsb.get_active_config_descriptor(dev, config); @@ -357,7 +357,7 @@ int XsLibUsb::get_active_config_descriptor(libusb_device *dev, struct libusb_con \param config the configuration descriptor to free */ -void XsLibUsb::free_config_descriptor(struct libusb_config_descriptor *config) +void XsLibUsb::free_config_descriptor(struct libusb_config_descriptor* config) { if (m_libUsb.free_config_descriptor) m_libUsb.free_config_descriptor(config); @@ -367,7 +367,7 @@ void XsLibUsb::free_config_descriptor(struct libusb_config_descriptor *config) \param dev a device \returns the bus number */ -uint8_t XsLibUsb::get_bus_number(libusb_device *dev) +uint8_t XsLibUsb::get_bus_number(libusb_device* dev) { if (m_libUsb.get_bus_number) return m_libUsb.get_bus_number(dev); @@ -382,7 +382,7 @@ uint8_t XsLibUsb::get_bus_number(libusb_device *dev) \param dev_handle a device handle \returns the underlying device */ -libusb_device * XsLibUsb::get_device(libusb_device_handle *dev_handle) +libusb_device* XsLibUsb::get_device(libusb_device_handle* dev_handle) { if (m_libUsb.get_device) return m_libUsb.get_device(dev_handle); @@ -394,7 +394,7 @@ libusb_device * XsLibUsb::get_device(libusb_device_handle *dev_handle) \param dev a device \returns the device address */ -uint8_t XsLibUsb::get_device_address(libusb_device *dev) +uint8_t XsLibUsb::get_device_address(libusb_device* dev) { if (m_libUsb.get_device_address) return m_libUsb.get_device_address(dev); @@ -410,7 +410,7 @@ uint8_t XsLibUsb::get_device_address(libusb_device *dev) \param desc output location for the descriptor data \returns 0 on success or a LIBUSB_ERROR code on failure */ -int XsLibUsb::get_device_descriptor(libusb_device *dev, struct libusb_device_descriptor *desc) +int XsLibUsb::get_device_descriptor(libusb_device* dev, struct libusb_device_descriptor* desc) { if (m_libUsb.get_device_descriptor) return m_libUsb.get_device_descriptor(dev, desc); @@ -435,7 +435,7 @@ int XsLibUsb::get_device_descriptor(libusb_device *dev, struct libusb_device_des \param list output location for a list of devices. Must be later freed with libusb_free_device_list(). \returns The number of devices in the outputted list, or any LIBUSB_ERROR code to errors encountered by the backend. */ -ssize_t XsLibUsb::get_device_list(libusb_context *ctx, libusb_device ***list) +ssize_t XsLibUsb::get_device_list(libusb_context* ctx, libusb_device*** list) { if (m_libUsb.get_device_list) return m_libUsb.get_device_list(ctx, list); @@ -448,7 +448,7 @@ ssize_t XsLibUsb::get_device_list(libusb_context *ctx, libusb_device ***list) \param list the list to free \param unref_devices whether to unref the devices in the list */ -void XsLibUsb::free_device_list(libusb_device **list, int unref_devices) +void XsLibUsb::free_device_list(libusb_device** list, int unref_devices) { if (m_libUsb.free_device_list) m_libUsb.free_device_list(list, unref_devices); @@ -464,7 +464,7 @@ void XsLibUsb::free_device_list(libusb_device **list, int unref_devices) \param length size of data buffer \returns number of bytes returned in data, or LIBUSB_ERROR code on failure */ -int XsLibUsb::get_string_descriptor_ascii(libusb_device_handle *dev, uint8_t desc_index, unsigned char *data, int length) +int XsLibUsb::get_string_descriptor_ascii(libusb_device_handle* dev, uint8_t desc_index, unsigned char* data, int length) { if (m_libUsb.get_string_descriptor_ascii) return m_libUsb.get_string_descriptor_ascii(dev, desc_index, data, length); @@ -503,7 +503,7 @@ int XsLibUsb::get_string_descriptor_ascii(libusb_device_handle *dev, uint8_t des \returns LIBUSB_ERROR_NO_DEVICE if the device has been disconnected \returns another LIBUSB_ERROR code on other failures */ -int XsLibUsb::bulk_transfer(libusb_device_handle *dev_handle, unsigned char endpoint, unsigned char *data, int length, int *actual_length, unsigned int timeout) +int XsLibUsb::bulk_transfer(libusb_device_handle* dev_handle, unsigned char endpoint, unsigned char* data, int length, int* actual_length, unsigned int timeout) { if (m_libUsb.bulk_transfer) return m_libUsb.bulk_transfer(dev_handle, endpoint, data, length, actual_length, timeout); @@ -538,7 +538,7 @@ int XsLibUsb::bulk_transfer(libusb_device_handle *dev_handle, unsigned char endp \param ctx the context to operate on, or NULL for the default context \param level debug level to set */ -void XsLibUsb::set_debug(libusb_context *ctx, int level) +void XsLibUsb::set_debug(libusb_context* ctx, int level) { if (m_libUsb.set_debug) m_libUsb.set_debug(ctx, level); diff --git a/lib/xspublic/xscontroller/xslibusb.h b/lib/xspublic/xscontroller/xslibusb.h index d73450f..e07451e 100644 --- a/lib/xspublic/xscontroller/xslibusb.h +++ b/lib/xspublic/xscontroller/xslibusb.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -69,28 +69,28 @@ #include -typedef int libUSB_init(libusb_context **ctx); -typedef void libUSB_exit(libusb_context *ctx); -typedef int libUSB_open(libusb_device *dev, libusb_device_handle **handle); -typedef void libUSB_close(libusb_device_handle *dev_handle); -typedef int libUSB_kernel_driver_active(libusb_device_handle *dev,int interface_number); -typedef int libUSB_attach_kernel_driver(libusb_device_handle *dev,int interface_number); -typedef int libUSB_detach_kernel_driver(libusb_device_handle *dev,int interface_number); -typedef libusb_device * libUSB_ref_device(libusb_device *dev); -typedef void libUSB_unref_device(libusb_device *dev); -typedef int libUSB_claim_interface(libusb_device_handle *dev,int interface_number); -typedef int libUSB_release_interface(libusb_device_handle *dev, int interface_number); -typedef int libUSB_get_active_config_descriptor(libusb_device *dev, struct libusb_config_descriptor **config); -typedef void libUSB_free_config_descriptor(struct libusb_config_descriptor *config); -typedef uint8_t libUSB_get_bus_number(libusb_device *dev); -typedef libusb_device * libUSB_get_device(libusb_device_handle *dev_handle); -typedef uint8_t libUSB_get_device_address(libusb_device *dev); -typedef int libUSB_get_device_descriptor(libusb_device *dev, struct libusb_device_descriptor *desc); -typedef ssize_t libUSB_get_device_list(libusb_context *ctx, libusb_device ***list); -typedef void libUSB_free_device_list(libusb_device **list, int unref_devices); -typedef int libUSB_get_string_descriptor_ascii(libusb_device_handle *dev, uint8_t desc_index, unsigned char *data, int length); -typedef int libUSB_bulk_transfer(libusb_device_handle *dev_handle, unsigned char endpoint, unsigned char *data, int length, int *actual_length, unsigned int timeout); -typedef void libUSB_set_debug(libusb_context *ctx, int level); +typedef int libUSB_init(libusb_context** ctx); +typedef void libUSB_exit(libusb_context* ctx); +typedef int libUSB_open(libusb_device* dev, libusb_device_handle** handle); +typedef void libUSB_close(libusb_device_handle* dev_handle); +typedef int libUSB_kernel_driver_active(libusb_device_handle* dev, int interface_number); +typedef int libUSB_attach_kernel_driver(libusb_device_handle* dev, int interface_number); +typedef int libUSB_detach_kernel_driver(libusb_device_handle* dev, int interface_number); +typedef libusb_device* libUSB_ref_device(libusb_device* dev); +typedef void libUSB_unref_device(libusb_device* dev); +typedef int libUSB_claim_interface(libusb_device_handle* dev, int interface_number); +typedef int libUSB_release_interface(libusb_device_handle* dev, int interface_number); +typedef int libUSB_get_active_config_descriptor(libusb_device* dev, struct libusb_config_descriptor** config); +typedef void libUSB_free_config_descriptor(struct libusb_config_descriptor* config); +typedef uint8_t libUSB_get_bus_number(libusb_device* dev); +typedef libusb_device* libUSB_get_device(libusb_device_handle* dev_handle); +typedef uint8_t libUSB_get_device_address(libusb_device* dev); +typedef int libUSB_get_device_descriptor(libusb_device* dev, struct libusb_device_descriptor* desc); +typedef ssize_t libUSB_get_device_list(libusb_context* ctx, libusb_device*** list); +typedef void libUSB_free_device_list(libusb_device** list, int unref_devices); +typedef int libUSB_get_string_descriptor_ascii(libusb_device_handle* dev, uint8_t desc_index, unsigned char* data, int length); +typedef int libUSB_bulk_transfer(libusb_device_handle* dev_handle, unsigned char endpoint, unsigned char* data, int length, int* actual_length, unsigned int timeout); +typedef void libUSB_set_debug(libusb_context* ctx, int level); struct XsLibraryLoader; @@ -126,28 +126,28 @@ class XsLibUsb struct LIBUSB_API { - libUSB_init (* init); - libUSB_exit (* exit); - libUSB_open (* open); - libUSB_close (* close); - libUSB_kernel_driver_active (* kernel_driver_active); - libUSB_attach_kernel_driver (* attach_kernel_driver); - libUSB_detach_kernel_driver (* detach_kernel_driver); - libUSB_ref_device (* ref_device); - libUSB_unref_device (* unref_device); - libUSB_claim_interface (* claim_interface); - libUSB_release_interface (* release_interface); - libUSB_get_active_config_descriptor (* get_active_config_descriptor); - libUSB_free_config_descriptor (* free_config_descriptor); - libUSB_get_bus_number (* get_bus_number); - libUSB_get_device (* get_device); - libUSB_get_device_address (* get_device_address); - libUSB_get_device_descriptor (* get_device_descriptor); - libUSB_get_device_list (* get_device_list); - libUSB_free_device_list (* free_device_list); - libUSB_get_string_descriptor_ascii (* get_string_descriptor_ascii); - libUSB_bulk_transfer (* bulk_transfer); - libUSB_set_debug (* set_debug); + libUSB_init* init; + libUSB_exit* exit; + libUSB_open* open; + libUSB_close* close; + libUSB_kernel_driver_active* kernel_driver_active; + libUSB_attach_kernel_driver* attach_kernel_driver; + libUSB_detach_kernel_driver* detach_kernel_driver; + libUSB_ref_device* ref_device; + libUSB_unref_device* unref_device; + libUSB_claim_interface* claim_interface; + libUSB_release_interface* release_interface; + libUSB_get_active_config_descriptor* get_active_config_descriptor; + libUSB_free_config_descriptor* free_config_descriptor; + libUSB_get_bus_number* get_bus_number; + libUSB_get_device* get_device; + libUSB_get_device_address* get_device_address; + libUSB_get_device_descriptor* get_device_descriptor; + libUSB_get_device_list* get_device_list; + libUSB_free_device_list* free_device_list; + libUSB_get_string_descriptor_ascii* get_string_descriptor_ascii; + libUSB_bulk_transfer* bulk_transfer; + libUSB_set_debug* set_debug; } m_libUsb; XsLibraryLoader* m_libraryLoader; @@ -157,6 +157,4 @@ class XsLibUsb }; #endif // HAVE_LIBUSB - -#endif // file guard - +#endif diff --git a/lib/xspublic/xscontroller/xsoperationalmode.h b/lib/xspublic/xscontroller/xsoperationalmode.h index dcfd509..8965723 100644 --- a/lib/xspublic/xscontroller/xsoperationalmode.h +++ b/lib/xspublic/xscontroller/xsoperationalmode.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xscontroller/xsorientationmode.h b/lib/xspublic/xscontroller/xsorientationmode.h index 7fa1852..6a18288 100644 --- a/lib/xspublic/xscontroller/xsorientationmode.h +++ b/lib/xspublic/xscontroller/xsorientationmode.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -70,7 +70,8 @@ */ /*! \brief Legacy orientation output mode selection */ -enum XsOrientationMode { +enum XsOrientationMode +{ OM_None, OM_Euler, OM_Quaternion, diff --git a/lib/xspublic/xscontroller/xsprocessingflag.h b/lib/xspublic/xscontroller/xsprocessingflag.h index 42cd0b0..5b7e6a8 100644 --- a/lib/xspublic/xscontroller/xsprocessingflag.h +++ b/lib/xspublic/xscontroller/xsprocessingflag.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -68,10 +68,10 @@ /*! Used to enable or disable some data processing options \sa XsDevice::setInitialBiasUpdateEnabled */ -enum XsProcessingFlag { +enum XsProcessingFlag +{ XPF_EnableInitialGyroBiasUpdate = 0x0001, XPF_FixedGravity = 0x0002 }; - #endif diff --git a/lib/xspublic/xscontroller/xsprotocoltype.h b/lib/xspublic/xscontroller/xsprotocoltype.h index 4a3df76..3f3b401 100644 --- a/lib/xspublic/xscontroller/xsprotocoltype.h +++ b/lib/xspublic/xscontroller/xsprotocoltype.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -69,11 +69,13 @@ @{ */ //! Protocol types (XsDevice::enableProtocol()) -enum XsProtocolType { - XPT_Xbus = 0 //!< The Xsens Xbus protocol, enabled by default, always 0 - ,XPT_Nmea //!< The NMEA protocol, only the messages that can be sent from Xsens devices are recognized - ,XPT_ImarFsas //!< An iMAR FSAS communication protocol - ,XPT_ImarIfog //!< An iMAR iFog communication protocol +enum XsProtocolType +{ + XPT_Xbus = 0 //!< The Xsens Xbus protocol, enabled by default, always 0 + , XPT_Nmea //!< The NMEA protocol, only the messages that can be sent from Xsens devices are recognized + , XPT_ImarFsas //!< An iMAR FSAS communication protocol + , XPT_ImarIfog //!< An iMAR iFog communication protocol + , XPT_ImarIfogUart //!< An iMAR iFog UART communication protocol }; /*! @} */ typedef enum XsProtocolType XsProtocolType; diff --git a/lib/xspublic/xscontroller/xsrejectreason.c b/lib/xspublic/xscontroller/xsrejectreason.c index f70bbf6..19bd348 100644 --- a/lib/xspublic/xscontroller/xsrejectreason.c +++ b/lib/xspublic/xscontroller/xsrejectreason.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -70,17 +70,17 @@ */ /*! \brief Convert the %XsRejectReason to a human readable string */ -const char *XsRejectReason_toString(XsRejectReason r) +const char* XsRejectReason_toString(XsRejectReason r) { switch (r) { - XS_ENUM_TO_STR_CASE(XRR_Unknown); - XS_ENUM_TO_STR_CASE(XRR_VersionMismatch); - XS_ENUM_TO_STR_CASE(XRR_Blacklisted); - XS_ENUM_TO_STR_CASE(XRR_StationIsDisconnecting); - XS_ENUM_TO_STR_CASE(XRR_SystemIsOperational); - default: - break; + XS_ENUM_TO_STR_CASE(XRR_Unknown); + XS_ENUM_TO_STR_CASE(XRR_VersionMismatch); + XS_ENUM_TO_STR_CASE(XRR_Blacklisted); + XS_ENUM_TO_STR_CASE(XRR_StationIsDisconnecting); + XS_ENUM_TO_STR_CASE(XRR_SystemIsOperational); + default: + break; } return "UnknownReason"; } diff --git a/lib/xspublic/xscontroller/xsrejectreason.h b/lib/xspublic/xscontroller/xsrejectreason.h index 4ca6823..2368fdd 100644 --- a/lib/xspublic/xscontroller/xsrejectreason.h +++ b/lib/xspublic/xscontroller/xsrejectreason.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -86,13 +86,13 @@ typedef enum XsRejectReason XsRejectReason; extern "C" { #endif -XDA_DLL_API const char *XsRejectReason_toString(XsRejectReason r); +XDA_DLL_API const char* XsRejectReason_toString(XsRejectReason r); #ifdef __cplusplus } // extern "C" /*! \brief \copybrief XsRejectReason_toString \sa XsRejectReason_toString */ -inline const char *toString(XsRejectReason r) +inline const char* toString(XsRejectReason r) { return XsRejectReason_toString(r); } diff --git a/lib/xspublic/xscontroller/xsscanner.cpp b/lib/xspublic/xscontroller/xsscanner.cpp index 9b0b503..d72bb74 100644 --- a/lib/xspublic/xscontroller/xsscanner.cpp +++ b/lib/xspublic/xscontroller/xsscanner.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -123,138 +123,152 @@ void XsScanner_enumerateSerialPorts_int(XsPortInfoArray* ports, int ignoreNonXse extern "C" { -/*! \brief Set a callback function for scan log progress and problem reporting - \details When set, any scan will use the provided callback function to report progress and failures. - Normal operation is not affected, so all return values for the scan functions remain valid. - \param cb The callback function to use. When set to NULL, no callbacks will be generated. -*/ -void XsScanner_setScanLogCallback(XsScanLogCallbackFunc cb) -{ - Scanner::setScanLogCallback(cb); -} + /*! \brief Set a callback function for scan log progress and problem reporting + \details When set, any scan will use the provided callback function to report progress and failures. + Normal operation is not affected, so all return values for the scan functions remain valid. + \param cb The callback function to use. When set to NULL, no callbacks will be generated. + */ + void XsScanner_setScanLogCallback(XsScanLogCallbackFunc cb) + { + Scanner::setScanLogCallback(cb); + } -/*! \relates XsScanner - \brief Scan all ports for Xsens devices. - \param[out] ports The list of detected ports. - \param[in] baudrate The baudrate to scan at. When set to XBR_Invalid, all known baudrates are scanned. - \param[in] singleScanTimeout The timeout of a scan of a single port at a single baud rate in ms. - \param[in] ignoreNonXsensDevices When non-zero (the default), only Xsens devices are returned. Otherwise other devices that comply with the Xsens message protocol will also be returned. - \param[in] detectRs485 Enable more extended scan to detect rs485 devices -*/ -void XsScanner_scanPorts(XsPortInfoArray* ports, XsBaudRate baudrate, int singleScanTimeout, int ignoreNonXsensDevices, int detectRs485) -{ - LOGXSSCAN(__FUNCTION__ << " baudrate " << baudrate << " singleScanTimeout " << singleScanTimeout << " ignoreNonXsensDevices " << ignoreNonXsensDevices << " detectRs485 " << detectRs485); - XsScanner_scanPorts_int(ports, baudrate, singleScanTimeout, ignoreNonXsensDevices, detectRs485); -} + /*! \relates XsScanner + \brief Scan all ports for Xsens devices. + \param[out] ports The list of detected ports. + \param[in] baudrate The baudrate to scan at. When set to XBR_Invalid, all known baudrates are scanned. + \param[in] singleScanTimeout The timeout of a scan of a single port at a single baud rate in ms. + \param[in] ignoreNonXsensDevices When non-zero (the default), only Xsens devices are returned. Otherwise other devices that comply with the Xsens message protocol will also be returned. + \param[in] detectRs485 Enable more extended scan to detect rs485 devices + */ + void XsScanner_scanPorts(XsPortInfoArray* ports, XsBaudRate baudrate, int singleScanTimeout, int ignoreNonXsensDevices, int detectRs485) + { + LOGXSSCAN(__FUNCTION__ << " baudrate " << baudrate << " singleScanTimeout " << singleScanTimeout << " ignoreNonXsensDevices " << ignoreNonXsensDevices << " detectRs485 " << detectRs485); + XsScanner_scanPorts_int(ports, baudrate, singleScanTimeout, ignoreNonXsensDevices, detectRs485); + } -/*! \relates XsScanner - \brief Scan a single port for Xsens devices. - \param[in,out] port The name of the port to scan should be in this parameter, the other contents will be filled by the function. - \param[in] baudrate The baudrate to scan at. When set to XBR_Invalid, all known baudrates are scanned. - \param[in] singleScanTimeout The timeout of a scan at a single baud rate in ms. - \param[in] detectRs485 Enable more extended scan to detect rs485 devices - \returns true if a device was found, false otherwise -*/ -int XsScanner_scanPort(XsPortInfo* port, XsBaudRate baudrate, int singleScanTimeout, int detectRs485) -{ - LOGXSSCAN(__FUNCTION__ << " baudrate " << baudrate << " singleScanTimeout " << singleScanTimeout << " detectRs485 " << detectRs485); - return XsScanner_scanPort_int(port, baudrate, singleScanTimeout, detectRs485); -} + /*! \relates XsScanner + \brief Scan a single port for Xsens devices. + \param[in,out] port The name of the port to scan should be in this parameter, the other contents will be filled by the function. + \param[in] baudrate The baudrate to scan at. When set to XBR_Invalid, all known baudrates are scanned. + \param[in] singleScanTimeout The timeout of a scan at a single baud rate in ms. + \param[in] detectRs485 Enable more extended scan to detect rs485 devices + \returns true if a device was found, false otherwise + */ + int XsScanner_scanPort(XsPortInfo* port, XsBaudRate baudrate, int singleScanTimeout, int detectRs485) + { + LOGXSSCAN(__FUNCTION__ << " baudrate " << baudrate << " singleScanTimeout " << singleScanTimeout << " detectRs485 " << detectRs485); + return XsScanner_scanPort_int(port, baudrate, singleScanTimeout, detectRs485); + } -/*! \copydoc XsScanner_enumerateSerialPorts_int */ -void XsScanner_enumerateSerialPorts(XsPortInfoArray* ports, int ignoreNonXsensDevices) -{ - LOGXSSCAN(__FUNCTION__ << " ignoreNonXsensDevices " << ignoreNonXsensDevices); - XsScanner_enumerateSerialPorts_int(ports, ignoreNonXsensDevices); -} + /*! \copydoc XsScanner_enumerateSerialPorts_int */ + void XsScanner_enumerateSerialPorts(XsPortInfoArray* ports, int ignoreNonXsensDevices) + { + LOGXSSCAN(__FUNCTION__ << " ignoreNonXsensDevices " << ignoreNonXsensDevices); + XsScanner_enumerateSerialPorts_int(ports, ignoreNonXsensDevices); + } -/*! \relates XsScanner - \brief Scan the supplied ports for Xsens devices. - \param[in,out] ports The list of ports to scan. Unresponsive devices will be removed from the list. - \param[in] baudrate The baudrate to scan at. When set to XBR_Invalid, all known baudrates are scanned. - \param[in] singleScanTimeout The timeout of a scan of a single port at a single baud rate in ms. - \param[in] detectRs485 Enable more extended scan to detect rs485 devices -*/ -void XsScanner_filterResponsiveDevices(XsPortInfoArray* ports, XsBaudRate baudrate, int singleScanTimeout, int detectRs485) -{ - LOGXSSCAN(__FUNCTION__ << " baudrate " << baudrate << " singleScanTimeout " << singleScanTimeout << " detectRs485 " << detectRs485); - assert(ports != nullptr); - if (!ports) - return; + /*! \relates XsScanner + \brief Scan the supplied ports for Xsens devices. + \param[in,out] ports The list of ports to scan. Unresponsive devices will be removed from the list. + \param[in] baudrate The baudrate to scan at. When set to XBR_Invalid, all known baudrates are scanned. + \param[in] singleScanTimeout The timeout of a scan of a single port at a single baud rate in ms. + \param[in] detectRs485 Enable more extended scan to detect rs485 devices + */ + void XsScanner_filterResponsiveDevices(XsPortInfoArray* ports, XsBaudRate baudrate, int singleScanTimeout, int detectRs485) + { + LOGXSSCAN(__FUNCTION__ << " baudrate " << baudrate << " singleScanTimeout " << singleScanTimeout << " detectRs485 " << detectRs485); + assert(ports != nullptr); + if (!ports) + return; - XsPortInfoArray tmp; - for (XsSize i = 0; i < ports->size(); ++i) - tmp.push_back(ports->at(i)); - Scanner::Accessor accessor; - if (accessor.scanner().xsFilterResponsiveDevices(tmp, baudrate, (uint32_t) singleScanTimeout, detectRs485 != 0)) + XsPortInfoArray tmp; + for (XsSize i = 0; i < ports->size(); ++i) + tmp.push_back(ports->at(i)); + Scanner::Accessor accessor; + if (accessor.scanner().xsFilterResponsiveDevices(tmp, baudrate, (uint32_t) singleScanTimeout, detectRs485 != 0)) + { + if (tmp.size()) + { + ports->assign(tmp.size(), &tmp[0]); + return; + } + } + ports->clear(); + } + + /*! \relates XsScanner + \brief List all compatible USB ports without scanning. + \param[out] ports The list of detected ports. + */ + void XsScanner_enumerateUsbDevices(XsPortInfoArray* ports) { + LOGXSSCAN(__FUNCTION__); + + assert(ports != nullptr); + if (!ports) + return; + + XsPortInfoArray tmp; + xsEnumerateUsbDevices(tmp); if (tmp.size()) - { ports->assign(tmp.size(), &tmp[0]); - return; - } + else + ports->clear(); } - ports->clear(); -} -/*! \relates XsScanner - \brief List all compatible USB ports without scanning. - \param[out] ports The list of detected ports. -*/ -void XsScanner_enumerateUsbDevices(XsPortInfoArray* ports) -{ - LOGXSSCAN(__FUNCTION__); + /*! \relates XsScanner + \brief Determine the USB hub that \a port is attached to + \param[out] hub The identifier of the hub that \a port is attached to. + \param[in] port The port for which to determine the USB hub. + */ + void XsScanner_scanUsbHub(XsUsbHubInfo* hub, const XsPortInfo* port) + { + LOGXSSCAN(__FUNCTION__); - assert(ports != nullptr); - if (!ports) - return; + assert(hub != nullptr && port != nullptr); + if (!hub || !port) + return; - XsPortInfoArray tmp; - xsEnumerateUsbDevices(tmp); - if (tmp.size()) - ports->assign(tmp.size(), &tmp[0]); - else - ports->clear(); -} + Scanner::Accessor accessor; + *hub = accessor.scanner().xsScanUsbHub(*port); + } -/*! \relates XsScanner - \brief Determine the USB hub that \a port is attached to - \param[out] hub The identifier of the hub that \a port is attached to. - \param[in] port The port for which to determine the USB hub. -*/ -void XsScanner_scanUsbHub(XsUsbHubInfo* hub, const XsPortInfo* port) -{ - LOGXSSCAN(__FUNCTION__); + void XsScanner_enumerateNetworkDevices(XsPortInfoArray* ports) + { + LOGXSSCAN(__FUNCTION__); + assert(ports != nullptr); + if (!ports) + return; - assert(hub != nullptr && port != nullptr); - if (!hub || !port) - return; + XsPortInfoArray tmp; - Scanner::Accessor accessor; - *hub = accessor.scanner().xsScanUsbHub(*port); -} + Scanner::Accessor accessor; + accessor.scanner().xsEnumerateNetworkDevices(tmp); + ports->swap(tmp); + } -void XsScanner_enumerateNetworkDevices(XsPortInfoArray* ports) -{ - LOGXSSCAN(__FUNCTION__); - assert(ports != nullptr); - if (!ports) - return; + void XsScanner_enumerateBluetoothDevices(XsPortInfoArray* ports) + { + LOGXSSCAN(__FUNCTION__); + assert(ports != nullptr); + if (!ports) + return; - XsPortInfoArray tmp; + XsPortInfoArray tmp; - Scanner::Accessor accessor; - accessor.scanner().xsEnumerateNetworkDevices(tmp); - ports->swap(tmp); -} + Scanner::Accessor accessor; + accessor.scanner().xsEnumerateBluetoothDevices(tmp); + ports->swap(tmp); + } -/*! \relates XsScanner - \brief Abort the currently running port scan(s) -*/ -void XsScanner_abortScan(void) -{ - LOGXSSCAN(__FUNCTION__); - XsScannerNamespace::abortPortScan = true; -} + /*! \relates XsScanner + \brief Abort the currently running port scan(s) + */ + void XsScanner_abortScan(void) + { + LOGXSSCAN(__FUNCTION__); + XsScannerNamespace::abortPortScan = true; + } } // extern "C" diff --git a/lib/xspublic/xscontroller/xsscanner.h b/lib/xspublic/xscontroller/xsscanner.h index 727cd15..282db9f 100644 --- a/lib/xspublic/xscontroller/xsscanner.h +++ b/lib/xspublic/xscontroller/xsscanner.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -97,13 +97,15 @@ XDA_DLL_API void XsScanner_filterResponsiveDevices(struct XsPortInfoArray* ports XDA_DLL_API void XsScanner_enumerateUsbDevices(struct XsPortInfoArray* ports); XDA_DLL_API void XsScanner_scanUsbHub(struct XsUsbHubInfo* hub, const struct XsPortInfo* port); XDA_DLL_API void XsScanner_enumerateNetworkDevices(struct XsPortInfoArray* ports); +XDA_DLL_API void XsScanner_enumerateBluetoothDevices(struct XsPortInfoArray* ports); XDA_DLL_API void XsScanner_abortScan(void); XDA_DLL_API void XsScanner_setScanLogCallback(XsScanLogCallbackFunc cb); #ifdef __cplusplus } // extern "C" -class XsScanner { +class XsScanner +{ public: /*! \brief Scan all ports for Xsens devices. \param[in] baudrate The baudrate to scan at. When set to XBR_Invalid, all known baudrates are scanned. @@ -116,14 +118,14 @@ class XsScanner { static inline XsPortInfoArray scanPorts(XsBaudRate baudrate = XBR_Invalid, int singleScanTimeout = 100, bool ignoreNonXsensDevices = true, bool detectRs485 = false) { XsPortInfoArray ports; - XsScanner_scanPorts(&ports, baudrate, singleScanTimeout, ignoreNonXsensDevices?1:0, detectRs485?1:0); + XsScanner_scanPorts(&ports, baudrate, singleScanTimeout, ignoreNonXsensDevices ? 1 : 0, detectRs485 ? 1 : 0); return ports; } //! \copydoc XsScanner_scanPort static inline bool XSNOCOMEXPORT scanPort(XsPortInfo& port, XsBaudRate baudrate = XBR_Invalid, int singleScanTimeout = 100, bool detectRs485 = false) { - return 0 != XsScanner_scanPort(&port, baudrate, singleScanTimeout, detectRs485?1:0); + return 0 != XsScanner_scanPort(&port, baudrate, singleScanTimeout, detectRs485 ? 1 : 0); } /*! \brief Scan a single port for Xsens devices. @@ -177,7 +179,7 @@ class XsScanner { static inline XsPortInfoArray enumerateSerialPorts(bool ignoreNonXsensDevices = true) { XsPortInfoArray ports; - XsScanner_enumerateSerialPorts(&ports, ignoreNonXsensDevices?1:0); + XsScanner_enumerateSerialPorts(&ports, ignoreNonXsensDevices ? 1 : 0); return ports; } @@ -193,7 +195,7 @@ class XsScanner { static inline XsPortInfoArray filterResponsiveDevices(const XsPortInfoArray& ports, XsBaudRate baudrate = XBR_Invalid, int singleScanTimeout = 100, bool detectRs485 = false) { XsPortInfoArray filtered(ports); - XsScanner_filterResponsiveDevices(&filtered, baudrate, singleScanTimeout, detectRs485?1:0); + XsScanner_filterResponsiveDevices(&filtered, baudrate, singleScanTimeout, detectRs485 ? 1 : 0); return filtered; } @@ -231,6 +233,17 @@ class XsScanner { return ports; } + /*! \brief List all compatible bluetooth devices without scanning. + \returns The list of detected bluetooth services. + \sa XsScanner_enumerateBluetoothDevices + */ + static inline XsPortInfoArray enumerateBluetoothDevices(void) + { + XsPortInfoArray ports; + XsScanner_enumerateBluetoothDevices(&ports); + return ports; + } + /*! \brief Abort the currently running port scan(s) \sa XsScanner_abortScan */ diff --git a/lib/xspublic/xscontroller/xsselftestresult.h b/lib/xspublic/xscontroller/xsselftestresult.h index c2cbdfb..5381080 100644 --- a/lib/xspublic/xscontroller/xsselftestresult.h +++ b/lib/xspublic/xscontroller/xsselftestresult.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -73,34 +73,37 @@ /*! \brief Enumeration of bits that describe whether the various self-tests succeeded \see XsSelfTestResult */ -enum XsSelfTestFlag { - XSTF_X = 0x01 - ,XSTF_Y = 0x02 - ,XSTF_Z = 0x04 - ,XSTF_AccShift = 0 - ,XSTF_AccX = XSTF_X << XSTF_AccShift - ,XSTF_AccY = XSTF_Y << XSTF_AccShift - ,XSTF_AccZ = XSTF_Z << XSTF_AccShift - ,XSTF_GyrShift = 3 - ,XSTF_GyrX = XSTF_X << XSTF_GyrShift - ,XSTF_GyrY = XSTF_Y << XSTF_GyrShift - ,XSTF_GyrZ = XSTF_Z << XSTF_GyrShift - ,XSTF_MagShift = 6 - ,XSTF_MagX = XSTF_X << XSTF_MagShift - ,XSTF_MagY = XSTF_Y << XSTF_MagShift - ,XSTF_MagZ = XSTF_Z << XSTF_MagShift - ,XSTF_Baro = 1<<9 - ,XSTF_Gnss = 1<<10 - ,XSTF_Battery = 1<<11 - ,XSTF_Flash = 1<<12 - ,XSTF_Button = 1<<13 +enum XsSelfTestFlag +{ + XSTF_X = 0x01 + , XSTF_Y = 0x02 + , XSTF_Z = 0x04 + , XSTF_AccShift = 0 + , XSTF_AccX = XSTF_X << XSTF_AccShift + , XSTF_AccY = XSTF_Y << XSTF_AccShift + , XSTF_AccZ = XSTF_Z << XSTF_AccShift + , XSTF_GyrShift = 3 + , XSTF_GyrX = XSTF_X << XSTF_GyrShift + , XSTF_GyrY = XSTF_Y << XSTF_GyrShift + , XSTF_GyrZ = XSTF_Z << XSTF_GyrShift + , XSTF_MagShift = 6 + , XSTF_MagX = XSTF_X << XSTF_MagShift + , XSTF_MagY = XSTF_Y << XSTF_MagShift + , XSTF_MagZ = XSTF_Z << XSTF_MagShift + , XSTF_Baro = 1 << 9 + , XSTF_Gnss = 1 << 10 + , XSTF_Battery = 1 << 11 + , XSTF_Flash = 1 << 12 + , XSTF_Button = 1 << 13 + , XSTF_Sync = 1 << 14 }; /*! @} */ typedef enum XsSelfTestFlag XsSelfTestFlag; /*! \brief Contains the results of a self-test performed by an Xsens device */ -struct XsSelfTestResult { +struct XsSelfTestResult +{ uint16_t m_flags; //!< Flags that specify which tests have passed #ifdef __cplusplus @@ -166,18 +169,18 @@ struct XsSelfTestResult { } /*! \brief Returns whether the barometer passed (true) or failed (false) its self-test - \details Only valid for MTi-7 + \details Only valid for MTi-7, 600-series \returns True if the baro selftest has passed - */ + */ inline bool baro() const { return (m_flags & XSTF_Baro) != 0; } /*! \brief Returns whether the gnss passed (true) or failed (false) its self-test - \details Only valid for MTi-7 + \details Only valid for MTi-7, MTi-670, MTi-680 \returns True if the gnss selftest has passed - */ + */ inline bool gnss() const { return (m_flags & XSTF_Gnss) != 0; @@ -186,24 +189,42 @@ struct XsSelfTestResult { /*! \brief Returns whether the fuelGauge passed (true) or failed (false) its self-test \details Only valid for DOT \returns True if the fuelGauge selftest has passed - */ + */ inline bool battery() const { return (m_flags & XSTF_Battery) != 0; } /*! \brief Returns whether the flash chip passed (true) or failed (false) its self-test - \details Only valid for DOT + \details Only valid for DOT, 600-series \returns True if the flash selftest has passed - */ + */ inline bool flash() const { return (m_flags & XSTF_Flash) != 0; } + /*! \brief Returns whether the onboard button passed (true) or failed (false) its self-test + \details Only valid for DOT + \returns True if the button selftest has passed + */ + inline bool button() const + { + return (m_flags & XSTF_Button) != 0; + } + + + /*! \brief Returns whether the sync lines passed (true) or failed (false) its self-test + \details Only valid for 600-series + \returns True if the sync lines selftest has passed + */ + inline bool sync() const + { + return (m_flags & XSTF_Sync) != 0; + } #endif }; typedef struct XsSelfTestResult XsSelfTestResult; -#endif // file guard +#endif diff --git a/lib/xspublic/xscontroller/xsusbhubinfo.c b/lib/xspublic/xscontroller/xsusbhubinfo.c index ed2631f..f5b80b6 100644 --- a/lib/xspublic/xscontroller/xsusbhubinfo.c +++ b/lib/xspublic/xscontroller/xsusbhubinfo.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -162,8 +162,8 @@ int XsUsbHubInfo_parentPathMatches(const XsUsbHubInfo* left, const XsUsbHubInfo* otherLastDot = strrchr(right->m_hub, '.'); thisLastDot = strrchr(left->m_hub, '.'); - dotPos = (int) (otherLastDot - right->m_hub); - tmp = (int) (thisLastDot - left->m_hub); + dotPos = (int)(otherLastDot - right->m_hub); + tmp = (int)(thisLastDot - left->m_hub); if (dotPos != tmp) return 0; diff --git a/lib/xspublic/xscontroller/xsusbhubinfo.h b/lib/xspublic/xscontroller/xsusbhubinfo.h index 06bfc1a..1825add 100644 --- a/lib/xspublic/xscontroller/xsusbhubinfo.h +++ b/lib/xspublic/xscontroller/xsusbhubinfo.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -96,13 +96,14 @@ int XDA_DLL_API XsUsbHubInfo_parentPathMatches(const XsUsbHubInfo* thisPtr, con /*! \struct XsUsbHubInfo \brief A structure that wraps USB hub information */ -struct XsUsbHubInfo { +struct XsUsbHubInfo +{ #ifdef __cplusplus /*! \brief Default constructor - \param hubid an optional hub identifier to initialize with - \sa XsUsbHubInfo_construct - */ + \param hubid an optional hub identifier to initialize with + \sa XsUsbHubInfo_construct + */ explicit XsUsbHubInfo(XsHubIdentifier hubid = 0) : m_hub(0) { @@ -117,10 +118,10 @@ struct XsUsbHubInfo { } /*! \brief Copy constructor - \param other the object to copy - \sa XsUsbHubInfo_copy \sa XsUsbHubInfo_construct - */ - XsUsbHubInfo(const XsUsbHubInfo &other) + \param other the object to copy + \sa XsUsbHubInfo_copy \sa XsUsbHubInfo_construct + */ + XsUsbHubInfo(const XsUsbHubInfo& other) : m_hub(0) { if (other.m_hub) @@ -128,11 +129,11 @@ struct XsUsbHubInfo { } /*! \brief Assigns \a other to this XsUsbHubInfo - \param other the object to copy - \returns a const reference to this info object - \sa XsUsbHubInfo_copy - */ - const XsUsbHubInfo& operator=(const XsUsbHubInfo &other) + \param other the object to copy + \returns a const reference to this info object + \sa XsUsbHubInfo_copy + */ + const XsUsbHubInfo& operator=(const XsUsbHubInfo& other) { if (this != &other) XsUsbHubInfo_copy(this, &other); @@ -140,11 +141,11 @@ struct XsUsbHubInfo { } /*! \brief \copybrief XsUsbHubInfo_parentPathMatches - * \param other the object to compare to - * \returns true if the two objects share the same immediate parent hub, false otherwise - * \sa XsUsbHubInfo_parentPathMatches - */ - bool parentPathMatches(const XsUsbHubInfo &other) const + \param other the object to compare to + \returns true if the two objects share the same immediate parent hub, false otherwise + \sa XsUsbHubInfo_parentPathMatches + */ + bool parentPathMatches(const XsUsbHubInfo& other) const { return 0 != XsUsbHubInfo_parentPathMatches(this, &other); } @@ -164,10 +165,10 @@ struct XsUsbHubInfo { } private: -//! \protectedsection + //! \protectedsection #endif XsHubIdentifier m_hub; //!< The identifier of the USB hub }; typedef struct XsUsbHubInfo XsUsbHubInfo; -#endif // file guard +#endif diff --git a/lib/xspublic/xscontroller/xswinusb.cpp b/lib/xspublic/xscontroller/xswinusb.cpp index 8f5a847..4dcb0b7 100644 --- a/lib/xspublic/xscontroller/xswinusb.cpp +++ b/lib/xspublic/xscontroller/xswinusb.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -171,7 +171,7 @@ BOOL XsWinUsb::Free(WINUSB_INTERFACE_HANDLE InterfaceHandle) \returns On success, TRUE. Otherwise FALSE. Use GetLastError() to get extended error information. */ -BOOL XsWinUsb::GetAssociatedInterface(WINUSB_INTERFACE_HANDLE InterfaceHandle,UCHAR AssociatedInterfaceIndex, PWINUSB_INTERFACE_HANDLE AssociatedInterfaceHandle) +BOOL XsWinUsb::GetAssociatedInterface(WINUSB_INTERFACE_HANDLE InterfaceHandle, UCHAR AssociatedInterfaceIndex, PWINUSB_INTERFACE_HANDLE AssociatedInterfaceHandle) { if (m_winUsb.GetAssociatedInterface) return m_winUsb.GetAssociatedInterface(InterfaceHandle, AssociatedInterfaceIndex, AssociatedInterfaceHandle); @@ -190,7 +190,7 @@ BOOL XsWinUsb::GetAssociatedInterface(WINUSB_INTERFACE_HANDLE InterfaceHandle,UC \returns On success, TRUE. Otherwise FALSE. Use GetLastError() to get extended error information. */ -BOOL XsWinUsb::GetDescriptor(WINUSB_INTERFACE_HANDLE InterfaceHandle,UCHAR DescriptorType, UCHAR Index,USHORT LanguageID,PUCHAR Buffer,ULONG BufferLength,PULONG LengthTransferred) +BOOL XsWinUsb::GetDescriptor(WINUSB_INTERFACE_HANDLE InterfaceHandle, UCHAR DescriptorType, UCHAR Index, USHORT LanguageID, PUCHAR Buffer, ULONG BufferLength, PULONG LengthTransferred) { if (m_winUsb.GetDescriptor) return m_winUsb.GetDescriptor(InterfaceHandle, DescriptorType, Index, LanguageID, Buffer, BufferLength, LengthTransferred); @@ -210,7 +210,7 @@ BOOL XsWinUsb::GetDescriptor(WINUSB_INTERFACE_HANDLE InterfaceHandle,UCHAR Descr \returns On success, TRUE. Otherwise FALSE. Use GetLastError() to get extended error information. */ -BOOL XsWinUsb::QueryInterfaceSettings(WINUSB_INTERFACE_HANDLE InterfaceHandle,UCHAR AlternateInterfaceNumber,PUSB_INTERFACE_DESCRIPTOR UsbAltInterfaceDescriptor) +BOOL XsWinUsb::QueryInterfaceSettings(WINUSB_INTERFACE_HANDLE InterfaceHandle, UCHAR AlternateInterfaceNumber, PUSB_INTERFACE_DESCRIPTOR UsbAltInterfaceDescriptor) { if (m_winUsb.QueryInterfaceSettings) return m_winUsb.QueryInterfaceSettings(InterfaceHandle, AlternateInterfaceNumber, UsbAltInterfaceDescriptor); @@ -228,7 +228,7 @@ BOOL XsWinUsb::QueryInterfaceSettings(WINUSB_INTERFACE_HANDLE InterfaceHandle,UC \returns On success, TRUE. Otherwise FALSE. Use GetLastError() to get extended error information. */ -BOOL XsWinUsb::QueryDeviceInformation(WINUSB_INTERFACE_HANDLE InterfaceHandle,ULONG InformationType,PULONG BufferLength,PVOID Buffer) +BOOL XsWinUsb::QueryDeviceInformation(WINUSB_INTERFACE_HANDLE InterfaceHandle, ULONG InformationType, PULONG BufferLength, PVOID Buffer) { if (m_winUsb.QueryDeviceInformation) return m_winUsb.QueryDeviceInformation(InterfaceHandle, InformationType, BufferLength, Buffer); @@ -246,7 +246,7 @@ BOOL XsWinUsb::QueryDeviceInformation(WINUSB_INTERFACE_HANDLE InterfaceHandle,UL \sa QueryInterfaceSettings */ -BOOL XsWinUsb::SetCurrentAlternateSetting(WINUSB_INTERFACE_HANDLE InterfaceHandle,UCHAR SettingNumber) +BOOL XsWinUsb::SetCurrentAlternateSetting(WINUSB_INTERFACE_HANDLE InterfaceHandle, UCHAR SettingNumber) { if (m_winUsb.SetCurrentAlternateSetting) return m_winUsb.SetCurrentAlternateSetting(InterfaceHandle, SettingNumber); @@ -262,7 +262,7 @@ BOOL XsWinUsb::SetCurrentAlternateSetting(WINUSB_INTERFACE_HANDLE InterfaceHandl \returns On success, TRUE. Otherwise FALSE. Use GetLastError() to get extended error information. */ -BOOL XsWinUsb::GetCurrentAlternateSetting(WINUSB_INTERFACE_HANDLE InterfaceHandle,PUCHAR SettingNumber) +BOOL XsWinUsb::GetCurrentAlternateSetting(WINUSB_INTERFACE_HANDLE InterfaceHandle, PUCHAR SettingNumber) { if (m_winUsb.GetCurrentAlternateSetting) return m_winUsb.GetCurrentAlternateSetting(InterfaceHandle, SettingNumber); @@ -288,7 +288,7 @@ BOOL XsWinUsb::GetCurrentAlternateSetting(WINUSB_INTERFACE_HANDLE InterfaceHandl \returns On success, TRUE. Otherwise FALSE. Use GetLastError() to get extended error information. */ -BOOL XsWinUsb::QueryPipe(WINUSB_INTERFACE_HANDLE InterfaceHandle,UCHAR AlternateInterfaceNumber,UCHAR PipeIndex,PWINUSB_PIPE_INFORMATION PipeInformation) +BOOL XsWinUsb::QueryPipe(WINUSB_INTERFACE_HANDLE InterfaceHandle, UCHAR AlternateInterfaceNumber, UCHAR PipeIndex, PWINUSB_PIPE_INFORMATION PipeInformation) { if (m_winUsb.QueryPipe) return m_winUsb.QueryPipe(InterfaceHandle, AlternateInterfaceNumber, PipeIndex, PipeInformation); @@ -354,7 +354,7 @@ BOOL XsWinUsb::QueryPipe(WINUSB_INTERFACE_HANDLE InterfaceHandle,UCHAR Alternate \returns On success, TRUE. Otherwise FALSE. Use GetLastError() to get extended error information. */ -BOOL XsWinUsb::SetPipePolicy(WINUSB_INTERFACE_HANDLE InterfaceHandle,UCHAR PipeID,ULONG PolicyType,ULONG ValueLength,PVOID Value) +BOOL XsWinUsb::SetPipePolicy(WINUSB_INTERFACE_HANDLE InterfaceHandle, UCHAR PipeID, ULONG PolicyType, ULONG ValueLength, PVOID Value) { if (m_winUsb.SetPipePolicy) return m_winUsb.SetPipePolicy(InterfaceHandle, PipeID, PolicyType, ValueLength, Value); @@ -371,7 +371,7 @@ BOOL XsWinUsb::SetPipePolicy(WINUSB_INTERFACE_HANDLE InterfaceHandle,UCHAR PipeI \returns On success, TRUE. Otherwise FALSE. Use GetLastError() to get extended error information. */ -BOOL XsWinUsb::GetPipePolicy(WINUSB_INTERFACE_HANDLE InterfaceHandle,UCHAR PipeID,ULONG PolicyType,PULONG ValueLength,PVOID Value) +BOOL XsWinUsb::GetPipePolicy(WINUSB_INTERFACE_HANDLE InterfaceHandle, UCHAR PipeID, ULONG PolicyType, PULONG ValueLength, PVOID Value) { if (m_winUsb.GetPipePolicy) return m_winUsb.GetPipePolicy(InterfaceHandle, PipeID, PolicyType, ValueLength, Value); @@ -390,7 +390,7 @@ BOOL XsWinUsb::GetPipePolicy(WINUSB_INTERFACE_HANDLE InterfaceHandle,UCHAR PipeI \returns On success, TRUE. Otherwise FALSE. Use GetLastError() to get extended error information. */ -BOOL XsWinUsb::ReadPipe(WINUSB_INTERFACE_HANDLE InterfaceHandle, UCHAR PipeID, PUCHAR Buffer, ULONG BufferLength, PULONG LengthTransferred,LPOVERLAPPED Overlapped) +BOOL XsWinUsb::ReadPipe(WINUSB_INTERFACE_HANDLE InterfaceHandle, UCHAR PipeID, PUCHAR Buffer, ULONG BufferLength, PULONG LengthTransferred, LPOVERLAPPED Overlapped) { if (m_winUsb.ReadPipe) return m_winUsb.ReadPipe(InterfaceHandle, PipeID, Buffer, BufferLength, LengthTransferred, Overlapped); @@ -409,7 +409,7 @@ BOOL XsWinUsb::ReadPipe(WINUSB_INTERFACE_HANDLE InterfaceHandle, UCHAR PipeID, P \returns On success, TRUE. Otherwise FALSE. Use GetLastError() to get extended error information. */ -BOOL XsWinUsb::WritePipe(WINUSB_INTERFACE_HANDLE InterfaceHandle, UCHAR PipeID, PUCHAR Buffer, ULONG BufferLength, PULONG LengthTransferred,LPOVERLAPPED Overlapped) +BOOL XsWinUsb::WritePipe(WINUSB_INTERFACE_HANDLE InterfaceHandle, UCHAR PipeID, PUCHAR Buffer, ULONG BufferLength, PULONG LengthTransferred, LPOVERLAPPED Overlapped) { if (m_winUsb.WritePipe) return m_winUsb.WritePipe(InterfaceHandle, PipeID, Buffer, BufferLength, LengthTransferred, Overlapped); @@ -535,7 +535,7 @@ BOOL XsWinUsb::SetPowerPolicy(WINUSB_INTERFACE_HANDLE InterfaceHandle, ULONG Pol \returns On success, TRUE. Otherwise FALSE. Use GetLastError() to get extended error information. */ -BOOL XsWinUsb::GetPowerPolicy(WINUSB_INTERFACE_HANDLE InterfaceHandle,ULONG PolicyType,PULONG ValueLength,PVOID Value) +BOOL XsWinUsb::GetPowerPolicy(WINUSB_INTERFACE_HANDLE InterfaceHandle, ULONG PolicyType, PULONG ValueLength, PVOID Value) { if (m_winUsb.GetPowerPolicy) return m_winUsb.GetPowerPolicy(InterfaceHandle, PolicyType, ValueLength, Value); @@ -559,7 +559,7 @@ BOOL XsWinUsb::GetPowerPolicy(WINUSB_INTERFACE_HANDLE InterfaceHandle,ULONG Poli \returns On success, TRUE. Otherwise FALSE. Use GetLastError() to get extended error information. */ -BOOL XsWinUsb::GetOverlappedResult(WINUSB_INTERFACE_HANDLE InterfaceHandle,LPOVERLAPPED lpOverlapped,LPDWORD lpNumberOfBytesTransferred,BOOL bWait) +BOOL XsWinUsb::GetOverlappedResult(WINUSB_INTERFACE_HANDLE InterfaceHandle, LPOVERLAPPED lpOverlapped, LPDWORD lpNumberOfBytesTransferred, BOOL bWait) { if (m_winUsb.GetOverlappedResult) return m_winUsb.GetOverlappedResult(InterfaceHandle, lpOverlapped, lpNumberOfBytesTransferred, bWait); diff --git a/lib/xspublic/xscontroller/xswinusb.h b/lib/xspublic/xscontroller/xswinusb.h index 9d750e1..95d91cc 100644 --- a/lib/xspublic/xscontroller/xswinusb.h +++ b/lib/xspublic/xscontroller/xswinusb.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -74,26 +74,26 @@ struct XsLibraryLoader; -typedef BOOL __stdcall WinUSB_Initialize (HANDLE DeviceHandle, WINUSB_INTERFACE_HANDLE* InterfaceHandle); -typedef BOOL __stdcall WinUSB_Free (WINUSB_INTERFACE_HANDLE InterfaceHandle); -typedef BOOL __stdcall WinUSB_GetAssociatedInterface (WINUSB_INTERFACE_HANDLE InterfaceHandle, UCHAR AssociatedInterfaceIndex, WINUSB_INTERFACE_HANDLE* AssociatedInterfaceHandle); -typedef BOOL __stdcall WinUSB_GetDescriptor (WINUSB_INTERFACE_HANDLE InterfaceHandle, UCHAR DescriptorType, UCHAR Index, USHORT LanguageID, PUCHAR Buffer, ULONG BufferLength, PULONG LengthTransferred); -typedef BOOL __stdcall WinUSB_QueryInterfaceSettings (WINUSB_INTERFACE_HANDLE InterfaceHandle, UCHAR AltSettingIndex, PUSB_INTERFACE_DESCRIPTOR UsbAltInterfaceDescriptor); -typedef BOOL __stdcall WinUSB_QueryDeviceInformation (WINUSB_INTERFACE_HANDLE InterfaceHandle, ULONG InformationType, PULONG BufferLength, PVOID Buffer); -typedef BOOL __stdcall WinUSB_SetCurrentAlternateSetting (WINUSB_INTERFACE_HANDLE InterfaceHandle, UCHAR AltSettingNumber); -typedef BOOL __stdcall WinUSB_GetCurrentAlternateSetting (WINUSB_INTERFACE_HANDLE InterfaceHandle, PUCHAR AltSettingNumber); -typedef BOOL __stdcall WinUSB_QueryPipe (WINUSB_INTERFACE_HANDLE InterfaceHandle, UCHAR AltSettingNumber, UCHAR PipeIndex, PWINUSB_PIPE_INFORMATION PipeInformation); -typedef BOOL __stdcall WinUSB_SetPipePolicy (WINUSB_INTERFACE_HANDLE InterfaceHandle, UCHAR PipeID, ULONG PolicyType, ULONG ValueLength, PVOID Value); -typedef BOOL __stdcall WinUSB_GetPipePolicy (WINUSB_INTERFACE_HANDLE InterfaceHandle, UCHAR PipeID, ULONG PolicyType, PULONG ValueLength, PVOID Value); -typedef BOOL __stdcall WinUSB_ReadPipe (WINUSB_INTERFACE_HANDLE InterfaceHandle, UCHAR PipeID, PUCHAR Buffer, ULONG BufferLength, PULONG LengthTransferred, LPOVERLAPPED Overlapped); -typedef BOOL __stdcall WinUSB_WritePipe (WINUSB_INTERFACE_HANDLE InterfaceHandle, UCHAR PipeID, PUCHAR Buffer, ULONG BufferLength, PULONG LengthTransferred, LPOVERLAPPED Overlapped); -typedef BOOL __stdcall WinUSB_ControlTransfer (WINUSB_INTERFACE_HANDLE InterfaceHandle, WINUSB_SETUP_PACKET SetupPacket, PUCHAR Buffer, ULONG BufferLength, PULONG LengthTransferred, LPOVERLAPPED Overlapped); -typedef BOOL __stdcall WinUSB_ResetPipe (WINUSB_INTERFACE_HANDLE InterfaceHandle, UCHAR PipeID); -typedef BOOL __stdcall WinUSB_AbortPipe (WINUSB_INTERFACE_HANDLE InterfaceHandle, UCHAR PipeID); -typedef BOOL __stdcall WinUSB_FlushPipe (WINUSB_INTERFACE_HANDLE InterfaceHandle, UCHAR PipeID); -typedef BOOL __stdcall WinUSB_SetPowerPolicy (WINUSB_INTERFACE_HANDLE InterfaceHandle, ULONG PolicyType, ULONG ValueLength, PVOID Value); -typedef BOOL __stdcall WinUSB_GetPowerPolicy (WINUSB_INTERFACE_HANDLE InterfaceHandle, ULONG PolicyType, PULONG ValueLength, PVOID Value); -typedef BOOL __stdcall WinUSB_GetOverlappedResult (WINUSB_INTERFACE_HANDLE InterfaceHandle, LPOVERLAPPED Overlapped, LPDWORD lpNumberOfBytesTransferred, BOOL bWait); +typedef BOOL __stdcall WinUSB_Initialize(HANDLE DeviceHandle, WINUSB_INTERFACE_HANDLE* InterfaceHandle); +typedef BOOL __stdcall WinUSB_Free(WINUSB_INTERFACE_HANDLE InterfaceHandle); +typedef BOOL __stdcall WinUSB_GetAssociatedInterface(WINUSB_INTERFACE_HANDLE InterfaceHandle, UCHAR AssociatedInterfaceIndex, WINUSB_INTERFACE_HANDLE* AssociatedInterfaceHandle); +typedef BOOL __stdcall WinUSB_GetDescriptor(WINUSB_INTERFACE_HANDLE InterfaceHandle, UCHAR DescriptorType, UCHAR Index, USHORT LanguageID, PUCHAR Buffer, ULONG BufferLength, PULONG LengthTransferred); +typedef BOOL __stdcall WinUSB_QueryInterfaceSettings(WINUSB_INTERFACE_HANDLE InterfaceHandle, UCHAR AltSettingIndex, PUSB_INTERFACE_DESCRIPTOR UsbAltInterfaceDescriptor); +typedef BOOL __stdcall WinUSB_QueryDeviceInformation(WINUSB_INTERFACE_HANDLE InterfaceHandle, ULONG InformationType, PULONG BufferLength, PVOID Buffer); +typedef BOOL __stdcall WinUSB_SetCurrentAlternateSetting(WINUSB_INTERFACE_HANDLE InterfaceHandle, UCHAR AltSettingNumber); +typedef BOOL __stdcall WinUSB_GetCurrentAlternateSetting(WINUSB_INTERFACE_HANDLE InterfaceHandle, PUCHAR AltSettingNumber); +typedef BOOL __stdcall WinUSB_QueryPipe(WINUSB_INTERFACE_HANDLE InterfaceHandle, UCHAR AltSettingNumber, UCHAR PipeIndex, PWINUSB_PIPE_INFORMATION PipeInformation); +typedef BOOL __stdcall WinUSB_SetPipePolicy(WINUSB_INTERFACE_HANDLE InterfaceHandle, UCHAR PipeID, ULONG PolicyType, ULONG ValueLength, PVOID Value); +typedef BOOL __stdcall WinUSB_GetPipePolicy(WINUSB_INTERFACE_HANDLE InterfaceHandle, UCHAR PipeID, ULONG PolicyType, PULONG ValueLength, PVOID Value); +typedef BOOL __stdcall WinUSB_ReadPipe(WINUSB_INTERFACE_HANDLE InterfaceHandle, UCHAR PipeID, PUCHAR Buffer, ULONG BufferLength, PULONG LengthTransferred, LPOVERLAPPED Overlapped); +typedef BOOL __stdcall WinUSB_WritePipe(WINUSB_INTERFACE_HANDLE InterfaceHandle, UCHAR PipeID, PUCHAR Buffer, ULONG BufferLength, PULONG LengthTransferred, LPOVERLAPPED Overlapped); +typedef BOOL __stdcall WinUSB_ControlTransfer(WINUSB_INTERFACE_HANDLE InterfaceHandle, WINUSB_SETUP_PACKET SetupPacket, PUCHAR Buffer, ULONG BufferLength, PULONG LengthTransferred, LPOVERLAPPED Overlapped); +typedef BOOL __stdcall WinUSB_ResetPipe(WINUSB_INTERFACE_HANDLE InterfaceHandle, UCHAR PipeID); +typedef BOOL __stdcall WinUSB_AbortPipe(WINUSB_INTERFACE_HANDLE InterfaceHandle, UCHAR PipeID); +typedef BOOL __stdcall WinUSB_FlushPipe(WINUSB_INTERFACE_HANDLE InterfaceHandle, UCHAR PipeID); +typedef BOOL __stdcall WinUSB_SetPowerPolicy(WINUSB_INTERFACE_HANDLE InterfaceHandle, ULONG PolicyType, ULONG ValueLength, PVOID Value); +typedef BOOL __stdcall WinUSB_GetPowerPolicy(WINUSB_INTERFACE_HANDLE InterfaceHandle, ULONG PolicyType, PULONG ValueLength, PVOID Value); +typedef BOOL __stdcall WinUSB_GetOverlappedResult(WINUSB_INTERFACE_HANDLE InterfaceHandle, LPOVERLAPPED Overlapped, LPDWORD lpNumberOfBytesTransferred, BOOL bWait); class XsWinUsb { @@ -126,26 +126,26 @@ class XsWinUsb typedef struct _WINUSB_API { - WinUSB_Initialize (* Initialize); - WinUSB_Free (* Free); - WinUSB_GetAssociatedInterface (*GetAssociatedInterface); - WinUSB_GetDescriptor (* GetDescriptor); - WinUSB_QueryInterfaceSettings (* QueryInterfaceSettings); - WinUSB_QueryDeviceInformation (* QueryDeviceInformation); - WinUSB_SetCurrentAlternateSetting (* SetCurrentAlternateSetting); - WinUSB_GetCurrentAlternateSetting (* GetCurrentAlternateSetting); - WinUSB_QueryPipe (* QueryPipe); - WinUSB_SetPipePolicy (* SetPipePolicy); - WinUSB_GetPipePolicy (* GetPipePolicy); - WinUSB_ReadPipe (* ReadPipe); - WinUSB_WritePipe (* WritePipe); - WinUSB_ControlTransfer (* ControlTransfer); - WinUSB_ResetPipe (* ResetPipe); - WinUSB_AbortPipe (* AbortPipe); - WinUSB_FlushPipe (* FlushPipe); - WinUSB_SetPowerPolicy (* SetPowerPolicy); - WinUSB_GetPowerPolicy (* GetPowerPolicy); - WinUSB_GetOverlappedResult (*GetOverlappedResult); + WinUSB_Initialize(* Initialize); + WinUSB_Free(* Free); + WinUSB_GetAssociatedInterface(*GetAssociatedInterface); + WinUSB_GetDescriptor(* GetDescriptor); + WinUSB_QueryInterfaceSettings(* QueryInterfaceSettings); + WinUSB_QueryDeviceInformation(* QueryDeviceInformation); + WinUSB_SetCurrentAlternateSetting(* SetCurrentAlternateSetting); + WinUSB_GetCurrentAlternateSetting(* GetCurrentAlternateSetting); + WinUSB_QueryPipe(* QueryPipe); + WinUSB_SetPipePolicy(* SetPipePolicy); + WinUSB_GetPipePolicy(* GetPipePolicy); + WinUSB_ReadPipe(* ReadPipe); + WinUSB_WritePipe(* WritePipe); + WinUSB_ControlTransfer(* ControlTransfer); + WinUSB_ResetPipe(* ResetPipe); + WinUSB_AbortPipe(* AbortPipe); + WinUSB_FlushPipe(* FlushPipe); + WinUSB_SetPowerPolicy(* SetPowerPolicy); + WinUSB_GetPowerPolicy(* GetPowerPolicy); + WinUSB_GetOverlappedResult(*GetOverlappedResult); } WINUSB_API; WINUSB_API m_winUsb; @@ -160,4 +160,3 @@ class XsWinUsb #endif // USE_WINUSB #endif - diff --git a/lib/xspublic/xstypes/datapacket_p.cpp b/lib/xspublic/xstypes/datapacket_p.cpp index 1224241..2fdf710 100644 --- a/lib/xspublic/xstypes/datapacket_p.cpp +++ b/lib/xspublic/xstypes/datapacket_p.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -91,7 +91,7 @@ DataPacketPrivate::~DataPacketPrivate() { clear(); } - catch(...) + catch (...) { } } diff --git a/lib/xspublic/xstypes/datapacket_p.h b/lib/xspublic/xstypes/datapacket_p.h index fa7e7ad..a0f0c5c 100644 --- a/lib/xspublic/xstypes/datapacket_p.h +++ b/lib/xspublic/xstypes/datapacket_p.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -90,516 +90,519 @@ /*! \cond XS_INTERNAL */ namespace XsDataPacket_Private { - /*! \brief Abstract Variant class for handling contents of XsDataPacket */ - class Variant - { - public: - /*! \brief Constructor, sets the dataId to \a id */ - Variant(XsDataIdentifier id) : m_id(id) {} - virtual ~Variant() {} - /*! \brief Read the data from message \a msg at \a offset and optionally using \a dSize */ - virtual XsSize readFromMessage(XsMessage const& msg, XsSize offset, XsSize dSize) = 0; - /*! \brief Write the data to message \a msg at \a offset */ - virtual void writeToMessage(XsMessage& msg, XsSize offset) const = 0; - /*! \brief Return the size the Variant would have in a message */ - virtual XsSize sizeInMsg() const = 0; - /*! \brief Create a copy of the Variant - \return A pointer to the newly created Variant - \note This needs to be reimplemented in each subclass! - */ - virtual Variant* clone() const = 0; - - /*! \brief Set the dataId to \a id */ - void setDataId(XsDataIdentifier id) - { - assert((m_id & XDI_FullTypeMask) == (id & XDI_FullTypeMask)); - m_id = id; - } - /*! \brief Return the dataId of the Variant */ - XsDataIdentifier dataId() const { return m_id; } - - /*! \brief Convert the Variant to a derived type if allowed, returns nullptr otherwise (and asserts in debug builds) */ - template - U& toDerived() - { - U* ptr = dynamic_cast(this); - assert(ptr); - return *ptr; - } +/*! \brief Abstract Variant class for handling contents of XsDataPacket */ +class Variant +{ +public: + /*! \brief Constructor, sets the dataId to \a id */ + Variant(XsDataIdentifier id) : m_id(id) {} + virtual ~Variant() {} + /*! \brief Read the data from message \a msg at \a offset and optionally using \a dSize */ + virtual XsSize readFromMessage(XsMessage const& msg, XsSize offset, XsSize dSize) = 0; + /*! \brief Write the data to message \a msg at \a offset */ + virtual void writeToMessage(XsMessage& msg, XsSize offset) const = 0; + /*! \brief Return the size the Variant would have in a message */ + virtual XsSize sizeInMsg() const = 0; + /*! \brief Create a copy of the Variant + \return A pointer to the newly created Variant + \note This needs to be reimplemented in each subclass! + */ + virtual Variant* clone() const = 0; - /*! \brief Convert the Variant to a derived type if allowed, returns nullptr otherwise (and asserts in debug builds) */ - template - U const& toDerived() const - { - U const* ptr = dynamic_cast(this); - assert(ptr); - return *ptr; - } + /*! \brief Set the dataId to \a id */ + void setDataId(XsDataIdentifier id) + { + assert((m_id & XDI_FullTypeMask) == (id & XDI_FullTypeMask)); + m_id = id; + } + /*! \brief Return the dataId of the Variant */ + XsDataIdentifier dataId() const + { + return m_id; + } - private: - XsDataIdentifier m_id; - }; + /*! \brief Convert the Variant to a derived type if allowed, returns nullptr otherwise (and asserts in debug builds) */ + template + U& toDerived() + { + U* ptr = dynamic_cast(this); + assert(ptr); + return *ptr; + } - /*! \brief Generic base class template for Variant subclasses */ - template - class GenericVariant : public Variant + /*! \brief Convert the Variant to a derived type if allowed, returns nullptr otherwise (and asserts in debug builds) */ + template + U const& toDerived() const { - public: - /*! \brief Constructor, sets the dataId to \a id */ - GenericVariant(XsDataIdentifier id) : Variant(id) {} + U const* ptr = dynamic_cast(this); + assert(ptr); + return *ptr; + } - /*! \brief Read the data from message \a msg at \a offset and optionally using \a dSize */ - XsSize readFromMessage(XsMessage const& msg, XsSize offset, XsSize sz) override - { - msg.getData(data(), dataId(), offset, C); - return sz; - } - /*! \brief Write the data to message \a msg at \a offset */ - void writeToMessage(XsMessage& msg, XsSize offset) const override - { - msg.setData(constData(), dataId(), offset, C); - } - /*! \brief Return a typed pointer to the contained data */ - virtual T* data() = 0; - /*! \brief Return a typed pointer to the contained data */ - virtual T const* constData() const = 0; +private: + XsDataIdentifier m_id; +}; - /*! \brief Return the size the Variant would have in a message */ - XsSize sizeInMsg() const override - { - return (XsSize)(ptrdiff_t) XsMessage::sizeInMsg(dataId(), C); - } - }; +/*! \brief Generic base class template for Variant subclasses */ +template +class GenericVariant : public Variant +{ +public: + /*! \brief Constructor, sets the dataId to \a id */ + GenericVariant(XsDataIdentifier id) : Variant(id) {} - /*! \brief Read the data from the message \a msg at the given \a offset */ - template<> - inline XsSize GenericVariant::readFromMessage(XsMessage const& msg, XsSize offset, XsSize sz) + /*! \brief Read the data from message \a msg at \a offset and optionally using \a dSize */ + XsSize readFromMessage(XsMessage const& msg, XsSize offset, XsSize sz) override { - *data() = 0; - *data() = ((uint64_t)XsMessage_getDataLong(&msg, offset)) << 32; - *data() += ((uint64_t)XsMessage_getDataLong(&msg, offset + 4)); + msg.getData(data(), dataId(), offset, C); return sz; } - - /*! \brief Write the data to the message \a msg at the given \a offset */ - template<> - inline void GenericVariant::writeToMessage(XsMessage& msg, XsSize offset) const + /*! \brief Write the data to message \a msg at \a offset */ + void writeToMessage(XsMessage& msg, XsSize offset) const override { - XsMessage_setDataLong(&msg, (uint32_t) ((*constData()) >> 32), offset); - XsMessage_setDataLong(&msg, (uint32_t) ((*constData()) & 0xFFFFFFFF), offset); + msg.setData(constData(), dataId(), offset, C); } + /*! \brief Return a typed pointer to the contained data */ + virtual T* data() = 0; + /*! \brief Return a typed pointer to the contained data */ + virtual T const* constData() const = 0; - /*! \brief Template base class for simple single value Variants. memcpy is assumed to result in valid objects */ - template - struct SimpleVariant : public GenericVariant + /*! \brief Return the size the Variant would have in a message */ + XsSize sizeInMsg() const override { - using GenericVariant::dataId; + return (XsSize)(ptrdiff_t) XsMessage::sizeInMsg(dataId(), C); + } +}; - /*! \brief Constructor, sets the dataId to \a id */ - SimpleVariant(XsDataIdentifier id) : GenericVariant(id), m_data() {} - /*! \brief Constructor, sets the dataId to \a id and data value to \a val */ - SimpleVariant(XsDataIdentifier id, T const& val) : GenericVariant(id), m_data(val) {} +/*! \brief Read the data from the message \a msg at the given \a offset */ +template<> +inline XsSize GenericVariant::readFromMessage(XsMessage const& msg, XsSize offset, XsSize sz) +{ + *data() = 0; + *data() = ((uint64_t)XsMessage_getDataLong(&msg, offset)) << 32; + *data() += ((uint64_t)XsMessage_getDataLong(&msg, offset + 4)); + return sz; +} - T m_data; //!< The contained data - /*! \brief Return a typed pointer to the contained data */ - T* data() override - { - return &m_data; - } - /*! \brief Return a typed pointer to the contained data */ - T const* constData() const override - { - return &m_data; - } +/*! \brief Write the data to the message \a msg at the given \a offset */ +template<> +inline void GenericVariant::writeToMessage(XsMessage& msg, XsSize offset) const +{ + XsMessage_setDataLong(&msg, (uint32_t)((*constData()) >> 32), offset); + XsMessage_setDataLong(&msg, (uint32_t)((*constData()) & 0xFFFFFFFF), offset); +} + +/*! \brief Template base class for simple single value Variants. memcpy is assumed to result in valid objects */ +template +struct SimpleVariant : public GenericVariant +{ + using GenericVariant::dataId; - /*! \brief Create a copy of the Variant + /*! \brief Constructor, sets the dataId to \a id */ + SimpleVariant(XsDataIdentifier id) : GenericVariant(id), m_data() {} + /*! \brief Constructor, sets the dataId to \a id and data value to \a val */ + SimpleVariant(XsDataIdentifier id, T const& val) : GenericVariant(id), m_data(val) {} + + T m_data; //!< The contained data + /*! \brief Return a typed pointer to the contained data */ + T* data() override + { + return &m_data; + } + /*! \brief Return a typed pointer to the contained data */ + T const* constData() const override + { + return &m_data; + } + + /*! \brief Create a copy of the Variant \return A pointer to the newly created Variant \note This needs to be reimplemented in each subclass! - */ - Variant* clone() const override - { - return new SimpleVariant(dataId(), m_data); - } - - /*! \brief Return the size the Variant would have in a message */ - XsSize sizeInMsg() const override - { - return XsMessage::sizeInMsg(dataId(), 1); - } - }; + */ + Variant* clone() const override + { + return new SimpleVariant(dataId(), m_data); + } - /*! \brief Template base class for complex or multi-value Variants. memcpy is NOT assumed to result in valid objects */ - template - struct ComplexVariant : public GenericVariant + /*! \brief Return the size the Variant would have in a message */ + XsSize sizeInMsg() const override { - using GenericVariant::dataId; - /*! \brief Constructor, sets the dataId to \a id */ - ComplexVariant(XsDataIdentifier id) : GenericVariant(id) {} - /*! \brief Constructor, sets the dataId to \a id and data value to \a val */ - ComplexVariant(XsDataIdentifier id, U const& val) : GenericVariant(id), m_data(val) {} + return XsMessage::sizeInMsg(dataId(), 1); + } +}; - U m_data; //!< The contained data - /*! \brief Return a typed pointer to the contained data */ - T* data() override - { - return const_cast(m_data.data()); - } - /*! \brief Return a typed pointer to the contained data */ - T const* constData() const override - { - return m_data.data(); - } +/*! \brief Template base class for complex or multi-value Variants. memcpy is NOT assumed to result in valid objects */ +template +struct ComplexVariant : public GenericVariant +{ + using GenericVariant::dataId; + /*! \brief Constructor, sets the dataId to \a id */ + ComplexVariant(XsDataIdentifier id) : GenericVariant(id) {} + /*! \brief Constructor, sets the dataId to \a id and data value to \a val */ + ComplexVariant(XsDataIdentifier id, U const& val) : GenericVariant(id), m_data(val) {} + + U m_data; //!< The contained data + /*! \brief Return a typed pointer to the contained data */ + T* data() override + { + return const_cast(m_data.data()); + } + /*! \brief Return a typed pointer to the contained data */ + T const* constData() const override + { + return m_data.data(); + } - /*! \brief Create a copy of the Variant + /*! \brief Create a copy of the Variant \return A pointer to the newly created Variant \note This needs to be reimplemented in each subclass! - */ - Variant* clone() const override - { - return new ComplexVariant(dataId(), m_data); - } - }; - - /*! \brief Variant containing an XsQuaternion value */ - struct XsQuaternionVariant : public ComplexVariant + */ + Variant* clone() const override { - /*! \brief Constructor, sets the dataId to \a id */ - XsQuaternionVariant(XsDataIdentifier id) : ComplexVariant(id) {} - /*! \brief Constructor, sets the dataId to \a id and data value to \a val */ - XsQuaternionVariant(XsDataIdentifier id, XsQuaternion const& val) : ComplexVariant(id, val) {} + return new ComplexVariant(dataId(), m_data); + } +}; - Variant* clone() const override - { - return new XsQuaternionVariant(dataId(), m_data); - } - }; +/*! \brief Variant containing an XsQuaternion value */ +struct XsQuaternionVariant : public ComplexVariant +{ + /*! \brief Constructor, sets the dataId to \a id */ + XsQuaternionVariant(XsDataIdentifier id) : ComplexVariant(id) {} + /*! \brief Constructor, sets the dataId to \a id and data value to \a val */ + XsQuaternionVariant(XsDataIdentifier id, XsQuaternion const& val) : ComplexVariant(id, val) {} - /*! \brief Variant containing an XsUShortVector value */ - struct XsUShortVectorVariant : public ComplexVariant + Variant* clone() const override { - /*! \brief Constructor, sets the dataId to \a id */ - XsUShortVectorVariant(XsDataIdentifier id) : ComplexVariant(id) {} - /*! \brief Constructor, sets the dataId to \a id and data value to \a val */ - XsUShortVectorVariant(XsDataIdentifier id, XsUShortVector const& val) : ComplexVariant(id, val) {} - Variant* clone() const override - { - return new XsUShortVectorVariant(dataId(), m_data); - } - }; + return new XsQuaternionVariant(dataId(), m_data); + } +}; - /*! \brief Variant containing an XsVector3 value */ - struct XsVector3Variant : public ComplexVariant +/*! \brief Variant containing an XsUShortVector value */ +struct XsUShortVectorVariant : public ComplexVariant +{ + /*! \brief Constructor, sets the dataId to \a id */ + XsUShortVectorVariant(XsDataIdentifier id) : ComplexVariant(id) {} + /*! \brief Constructor, sets the dataId to \a id and data value to \a val */ + XsUShortVectorVariant(XsDataIdentifier id, XsUShortVector const& val) : ComplexVariant(id, val) {} + Variant* clone() const override { - /*! \brief Constructor, sets the dataId to \a id */ - XsVector3Variant(XsDataIdentifier id) : ComplexVariant(id) {} - /*! \brief Constructor, sets the dataId to \a id and data value to \a val */ - XsVector3Variant(XsDataIdentifier id, XsVector const& val) : ComplexVariant(id, val) - { - assert(val.size() == 3); - } - Variant* clone() const override - { - return new XsVector3Variant(dataId(), m_data); - } - }; + return new XsUShortVectorVariant(dataId(), m_data); + } +}; - /*! \brief Variant containing an XsVector value */ - struct XsVector2Variant : public ComplexVariant +/*! \brief Variant containing an XsVector3 value */ +struct XsVector3Variant : public ComplexVariant +{ + /*! \brief Constructor, sets the dataId to \a id */ + XsVector3Variant(XsDataIdentifier id) : ComplexVariant(id) {} + /*! \brief Constructor, sets the dataId to \a id and data value to \a val */ + XsVector3Variant(XsDataIdentifier id, XsVector const& val) : ComplexVariant(id, val) { - /*! \brief Constructor, sets the dataId to \a id */ - XsVector2Variant(XsDataIdentifier id) : ComplexVariant(id, XsVector(2,0)) {} - /*! \brief Constructor, sets the dataId to \a id and data value to \a val */ - XsVector2Variant(XsDataIdentifier id, XsVector const& val) : ComplexVariant(id, val) - { - assert(val.size() == 2); - } - Variant* clone() const override - { - return new XsVector2Variant(dataId(), m_data); - } - }; + assert(val.size() == 3); + } + Variant* clone() const override + { + return new XsVector3Variant(dataId(), m_data); + } +}; - /*! \brief Variant containing an XsScrData value */ - struct XsScrDataVariant : public Variant +/*! \brief Variant containing an XsVector value */ +struct XsVector2Variant : public ComplexVariant +{ + /*! \brief Constructor, sets the dataId to \a id */ + XsVector2Variant(XsDataIdentifier id) : ComplexVariant(id, XsVector(2, 0)) {} + /*! \brief Constructor, sets the dataId to \a id and data value to \a val */ + XsVector2Variant(XsDataIdentifier id, XsVector const& val) : ComplexVariant(id, val) { - /*! \brief Constructor, sets the dataId to \a id */ - XsScrDataVariant(XsDataIdentifier id) : Variant(id) {} - /*! \brief Constructor, sets the dataId to \a id and data value to \a val */ - XsScrDataVariant(XsDataIdentifier id, XsScrData const& val) : Variant(id), m_data(val) {} - Variant* clone() const override - { - return new XsScrDataVariant(dataId(), m_data); - } + assert(val.size() == 2); + } + Variant* clone() const override + { + return new XsVector2Variant(dataId(), m_data); + } +}; - XsScrData m_data; //!< The contained data - XsSize readFromMessage(XsMessage const& msg, XsSize offset, XsSize sz) override - { - for (XsSize i = 0; i < 3; ++i, offset +=2) - m_data.m_acc[i] = msg.getDataShort(offset); - for (XsSize i = 0; i < 3; ++i, offset +=2) - m_data.m_gyr[i] = msg.getDataShort(offset); - for (XsSize i = 0; i < 3; ++i, offset +=2) - m_data.m_mag[i] = msg.getDataShort(offset); - m_data.m_temp = msg.getDataShort(offset); - return sz; - } - void writeToMessage(XsMessage& msg, XsSize offset) const override - { - for (XsSize i = 0; i < 3; ++i, offset +=2) - msg.setDataShort(m_data.m_acc[i], offset); - for (XsSize i = 0; i < 3; ++i, offset +=2) - msg.setDataShort(m_data.m_gyr[i], offset); - for (XsSize i = 0; i < 3; ++i, offset +=2) - msg.setDataShort(m_data.m_mag[i], offset); - msg.setDataShort(m_data.m_temp, offset); - } +/*! \brief Variant containing an XsScrData value */ +struct XsScrDataVariant : public Variant +{ + /*! \brief Constructor, sets the dataId to \a id */ + XsScrDataVariant(XsDataIdentifier id) : Variant(id) {} + /*! \brief Constructor, sets the dataId to \a id and data value to \a val */ + XsScrDataVariant(XsDataIdentifier id, XsScrData const& val) : Variant(id), m_data(val) {} + Variant* clone() const override + { + return new XsScrDataVariant(dataId(), m_data); + } - XsSize sizeInMsg() const override - { - return 10*sizeof(uint16_t); - } - }; + XsScrData m_data; //!< The contained data + XsSize readFromMessage(XsMessage const& msg, XsSize offset, XsSize sz) override + { + for (XsSize i = 0; i < 3; ++i, offset += 2) + m_data.m_acc[i] = msg.getDataShort(offset); + for (XsSize i = 0; i < 3; ++i, offset += 2) + m_data.m_gyr[i] = msg.getDataShort(offset); + for (XsSize i = 0; i < 3; ++i, offset += 2) + m_data.m_mag[i] = msg.getDataShort(offset); + m_data.m_temp = msg.getDataShort(offset); + return sz; + } + void writeToMessage(XsMessage& msg, XsSize offset) const override + { + for (XsSize i = 0; i < 3; ++i, offset += 2) + msg.setDataShort(m_data.m_acc[i], offset); + for (XsSize i = 0; i < 3; ++i, offset += 2) + msg.setDataShort(m_data.m_gyr[i], offset); + for (XsSize i = 0; i < 3; ++i, offset += 2) + msg.setDataShort(m_data.m_mag[i], offset); + msg.setDataShort(m_data.m_temp, offset); + } - /*! \brief Variant containing an XsTriggerIndication value */ - struct XsTriggerIndicationDataVariant : public Variant + XsSize sizeInMsg() const override { - /*! \brief Constructor, sets the dataId to \a id */ - XsTriggerIndicationDataVariant(XsDataIdentifier id) : Variant(id) {} - /*! \brief Constructor, sets the dataId to \a id and data value to \a val */ - XsTriggerIndicationDataVariant(XsDataIdentifier id, XsTriggerIndicationData const& val) : Variant(id), m_data(val) {} - Variant* clone() const override - { - return new XsTriggerIndicationDataVariant(dataId(), m_data); - } + return 10 * sizeof(uint16_t); + } +}; - XsTriggerIndicationData m_data; //!< The contained data - XsSize readFromMessage(XsMessage const& msg, XsSize offset, XsSize sz) override - { - m_data.m_line = XsMessage_getDataByte (&msg, offset + 0); - m_data.m_polarity = XsMessage_getDataByte (&msg, offset + 1); - m_data.m_timestamp = XsMessage_getDataLong (&msg, offset + 2); - m_data.m_frameNumber = XsMessage_getDataShort(&msg, offset + 6); - return sz; - } - void writeToMessage(XsMessage& msg, XsSize offset) const override - { - XsMessage_setDataByte (&msg, m_data.m_line, offset + 0); - XsMessage_setDataByte (&msg, m_data.m_polarity, offset + 1); - XsMessage_setDataLong (&msg, m_data.m_timestamp, offset + 2); - XsMessage_setDataShort(&msg, m_data.m_frameNumber, offset + 6); - } +/*! \brief Variant containing an XsTriggerIndication value */ +struct XsTriggerIndicationDataVariant : public Variant +{ + /*! \brief Constructor, sets the dataId to \a id */ + XsTriggerIndicationDataVariant(XsDataIdentifier id) : Variant(id) {} + /*! \brief Constructor, sets the dataId to \a id and data value to \a val */ + XsTriggerIndicationDataVariant(XsDataIdentifier id, XsTriggerIndicationData const& val) : Variant(id), m_data(val) {} + Variant* clone() const override + { + return new XsTriggerIndicationDataVariant(dataId(), m_data); + } - XsSize sizeInMsg() const override - { - return 8; - } - }; + XsTriggerIndicationData m_data; //!< The contained data + XsSize readFromMessage(XsMessage const& msg, XsSize offset, XsSize sz) override + { + m_data.m_line = XsMessage_getDataByte(&msg, offset + 0); + m_data.m_polarity = XsMessage_getDataByte(&msg, offset + 1); + m_data.m_timestamp = XsMessage_getDataLong(&msg, offset + 2); + m_data.m_frameNumber = XsMessage_getDataShort(&msg, offset + 6); + return sz; + } + void writeToMessage(XsMessage& msg, XsSize offset) const override + { + XsMessage_setDataByte(&msg, m_data.m_line, offset + 0); + XsMessage_setDataByte(&msg, m_data.m_polarity, offset + 1); + XsMessage_setDataLong(&msg, m_data.m_timestamp, offset + 2); + XsMessage_setDataShort(&msg, m_data.m_frameNumber, offset + 6); + } - /*! \brief Variant containing an XsEuler value */ - struct XsEulerVariant : public ComplexVariant + XsSize sizeInMsg() const override { - /*! \brief Constructor, sets the dataId to \a id */ - XsEulerVariant(XsDataIdentifier id) : ComplexVariant(id) {} - /*! \brief Constructor, sets the dataId to \a id and data value to \a val */ - XsEulerVariant(XsDataIdentifier id, XsEuler const& val) : ComplexVariant(id, val) {} - Variant* clone() const override - { - return new XsEulerVariant(dataId(), m_data); - } - }; + return 8; + } +}; - /*! \brief Variant containing an XsMatrix value */ - struct XsMatrixVariant : public ComplexVariant +/*! \brief Variant containing an XsEuler value */ +struct XsEulerVariant : public ComplexVariant +{ + /*! \brief Constructor, sets the dataId to \a id */ + XsEulerVariant(XsDataIdentifier id) : ComplexVariant(id) {} + /*! \brief Constructor, sets the dataId to \a id and data value to \a val */ + XsEulerVariant(XsDataIdentifier id, XsEuler const& val) : ComplexVariant(id, val) {} + Variant* clone() const override { - /*! \brief Constructor, sets the dataId to \a id */ - XsMatrixVariant(XsDataIdentifier id) : ComplexVariant(id) {} - /*! \brief Constructor, sets the dataId to \a id and data value to \a val */ - XsMatrixVariant(XsDataIdentifier id, XsMatrix const& val) : ComplexVariant(id, val) {} - Variant* clone() const override - { - return new XsMatrixVariant(dataId(), m_data); - } + return new XsEulerVariant(dataId(), m_data); + } +}; - XsSize readFromMessage(XsMessage const& msg, XsSize offset, XsSize sz) override - { - XsSize ds = XsMessage_getFPValueSize(dataId()); - XsSize k = 0; - for (int i=0 ; i<3 ; ++i) - for (XsSize j=0 ; j<3 ; ++j, k+=ds) - XsMessage_getDataRealValuesById(&msg, dataId(), &m_data[j][i], offset+k, 1); - return sz; - } - void writeToMessage(XsMessage& msg, XsSize offset) const override - { - XsSize ds = XsMessage_getFPValueSize(dataId()); - XsSize k = 0; - for (int i=0 ; i<3 ; ++i) - for (XsSize j=0 ; j<3 ; ++j, k+=ds) - XsMessage_setDataRealValuesById(&msg, dataId(), &m_data[j][i], offset+k, 1); - } - }; +/*! \brief Variant containing an XsMatrix value */ +struct XsMatrixVariant : public ComplexVariant +{ + /*! \brief Constructor, sets the dataId to \a id */ + XsMatrixVariant(XsDataIdentifier id) : ComplexVariant(id) {} + /*! \brief Constructor, sets the dataId to \a id and data value to \a val */ + XsMatrixVariant(XsDataIdentifier id, XsMatrix const& val) : ComplexVariant(id, val) {} + Variant* clone() const override + { + return new XsMatrixVariant(dataId(), m_data); + } - /*! \brief Variant containing an XsRange value */ - struct XsRangeVariant : public Variant + XsSize readFromMessage(XsMessage const& msg, XsSize offset, XsSize sz) override { - /*! \brief Constructor, sets the dataId to \a id */ - XsRangeVariant(XsDataIdentifier id) : Variant(id) {} - /*! \brief Constructor, sets the dataId to \a id and data value to \a val */ - XsRangeVariant(XsDataIdentifier id, XsRange const& val) : Variant(id), m_data(val) {} - Variant* clone() const override - { - return new XsRangeVariant(dataId(), m_data); - } + XsSize ds = XsMessage_getFPValueSize(dataId()); + XsSize k = 0; + for (int i = 0 ; i < 3 ; ++i) + for (XsSize j = 0 ; j < 3 ; ++j, k += ds) + XsMessage_getDataRealValuesById(&msg, dataId(), &m_data[j][i], offset + k, 1); + return sz; + } + void writeToMessage(XsMessage& msg, XsSize offset) const override + { + XsSize ds = XsMessage_getFPValueSize(dataId()); + XsSize k = 0; + for (int i = 0 ; i < 3 ; ++i) + for (XsSize j = 0 ; j < 3 ; ++j, k += ds) + XsMessage_setDataRealValuesById(&msg, dataId(), &m_data[j][i], offset + k, 1); + } +}; - XsRange m_data; //!< The contained data - XsSize readFromMessage(XsMessage const& msg, XsSize offset, XsSize sz) override - { - // unwrap - uint16_t first = (uint16_t) XsMessage_getDataShort(&msg, offset + 0); - uint16_t last = (uint16_t) XsMessage_getDataShort(&msg, offset + 2); - m_data.setRange(first, (int)((uint16_t)(last - first)) + (int)first); - return sz; - } - void writeToMessage(XsMessage& msg, XsSize offset) const override - { - XsMessage_setDataShort(&msg, (uint16_t) m_data.first(), offset + 0); - XsMessage_setDataShort(&msg, (uint16_t) m_data.last(), offset + 2); - } +/*! \brief Variant containing an XsRange value */ +struct XsRangeVariant : public Variant +{ + /*! \brief Constructor, sets the dataId to \a id */ + XsRangeVariant(XsDataIdentifier id) : Variant(id) {} + /*! \brief Constructor, sets the dataId to \a id and data value to \a val */ + XsRangeVariant(XsDataIdentifier id, XsRange const& val) : Variant(id), m_data(val) {} + Variant* clone() const override + { + return new XsRangeVariant(dataId(), m_data); + } - XsSize sizeInMsg() const override - { - return 4; - } - }; + XsRange m_data; //!< The contained data + XsSize readFromMessage(XsMessage const& msg, XsSize offset, XsSize sz) override + { + // unwrap + uint16_t first = (uint16_t) XsMessage_getDataShort(&msg, offset + 0); + uint16_t last = (uint16_t) XsMessage_getDataShort(&msg, offset + 2); + m_data.setRange(first, (int)((uint16_t)(last - first)) + (int)first); + return sz; + } + void writeToMessage(XsMessage& msg, XsSize offset) const override + { + XsMessage_setDataShort(&msg, (uint16_t) m_data.first(), offset + 0); + XsMessage_setDataShort(&msg, (uint16_t) m_data.last(), offset + 2); + } - /*! \brief Variant containing an XsTimeInfo value */ - struct XsTimeInfoVariant : public Variant + XsSize sizeInMsg() const override { - /*! \brief Constructor, sets the dataId to \a id */ - XsTimeInfoVariant(XsDataIdentifier id) : Variant(id), m_data() {} - /*! \brief Constructor, sets the dataId to \a id and data value to \a val */ - XsTimeInfoVariant(XsDataIdentifier id, XsTimeInfo const& val) : Variant(id), m_data(val) {} - Variant* clone() const override - { - return new XsTimeInfoVariant(dataId(), m_data); - } + return 4; + } +}; - XsTimeInfo m_data; //!< The contained data - XsSize readFromMessage(XsMessage const& msg, XsSize offset, XsSize sz) override - { - m_data.m_nano = XsMessage_getDataLong(&msg, offset); - m_data.m_year = XsMessage_getDataShort(&msg, offset+4); +/*! \brief Variant containing an XsTimeInfo value */ +struct XsTimeInfoVariant : public Variant +{ + /*! \brief Constructor, sets the dataId to \a id */ + XsTimeInfoVariant(XsDataIdentifier id) : Variant(id), m_data() {} + /*! \brief Constructor, sets the dataId to \a id and data value to \a val */ + XsTimeInfoVariant(XsDataIdentifier id, XsTimeInfo const& val) : Variant(id), m_data(val) {} + Variant* clone() const override + { + return new XsTimeInfoVariant(dataId(), m_data); + } - // month, day, hour, minute, second and valid - uint8_t* bareByte = (uint8_t*) &m_data.m_month; - for (XsSize i=0; i < 6; ++i) - bareByte[i] = XsMessage_getDataByte(&msg, offset + 6 + i); + XsTimeInfo m_data; //!< The contained data + XsSize readFromMessage(XsMessage const& msg, XsSize offset, XsSize sz) override + { + m_data.m_nano = XsMessage_getDataLong(&msg, offset); + m_data.m_year = XsMessage_getDataShort(&msg, offset + 4); - m_data.m_utcOffset = 0; - return sz; - } - void writeToMessage(XsMessage& msg, XsSize offset) const override - { - // update - XsMessage_setDataLong(&msg, m_data.m_nano, offset); - XsMessage_setDataShort(&msg, m_data.m_year, offset + 4); + // month, day, hour, minute, second and valid + uint8_t* bareByte = (uint8_t*) &m_data.m_month; + for (XsSize i = 0; i < 6; ++i) + bareByte[i] = XsMessage_getDataByte(&msg, offset + 6 + i); - // month, day, hour, minute, second and valid - uint8_t const* bareByte = (uint8_t const*) &m_data.m_month; - for (XsSize i=0; i<6; ++i) - XsMessage_setDataByte(&msg, bareByte[i], offset + 6 + i); + m_data.m_utcOffset = 0; + return sz; + } + void writeToMessage(XsMessage& msg, XsSize offset) const override + { + // update + XsMessage_setDataLong(&msg, m_data.m_nano, offset); + XsMessage_setDataShort(&msg, m_data.m_year, offset + 4); - // utcOffset is ignored and assumed to be 0, use makeUtc to ensure this if necessary - } + // month, day, hour, minute, second and valid + uint8_t const* bareByte = (uint8_t const*) &m_data.m_month; + for (XsSize i = 0; i < 6; ++i) + XsMessage_setDataByte(&msg, bareByte[i], offset + 6 + i); - XsSize sizeInMsg() const override - { - return 12; - } - }; + // utcOffset is ignored and assumed to be 0, use makeUtc to ensure this if necessary + } - /*! \brief Variant containing an XsRawGnssPvtData value */ - struct XsRawGnssPvtDataVariant : public Variant + XsSize sizeInMsg() const override { - /*! \brief Constructor, sets the dataId to \a id */ - XsRawGnssPvtDataVariant(XsDataIdentifier id) : Variant(id), m_data() {} - /*! \brief Constructor, sets the dataId to \a id and data value to \a val */ - XsRawGnssPvtDataVariant(XsDataIdentifier id, XsRawGnssPvtData const& val) : Variant(id), m_data(val) {} - Variant* clone() const override - { - return new XsRawGnssPvtDataVariant(dataId(), m_data); - } + return 12; + } +}; - XsRawGnssPvtData m_data; //!< The contained data - XsSize readFromMessage(XsMessage const& msg, XsSize offset, XsSize sz) override - { - m_data.m_itow = XsMessage_getDataLong(&msg, offset + 0); - m_data.m_year = XsMessage_getDataShort(&msg, offset + 4); - m_data.m_month = XsMessage_getDataByte(&msg, offset + 6); - m_data.m_day = XsMessage_getDataByte(&msg, offset + 7); - m_data.m_hour = XsMessage_getDataByte(&msg, offset + 8); - m_data.m_min = XsMessage_getDataByte(&msg, offset + 9); - m_data.m_sec = XsMessage_getDataByte(&msg, offset + 10); - m_data.m_valid = XsMessage_getDataByte(&msg, offset + 11); - m_data.m_tAcc = XsMessage_getDataLong(&msg, offset + 12); - m_data.m_nano = XsMessage_getDataLong(&msg, offset + 16); - m_data.m_fixType = XsMessage_getDataByte(&msg, offset + 20); - m_data.m_flags = XsMessage_getDataByte(&msg, offset + 21); - m_data.m_numSv = XsMessage_getDataByte(&msg, offset + 22); - m_data.m_res1 = XsMessage_getDataByte(&msg, offset + 23); - m_data.m_lon = XsMessage_getDataLong(&msg, offset + 24); - m_data.m_lat = XsMessage_getDataLong(&msg, offset + 28); - m_data.m_height = XsMessage_getDataLong(&msg, offset + 32); - m_data.m_hMsl = XsMessage_getDataLong(&msg, offset + 36); - m_data.m_hAcc = XsMessage_getDataLong(&msg, offset + 40); - m_data.m_vAcc = XsMessage_getDataLong(&msg, offset + 44); - m_data.m_velN = XsMessage_getDataLong(&msg, offset + 48); - m_data.m_velE = XsMessage_getDataLong(&msg, offset + 52); - m_data.m_velD = XsMessage_getDataLong(&msg, offset + 56); - m_data.m_gSpeed = XsMessage_getDataLong(&msg, offset + 60); - m_data.m_headMot = XsMessage_getDataLong(&msg, offset + 64); - m_data.m_sAcc = XsMessage_getDataLong(&msg, offset + 68); - m_data.m_headAcc = XsMessage_getDataLong(&msg, offset + 72); - m_data.m_headVeh = XsMessage_getDataLong(&msg, offset + 76); - m_data.m_gdop = XsMessage_getDataShort(&msg, offset + 80); - m_data.m_pdop = XsMessage_getDataShort(&msg, offset + 82); - m_data.m_tdop = XsMessage_getDataShort(&msg, offset + 84); - m_data.m_vdop = XsMessage_getDataShort(&msg, offset + 86); - m_data.m_hdop = XsMessage_getDataShort(&msg, offset + 88); - m_data.m_ndop = XsMessage_getDataShort(&msg, offset + 90); - m_data.m_edop = XsMessage_getDataShort(&msg, offset + 92); - return sz; - } - void writeToMessage(XsMessage& msg, XsSize offset) const override - { - XsMessage_setDataLong (&msg, m_data.m_itow, offset + 0); +/*! \brief Variant containing an XsRawGnssPvtData value */ +struct XsRawGnssPvtDataVariant : public Variant +{ + /*! \brief Constructor, sets the dataId to \a id */ + XsRawGnssPvtDataVariant(XsDataIdentifier id) : Variant(id), m_data() {} + /*! \brief Constructor, sets the dataId to \a id and data value to \a val */ + XsRawGnssPvtDataVariant(XsDataIdentifier id, XsRawGnssPvtData const& val) : Variant(id), m_data(val) {} + Variant* clone() const override + { + return new XsRawGnssPvtDataVariant(dataId(), m_data); + } + + XsRawGnssPvtData m_data; //!< The contained data + XsSize readFromMessage(XsMessage const& msg, XsSize offset, XsSize sz) override + { + m_data.m_itow = XsMessage_getDataLong(&msg, offset + 0); + m_data.m_year = XsMessage_getDataShort(&msg, offset + 4); + m_data.m_month = XsMessage_getDataByte(&msg, offset + 6); + m_data.m_day = XsMessage_getDataByte(&msg, offset + 7); + m_data.m_hour = XsMessage_getDataByte(&msg, offset + 8); + m_data.m_min = XsMessage_getDataByte(&msg, offset + 9); + m_data.m_sec = XsMessage_getDataByte(&msg, offset + 10); + m_data.m_valid = XsMessage_getDataByte(&msg, offset + 11); + m_data.m_tAcc = XsMessage_getDataLong(&msg, offset + 12); + m_data.m_nano = XsMessage_getDataLong(&msg, offset + 16); + m_data.m_fixType = XsMessage_getDataByte(&msg, offset + 20); + m_data.m_flags = XsMessage_getDataByte(&msg, offset + 21); + m_data.m_numSv = XsMessage_getDataByte(&msg, offset + 22); + m_data.m_res1 = XsMessage_getDataByte(&msg, offset + 23); + m_data.m_lon = XsMessage_getDataLong(&msg, offset + 24); + m_data.m_lat = XsMessage_getDataLong(&msg, offset + 28); + m_data.m_height = XsMessage_getDataLong(&msg, offset + 32); + m_data.m_hMsl = XsMessage_getDataLong(&msg, offset + 36); + m_data.m_hAcc = XsMessage_getDataLong(&msg, offset + 40); + m_data.m_vAcc = XsMessage_getDataLong(&msg, offset + 44); + m_data.m_velN = XsMessage_getDataLong(&msg, offset + 48); + m_data.m_velE = XsMessage_getDataLong(&msg, offset + 52); + m_data.m_velD = XsMessage_getDataLong(&msg, offset + 56); + m_data.m_gSpeed = XsMessage_getDataLong(&msg, offset + 60); + m_data.m_headMot = XsMessage_getDataLong(&msg, offset + 64); + m_data.m_sAcc = XsMessage_getDataLong(&msg, offset + 68); + m_data.m_headAcc = XsMessage_getDataLong(&msg, offset + 72); + m_data.m_headVeh = XsMessage_getDataLong(&msg, offset + 76); + m_data.m_gdop = XsMessage_getDataShort(&msg, offset + 80); + m_data.m_pdop = XsMessage_getDataShort(&msg, offset + 82); + m_data.m_tdop = XsMessage_getDataShort(&msg, offset + 84); + m_data.m_vdop = XsMessage_getDataShort(&msg, offset + 86); + m_data.m_hdop = XsMessage_getDataShort(&msg, offset + 88); + m_data.m_ndop = XsMessage_getDataShort(&msg, offset + 90); + m_data.m_edop = XsMessage_getDataShort(&msg, offset + 92); + return sz; + } + void writeToMessage(XsMessage& msg, XsSize offset) const override + { + XsMessage_setDataLong(&msg, m_data.m_itow, offset + 0); XsMessage_setDataShort(&msg, m_data.m_year, offset + 4); - XsMessage_setDataByte (&msg, m_data.m_month, offset + 6); - XsMessage_setDataByte (&msg, m_data.m_day, offset + 7); - XsMessage_setDataByte (&msg, m_data.m_hour, offset + 8); - XsMessage_setDataByte (&msg, m_data.m_min, offset + 9); - XsMessage_setDataByte (&msg, m_data.m_sec, offset + 10); - XsMessage_setDataByte (&msg, m_data.m_valid, offset + 11); - XsMessage_setDataLong (&msg, m_data.m_tAcc, offset + 12); - XsMessage_setDataLong (&msg, m_data.m_nano, offset + 16); - XsMessage_setDataByte (&msg, m_data.m_fixType, offset + 20); - XsMessage_setDataByte (&msg, m_data.m_flags, offset + 21); - XsMessage_setDataByte (&msg, m_data.m_numSv, offset + 22); - XsMessage_setDataByte (&msg, m_data.m_res1, offset + 23); - XsMessage_setDataLong (&msg, m_data.m_lon, offset + 24); - XsMessage_setDataLong (&msg, m_data.m_lat, offset + 28); - XsMessage_setDataLong (&msg, m_data.m_height, offset + 32); - XsMessage_setDataLong (&msg, m_data.m_hMsl, offset + 36); - XsMessage_setDataLong (&msg, m_data.m_hAcc, offset + 40); - XsMessage_setDataLong (&msg, m_data.m_vAcc, offset + 44); - XsMessage_setDataLong (&msg, m_data.m_velN, offset + 48); - XsMessage_setDataLong (&msg, m_data.m_velE, offset + 52); - XsMessage_setDataLong (&msg, m_data.m_velD, offset + 56); - XsMessage_setDataLong (&msg, m_data.m_gSpeed, offset + 60); - XsMessage_setDataLong (&msg, m_data.m_headMot, offset + 64); - XsMessage_setDataLong (&msg, m_data.m_sAcc, offset + 68); - XsMessage_setDataLong (&msg, m_data.m_headAcc, offset + 72); - XsMessage_setDataLong (&msg, m_data.m_headVeh, offset + 76); + XsMessage_setDataByte(&msg, m_data.m_month, offset + 6); + XsMessage_setDataByte(&msg, m_data.m_day, offset + 7); + XsMessage_setDataByte(&msg, m_data.m_hour, offset + 8); + XsMessage_setDataByte(&msg, m_data.m_min, offset + 9); + XsMessage_setDataByte(&msg, m_data.m_sec, offset + 10); + XsMessage_setDataByte(&msg, m_data.m_valid, offset + 11); + XsMessage_setDataLong(&msg, m_data.m_tAcc, offset + 12); + XsMessage_setDataLong(&msg, m_data.m_nano, offset + 16); + XsMessage_setDataByte(&msg, m_data.m_fixType, offset + 20); + XsMessage_setDataByte(&msg, m_data.m_flags, offset + 21); + XsMessage_setDataByte(&msg, m_data.m_numSv, offset + 22); + XsMessage_setDataByte(&msg, m_data.m_res1, offset + 23); + XsMessage_setDataLong(&msg, m_data.m_lon, offset + 24); + XsMessage_setDataLong(&msg, m_data.m_lat, offset + 28); + XsMessage_setDataLong(&msg, m_data.m_height, offset + 32); + XsMessage_setDataLong(&msg, m_data.m_hMsl, offset + 36); + XsMessage_setDataLong(&msg, m_data.m_hAcc, offset + 40); + XsMessage_setDataLong(&msg, m_data.m_vAcc, offset + 44); + XsMessage_setDataLong(&msg, m_data.m_velN, offset + 48); + XsMessage_setDataLong(&msg, m_data.m_velE, offset + 52); + XsMessage_setDataLong(&msg, m_data.m_velD, offset + 56); + XsMessage_setDataLong(&msg, m_data.m_gSpeed, offset + 60); + XsMessage_setDataLong(&msg, m_data.m_headMot, offset + 64); + XsMessage_setDataLong(&msg, m_data.m_sAcc, offset + 68); + XsMessage_setDataLong(&msg, m_data.m_headAcc, offset + 72); + XsMessage_setDataLong(&msg, m_data.m_headVeh, offset + 76); XsMessage_setDataShort(&msg, m_data.m_gdop, offset + 80); XsMessage_setDataShort(&msg, m_data.m_pdop, offset + 82); XsMessage_setDataShort(&msg, m_data.m_tdop, offset + 84); @@ -607,411 +610,434 @@ namespace XsDataPacket_Private XsMessage_setDataShort(&msg, m_data.m_hdop, offset + 88); XsMessage_setDataShort(&msg, m_data.m_ndop, offset + 90); XsMessage_setDataShort(&msg, m_data.m_edop, offset + 92); - } + } - XsSize sizeInMsg() const override - { - return 94; - } - }; + XsSize sizeInMsg() const override + { + return 94; + } +}; - /*! \brief Variant containing an XsRawGnssSatInfo value */ - struct XsRawGnssSatInfoVariant : public Variant +/*! \brief Variant containing an XsRawGnssSatInfo value */ +struct XsRawGnssSatInfoVariant : public Variant +{ + /*! \brief Constructor, sets the dataId to \a id */ + XsRawGnssSatInfoVariant(XsDataIdentifier id) : Variant(id), m_data() {} + /*! \brief Constructor, sets the dataId to \a id and data value to \a val */ + XsRawGnssSatInfoVariant(XsDataIdentifier id, XsRawGnssSatInfo const& val) : Variant(id), m_data(val) {} + Variant* clone() const override { - /*! \brief Constructor, sets the dataId to \a id */ - XsRawGnssSatInfoVariant(XsDataIdentifier id) : Variant(id), m_data() {} - /*! \brief Constructor, sets the dataId to \a id and data value to \a val */ - XsRawGnssSatInfoVariant(XsDataIdentifier id, XsRawGnssSatInfo const& val) : Variant(id), m_data(val) {} - Variant* clone() const override - { - return new XsRawGnssSatInfoVariant(dataId(), m_data); - } + return new XsRawGnssSatInfoVariant(dataId(), m_data); + } - XsRawGnssSatInfo m_data; //!< The contained data - XsSize readFromMessage(XsMessage const& msg, XsSize offset, XsSize sz) override + XsRawGnssSatInfo m_data; //!< The contained data + XsSize readFromMessage(XsMessage const& msg, XsSize offset, XsSize sz) override + { + m_data.m_itow = XsMessage_getDataLong(&msg, offset + 0); + m_data.m_numSvs = XsMessage_getDataByte(&msg, offset + 4); + m_data.m_res1 = XsMessage_getDataByte(&msg, offset + 5); + m_data.m_res2 = XsMessage_getDataByte(&msg, offset + 6); + m_data.m_res3 = XsMessage_getDataByte(&msg, offset + 7); + + offset = offset + 8; + for (uint8_t i = 0; i < m_data.m_numSvs; ++i) { - m_data.m_itow = XsMessage_getDataLong(&msg, offset + 0); - m_data.m_numSvs = XsMessage_getDataByte(&msg, offset + 4); - m_data.m_res1 = XsMessage_getDataByte(&msg, offset + 5); - m_data.m_res2 = XsMessage_getDataByte(&msg, offset + 6); - m_data.m_res3 = XsMessage_getDataByte(&msg, offset + 7); - - offset = offset + 8; - for (uint8_t i = 0; i < m_data.m_numSvs; ++i) - { - m_data.m_satInfos[i].m_gnssId = XsMessage_getDataByte(&msg, offset + 0); - m_data.m_satInfos[i].m_svId = XsMessage_getDataByte(&msg, offset + 1); - m_data.m_satInfos[i].m_cno = XsMessage_getDataByte(&msg, offset + 2); - m_data.m_satInfos[i].m_flags = XsMessage_getDataByte(&msg, offset + 3); - offset += 4; - } - return sz; + m_data.m_satInfos[i].m_gnssId = XsMessage_getDataByte(&msg, offset + 0); + m_data.m_satInfos[i].m_svId = XsMessage_getDataByte(&msg, offset + 1); + m_data.m_satInfos[i].m_cno = XsMessage_getDataByte(&msg, offset + 2); + m_data.m_satInfos[i].m_flags = XsMessage_getDataByte(&msg, offset + 3); + offset += 4; } - void writeToMessage(XsMessage& msg, XsSize offset) const override - { + return sz; + } + void writeToMessage(XsMessage& msg, XsSize offset) const override + { XsMessage_setDataLong(&msg, m_data.m_itow, offset + 0); XsMessage_setDataByte(&msg, m_data.m_numSvs, offset + 4); XsMessage_setDataByte(&msg, m_data.m_res1, offset + 5); XsMessage_setDataByte(&msg, m_data.m_res2, offset + 6); XsMessage_setDataByte(&msg, m_data.m_res3, offset + 7); - offset = offset + 8; - for (uint8_t i = 0; i < m_data.m_numSvs; ++i) - { + offset = offset + 8; + for (uint8_t i = 0; i < m_data.m_numSvs; ++i) + { XsMessage_setDataByte(&msg, m_data.m_satInfos[i].m_gnssId, offset + 0); XsMessage_setDataByte(&msg, m_data.m_satInfos[i].m_svId, offset + 1); XsMessage_setDataByte(&msg, m_data.m_satInfos[i].m_cno, offset + 2); XsMessage_setDataByte(&msg, m_data.m_satInfos[i].m_flags, offset + 3); - offset += 4; - } + offset += 4; } + } - XsSize sizeInMsg() const override - { - return 8 + 4*m_data.m_numSvs; - } - }; - - /*! \brief Variant containing an XsFullSnaphsot value */ - struct XsFullSnapshotVariant : public Variant + XsSize sizeInMsg() const override { - /*! \brief Constructor, sets the dataId to \a id */ - XsFullSnapshotVariant(XsDataIdentifier id) : Variant(id) {} - /*! \brief Constructor, sets the dataId to \a id and data value to \a val */ - XsFullSnapshotVariant(XsDataIdentifier id, XsSnapshot const& val) : Variant(id), m_data(val) {} + return 8 + 4 * m_data.m_numSvs; + } +}; - Variant* clone() const override - { - return new XsFullSnapshotVariant(dataId(), m_data); - } +/*! \brief Variant containing an XsFullSnaphsot value */ +struct XsFullSnapshotVariant : public Variant +{ + /*! \brief Constructor, sets the dataId to \a id */ + XsFullSnapshotVariant(XsDataIdentifier id) : Variant(id) {} + /*! \brief Constructor, sets the dataId to \a id and data value to \a val */ + XsFullSnapshotVariant(XsDataIdentifier id, XsSnapshot const& val) : Variant(id), m_data(val) {} - XsSnapshot m_data; //!< The contained data - XsSize readFromMessage(XsMessage const& msg, XsSize offset, XsSize sz) override - { - m_data.m_frameNumber = XsMessage_getDataShort(&msg, offset); offset += 2; - m_data.m_timestamp = XsMessage_getDataLongLong(&msg, offset); offset += 8; - for (int i = 0; i < 4; ++i, offset += 4) - m_data.m_iQ[i] = XsMessage_getDataLong(&msg, offset); - for (int i = 0; i < 3; ++i, offset += 8) - m_data.m_iV[i] = XsMessage_getDataLongLong(&msg, offset); - for (int i = 0; i < 3; ++i, offset += 4) - m_data.m_mag[i] = XsMessage_getDataLong(&msg, offset); - - m_data.m_baro = XsMessage_getDataLong(&msg, offset); offset += 4; - m_data.m_accClippingCounter = XsMessage_getDataByte(&msg, offset); offset += 1; - m_data.m_gyrClippingCounter = XsMessage_getDataByte(&msg, offset); offset += 1; - m_data.m_status = XsMessage_getDataShort(&msg, offset); - m_data.m_type = ST_Full; - return sz; - } + Variant* clone() const override + { + return new XsFullSnapshotVariant(dataId(), m_data); + } - void writeToMessage(XsMessage& msg, XsSize offset) const override - { - assert(m_data.m_type == ST_Full); - XsMessage_setDataShort(&msg, (uint16_t) m_data.m_frameNumber, offset); offset += 2; - XsMessage_setDataLongLong(&msg, m_data.m_timestamp, offset); offset += 8; - for (int i = 0; i < 4; ++i, offset += 4) - XsMessage_setDataLong(&msg, m_data.m_iQ[i], offset); - for (int i = 0; i < 3; ++i, offset += 8) - XsMessage_setDataLongLong(&msg, m_data.m_iV[i], offset); - for (int i = 0; i < 3; ++i, offset += 4) - XsMessage_setDataLong(&msg, m_data.m_mag[i], offset); - - XsMessage_setDataLong(&msg, m_data.m_baro, offset); offset += 4; - XsMessage_setDataByte(&msg, m_data.m_accClippingCounter, offset); offset += 1; - XsMessage_setDataByte(&msg, m_data.m_gyrClippingCounter, offset); offset += 1; - XsMessage_setDataShort(&msg, m_data.m_status, offset); - } + XsSnapshot m_data; //!< The contained data + XsSize readFromMessage(XsMessage const& msg, XsSize offset, XsSize sz) override + { + m_data.m_frameNumber = XsMessage_getDataShort(&msg, offset); + offset += 2; + m_data.m_timestamp = XsMessage_getDataLongLong(&msg, offset); + offset += 8; + for (int i = 0; i < 4; ++i, offset += 4) + m_data.m_iQ[i] = XsMessage_getDataLong(&msg, offset); + for (int i = 0; i < 3; ++i, offset += 8) + m_data.m_iV[i] = XsMessage_getDataLongLong(&msg, offset); + for (int i = 0; i < 3; ++i, offset += 4) + m_data.m_mag[i] = XsMessage_getDataLong(&msg, offset); + + m_data.m_baro = XsMessage_getDataLong(&msg, offset); + offset += 4; + m_data.m_accClippingCounter = XsMessage_getDataByte(&msg, offset); + offset += 1; + m_data.m_gyrClippingCounter = XsMessage_getDataByte(&msg, offset); + offset += 1; + m_data.m_status = XsMessage_getDataShort(&msg, offset); + m_data.m_type = ST_Full; + return sz; + } - XsSize sizeInMsg() const override - { - return 70; - } - }; + void writeToMessage(XsMessage& msg, XsSize offset) const override + { + assert(m_data.m_type == ST_Full); + XsMessage_setDataShort(&msg, (uint16_t) m_data.m_frameNumber, offset); + offset += 2; + XsMessage_setDataLongLong(&msg, m_data.m_timestamp, offset); + offset += 8; + for (int i = 0; i < 4; ++i, offset += 4) + XsMessage_setDataLong(&msg, m_data.m_iQ[i], offset); + for (int i = 0; i < 3; ++i, offset += 8) + XsMessage_setDataLongLong(&msg, m_data.m_iV[i], offset); + for (int i = 0; i < 3; ++i, offset += 4) + XsMessage_setDataLong(&msg, m_data.m_mag[i], offset); + + XsMessage_setDataLong(&msg, m_data.m_baro, offset); + offset += 4; + XsMessage_setDataByte(&msg, m_data.m_accClippingCounter, offset); + offset += 1; + XsMessage_setDataByte(&msg, m_data.m_gyrClippingCounter, offset); + offset += 1; + XsMessage_setDataShort(&msg, m_data.m_status, offset); + } - /*! \brief Variant containing an XsAwindaSnapshot value */ - struct XsAwindaSnapshotVariant : public Variant + XsSize sizeInMsg() const override { - /*! \brief Constructor, sets the dataId to \a id */ - XsAwindaSnapshotVariant(XsDataIdentifier id) : Variant(id) - { - m_data.m_type = ST_Awinda; - } - /*! \brief Constructor, sets the dataId to \a id and data value to \a val */ - XsAwindaSnapshotVariant(XsDataIdentifier id, XsSnapshot const& val) : Variant(id), m_data(val) - { - m_data.m_type = ST_Awinda; - } - Variant* clone() const override - { - return new XsAwindaSnapshotVariant(dataId(), m_data); - } + return 70; + } +}; - XsSnapshot m_data; //!< The contained data - XsSize readFromMessage(XsMessage const& msg, XsSize offset, XsSize sz) override - { - m_data.m_deviceId = XsMessage_getDataLong(&msg, offset); offset += 4; - m_data.m_frameNumber = XsMessage_getDataLong(&msg, offset); offset += 4; - for (int i = 0; i < 3; ++i, offset += 4) - m_data.m_iQ[i] = XsMessage_getDataLong(&msg, offset); - for (int i = 0; i < 3; ++i, offset += 4) - m_data.m_iV[i] = (int64_t)(int32_t) XsMessage_getDataLong(&msg, offset); - for (int i = 0; i < 3; ++i, offset += 2) - m_data.m_mag[i] = (int32_t)(int16_t) XsMessage_getDataShort(&msg, offset); - m_data.m_baro = XsMessage_getDataLong(&msg, offset); offset += 4; - m_data.m_status = XsMessage_getDataShort(&msg, offset); offset += 2; - m_data.m_accClippingCounter = XsMessage_getDataByte(&msg, offset); offset += 1; - m_data.m_gyrClippingCounter = XsMessage_getDataByte(&msg, offset); - m_data.m_type = ST_Awinda; - return sz; - } - void writeToMessage(XsMessage& msg, XsSize offset) const override - { - assert(m_data.m_type == ST_Awinda); - XsMessage_setDataLong(&msg, m_data.m_deviceId.legacyDeviceId(), offset); offset += 4; - XsMessage_setDataLong(&msg, m_data.m_frameNumber, offset); offset += 4; - for (int i = 0; i < 3; ++i, offset += 4) - XsMessage_setDataLong(&msg, m_data.m_iQ[i], offset); - for (int i = 0; i < 3; ++i, offset += 4) - XsMessage_setDataLong(&msg, (uint32_t)(int32_t) m_data.m_iV[i], offset); - for (int i = 0; i < 3; ++i, offset += 2) - XsMessage_setDataShort(&msg, (uint16_t)(int16_t) m_data.m_mag[i], offset); - XsMessage_setDataLong(&msg, m_data.m_baro, offset); offset += 4; - XsMessage_setDataShort(&msg, m_data.m_status, offset); offset += 2; - XsMessage_setDataByte(&msg, m_data.m_accClippingCounter, offset); offset += 1; - XsMessage_setDataByte(&msg, m_data.m_gyrClippingCounter, offset); - } +/*! \brief Variant containing an XsAwindaSnapshot value */ +struct XsAwindaSnapshotVariant : public Variant +{ + /*! \brief Constructor, sets the dataId to \a id */ + XsAwindaSnapshotVariant(XsDataIdentifier id) : Variant(id) + { + m_data.m_type = ST_Awinda; + } + /*! \brief Constructor, sets the dataId to \a id and data value to \a val */ + XsAwindaSnapshotVariant(XsDataIdentifier id, XsSnapshot const& val) : Variant(id), m_data(val) + { + m_data.m_type = ST_Awinda; + } + Variant* clone() const override + { + return new XsAwindaSnapshotVariant(dataId(), m_data); + } - XsSize sizeInMsg() const override - { - return 46; - } - }; + XsSnapshot m_data; //!< The contained data + XsSize readFromMessage(XsMessage const& msg, XsSize offset, XsSize sz) override + { + m_data.m_deviceId = XsMessage_getDataLong(&msg, offset); + offset += 4; + m_data.m_frameNumber = XsMessage_getDataLong(&msg, offset); + offset += 4; + for (int i = 0; i < 3; ++i, offset += 4) + m_data.m_iQ[i] = XsMessage_getDataLong(&msg, offset); + for (int i = 0; i < 3; ++i, offset += 4) + m_data.m_iV[i] = (int64_t)(int32_t) XsMessage_getDataLong(&msg, offset); + for (int i = 0; i < 3; ++i, offset += 2) + m_data.m_mag[i] = (int32_t)(int16_t) XsMessage_getDataShort(&msg, offset); + m_data.m_baro = XsMessage_getDataLong(&msg, offset); + offset += 4; + m_data.m_status = XsMessage_getDataShort(&msg, offset); + offset += 2; + m_data.m_accClippingCounter = XsMessage_getDataByte(&msg, offset); + offset += 1; + m_data.m_gyrClippingCounter = XsMessage_getDataByte(&msg, offset); + m_data.m_type = ST_Awinda; + return sz; + } + void writeToMessage(XsMessage& msg, XsSize offset) const override + { + assert(m_data.m_type == ST_Awinda); + XsMessage_setDataLong(&msg, m_data.m_deviceId.legacyDeviceId(), offset); + offset += 4; + XsMessage_setDataLong(&msg, m_data.m_frameNumber, offset); + offset += 4; + for (int i = 0; i < 3; ++i, offset += 4) + XsMessage_setDataLong(&msg, m_data.m_iQ[i], offset); + for (int i = 0; i < 3; ++i, offset += 4) + XsMessage_setDataLong(&msg, (uint32_t)(int32_t) m_data.m_iV[i], offset); + for (int i = 0; i < 3; ++i, offset += 2) + XsMessage_setDataShort(&msg, (uint16_t)(int16_t) m_data.m_mag[i], offset); + XsMessage_setDataLong(&msg, m_data.m_baro, offset); + offset += 4; + XsMessage_setDataShort(&msg, m_data.m_status, offset); + offset += 2; + XsMessage_setDataByte(&msg, m_data.m_accClippingCounter, offset); + offset += 1; + XsMessage_setDataByte(&msg, m_data.m_gyrClippingCounter, offset); + } - /*! \brief Variant containing an XsByteArray value */ - struct XsByteArrayVariant : public Variant + XsSize sizeInMsg() const override { - /*! \brief Constructor, sets the dataId to \a id */ - XsByteArrayVariant(XsDataIdentifier id) : Variant(id) - { - } - /*! \brief Constructor, sets the dataId to \a id and data value to \a val */ - XsByteArrayVariant(XsDataIdentifier id, XsByteArray const& val) : Variant(id), m_data(val) - { - } - Variant* clone() const override - { - return new XsByteArrayVariant(dataId(), m_data); - } + return 46; + } +}; - XsByteArray m_data; //!< The contained data - XsSize readFromMessage(XsMessage const& msg, XsSize offset, XsSize dSize) override +/*! \brief Variant containing an XsByteArray value */ +struct XsByteArrayVariant : public Variant +{ + /*! \brief Constructor, sets the dataId to \a id */ + XsByteArrayVariant(XsDataIdentifier id) : Variant(id) + { + } + /*! \brief Constructor, sets the dataId to \a id and data value to \a val */ + XsByteArrayVariant(XsDataIdentifier id, XsByteArray const& val) : Variant(id), m_data(val) + { + } + Variant* clone() const override + { + return new XsByteArrayVariant(dataId(), m_data); + } + + XsByteArray m_data; //!< The contained data + XsSize readFromMessage(XsMessage const& msg, XsSize offset, XsSize dSize) override + { + if (dSize) { - if (dSize) + if (dSize < 255) + { + m_data.assign(dSize, XsMessage_getDataBuffer(&msg, offset)); + return dSize; + } + else { - if (dSize < 255) + const uint16_t exactId = XsMessage_getDataShort(&msg, offset - 3); + XsSize msgSize = msg.getDataSize(); + XsSize total = 0; + + // look ahead to get total size + XsSize sSize = dSize; + XsSize sOffset = offset; + XsSize tSize = 255; + while (sSize == 255) { - m_data.assign(dSize, XsMessage_getDataBuffer(&msg, offset)); - return dSize; + sOffset += 255; + if (sOffset >= msgSize) + break; + uint16_t nextId = XsMessage_getDataShort(&msg, sOffset); + if (nextId != exactId) + break; + sSize = XsMessage_getDataByte(&msg, sOffset + 2); + sOffset += 3; + tSize += sSize; } - else - { - const uint16_t exactId = XsMessage_getDataShort(&msg, offset-3); - XsSize msgSize = msg.getDataSize(); - XsSize total = 0; - - // look ahead to get total size - XsSize sSize = dSize; - XsSize sOffset = offset; - XsSize tSize = 255; - while (sSize == 255) - { - sOffset += 255; - if (sOffset >= msgSize) - break; - uint16_t nextId = XsMessage_getDataShort(&msg, sOffset); - if (nextId != exactId) - break; - sSize = XsMessage_getDataByte(&msg, sOffset+2); - sOffset += 3; - tSize += sSize; - } - m_data.setSize(tSize); + m_data.setSize(tSize); - sOffset = 0; - while (dSize == 255) + sOffset = 0; + while (dSize == 255) + { + memcpy(m_data.data() + sOffset, XsMessage_getDataBuffer(&msg, offset), 255); + total += 258; + offset += 255; + sOffset += 255; + if (offset >= msgSize) { - memcpy(m_data.data()+sOffset, XsMessage_getDataBuffer(&msg, offset), 255); - total += 258; - offset += 255; - sOffset += 255; - if (offset >= msgSize) - { - dSize = 0; - break; - } - uint16_t nextId = XsMessage_getDataShort(&msg, offset); - if (nextId != exactId) - { - dSize = 0; - break; - } - dSize = XsMessage_getDataByte(&msg, offset+2); - offset += 3; + dSize = 0; + break; } - if (dSize) + uint16_t nextId = XsMessage_getDataShort(&msg, offset); + if (nextId != exactId) { - memcpy(m_data.data()+sOffset, XsMessage_getDataBuffer(&msg, offset), dSize); - total += dSize; + dSize = 0; + break; } - return total; + dSize = XsMessage_getDataByte(&msg, offset + 2); + offset += 3; } - } - else - { - m_data.clear(); - return 0; + if (dSize) + { + memcpy(m_data.data() + sOffset, XsMessage_getDataBuffer(&msg, offset), dSize); + total += dSize; + } + return total; } } - void writeToMessage(XsMessage& msg, XsSize offset) const override + else + { + m_data.clear(); + return 0; + } + } + void writeToMessage(XsMessage& msg, XsSize offset) const override + { + if (m_data.size()) { - if (m_data.size()) + if (m_data.size() < 255) + XsMessage_setDataBuffer(&msg, m_data.data(), m_data.size(), offset); + else { - if (m_data.size() < 255) - XsMessage_setDataBuffer(&msg, m_data.data(), m_data.size(), offset); - else + XsSize dataOffset = 0; + XsSize remainingSize = m_data.size(); + while (remainingSize >= 255) { - XsSize dataOffset = 0; - XsSize remainingSize = m_data.size(); - while (remainingSize >= 255) - { - XsMessage_setDataBuffer(&msg, m_data.data() + dataOffset, 255, offset); - offset += 258; - dataOffset += 255; - remainingSize -= 255; - } - if (remainingSize) - XsMessage_setDataBuffer(&msg, m_data.data() + dataOffset, remainingSize, offset); + XsMessage_setDataBuffer(&msg, m_data.data() + dataOffset, 255, offset); + offset += 258; + dataOffset += 255; + remainingSize -= 255; } + if (remainingSize) + XsMessage_setDataBuffer(&msg, m_data.data() + dataOffset, remainingSize, offset); } } + } - XsSize sizeInMsg() const override - { - return m_data.size(); - } - }; + XsSize sizeInMsg() const override + { + return m_data.size(); + } +}; - /*! \brief Variant containing an XsGloveSnapshotVariant value */ - struct XsGloveSnapshotVariant : public Variant +/*! \brief Variant containing an XsGloveSnapshotVariant value */ +struct XsGloveSnapshotVariant : public Variant +{ + /*! \brief Constructor, sets the dataId to \a id */ + XsGloveSnapshotVariant(XsDataIdentifier id) : Variant(id) { - /*! \brief Constructor, sets the dataId to \a id */ - XsGloveSnapshotVariant(XsDataIdentifier id) : Variant(id) - { - } - /*! \brief Constructor, sets the dataId to \a id and data value to \a val */ - XsGloveSnapshotVariant(XsDataIdentifier id, XsGloveSnapshot const& val) : Variant(id), m_data(val) - { - } - Variant* clone() const override - { - return new XsGloveSnapshotVariant(dataId(), m_data); - } + } + /*! \brief Constructor, sets the dataId to \a id and data value to \a val */ + XsGloveSnapshotVariant(XsDataIdentifier id, XsGloveSnapshot const& val) : Variant(id), m_data(val) + { + } + Variant* clone() const override + { + return new XsGloveSnapshotVariant(dataId(), m_data); + } - XsGloveSnapshot m_data; //!< The contained data - XsSize readFromMessage(XsMessage const& msg, XsSize offset, XsSize dSize) override - { - const uint16_t exactId = XsMessage_getDataShort(&msg, offset - 3); - assert(dSize == 255); - if (dSize != 255) - return 0; - memcpy((uint8_t*)&m_data, XsMessage_getDataBuffer(&msg, offset), 255); - offset += 255; - uint16_t nextId = XsMessage_getDataShort(&msg, offset); - assert(nextId == exactId); - if (nextId != exactId) - return 0; - dSize = XsMessage_getDataByte(&msg, offset + 2); - assert(dSize == 124); - if (dSize != 124) - return 0; - offset += 3; - memcpy(((uint8_t*)&m_data)+255, XsMessage_getDataBuffer(&msg, offset), 124); - - // loop over finger / struct and swap bytes - m_data.m_frameNumber = (uint32_t)swapEndian32(m_data.m_frameNumber); - m_data.m_validSampleFlags = (uint16_t)swapEndian16(m_data.m_validSampleFlags); - - for (int i = 0; i < 12; ++i) + XsGloveSnapshot m_data; //!< The contained data + XsSize readFromMessage(XsMessage const& msg, XsSize offset, XsSize dSize) override + { + const uint16_t exactId = XsMessage_getDataShort(&msg, offset - 3); + assert(dSize == 255); + if (dSize != 255) + return 0; + memcpy((uint8_t*)&m_data, XsMessage_getDataBuffer(&msg, offset), 255); + offset += 255; + uint16_t nextId = XsMessage_getDataShort(&msg, offset); + assert(nextId == exactId); + if (nextId != exactId) + return 0; + dSize = XsMessage_getDataByte(&msg, offset + 2); + assert(dSize == 124); + if (dSize != 124) + return 0; + offset += 3; + memcpy(((uint8_t*)&m_data) + 255, XsMessage_getDataBuffer(&msg, offset), 124); + + // loop over finger / struct and swap bytes + m_data.m_frameNumber = (uint32_t)swapEndian32(m_data.m_frameNumber); + m_data.m_validSampleFlags = (uint16_t)swapEndian16(m_data.m_validSampleFlags); + + for (int i = 0; i < 12; ++i) + { + for (int j = 0; j < 3; ++j) { - for (int j = 0; j < 3; ++j) { - { - auto tmp = m_data.m_fingers[i].m_iV[j]; - m_data.m_fingers[i].m_iV[j] = (int32_t)swapEndian32(tmp); - } - { - auto tmp = m_data.m_fingers[i].m_mag[j]; - m_data.m_fingers[i].m_mag[j] = (int16_t)swapEndian16(tmp); - } + auto tmp = m_data.m_fingers[i].m_iV[j]; + m_data.m_fingers[i].m_iV[j] = (int32_t)swapEndian32(tmp); } { - auto tmp = m_data.m_fingers[i].m_flags; - m_data.m_fingers[i].m_flags = swapEndian16(tmp); + auto tmp = m_data.m_fingers[i].m_mag[j]; + m_data.m_fingers[i].m_mag[j] = (int16_t)swapEndian16(tmp); } } - - return 255+124+3; - } - void writeToMessage(XsMessage& msg, XsSize offset) const override - { - assert(0); //not expected to be called - XsMessage_setDataLong(&msg, m_data.m_frameNumber, offset); + { + auto tmp = m_data.m_fingers[i].m_flags; + m_data.m_fingers[i].m_flags = swapEndian16(tmp); + } } - XsSize sizeInMsg() const override - { - return sizeof(XsGloveSnapshot); - } - }; + return 255 + 124 + 3; + } + void writeToMessage(XsMessage& msg, XsSize offset) const override + { + assert(0); //not expected to be called + XsMessage_setDataLong(&msg, m_data.m_frameNumber, offset); + } - /*! \brief Variant containing an XsGloveDataVariant value */ - struct XsGloveDataVariant : public Variant + XsSize sizeInMsg() const override { - /*! \brief Constructor, sets the dataId to \a id */ - XsGloveDataVariant(XsDataIdentifier id) : Variant(id), m_data() {} - /*! \brief Constructor, sets the dataId to \a id and data value to \a val */ - XsGloveDataVariant(XsDataIdentifier id, XsGloveData const& val) : Variant(id), m_data(val) {} - Variant* clone() const override - { - return new XsGloveDataVariant(dataId(), m_data); - } + return sizeof(XsGloveSnapshot); + } +}; - XsGloveData m_data; //!< The contained data - XsSize readFromMessage(XsMessage const& msg, XsSize offset, XsSize sz) override - { - assert(0); //not expected to be called - XsMessage_getDataLong(&msg, offset); - return sz; - } - void writeToMessage(XsMessage& msg, XsSize offset) const override - { - assert(0); //not expected to be called - XsMessage_setDataLong(&msg, m_data.frameNumber(), offset); - } +/*! \brief Variant containing an XsGloveDataVariant value */ +struct XsGloveDataVariant : public Variant +{ + /*! \brief Constructor, sets the dataId to \a id */ + XsGloveDataVariant(XsDataIdentifier id) : Variant(id), m_data() {} + /*! \brief Constructor, sets the dataId to \a id and data value to \a val */ + XsGloveDataVariant(XsDataIdentifier id, XsGloveData const& val) : Variant(id), m_data(val) {} + Variant* clone() const override + { + return new XsGloveDataVariant(dataId(), m_data); + } - XsSize sizeInMsg() const override - { - return sizeof(XsGloveData); - } - }; + XsGloveData m_data; //!< The contained data + XsSize readFromMessage(XsMessage const& msg, XsSize offset, XsSize sz) override + { + assert(0); //not expected to be called + XsMessage_getDataLong(&msg, offset); + return sz; + } + void writeToMessage(XsMessage& msg, XsSize offset) const override + { + assert(0); //not expected to be called + XsMessage_setDataLong(&msg, m_data.frameNumber(), offset); + } + + XsSize sizeInMsg() const override + { + return sizeof(XsGloveData); + } +}; } typedef std::map MapType; struct DataPacketPrivate : private MapType { - DataPacketPrivate() : m_refCount(1) { ++m_created; } + DataPacketPrivate() : m_refCount(1) + { + ++m_created; + } DataPacketPrivate(DataPacketPrivate const&); ~DataPacketPrivate(); DataPacketPrivate& operator = (const DataPacketPrivate& p); diff --git a/lib/xspublic/xstypes/pstdint.h b/lib/xspublic/xstypes/pstdint.h index 1c35b0d..c347e8e 100644 --- a/lib/xspublic/xstypes/pstdint.h +++ b/lib/xspublic/xstypes/pstdint.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -63,805 +63,805 @@ // /* A portable stdint.h - **************************************************************************** - * BSD License: - **************************************************************************** - * - * Copyright (c) 2005-2007 Paul Hsieh - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * 3. The name of the author may not be used to endorse or promote products - * derived from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR - * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES - * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. - * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT - * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, - * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY - * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF - * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - **************************************************************************** - * - * Version 0.1.11 - * - * The ANSI C standard committee, for the C99 standard, specified the - * inclusion of a new standard include file called stdint.h. This is - * a very useful and long desired include file which contains several - * very precise definitions for integer scalar types that is - * critically important for making portable several classes of - * applications including cryptography, hashing, variable length - * integer libraries and so on. But for most developers its likely - * useful just for programming sanity. - * - * The problem is that most compiler vendors have decided not to - * implement the C99 standard, and the next C++ language standard - * (which has a lot more mindshare these days) will be a long time in - * coming and its unknown whether or not it will include stdint.h or - * how much adoption it will have. Either way, it will be a long time - * before all compilers come with a stdint.h and it also does nothing - * for the extremely large number of compilers available today which - * do not include this file, or anything comparable to it. - * - * So that's what this file is all about. Its an attempt to build a - * single universal include file that works on as many platforms as - * possible to deliver what stdint.h is supposed to. A few things - * that should be noted about this file: - * - * 1) It is not guaranteed to be portable and/or present an identical - * interface on all platforms. The extreme variability of the - * ANSI C standard makes this an impossibility right from the - * very get go. Its really only meant to be useful for the vast - * majority of platforms that possess the capability of - * implementing usefully and precisely defined, standard sized - * integer scalars. Systems which are not intrinsically 2s - * complement may produce invalid constants. - * - * 2) There is an unavoidable use of non-reserved symbols. - * - * 3) Other standard include files are invoked. - * - * 4) This file may come in conflict with future platforms that do - * include stdint.h. The hope is that one or the other can be - * used with no real difference. - * - * 5) In the current verison, if your platform can't represent - * int32_t, int16_t and int8_t, it just dumps out with a compiler - * error. - * - * 6) 64 bit integers may or may not be defined. Test for their - * presence with the test: #ifdef INT64_MAX or #ifdef UINT64_MAX. - * Note that this is different from the C99 specification which - * requires the existence of 64 bit support in the compiler. If - * this is not defined for your platform, yet it is capable of - * dealing with 64 bits then it is because this file has not yet - * been extended to cover all of your system's capabilities. - * - * 7) (u)intptr_t may or may not be defined. Test for its presence - * with the test: #ifdef PTRDIFF_MAX. If this is not defined - * for your platform, then it is because this file has not yet - * been extended to cover all of your system's capabilities, not - * because its optional. - * - * 8) The following might not been defined even if your platform is - * capable of defining it: - * - * WCHAR_MIN - * WCHAR_MAX - * (u)int64_t - * PTRDIFF_MIN - * PTRDIFF_MAX - * (u)intptr_t - * - * 9) The following have not been defined: - * - * WINT_MIN - * WINT_MAX - * - * 10) The criteria for defining (u)int_least(*)_t isn't clear, - * except for systems which don't have a type that precisely - * defined 8, 16, or 32 bit types (which this include file does - * not support anyways). Default definitions have been given. - * - * 11) The criteria for defining (u)int_fast(*)_t isn't something I - * would trust to any particular compiler vendor or the ANSI C - * committee. It is well known that "compatible systems" are - * commonly created that have very different performance - * characteristics from the systems they are compatible with, - * especially those whose vendors make both the compiler and the - * system. Default definitions have been given, but its strongly - * recommended that users never use these definitions for any - * reason (they do *NOT* deliver any serious guarantee of - * improved performance -- not in this file, nor any vendor's - * stdint.h). - * - * 12) The following macros: - * - * PRINTF_INTMAX_MODIFIER - * PRINTF_INT64_MODIFIER - * PRINTF_INT32_MODIFIER - * PRINTF_INT16_MODIFIER - * PRINTF_LEAST64_MODIFIER - * PRINTF_LEAST32_MODIFIER - * PRINTF_LEAST16_MODIFIER - * PRINTF_INTPTR_MODIFIER - * - * are strings which have been defined as the modifiers required - * for the "d", "u" and "x" printf formats to correctly output - * (u)intmax_t, (u)int64_t, (u)int32_t, (u)int16_t, (u)least64_t, - * (u)least32_t, (u)least16_t and (u)intptr_t types respectively. - * PRINTF_INTPTR_MODIFIER is not defined for some systems which - * provide their own stdint.h. PRINTF_INT64_MODIFIER is not - * defined if INT64_MAX is not defined. These are an extension - * beyond what C99 specifies must be in stdint.h. - * - * In addition, the following macros are defined: - * - * PRINTF_INTMAX_HEX_WIDTH - * PRINTF_INT64_HEX_WIDTH - * PRINTF_INT32_HEX_WIDTH - * PRINTF_INT16_HEX_WIDTH - * PRINTF_INT8_HEX_WIDTH - * PRINTF_INTMAX_DEC_WIDTH - * PRINTF_INT64_DEC_WIDTH - * PRINTF_INT32_DEC_WIDTH - * PRINTF_INT16_DEC_WIDTH - * PRINTF_INT8_DEC_WIDTH - * - * Which specifies the maximum number of characters required to - * print the number of that type in either hexadecimal or decimal. - * These are an extension beyond what C99 specifies must be in - * stdint.h. - * - * Compilers tested (all with 0 warnings at their highest respective - * settings): Borland Turbo C 2.0, WATCOM C/C++ 11.0 (16 bits and 32 - * bits), Microsoft Visual C++ 6.0 (32 bit), Microsoft Visual Studio - * .net (VC7), Intel C++ 4.0, GNU gcc v3.3.3 - * - * This file should be considered a work in progress. Suggestions for - * improvements, especially those which increase coverage are strongly - * encouraged. - * - * Acknowledgements - * - * The following people have made significant contributions to the - * development and testing of this file: - * - * Chris Howie - * John Steele Scott - * Dave Thorup - * - */ + **************************************************************************** + BSD License: + **************************************************************************** + + Copyright (c) 2005-2007 Paul Hsieh + All rights reserved. + + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions + are met: + + 1. Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + 2. Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + 3. The name of the author may not be used to endorse or promote products + derived from this software without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR + IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES + OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, + INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF + THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + + **************************************************************************** + + Version 0.1.11 + + The ANSI C standard committee, for the C99 standard, specified the + inclusion of a new standard include file called stdint.h. This is + a very useful and long desired include file which contains several + very precise definitions for integer scalar types that is + critically important for making portable several classes of + applications including cryptography, hashing, variable length + integer libraries and so on. But for most developers its likely + useful just for programming sanity. + + The problem is that most compiler vendors have decided not to + implement the C99 standard, and the next C++ language standard + (which has a lot more mindshare these days) will be a long time in + coming and its unknown whether or not it will include stdint.h or + how much adoption it will have. Either way, it will be a long time + before all compilers come with a stdint.h and it also does nothing + for the extremely large number of compilers available today which + do not include this file, or anything comparable to it. + + So that's what this file is all about. Its an attempt to build a + single universal include file that works on as many platforms as + possible to deliver what stdint.h is supposed to. A few things + that should be noted about this file: + + 1) It is not guaranteed to be portable and/or present an identical + interface on all platforms. The extreme variability of the + ANSI C standard makes this an impossibility right from the + very get go. Its really only meant to be useful for the vast + majority of platforms that possess the capability of + implementing usefully and precisely defined, standard sized + integer scalars. Systems which are not intrinsically 2s + complement may produce invalid constants. + + 2) There is an unavoidable use of non-reserved symbols. + + 3) Other standard include files are invoked. + + 4) This file may come in conflict with future platforms that do + include stdint.h. The hope is that one or the other can be + used with no real difference. + + 5) In the current verison, if your platform can't represent + int32_t, int16_t and int8_t, it just dumps out with a compiler + error. + + 6) 64 bit integers may or may not be defined. Test for their + presence with the test: #ifdef INT64_MAX or #ifdef UINT64_MAX. + Note that this is different from the C99 specification which + requires the existence of 64 bit support in the compiler. If + this is not defined for your platform, yet it is capable of + dealing with 64 bits then it is because this file has not yet + been extended to cover all of your system's capabilities. + + 7) (u)intptr_t may or may not be defined. Test for its presence + with the test: #ifdef PTRDIFF_MAX. If this is not defined + for your platform, then it is because this file has not yet + been extended to cover all of your system's capabilities, not + because its optional. + + 8) The following might not been defined even if your platform is + capable of defining it: + + WCHAR_MIN + WCHAR_MAX + (u)int64_t + PTRDIFF_MIN + PTRDIFF_MAX + (u)intptr_t + + 9) The following have not been defined: + + WINT_MIN + WINT_MAX + + 10) The criteria for defining (u)int_least(*)_t isn't clear, + except for systems which don't have a type that precisely + defined 8, 16, or 32 bit types (which this include file does + not support anyways). Default definitions have been given. + + 11) The criteria for defining (u)int_fast(*)_t isn't something I + would trust to any particular compiler vendor or the ANSI C + committee. It is well known that "compatible systems" are + commonly created that have very different performance + characteristics from the systems they are compatible with, + especially those whose vendors make both the compiler and the + system. Default definitions have been given, but its strongly + recommended that users never use these definitions for any + reason (they do *NOT* deliver any serious guarantee of + improved performance -- not in this file, nor any vendor's + stdint.h). + + 12) The following macros: + + PRINTF_INTMAX_MODIFIER + PRINTF_INT64_MODIFIER + PRINTF_INT32_MODIFIER + PRINTF_INT16_MODIFIER + PRINTF_LEAST64_MODIFIER + PRINTF_LEAST32_MODIFIER + PRINTF_LEAST16_MODIFIER + PRINTF_INTPTR_MODIFIER + + are strings which have been defined as the modifiers required + for the "d", "u" and "x" printf formats to correctly output + (u)intmax_t, (u)int64_t, (u)int32_t, (u)int16_t, (u)least64_t, + (u)least32_t, (u)least16_t and (u)intptr_t types respectively. + PRINTF_INTPTR_MODIFIER is not defined for some systems which + provide their own stdint.h. PRINTF_INT64_MODIFIER is not + defined if INT64_MAX is not defined. These are an extension + beyond what C99 specifies must be in stdint.h. + + In addition, the following macros are defined: + + PRINTF_INTMAX_HEX_WIDTH + PRINTF_INT64_HEX_WIDTH + PRINTF_INT32_HEX_WIDTH + PRINTF_INT16_HEX_WIDTH + PRINTF_INT8_HEX_WIDTH + PRINTF_INTMAX_DEC_WIDTH + PRINTF_INT64_DEC_WIDTH + PRINTF_INT32_DEC_WIDTH + PRINTF_INT16_DEC_WIDTH + PRINTF_INT8_DEC_WIDTH + + Which specifies the maximum number of characters required to + print the number of that type in either hexadecimal or decimal. + These are an extension beyond what C99 specifies must be in + stdint.h. + + Compilers tested (all with 0 warnings at their highest respective + settings): Borland Turbo C 2.0, WATCOM C/C++ 11.0 (16 bits and 32 + bits), Microsoft Visual C++ 6.0 (32 bit), Microsoft Visual Studio + .net (VC7), Intel C++ 4.0, GNU gcc v3.3.3 + + This file should be considered a work in progress. Suggestions for + improvements, especially those which increase coverage are strongly + encouraged. + + Acknowledgements + + The following people have made significant contributions to the + development and testing of this file: + + Chris Howie + John Steele Scott + Dave Thorup + +*/ #ifndef _PSTDINT_H_INCLUDED #include #include /* - * For gcc with _STDINT_H, fill in the PRINTF_INT*_MODIFIER macros, and - * do nothing else. On the Mac OS X version of gcc this is _STDINT_H_. - */ + For gcc with _STDINT_H, fill in the PRINTF_INT*_MODIFIER macros, and + do nothing else. On the Mac OS X version of gcc this is _STDINT_H_. +*/ #if (defined(__STDC__) && __STDC__ && defined(__STDC_VERSION__) && __STDC_VERSION__ >= 199901L) || (defined (__WATCOMC__) && __WATCOMC__ >= 1250) || (defined(__GNUC__) && __GNUC__ > 2) || (defined(_MSC_VER) && _MSC_VER >= 1600) || defined(IAR_ARM_CM3) || (defined(__ICCARM__) && __ICCARM__) || (defined(_ADI_COMPILER) && _ADI_COMPILER) || (defined(__ICC8051__) && __ICC8051__) || (defined(__arm__) && defined(__ARMCC_VERSION)) -#include + #include #endif #if (defined (_STDINT_H_INCLUDED) || defined (_STDINT) || defined (_STDINT_H) || defined (_STDINT_H_) || defined (BOOST_CSTDINT_HPP)) && !defined (_PSTDINT_H_INCLUDED) || defined(__stdint_h) -#define _PSTDINT_H_INCLUDED -# ifndef PRINTF_INT64_MODIFIER -# if (__WORDSIZE == 64) -# if defined(__APPLE__) -# define PRINTF_INT64_MODIFIER "ll" -# else -# define PRINTF_INT64_MODIFIER "l" -# endif -# else -# define PRINTF_INT64_MODIFIER "ll" -# endif -# endif -# ifndef PRINTF_INT32_MODIFIER -# if defined(__APPLE__) -# define PRINTF_INT32_MODIFIER "" -# elif defined(__ARMEL__) && !defined(__linux) -# define PRINTF_INT32_MODIFIER "l" -# else -# define PRINTF_INT32_MODIFIER "" -# endif -# endif -# ifndef PRINTF_INT16_MODIFIER -# define PRINTF_INT16_MODIFIER "h" -# endif -# ifndef PRINTF_INTMAX_MODIFIER -# define PRINTF_INTMAX_MODIFIER PRINTF_INT64_MODIFIER -# endif -# ifndef PRINTF_INT64_HEX_WIDTH -# define PRINTF_INT64_HEX_WIDTH "16" -# endif -# ifndef PRINTF_INT32_HEX_WIDTH -# define PRINTF_INT32_HEX_WIDTH "8" -# endif -# ifndef PRINTF_INT16_HEX_WIDTH -# define PRINTF_INT16_HEX_WIDTH "4" -# endif -# ifndef PRINTF_INT8_HEX_WIDTH -# define PRINTF_INT8_HEX_WIDTH "2" -# endif -# ifndef PRINTF_INT64_DEC_WIDTH -# define PRINTF_INT64_DEC_WIDTH "20" -# endif -# ifndef PRINTF_INT32_DEC_WIDTH -# define PRINTF_INT32_DEC_WIDTH "10" -# endif -# ifndef PRINTF_INT16_DEC_WIDTH -# define PRINTF_INT16_DEC_WIDTH "5" -# endif -# ifndef PRINTF_INT8_DEC_WIDTH -# define PRINTF_INT8_DEC_WIDTH "3" -# endif -# ifndef PRINTF_INTMAX_HEX_WIDTH -# define PRINTF_INTMAX_HEX_WIDTH PRINTF_INT64_HEX_WIDTH -# endif -# ifndef PRINTF_INTMAX_DEC_WIDTH -# define PRINTF_INTMAX_DEC_WIDTH PRINTF_INT64_DEC_WIDTH -# endif - -/* - * Something really weird is going on with Open Watcom. Just pull some of - * these duplicated definitions from Open Watcom's stdint.h file for now. - */ - -# if defined (__WATCOMC__) && __WATCOMC__ >= 1250 -# if !defined (INT64_C) -# define INT64_C(x) (x + (INT64_MAX - INT64_MAX)) -# endif -# if !defined (UINT64_C) -# define UINT64_C(x) (x + (UINT64_MAX - UINT64_MAX)) -# endif -# if !defined (INT32_C) -# define INT32_C(x) (x + (INT32_MAX - INT32_MAX)) -# endif -# if !defined (UINT32_C) -# define UINT32_C(x) (x + (UINT32_MAX - UINT32_MAX)) -# endif -# if !defined (INT16_C) -# define INT16_C(x) (x) -# endif -# if !defined (UINT16_C) -# define UINT16_C(x) (x) -# endif -# if !defined (INT8_C) -# define INT8_C(x) (x) -# endif -# if !defined (UINT8_C) -# define UINT8_C(x) (x) -# endif -# if !defined (UINT64_MAX) -# define UINT64_MAX 18446744073709551615ULL -# endif -# if !defined (INT64_MAX) -# define INT64_MAX 9223372036854775807LL -# endif -# if !defined (UINT32_MAX) -# define UINT32_MAX 4294967295UL -# endif -# if !defined (INT32_MAX) -# define INT32_MAX 2147483647L -# endif -# if !defined (INTMAX_MAX) -# define INTMAX_MAX INT64_MAX -# endif -# if !defined (INTMAX_MIN) -# define INTMAX_MIN INT64_MIN -# endif -# endif - -/* Some of the targets compiled by Visual Dsp++ (like the Sharc) do not have a notion of 8bit of 16bit value + #define _PSTDINT_H_INCLUDED + #ifndef PRINTF_INT64_MODIFIER + #if (__WORDSIZE == 64) + #if defined(__APPLE__) + #define PRINTF_INT64_MODIFIER "ll" + #else + #define PRINTF_INT64_MODIFIER "l" + #endif + #else + #define PRINTF_INT64_MODIFIER "ll" + #endif + #endif + #ifndef PRINTF_INT32_MODIFIER + #if defined(__APPLE__) + #define PRINTF_INT32_MODIFIER "" + #elif defined(__ARMEL__) && !defined(__linux) + #define PRINTF_INT32_MODIFIER "l" + #else + #define PRINTF_INT32_MODIFIER "" + #endif + #endif + #ifndef PRINTF_INT16_MODIFIER + #define PRINTF_INT16_MODIFIER "h" + #endif + #ifndef PRINTF_INTMAX_MODIFIER + #define PRINTF_INTMAX_MODIFIER PRINTF_INT64_MODIFIER + #endif + #ifndef PRINTF_INT64_HEX_WIDTH + #define PRINTF_INT64_HEX_WIDTH "16" + #endif + #ifndef PRINTF_INT32_HEX_WIDTH + #define PRINTF_INT32_HEX_WIDTH "8" + #endif + #ifndef PRINTF_INT16_HEX_WIDTH + #define PRINTF_INT16_HEX_WIDTH "4" + #endif + #ifndef PRINTF_INT8_HEX_WIDTH + #define PRINTF_INT8_HEX_WIDTH "2" + #endif + #ifndef PRINTF_INT64_DEC_WIDTH + #define PRINTF_INT64_DEC_WIDTH "20" + #endif + #ifndef PRINTF_INT32_DEC_WIDTH + #define PRINTF_INT32_DEC_WIDTH "10" + #endif + #ifndef PRINTF_INT16_DEC_WIDTH + #define PRINTF_INT16_DEC_WIDTH "5" + #endif + #ifndef PRINTF_INT8_DEC_WIDTH + #define PRINTF_INT8_DEC_WIDTH "3" + #endif + #ifndef PRINTF_INTMAX_HEX_WIDTH + #define PRINTF_INTMAX_HEX_WIDTH PRINTF_INT64_HEX_WIDTH + #endif + #ifndef PRINTF_INTMAX_DEC_WIDTH + #define PRINTF_INTMAX_DEC_WIDTH PRINTF_INT64_DEC_WIDTH + #endif + + /* + Something really weird is going on with Open Watcom. Just pull some of + these duplicated definitions from Open Watcom's stdint.h file for now. + */ + + #if defined (__WATCOMC__) && __WATCOMC__ >= 1250 + #if !defined (INT64_C) + #define INT64_C(x) (x + (INT64_MAX - INT64_MAX)) + #endif + #if !defined (UINT64_C) + #define UINT64_C(x) (x + (UINT64_MAX - UINT64_MAX)) + #endif + #if !defined (INT32_C) + #define INT32_C(x) (x + (INT32_MAX - INT32_MAX)) + #endif + #if !defined (UINT32_C) + #define UINT32_C(x) (x + (UINT32_MAX - UINT32_MAX)) + #endif + #if !defined (INT16_C) + #define INT16_C(x) (x) + #endif + #if !defined (UINT16_C) + #define UINT16_C(x) (x) + #endif + #if !defined (INT8_C) + #define INT8_C(x) (x) + #endif + #if !defined (UINT8_C) + #define UINT8_C(x) (x) + #endif + #if !defined (UINT64_MAX) + #define UINT64_MAX 18446744073709551615ULL + #endif + #if !defined (INT64_MAX) + #define INT64_MAX 9223372036854775807LL + #endif + #if !defined (UINT32_MAX) + #define UINT32_MAX 4294967295UL + #endif + #if !defined (INT32_MAX) + #define INT32_MAX 2147483647L + #endif + #if !defined (INTMAX_MAX) + #define INTMAX_MAX INT64_MAX + #endif + #if !defined (INTMAX_MIN) + #define INTMAX_MIN INT64_MIN + #endif + #endif + + /* Some of the targets compiled by Visual Dsp++ (like the Sharc) do not have a notion of 8bit of 16bit value We define these here although the actual storage size is 32 bits -*/ -#if (defined(_ADI_COMPILER) && _ADI_COMPILER) -# ifndef int8_t - typedef signed char int8_t; -# endif -# ifndef uint8_t - typedef unsigned char uint8_t; -# endif -# ifndef int16_t - typedef signed short int16_t; -# endif -# ifndef uint16_t - typedef unsigned short uint16_t; -# endif -#endif //(defined(_ADI_COMPILER) && _ADI_COMPILER) + */ + #if (defined(_ADI_COMPILER) && _ADI_COMPILER) + #ifndef int8_t + typedef signed char int8_t; + #endif + #ifndef uint8_t + typedef unsigned char uint8_t; + #endif + #ifndef int16_t + typedef signed short int16_t; + #endif + #ifndef uint16_t + typedef unsigned short uint16_t; + #endif + #endif //(defined(_ADI_COMPILER) && _ADI_COMPILER) #endif #ifndef _PSTDINT_H_INCLUDED -#define _PSTDINT_H_INCLUDED - -#ifndef SIZE_MAX -#ifdef DOXYGEN -# define SIZE_MAX (~0) -#else -# define SIZE_MAX (~(size_t)0) -#endif -#endif - -/* - * Deduce the type assignments from limits.h under the assumption that - * integer sizes in bits are powers of 2, and follow the ANSI - * definitions. - */ - -#ifndef UINT8_MAX -# define UINT8_MAX 0xff -#endif -#ifndef uint8_t -# if (UCHAR_MAX == UINT8_MAX) || defined (S_SPLINT_S) - typedef unsigned char uint8_t; -# define UINT8_C(v) ((uint8_t) v) -# else -# error "Platform not supported" -# endif -#endif - -#ifndef INT8_MAX -# define INT8_MAX 0x7f -#endif -#ifndef INT8_MIN -# define INT8_MIN INT8_C(0x80) -#endif -#ifndef int8_t -# if (SCHAR_MAX == INT8_MAX) || defined (S_SPLINT_S) - typedef signed char int8_t; -# define INT8_C(v) ((int8_t) v) -# else -# error "Platform not supported" -# endif -#endif - -#ifndef UINT16_MAX -# define UINT16_MAX 0xffff -#endif -#ifndef uint16_t -#if (UINT_MAX == UINT16_MAX) || defined (S_SPLINT_S) - typedef unsigned int uint16_t; -# ifndef PRINTF_INT16_MODIFIER -# define PRINTF_INT16_MODIFIER "" -# endif -# define UINT16_C(v) ((uint16_t) (v)) -#elif (USHRT_MAX == UINT16_MAX) - typedef unsigned short uint16_t; -# define UINT16_C(v) ((uint16_t) (v)) -# ifndef PRINTF_INT16_MODIFIER -# define PRINTF_INT16_MODIFIER "h" -# endif -#else -#error "Platform not supported" -#endif -#endif - -#ifndef INT16_MAX -# define INT16_MAX 0x7fff -#endif -#ifndef INT16_MIN -# define INT16_MIN INT16_C(0x8000) -#endif -#ifndef int16_t -#if (INT_MAX == INT16_MAX) || defined (S_SPLINT_S) - typedef signed int int16_t; -# define INT16_C(v) ((int16_t) (v)) -# ifndef PRINTF_INT16_MODIFIER -# define PRINTF_INT16_MODIFIER "" -# endif -#elif (SHRT_MAX == INT16_MAX) - typedef signed short int16_t; -# define INT16_C(v) ((int16_t) (v)) -# ifndef PRINTF_INT16_MODIFIER -# define PRINTF_INT16_MODIFIER "h" -# endif -#else -#error "Platform not supported" -#endif -#endif - -#ifndef UINT32_MAX -# define UINT32_MAX (0xffffffffUL) -#endif -#ifndef uint32_t -#if (SIZE_MAX == UINT32_MAX) && !defined (S_SPLINT_S) && !defined(_lint) -typedef unsigned int uint32_t; -# ifndef PRINTF_INT32_MODIFIER -# define PRINTF_INT32_MODIFIER "" -# endif -# define UINT32_C(v) v ## U -#elif (ULONG_MAX == UINT32_MAX) || defined (S_SPLINT_S) - typedef unsigned long uint32_t; -# define UINT32_C(v) v ## UL -# ifndef PRINTF_INT32_MODIFIER -# define PRINTF_INT32_MODIFIER "l" -# endif -#elif (USHRT_MAX == UINT32_MAX) - typedef unsigned short uint32_t; -# define UINT32_C(v) ((unsigned short) (v)) -# ifndef PRINTF_INT32_MODIFIER -# define PRINTF_INT32_MODIFIER "" -# endif -#else -#error "Platform not supported" -#endif -#endif - -#ifndef INT32_MAX -# define INT32_MAX (0x7fffffffL) -#endif -#ifndef INT32_MIN -# define INT32_MIN INT32_C(0x80000000) -#endif -#ifndef int32_t -#if (SIZE_MAX/2 == INT32_MAX) && defined (__GNUC__) && (__GNUC__ > 3) - typedef signed int int32_t; -# define INT32_C(v) v -# ifndef PRINTF_INT32_MODIFIER -# define PRINTF_INT32_MODIFIER "" -# endif -#elif (LONG_MAX == INT32_MAX) || defined (S_SPLINT_S) - typedef signed long int32_t; -# define INT32_C(v) v ## L -# ifndef PRINTF_INT32_MODIFIER -# define PRINTF_INT32_MODIFIER "l" -# endif -#elif (SIZE_MAX/2 == INT32_MAX) - typedef signed int int32_t; -# define INT32_C(v) v -# ifndef PRINTF_INT32_MODIFIER -# define PRINTF_INT32_MODIFIER "" -# endif -#elif (SHRT_MAX == INT32_MAX) - typedef signed short int32_t; -# define INT32_C(v) ((short) (v)) -# ifndef PRINTF_INT32_MODIFIER -# define PRINTF_INT32_MODIFIER "" -# endif -#else -#error "Platform not supported" -#endif -#endif - -/* - * The macro stdint_int64_defined is temporarily used to record - * whether or not 64 integer support is available. It must be - * defined for any 64 integer extensions for new platforms that are - * added. - */ - -#undef stdint_int64_defined -#if (defined(__STDC__) && defined(__STDC_VERSION__)) || defined (S_SPLINT_S) -# if (__STDC__ && __STDC_VERSION >= 199901L) || defined (S_SPLINT_S) -# define stdint_int64_defined - typedef long long int64_t; - typedef unsigned long long uint64_t; -# define UINT64_C(v) v ## ULL -# define INT64_C(v) v ## LL -# ifndef PRINTF_INT64_MODIFIER -# define PRINTF_INT64_MODIFIER "ll" -# endif -# endif -#endif - -#if !defined (stdint_int64_defined) -# if defined(__GNUC__) && __WORDSIZE == 64 -# define stdint_int64_defined - __extension__ typedef long int64_t; - __extension__ typedef unsigned long uint64_t; -# define UINT64_C(v) v ## UL -# define INT64_C(v) v ## L -# ifndef PRINTF_INT64_MODIFIER -# define PRINTF_INT64_MODIFIER "l" -# endif -#elif defined(__GNUC__) -# define stdint_int64_defined - __extension__ typedef long long int64_t; - __extension__ typedef unsigned long long uint64_t; -# define UINT64_C(v) v ## ULL -# define INT64_C(v) v ## LL -# ifndef PRINTF_INT64_MODIFIER -# define PRINTF_INT64_MODIFIER "ll" -# endif -# elif defined(__MWERKS__) || defined (__SUNPRO_C) || defined (__SUNPRO_CC) || defined (__APPLE_CC__) || defined (_LONG_LONG) || defined (_CRAYC) || defined (S_SPLINT_S) -# define stdint_int64_defined - typedef long long int64_t; - typedef unsigned long long uint64_t; -# define UINT64_C(v) v ## ULL -# define INT64_C(v) v ## LL -# ifndef PRINTF_INT64_MODIFIER -# define PRINTF_INT64_MODIFIER "ll" -# endif -# elif (defined(__WATCOMC__) && defined(__WATCOM_INT64__)) || (defined(_MSC_VER) && _INTEGRAL_MAX_BITS >= 64) || (defined (__BORLANDC__) && __BORLANDC__ > 0x460) || defined (__alpha) || defined (__DECC) -# define stdint_int64_defined - typedef __int64 int64_t; - typedef unsigned __int64 uint64_t; -# define UINT64_C(v) v ## UI64 -# define INT64_C(v) v ## I64 -# ifndef PRINTF_INT64_MODIFIER -# define PRINTF_INT64_MODIFIER "I64" -# endif -# endif -#endif - -#if !defined (LONG_LONG_MAX) && defined (INT64_C) -# define LONG_LONG_MAX INT64_C (9223372036854775807LL) -#endif -#ifndef ULONG_LONG_MAX -# define ULONG_LONG_MAX UINT64_C (18446744073709551615ULL) -#endif - -#if !defined (INT64_MAX) && defined (INT64_C) -# define INT64_MAX INT64_C (9223372036854775807LL) -#endif -#if !defined (INT64_MIN) && defined (INT64_C) -# define INT64_MIN INT64_C (-9223372036854775808LL) -#endif -#if !defined (UINT64_MAX) && defined (INT64_C) -# define UINT64_MAX UINT64_C (18446744073709551615ULL) -#endif - -/* - * Width of hexadecimal for number field. - */ - -#ifndef PRINTF_INT64_HEX_WIDTH -# define PRINTF_INT64_HEX_WIDTH "16" -#endif -#ifndef PRINTF_INT32_HEX_WIDTH -# define PRINTF_INT32_HEX_WIDTH "8" -#endif -#ifndef PRINTF_INT16_HEX_WIDTH -# define PRINTF_INT16_HEX_WIDTH "4" -#endif -#ifndef PRINTF_INT8_HEX_WIDTH -# define PRINTF_INT8_HEX_WIDTH "2" -#endif - -#ifndef PRINTF_INT64_DEC_WIDTH -# define PRINTF_INT64_DEC_WIDTH "20" -#endif -#ifndef PRINTF_INT32_DEC_WIDTH -# define PRINTF_INT32_DEC_WIDTH "10" -#endif -#ifndef PRINTF_INT16_DEC_WIDTH -# define PRINTF_INT16_DEC_WIDTH "5" -#endif -#ifndef PRINTF_INT8_DEC_WIDTH -# define PRINTF_INT8_DEC_WIDTH "3" -#endif - -/* - * Ok, lets not worry about 128 bit integers for now. Moore's law says - * we don't need to worry about that until about 2040 at which point - * we'll have bigger things to worry about. - */ - -#ifdef stdint_int64_defined - typedef int64_t intmax_t; - typedef uint64_t uintmax_t; -# define INTMAX_MAX INT64_MAX -# define INTMAX_MIN INT64_MIN -# define UINTMAX_MAX UINT64_MAX -# define UINTMAX_C(v) UINT64_C(v) -# define INTMAX_C(v) INT64_C(v) -# ifndef PRINTF_INTMAX_MODIFIER -# define PRINTF_INTMAX_MODIFIER PRINTF_INT64_MODIFIER -# endif -# ifndef PRINTF_INTMAX_HEX_WIDTH -# define PRINTF_INTMAX_HEX_WIDTH PRINTF_INT64_HEX_WIDTH -# endif -# ifndef PRINTF_INTMAX_DEC_WIDTH -# define PRINTF_INTMAX_DEC_WIDTH PRINTF_INT64_DEC_WIDTH -# endif -#else - typedef int32_t intmax_t; - typedef uint32_t uintmax_t; -# define INTMAX_MAX INT32_MAX -# define UINTMAX_MAX UINT32_MAX -# define UINTMAX_C(v) UINT32_C(v) -# define INTMAX_C(v) INT32_C(v) -# ifndef PRINTF_INTMAX_MODIFIER -# define PRINTF_INTMAX_MODIFIER PRINTF_INT32_MODIFIER -# endif -# ifndef PRINTF_INTMAX_HEX_WIDTH -# define PRINTF_INTMAX_HEX_WIDTH PRINTF_INT32_HEX_WIDTH -# endif -# ifndef PRINTF_INTMAX_DEC_WIDTH -# define PRINTF_INTMAX_DEC_WIDTH PRINTF_INT32_DEC_WIDTH -# endif -#endif - -/* - * Because this file currently only supports platforms which have - * precise powers of 2 as bit sizes for the default integers, the - * least definitions are all trivial. Its possible that a future - * version of this file could have different definitions. - */ - -#ifndef stdint_least_defined - typedef int8_t int_least8_t; - typedef uint8_t uint_least8_t; - typedef int16_t int_least16_t; - typedef uint16_t uint_least16_t; - typedef int32_t int_least32_t; - typedef uint32_t uint_least32_t; -# define PRINTF_LEAST32_MODIFIER PRINTF_INT32_MODIFIER -# define PRINTF_LEAST16_MODIFIER PRINTF_INT16_MODIFIER -# define UINT_LEAST8_MAX UINT8_MAX -# define INT_LEAST8_MAX INT8_MAX -# define UINT_LEAST16_MAX UINT16_MAX -# define INT_LEAST16_MAX INT16_MAX -# define UINT_LEAST32_MAX UINT32_MAX -# define INT_LEAST32_MAX INT32_MAX -# define INT_LEAST8_MIN INT8_MIN -# define INT_LEAST16_MIN INT16_MIN -# define INT_LEAST32_MIN INT32_MIN -# ifdef stdint_int64_defined - typedef int64_t int_least64_t; - typedef uint64_t uint_least64_t; -# define PRINTF_LEAST64_MODIFIER PRINTF_INT64_MODIFIER -# define UINT_LEAST64_MAX UINT64_MAX -# define INT_LEAST64_MAX INT64_MAX -# define INT_LEAST64_MIN INT64_MIN -# endif -#endif -#undef stdint_least_defined - -/* - * The ANSI C committee pretending to know or specify anything about - * performance is the epitome of misguided arrogance. The mandate of - * this file is to *ONLY* ever support that absolute minimum - * definition of the fast integer types, for compatibility purposes. - * No extensions, and no attempt to suggest what may or may not be a - * faster integer type will ever be made in this file. Developers are - * warned to stay away from these types when using this or any other - * stdint.h. - */ - -#ifndef int_fast8_t -typedef int_least8_t int_fast8_t; -#endif -#ifndef uint_fast8_t -typedef uint_least8_t uint_fast8_t; -#endif -#ifndef int_fast16_t -typedef int_least16_t int_fast16_t; -#endif -#ifndef uint_fast16_t -typedef uint_least16_t uint_fast16_t; -#endif -#ifndef int_fast32_t -typedef int_least32_t int_fast32_t; -#endif -#ifndef uint_fast32_t -typedef uint_least32_t uint_fast32_t; -#endif -#define UINT_FAST8_MAX UINT_LEAST8_MAX -#define INT_FAST8_MAX INT_LEAST8_MAX -#define UINT_FAST16_MAX UINT_LEAST16_MAX -#define INT_FAST16_MAX INT_LEAST16_MAX -#define UINT_FAST32_MAX UINT_LEAST32_MAX -#define INT_FAST32_MAX INT_LEAST32_MAX -#define INT_FAST8_MIN INT_LEAST8_MIN -#define INT_FAST16_MIN INT_LEAST16_MIN -#define INT_FAST32_MIN INT_LEAST32_MIN -#ifdef stdint_int64_defined - typedef int_least64_t int_fast64_t; - typedef uint_least64_t uint_fast64_t; -# define UINT_FAST64_MAX UINT_LEAST64_MAX -# define INT_FAST64_MAX INT_LEAST64_MAX -# define INT_FAST64_MIN INT_LEAST64_MIN -#endif - -/* - * Whatever piecemeal, per compiler thing we can do about the wchar_t - * type limits. - */ - -#if defined(__WATCOMC__) || defined(_MSC_VER) || defined (__GNUC__) -# include -# ifndef WCHAR_MIN -# define WCHAR_MIN 0 -# endif -# ifndef WCHAR_MAX -# define WCHAR_MAX ((wchar_t)-1) -# endif -#endif - -/* - * Whatever piecemeal, per compiler/platform thing we can do about the - * (u)intptr_t types and limits. - */ - -#if defined (_MSC_VER) && defined (_UINTPTR_T_DEFINED) -# define STDINT_H_UINTPTR_T_DEFINED -#endif - -#ifndef STDINT_H_UINTPTR_T_DEFINED -# if defined (__alpha__) || defined (__ia64__) || defined (__x86_64__) || defined (_WIN64) -# define stdint_intptr_bits 64 -# elif defined (__WATCOMC__) || defined (__TURBOC__) -# if defined(__TINY__) || defined(__SMALL__) || defined(__MEDIUM__) -# define stdint_intptr_bits 16 -# else -# define stdint_intptr_bits 32 -# endif -# elif defined (__i386__) || defined (_WIN32) || defined (WIN32) -# define stdint_intptr_bits 32 -# elif defined (__INTEL_COMPILER) -/* TODO -- what will Intel do about x86-64? */ -# endif - -# ifdef stdint_intptr_bits -# define stdint_intptr_glue3_i(a,b,c) a##b##c -# define stdint_intptr_glue3(a,b,c) stdint_intptr_glue3_i(a,b,c) -# ifndef PRINTF_INTPTR_MODIFIER -# define PRINTF_INTPTR_MODIFIER stdint_intptr_glue3(PRINTF_INT,stdint_intptr_bits,_MODIFIER) -# endif -# ifndef PTRDIFF_MAX -# define PTRDIFF_MAX stdint_intptr_glue3(INT,stdint_intptr_bits,_MAX) -# endif -# ifndef PTRDIFF_MIN -# define PTRDIFF_MIN stdint_intptr_glue3(INT,stdint_intptr_bits,_MIN) -# endif -# ifndef UINTPTR_MAX -# define UINTPTR_MAX stdint_intptr_glue3(UINT,stdint_intptr_bits,_MAX) -# endif -# ifndef INTPTR_MAX -# define INTPTR_MAX stdint_intptr_glue3(INT,stdint_intptr_bits,_MAX) -# endif -# ifndef INTPTR_MIN -# define INTPTR_MIN stdint_intptr_glue3(INT,stdint_intptr_bits,_MIN) -# endif -# ifndef INTPTR_C -# define INTPTR_C(x) stdint_intptr_glue3(INT,stdint_intptr_bits,_C)(x) -# endif -# ifndef UINTPTR_C -# define UINTPTR_C(x) stdint_intptr_glue3(UINT,stdint_intptr_bits,_C)(x) -# endif - typedef stdint_intptr_glue3(uint,stdint_intptr_bits,_t) uintptr_t; - typedef stdint_intptr_glue3( int,stdint_intptr_bits,_t) intptr_t; -# else -/* TODO -- This following is likely wrong for some platforms, and does - nothing for the definition of uintptr_t. */ - typedef ptrdiff_t intptr_t; -# endif -# define STDINT_H_UINTPTR_T_DEFINED -#endif - -/* - * Assumes sig_atomic_t is signed and we have a 2s complement machine. - */ - -#ifndef SIG_ATOMIC_MAX -# define SIG_ATOMIC_MAX ((((sig_atomic_t) 1) << (sizeof (sig_atomic_t)*CHAR_BIT-1)) - 1) -#endif + #define _PSTDINT_H_INCLUDED + + #ifndef SIZE_MAX + #ifdef DOXYGEN + #define SIZE_MAX (~0) + #else + #define SIZE_MAX (~(size_t)0) + #endif + #endif + + /* + Deduce the type assignments from limits.h under the assumption that + integer sizes in bits are powers of 2, and follow the ANSI + definitions. + */ + + #ifndef UINT8_MAX + #define UINT8_MAX 0xff + #endif + #ifndef uint8_t + #if (UCHAR_MAX == UINT8_MAX) || defined (S_SPLINT_S) + typedef unsigned char uint8_t; + #define UINT8_C(v) ((uint8_t) v) + #else + # error "Platform not supported" + #endif + #endif + + #ifndef INT8_MAX + #define INT8_MAX 0x7f + #endif + #ifndef INT8_MIN + #define INT8_MIN INT8_C(0x80) + #endif + #ifndef int8_t + #if (SCHAR_MAX == INT8_MAX) || defined (S_SPLINT_S) + typedef signed char int8_t; + #define INT8_C(v) ((int8_t) v) + #else + # error "Platform not supported" + #endif + #endif + + #ifndef UINT16_MAX + #define UINT16_MAX 0xffff + #endif + #ifndef uint16_t + #if (UINT_MAX == UINT16_MAX) || defined (S_SPLINT_S) + typedef unsigned int uint16_t; + #ifndef PRINTF_INT16_MODIFIER + #define PRINTF_INT16_MODIFIER "" + #endif + #define UINT16_C(v) ((uint16_t) (v)) + #elif (USHRT_MAX == UINT16_MAX) + typedef unsigned short uint16_t; + #define UINT16_C(v) ((uint16_t) (v)) + #ifndef PRINTF_INT16_MODIFIER + #define PRINTF_INT16_MODIFIER "h" + #endif + #else + #error "Platform not supported" + #endif + #endif + + #ifndef INT16_MAX + #define INT16_MAX 0x7fff + #endif + #ifndef INT16_MIN + #define INT16_MIN INT16_C(0x8000) + #endif + #ifndef int16_t + #if (INT_MAX == INT16_MAX) || defined (S_SPLINT_S) + typedef signed int int16_t; + #define INT16_C(v) ((int16_t) (v)) + #ifndef PRINTF_INT16_MODIFIER + #define PRINTF_INT16_MODIFIER "" + #endif + #elif (SHRT_MAX == INT16_MAX) + typedef signed short int16_t; + #define INT16_C(v) ((int16_t) (v)) + #ifndef PRINTF_INT16_MODIFIER + #define PRINTF_INT16_MODIFIER "h" + #endif + #else + #error "Platform not supported" + #endif + #endif + + #ifndef UINT32_MAX + #define UINT32_MAX (0xffffffffUL) + #endif + #ifndef uint32_t + #if (SIZE_MAX == UINT32_MAX) && !defined (S_SPLINT_S) && !defined(_lint) + typedef unsigned int uint32_t; + #ifndef PRINTF_INT32_MODIFIER + #define PRINTF_INT32_MODIFIER "" + #endif + #define UINT32_C(v) v ## U + #elif (ULONG_MAX == UINT32_MAX) || defined (S_SPLINT_S) + typedef unsigned long uint32_t; + #define UINT32_C(v) v ## UL + #ifndef PRINTF_INT32_MODIFIER + #define PRINTF_INT32_MODIFIER "l" + #endif + #elif (USHRT_MAX == UINT32_MAX) + typedef unsigned short uint32_t; + #define UINT32_C(v) ((unsigned short) (v)) + #ifndef PRINTF_INT32_MODIFIER + #define PRINTF_INT32_MODIFIER "" + #endif + #else + #error "Platform not supported" + #endif + #endif + + #ifndef INT32_MAX + #define INT32_MAX (0x7fffffffL) + #endif + #ifndef INT32_MIN + #define INT32_MIN INT32_C(0x80000000) + #endif + #ifndef int32_t + #if (SIZE_MAX/2 == INT32_MAX) && defined (__GNUC__) && (__GNUC__ > 3) + typedef signed int int32_t; + #define INT32_C(v) v + #ifndef PRINTF_INT32_MODIFIER + #define PRINTF_INT32_MODIFIER "" + #endif + #elif (LONG_MAX == INT32_MAX) || defined (S_SPLINT_S) + typedef signed long int32_t; + #define INT32_C(v) v ## L + #ifndef PRINTF_INT32_MODIFIER + #define PRINTF_INT32_MODIFIER "l" + #endif + #elif (SIZE_MAX/2 == INT32_MAX) + typedef signed int int32_t; + #define INT32_C(v) v + #ifndef PRINTF_INT32_MODIFIER + #define PRINTF_INT32_MODIFIER "" + #endif + #elif (SHRT_MAX == INT32_MAX) + typedef signed short int32_t; + #define INT32_C(v) ((short) (v)) + #ifndef PRINTF_INT32_MODIFIER + #define PRINTF_INT32_MODIFIER "" + #endif + #else + #error "Platform not supported" + #endif + #endif + + /* + The macro stdint_int64_defined is temporarily used to record + whether or not 64 integer support is available. It must be + defined for any 64 integer extensions for new platforms that are + added. + */ + + #undef stdint_int64_defined + #if (defined(__STDC__) && defined(__STDC_VERSION__)) || defined (S_SPLINT_S) + #if (__STDC__ && __STDC_VERSION >= 199901L) || defined (S_SPLINT_S) + #define stdint_int64_defined + typedef long long int64_t; + typedef unsigned long long uint64_t; + #define UINT64_C(v) v ## ULL + #define INT64_C(v) v ## LL + #ifndef PRINTF_INT64_MODIFIER + #define PRINTF_INT64_MODIFIER "ll" + #endif + #endif + #endif + + #if !defined (stdint_int64_defined) + #if defined(__GNUC__) && __WORDSIZE == 64 + #define stdint_int64_defined + __extension__ typedef long int64_t; + __extension__ typedef unsigned long uint64_t; + #define UINT64_C(v) v ## UL + #define INT64_C(v) v ## L + #ifndef PRINTF_INT64_MODIFIER + #define PRINTF_INT64_MODIFIER "l" + #endif + #elif defined(__GNUC__) + #define stdint_int64_defined + __extension__ typedef long long int64_t; + __extension__ typedef unsigned long long uint64_t; + #define UINT64_C(v) v ## ULL + #define INT64_C(v) v ## LL + #ifndef PRINTF_INT64_MODIFIER + #define PRINTF_INT64_MODIFIER "ll" + #endif + #elif defined(__MWERKS__) || defined (__SUNPRO_C) || defined (__SUNPRO_CC) || defined (__APPLE_CC__) || defined (_LONG_LONG) || defined (_CRAYC) || defined (S_SPLINT_S) + #define stdint_int64_defined + typedef long long int64_t; + typedef unsigned long long uint64_t; + #define UINT64_C(v) v ## ULL + #define INT64_C(v) v ## LL + #ifndef PRINTF_INT64_MODIFIER + #define PRINTF_INT64_MODIFIER "ll" + #endif + #elif (defined(__WATCOMC__) && defined(__WATCOM_INT64__)) || (defined(_MSC_VER) && _INTEGRAL_MAX_BITS >= 64) || (defined (__BORLANDC__) && __BORLANDC__ > 0x460) || defined (__alpha) || defined (__DECC) + #define stdint_int64_defined + typedef __int64 int64_t; + typedef unsigned __int64 uint64_t; + #define UINT64_C(v) v ## UI64 + #define INT64_C(v) v ## I64 + #ifndef PRINTF_INT64_MODIFIER + #define PRINTF_INT64_MODIFIER "I64" + #endif + #endif + #endif + + #if !defined (LONG_LONG_MAX) && defined (INT64_C) + #define LONG_LONG_MAX INT64_C (9223372036854775807LL) + #endif + #ifndef ULONG_LONG_MAX + #define ULONG_LONG_MAX UINT64_C (18446744073709551615ULL) + #endif + + #if !defined (INT64_MAX) && defined (INT64_C) + #define INT64_MAX INT64_C (9223372036854775807LL) + #endif + #if !defined (INT64_MIN) && defined (INT64_C) + #define INT64_MIN INT64_C (-9223372036854775808LL) + #endif + #if !defined (UINT64_MAX) && defined (INT64_C) + #define UINT64_MAX UINT64_C (18446744073709551615ULL) + #endif + + /* + Width of hexadecimal for number field. + */ + + #ifndef PRINTF_INT64_HEX_WIDTH + #define PRINTF_INT64_HEX_WIDTH "16" + #endif + #ifndef PRINTF_INT32_HEX_WIDTH + #define PRINTF_INT32_HEX_WIDTH "8" + #endif + #ifndef PRINTF_INT16_HEX_WIDTH + #define PRINTF_INT16_HEX_WIDTH "4" + #endif + #ifndef PRINTF_INT8_HEX_WIDTH + #define PRINTF_INT8_HEX_WIDTH "2" + #endif + + #ifndef PRINTF_INT64_DEC_WIDTH + #define PRINTF_INT64_DEC_WIDTH "20" + #endif + #ifndef PRINTF_INT32_DEC_WIDTH + #define PRINTF_INT32_DEC_WIDTH "10" + #endif + #ifndef PRINTF_INT16_DEC_WIDTH + #define PRINTF_INT16_DEC_WIDTH "5" + #endif + #ifndef PRINTF_INT8_DEC_WIDTH + #define PRINTF_INT8_DEC_WIDTH "3" + #endif + + /* + Ok, lets not worry about 128 bit integers for now. Moore's law says + we don't need to worry about that until about 2040 at which point + we'll have bigger things to worry about. + */ + + #ifdef stdint_int64_defined + typedef int64_t intmax_t; + typedef uint64_t uintmax_t; + #define INTMAX_MAX INT64_MAX + #define INTMAX_MIN INT64_MIN + #define UINTMAX_MAX UINT64_MAX + #define UINTMAX_C(v) UINT64_C(v) + #define INTMAX_C(v) INT64_C(v) + #ifndef PRINTF_INTMAX_MODIFIER + #define PRINTF_INTMAX_MODIFIER PRINTF_INT64_MODIFIER + #endif + #ifndef PRINTF_INTMAX_HEX_WIDTH + #define PRINTF_INTMAX_HEX_WIDTH PRINTF_INT64_HEX_WIDTH + #endif + #ifndef PRINTF_INTMAX_DEC_WIDTH + #define PRINTF_INTMAX_DEC_WIDTH PRINTF_INT64_DEC_WIDTH + #endif + #else + typedef int32_t intmax_t; + typedef uint32_t uintmax_t; + #define INTMAX_MAX INT32_MAX + #define UINTMAX_MAX UINT32_MAX + #define UINTMAX_C(v) UINT32_C(v) + #define INTMAX_C(v) INT32_C(v) + #ifndef PRINTF_INTMAX_MODIFIER + #define PRINTF_INTMAX_MODIFIER PRINTF_INT32_MODIFIER + #endif + #ifndef PRINTF_INTMAX_HEX_WIDTH + #define PRINTF_INTMAX_HEX_WIDTH PRINTF_INT32_HEX_WIDTH + #endif + #ifndef PRINTF_INTMAX_DEC_WIDTH + #define PRINTF_INTMAX_DEC_WIDTH PRINTF_INT32_DEC_WIDTH + #endif + #endif + + /* + Because this file currently only supports platforms which have + precise powers of 2 as bit sizes for the default integers, the + least definitions are all trivial. Its possible that a future + version of this file could have different definitions. + */ + + #ifndef stdint_least_defined + typedef int8_t int_least8_t; + typedef uint8_t uint_least8_t; + typedef int16_t int_least16_t; + typedef uint16_t uint_least16_t; + typedef int32_t int_least32_t; + typedef uint32_t uint_least32_t; + #define PRINTF_LEAST32_MODIFIER PRINTF_INT32_MODIFIER + #define PRINTF_LEAST16_MODIFIER PRINTF_INT16_MODIFIER + #define UINT_LEAST8_MAX UINT8_MAX + #define INT_LEAST8_MAX INT8_MAX + #define UINT_LEAST16_MAX UINT16_MAX + #define INT_LEAST16_MAX INT16_MAX + #define UINT_LEAST32_MAX UINT32_MAX + #define INT_LEAST32_MAX INT32_MAX + #define INT_LEAST8_MIN INT8_MIN + #define INT_LEAST16_MIN INT16_MIN + #define INT_LEAST32_MIN INT32_MIN + #ifdef stdint_int64_defined + typedef int64_t int_least64_t; + typedef uint64_t uint_least64_t; + #define PRINTF_LEAST64_MODIFIER PRINTF_INT64_MODIFIER + #define UINT_LEAST64_MAX UINT64_MAX + #define INT_LEAST64_MAX INT64_MAX + #define INT_LEAST64_MIN INT64_MIN + #endif + #endif + #undef stdint_least_defined + + /* + The ANSI C committee pretending to know or specify anything about + performance is the epitome of misguided arrogance. The mandate of + this file is to *ONLY* ever support that absolute minimum + definition of the fast integer types, for compatibility purposes. + No extensions, and no attempt to suggest what may or may not be a + faster integer type will ever be made in this file. Developers are + warned to stay away from these types when using this or any other + stdint.h. + */ + + #ifndef int_fast8_t + typedef int_least8_t int_fast8_t; + #endif + #ifndef uint_fast8_t + typedef uint_least8_t uint_fast8_t; + #endif + #ifndef int_fast16_t + typedef int_least16_t int_fast16_t; + #endif + #ifndef uint_fast16_t + typedef uint_least16_t uint_fast16_t; + #endif + #ifndef int_fast32_t + typedef int_least32_t int_fast32_t; + #endif + #ifndef uint_fast32_t + typedef uint_least32_t uint_fast32_t; + #endif + #define UINT_FAST8_MAX UINT_LEAST8_MAX + #define INT_FAST8_MAX INT_LEAST8_MAX + #define UINT_FAST16_MAX UINT_LEAST16_MAX + #define INT_FAST16_MAX INT_LEAST16_MAX + #define UINT_FAST32_MAX UINT_LEAST32_MAX + #define INT_FAST32_MAX INT_LEAST32_MAX + #define INT_FAST8_MIN INT_LEAST8_MIN + #define INT_FAST16_MIN INT_LEAST16_MIN + #define INT_FAST32_MIN INT_LEAST32_MIN + #ifdef stdint_int64_defined + typedef int_least64_t int_fast64_t; + typedef uint_least64_t uint_fast64_t; + #define UINT_FAST64_MAX UINT_LEAST64_MAX + #define INT_FAST64_MAX INT_LEAST64_MAX + #define INT_FAST64_MIN INT_LEAST64_MIN + #endif + + /* + Whatever piecemeal, per compiler thing we can do about the wchar_t + type limits. + */ + + #if defined(__WATCOMC__) || defined(_MSC_VER) || defined (__GNUC__) + #include + #ifndef WCHAR_MIN + #define WCHAR_MIN 0 + #endif + #ifndef WCHAR_MAX + #define WCHAR_MAX ((wchar_t)-1) + #endif + #endif + + /* + Whatever piecemeal, per compiler/platform thing we can do about the + (u)intptr_t types and limits. + */ + + #if defined (_MSC_VER) && defined (_UINTPTR_T_DEFINED) + #define STDINT_H_UINTPTR_T_DEFINED + #endif + + #ifndef STDINT_H_UINTPTR_T_DEFINED + #if defined (__alpha__) || defined (__ia64__) || defined (__x86_64__) || defined (_WIN64) + #define stdint_intptr_bits 64 + #elif defined (__WATCOMC__) || defined (__TURBOC__) + #if defined(__TINY__) || defined(__SMALL__) || defined(__MEDIUM__) + #define stdint_intptr_bits 16 + #else + #define stdint_intptr_bits 32 + #endif + #elif defined (__i386__) || defined (_WIN32) || defined (WIN32) + #define stdint_intptr_bits 32 + #elif defined (__INTEL_COMPILER) + /* TODO -- what will Intel do about x86-64? */ + #endif + + #ifdef stdint_intptr_bits + #define stdint_intptr_glue3_i(a,b,c) a##b##c + #define stdint_intptr_glue3(a,b,c) stdint_intptr_glue3_i(a,b,c) + #ifndef PRINTF_INTPTR_MODIFIER + #define PRINTF_INTPTR_MODIFIER stdint_intptr_glue3(PRINTF_INT,stdint_intptr_bits,_MODIFIER) + #endif + #ifndef PTRDIFF_MAX + #define PTRDIFF_MAX stdint_intptr_glue3(INT,stdint_intptr_bits,_MAX) + #endif + #ifndef PTRDIFF_MIN + #define PTRDIFF_MIN stdint_intptr_glue3(INT,stdint_intptr_bits,_MIN) + #endif + #ifndef UINTPTR_MAX + #define UINTPTR_MAX stdint_intptr_glue3(UINT,stdint_intptr_bits,_MAX) + #endif + #ifndef INTPTR_MAX + #define INTPTR_MAX stdint_intptr_glue3(INT,stdint_intptr_bits,_MAX) + #endif + #ifndef INTPTR_MIN + #define INTPTR_MIN stdint_intptr_glue3(INT,stdint_intptr_bits,_MIN) + #endif + #ifndef INTPTR_C + #define INTPTR_C(x) stdint_intptr_glue3(INT,stdint_intptr_bits,_C)(x) + #endif + #ifndef UINTPTR_C + #define UINTPTR_C(x) stdint_intptr_glue3(UINT,stdint_intptr_bits,_C)(x) + #endif + typedef stdint_intptr_glue3(uint, stdint_intptr_bits, _t) uintptr_t; + typedef stdint_intptr_glue3(int, stdint_intptr_bits, _t) intptr_t; + #else + /* TODO -- This following is likely wrong for some platforms, and does + nothing for the definition of uintptr_t. */ + typedef ptrdiff_t intptr_t; + #endif + #define STDINT_H_UINTPTR_T_DEFINED + #endif + + /* + Assumes sig_atomic_t is signed and we have a 2s complement machine. + */ + + #ifndef SIG_ATOMIC_MAX + #define SIG_ATOMIC_MAX ((((sig_atomic_t) 1) << (sizeof (sig_atomic_t)*CHAR_BIT-1)) - 1) + #endif #endif #if defined (__TEST_PSTDINT_FOR_CORRECTNESS) /* - * Please compile with the maximum warning settings to make sure macros are not - * defined more than once. - */ + Please compile with the maximum warning settings to make sure macros are not + defined more than once. +*/ #include #include @@ -877,43 +877,53 @@ typedef uint_least32_t uint_fast32_t; #define TESTUMAX(bits) glue3(u,bits,=) glue3(~,u,bits); if (glue3(UINT,bits,_MAX) glue3(!=,u,bits)) printf ("Something wrong with UINT%d_MAX\n", bits) -int main () { - DECL(I,8) - DECL(U,8) - DECL(I,16) - DECL(U,16) - DECL(I,32) - DECL(U,32) +int main() +{ + DECL(I, 8) + DECL(U, 8) + DECL(I, 16) + DECL(U, 16) + DECL(I, 32) + DECL(U, 32) #ifdef INT64_MAX - DECL(I,64) - DECL(U,64) + DECL(I, 64) + DECL(U, 64) #endif intmax_t imax = INTMAX_C(0); uintmax_t umax = UINTMAX_C(0); char str0[256], str1[256]; - sprintf (str0, "%d %x\n", 0, ~0); - - sprintf (str1, "%d %x\n", i8, ~0); - if (0 != strcmp (str0, str1)) printf ("Something wrong with i8 : %s\n", str1); - sprintf (str1, "%u %x\n", u8, ~0); - if (0 != strcmp (str0, str1)) printf ("Something wrong with u8 : %s\n", str1); - sprintf (str1, "%d %x\n", i16, ~0); - if (0 != strcmp (str0, str1)) printf ("Something wrong with i16 : %s\n", str1); - sprintf (str1, "%u %x\n", u16, ~0); - if (0 != strcmp (str0, str1)) printf ("Something wrong with u16 : %s\n", str1); - sprintf (str1, "%" PRINTF_INT32_MODIFIER "d %x\n", i32, ~0); - if (0 != strcmp (str0, str1)) printf ("Something wrong with i32 : %s\n", str1); - sprintf (str1, "%" PRINTF_INT32_MODIFIER "u %x\n", u32, ~0); - if (0 != strcmp (str0, str1)) printf ("Something wrong with u32 : %s\n", str1); + sprintf(str0, "%d %x\n", 0, ~0); + + sprintf(str1, "%d %x\n", i8, ~0); + if (0 != strcmp(str0, str1)) + printf("Something wrong with i8 : %s\n", str1); + sprintf(str1, "%u %x\n", u8, ~0); + if (0 != strcmp(str0, str1)) + printf("Something wrong with u8 : %s\n", str1); + sprintf(str1, "%d %x\n", i16, ~0); + if (0 != strcmp(str0, str1)) + printf("Something wrong with i16 : %s\n", str1); + sprintf(str1, "%u %x\n", u16, ~0); + if (0 != strcmp(str0, str1)) + printf("Something wrong with u16 : %s\n", str1); + sprintf(str1, "%" PRINTF_INT32_MODIFIER "d %x\n", i32, ~0); + if (0 != strcmp(str0, str1)) + printf("Something wrong with i32 : %s\n", str1); + sprintf(str1, "%" PRINTF_INT32_MODIFIER "u %x\n", u32, ~0); + if (0 != strcmp(str0, str1)) + printf("Something wrong with u32 : %s\n", str1); #ifdef INT64_MAX - sprintf (str1, "%" PRINTF_INT64_MODIFIER "d %x\n", i64, ~0); - if (0 != strcmp (str0, str1)) printf ("Something wrong with i64 : %s\n", str1); + sprintf(str1, "%" PRINTF_INT64_MODIFIER "d %x\n", i64, ~0); + if (0 != strcmp(str0, str1)) + printf("Something wrong with i64 : %s\n", str1); #endif - sprintf (str1, "%" PRINTF_INTMAX_MODIFIER "d %x\n", imax, ~0); - if (0 != strcmp (str0, str1)) printf ("Something wrong with imax : %s\n", str1); - sprintf (str1, "%" PRINTF_INTMAX_MODIFIER "u %x\n", umax, ~0); - if (0 != strcmp (str0, str1)) printf ("Something wrong with umax : %s\n", str1); + sprintf(str1, "%" PRINTF_INTMAX_MODIFIER "d %x\n", imax, ~0); + if (0 != strcmp(str0, str1)) + printf("Something wrong with imax : %s\n", str1); + sprintf(str1, "%" PRINTF_INTMAX_MODIFIER "u %x\n", umax, ~0); + if (0 != strcmp(str0, str1)) + printf("Something wrong with umax : %s\n", str1); TESTUMAX(8); TESTUMAX(16); @@ -928,7 +938,7 @@ int main () { #endif #if defined(_STDINT) && defined(_MSC_VER) -#define stdint_int64_defined + #define stdint_int64_defined #endif #endif diff --git a/lib/xspublic/xstypes/resource.h b/lib/xspublic/xstypes/resource.h index 9b99df4..c204c8b 100644 --- a/lib/xspublic/xstypes/resource.h +++ b/lib/xspublic/xstypes/resource.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -70,10 +70,10 @@ // Next default values for new objects // #ifdef APSTUDIO_INVOKED -#ifndef APSTUDIO_READONLY_SYMBOLS -#define _APS_NEXT_RESOURCE_VALUE 101 -#define _APS_NEXT_COMMAND_VALUE 40001 -#define _APS_NEXT_CONTROL_VALUE 1001 -#define _APS_NEXT_SYMED_VALUE 101 -#endif + #ifndef APSTUDIO_READONLY_SYMBOLS + #define _APS_NEXT_RESOURCE_VALUE 101 + #define _APS_NEXT_COMMAND_VALUE 40001 + #define _APS_NEXT_CONTROL_VALUE 1001 + #define _APS_NEXT_SYMED_VALUE 101 + #endif #endif diff --git a/lib/xspublic/xstypes/xsanalogindata.h b/lib/xspublic/xstypes/xsanalogindata.h index 7598081..3180df2 100644 --- a/lib/xspublic/xstypes/xsanalogindata.h +++ b/lib/xspublic/xstypes/xsanalogindata.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -68,7 +68,8 @@ #include "pstdint.h" /*! \brief Data from analog inputs from sensors. */ -struct XsAnalogInData { +struct XsAnalogInData +{ uint16_t m_data; /*!< \brief The data */ #ifdef __cplusplus /*! \brief Construct a nulled analog data item */ diff --git a/lib/xspublic/xstypes/xsarray.c b/lib/xspublic/xstypes/xsarray.c index c7f4604..14eda5f 100644 --- a/lib/xspublic/xstypes/xsarray.c +++ b/lib/xspublic/xstypes/xsarray.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -82,13 +82,13 @@ /*! \endcond */ #ifdef DOXYGEN -/*! \brief Initializes the XsArray-derived object with space for \a count items and copies them from \a src + /*! \brief Initializes the XsArray-derived object with space for \a count items and copies them from \a src \details This function initializes the object reserving \a count items in the buffer. \a count may be 0. If \a src is not 0, \a count items from \a src will be copied. \param count The number of items to reserve space for. When \a src is not NULL, thisArray is also the number of items copied from \a src \param src A pointer to an array of objects to copy, may be NULL, ignored when \a count is 0 -*/ -void XsArray_constructDerived(void* thisPtr, XsSize count, void const* src); + */ + void XsArray_constructDerived(void* thisPtr, XsSize count, void const* src); #endif /*! \relates XsArray @@ -108,28 +108,26 @@ void XsArray_construct(void* thisPtr, XsArrayDescriptor const* const descriptor, if (thisArray->m_size) { // init to size - *((void**) &thisArray->m_data) = malloc(thisArray->m_size*elemSize(thisArray)); + *((void**) &thisArray->m_data) = malloc(thisArray->m_size * elemSize(thisArray)); XsArray_incAllocCount(); // init the configurations if (src) { if (thisArray->m_descriptor->rawCopy) - { thisArray->m_descriptor->rawCopy(thisArray->m_data, src, thisArray->m_size, thisArray->m_descriptor->itemSize); - } else { XsSize i; assert(thisArray->m_descriptor->itemCopyConstruct); - for (i=0; im_size; ++i) + for (i = 0; i < thisArray->m_size; ++i) thisArray->m_descriptor->itemCopyConstruct(elemAt(thisArray->m_data, i), elemAt(src, i)); } } else if (thisArray->m_descriptor->itemConstruct) { XsSize i; - for (i=0; im_size; ++i) + for (i = 0; i < thisArray->m_size; ++i) thisArray->m_descriptor->itemConstruct(elemAt(thisArray->m_data, i)); } } @@ -164,7 +162,7 @@ void XsArray_destruct(void* thisPtr) XsSize i; // clear contents if (thisArray->m_descriptor->itemDestruct) - for (i=0; im_reserved; ++i) + for (i = 0; i < thisArray->m_reserved; ++i) thisArray->m_descriptor->itemDestruct(elemAt(thisArray->m_data, i)); free((void*) thisArray->m_data); XsArray_incFreeCount(); @@ -203,18 +201,16 @@ void XsArray_assign(void* thisPtr, XsSize count, void const* src) // no reallocation necessary, clear excess objects if (thisArray->m_descriptor->itemDestruct) - for (i=count; im_size; ++i) + for (i = count; i < thisArray->m_size; ++i) thisArray->m_descriptor->itemDestruct(elemAt(thisArray->m_data, i)); if (src) { if (thisArray->m_descriptor->rawCopy) - { thisArray->m_descriptor->rawCopy(thisArray->m_data, src, count, thisArray->m_descriptor->itemSize); - } else { - for (i=0; im_descriptor->itemCopy(elemAt(thisArray->m_data, i), elemAt(src, i)); } } @@ -287,17 +283,17 @@ void XsArray_reserve(void* thisPtr, XsSize count) *((XsSize*) &tmp.m_reserved) = count; // init to size - *((void**) &tmp.m_data) = malloc(tmp.m_reserved*elemSize(thisArray)); + *((void**) &tmp.m_data) = malloc(tmp.m_reserved * elemSize(thisArray)); assert(tmp.m_data); if (!tmp.m_data) return; XsArray_incAllocCount(); if (thisArray->m_descriptor->itemConstruct) - for (i=0; im_descriptor->itemConstruct(elemAt(tmp.m_data, i)); - for (i=0; im_size; ++i) + for (i = 0; i < thisArray->m_size; ++i) thisArray->m_descriptor->itemSwap(elemAt(thisArray->m_data, i), elemAt(tmp.m_data, i)); XsArray_destruct(thisArray); @@ -312,9 +308,7 @@ void XsArray_copy(void* thisPtr, void const* src) XsArray const* srcArray = (XsArray const*) src; if (srcArray == thisArray) - { return; - } XsArray_assign(thisArray, srcArray->m_size, srcArray->m_data); } @@ -337,13 +331,11 @@ void XsArray_append(void* thisPtr, void const* other) XsArray_reserve(thisArray, thisArray->m_size + thisArray->m_size); // maybe reserve more here? if (thisArray->m_descriptor->rawCopy) - { thisArray->m_descriptor->rawCopy(elemAt(thisArray->m_data, thisArray->m_size), thisArray->m_data, thisArray->m_size, thisArray->m_descriptor->itemSize); - } else { - for (i=0; im_size; ++i) - thisArray->m_descriptor->itemCopy(elemAt(thisArray->m_data, i+thisArray->m_size), elemAt(thisArray->m_data, i)); + for (i = 0; i < thisArray->m_size; ++i) + thisArray->m_descriptor->itemCopy(elemAt(thisArray->m_data, i + thisArray->m_size), elemAt(thisArray->m_data, i)); } *((XsSize*) &thisArray->m_size) = thisArray->m_size + thisArray->m_size; @@ -360,13 +352,11 @@ void XsArray_append(void* thisPtr, void const* other) XsArray_reserve(thisArray, thisArray->m_size + otherArray->m_size); // maybe reserve more here? if (thisArray->m_descriptor->rawCopy) - { thisArray->m_descriptor->rawCopy(elemAt(thisArray->m_data, thisArray->m_size), otherArray->m_data, otherArray->m_size, thisArray->m_descriptor->itemSize); - } else { - for (i=0; im_size; ++i) - thisArray->m_descriptor->itemCopy(elemAt(thisArray->m_data, i+thisArray->m_size), elemAt(otherArray->m_data, i)); + for (i = 0; i < otherArray->m_size; ++i) + thisArray->m_descriptor->itemCopy(elemAt(thisArray->m_data, i + thisArray->m_size), elemAt(otherArray->m_data, i)); } *((XsSize*) &thisArray->m_size) = thisArray->m_size + otherArray->m_size; @@ -383,27 +373,25 @@ void XsArray_insert(void* thisPtr, XsSize index, XsSize count, void const* src) { XsSize s; XsArray* thisArray = (XsArray*) thisPtr; - XsSize i,d = count; + XsSize i, d = count; if (thisArray->m_size + count > thisArray->m_reserved) - XsArray_reserve(thisArray, ((thisArray->m_size + count)*3)/2); // we reserve 50% more space here to handle multiple sequential insertions efficiently + XsArray_reserve(thisArray, ((thisArray->m_size + count) * 3) / 2); // we reserve 50% more space here to handle multiple sequential insertions efficiently // fix index if beyond end of list if (index > thisArray->m_size) index = thisArray->m_size; // move items to the back by swapping - for (i = thisArray->m_size-1; i >= index && i < thisArray->m_size; --i) - thisArray->m_descriptor->itemSwap(elemAt(thisArray->m_data, i), elemAt(thisArray->m_data, i+d)); + for (i = thisArray->m_size - 1; i >= index && i < thisArray->m_size; --i) + thisArray->m_descriptor->itemSwap(elemAt(thisArray->m_data, i), elemAt(thisArray->m_data, i + d)); // copy items to the array if (thisArray->m_descriptor->rawCopy) - { thisArray->m_descriptor->rawCopy(elemAt(thisArray->m_data, index), src, count, thisArray->m_descriptor->itemSize); - } else { for (s = 0; s < count; ++s) - thisArray->m_descriptor->itemCopy(elemAt(thisArray->m_data, s+index), elemAt(src, s)); + thisArray->m_descriptor->itemCopy(elemAt(thisArray->m_data, s + index), elemAt(src, s)); } // update size @@ -444,7 +432,7 @@ void XsArray_swap(void* a, void* b) *((XsSize*) &aArray->m_flags) = bArray->m_flags; *((XsSize*) &bArray->m_flags) = tmp.m_flags; } - else + else { // elementwise swap XsSize i; @@ -465,14 +453,14 @@ void XsArray_erase(void* thisPtr, XsSize index, XsSize count) if (count == 0 || index >= thisArray->m_size) return; - if (count+index > thisArray->m_size) + if (count + index > thisArray->m_size) count = thisArray->m_size - index; newCount = thisArray->m_size - count; // move items into the gap by swapping for (i = index; i < newCount; ++i) - thisArray->m_descriptor->itemSwap(elemAt(thisArray->m_data, i), elemAt(thisArray->m_data, i+count)); + thisArray->m_descriptor->itemSwap(elemAt(thisArray->m_data, i), elemAt(thisArray->m_data, i + count)); *((XsSize*) &thisArray->m_size) = newCount; } @@ -498,7 +486,7 @@ int XsArray_compare(void const* a, void const* b) return 0; if (aArray->m_size != bArray->m_size) - return (aArray->m_size < bArray->m_size)?-1:1; + return (aArray->m_size < bArray->m_size) ? -1 : 1; assert(aArray->m_descriptor->itemCompare); // we could theoretically only check the sizes and ignore the element-comparison in thisArray case @@ -533,7 +521,7 @@ int XsArray_comparePredicate(void const* a, void const* b, XsArrayItemCompareFun return 0; if (aArray->m_size != bArray->m_size) - return (aArray->m_size < bArray->m_size)?-1:1; + return (aArray->m_size < bArray->m_size) ? -1 : 1; assert(predicate); // we could theoretically only check the sizes and ignore the element-comparison in thisArray case @@ -568,7 +556,7 @@ int XsArray_compareSet(void const* a, void const* b) return 0; if (aArray->m_size != bArray->m_size) - return (aArray->m_size < bArray->m_size)?-1:1; + return (aArray->m_size < bArray->m_size) ? -1 : 1; for (n = 0; n < aArray->m_size; ++n) // loop over all elements of list aArray { @@ -594,7 +582,7 @@ int XsArray_compareSet(void const* a, void const* b) \param needle A pointer to the value to search for \returns The index of where \a needle was found or -1 if it wasn't found. */ -int XsArray_find(void const* thisPtr, void const* needle) +ptrdiff_t XsArray_find(void const* thisPtr, void const* needle) { XsSize i; XsArray const* thisArray = (XsArray const*) thisPtr; @@ -603,7 +591,7 @@ int XsArray_find(void const* thisPtr, void const* needle) // we could theoretically only check the sizes and ignore the element-comparison in thisArray case for (i = 0; i < thisArray->m_size; ++i) // loop over all elements of the lists if (!thisArray->m_descriptor->itemCompare(elemAt(thisArray->m_data, i), needle)) - return (int) i; + return (ptrdiff_t) i; return -1; } @@ -616,7 +604,7 @@ int XsArray_find(void const* thisPtr, void const* needle) pointers to items, supplied as such: predicate(item, needle) \returns The index of where \a needle was found or -1 if it wasn't found. */ -int XsArray_findPredicate(void const* thisPtr, void const* needle, XsArrayItemCompareFunc predicate) +ptrdiff_t XsArray_findPredicate(void const* thisPtr, void const* needle, XsArrayItemCompareFunc predicate) { XsSize i; XsArray const* thisArray = (XsArray const*) thisPtr; @@ -625,7 +613,7 @@ int XsArray_findPredicate(void const* thisPtr, void const* needle, XsArrayItemCo // we could theoretically only check the sizes and ignore the element-comparison in thisArray case for (i = 0; i < thisArray->m_size; ++i) // loop over all elements of the lists if (!predicate(elemAt(thisArray->m_data, i), needle)) - return (int) i; + return (ptrdiff_t) i; return -1; } @@ -660,18 +648,16 @@ void* XsArray_atIndex(void* thisPtr, XsSize index) */ void XsArray_removeDuplicates(void* thisPtr) { - XsSize i,j; + XsSize i, j; XsArray* thisArray = (XsArray*) thisPtr; if (thisArray->m_size > 1) { - for (i = 0; i < thisArray->m_size-1; ++i) + for (i = 0; i < thisArray->m_size - 1; ++i) { - for (j = thisArray->m_size-1; j > i; --j) + for (j = thisArray->m_size - 1; j > i; --j) { if (!thisArray->m_descriptor->itemCompare(elemAt(thisArray->m_data, i), elemAt(thisArray->m_data, j))) - { XsArray_erase(thisPtr, j, 1); - } } } } @@ -683,18 +669,16 @@ void XsArray_removeDuplicates(void* thisPtr) */ void XsArray_removeDuplicatesPredicate(void* thisPtr, XsArrayItemCompareFunc predicate) { - XsSize i,j; + XsSize i, j; XsArray* thisArray = (XsArray*) thisPtr; if (thisArray->m_size > 1) { - for (i = 0; i < thisArray->m_size-1; ++i) + for (i = 0; i < thisArray->m_size - 1; ++i) { - for (j = thisArray->m_size-1; j > i; --j) + for (j = thisArray->m_size - 1; j > i; --j) { if (!predicate(elemAt(thisArray->m_data, i), elemAt(thisArray->m_data, j))) - { XsArray_erase(thisPtr, j, 1); - } } } } @@ -722,7 +706,7 @@ int XsArray_empty(void const* thisPtr) */ void XsArray_rawCopy(void* to, void const* from, XsSize count, XsSize iSize) { - memcpy(to, from, count*iSize); + memcpy(to, from, count * iSize); } /*! \relates XsArray @@ -745,7 +729,7 @@ void XsArray_reverse(void* thisPtr) XsArray* thisArray = (XsArray*) thisPtr; half = thisArray->m_size >> 1; for (i = 0; i < half; ++i) - thisArray->m_descriptor->itemSwap(elemAt(thisArray->m_data, i), elemAt(thisArray->m_data, (thisArray->m_size-1)-i)); + thisArray->m_descriptor->itemSwap(elemAt(thisArray->m_data, i), elemAt(thisArray->m_data, (thisArray->m_size - 1) - i)); } /*! @} */ diff --git a/lib/xspublic/xstypes/xsarray.h b/lib/xspublic/xstypes/xsarray.h index e9a6926..3179548 100644 --- a/lib/xspublic/xstypes/xsarray.h +++ b/lib/xspublic/xstypes/xsarray.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -95,7 +95,8 @@ typedef void (*XsArrayRawCopy)(void*, void const*, XsSize, XsSize); \details Ususally there is one static instance per type of array that will be used by all XsArrays of that type. */ -struct XsArrayDescriptor { +struct XsArrayDescriptor +{ #ifndef __cplusplus const #else @@ -116,6 +117,12 @@ typedef struct XsArrayDescriptor XsArrayDescriptor; #ifdef __cplusplus #include #include + +#if __cplusplus >= 201103L && !defined(XSENS_HAVE_TYPE_TRAITS) +#include +#define XSENS_HAVE_TYPE_TRAITS +#endif + extern "C" { #endif @@ -133,8 +140,8 @@ XSTYPES_DLL_API void XsArray_swap(void* a, void* b); XSTYPES_DLL_API int XsArray_compare(void const* a, void const* b); XSTYPES_DLL_API int XsArray_compareSet(void const* a, void const* b); XSTYPES_DLL_API int XsArray_comparePredicate(void const* a, void const* b, XsArrayItemCompareFunc predicate); -XSTYPES_DLL_API int XsArray_find(void const* thisPtr, void const* needle); -XSTYPES_DLL_API int XsArray_findPredicate(void const* thisPtr, void const* needle, XsArrayItemCompareFunc predicate); +XSTYPES_DLL_API ptrdiff_t XsArray_find(void const* thisPtr, void const* needle); +XSTYPES_DLL_API ptrdiff_t XsArray_findPredicate(void const* thisPtr, void const* needle, XsArrayItemCompareFunc predicate); XSTYPES_DLL_API int XsArray_empty(void const* thisPtr); XSTYPES_DLL_API void const* XsArray_at(void const* thisPtr, XsSize index); XSTYPES_DLL_API void* XsArray_atIndex(void* thisPtr, XsSize index); @@ -144,7 +151,8 @@ XSTYPES_DLL_API void XsArray_rawCopy(void* to, void const* from, XsSize count, X XSTYPES_DLL_API void XsArray_sort(void* thisPtr); XSTYPES_DLL_API void XsArray_reverse(void* thisPtr); -struct XsArray { +struct XsArray +{ XSARRAY_DECL(void) #ifdef __cplusplus //! \copydoc XsArray_construct @@ -226,7 +234,8 @@ typedef struct XsArray XsArray; \tparam I The class that inherits from the XsArrayImpl. Some functions (such as the streaming operator) require the inheriting type to be returned for proper functionality. */ template -struct XsArrayImpl : private XsArray { +struct XsArrayImpl : private XsArray +{ //! \brief The contained type typedef T value_type; @@ -240,7 +249,7 @@ struct XsArrayImpl : private XsArray { \param count the number of items in src \param src pointer to an array of output configurations \sa XsArray_construct - */ + */ inline explicit XsArrayImpl(XsSize count = 0, T const* src = 0) : XsArray(&D, count, src) { @@ -258,7 +267,7 @@ struct XsArrayImpl : private XsArray { inline explicit XsArrayImpl(Iterator const& beginIt, Iterator const& endIt) : XsArray(&D, 0, 0) { - ptrdiff_t diff = endIt-beginIt; + ptrdiff_t diff = endIt - beginIt; if (diff > 0) { reserve((XsSize) diff); @@ -291,7 +300,7 @@ struct XsArrayImpl : private XsArray { \param predicate the custom item comparison predicate to apply \returns 0 if the two arrays are considered equal \sa XsArray_comparePredicate - */ + */ inline int comparePredicate(ArrayImpl const& other, XsArrayItemCompareFunc predicate) const { return XsArray_comparePredicate(this, &other, predicate); @@ -302,7 +311,7 @@ struct XsArrayImpl : private XsArray { \param predicate the custom item comparison predicate to apply \returns true if the two arrays are considered equal \sa XsArray_comparePredicate - */ + */ inline bool isEqualPredicate(ArrayImpl const& other, XsArrayItemCompareFunc predicate) const { return !XsArray_comparePredicate(this, &other, predicate); @@ -313,7 +322,7 @@ struct XsArrayImpl : private XsArray { \param other the array to compare against \returns true if the two arrays are equal \sa XsArray_compareArray - */ + */ inline bool operator == (ArrayImpl const& other) const { return !XsArray_compare(this, &other); @@ -351,7 +360,8 @@ struct XsArrayImpl : private XsArray { #ifndef XSENS_NOITERATOR /*! \brief STL-style iterator */ template - struct IteratorImplBase { + struct IteratorImplBase + { public: //! \brief Difference between two items typedef ptrdiff_t difference_type; @@ -377,16 +387,37 @@ struct XsArrayImpl : private XsArray { //! \brief Copy constructor inline IteratorImplBase(this_type const& i) : m_ptr(i.m_ptr) {} public: +#ifndef SWIG /*! \brief indexed data access operator */ - inline value_type const& operator[] (ptrdiff_t index) const + template + inline T const& operator[](J index) const { - return *ptrAt(m_ptr, F*index); +#ifdef XSENS_HAVE_TYPE_TRAITS + static_assert(std::is_integral::value || std::is_enum::value, "Integral index required."); +#endif + return *ptrAt(m_ptr, F * static_cast(index)); } /*! \brief indexed data access operator */ - inline value_type& operator[] (ptrdiff_t index) + template + inline T& operator[](J index) { - return *ptrAt(m_ptr, F*index); +#ifdef XSENS_HAVE_TYPE_TRAITS + static_assert(std::is_integral::value || std::is_enum::value, "Integral index required."); +#endif + return *ptrAt(m_ptr, F * static_cast(index)); + } +#else + /*! \brief indexed data access operator */ + inline T const& operator[](int index) const + { + return *ptrAt(m_ptr, F * static_cast(index)); } + /*! \brief indexed data access operator */ + inline T& operator[](int index) + { + return *ptrAt(m_ptr, F * static_cast(index)); + } +#endif //! \brief Assignment operator inline this_type& operator =(void* p) { @@ -434,24 +465,24 @@ struct XsArrayImpl : private XsArray { //! \brief Increment by \a count operator inline this_type const& operator +=(ptrdiff_t count) { - m_ptr = ptrAt(m_ptr, F*count); + m_ptr = ptrAt(m_ptr, F * count); return *(this_type*)this; } //! \brief Decrement by \a count operator inline this_type const& operator -=(ptrdiff_t count) { - m_ptr = ptrAt(m_ptr, -F*count); + m_ptr = ptrAt(m_ptr, -F * count); return *(this_type*)this; } //! \brief Addition by \a count operator inline this_type operator +(ptrdiff_t count) const { - return this_type(ptrAt(m_ptr, F*count)); + return this_type(ptrAt(m_ptr, F * count)); } //! \brief Subtraction by \a count operator inline this_type operator -(ptrdiff_t count) const { - return this_type(ptrAt(m_ptr, -F*count)); + return this_type(ptrAt(m_ptr, -F * count)); } /*! \brief Returns the difference in number of items between the two iterators \details The function computes the difference between this iterator and \a other. The @@ -465,23 +496,50 @@ struct XsArrayImpl : private XsArray { return (F * (reinterpret_cast(m_ptr) - reinterpret_cast(other.m_ptr))) / D.itemSize; } //! \brief Iterator comparison - inline bool operator == (this_type const& i) const { return m_ptr == i.m_ptr; } + inline bool operator == (this_type const& i) const + { + return m_ptr == i.m_ptr; + } //! \brief Iterator comparison, taking direction into account - inline bool operator <= (this_type const& i) const { return (F==1)?(m_ptr <= i.m_ptr):(m_ptr >= i.m_ptr); } + inline bool operator <= (this_type const& i) const + { + return (F == 1) ? (m_ptr <= i.m_ptr) : (m_ptr >= i.m_ptr); + } //! \brief Iterator comparison, taking direction into account - inline bool operator < (this_type const& i) const { return (F==1)?(m_ptr < i.m_ptr):(m_ptr > i.m_ptr); } + inline bool operator < (this_type const& i) const + { + return (F == 1) ? (m_ptr < i.m_ptr) : (m_ptr > i.m_ptr); + } //! \brief Iterator comparison - inline bool operator != (this_type const& i) const { return m_ptr != i.m_ptr; } + inline bool operator != (this_type const& i) const + { + return m_ptr != i.m_ptr; + } //! \brief Iterator comparison, taking direction into account - inline bool operator >= (this_type const& i) const { return (F==1)?(m_ptr >= i.m_ptr):(m_ptr <= i.m_ptr); } + inline bool operator >= (this_type const& i) const + { + return (F == 1) ? (m_ptr >= i.m_ptr) : (m_ptr <= i.m_ptr); + } //! \brief Iterator comparison, taking direction into account - inline bool operator > (this_type const& i) const { return (F==1)?(m_ptr > i.m_ptr):(m_ptr < i.m_ptr); } + inline bool operator > (this_type const& i) const + { + return (F == 1) ? (m_ptr > i.m_ptr) : (m_ptr < i.m_ptr); + } //! \brief Dereferencing operator - inline R& operator *() const { return *(R*) ptr(); } + inline R& operator *() const + { + return *(R*) ptr(); + } //! \brief Pointer operator - inline R* operator ->() const { return (R*) ptr(); } + inline R* operator ->() const + { + return (R*) ptr(); + } //! \brief Access to internal pointer object, use should be avoided - inline T* ptr() const { return m_ptr; } + inline T* ptr() const + { + return m_ptr; + } private: //! \brief The internal pointer T* m_ptr; @@ -527,44 +585,92 @@ struct XsArrayImpl : private XsArray { //! \brief STL-style mutable forward iterator typedef IteratorImpl<1> iterator; //! \brief STL-style mutable reverse iterator - typedef IteratorImpl<-1> reverse_iterator; + typedef IteratorImpl < -1 > reverse_iterator; //! \brief STL-style mutable const forward iterator typedef IteratorImplConst<1> const_iterator; //! \brief STL-style mutable const reverse iterator - typedef IteratorImplConst<-1> const_reverse_iterator; + typedef IteratorImplConst < -1 > const_reverse_iterator; /*! \brief STL-style const_iterator to the first data item in the array */ - inline const_iterator begin() const { return const_iterator(m_data); } + inline const_iterator begin() const + { + return const_iterator(m_data); + } /*! \brief STL-style const_iterator to the first data item past the end of the array */ - inline const_iterator end() const { return begin() + (ptrdiff_t) size(); } + inline const_iterator end() const + { + return begin() + (ptrdiff_t) size(); + } /*! \brief STL-style const_reverse_iterator to the first data item in the reversed array */ - inline const_reverse_iterator rbegin() const { return rend() - (ptrdiff_t) size(); } + inline const_reverse_iterator rbegin() const + { + return rend() - (ptrdiff_t) size(); + } /*! \brief STL-style const_reverse_iterator to the first data item past the end of the reversed array */ - inline const_reverse_iterator rend() const { return const_reverse_iterator(m_data) + (ptrdiff_t) 1; } + inline const_reverse_iterator rend() const + { + return const_reverse_iterator(m_data) + (ptrdiff_t) 1; + } /*! \brief STL-style iterator to the first data item in the array */ - inline iterator begin() { return iterator(m_data); } + inline iterator begin() + { + return iterator(m_data); + } /*! \brief STL-style iterator to the first data item past the end of the array */ - inline iterator end() { return begin() + (ptrdiff_t) size(); } + inline iterator end() + { + return begin() + (ptrdiff_t) size(); + } /*! \brief STL-style reverse_iterator to the first data item in the reversed array */ - inline reverse_iterator rbegin() { return rend() - (ptrdiff_t) size(); } + inline reverse_iterator rbegin() + { + return rend() - (ptrdiff_t) size(); + } /*! \brief STL-style reverse_iterator to the first data item past the end of the reversed array */ - inline reverse_iterator rend() { return reverse_iterator(m_data) + (ptrdiff_t) 1; } + inline reverse_iterator rend() + { + return reverse_iterator(m_data) + (ptrdiff_t) 1; + } #endif +#ifndef SWIG /*! \brief indexed data access operator */ - inline T const& operator[] (XsSize index) const + template + inline T const& operator[](J index) const { - assert(index < m_size); - return *ptrAt(m_data, (ptrdiff_t) index); +#ifdef XSENS_HAVE_TYPE_TRAITS + static_assert(std::is_integral::value || std::is_enum::value, "Integral index required."); +#endif + assert(static_cast(index) < m_size); + return *ptrAt(m_data, static_cast(index)); } /*! \brief indexed data access operator */ - inline T& operator[] (XsSize index) + template + inline T& operator[](J index) { - assert(index < m_size); - return *ptrAt(m_data, (ptrdiff_t) index); +#ifdef XSENS_HAVE_TYPE_TRAITS + static_assert(std::is_integral::value || std::is_enum::value, "Integral index required."); +#endif + assert(static_cast(index) < m_size); + return *ptrAt(m_data, static_cast(index)); + } +#else + /*! \brief indexed data access operator */ + inline T const& operator[](int index) const + { + assert(static_cast(index) < m_size); + return *ptrAt(m_data, static_cast(index)); + } + /*! \brief indexed data access operator */ + inline T& operator[](int index) + { + assert(static_cast(index) < m_size); + return *ptrAt(m_data, static_cast(index)); } +#endif + /*! \brief indexed data access \sa operator[] \param index Index of item to access. \returns The item at \a index (by value). */ inline T value(XsSize index) const { @@ -596,7 +702,7 @@ struct XsArrayImpl : private XsArray { if (!m_size) throw std::out_of_range("out of range"); #endif - return *ptrAt(m_data, m_size-1); + return *ptrAt(m_data, m_size - 1); } /*! \brief indexed data access \sa operator[] \param index Index of item to access. \returns The item at \a index (by reference). */ inline T const& at(XsSize index) const @@ -607,7 +713,7 @@ struct XsArrayImpl : private XsArray { if (index >= m_size) throw std::out_of_range("index out of range"); #endif - return *ptrAt(m_data, index); + return *ptrAt(m_data, (ptrdiff_t) index); } /*! \brief indexed data access \sa operator[] \param index Index of item to access. \returns The item at \a index (by reference). */ inline T& at(XsSize index) @@ -685,15 +791,15 @@ struct XsArrayImpl : private XsArray { /*! \brief Adds \a item to the end of the array \sa XsArray_insert \param item The item to append to the array. */ inline void push_back(T const& item) { - insert(&item, (XsSize) -1, 1); + insert(&item, (XsSize) - 1, 1); } /*! \brief Removes \a count items from the end of the array \sa XsArray_erase \param count The number items to remove */ inline void pop_back(XsSize count = 1) { if (count >= size()) - erase(0, (XsSize) -1); + erase(0, (XsSize) - 1); else - erase(size()-count, count); + erase(size() - count, count); } /*! \brief Adds \a item to the start of the array \sa XsArray_insert \param item The item to insert at the front of the array */ inline void push_front(T const& item) @@ -709,7 +815,7 @@ struct XsArrayImpl : private XsArray { \returns The number of items currently in the array \sa reserved \sa setSize \sa resize */ - inline XsSize size() const + inline XsSize size() const noexcept { return m_size; } @@ -767,17 +873,23 @@ struct XsArrayImpl : private XsArray { return *this; } /*! \brief Returns whether the array is empty. \details This differs slightly from a straight check for size() != 0 in that it also works for fixed-size XsArrays. \returns true if the array is empty. */ - inline bool empty() const + inline bool empty() const noexcept { return (size() == 0) || (m_data == 0) || (m_flags & XSDF_Empty); } #ifndef XSENS_NOITERATOR /*! \brief Return the inheriting object */ - inline I const& inherited() const { return *static_cast(this); } + inline I const& inherited() const + { + return *static_cast(this); + } /*! \brief Return the inheriting object */ - inline I& inherited() { return *static_cast(this); } + inline I& inherited() + { + return *static_cast(this); + } #endif /*! \brief Swap the contents of the array with those of \a other. \param other The array to swap contents with. \sa XsArray_swap*/ @@ -817,13 +929,13 @@ struct XsArrayImpl : private XsArray { #endif /*! \copydoc XsArray_find */ - inline int find(T const& needle) const + inline ptrdiff_t find(T const& needle) const { return XsArray_find(this, &needle); } #ifndef SWIG /*! \copydoc XsArray_findPredicate */ - inline int findPredicate(T const& needle, XsArrayItemCompareFunc predicate) const + inline ptrdiff_t findPredicate(T const& needle, XsArrayItemCompareFunc predicate) const { return XsArray_findPredicate(this, &needle, predicate); } @@ -835,12 +947,12 @@ struct XsArrayImpl : private XsArray { beginning of the array it returns 0. If it points beyond the end, it returns the current size() */ template - XsSize indexOf(IteratorImplBase const& it) const + XsSize indexOf(IteratorImplBase const& it) const { ptrdiff_t d = ((char const*) it.ptr() - (char const*) m_data); if (d >= 0) { - XsSize r = d/D.itemSize; + XsSize r = d / D.itemSize; if (r <= size()) return r; return size(); @@ -885,7 +997,7 @@ struct XsArrayImpl : private XsArray { */ inline static const T* ptrAt(void const* ptr, ptrdiff_t count) { - return (const T*)(void const*)(((char const*)ptr)+count*(ptrdiff_t)D.itemSize); + return (const T*)(void const*)(((char const*)ptr) + count * (ptrdiff_t)D.itemSize); } /*! \internal @@ -899,7 +1011,7 @@ struct XsArrayImpl : private XsArray { */ inline static T* ptrAt(void* ptr, ptrdiff_t count) { - return (T*)(void*)(((char*)ptr)+count*(ptrdiff_t)D.itemSize); + return (T*)(void*)(((char*)ptr) + count * (ptrdiff_t)D.itemSize); } }; #endif diff --git a/lib/xspublic/xstypes/xsbaud.c b/lib/xspublic/xstypes/xsbaud.c index 7ef1fa6..58be5bf 100644 --- a/lib/xspublic/xstypes/xsbaud.c +++ b/lib/xspublic/xstypes/xsbaud.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -76,25 +76,38 @@ */ XsBaudRate XsBaud_codeToRate(XsBaudCode baudcode) { - switch(baudcode) + switch (baudcode) { - case XBC_4k8: return XBR_4800; - case XBC_9k6: return XBR_9600; -// case XBC_14k4: return XBR_14k4; - case XBC_19k2: return XBR_19k2; -// case XBC_28k8: return XBR_28k8; - case XBC_38k4: return XBR_38k4; - case XBC_57k6: return XBR_57k6; -// case XBC_76k8: return XBR_76k8; - case XBC_115k2: return XBR_115k2; - case XBC_230k4: return XBR_230k4; - case XBC_460k8: return XBR_460k8; - case XBC_921k6: - case XBC_921k6Legacy: return XBR_921k6; - case XBC_2MegaBaud: return XBR_2000k; - case XBC_3_5MegaBaud: return XBR_3500k; - case XBC_4MegaBaud: return XBR_4000k; - default: return XBR_Invalid; + case XBC_4k8: + return XBR_4800; + case XBC_9k6: + return XBR_9600; + // case XBC_14k4: return XBR_14k4; + case XBC_19k2: + return XBR_19k2; + // case XBC_28k8: return XBR_28k8; + case XBC_38k4: + return XBR_38k4; + case XBC_57k6: + return XBR_57k6; + // case XBC_76k8: return XBR_76k8; + case XBC_115k2: + return XBR_115k2; + case XBC_230k4: + return XBR_230k4; + case XBC_460k8: + return XBR_460k8; + case XBC_921k6: + case XBC_921k6Legacy: + return XBR_921k6; + case XBC_2MegaBaud: + return XBR_2000k; + case XBC_3_5MegaBaud: + return XBR_3500k; + case XBC_4MegaBaud: + return XBR_4000k; + default: + return XBR_Invalid; } } @@ -106,24 +119,37 @@ XsBaudRate XsBaud_codeToRate(XsBaudCode baudcode) */ XsBaudCode XsBaud_rateToCode(XsBaudRate baudrate) { - switch(baudrate) + switch (baudrate) { - case XBR_4800: return XBC_4k8; - case XBR_9600: return XBC_9k6; -// case XBR_14k4: return XBC_14k4; - case XBR_19k2: return XBC_19k2; -// case XBR_28k8: return XBC_28k8; - case XBR_38k4: return XBC_38k4; - case XBR_57k6: return XBC_57k6; -// case XBR_76k8: return XBC_76k8; - case XBR_115k2: return XBC_115k2; - case XBR_230k4: return XBC_230k4; - case XBR_460k8: return XBC_460k8; - case XBR_921k6: return XBC_921k6Legacy; - case XBR_2000k: return XBC_2MegaBaud; - case XBR_3500k: return XBC_3_5MegaBaud; - case XBR_4000k: return XBC_4MegaBaud; - default: return XBC_Invalid; + case XBR_4800: + return XBC_4k8; + case XBR_9600: + return XBC_9k6; + // case XBR_14k4: return XBC_14k4; + case XBR_19k2: + return XBC_19k2; + // case XBR_28k8: return XBC_28k8; + case XBR_38k4: + return XBC_38k4; + case XBR_57k6: + return XBC_57k6; + // case XBR_76k8: return XBC_76k8; + case XBR_115k2: + return XBC_115k2; + case XBR_230k4: + return XBC_230k4; + case XBR_460k8: + return XBC_460k8; + case XBR_921k6: + return XBC_921k6Legacy; + case XBR_2000k: + return XBC_2MegaBaud; + case XBR_3500k: + return XBC_3_5MegaBaud; + case XBR_4000k: + return XBC_4MegaBaud; + default: + return XBC_Invalid; } } @@ -135,24 +161,37 @@ XsBaudCode XsBaud_rateToCode(XsBaudRate baudrate) */ int XsBaud_rateToNumeric(XsBaudRate baudrate) { - switch(baudrate) + switch (baudrate) { - case XBR_4800: return 4800; - case XBR_9600: return 9600; -// case XBR_14k4: return 14400; - case XBR_19k2: return 19200; -// case XBR_28k8: return 28800; - case XBR_38k4: return 38400; - case XBR_57k6: return 57600; -// case XBR_76k8: return 76800; - case XBR_115k2: return 115200; - case XBR_230k4: return 230400; - case XBR_460k8: return 460800; - case XBR_921k6: return 921600; - case XBR_2000k: return 2000000; - case XBR_3500k: return 3500000; - case XBR_4000k: return 4000000; - default: return 0; + case XBR_4800: + return 4800; + case XBR_9600: + return 9600; + // case XBR_14k4: return 14400; + case XBR_19k2: + return 19200; + // case XBR_28k8: return 28800; + case XBR_38k4: + return 38400; + case XBR_57k6: + return 57600; + // case XBR_76k8: return 76800; + case XBR_115k2: + return 115200; + case XBR_230k4: + return 230400; + case XBR_460k8: + return 460800; + case XBR_921k6: + return 921600; + case XBR_2000k: + return 2000000; + case XBR_3500k: + return 3500000; + case XBR_4000k: + return 4000000; + default: + return 0; } } @@ -164,25 +203,38 @@ int XsBaud_rateToNumeric(XsBaudRate baudrate) */ XsBaudRate XsBaud_numericToRate(int numeric) { - switch(numeric) + switch (numeric) { - case 4800: return XBR_4800; - case 9600: return XBR_9600; -// case 14400: return XBR_14k4; - case 19200: return XBR_19k2; -// case 28800: return XBR_28k8; - case 38400: return XBR_38k4; - case 57600: return XBR_57k6; -// case 76800: return XBR_76k8; - case 115200: return XBR_115k2; - case 230400: return XBR_230k4; - case 460800: return XBR_460k8; - case 921600: return XBR_921k6; - case 2000000: return XBR_2000k; - case 3500000: return XBR_3500k; - case 4000000: return XBR_4000k; - default: return XBR_Invalid; + case 4800: + return XBR_4800; + case 9600: + return XBR_9600; + // case 14400: return XBR_14k4; + case 19200: + return XBR_19k2; + // case 28800: return XBR_28k8; + case 38400: + return XBR_38k4; + case 57600: + return XBR_57k6; + // case 76800: return XBR_76k8; + case 115200: + return XBR_115k2; + case 230400: + return XBR_230k4; + case 460800: + return XBR_460k8; + case 921600: + return XBR_921k6; + case 2000000: + return XBR_2000k; + case 3500000: + return XBR_3500k; + case 4000000: + return XBR_4000k; + default: + return XBR_Invalid; } } - /*! @} */ +/*! @} */ diff --git a/lib/xspublic/xstypes/xsbaud.h b/lib/xspublic/xstypes/xsbaud.h index fde22e4..5f6924d 100644 --- a/lib/xspublic/xstypes/xsbaud.h +++ b/lib/xspublic/xstypes/xsbaud.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -95,27 +95,28 @@ XSTYPES_DLL_API XsBaudRate XsBaud_numericToRate(int numeric); /*! \namespace XsBaud \brief Namespace for Baud rate and Baud code constants and conversions */ -namespace XsBaud { - /*! \copydoc XsBaud_codeToRate */ - inline XsBaudRate codeToRate(XsBaudCode baudcode) - { - return XsBaud_codeToRate(baudcode); - } - /*! \copydoc XsBaud_rateToCode */ - inline XsBaudCode rateToCode(XsBaudRate baudrate) - { - return XsBaud_rateToCode(baudrate); - } - /*! \copydoc XsBaud_rateToNumeric */ - inline int rateToNumeric(XsBaudRate baudrate) - { - return XsBaud_rateToNumeric(baudrate); - } - /*! \copydoc XsBaud_numericToRate*/ - inline XsBaudRate numericToRate(int numeric) - { - return XsBaud_numericToRate(numeric); - } +namespace XsBaud +{ +/*! \copydoc XsBaud_codeToRate */ +inline XsBaudRate codeToRate(XsBaudCode baudcode) +{ + return XsBaud_codeToRate(baudcode); +} +/*! \copydoc XsBaud_rateToCode */ +inline XsBaudCode rateToCode(XsBaudRate baudrate) +{ + return XsBaud_rateToCode(baudrate); +} +/*! \copydoc XsBaud_rateToNumeric */ +inline int rateToNumeric(XsBaudRate baudrate) +{ + return XsBaud_rateToNumeric(baudrate); +} +/*! \copydoc XsBaud_numericToRate*/ +inline XsBaudRate numericToRate(int numeric) +{ + return XsBaud_numericToRate(numeric); +} } #ifndef XSENS_NO_STL @@ -123,11 +124,11 @@ namespace XsBaud { namespace std { - template - basic_ostream<_CharT, _Traits>& operator<<(basic_ostream<_CharT, _Traits>& o, XsBaudRate const& xd) - { - return (o << XsBaud::rateToNumeric(xd)); - } +template +basic_ostream<_CharT, _Traits>& operator<<(basic_ostream<_CharT, _Traits>& o, XsBaudRate const& xd) +{ + return (o << XsBaud::rateToNumeric(xd)); +} } #endif diff --git a/lib/xspublic/xstypes/xsbaudcode.h b/lib/xspublic/xstypes/xsbaudcode.h index d796831..11cd304 100644 --- a/lib/xspublic/xstypes/xsbaudcode.h +++ b/lib/xspublic/xstypes/xsbaudcode.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -70,16 +70,17 @@ /*! \brief Internal baud rate configuration codes */ -enum XSNOCOMEXPORT XsBaudCode { +enum XSNOCOMEXPORT XsBaudCode +{ // Baudrate codes for SetBaudrate message XBC_4k8 = 0x0B, //!< 4k8 (4800 bps) XBC_9k6 = 0x09, //!< 9k6 (9600 bps) -// XBC_14k4 = 0x08, + // XBC_14k4 = 0x08, XBC_19k2 = 0x07, //!< 19k2 (19200 bps) -// XBC_28k8 = 0x06, + // XBC_28k8 = 0x06, XBC_38k4 = 0x05, //!< 38k4 (38400 bps) XBC_57k6 = 0x04, //!< 57k6 (57600 bps) -// XBC_76k8 = 0x03, + // XBC_76k8 = 0x03, XBC_115k2 = 0x02, //!< 115k2 (115200 bps) XBC_230k4 = 0x01, //!< 230k4 (230400 bps) XBC_460k8 = 0x00, //!< 460k8 (460800 bps) diff --git a/lib/xspublic/xstypes/xsbaudrate.h b/lib/xspublic/xstypes/xsbaudrate.h index 991ff7c..ab8bd81 100644 --- a/lib/xspublic/xstypes/xsbaudrate.h +++ b/lib/xspublic/xstypes/xsbaudrate.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -66,15 +66,15 @@ #define XSBAUDRATE_H #ifdef _WIN32 -# include + #include #else -# include -# ifndef B460800 -# undef B230400 -# define B230400 0010003 -# define B460800 0010004 -# define B921600 0010007 -# endif + #include + #ifndef B460800 + #undef B230400 + #define B230400 0010003 + #define B460800 0010004 + #define B921600 0010007 + #endif #endif /*! \addtogroup enums Global enumerations @@ -84,13 +84,14 @@ //AUTO namespace xstypes { /*! \brief Communication speed. */ -enum XsBaudRate { +enum XsBaudRate +{ XBR_Invalid = 0, //!< Not a valid baud rate XBR_4800 = CBR_4800, //!< 4k8 (4800 bps) XBR_9600 = CBR_9600, //!< 9k6 (9600 bps) -// XBR_14k4 = CBR_14400, + // XBR_14k4 = CBR_14400, XBR_19k2 = CBR_19200, //!< 19k2 (19200 bps) -// XBR_28k8 = 28800, + // XBR_28k8 = 28800, XBR_38k4 = CBR_38400, //!< 38k4 (38400 bps) XBR_57k6 = CBR_57600, //!< 57k6 (57600 bps) XBR_115k2 = CBR_115200, //!< 115k2 (115200 bps) @@ -106,28 +107,28 @@ enum XsBaudRate { #else /*! \brief Communication speed. */ -enum XSNOCOMEXPORT XsBaudRate { +enum XSNOCOMEXPORT XsBaudRate +{ // support high baudrates on MAC OS X #ifndef SWIG - #ifndef B2000000 - #define B2000000 2000000 - #endif - #ifndef B3500000 - #define B3500000 3500000 - #endif - #ifndef B4000000 - #define B4000000 4000000 - #endif +#define B2000000 2000000 +#endif +#ifndef B3500000 +#define B3500000 3500000 +#endif +#ifndef B4000000 +#define B4000000 4000000 +#endif #endif XBR_Invalid = 0, //!< Not a valid baud rate XBR_4800 = B4800, //!< 4k8 (4800 bps) XBR_9600 = B9600, //!< 9k6 (9600 bps) -// XBR_14k4 = B14400, + // XBR_14k4 = B14400, XBR_19k2 = B19200, //!< 19k2 (19200 bps) -// XBR_28k8 = B28800, + // XBR_28k8 = B28800, XBR_38k4 = B38400, //!< 38k4 (38400 bps) XBR_57k6 = B57600, //!< 57k6 (57600 bps) XBR_115k2 = B115200, //!< 115k2 (115200 bps) diff --git a/lib/xspublic/xstypes/xsbusid.h b/lib/xspublic/xstypes/xsbusid.h index 759e07e..c484302 100644 --- a/lib/xspublic/xstypes/xsbusid.h +++ b/lib/xspublic/xstypes/xsbusid.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xstypes/xsbytearray.c b/lib/xspublic/xstypes/xsbytearray.c index 28a36ec..edd12c2 100644 --- a/lib/xspublic/xstypes/xsbytearray.c +++ b/lib/xspublic/xstypes/xsbytearray.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -100,7 +100,8 @@ static int compareUint8(uint8_t const* a, uint8_t const* b) } //! \brief Descriptor for XsByteArray -XsArrayDescriptor const g_xsByteArrayDescriptor = { +XsArrayDescriptor const g_xsByteArrayDescriptor = +{ sizeof(uint8_t), XSEXPCASTITEMSWAP swapUint8, // swap 0, // construct diff --git a/lib/xspublic/xstypes/xsbytearray.h b/lib/xspublic/xstypes/xsbytearray.h index 813d9b5..9b88640 100644 --- a/lib/xspublic/xstypes/xsbytearray.h +++ b/lib/xspublic/xstypes/xsbytearray.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -97,16 +97,17 @@ XSTYPES_DLL_API void XsByteArray_construct(XsByteArray* thisPtr, XsSize count, u #ifdef __cplusplus } // extern "C" -struct XsByteArray : public XsArrayImpl { +struct XsByteArray : public XsArrayImpl +{ //! \brief Constructs an XsByteArray inline explicit XsByteArray(XsSize sz = 0, uint8_t const* src = 0) - : ArrayImpl(sz, src) + : ArrayImpl(sz, src) { } //! \brief Constructs an XsByteArray as a copy of \a other inline XsByteArray(XsByteArray const& other) - : ArrayImpl(other) + : ArrayImpl(other) { } @@ -136,14 +137,26 @@ struct XsByteArray : public XsArrayImpl(src.c_str())); + assign(src.size() + 1, reinterpret_cast(src.c_str())); } //! \brief Return a pointer to the internal data buffer - inline uint8_t* data() { return begin().operator ->(); } + inline uint8_t* data() + { + return begin().operator ->(); + } //! \brief Return a pointer to the internal data buffer - inline uint8_t const* data() const { return begin().operator ->(); } + inline uint8_t const* data() const + { + return begin().operator ->(); + } + + //! \brief Return an XsString with a copy of the contained data + inline XsString toXsString() const + { + return XsString(size(), reinterpret_cast(data())); + } /*! \brief Return the data at position \a offset converted into a T \details This function will translate a part of the contained data into a type T. T needs to be a real @@ -155,9 +168,9 @@ struct XsByteArray : public XsArrayImpl inline T getValue(XsSize offset) const { - assert(offset+sizeof(T) <= size()); + assert(offset + sizeof(T) <= size()); T tmp; - memcpy(&tmp, data()+offset, sizeof(T)); + memcpy(&tmp, data() + offset, sizeof(T)); return tmp; } @@ -170,7 +183,7 @@ struct XsByteArray : public XsArrayImpl= 201103L || _MSVC_LANG >= 201103L + static_assert(int(XCCI_LowestIdentifier) >= int(XCDI_HighestIdentifier), "XsCanConfigIdentifier and XsCanDataIdentifier ranges must be mutually exclusive"); +#endif + +typedef enum XsCanConfigIdentifier XsCanConfigIdentifier; + +#endif diff --git a/lib/xspublic/xstypes/xscandataidentifier.h b/lib/xspublic/xstypes/xscandataidentifier.h index fa69a36..28e2c28 100644 --- a/lib/xspublic/xstypes/xscandataidentifier.h +++ b/lib/xspublic/xstypes/xscandataidentifier.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -92,7 +92,7 @@ enum XsCanDataIdentifier XCDI_UtcTime = 0x07, /* Group Status Messages */ - XCDI_StatusWord = 0x11, + XCDI_StatusWord = 0x11, /* Group Quaternion Messages */ XCDI_Quaternion = 0x21, @@ -126,7 +126,8 @@ enum XsCanDataIdentifier XCDI_Velocity = 0x76, XCDI_Latitude = 0x77, XCDI_Longitude = 0x78, - XCDI_GnssSatInfo = 0x79, + XCDI_GnssReceiverStatus = 0x79, + XCDI_GnssReceiverDop = 0x7A, XCDI_EndOfGroup, //Keep this entry second to last. XCDI_HighestIdentifier, //Keep this entry last. Don't assign IDs with a higher value than this. diff --git a/lib/xspublic/xstypes/xscanframeformat.h b/lib/xspublic/xstypes/xscanframeformat.h index 2181fe3..419ea28 100644 --- a/lib/xspublic/xstypes/xscanframeformat.h +++ b/lib/xspublic/xstypes/xscanframeformat.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xstypes/xscanoutputconfiguration.c b/lib/xspublic/xstypes/xscanoutputconfiguration.c index 02bfc2b..372fc1c 100644 --- a/lib/xspublic/xstypes/xscanoutputconfiguration.c +++ b/lib/xspublic/xstypes/xscanoutputconfiguration.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xstypes/xscanoutputconfiguration.h b/lib/xspublic/xstypes/xscanoutputconfiguration.h index 2e21979..3b71de2 100644 --- a/lib/xspublic/xstypes/xscanoutputconfiguration.h +++ b/lib/xspublic/xstypes/xscanoutputconfiguration.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -94,7 +94,8 @@ XSTYPES_DLL_API void XsCanOutputConfiguration_swap(struct XsCanOutputConfigurati itself to its maximum frequency for the data type. If it is 0xFFFF and reported by the device, the data has no maximum frequency, but is sent along with appropriate packets (e.g. packet counter) */ -struct XsCanOutputConfiguration { +struct XsCanOutputConfiguration +{ XsCanFrameFormat m_frameFormat; //!< The frame format of the CAN message XsCanDataIdentifier m_dataIdentifier; //!< The data identifier uint32_t m_id; //!< The 11 or 29 bit ID identifier @@ -102,7 +103,7 @@ struct XsCanOutputConfiguration { #ifdef __cplusplus //! Constructor, initializes to an empty object - XsCanOutputConfiguration() + inline XsCanOutputConfiguration() : m_frameFormat(XCFF_11Bit_Identifier) , m_dataIdentifier(XCDI_Invalid) , m_id(0) @@ -110,21 +111,27 @@ struct XsCanOutputConfiguration { {} //! Constructor, initializes to specified values - XsCanOutputConfiguration(XsCanFrameFormat il, XsCanDataIdentifier di, uint32_t id, uint16_t freq) + inline XsCanOutputConfiguration(XsCanFrameFormat il, XsCanDataIdentifier di, uint32_t id, uint16_t freq) : m_frameFormat(il) , m_dataIdentifier(di) , m_id(id) , m_frequency(freq) {} - //! Comparison operator - bool operator == (const XsCanOutputConfiguration& other) const + //! Equality comparison operator + inline bool operator == (const XsCanOutputConfiguration& other) const { return (m_frameFormat == other.m_frameFormat && m_dataIdentifier == other.m_dataIdentifier && m_id == other.m_id && m_frequency == other.m_frequency); } + + //! Inequality comparison operator + inline bool operator != (const XsCanOutputConfiguration& other) const + { + return !(*this == other); + } #endif }; typedef struct XsCanOutputConfiguration XsCanOutputConfiguration; diff --git a/lib/xspublic/xstypes/xscanoutputconfigurationarray.c b/lib/xspublic/xstypes/xscanoutputconfigurationarray.c index 8613f66..5ee4ae9 100644 --- a/lib/xspublic/xstypes/xscanoutputconfigurationarray.c +++ b/lib/xspublic/xstypes/xscanoutputconfigurationarray.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -97,9 +97,7 @@ int compareXsCanOutputConfiguration(XsCanOutputConfiguration const* a, XsCanOutp if (a->m_frequency == b->m_frequency) { if (a->m_id == b->m_id) - { return (a->m_frameFormat < b->m_frameFormat) ? -1 : 1; - } return (a->m_id < b->m_id) ? -1 : 1; } return (a->m_frequency < b->m_frequency) ? -1 : 1; @@ -112,7 +110,8 @@ int compareXsCanOutputConfiguration(XsCanOutputConfiguration const* a, XsCanOutp //! \brief Descriptor for XsCanOutputConfigurationArray -XsArrayDescriptor const g_xsCanOutputConfigurationArrayDescriptor = { +XsArrayDescriptor const g_xsCanOutputConfigurationArrayDescriptor = +{ sizeof(XsCanOutputConfiguration), XSEXPCASTITEMSWAP swapXsCanOutputConfiguration, // swap 0, // construct diff --git a/lib/xspublic/xstypes/xscanoutputconfigurationarray.h b/lib/xspublic/xstypes/xscanoutputconfigurationarray.h index 060ba5a..a416976 100644 --- a/lib/xspublic/xstypes/xscanoutputconfigurationarray.h +++ b/lib/xspublic/xstypes/xscanoutputconfigurationarray.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -87,16 +87,17 @@ XSTYPES_DLL_API void XsCanOutputConfigurationArray_construct(XsCanOutputConfigur #ifdef __cplusplus } // extern "C" -struct XsCanOutputConfigurationArray : public XsArrayImpl { +struct XsCanOutputConfigurationArray : public XsArrayImpl +{ //! \brief Constructs an XsCanOutputConfigurationArray inline explicit XsCanOutputConfigurationArray(XsSize sz = 0, XsCanOutputConfiguration const* src = 0) - : ArrayImpl(sz, src) + : ArrayImpl(sz, src) { } //! \brief Constructs an XsCanOutputConfigurationArray as a copy of \a other inline XsCanOutputConfigurationArray(XsCanOutputConfigurationArray const& other) - : ArrayImpl(other) + : ArrayImpl(other) { } diff --git a/lib/xspublic/xstypes/xscontrolline.h b/lib/xspublic/xstypes/xscontrolline.h index 23334cf..93933ac 100644 --- a/lib/xspublic/xstypes/xscontrolline.h +++ b/lib/xspublic/xstypes/xscontrolline.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -69,7 +69,8 @@ @{ */ /*! \brief Serial control lines. */ -enum XsControlLine { +enum XsControlLine +{ XCL_DCD = 0x0001, //!< pin 1: Carrier Detect XCL_RD = 0x0002, //!< pin 2: Received Data XCL_TD = 0x0004, //!< pin 3: Transmitted Data diff --git a/lib/xspublic/xstypes/xscopy.h b/lib/xspublic/xstypes/xscopy.h index 7f0907f..1117efa 100644 --- a/lib/xspublic/xstypes/xscopy.h +++ b/lib/xspublic/xstypes/xscopy.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -94,4 +94,30 @@ #define XSLISTSWAP(C) XSLISTSWAP2(C, C##Array) +#ifdef __cplusplus +/*! \brief Byte-wise copy for data (de-)serialization + \details The function essentially performs a memcpy from \a src to \a tgt using the size of \a tgt + \param tgt The destination of the copy (reference) + \param src The source of the copy (pointer) +*/ +template +inline static void xsByteCopy(T& tgt, void const* src) +{ + memcpy(&tgt, src, sizeof(T)); +} + +/*! \brief Byte-wise copy for data (de-)serialization followed by a multiplication + \details The function essentially performs a memcpy from \a src to \a tgt using the size of \a tgt + \param tgt The destination of the copy (reference) + \param src The source of the copy (pointer) + \param mul The multiplication value +*/ +template +inline static void xsByteCopyMultiply(T& tgt, void const* src, T mul) +{ + memcpy(&tgt, src, sizeof(T)); + tgt *= mul; +} +#endif + #endif diff --git a/lib/xspublic/xstypes/xsdataidentifier.h b/lib/xspublic/xstypes/xsdataidentifier.h index a49d126..a307bd8 100644 --- a/lib/xspublic/xstypes/xsdataidentifier.h +++ b/lib/xspublic/xstypes/xsdataidentifier.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -178,13 +178,13 @@ enum XsDataIdentifier XDI_RetransmissionFlag = 0x0001, //!< Bit indicating if the snapshot if from a retransmission XDI_AwindaSnapshot = 0xC810, //!< Awinda type snapshot XDI_FullSnapshot = 0xC820, //!< Full snapshot - XDI_GloveSnapshotLeft = 0xC830, //!< Glove Snapshot for Left Hand - XDI_GloveSnapshotRight = 0xC840, //!< Glove Snapshot for Right Hand + XDI_GloveSnapshotLeft = 0xC830, //!< Glove Snapshot for Left Hand + XDI_GloveSnapshotRight = 0xC840, //!< Glove Snapshot for Right Hand // The numbers of the GloveData items should match the GloveSnapshot items, but in the CA range XDI_GloveDataGroup = 0xCA00, //!< Group for usable glove data - XDI_GloveDataLeft = 0xCA30, //!< Glove Data for Left Hand - XDI_GloveDataRight = 0xCA40, //!< Glove Data for Right Hand + XDI_GloveDataLeft = 0xCA30, //!< Glove Data for Left Hand + XDI_GloveDataRight = 0xCA40, //!< Glove Data for Right Hand XDI_VelocityGroup = 0xD000, //!< Group for velocity related outputs XDI_VelocityXYZ = 0xD010, //!< Velocity in XYZ coordinate frame @@ -204,15 +204,15 @@ typedef enum XsDataIdentifier XsDataIdentifier; #ifdef __cplusplus inline XsDataIdentifier operator | (XsDataIdentifier a, XsDataIdentifier b) { - return (XsDataIdentifier) ((int) a | (int) b); + return (XsDataIdentifier)((int) a | (int) b); } inline XsDataIdentifier operator & (XsDataIdentifier a, XsDataIdentifier b) { - return (XsDataIdentifier) ((int) a & (int) b); + return (XsDataIdentifier)((int) a & (int) b); } -inline XsDataIdentifier operator ~ (XsDataIdentifier a) +inline XsDataIdentifier operator ~(XsDataIdentifier a) { return (XsDataIdentifier) ~((unsigned short)a); } diff --git a/lib/xspublic/xstypes/xsdataidentifiervalue.h b/lib/xspublic/xstypes/xsdataidentifiervalue.h index ef7d28a..cabcc2c 100644 --- a/lib/xspublic/xstypes/xsdataidentifiervalue.h +++ b/lib/xspublic/xstypes/xsdataidentifiervalue.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xstypes/xsdatapacket.cpp b/lib/xspublic/xstypes/xsdatapacket.cpp index 0ec6a86..d4df539 100644 --- a/lib/xspublic/xstypes/xsdatapacket.cpp +++ b/lib/xspublic/xstypes/xsdatapacket.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -91,143 +91,143 @@ Variant* createVariant(XsDataIdentifier id) // It may be faster to create a static map with construct functions instead of this switch, but this is a much simpler implementation switch (id & XDI_FullTypeMask) { - //XDI_TemperatureGroup = 0x0800, - case XDI_Temperature: - return new SimpleVariant(id); - //case XDI_TimestampGroup :// 0x1000, - case XDI_UtcTime :// 0x1010, - return new XsTimeInfoVariant(id); - case XDI_PacketCounter :// 0x1020, - return new SimpleVariant(id); - case XDI_Itow :// 0x1030, - return new SimpleVariant(id); - case XDI_GnssAge :// 0x1040, - return new SimpleVariant(id); - case XDI_PressureAge :// 0x1050, - return new SimpleVariant(id); - case XDI_SampleTimeFine :// 0x1060, - case XDI_SampleTimeCoarse :// 0x1070, - return new SimpleVariant(id); - case XDI_FrameRange :// 0x1080, // add for MTw (if needed) - return new XsRangeVariant(id); - case XDI_PacketCounter8 :// 0x1090, - return new SimpleVariant(id); - case XDI_SampleTime64 :// 0x10A0, - return new SimpleVariant(id); - - //case XDI_OrientationGroup :// 0x2000, - case XDI_Quaternion :// 0x2010, - return new XsQuaternionVariant(id); - case XDI_RotationMatrix :// 0x2020, - return new XsMatrixVariant(id); - case XDI_EulerAngles :// 0x2030, - return new XsEulerVariant(id); - - //case XDI_PressureGroup :// 0x3000, - case XDI_BaroPressure :// 0x3010, - return new SimpleVariant(id); - - //case XDI_AccelerationGroup :// 0x4000, - case XDI_DeltaV :// 0x4010, - case XDI_Acceleration :// 0x4020, - case XDI_FreeAcceleration :// 0x4030, - case XDI_AccelerationHR :// 0x4040, - return new XsVector3Variant(id); - - //case XDI_PositionGroup :// 0x5000, - case XDI_AltitudeMsl :// 0x5010, - case XDI_AltitudeEllipsoid :// 0x5020, - return new SimpleVariant(id); - case XDI_PositionEcef :// 0x5030, - return new XsVector3Variant(id); - case XDI_LatLon :// 0x5040, - return new XsVector2Variant(id); - - //case XDI_SnapshotGroup :// 0xC800, - //case XDI_RetransmissionMask :// 0x0001, - //case XDI_RetransmissionFlag :// 0x0001, - case XDI_AwindaSnapshot :// 0xC810, - return new XsAwindaSnapshotVariant(id); - case XDI_FullSnapshot :// 0xC820, - return new XsFullSnapshotVariant(id); - - //case XDI_GnssGroup :// 0x7000, - case XDI_GnssPvtData :// 0x7010, - return new XsRawGnssPvtDataVariant(id); - case XDI_GnssSatInfo :// 0x7020, - return new XsRawGnssSatInfoVariant(id); - case XDI_GnssPvtPulse :// 0x7030 - return new SimpleVariant(id); - - //case XDI_AngularVelocityGroup :// 0x8000, - case XDI_RateOfTurn :// 0x8020, - case XDI_RateOfTurnHR :// 0x8040, - return new XsVector3Variant(id); - case XDI_DeltaQ :// 0x8030, - return new XsQuaternionVariant(id); - - //case XDI_RawSensorGroup :// 0xA000, - //case XDI_RawUnsigned :// 0x0000, //!< Tracker produces unsigned raw values, usually fixed behavior - //case XDI_RawSigned :// 0x0001, //!< Tracker produces signed raw values, usually fixed behavior - case XDI_RawAccGyrMagTemp :// 0xA010, - return new XsScrDataVariant(id); - - case XDI_RawGyroTemp :// 0xA020, - case XDI_RawAcc :// 0xA030, - case XDI_RawGyr :// 0xA040, - case XDI_RawMag :// 0xA050, - return new XsUShortVectorVariant(id); - - case XDI_RawDeltaQ :// 0xA060, - return new XsQuaternionVariant(id); - case XDI_RawDeltaV :// 0xA070, - return new XsVector3Variant(id); - - //case XDI_AnalogInGroup :// 0xB000, - case XDI_AnalogIn1 :// 0xB010, - case XDI_AnalogIn2 :// 0xB020, - return new SimpleVariant(id); - - //case XDI_MagneticGroup :// 0xC000, - case XDI_MagneticField :// 0xC020, - case XDI_MagneticFieldCorrected :// 0xC040, - - //case XDI_VelocityGroup :// 0xD000, - case XDI_VelocityXYZ :// 0xD010, - return new XsVector3Variant(id); - - //case XDI_StatusGroup :// 0xE000, - case XDI_StatusByte :// 0xE010, - return new SimpleVariant(id); - case XDI_StatusWord :// 0xE020, - return new SimpleVariant(id); - case XDI_Rssi :// 0xE040, - return new SimpleVariant(id); - case XDI_DeviceId :// 0xE080, - return new SimpleVariant(id); - case XDI_LocationId :// 0xE090 - return new SimpleVariant(id); - - //case XDI_IndicationGroup :// 0x4800, // 0100.1000 -> bit reverse = 0001.0010 -> type 18 - case XDI_TriggerIn1 :// 0x4810, - case XDI_TriggerIn2 :// 0x4820, - return new XsTriggerIndicationDataVariant(id); - - case XDI_RawBlob :// 0xA080 - return new XsByteArrayVariant(id); - - case XDI_GloveSnapshotLeft: // 0xC830 - case XDI_GloveSnapshotRight: // 0xC840 - return new XsGloveSnapshotVariant(id); - - case XDI_GloveDataLeft: // 0xC930 - case XDI_GloveDataRight: // 0xC940 - return new XsGloveDataVariant(id); - - default: - //JLERRORG("Unknown id: " << id); - assert(0); - return nullptr; + //XDI_TemperatureGroup = 0x0800, + case XDI_Temperature: + return new SimpleVariant(id); + //case XDI_TimestampGroup :// 0x1000, + case XDI_UtcTime :// 0x1010, + return new XsTimeInfoVariant(id); + case XDI_PacketCounter :// 0x1020, + return new SimpleVariant(id); + case XDI_Itow :// 0x1030, + return new SimpleVariant(id); + case XDI_GnssAge :// 0x1040, + return new SimpleVariant(id); + case XDI_PressureAge :// 0x1050, + return new SimpleVariant(id); + case XDI_SampleTimeFine :// 0x1060, + case XDI_SampleTimeCoarse :// 0x1070, + return new SimpleVariant(id); + case XDI_FrameRange :// 0x1080, // add for MTw (if needed) + return new XsRangeVariant(id); + case XDI_PacketCounter8 :// 0x1090, + return new SimpleVariant(id); + case XDI_SampleTime64 :// 0x10A0, + return new SimpleVariant(id); + + //case XDI_OrientationGroup :// 0x2000, + case XDI_Quaternion :// 0x2010, + return new XsQuaternionVariant(id); + case XDI_RotationMatrix :// 0x2020, + return new XsMatrixVariant(id); + case XDI_EulerAngles :// 0x2030, + return new XsEulerVariant(id); + + //case XDI_PressureGroup :// 0x3000, + case XDI_BaroPressure :// 0x3010, + return new SimpleVariant(id); + + //case XDI_AccelerationGroup :// 0x4000, + case XDI_DeltaV :// 0x4010, + case XDI_Acceleration :// 0x4020, + case XDI_FreeAcceleration :// 0x4030, + case XDI_AccelerationHR :// 0x4040, + return new XsVector3Variant(id); + + //case XDI_PositionGroup :// 0x5000, + case XDI_AltitudeMsl :// 0x5010, + case XDI_AltitudeEllipsoid :// 0x5020, + return new SimpleVariant(id); + case XDI_PositionEcef :// 0x5030, + return new XsVector3Variant(id); + case XDI_LatLon :// 0x5040, + return new XsVector2Variant(id); + + //case XDI_SnapshotGroup :// 0xC800, + //case XDI_RetransmissionMask :// 0x0001, + //case XDI_RetransmissionFlag :// 0x0001, + case XDI_AwindaSnapshot :// 0xC810, + return new XsAwindaSnapshotVariant(id); + case XDI_FullSnapshot :// 0xC820, + return new XsFullSnapshotVariant(id); + + //case XDI_GnssGroup :// 0x7000, + case XDI_GnssPvtData :// 0x7010, + return new XsRawGnssPvtDataVariant(id); + case XDI_GnssSatInfo :// 0x7020, + return new XsRawGnssSatInfoVariant(id); + case XDI_GnssPvtPulse :// 0x7030 + return new SimpleVariant(id); + + //case XDI_AngularVelocityGroup :// 0x8000, + case XDI_RateOfTurn :// 0x8020, + case XDI_RateOfTurnHR :// 0x8040, + return new XsVector3Variant(id); + case XDI_DeltaQ :// 0x8030, + return new XsQuaternionVariant(id); + + //case XDI_RawSensorGroup :// 0xA000, + //case XDI_RawUnsigned :// 0x0000, //!< Tracker produces unsigned raw values, usually fixed behavior + //case XDI_RawSigned :// 0x0001, //!< Tracker produces signed raw values, usually fixed behavior + case XDI_RawAccGyrMagTemp :// 0xA010, + return new XsScrDataVariant(id); + + case XDI_RawGyroTemp :// 0xA020, + case XDI_RawAcc :// 0xA030, + case XDI_RawGyr :// 0xA040, + case XDI_RawMag :// 0xA050, + return new XsUShortVectorVariant(id); + + case XDI_RawDeltaQ :// 0xA060, + return new XsQuaternionVariant(id); + case XDI_RawDeltaV :// 0xA070, + return new XsVector3Variant(id); + + //case XDI_AnalogInGroup :// 0xB000, + case XDI_AnalogIn1 :// 0xB010, + case XDI_AnalogIn2 :// 0xB020, + return new SimpleVariant(id); + + //case XDI_MagneticGroup :// 0xC000, + case XDI_MagneticField :// 0xC020, + case XDI_MagneticFieldCorrected :// 0xC040, + + //case XDI_VelocityGroup :// 0xD000, + case XDI_VelocityXYZ :// 0xD010, + return new XsVector3Variant(id); + + //case XDI_StatusGroup :// 0xE000, + case XDI_StatusByte :// 0xE010, + return new SimpleVariant(id); + case XDI_StatusWord :// 0xE020, + return new SimpleVariant(id); + case XDI_Rssi :// 0xE040, + return new SimpleVariant(id); + case XDI_DeviceId :// 0xE080, + return new SimpleVariant(id); + case XDI_LocationId :// 0xE090 + return new SimpleVariant(id); + + //case XDI_IndicationGroup :// 0x4800, // 0100.1000 -> bit reverse = 0001.0010 -> type 18 + case XDI_TriggerIn1 :// 0x4810, + case XDI_TriggerIn2 :// 0x4820, + return new XsTriggerIndicationDataVariant(id); + + case XDI_RawBlob :// 0xA080 + return new XsByteArrayVariant(id); + + case XDI_GloveSnapshotLeft: // 0xC830 + case XDI_GloveSnapshotRight: // 0xC840 + return new XsGloveSnapshotVariant(id); + + case XDI_GloveDataLeft: // 0xC930 + case XDI_GloveDataRight: // 0xC940 + return new XsGloveDataVariant(id); + + default: + //JLERRORG("Unknown id: " << id); + assert(0); + return nullptr; } } @@ -290,24 +290,25 @@ void genericSet(XsDataPacket* thisPtr, T const* val, XsDataIdentifier id) } template > -struct GenericSimple { -static T get(const XsDataPacket* thisPtr, XsDataIdentifier id, T const& failValue = T()) +struct GenericSimple { - auto it = MAP.find(id); - if (it != MAP.end()) - return it->second->toDerived().m_data; - return failValue; -} + static T get(const XsDataPacket* thisPtr, XsDataIdentifier id, T const& failValue = T()) + { + auto it = MAP.find(id); + if (it != MAP.end()) + return it->second->toDerived().m_data; + return failValue; + } -static void set(XsDataPacket* thisPtr, T val, XsDataIdentifier id) -{ - detach(thisPtr); - auto it = MAP.find(id); - if (it != MAP.end()) - it->second->toDerived().m_data = val; - else - MAP.insert(id, new V(id, val)); -} + static void set(XsDataPacket* thisPtr, T val, XsDataIdentifier id) + { + detach(thisPtr); + auto it = MAP.find(id); + if (it != MAP.end()) + it->second->toDerived().m_data = val; + else + MAP.insert(id, new V(id, val)); + } }; inline bool genericContains(const XsDataPacket* thisPtr, XsDataIdentifier id) @@ -350,57 +351,57 @@ XsQuaternion preRotFromXdi(XsDataIdentifier actual, XsDataIdentifier desired) { static const XsQuaternion q_id(1, 0, 0, 0); // x -> x static const XsQuaternion q_nwu2ned(0, 1, 0, 0); // nwu -> ned = 180 degrees x - static const XsQuaternion q_enu2nwu(1.4142135623730950488016887242097*0.5, 0, 0, -1.4142135623730950488016887242097*0.5); // enu -> nwu = 90 degrees z - static const XsQuaternion q_enu2ned(0, -1.4142135623730950488016887242097*0.5, -1.4142135623730950488016887242097*0.5, 0); // enu -> ned = 90 degrees z followed by 180 degrees x + static const XsQuaternion q_enu2nwu(1.4142135623730950488016887242097 * 0.5, 0, 0, -1.4142135623730950488016887242097 * 0.5); // enu -> nwu = 90 degrees z + static const XsQuaternion q_enu2ned(0, -1.4142135623730950488016887242097 * 0.5, -1.4142135623730950488016887242097 * 0.5, 0); // enu -> ned = 90 degrees z followed by 180 degrees x static const XsQuaternion q_ned2nwu(0, -1, 0, 0); - static const XsQuaternion q_nwu2enu(1.4142135623730950488016887242097*0.5, 0, 0, 1.4142135623730950488016887242097*0.5); - static const XsQuaternion q_ned2enu(0, 1.4142135623730950488016887242097*0.5, 1.4142135623730950488016887242097*0.5, 0); + static const XsQuaternion q_nwu2enu(1.4142135623730950488016887242097 * 0.5, 0, 0, 1.4142135623730950488016887242097 * 0.5); + static const XsQuaternion q_ned2enu(0, 1.4142135623730950488016887242097 * 0.5, 1.4142135623730950488016887242097 * 0.5, 0); switch (desired & XDI_CoordSysMask) { - default: - case XDI_CoordSysEnu: - switch (actual & XDI_CoordSysMask) - { default: case XDI_CoordSysEnu: - return q_id; - - case XDI_CoordSysNed: - return q_ned2enu; + switch (actual & XDI_CoordSysMask) + { + default: + case XDI_CoordSysEnu: + return q_id; - case XDI_CoordSysNwu: - return q_nwu2enu; - } + case XDI_CoordSysNed: + return q_ned2enu; - case XDI_CoordSysNed: - switch (actual & XDI_CoordSysMask) - { - default: - case XDI_CoordSysEnu: - return q_enu2ned; + case XDI_CoordSysNwu: + return q_nwu2enu; + } case XDI_CoordSysNed: - return q_id; - - case XDI_CoordSysNwu: - return q_nwu2ned; - } + switch (actual & XDI_CoordSysMask) + { + default: + case XDI_CoordSysEnu: + return q_enu2ned; - case XDI_CoordSysNwu: - switch (actual & XDI_CoordSysMask) - { - default: - case XDI_CoordSysEnu: - return q_enu2nwu; + case XDI_CoordSysNed: + return q_id; - case XDI_CoordSysNed: - return q_ned2nwu; + case XDI_CoordSysNwu: + return q_nwu2ned; + } case XDI_CoordSysNwu: - return q_id; - } + switch (actual & XDI_CoordSysMask) + { + default: + case XDI_CoordSysEnu: + return q_enu2nwu; + + case XDI_CoordSysNed: + return q_ned2nwu; + + case XDI_CoordSysNwu: + return q_id; + } } } @@ -419,2207 +420,2209 @@ XsQuaternion preRotFromXdi(XsDataIdentifier actual, XsDataIdentifier desired) extern "C" { -/*! \brief Inits a data packet, the packet will be empty after construction -*/ -void XsDataPacket_construct(XsDataPacket* thisPtr) -{ - thisPtr->d = new DataPacketPrivate; - thisPtr->m_deviceId = 0; - thisPtr->m_toa = 0; - thisPtr->m_packetId = -1; - thisPtr->m_etos = 0; -} + /*! \brief Inits a data packet, the packet will be empty after construction + */ + void XsDataPacket_construct(XsDataPacket* thisPtr) + { + thisPtr->d = new DataPacketPrivate; + thisPtr->m_deviceId = 0; + thisPtr->m_toa = 0; + thisPtr->m_packetId = -1; + thisPtr->m_etos = 0; + } -/*! \brief Initializes a data packet as a (referenced) copy of \a src - \param src The data packet to reference-copy from -*/ -void XsDataPacket_copyConstruct(XsDataPacket* thisPtr, XsDataPacket const* src) -{ - ++src->d->m_refCount; - thisPtr->d = src->d; - thisPtr->m_deviceId = src->m_deviceId; - thisPtr->m_toa = src->m_toa; - thisPtr->m_packetId = src->m_packetId; - thisPtr->m_etos = src->m_etos; -} + /*! \brief Initializes a data packet as a (referenced) copy of \a src + \param src The data packet to reference-copy from + */ + void XsDataPacket_copyConstruct(XsDataPacket* thisPtr, XsDataPacket const* src) + { + ++src->d->m_refCount; + thisPtr->d = src->d; + thisPtr->m_deviceId = src->m_deviceId; + thisPtr->m_toa = src->m_toa; + thisPtr->m_packetId = src->m_packetId; + thisPtr->m_etos = src->m_etos; + } -/*! \brief Clears and frees data in an XsDataPacket -*/ -void XsDataPacket_destruct(XsDataPacket* thisPtr) -{ - if (thisPtr->d && --thisPtr->d->m_refCount == 0) - delete thisPtr->d; - thisPtr->d = nullptr; -} + /*! \brief Clears and frees data in an XsDataPacket + */ + void XsDataPacket_destruct(XsDataPacket* thisPtr) + { + if (thisPtr->d && --thisPtr->d->m_refCount == 0) + delete thisPtr->d; + thisPtr->d = nullptr; + } -/*! \brief Clears all data in an XsDataPacket - \param id The id to clear, supply XDI_None to clear the entire object -*/ -void XsDataPacket_clear(XsDataPacket* thisPtr, XsDataIdentifier id) -{ - detach(thisPtr); - if (id == XDI_None) + /*! \brief Clears all data in an XsDataPacket + \param id The id to clear, supply XDI_None to clear the entire object + */ + void XsDataPacket_clear(XsDataPacket* thisPtr, XsDataIdentifier id) { - XsDataPacket_destruct(thisPtr); - XsDataPacket_construct(thisPtr); + detach(thisPtr); + if (id == XDI_None) + { + XsDataPacket_destruct(thisPtr); + XsDataPacket_construct(thisPtr); + } + else + MAP.erase(id); } - else - MAP.erase(id); -} -/*! \brief Copy the XsDataPacket to \a copy - \param copy The object to copy to - \param src The source to copy from -*/ -void XsDataPacket_copy(XsDataPacket* copy, XsDataPacket const* src) -{ - if (copy->d != src->d) + /*! \brief Copy the XsDataPacket to \a copy + \param copy The object to copy to + \param src The source to copy from + */ + void XsDataPacket_copy(XsDataPacket* copy, XsDataPacket const* src) { - ++src->d->m_refCount; - if (--copy->d->m_refCount == 0) - delete copy->d; - copy->d = src->d; - } - copy->m_deviceId = src->m_deviceId; - copy->m_toa = src->m_toa; - copy->m_packetId = src->m_packetId; - copy->m_etos = src->m_etos; -} + if (copy->d != src->d) + { + ++src->d->m_refCount; + if (--copy->d->m_refCount == 0) + delete copy->d; + copy->d = src->d; + } + copy->m_deviceId = src->m_deviceId; + copy->m_toa = src->m_toa; + copy->m_packetId = src->m_packetId; + copy->m_etos = src->m_etos; + } -/*! \brief Swaps the XsDataPackets in \a thisPtr and \a other - \param other The object to swap with -*/ -void XsDataPacket_swap(XsDataPacket* thisPtr, XsDataPacket* other) -{ - using std::swap; - swap(thisPtr->d, other->d); - swap(thisPtr->m_deviceId, other->m_deviceId); - swap(thisPtr->m_toa, other->m_toa); - swap(thisPtr->m_packetId, other->m_packetId); - swap(thisPtr->m_etos, other->m_etos); -} + /*! \brief Swaps the XsDataPackets in \a thisPtr and \a other + \param other The object to swap with + */ + void XsDataPacket_swap(XsDataPacket* thisPtr, XsDataPacket* other) + { + using std::swap; + swap(thisPtr->d, other->d); + swap(thisPtr->m_deviceId, other->m_deviceId); + swap(thisPtr->m_toa, other->m_toa); + swap(thisPtr->m_packetId, other->m_packetId); + swap(thisPtr->m_etos, other->m_etos); + } -/*! \brief Returns whether the datapacket is empty - \return True when the XsDataPacket is empty -*/ -int XsDataPacket_empty(const XsDataPacket* thisPtr) -{ - return 0 != MAP.empty(); -} + /*! \brief Returns whether the datapacket is empty + \return True when the XsDataPacket is empty + */ + int XsDataPacket_empty(const XsDataPacket* thisPtr) + { + return 0 != MAP.empty(); + } -/*! \brief Returns the number of individual items in the XsDataPacket - \return The number of individual items in the XsDataPacket -*/ -int XsDataPacket_itemCount(const XsDataPacket* thisPtr) -{ - assert(thisPtr); - return (int) MAP.size(); -} + /*! \brief Returns the number of individual items in the XsDataPacket + \return The number of individual items in the XsDataPacket + */ + int XsDataPacket_itemCount(const XsDataPacket* thisPtr) + { + assert(thisPtr); + return (int) MAP.size(); + } -/*! \brief Returns the dataformat of a specific data identifier in the packet + /*! \brief Returns the dataformat of a specific data identifier in the packet - \param id : The XsDataIdentifier to query - \returns Returns XDI_None if the packet does not contain the dataidentifier, the data - format otherwise + \param id : The XsDataIdentifier to query + \returns Returns XDI_None if the packet does not contain the dataidentifier, the data + format otherwise - \sa XsDataIdentifier -*/ -XsDataIdentifier XsDataPacket_dataFormat(const XsDataPacket* thisPtr, XsDataIdentifier id) -{ - auto it = MAP.find(id); - if (it == MAP.end()) - return XDI_None; - return it->second->dataId() & XDI_SubFormatMask; -} + \sa XsDataIdentifier + */ + XsDataIdentifier XsDataPacket_dataFormat(const XsDataPacket* thisPtr, XsDataIdentifier id) + { + auto it = MAP.find(id); + if (it == MAP.end()) + return XDI_None; + return it->second->dataId() & XDI_SubFormatMask; + } -/*! \brief helper for XsDataPacket_convertRawVector */ -static XsReal signed_cast(uint16_t v) -{ - return (int16_t)v; -} + /*! \brief helper for XsDataPacket_convertRawVector */ + static XsReal signed_cast(uint16_t v) + { + return (int16_t)v; + } -/*! \brief helper for XsDataPacket_convertRawVector */ -static XsReal unsigned_cast(uint16_t v) -{ - return v; -} + /*! \brief helper for XsDataPacket_convertRawVector */ + static XsReal unsigned_cast(uint16_t v) + { + return v; + } -/*! \brief The raw accelerometer component of a data item. + /*! \brief The raw accelerometer component of a data item. - \param returnVal : An XsUShortVector to put the requested data in + \param returnVal : An XsUShortVector to put the requested data in - \returns A XsUShortVector containing the x, y and z axis values in that order -*/ -XsUShortVector* XsDataPacket_rawAcceleration(const XsDataPacket* thisPtr, XsUShortVector* returnVal) -{ - return rawVector(thisPtr, returnVal, XDI_RawAcc, &XsScrData::m_acc); -} + \returns A XsUShortVector containing the x, y and z axis values in that order + */ + XsUShortVector* XsDataPacket_rawAcceleration(const XsDataPacket* thisPtr, XsUShortVector* returnVal) + { + return rawVector(thisPtr, returnVal, XDI_RawAcc, &XsScrData::m_acc); + } -/*! \brief Check if data item contains Raw Accelerometer data - \returns true if this packet contains raw acceleration data -*/ -int XsDataPacket_containsRawAcceleration(const XsDataPacket* thisPtr) -{ - return MAP.find(XDI_RawAccGyrMagTemp) != MAP.end() || + /*! \brief Check if data item contains Raw Accelerometer data + \returns true if this packet contains raw acceleration data + */ + int XsDataPacket_containsRawAcceleration(const XsDataPacket* thisPtr) + { + return MAP.find(XDI_RawAccGyrMagTemp) != MAP.end() || MAP.find(XDI_RawAcc) != MAP.end(); -} + } -/*! \brief Add/update raw accelerometer data for the item + /*! \brief Add/update raw accelerometer data for the item - \param vec : The data to update the XsDataPacket with + \param vec : The data to update the XsDataPacket with - \details This will add the raw acceleration from \a vec to the data packet. If - the packet already contains raw acceleration, it will be replaced. -*/ -void XsDataPacket_setRawAcceleration(XsDataPacket* thisPtr, const XsUShortVector* vec) -{ - setRawVector(thisPtr, vec, XDI_RawAcc, &XsScrData::m_acc); -} + \details This will add the raw acceleration from \a vec to the data packet. If + the packet already contains raw acceleration, it will be replaced. + */ + void XsDataPacket_setRawAcceleration(XsDataPacket* thisPtr, const XsUShortVector* vec) + { + setRawVector(thisPtr, vec, XDI_RawAcc, &XsScrData::m_acc); + } -/*! \brief The raw gyroscope component of a data item. + /*! \brief The raw gyroscope component of a data item. - \param returnVal : An XsUShortVector to put the requested data in + \param returnVal : An XsUShortVector to put the requested data in - \returns A XsUShortVector containing the x, y and z axis values in that order -*/ -XsUShortVector* XsDataPacket_rawGyroscopeData(const XsDataPacket* thisPtr, XsUShortVector* returnVal) -{ - return rawVector(thisPtr, returnVal, XDI_RawGyr, &XsScrData::m_gyr); -} + \returns A XsUShortVector containing the x, y and z axis values in that order + */ + XsUShortVector* XsDataPacket_rawGyroscopeData(const XsDataPacket* thisPtr, XsUShortVector* returnVal) + { + return rawVector(thisPtr, returnVal, XDI_RawGyr, &XsScrData::m_gyr); + } -/*! \brief Check if data item contains raw gyroscope data - \returns true if this packet contains raw gyroscope data -*/ -int XsDataPacket_containsRawGyroscopeData(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_RawAccGyrMagTemp) || genericContains(thisPtr, XDI_RawGyr); -} + /*! \brief Check if data item contains raw gyroscope data + \returns true if this packet contains raw gyroscope data + */ + int XsDataPacket_containsRawGyroscopeData(const XsDataPacket* thisPtr) + { + return genericContains(thisPtr, XDI_RawAccGyrMagTemp) || genericContains(thisPtr, XDI_RawGyr); + } -/*! \brief Add/update raw gyroscope data for the item - \param vec The new data to set -*/ -void XsDataPacket_setRawGyroscopeData(XsDataPacket* thisPtr, const XsUShortVector* vec) -{ - setRawVector(thisPtr, vec, XDI_RawGyr, &XsScrData::m_gyr); -} + /*! \brief Add/update raw gyroscope data for the item + \param vec The new data to set + */ + void XsDataPacket_setRawGyroscopeData(XsDataPacket* thisPtr, const XsUShortVector* vec) + { + setRawVector(thisPtr, vec, XDI_RawGyr, &XsScrData::m_gyr); + } -/*! \brief The raw magnetometer component of a data item. + /*! \brief The raw magnetometer component of a data item. - \param returnVal : An XsUShortVector to put the requested data in + \param returnVal : An XsUShortVector to put the requested data in - \returns A XsUShortVector containing the x, y and z axis values in that order -*/ -XsUShortVector* XsDataPacket_rawMagneticField(const XsDataPacket* thisPtr, XsUShortVector* returnVal) -{ - return rawVector(thisPtr, returnVal, XDI_RawMag, &XsScrData::m_mag); -} + \returns A XsUShortVector containing the x, y and z axis values in that order + */ + XsUShortVector* XsDataPacket_rawMagneticField(const XsDataPacket* thisPtr, XsUShortVector* returnVal) + { + return rawVector(thisPtr, returnVal, XDI_RawMag, &XsScrData::m_mag); + } -/*! \brief Check if data item contains raw magnetometer data - \returns true if this packet contains raw magnetometer data -*/ -int XsDataPacket_containsRawMagneticField(const XsDataPacket* thisPtr) -{ - return MAP.find(XDI_RawAccGyrMagTemp) != MAP.end() || + /*! \brief Check if data item contains raw magnetometer data + \returns true if this packet contains raw magnetometer data + */ + int XsDataPacket_containsRawMagneticField(const XsDataPacket* thisPtr) + { + return MAP.find(XDI_RawAccGyrMagTemp) != MAP.end() || MAP.find(XDI_RawMag) != MAP.end(); -} - -/*! \brief Add/update raw magnetometer data for the item - \param vec The new data to set -*/ -void XsDataPacket_setRawMagneticField(XsDataPacket* thisPtr, const XsUShortVector* vec) -{ - setRawVector(thisPtr, vec, XDI_RawMag, &XsScrData::m_mag); -} + } -/*! \brief The raw temperature component of a data item. + /*! \brief Add/update raw magnetometer data for the item + \param vec The new data to set + */ + void XsDataPacket_setRawMagneticField(XsDataPacket* thisPtr, const XsUShortVector* vec) + { + setRawVector(thisPtr, vec, XDI_RawMag, &XsScrData::m_mag); + } - \returns An uint16_t containing the raw temperature value -*/ -uint16_t XsDataPacket_rawTemperature(const XsDataPacket* thisPtr) -{ - auto it = MAP.find(XDI_RawAccGyrMagTemp); - if (it != MAP.end()) - return it->second->toDerived().m_data.m_temp; - else - return 0; -} + /*! \brief The raw temperature component of a data item. -/*! \brief Check if data item contains raw temperature data - \returns true if this packet contains raw temperature data -*/ -int XsDataPacket_containsRawTemperature(const XsDataPacket* thisPtr) -{ - return MAP.find(XDI_RawAccGyrMagTemp) != MAP.end(); -} + \returns An uint16_t containing the raw temperature value + */ + uint16_t XsDataPacket_rawTemperature(const XsDataPacket* thisPtr) + { + auto it = MAP.find(XDI_RawAccGyrMagTemp); + if (it != MAP.end()) + return it->second->toDerived().m_data.m_temp; + else + return 0; + } -/*! \brief Add/update raw temperature data for the item - \param temp The new data to set -*/ -void XsDataPacket_setRawTemperature(XsDataPacket* thisPtr, uint16_t temp) -{ - detach(thisPtr); - auto it = MAP.find(XDI_RawAccGyrMagTemp); - if (it != MAP.end()) + /*! \brief Check if data item contains raw temperature data + \returns true if this packet contains raw temperature data + */ + int XsDataPacket_containsRawTemperature(const XsDataPacket* thisPtr) { - it->second->toDerived().m_data.m_temp = temp; + return MAP.find(XDI_RawAccGyrMagTemp) != MAP.end(); } - else + + /*! \brief Add/update raw temperature data for the item + \param temp The new data to set + */ + void XsDataPacket_setRawTemperature(XsDataPacket* thisPtr, uint16_t temp) { - auto v = new XsScrDataVariant(XDI_RawAccGyrMagTemp); - v->m_data.m_temp = temp; - MAP.insert(XDI_RawAccGyrMagTemp, v); + detach(thisPtr); + auto it = MAP.find(XDI_RawAccGyrMagTemp); + if (it != MAP.end()) + it->second->toDerived().m_data.m_temp = temp; + else + { + auto v = new XsScrDataVariant(XDI_RawAccGyrMagTemp); + v->m_data.m_temp = temp; + MAP.insert(XDI_RawAccGyrMagTemp, v); + } } -} -/*! \brief The raw gyroscope temperature component of a data item. + /*! \brief The raw gyroscope temperature component of a data item. - \param returnVal : An XsUShortVector to put the requested data in + \param returnVal : An XsUShortVector to put the requested data in - \returns A XsUShortVector containing the x, y and z axis values in that order -*/ -XsUShortVector* XsDataPacket_rawGyroscopeTemperatureData(const XsDataPacket* thisPtr, XsUShortVector* returnVal) -{ - assert(returnVal); - auto it = MAP.find(XDI_RawGyroTemp); - if (it != MAP.end()) - *returnVal = it->second->toDerived().m_data; - return returnVal; -} + \returns A XsUShortVector containing the x, y and z axis values in that order + */ + XsUShortVector* XsDataPacket_rawGyroscopeTemperatureData(const XsDataPacket* thisPtr, XsUShortVector* returnVal) + { + assert(returnVal); + auto it = MAP.find(XDI_RawGyroTemp); + if (it != MAP.end()) + *returnVal = it->second->toDerived().m_data; + return returnVal; + } -/*! \brief Check if data item contains raw gyroscope temperature data - \returns true if this packet contains raw gyroscope temperature data -*/ -int XsDataPacket_containsRawGyroscopeTemperatureData(const XsDataPacket* thisPtr) -{ - return MAP.find(XDI_RawGyroTemp) != MAP.end(); -} + /*! \brief Check if data item contains raw gyroscope temperature data + \returns true if this packet contains raw gyroscope temperature data + */ + int XsDataPacket_containsRawGyroscopeTemperatureData(const XsDataPacket* thisPtr) + { + return MAP.find(XDI_RawGyroTemp) != MAP.end(); + } -/*! \brief Add/update raw gyroscope temperature data for the item - \param vec The new data to set -*/ -void XsDataPacket_setRawGyroscopeTemperatureData(XsDataPacket* thisPtr, const XsUShortVector* vec) -{ - detach(thisPtr); - auto it = MAP.find(XDI_RawGyroTemp); - if (it != MAP.end()) - it->second->toDerived().m_data = *vec; - else + /*! \brief Add/update raw gyroscope temperature data for the item + \param vec The new data to set + */ + void XsDataPacket_setRawGyroscopeTemperatureData(XsDataPacket* thisPtr, const XsUShortVector* vec) { - auto v = new XsUShortVectorVariant(XDI_RawGyroTemp); - v->m_data = *vec; - MAP.insert(XDI_RawGyroTemp, v); + detach(thisPtr); + auto it = MAP.find(XDI_RawGyroTemp); + if (it != MAP.end()) + it->second->toDerived().m_data = *vec; + else + { + auto v = new XsUShortVectorVariant(XDI_RawGyroTemp); + v->m_data = *vec; + MAP.insert(XDI_RawGyroTemp, v); + } } -} -/*! \brief Return the raw data component of a data item. - \param returnVal The object to store the requested data in - \return The raw data component of a data item. -*/ -XsScrData* XsDataPacket_rawData(const XsDataPacket* thisPtr, XsScrData* returnVal) -{ - assert(returnVal); - auto it = MAP.find(XDI_RawAccGyrMagTemp); - if (it != MAP.end()) - *returnVal = it->second->toDerived().m_data; - else + /*! \brief Return the raw data component of a data item. + \param returnVal The object to store the requested data in + \return The raw data component of a data item. + */ + XsScrData* XsDataPacket_rawData(const XsDataPacket* thisPtr, XsScrData* returnVal) { - for (XsSize i = 0; i < 3; ++i) + assert(returnVal); + auto it = MAP.find(XDI_RawAccGyrMagTemp); + if (it != MAP.end()) + *returnVal = it->second->toDerived().m_data; + else { - returnVal->m_acc[i] = 0; - returnVal->m_gyr[i] = 0; - returnVal->m_mag[i] = 0; + for (XsSize i = 0; i < 3; ++i) + { + returnVal->m_acc[i] = 0; + returnVal->m_gyr[i] = 0; + returnVal->m_mag[i] = 0; + } + returnVal->m_temp = 0; } - returnVal->m_temp = 0; + return returnVal; // not found } - return returnVal; // not found -} -/*! \brief Check if data item contains raw data - \returns true if this packet contains raw data -*/ -int XsDataPacket_containsRawData(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_RawAccGyrMagTemp); -} + /*! \brief Check if data item contains raw data + \returns true if this packet contains raw data + */ + int XsDataPacket_containsRawData(const XsDataPacket* thisPtr) + { + return genericContains(thisPtr, XDI_RawAccGyrMagTemp); + } -/*! \brief Add/update raw data for the item - \param data The new data to set -*/ -void XsDataPacket_setRawData(XsDataPacket* thisPtr, const XsScrData* data) -{ - detach(thisPtr); - auto it = MAP.find(XDI_RawAccGyrMagTemp); - if (it != MAP.end()) - it->second->toDerived().m_data = *data; - else + /*! \brief Add/update raw data for the item + \param data The new data to set + */ + void XsDataPacket_setRawData(XsDataPacket* thisPtr, const XsScrData* data) { - auto v = new XsScrDataVariant(XDI_RawAccGyrMagTemp); - v->m_data = *data; - MAP.insert(XDI_RawAccGyrMagTemp, v); + detach(thisPtr); + auto it = MAP.find(XDI_RawAccGyrMagTemp); + if (it != MAP.end()) + it->second->toDerived().m_data = *data; + else + { + auto v = new XsScrDataVariant(XDI_RawAccGyrMagTemp); + v->m_data = *data; + MAP.insert(XDI_RawAccGyrMagTemp, v); + } } -} -/*! \brief The delta velocity (deltaV) component of a data item. -\param returnVal : The XsVector that the deltaV will be assigned to -\returns An XsVector containing the x, y and z axis values in that order -*/ -XsVector* XsDataPacket_velocityIncrement(const XsDataPacket* thisPtr, XsVector* returnVal) -{ - return genericGet(thisPtr, returnVal, XDI_DeltaV); -} + /*! \brief The delta velocity (deltaV) component of a data item. + \param returnVal : The XsVector that the deltaV will be assigned to + \returns An XsVector containing the x, y and z axis values in that order + */ + XsVector* XsDataPacket_velocityIncrement(const XsDataPacket* thisPtr, XsVector* returnVal) + { + return genericGet(thisPtr, returnVal, XDI_DeltaV); + } -/*! \brief Check if data item contains delta velocity data -\returns true if this packet contains delta velocity data -*/ -int XsDataPacket_containsVelocityIncrement(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_DeltaV); -} + /*! \brief Check if data item contains delta velocity data + \returns true if this packet contains delta velocity data + */ + int XsDataPacket_containsVelocityIncrement(const XsDataPacket* thisPtr) + { + return genericContains(thisPtr, XDI_DeltaV); + } -/*! \brief Add/update delta velocity data for the item -\param vec : The data to update the XsDataPacket with -*/ -void XsDataPacket_setVelocityIncrement(XsDataPacket* thisPtr, const XsVector* vec) -{ - genericSet(thisPtr, vec, XDI_DeltaV | XDI_SubFormatDouble); -} + /*! \brief Add/update delta velocity data for the item + \param vec : The data to update the XsDataPacket with + */ + void XsDataPacket_setVelocityIncrement(XsDataPacket* thisPtr, const XsVector* vec) + { + genericSet(thisPtr, vec, XDI_DeltaV | XDI_SubFormatDouble); + } -/*! \brief The calibrated accelerometer component of a data item. + /*! \brief The calibrated accelerometer component of a data item. - \param returnVal : The XsVector that the calibrated acceleration will be assigned to + \param returnVal : The XsVector that the calibrated acceleration will be assigned to - \returns A XsVector containing the x, y and z axis values in that order -*/ -XsVector* XsDataPacket_calibratedAcceleration(const XsDataPacket* thisPtr, XsVector* returnVal) -{ - return genericGet(thisPtr, returnVal, XDI_Acceleration); -} + \returns A XsVector containing the x, y and z axis values in that order + */ + XsVector* XsDataPacket_calibratedAcceleration(const XsDataPacket* thisPtr, XsVector* returnVal) + { + return genericGet(thisPtr, returnVal, XDI_Acceleration); + } -/*! \brief Check if data item contains calibrated accelerometer data - \returns true if this packet contains calibrated accelerometer data -*/ -int XsDataPacket_containsCalibratedAcceleration(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_Acceleration); -} + /*! \brief Check if data item contains calibrated accelerometer data + \returns true if this packet contains calibrated accelerometer data + */ + int XsDataPacket_containsCalibratedAcceleration(const XsDataPacket* thisPtr) + { + return genericContains(thisPtr, XDI_Acceleration); + } -/*! \brief Add/update calibrated accelerometer data for the item - \param vec : The data to update the XsDataPacket with -*/ -void XsDataPacket_setCalibratedAcceleration(XsDataPacket* thisPtr, const XsVector* vec) -{ - genericSet(thisPtr, vec, XDI_Acceleration | XDI_SubFormatDouble); -} + /*! \brief Add/update calibrated accelerometer data for the item + \param vec : The data to update the XsDataPacket with + */ + void XsDataPacket_setCalibratedAcceleration(XsDataPacket* thisPtr, const XsVector* vec) + { + genericSet(thisPtr, vec, XDI_Acceleration | XDI_SubFormatDouble); + } -/*! \brief The delta quaternion (deltaQ) component of a data item. -\param returnVal : The XsQuaternion that the deltaQ will be assigned to -\returns An XsQuaternion containing the deltaQ value -*/ -XsQuaternion* XsDataPacket_orientationIncrement(const XsDataPacket* thisPtr, XsQuaternion* returnVal) -{ - return genericGet(thisPtr, returnVal, XDI_DeltaQ); -} + /*! \brief The delta quaternion (deltaQ) component of a data item. + \param returnVal : The XsQuaternion that the deltaQ will be assigned to + \returns An XsQuaternion containing the deltaQ value + */ + XsQuaternion* XsDataPacket_orientationIncrement(const XsDataPacket* thisPtr, XsQuaternion* returnVal) + { + return genericGet(thisPtr, returnVal, XDI_DeltaQ); + } -/*! \brief Check if data item contains delta quaternion data -\returns true if this packet contains delta quaternion data -*/ -int XsDataPacket_containsOrientationIncrement(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_DeltaQ); -} + /*! \brief Check if data item contains delta quaternion data + \returns true if this packet contains delta quaternion data + */ + int XsDataPacket_containsOrientationIncrement(const XsDataPacket* thisPtr) + { + return genericContains(thisPtr, XDI_DeltaQ); + } -/*! \brief Add/update delta quaternion data for the item -\param quat : The data to update the XsDataPacket with -*/ -void XsDataPacket_setOrientationIncrement(XsDataPacket* thisPtr, const XsQuaternion* quat) -{ - genericSet(thisPtr, quat, XDI_DeltaQ | XDI_SubFormatDouble); -} + /*! \brief Add/update delta quaternion data for the item + \param quat : The data to update the XsDataPacket with + */ + void XsDataPacket_setOrientationIncrement(XsDataPacket* thisPtr, const XsQuaternion* quat) + { + genericSet(thisPtr, quat, XDI_DeltaQ | XDI_SubFormatDouble); + } -/*! \brief The calibrated gyroscope component of a data item. + /*! \brief The calibrated gyroscope component of a data item. - \param returnVal : An XsVector to put the requested data in + \param returnVal : An XsVector to put the requested data in - \returns A XsUShortVector containing the x, y and z axis values in that order -*/ -XsVector* XsDataPacket_calibratedGyroscopeData(const XsDataPacket* thisPtr, XsVector* returnVal) -{ - return genericGet(thisPtr, returnVal, XDI_RateOfTurn); -} + \returns A XsUShortVector containing the x, y and z axis values in that order + */ + XsVector* XsDataPacket_calibratedGyroscopeData(const XsDataPacket* thisPtr, XsVector* returnVal) + { + return genericGet(thisPtr, returnVal, XDI_RateOfTurn); + } -/*! \brief Check if data item contains calibrated gyroscope data - \returns true if this packet contains calibrated gyroscope data -*/ -int XsDataPacket_containsCalibratedGyroscopeData(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_RateOfTurn); -} + /*! \brief Check if data item contains calibrated gyroscope data + \returns true if this packet contains calibrated gyroscope data + */ + int XsDataPacket_containsCalibratedGyroscopeData(const XsDataPacket* thisPtr) + { + return genericContains(thisPtr, XDI_RateOfTurn); + } -/*! \brief Add/update calibrated gyroscope data for the item - \param vec : The data to update the XsDataPacket with -*/ -void XsDataPacket_setCalibratedGyroscopeData(XsDataPacket* thisPtr, const XsVector* vec) -{ - genericSet(thisPtr, vec, XDI_RateOfTurn | XDI_SubFormatDouble); -} + /*! \brief Add/update calibrated gyroscope data for the item + \param vec : The data to update the XsDataPacket with + */ + void XsDataPacket_setCalibratedGyroscopeData(XsDataPacket* thisPtr, const XsVector* vec) + { + genericSet(thisPtr, vec, XDI_RateOfTurn | XDI_SubFormatDouble); + } -/*! \brief The calibrated magnetometer component of a data item. + /*! \brief The calibrated magnetometer component of a data item. - \param returnVal : An XsVector to put the requested in + \param returnVal : An XsVector to put the requested in - \returns A XsUShortVector containing the x, y and z axis values in that order -*/ -XsVector* XsDataPacket_calibratedMagneticField(const XsDataPacket* thisPtr, XsVector* returnVal) -{ - return genericGet(thisPtr, returnVal, XDI_MagneticField); -} + \returns A XsUShortVector containing the x, y and z axis values in that order + */ + XsVector* XsDataPacket_calibratedMagneticField(const XsDataPacket* thisPtr, XsVector* returnVal) + { + return genericGet(thisPtr, returnVal, XDI_MagneticField); + } -/*! \brief Check if data item contains calibrated magnetometer data - \returns true if this packet contains calibrated magnetometer data -*/ -int XsDataPacket_containsCalibratedMagneticField(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_MagneticField); -} + /*! \brief Check if data item contains calibrated magnetometer data + \returns true if this packet contains calibrated magnetometer data + */ + int XsDataPacket_containsCalibratedMagneticField(const XsDataPacket* thisPtr) + { + return genericContains(thisPtr, XDI_MagneticField); + } -/*! \brief Add/update calibrated magnetometer data for the item - \param vec : The data to update the XsDataPacket with -*/ -void XsDataPacket_setCalibratedMagneticField(XsDataPacket* thisPtr, const XsVector* vec) -{ - genericSet(thisPtr, vec, XDI_MagneticField | XDI_SubFormatDouble); -} + /*! \brief Add/update calibrated magnetometer data for the item + \param vec : The data to update the XsDataPacket with + */ + void XsDataPacket_setCalibratedMagneticField(XsDataPacket* thisPtr, const XsVector* vec) + { + genericSet(thisPtr, vec, XDI_MagneticField | XDI_SubFormatDouble); + } -/*! \brief Return the calibrated Data component of a data item. - \param returnVal Storage for the requested data - \returns Returns the supplied \a returnVal filled with the requested data -*/ -XsCalibratedData* XsDataPacket_calibratedData(const XsDataPacket* thisPtr, XsCalibratedData* returnVal) -{ - assert(returnVal); + /*! \brief Return the calibrated Data component of a data item. + \param returnVal Storage for the requested data + \returns Returns the supplied \a returnVal filled with the requested data + */ + XsCalibratedData* XsDataPacket_calibratedData(const XsDataPacket* thisPtr, XsCalibratedData* returnVal) + { + assert(returnVal); - XsDataPacket_calibratedAcceleration(thisPtr, &returnVal->m_acc); - XsDataPacket_calibratedGyroscopeData(thisPtr, &returnVal->m_gyr); - XsDataPacket_calibratedMagneticField(thisPtr, &returnVal->m_mag); + XsDataPacket_calibratedAcceleration(thisPtr, &returnVal->m_acc); + XsDataPacket_calibratedGyroscopeData(thisPtr, &returnVal->m_gyr); + XsDataPacket_calibratedMagneticField(thisPtr, &returnVal->m_mag); - return returnVal; -} + return returnVal; + } -/*! \brief Check if data item contains calibrated Data - \returns Returns whether the packet contains calibrated data or not - \note Calibrated data is only present if *all* components are present. -*/ -int XsDataPacket_containsCalibratedData(const XsDataPacket* thisPtr) -{ - // Note: calibrated data is only present if *all* components are present - return genericContains(thisPtr, XDI_Acceleration) && + /*! \brief Check if data item contains calibrated Data + \returns Returns whether the packet contains calibrated data or not + \note Calibrated data is only present if *all* components are present. + */ + int XsDataPacket_containsCalibratedData(const XsDataPacket* thisPtr) + { + // Note: calibrated data is only present if *all* components are present + return genericContains(thisPtr, XDI_Acceleration) && genericContains(thisPtr, XDI_RateOfTurn) && genericContains(thisPtr, XDI_MagneticField); -} + } -/*! \brief Add/update calibrated Data for the item - \param data : The data to update the XsDataPacket with - */ -void XsDataPacket_setCalibratedData(XsDataPacket* thisPtr, const XsCalibratedData* data) -{ - XsDataPacket_setCalibratedAcceleration(thisPtr, &data->m_acc); - XsDataPacket_setCalibratedGyroscopeData(thisPtr, &data->m_gyr); - XsDataPacket_setCalibratedMagneticField(thisPtr, &data->m_mag); -} + /*! \brief Add/update calibrated Data for the item + \param data : The data to update the XsDataPacket with + */ + void XsDataPacket_setCalibratedData(XsDataPacket* thisPtr, const XsCalibratedData* data) + { + XsDataPacket_setCalibratedAcceleration(thisPtr, &data->m_acc); + XsDataPacket_setCalibratedGyroscopeData(thisPtr, &data->m_gyr); + XsDataPacket_setCalibratedMagneticField(thisPtr, &data->m_mag); + } -/*! \brief The corrected magnetometer component of a data item (ICC result). + /*! \brief The corrected magnetometer component of a data item (ICC result). -\param returnVal : An XsVector to put the requested in + \param returnVal : An XsVector to put the requested in -\returns A XsUShortVector containing the x, y and z axis values in that order -*/ -XsVector* XsDataPacket_correctedMagneticField(const XsDataPacket* thisPtr, XsVector* returnVal) -{ - return genericGet(thisPtr, returnVal, XDI_MagneticFieldCorrected); -} + \returns A XsUShortVector containing the x, y and z axis values in that order + */ + XsVector* XsDataPacket_correctedMagneticField(const XsDataPacket* thisPtr, XsVector* returnVal) + { + return genericGet(thisPtr, returnVal, XDI_MagneticFieldCorrected); + } -/*! \brief Check if data item contains corrected magnetometer data (ICC result). -\returns true if this packet contains corrected magnetometer data -*/ -int XsDataPacket_containsCorrectedMagneticField(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_MagneticFieldCorrected); -} + /*! \brief Check if data item contains corrected magnetometer data (ICC result). + \returns true if this packet contains corrected magnetometer data + */ + int XsDataPacket_containsCorrectedMagneticField(const XsDataPacket* thisPtr) + { + return genericContains(thisPtr, XDI_MagneticFieldCorrected); + } -/*! \brief Add/update corrected magnetometer data for the item (ICC result). -\param vec : The data to update the XsDataPacket with -*/ -void XsDataPacket_setCorrectedMagneticField(XsDataPacket* thisPtr, const XsVector* vec) -{ - genericSet(thisPtr, vec, XDI_MagneticFieldCorrected | XDI_SubFormatDouble); -} + /*! \brief Add/update corrected magnetometer data for the item (ICC result). + \param vec : The data to update the XsDataPacket with + */ + void XsDataPacket_setCorrectedMagneticField(XsDataPacket* thisPtr, const XsVector* vec) + { + genericSet(thisPtr, vec, XDI_MagneticFieldCorrected | XDI_SubFormatDouble); + } -/*! \brief Return the orientation component of a data item as a quaternion. + /*! \brief Return the orientation component of a data item as a quaternion. - \param returnVal An %XsQuaternion to put the requested orientation in - \param coordinateSystem The coordinate system of the requested orientation. If this does not match - the stored coordinate system, it will be transformed to the requested orientation. + \param returnVal An %XsQuaternion to put the requested orientation in + \param coordinateSystem The coordinate system of the requested orientation. If this does not match + the stored coordinate system, it will be transformed to the requested orientation. - \returns An XsQuaternion containing the orientation data -*/ -XsQuaternion* XsDataPacket_orientationQuaternion(const XsDataPacket* thisPtr, XsQuaternion* returnVal, XsDataIdentifier coordinateSystem) -{ - assert(returnVal); - auto it = MAP.find(XDI_Quaternion); - if (it != MAP.end()) + \returns An XsQuaternion containing the orientation data + */ + XsQuaternion* XsDataPacket_orientationQuaternion(const XsDataPacket* thisPtr, XsQuaternion* returnVal, XsDataIdentifier coordinateSystem) { - *returnVal = it->second->toDerived().m_data; - XsDataIdentifier foundId = it->second->dataId(); - if ((coordinateSystem & XDI_CoordSysMask) != (foundId & XDI_CoordSysMask)) + assert(returnVal); + auto it = MAP.find(XDI_Quaternion); + if (it != MAP.end()) + { + *returnVal = it->second->toDerived().m_data; + XsDataIdentifier foundId = it->second->dataId(); + if ((coordinateSystem & XDI_CoordSysMask) != (foundId & XDI_CoordSysMask)) + { + XsQuaternion rot; + rot = preRotFromXdi(foundId, coordinateSystem); + XsQuaternion_multiply(&rot, returnVal, returnVal); + } + } + else if (XsDataPacket_containsOrientationMatrix(thisPtr)) + { + XsMatrix3x3 m; + XsDataPacket_orientationMatrix(thisPtr, &m, coordinateSystem); + returnVal->fromRotationMatrix(m); + } + else if (XsDataPacket_containsOrientationEuler(thisPtr)) { - XsQuaternion rot; - rot = preRotFromXdi(foundId, coordinateSystem); - XsQuaternion_multiply(&rot, returnVal, returnVal); + XsEuler eul; + XsDataPacket_orientationEuler(thisPtr, &eul, coordinateSystem); + returnVal->fromEulerAngles(eul); } + //else + // memset(returnVal->m_data, 0, 4*sizeof(XsReal)); + + return returnVal; } - else if (XsDataPacket_containsOrientationMatrix(thisPtr)) + + /*! \brief Removes all orientations from the datapacket */ + static void removeAllOrientations(XsDataPacket* thisPtr) { - XsMatrix3x3 m; - XsDataPacket_orientationMatrix(thisPtr, &m, coordinateSystem); - returnVal->fromRotationMatrix(m); + detach(thisPtr); + MAP.erase(XDI_Quaternion); + MAP.erase(XDI_RotationMatrix); + MAP.erase(XDI_EulerAngles); } - else if (XsDataPacket_containsOrientationEuler(thisPtr)) + + /*! \brief Add/update quaternion orientation Data for the item + \param data The new data to set + \param coordinateSystem The coordinate system of the requested orientation. + */ + void XsDataPacket_setOrientationQuaternion(XsDataPacket* thisPtr, const XsQuaternion* data, XsDataIdentifier coordinateSystem) { - XsEuler eul; - XsDataPacket_orientationEuler(thisPtr, &eul, coordinateSystem); - returnVal->fromEulerAngles(eul); + // always create a new one + removeAllOrientations(thisPtr); + MAP.insert(XDI_Quaternion, new XsQuaternionVariant(XDI_Quaternion | XDI_SubFormatDouble | (coordinateSystem & XDI_CoordSysMask), *data)); } - //else - // memset(returnVal->m_data, 0, 4*sizeof(XsReal)); - - return returnVal; -} - -/*! \brief Removes all orientations from the datapacket */ -static void removeAllOrientations(XsDataPacket* thisPtr) -{ - detach(thisPtr); - MAP.erase(XDI_Quaternion); - MAP.erase(XDI_RotationMatrix); - MAP.erase(XDI_EulerAngles); -} - -/*! \brief Add/update quaternion orientation Data for the item - \param data The new data to set - \param coordinateSystem The coordinate system of the requested orientation. -*/ -void XsDataPacket_setOrientationQuaternion(XsDataPacket* thisPtr, const XsQuaternion* data, XsDataIdentifier coordinateSystem) -{ - // always create a new one - removeAllOrientations(thisPtr); - MAP.insert(XDI_Quaternion, new XsQuaternionVariant(XDI_Quaternion | XDI_SubFormatDouble | (coordinateSystem & XDI_CoordSysMask), *data)); -} -/*! \brief Return the orientation component of a data item as a euler angles. + /*! \brief Return the orientation component of a data item as a euler angles. - \param returnVal An %XsEuler to put the requested orientation in - \param coordinateSystem The coordinate system of the requested orientation. If this does not match - the stored coordinate system, it will be transformed to the requested orientation. + \param returnVal An %XsEuler to put the requested orientation in + \param coordinateSystem The coordinate system of the requested orientation. If this does not match + the stored coordinate system, it will be transformed to the requested orientation. - \returns A %XsEuler containing the orientation data -*/ -XsEuler* XsDataPacket_orientationEuler(const XsDataPacket* thisPtr, XsEuler* returnVal, XsDataIdentifier coordinateSystem) -{ - assert(returnVal); - auto it = MAP.find(XDI_EulerAngles); - if (it != MAP.end()) + \returns A %XsEuler containing the orientation data + */ + XsEuler* XsDataPacket_orientationEuler(const XsDataPacket* thisPtr, XsEuler* returnVal, XsDataIdentifier coordinateSystem) { - *returnVal = it->second->toDerived().m_data; - XsDataIdentifier foundId = it->second->dataId(); - if ((coordinateSystem & XDI_CoordSysMask) != (foundId & XDI_CoordSysMask)) + assert(returnVal); + auto it = MAP.find(XDI_EulerAngles); + if (it != MAP.end()) { - XsQuaternion rot, q; - rot = preRotFromXdi(foundId, coordinateSystem); - XsQuaternion_fromEulerAngles(&q, returnVal); - XsQuaternion_multiply(&rot, &q, &q); - XsEuler_fromQuaternion(returnVal, &q); + *returnVal = it->second->toDerived().m_data; + XsDataIdentifier foundId = it->second->dataId(); + if ((coordinateSystem & XDI_CoordSysMask) != (foundId & XDI_CoordSysMask)) + { + XsQuaternion rot, q; + rot = preRotFromXdi(foundId, coordinateSystem); + XsQuaternion_fromEulerAngles(&q, returnVal); + XsQuaternion_multiply(&rot, &q, &q); + XsEuler_fromQuaternion(returnVal, &q); + } } + else if (XsDataPacket_containsOrientationMatrix(thisPtr)) + { + XsMatrix3x3 m; + XsDataPacket_orientationMatrix(thisPtr, &m, coordinateSystem); + XsQuaternion q; + q.fromRotationMatrix(m); + returnVal->fromQuaternion(q); + } + else if (XsDataPacket_containsOrientationQuaternion(thisPtr)) + { + XsQuaternion q; + XsDataPacket_orientationQuaternion(thisPtr, &q, coordinateSystem); + returnVal->fromQuaternion(q); + } + //else + //{ + // returnVal->m_x = XsMath_zero; + // returnVal->m_y = XsMath_zero; + // returnVal->m_z = XsMath_zero; + //} + + return returnVal; } - else if (XsDataPacket_containsOrientationMatrix(thisPtr)) - { - XsMatrix3x3 m; - XsDataPacket_orientationMatrix(thisPtr, &m, coordinateSystem); - XsQuaternion q; - q.fromRotationMatrix(m); - returnVal->fromQuaternion(q); - } - else if (XsDataPacket_containsOrientationQuaternion(thisPtr)) + + /*! \brief Add/update quaternion orientation Data for the item + \param data The new data to set + \param coordinateSystem The coordinate system of the orientation. + */ + void XsDataPacket_setOrientationEuler(XsDataPacket* thisPtr, const XsEuler* data, XsDataIdentifier coordinateSystem) { - XsQuaternion q; - XsDataPacket_orientationQuaternion(thisPtr, &q, coordinateSystem); - returnVal->fromQuaternion(q); + // always create a new one + removeAllOrientations(thisPtr); + MAP.insert(XDI_EulerAngles, new XsEulerVariant(XDI_EulerAngles | XDI_SubFormatDouble | (coordinateSystem & XDI_CoordSysMask), *data)); } - //else - //{ - // returnVal->m_x = XsMath_zero; - // returnVal->m_y = XsMath_zero; - // returnVal->m_z = XsMath_zero; - //} - - return returnVal; -} - -/*! \brief Add/update quaternion orientation Data for the item - \param data The new data to set - \param coordinateSystem The coordinate system of the orientation. -*/ -void XsDataPacket_setOrientationEuler(XsDataPacket* thisPtr, const XsEuler* data, XsDataIdentifier coordinateSystem) -{ - // always create a new one - removeAllOrientations(thisPtr); - MAP.insert(XDI_EulerAngles, new XsEulerVariant(XDI_EulerAngles | XDI_SubFormatDouble | (coordinateSystem & XDI_CoordSysMask), *data)); -} -/*! \brief Return the orientation component of a data item as a orientation matrix. + /*! \brief Return the orientation component of a data item as a orientation matrix. - \param returnVal An %XsMatrix to put the requested orientation in - \param coordinateSystem The coordinate system of the requested orientation. If this does not match - the stored coordinate system, it will be transformed to the requested orientation. + \param returnVal An %XsMatrix to put the requested orientation in + \param coordinateSystem The coordinate system of the requested orientation. If this does not match + the stored coordinate system, it will be transformed to the requested orientation. - \returns An %XsMatrix containing the orientation data -*/ -XsMatrix* XsDataPacket_orientationMatrix(const XsDataPacket* thisPtr, XsMatrix* returnVal, XsDataIdentifier coordinateSystem) -{ - assert(returnVal); - auto it = MAP.find(XDI_RotationMatrix); - if (it != MAP.end()) + \returns An %XsMatrix containing the orientation data + */ + XsMatrix* XsDataPacket_orientationMatrix(const XsDataPacket* thisPtr, XsMatrix* returnVal, XsDataIdentifier coordinateSystem) { - *returnVal = it->second->toDerived().m_data; - XsDataIdentifier foundId = it->second->dataId(); - if ((coordinateSystem & XDI_CoordSysMask) != (foundId & XDI_CoordSysMask)) + assert(returnVal); + auto it = MAP.find(XDI_RotationMatrix); + if (it != MAP.end()) + { + *returnVal = it->second->toDerived().m_data; + XsDataIdentifier foundId = it->second->dataId(); + if ((coordinateSystem & XDI_CoordSysMask) != (foundId & XDI_CoordSysMask)) + { + XsQuaternion rot, q; + rot = preRotFromXdi(foundId, coordinateSystem); + XsQuaternion_fromRotationMatrix(&q, returnVal); + XsQuaternion_multiply(&rot, &q, &q); + XsMatrix_fromQuaternion(returnVal, &q); + } + } + else if (XsDataPacket_containsOrientationQuaternion(thisPtr)) { - XsQuaternion rot, q; - rot = preRotFromXdi(foundId, coordinateSystem); - XsQuaternion_fromRotationMatrix(&q, returnVal); - XsQuaternion_multiply(&rot, &q, &q); - XsMatrix_fromQuaternion(returnVal, &q); + XsQuaternion q; + XsDataPacket_orientationQuaternion(thisPtr, &q, coordinateSystem); + returnVal->fromQuaternion(q); } + else if (XsDataPacket_containsOrientationEuler(thisPtr)) + { + XsEuler eul; + XsDataPacket_orientationEuler(thisPtr, &eul, coordinateSystem); + XsQuaternion q; + q.fromEulerAngles(eul); + returnVal->fromQuaternion(q); + } + //else + // memset(returnVal->m_data, 0, 4*sizeof(XsReal)); + + return returnVal; } - else if (XsDataPacket_containsOrientationQuaternion(thisPtr)) - { - XsQuaternion q; - XsDataPacket_orientationQuaternion(thisPtr, &q, coordinateSystem); - returnVal->fromQuaternion(q); - } - else if (XsDataPacket_containsOrientationEuler(thisPtr)) + + /*! \brief Add/update quaternion orientation Data for the item + \param data The new data to set + \param coordinateSystem The coordinate system of the orientation. + */ + void XsDataPacket_setOrientationMatrix(XsDataPacket* thisPtr, const XsMatrix* data, XsDataIdentifier coordinateSystem) { - XsEuler eul; - XsDataPacket_orientationEuler(thisPtr, &eul, coordinateSystem); - XsQuaternion q; - q.fromEulerAngles(eul); - returnVal->fromQuaternion(q); + // always create a new one + removeAllOrientations(thisPtr); + MAP.insert(XDI_RotationMatrix, new XsMatrixVariant(XDI_RotationMatrix | XDI_SubFormatDouble | (coordinateSystem & XDI_CoordSysMask), *data)); } - //else - // memset(returnVal->m_data, 0, 4*sizeof(XsReal)); - - return returnVal; -} -/*! \brief Add/update quaternion orientation Data for the item - \param data The new data to set - \param coordinateSystem The coordinate system of the orientation. -*/ -void XsDataPacket_setOrientationMatrix(XsDataPacket* thisPtr, const XsMatrix* data, XsDataIdentifier coordinateSystem) -{ - // always create a new one - removeAllOrientations(thisPtr); - MAP.insert(XDI_RotationMatrix, new XsMatrixVariant(XDI_RotationMatrix | XDI_SubFormatDouble | (coordinateSystem & XDI_CoordSysMask), *data)); -} - -/*! \brief Check if data item contains orientation Data of any kind - \returns true if this packet contains orientation data -*/ -int XsDataPacket_containsOrientation(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_Quaternion) || + /*! \brief Check if data item contains orientation Data of any kind + \returns true if this packet contains orientation data + */ + int XsDataPacket_containsOrientation(const XsDataPacket* thisPtr) + { + return genericContains(thisPtr, XDI_Quaternion) || genericContains(thisPtr, XDI_EulerAngles) || genericContains(thisPtr, XDI_RotationMatrix); -} + } -/*! \brief Returns the data identifier of the first orientation data of any kind in the packet - \returns The %XsDataIdentifier of the first orientation data of any kind in the packet -*/ -XsDataIdentifier XsDataPacket_orientationIdentifier(const XsDataPacket* thisPtr) -{ - auto it = MAP.find(XDI_Quaternion); - if (it != MAP.end()) - return it->second->dataId(); + /*! \brief Returns the data identifier of the first orientation data of any kind in the packet + \returns The %XsDataIdentifier of the first orientation data of any kind in the packet + */ + XsDataIdentifier XsDataPacket_orientationIdentifier(const XsDataPacket* thisPtr) + { + auto it = MAP.find(XDI_Quaternion); + if (it != MAP.end()) + return it->second->dataId(); - it = MAP.find(XDI_EulerAngles); - if (it != MAP.end()) - return it->second->dataId(); + it = MAP.find(XDI_EulerAngles); + if (it != MAP.end()) + return it->second->dataId(); - it = MAP.find(XDI_RotationMatrix); - if (it != MAP.end()) - return it->second->dataId(); + it = MAP.find(XDI_RotationMatrix); + if (it != MAP.end()) + return it->second->dataId(); - return XDI_None; -} + return XDI_None; + } -/*! \brief Returns the coordinate system of the first orientation data of any kind in the packet - \returns The XsDataIdentifier of the coordinate system of the first orientation data of any kind in the packet -*/ -XsDataIdentifier XsDataPacket_coordinateSystemOrientation(const XsDataPacket* thisPtr) -{ - return XsDataPacket_orientationIdentifier(thisPtr) & XDI_CoordSysMask; -} + /*! \brief Returns the coordinate system of the first orientation data of any kind in the packet + \returns The XsDataIdentifier of the coordinate system of the first orientation data of any kind in the packet + */ + XsDataIdentifier XsDataPacket_coordinateSystemOrientation(const XsDataPacket* thisPtr) + { + return XsDataPacket_orientationIdentifier(thisPtr) & XDI_CoordSysMask; + } -/*! \brief Check if data item contains pressure data - \returns true if this packet contains pressure data -*/ -int XsDataPacket_containsPressure(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_BaroPressure); -} + /*! \brief Check if data item contains pressure data + \returns true if this packet contains pressure data + */ + int XsDataPacket_containsPressure(const XsDataPacket* thisPtr) + { + return genericContains(thisPtr, XDI_BaroPressure); + } -/*! \brief Check if data item contains pressure age data - \returns true if this packet contains pressure age data -*/ -int XsDataPacket_containsPressureAge(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_PressureAge); -} + /*! \brief Check if data item contains pressure age data + \returns true if this packet contains pressure age data + */ + int XsDataPacket_containsPressureAge(const XsDataPacket* thisPtr) + { + return genericContains(thisPtr, XDI_PressureAge); + } -/*! \brief The air pressure component of a data item. + /*! \brief The air pressure component of a data item. - \param returnVal : An XsPressure object to put the requested in + \param returnVal : An XsPressure object to put the requested in - \returns An XsPressure object containing the pressure and if available the pressure age -*/ -XsPressure* XsDataPacket_pressure(const XsDataPacket* thisPtr, XsPressure* returnVal) -{ - assert(returnVal); - auto it = MAP.find(XDI_BaroPressure); - if (it != MAP.end()) + \returns An XsPressure object containing the pressure and if available the pressure age + */ + XsPressure* XsDataPacket_pressure(const XsDataPacket* thisPtr, XsPressure* returnVal) { - returnVal->m_pressure = it->second->toDerived>().m_data; - returnVal->m_pressureAge = 0; - } + assert(returnVal); + auto it = MAP.find(XDI_BaroPressure); + if (it != MAP.end()) + { + returnVal->m_pressure = it->second->toDerived>().m_data; + returnVal->m_pressureAge = 0; + } - it = MAP.find(XDI_PressureAge); - if (it != MAP.end()) - returnVal->m_pressureAge = it->second->toDerived>().m_data; + it = MAP.find(XDI_PressureAge); + if (it != MAP.end()) + returnVal->m_pressureAge = it->second->toDerived>().m_data; - return returnVal; -} + return returnVal; + } -/*! \brief Add/update pressure data for the item - \param data The new data to set -*/ -void XsDataPacket_setPressure(XsDataPacket* thisPtr, const XsPressure* data) -{ - GenericSimple::set(thisPtr, (uint32_t) XsMath::doubleToLong(data->m_pressure), XDI_BaroPressure); - GenericSimple::set(thisPtr, data->m_pressureAge, XDI_PressureAge); -} + /*! \brief Add/update pressure data for the item + \param data The new data to set + */ + void XsDataPacket_setPressure(XsDataPacket* thisPtr, const XsPressure* data) + { + GenericSimple::set(thisPtr, (uint32_t) XsMath::doubleToLong(data->m_pressure), XDI_BaroPressure); + GenericSimple::set(thisPtr, data->m_pressureAge, XDI_PressureAge); + } -/*! \brief Return the strapdown integration data component of a data item. - \param returnVal Storage for the requested data - \returns Returns the supplied \a returnVal filled with the requested data -*/ -XsSdiData* XsDataPacket_sdiData(const XsDataPacket* thisPtr, XsSdiData* returnVal) -{ - assert(returnVal); - auto it = MAP.find(XDI_DeltaQ); - if (it != MAP.end()) - returnVal->setOrientationIncrement(it->second->toDerived().m_data); + /*! \brief Return the strapdown integration data component of a data item. + \param returnVal Storage for the requested data + \returns Returns the supplied \a returnVal filled with the requested data + */ + XsSdiData* XsDataPacket_sdiData(const XsDataPacket* thisPtr, XsSdiData* returnVal) + { + assert(returnVal); + auto it = MAP.find(XDI_DeltaQ); + if (it != MAP.end()) + returnVal->setOrientationIncrement(it->second->toDerived().m_data); - it = MAP.find(XDI_DeltaV); - if (it != MAP.end()) - returnVal->setVelocityIncrement(it->second->toDerived().m_data); + it = MAP.find(XDI_DeltaV); + if (it != MAP.end()) + returnVal->setVelocityIncrement(it->second->toDerived().m_data); - return returnVal; -} + return returnVal; + } -/*! \brief Check if data item contains strapdown integration data - \returns Returns true if this packet contains sdi data -*/ -int XsDataPacket_containsSdiData(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_DeltaQ) && + /*! \brief Check if data item contains strapdown integration data + \returns Returns true if this packet contains sdi data + */ + int XsDataPacket_containsSdiData(const XsDataPacket* thisPtr) + { + return genericContains(thisPtr, XDI_DeltaQ) && genericContains(thisPtr, XDI_DeltaV); -} + } -/*! \brief Add/update strapdown integration data for the item - \param data The updated data -*/ -void XsDataPacket_setSdiData(XsDataPacket* thisPtr, const XsSdiData* data) -{ - genericSet(thisPtr, &data->orientationIncrement(), XDI_DeltaQ | XDI_SubFormatDouble); - genericSet(thisPtr, &data->velocityIncrement(), XDI_DeltaV | XDI_SubFormatDouble); -} + /*! \brief Add/update strapdown integration data for the item + \param data The updated data + */ + void XsDataPacket_setSdiData(XsDataPacket* thisPtr, const XsSdiData* data) + { + genericSet(thisPtr, &data->orientationIncrement(), XDI_DeltaQ | XDI_SubFormatDouble); + genericSet(thisPtr, &data->velocityIncrement(), XDI_DeltaV | XDI_SubFormatDouble); + } -/*! \brief Return the glove data component of a data item. - \param returnVal Storage for the requested data - \param hand Which hand to get data for, must be either XHI_LeftHand or XHI_RightHand - \returns Returns the supplied \a returnVal filled with the requested data -*/ -XsGloveData* XsDataPacket_gloveData(const XsDataPacket* thisPtr, XsGloveData* returnVal, XsHandId hand) -{ - switch (hand) - { - case XHI_LeftHand: - return genericGet(thisPtr, returnVal, XDI_GloveDataLeft); - case XHI_RightHand: - return genericGet(thisPtr, returnVal, XDI_GloveDataRight); - case XHI_Unknown: - default: - XsGloveData_destruct(returnVal); - return returnVal; + /*! \brief Return the glove data component of a data item. + \param returnVal Storage for the requested data + \param hand Which hand to get data for, must be either XHI_LeftHand or XHI_RightHand + \returns Returns the supplied \a returnVal filled with the requested data + */ + XsGloveData* XsDataPacket_gloveData(const XsDataPacket* thisPtr, XsGloveData* returnVal, XsHandId hand) + { + switch (hand) + { + case XHI_LeftHand: + return genericGet(thisPtr, returnVal, XDI_GloveDataLeft); + case XHI_RightHand: + return genericGet(thisPtr, returnVal, XDI_GloveDataRight); + case XHI_Unknown: + default: + XsGloveData_destruct(returnVal); + return returnVal; + } } -} -/*! \brief Check if data item contains glove data - \param hand Which hand to get data for, must be either XHI_LeftHand or XHI_RightHand for a particular side or XHI_Unknown for any side - \returns Returns true if this packet contains sdi data -*/ -int XsDataPacket_containsGloveData(const XsDataPacket* thisPtr, XsHandId hand) -{ - switch (hand) + /*! \brief Check if data item contains glove data + \param hand Which hand to get data for, must be either XHI_LeftHand or XHI_RightHand for a particular side or XHI_Unknown for any side + \returns Returns true if this packet contains sdi data + */ + int XsDataPacket_containsGloveData(const XsDataPacket* thisPtr, XsHandId hand) { - case XHI_LeftHand: - return genericContains(thisPtr, XDI_GloveDataLeft); - case XHI_RightHand: - return genericContains(thisPtr, XDI_GloveDataRight); - case XHI_Unknown: - return genericContains(thisPtr, XDI_GloveDataLeft) || genericContains(thisPtr, XDI_GloveDataRight); - default: - return false; + switch (hand) + { + case XHI_LeftHand: + return genericContains(thisPtr, XDI_GloveDataLeft); + case XHI_RightHand: + return genericContains(thisPtr, XDI_GloveDataRight); + case XHI_Unknown: + return genericContains(thisPtr, XDI_GloveDataLeft) || genericContains(thisPtr, XDI_GloveDataRight); + default: + return false; + } } -} -/*! \brief Add/update strapdown integration data for the item - \param data The updated data - \param hand Which hand to get data for, must be either XHI_LeftHand or XHI_RightHand -*/ -void XsDataPacket_setGloveData(XsDataPacket* thisPtr, const XsGloveData* data, XsHandId hand) -{ - switch (hand) - { - case XHI_LeftHand: - genericSet(thisPtr, data, XDI_GloveDataLeft); - break; - case XHI_RightHand: - genericSet(thisPtr, data, XDI_GloveDataRight); - break; - case XHI_Unknown: - default: - break; + /*! \brief Add/update strapdown integration data for the item + \param data The updated data + \param hand Which hand to get data for, must be either XHI_LeftHand or XHI_RightHand + */ + void XsDataPacket_setGloveData(XsDataPacket* thisPtr, const XsGloveData* data, XsHandId hand) + { + switch (hand) + { + case XHI_LeftHand: + genericSet(thisPtr, data, XDI_GloveDataLeft); + break; + case XHI_RightHand: + genericSet(thisPtr, data, XDI_GloveDataRight); + break; + case XHI_Unknown: + default: + break; + } } -} -/*! \brief The device id of a data item. + /*! \brief The device id of a data item. - \param returnVal an XsDeviceId object to put the requested data in + \param returnVal an XsDeviceId object to put the requested data in - \returns An XsDeviceId object containing the requested device ID -*/ -XsDeviceId* XsDataPacket_storedDeviceId(const XsDataPacket* thisPtr, XsDeviceId* returnVal) -{ - return genericGet>(thisPtr, returnVal, XDI_DeviceId); -} - -/*! \brief Return non-zero if this data packet stores a device ID - \returns non-zero (true) if a device ID is available, zero (false) otherwise -*/ -int XsDataPacket_containsStoredDeviceId(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_DeviceId); -} + \returns An XsDeviceId object containing the requested device ID + */ + XsDeviceId* XsDataPacket_storedDeviceId(const XsDataPacket* thisPtr, XsDeviceId* returnVal) + { + return genericGet>(thisPtr, returnVal, XDI_DeviceId); + } -/*! \brief Add or update device id for the item - \param data the device ID to store in the packet -*/ -void XsDataPacket_setStoredDeviceId(XsDataPacket* thisPtr, const XsDeviceId* data) -{ - GenericSimple::set(thisPtr, data->legacyDeviceId(), XDI_DeviceId); -} + /*! \brief Return non-zero if this data packet stores a device ID + \returns non-zero (true) if a device ID is available, zero (false) otherwise + */ + int XsDataPacket_containsStoredDeviceId(const XsDataPacket* thisPtr) + { + return genericContains(thisPtr, XDI_DeviceId); + } -/*! \brief The location ID of a data item. + /*! \brief Add or update device id for the item + \param data the device ID to store in the packet + */ + void XsDataPacket_setStoredDeviceId(XsDataPacket* thisPtr, const XsDeviceId* data) + { + GenericSimple::set(thisPtr, data->legacyDeviceId(), XDI_DeviceId); + } -\returns The requested location ID -*/ -uint16_t XsDataPacket_storedLocationId(const XsDataPacket* thisPtr) -{ - return GenericSimple::get(thisPtr, XDI_LocationId); -} + /*! \brief The location ID of a data item. -/*! \brief Return non-zero if this data packet stores a location ID -\returns non-zero (true) if a location ID is available, zero (false) otherwise -*/ -int XsDataPacket_containsStoredLocationId(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_LocationId); -} + \returns The requested location ID + */ + uint16_t XsDataPacket_storedLocationId(const XsDataPacket* thisPtr) + { + return GenericSimple::get(thisPtr, XDI_LocationId); + } -/*! \brief Add or update location ID for the item -\param data the location ID to store in the packet -*/ -void XsDataPacket_setStoredLocationId(XsDataPacket* thisPtr, uint16_t data) -{ - GenericSimple::set(thisPtr, data, XDI_LocationId); -} + /*! \brief Return non-zero if this data packet stores a location ID + \returns non-zero (true) if a location ID is available, zero (false) otherwise + */ + int XsDataPacket_containsStoredLocationId(const XsDataPacket* thisPtr) + { + return genericContains(thisPtr, XDI_LocationId); + } -/*! \brief The temperature component of a data item. - \returns A double containing the temperature value, -1000.0 if the packet does not contain temperature -*/ -double XsDataPacket_temperature(const XsDataPacket* thisPtr) -{ - return GenericSimple::get(thisPtr, XDI_Temperature); -} + /*! \brief Add or update location ID for the item + \param data the location ID to store in the packet + */ + void XsDataPacket_setStoredLocationId(XsDataPacket* thisPtr, uint16_t data) + { + GenericSimple::set(thisPtr, data, XDI_LocationId); + } -/*! \brief Check if data item contains temperature data - \returns true if this packet contains temperature data -*/ -int XsDataPacket_containsTemperature(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_Temperature); -} + /*! \brief The temperature component of a data item. + \returns A double containing the temperature value, -1000.0 if the packet does not contain temperature + */ + double XsDataPacket_temperature(const XsDataPacket* thisPtr) + { + return GenericSimple::get(thisPtr, XDI_Temperature); + } -/*! \brief Adds or updates the temperature data in the datapacket - \details The \a temp is added to the datapacket. If the packet already contains - temperature it is replaced with the new value. - \param temperature : The temperature to set -*/ -void XsDataPacket_setTemperature(XsDataPacket* thisPtr, double temperature) -{ - GenericSimple::set(thisPtr, temperature, XDI_Temperature | XDI_SubFormatDouble); -} + /*! \brief Check if data item contains temperature data + \returns true if this packet contains temperature data + */ + int XsDataPacket_containsTemperature(const XsDataPacket* thisPtr) + { + return genericContains(thisPtr, XDI_Temperature); + } -/*! \brief The analog in 1 component of a data item. - \param returnVal : The XsAnalogInData object that the analog in 1 value will be assigned to - \returns An XsAnalogInData containing the analog in 1 value -*/ -XsAnalogInData* XsDataPacket_analogIn1Data(const XsDataPacket* thisPtr, XsAnalogInData* returnVal) -{ - return genericGet>(thisPtr, returnVal, XDI_AnalogIn1); -} + /*! \brief Adds or updates the temperature data in the datapacket + \details The \a temp is added to the datapacket. If the packet already contains + temperature it is replaced with the new value. + \param temperature : The temperature to set + */ + void XsDataPacket_setTemperature(XsDataPacket* thisPtr, double temperature) + { + GenericSimple::set(thisPtr, temperature, XDI_Temperature | XDI_SubFormatDouble); + } -/*! \brief Check if data item contains analog in 1 data - \returns true if this packet contains analog in 1 data -*/ -int XsDataPacket_containsAnalogIn1Data(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_AnalogIn1); -} + /*! \brief The analog in 1 component of a data item. + \param returnVal : The XsAnalogInData object that the analog in 1 value will be assigned to + \returns An XsAnalogInData containing the analog in 1 value + */ + XsAnalogInData* XsDataPacket_analogIn1Data(const XsDataPacket* thisPtr, XsAnalogInData* returnVal) + { + return genericGet>(thisPtr, returnVal, XDI_AnalogIn1); + } -/*! \brief Add/update analog in 1 data for the item - \param data The \a data is added to the datapacket. If the packet already contains - analogin1 data, it is replaced with the new value -*/ -void XsDataPacket_setAnalogIn1Data(XsDataPacket* thisPtr, const XsAnalogInData* data) -{ - GenericSimple::set(thisPtr, data->m_data, XDI_AnalogIn1); -} + /*! \brief Check if data item contains analog in 1 data + \returns true if this packet contains analog in 1 data + */ + int XsDataPacket_containsAnalogIn1Data(const XsDataPacket* thisPtr) + { + return genericContains(thisPtr, XDI_AnalogIn1); + } -/*! \brief The analog in 2 component of a data item. - \param returnVal : The XsAnalogInData object that the analog in 2 value will be assigned to - \returns An XsAnalogInData containing the analog in 2 value -*/ -XsAnalogInData* XsDataPacket_analogIn2Data(const XsDataPacket* thisPtr, XsAnalogInData* returnVal) -{ - return genericGet>(thisPtr, returnVal, XDI_AnalogIn2); -} + /*! \brief Add/update analog in 1 data for the item + \param data The \a data is added to the datapacket. If the packet already contains + analogin1 data, it is replaced with the new value + */ + void XsDataPacket_setAnalogIn1Data(XsDataPacket* thisPtr, const XsAnalogInData* data) + { + GenericSimple::set(thisPtr, data->m_data, XDI_AnalogIn1); + } -/*! \brief Check if data item contains analog in 2 data - \returns true if this packet contains analog in 2 data -*/ -int XsDataPacket_containsAnalogIn2Data(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_AnalogIn2); -} + /*! \brief The analog in 2 component of a data item. + \param returnVal : The XsAnalogInData object that the analog in 2 value will be assigned to + \returns An XsAnalogInData containing the analog in 2 value + */ + XsAnalogInData* XsDataPacket_analogIn2Data(const XsDataPacket* thisPtr, XsAnalogInData* returnVal) + { + return genericGet>(thisPtr, returnVal, XDI_AnalogIn2); + } -/*! \brief Add/update analog in 2 data for the item - \param data The \a data is added to the datapacket. If the packet already contains - analogin1 data, it is replaced with the new value -*/ -void XsDataPacket_setAnalogIn2Data(XsDataPacket* thisPtr, const XsAnalogInData* data) -{ - GenericSimple::set(thisPtr, data->m_data, XDI_AnalogIn2); -} + /*! \brief Check if data item contains analog in 2 data + \returns true if this packet contains analog in 2 data + */ + int XsDataPacket_containsAnalogIn2Data(const XsDataPacket* thisPtr) + { + return genericContains(thisPtr, XDI_AnalogIn2); + } -/*! \brief The position lat lon alt component of a data item. + /*! \brief Add/update analog in 2 data for the item + \param data The \a data is added to the datapacket. If the packet already contains + analogin1 data, it is replaced with the new value + */ + void XsDataPacket_setAnalogIn2Data(XsDataPacket* thisPtr, const XsAnalogInData* data) + { + GenericSimple::set(thisPtr, data->m_data, XDI_AnalogIn2); + } - \param returnVal : The XsVector to return the requested data in + /*! \brief The position lat lon alt component of a data item. - \returns An XsVector containing the latitude, longitude and altitude values in that order -*/ -XsVector* XsDataPacket_positionLLA(const XsDataPacket* thisPtr, XsVector* returnVal) -{ - assert(returnVal); - returnVal->setSize(3); + \param returnVal : The XsVector to return the requested data in - XsVector latlon(2); - genericGet(thisPtr, &latlon, XDI_LatLon); - if (!latlon.empty()) - { - (*returnVal)[0] = latlon[0]; - (*returnVal)[1] = latlon[1]; - } - else + \returns An XsVector containing the latitude, longitude and altitude values in that order + */ + XsVector* XsDataPacket_positionLLA(const XsDataPacket* thisPtr, XsVector* returnVal) { - (*returnVal)[0] = 0; - (*returnVal)[1] = 0; - } - // This triggers a warning when XsReal is float, which is deliberately not silenced - (*returnVal)[2] = GenericSimple::get(thisPtr, XDI_AltitudeEllipsoid); + assert(returnVal); + returnVal->setSize(3); - return returnVal; -} + XsVector latlon(2); + genericGet(thisPtr, &latlon, XDI_LatLon); + if (!latlon.empty()) + { + (*returnVal)[0] = latlon[0]; + (*returnVal)[1] = latlon[1]; + } + else + { + (*returnVal)[0] = 0; + (*returnVal)[1] = 0; + } + // This triggers a warning when XsReal is float, which is deliberately not silenced + (*returnVal)[2] = GenericSimple::get(thisPtr, XDI_AltitudeEllipsoid); -/*! \brief Check if data item contains position lat lon alt data - \returns true if this packet contains position lat lon alt data -*/ -int XsDataPacket_containsPositionLLA(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_LatLon) && + return returnVal; + } + + /*! \brief Check if data item contains position lat lon alt data + \returns true if this packet contains position lat lon alt data + */ + int XsDataPacket_containsPositionLLA(const XsDataPacket* thisPtr) + { + return genericContains(thisPtr, XDI_LatLon) && genericContains(thisPtr, XDI_AltitudeEllipsoid); -} + } -/*! \brief Add/update position lat lon alt data for the item + /*! \brief Add/update position lat lon alt data for the item - \param data : The XsVector that conrtains the Lat/Long/Alt data to store in the packet -*/ -void XsDataPacket_setPositionLLA(XsDataPacket* thisPtr, const XsVector* data) -{ - XsVector latlon(2); - latlon[0] = (*data)[0]; - latlon[1] = (*data)[1]; + \param data : The XsVector that conrtains the Lat/Long/Alt data to store in the packet + */ + void XsDataPacket_setPositionLLA(XsDataPacket* thisPtr, const XsVector* data) + { + XsVector latlon(2); + latlon[0] = (*data)[0]; + latlon[1] = (*data)[1]; - genericSet(thisPtr, &latlon, XDI_LatLon | XDI_SubFormatDouble); - GenericSimple::set(thisPtr, (*data)[2], XDI_AltitudeEllipsoid | XDI_SubFormatDouble); -} + genericSet(thisPtr, &latlon, XDI_LatLon | XDI_SubFormatDouble); + GenericSimple::set(thisPtr, (*data)[2], XDI_AltitudeEllipsoid | XDI_SubFormatDouble); + } -/*! \brief The position latitude longitude component of a data item. + /*! \brief The position latitude longitude component of a data item. - \param returnVal : The XsVector to return the requested data in + \param returnVal : The XsVector to return the requested data in - \returns An XsVector containing the latitude and longitude values in that order - \sa XsDataPacket_containsLatitudeLongitude - \sa XsDataPacket_positionLLA \sa XsDataPacket_altitude -*/ -XsVector* XsDataPacket_latitudeLongitude(const XsDataPacket* thisPtr, XsVector* returnVal) -{ - return genericGet(thisPtr, returnVal, XDI_LatLon); -} + \returns An XsVector containing the latitude and longitude values in that order + \sa XsDataPacket_containsLatitudeLongitude + \sa XsDataPacket_positionLLA \sa XsDataPacket_altitude + */ + XsVector* XsDataPacket_latitudeLongitude(const XsDataPacket* thisPtr, XsVector* returnVal) + { + return genericGet(thisPtr, returnVal, XDI_LatLon); + } -/*! \brief Check if data item contains position latitude longitude data - \returns true if this packet contains position latitude longitude data - \sa XsDataPacket_containsPositionLLA \sa XsDataPacket_containsAltitude -*/ -int XsDataPacket_containsLatitudeLongitude(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_LatLon); -} + /*! \brief Check if data item contains position latitude longitude data + \returns true if this packet contains position latitude longitude data + \sa XsDataPacket_containsPositionLLA \sa XsDataPacket_containsAltitude + */ + int XsDataPacket_containsLatitudeLongitude(const XsDataPacket* thisPtr) + { + return genericContains(thisPtr, XDI_LatLon); + } -/*! \brief Add/update position latitude longitude data for the item + /*! \brief Add/update position latitude longitude data for the item - \param data : The XsVector that contains the latitude longitude data to store in the packet - \sa XsDataPacket_setPositionLLA \sa XsDataPacket_setAltitude -*/ -void XsDataPacket_setLatitudeLongitude(XsDataPacket* thisPtr, const XsVector* data) -{ - genericSet(thisPtr, data, XDI_LatLon | XDI_SubFormatDouble); -} + \param data : The XsVector that contains the latitude longitude data to store in the packet + \sa XsDataPacket_setPositionLLA \sa XsDataPacket_setAltitude + */ + void XsDataPacket_setLatitudeLongitude(XsDataPacket* thisPtr, const XsVector* data) + { + genericSet(thisPtr, data, XDI_LatLon | XDI_SubFormatDouble); + } -/*! \brief The position altitude component of a data item. + /*! \brief The position altitude component of a data item. - \returns The altitude stored in the packet or XsMath_infinity if no altitude is available - \sa XsDataPacket_containsAltitude - \sa XsDataPacket_positionLLA \sa XsDataPacket_latitudeLongitude -*/ -double XsDataPacket_altitude(const XsDataPacket* thisPtr) -{ - return GenericSimple::get(thisPtr, XDI_AltitudeEllipsoid); -} + \returns The altitude stored in the packet or XsMath_infinity if no altitude is available + \sa XsDataPacket_containsAltitude + \sa XsDataPacket_positionLLA \sa XsDataPacket_latitudeLongitude + */ + double XsDataPacket_altitude(const XsDataPacket* thisPtr) + { + return GenericSimple::get(thisPtr, XDI_AltitudeEllipsoid); + } -/*! \brief Check if data item contains position altitude data - \returns true if this packet contains position altitude data - \sa XsDataPacket_containsPositionLLA \sa XsDataPacket_containsLatitudeLongitude -*/ -int XsDataPacket_containsAltitude(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_AltitudeEllipsoid); -} + /*! \brief Check if data item contains position altitude data + \returns true if this packet contains position altitude data + \sa XsDataPacket_containsPositionLLA \sa XsDataPacket_containsLatitudeLongitude + */ + int XsDataPacket_containsAltitude(const XsDataPacket* thisPtr) + { + return genericContains(thisPtr, XDI_AltitudeEllipsoid); + } -/*! \brief Add/update altitude data for the item - \param data : The altitude data to store in the packet - \sa XsDataPacket_setPositionLLA \sa XsDataPacket_setLatitudeLongitude -*/ -void XsDataPacket_setAltitude(XsDataPacket* thisPtr, double data) -{ - GenericSimple::set(thisPtr, data, XDI_AltitudeEllipsoid | XDI_SubFormatDouble); -} + /*! \brief Add/update altitude data for the item + \param data : The altitude data to store in the packet + \sa XsDataPacket_setPositionLLA \sa XsDataPacket_setLatitudeLongitude + */ + void XsDataPacket_setAltitude(XsDataPacket* thisPtr, double data) + { + GenericSimple::set(thisPtr, data, XDI_AltitudeEllipsoid | XDI_SubFormatDouble); + } -/*! \brief The position altitude above MSL component of a data item. + /*! \brief The position altitude above MSL component of a data item. - \returns The altitude stored in the packet or XsMath_infinity if no altitude is available - \sa XsDataPacket_containsAltitudeMsl -*/ -double XsDataPacket_altitudeMsl(const XsDataPacket* thisPtr) -{ - return GenericSimple::get(thisPtr, XDI_AltitudeMsl); -} + \returns The altitude stored in the packet or XsMath_infinity if no altitude is available + \sa XsDataPacket_containsAltitudeMsl + */ + double XsDataPacket_altitudeMsl(const XsDataPacket* thisPtr) + { + return GenericSimple::get(thisPtr, XDI_AltitudeMsl); + } -/*! \brief Check if data item contains position altitude above MSL data + /*! \brief Check if data item contains position altitude above MSL data - \returns true if this packet contains position altitude data -*/ -int XsDataPacket_containsAltitudeMsl(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_AltitudeMsl); -} + \returns true if this packet contains position altitude data + */ + int XsDataPacket_containsAltitudeMsl(const XsDataPacket* thisPtr) + { + return genericContains(thisPtr, XDI_AltitudeMsl); + } -/*! \brief Add/update altitude above MSL data for the item + /*! \brief Add/update altitude above MSL data for the item - \param data : The altitude data to store in the packet -*/ -void XsDataPacket_setAltitudeMsl(XsDataPacket* thisPtr, double data) -{ - GenericSimple::set(thisPtr, data, XDI_AltitudeMsl | XDI_SubFormatDouble); -} + \param data : The altitude data to store in the packet + */ + void XsDataPacket_setAltitudeMsl(XsDataPacket* thisPtr, double data) + { + GenericSimple::set(thisPtr, data, XDI_AltitudeMsl | XDI_SubFormatDouble); + } -/*! \brief The velocity NWU component of a data item. + /*! \brief The velocity NWU component of a data item. - \param returnVal : The XsVector to put the data in - \param coordinateSystem The coordinate system of the requested velocity. If this does not match - the stored coordinate system, it will be transformed to the requested velocity. - \returns A XsVector containing the x, y and z axis values in that order -*/ -XsVector* XsDataPacket_velocity(const XsDataPacket* thisPtr, XsVector* returnVal, XsDataIdentifier coordinateSystem) -{ - assert(returnVal); - auto it = MAP.find(XDI_VelocityXYZ); - if (it != MAP.end()) + \param returnVal : The XsVector to put the data in + \param coordinateSystem The coordinate system of the requested velocity. If this does not match + the stored coordinate system, it will be transformed to the requested velocity. + \returns A XsVector containing the x, y and z axis values in that order + */ + XsVector* XsDataPacket_velocity(const XsDataPacket* thisPtr, XsVector* returnVal, XsDataIdentifier coordinateSystem) { - *returnVal = it->second->toDerived().m_data; - XsDataIdentifier actualCsys = (it->second->dataId() & XDI_CoordSysMask); - XsDataIdentifier desiredCsys = (coordinateSystem & XDI_CoordSysMask); - if (actualCsys != desiredCsys) + assert(returnVal); + auto it = MAP.find(XDI_VelocityXYZ); + if (it != MAP.end()) { - XsVector3 vel(*returnVal); - XsVector& rv = *returnVal; - - switch (desiredCsys) + *returnVal = it->second->toDerived().m_data; + XsDataIdentifier actualCsys = (it->second->dataId() & XDI_CoordSysMask); + XsDataIdentifier desiredCsys = (coordinateSystem & XDI_CoordSysMask); + if (actualCsys != desiredCsys) { - case XDI_CoordSysEnu: - { - if (actualCsys == XDI_CoordSysNwu) - { - rv[0] = -vel[1]; - rv[1] = vel[0]; - rv[2] = vel[2]; - } - else if (actualCsys == XDI_CoordSysNed) - { - rv[0] = vel[1]; - rv[1] = vel[0]; - rv[2] = -vel[2]; - } - } break; + XsVector3 vel(*returnVal); + XsVector& rv = *returnVal; - case XDI_CoordSysNwu: + switch (desiredCsys) { - if (actualCsys == XDI_CoordSysEnu) + case XDI_CoordSysEnu: { - rv[0] = vel[1]; - rv[1] = -vel[0]; - rv[2] = vel[2]; + if (actualCsys == XDI_CoordSysNwu) + { + rv[0] = -vel[1]; + rv[1] = vel[0]; + rv[2] = vel[2]; + } + else if (actualCsys == XDI_CoordSysNed) + { + rv[0] = vel[1]; + rv[1] = vel[0]; + rv[2] = -vel[2]; + } } - else if (actualCsys == XDI_CoordSysNed) + break; + + case XDI_CoordSysNwu: { - rv[0] = vel[0]; - rv[1] = -vel[1]; - rv[2] = -vel[2]; + if (actualCsys == XDI_CoordSysEnu) + { + rv[0] = vel[1]; + rv[1] = -vel[0]; + rv[2] = vel[2]; + } + else if (actualCsys == XDI_CoordSysNed) + { + rv[0] = vel[0]; + rv[1] = -vel[1]; + rv[2] = -vel[2]; + } } - } break; + break; - case XDI_CoordSysNed: - { - if (actualCsys == XDI_CoordSysEnu) + case XDI_CoordSysNed: { - rv[0] = vel[1]; - rv[1] = vel[0]; - rv[2] = -vel[2]; + if (actualCsys == XDI_CoordSysEnu) + { + rv[0] = vel[1]; + rv[1] = vel[0]; + rv[2] = -vel[2]; + } + else if (actualCsys == XDI_CoordSysNwu) + { + rv[0] = vel[0]; + rv[1] = -vel[1]; + rv[2] = -vel[2]; + } } - else if (actualCsys == XDI_CoordSysNwu) + break; + + default: { rv[0] = vel[0]; - rv[1] = -vel[1]; - rv[2] = -vel[2]; + rv[1] = vel[1]; + rv[2] = vel[2]; } - } break; - - default: - { - rv[0] = vel[0]; - rv[1] = vel[1]; - rv[2] = vel[2]; - } break; + break; + } } } - } - return returnVal; -} + return returnVal; + } -/*! \brief Check if data item contains velocity NED data - \returns true if this packet contains velocity NED data -*/ -int XsDataPacket_containsVelocity(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_VelocityXYZ); -} + /*! \brief Check if data item contains velocity NED data + \returns true if this packet contains velocity NED data + */ + int XsDataPacket_containsVelocity(const XsDataPacket* thisPtr) + { + return genericContains(thisPtr, XDI_VelocityXYZ); + } -/*! \brief Add/update velocity NED data for the item - \param data The new data to set - \param coordinateSystem The coordinate system of the requested orientation. -*/ -void XsDataPacket_setVelocity(XsDataPacket* thisPtr, const XsVector* data, XsDataIdentifier coordinateSystem) -{ - detach(thisPtr); - auto it = MAP.find(XDI_VelocityXYZ); - if (it != MAP.end()) - MAP.erase(it); + /*! \brief Add/update velocity NED data for the item + \param data The new data to set + \param coordinateSystem The coordinate system of the requested orientation. + */ + void XsDataPacket_setVelocity(XsDataPacket* thisPtr, const XsVector* data, XsDataIdentifier coordinateSystem) + { + detach(thisPtr); + auto it = MAP.find(XDI_VelocityXYZ); + if (it != MAP.end()) + MAP.erase(it); - genericSet(thisPtr, data, XDI_VelocityXYZ | XDI_SubFormatDouble | (coordinateSystem & XDI_CoordSysMask)); -} + genericSet(thisPtr, data, XDI_VelocityXYZ | XDI_SubFormatDouble | (coordinateSystem & XDI_CoordSysMask)); + } -/*! \brief Returns the data identifier of the first velocity data of any kind in the packet - \returns The %XsDataIdentifier of the first velocity data of any kind in the packet -*/ -XsDataIdentifier XsDataPacket_velocityIdentifier(const XsDataPacket* thisPtr) -{ - auto it = MAP.find(XDI_VelocityXYZ); - if (it != MAP.end()) - return it->second->dataId(); - return XDI_None; -} + /*! \brief Returns the data identifier of the first velocity data of any kind in the packet + \returns The %XsDataIdentifier of the first velocity data of any kind in the packet + */ + XsDataIdentifier XsDataPacket_velocityIdentifier(const XsDataPacket* thisPtr) + { + auto it = MAP.find(XDI_VelocityXYZ); + if (it != MAP.end()) + return it->second->dataId(); + return XDI_None; + } -/*! \brief Returns the coordinate system of the first velocity data of any kind in the packet - \returns The XsDataIdentifier of the coordinate system of the first velocity data of any kind in the packet -*/ -XsDataIdentifier XsDataPacket_coordinateSystemVelocity(const XsDataPacket* thisPtr) -{ - return XsDataPacket_velocityIdentifier(thisPtr) & XDI_CoordSysMask; -} + /*! \brief Returns the coordinate system of the first velocity data of any kind in the packet + \returns The XsDataIdentifier of the coordinate system of the first velocity data of any kind in the packet + */ + XsDataIdentifier XsDataPacket_coordinateSystemVelocity(const XsDataPacket* thisPtr) + { + return XsDataPacket_velocityIdentifier(thisPtr) & XDI_CoordSysMask; + } -/*! \brief The status component of a data item. + /*! \brief The status component of a data item. - \returns An uint32_t containing the status value -*/ -uint32_t XsDataPacket_status(const XsDataPacket* thisPtr) -{ - auto it = MAP.find(XDI_StatusWord); - if (it != MAP.end()) - return it->second->toDerived>().m_data; + \returns An uint32_t containing the status value + */ + uint32_t XsDataPacket_status(const XsDataPacket* thisPtr) + { + auto it = MAP.find(XDI_StatusWord); + if (it != MAP.end()) + return it->second->toDerived>().m_data; - it = MAP.find(XDI_StatusByte); - if (it != MAP.end()) - return it->second->toDerived>().m_data; + it = MAP.find(XDI_StatusByte); + if (it != MAP.end()) + return it->second->toDerived>().m_data; - return 0; -} + return 0; + } -/*! \brief Check if data item contains detailed status data - \returns true if this packet contains detailed status data -*/ -int XsDataPacket_containsDetailedStatus(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_StatusWord); -} + /*! \brief Check if data item contains detailed status data + \returns true if this packet contains detailed status data + */ + int XsDataPacket_containsDetailedStatus(const XsDataPacket* thisPtr) + { + return genericContains(thisPtr, XDI_StatusWord); + } -/*! \brief Check if data item contains legacy status data - \returns true if this packet contains legacy status data -*/ -int XsDataPacket_containsStatus(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_StatusWord) || + /*! \brief Check if data item contains legacy status data + \returns true if this packet contains legacy status data + */ + int XsDataPacket_containsStatus(const XsDataPacket* thisPtr) + { + return genericContains(thisPtr, XDI_StatusWord) || genericContains(thisPtr, XDI_StatusByte); -} + } -/*! \brief Add/update status data for the item - \param data The new data to set -*/ -void XsDataPacket_setStatusByte(XsDataPacket* thisPtr, uint8_t data) -{ - detach(thisPtr); - if (genericContains(thisPtr, XDI_StatusWord)) + /*! \brief Add/update status data for the item + \param data The new data to set + */ + void XsDataPacket_setStatusByte(XsDataPacket* thisPtr, uint8_t data) { - uint32_t word = GenericSimple::get(thisPtr, XDI_StatusWord); - word = ((word & (~0xFF)) | data); - XsDataPacket_setStatus(thisPtr, word); + detach(thisPtr); + if (genericContains(thisPtr, XDI_StatusWord)) + { + uint32_t word = GenericSimple::get(thisPtr, XDI_StatusWord); + word = ((word & (~0xFF)) | data); + XsDataPacket_setStatus(thisPtr, word); + } + else + GenericSimple::set(thisPtr, data, XDI_StatusByte); } - else - GenericSimple::set(thisPtr, data, XDI_StatusByte); -} -/*! \brief Add/update status data for the item - \param data The new data to set -*/ -void XsDataPacket_setStatus(XsDataPacket* thisPtr, uint32_t data) -{ - detach(thisPtr); - auto it = MAP.find(XDI_StatusByte); - if (it != MAP.end()) - MAP.erase(it); + /*! \brief Add/update status data for the item + \param data The new data to set + */ + void XsDataPacket_setStatus(XsDataPacket* thisPtr, uint32_t data) + { + detach(thisPtr); + auto it = MAP.find(XDI_StatusByte); + if (it != MAP.end()) + MAP.erase(it); - GenericSimple::set(thisPtr, data, XDI_StatusWord); -} + GenericSimple::set(thisPtr, data, XDI_StatusWord); + } -/*! \brief Returns the trigger indication data of a packet - If the packet does not contain the requested data, the return val struct will be set to all zeroes - \param[in] triggerId The trigger data identifier to add data for (e.g. XDI_TriggerIn1 or XDI_TriggerIn2) - \param[out] returnVal pointer to the trigger indication data of a packet. - \note returnVal should point to a buffer large enough to hold sizeof(XsTriggerIndicationData) bytes of data - \returns Returns a pointer to the trigger indication data of a packet -*/ -XsTriggerIndicationData* XsDataPacket_triggerIndication(const XsDataPacket* thisPtr, XsDataIdentifier triggerId, XsTriggerIndicationData* returnVal) -{ - return genericGet(thisPtr, returnVal, triggerId); -} + /*! \brief Returns the trigger indication data of a packet + If the packet does not contain the requested data, the return val struct will be set to all zeroes + \param[in] triggerId The trigger data identifier to add data for (e.g. XDI_TriggerIn1 or XDI_TriggerIn2) + \param[out] returnVal pointer to the trigger indication data of a packet. + \note returnVal should point to a buffer large enough to hold sizeof(XsTriggerIndicationData) bytes of data + \returns Returns a pointer to the trigger indication data of a packet + */ + XsTriggerIndicationData* XsDataPacket_triggerIndication(const XsDataPacket* thisPtr, XsDataIdentifier triggerId, XsTriggerIndicationData* returnVal) + { + return genericGet(thisPtr, returnVal, triggerId); + } -/*! \brief Check if data item contains trigger indication data - \param[in] triggerId The trigger data identifier to check (e.g. XDI_TriggerIn1 or XDI_TriggerIn2) - \returns true if this packet contains trigger indication data -*/ -int XsDataPacket_containsTriggerIndication(const XsDataPacket* thisPtr, XsDataIdentifier triggerId) -{ - return genericContains(thisPtr, triggerId); -} + /*! \brief Check if data item contains trigger indication data + \param[in] triggerId The trigger data identifier to check (e.g. XDI_TriggerIn1 or XDI_TriggerIn2) + \returns true if this packet contains trigger indication data + */ + int XsDataPacket_containsTriggerIndication(const XsDataPacket* thisPtr, XsDataIdentifier triggerId) + { + return genericContains(thisPtr, triggerId); + } -/*! \brief Add/update trigger indication data for the item - \param[in] triggerId The trigger data identifier to add data for (e.g. XDI_TriggerIn1 or XDI_TriggerIn2) - \param[in] triggerIndicationData pointer the a XsTriggerIndicationData buffer containing the data to set -*/ -void XsDataPacket_setTriggerIndication(XsDataPacket* thisPtr, XsDataIdentifier triggerId, const XsTriggerIndicationData *triggerIndicationData) -{ - genericSet(thisPtr, triggerIndicationData, triggerId); -} + /*! \brief Add/update trigger indication data for the item + \param[in] triggerId The trigger data identifier to add data for (e.g. XDI_TriggerIn1 or XDI_TriggerIn2) + \param[in] triggerIndicationData pointer the a XsTriggerIndicationData buffer containing the data to set + */ + void XsDataPacket_setTriggerIndication(XsDataPacket* thisPtr, XsDataIdentifier triggerId, const XsTriggerIndicationData* triggerIndicationData) + { + genericSet(thisPtr, triggerIndicationData, triggerId); + } -/*! \brief Return the 8 bit packet counter of a packet + /*! \brief Return the 8 bit packet counter of a packet - \details This function returns an 8 bit packet counter as used by some third party devices + \details This function returns an 8 bit packet counter as used by some third party devices - \returns Returns the 8 bit packet counter of a packet -*/ -uint8_t XsDataPacket_packetCounter8(const XsDataPacket* thisPtr) -{ - return GenericSimple::get(thisPtr, XDI_PacketCounter8); -} + \returns Returns the 8 bit packet counter of a packet + */ + uint8_t XsDataPacket_packetCounter8(const XsDataPacket* thisPtr) + { + return GenericSimple::get(thisPtr, XDI_PacketCounter8); + } -/*! \brief Check if data item contains an 8 bit packet counter - \returns true if this packet contains an 8 bit packet counter -*/ -int XsDataPacket_containsPacketCounter8(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_PacketCounter8); -} + /*! \brief Check if data item contains an 8 bit packet counter + \returns true if this packet contains an 8 bit packet counter + */ + int XsDataPacket_containsPacketCounter8(const XsDataPacket* thisPtr) + { + return genericContains(thisPtr, XDI_PacketCounter8); + } -/*! \brief Add/update 8 bit packet counter data for the item - \param counter The new data to set -*/ -void XsDataPacket_setPacketCounter8(XsDataPacket* thisPtr, uint8_t counter) -{ - GenericSimple::set(thisPtr, counter, XDI_PacketCounter8); -} + /*! \brief Add/update 8 bit packet counter data for the item + \param counter The new data to set + */ + void XsDataPacket_setPacketCounter8(XsDataPacket* thisPtr, uint8_t counter) + { + GenericSimple::set(thisPtr, counter, XDI_PacketCounter8); + } -/*! \brief Return the packet/frame counter of a packet + /*! \brief Return the packet/frame counter of a packet - \details For strapdown integration data, this function will return the m_wlastFrameNumber - For other data, this function will return the m_sc + \details For strapdown integration data, this function will return the m_wlastFrameNumber + For other data, this function will return the m_sc - This way there is a function that will always return the counter of a packet + This way there is a function that will always return the counter of a packet - \returns Returns the packet/frame counter of a packet -*/ -uint16_t XsDataPacket_packetCounter(const XsDataPacket* thisPtr) -{ - auto it = MAP.find(XDI_PacketCounter); - if (it != MAP.end()) - return it->second->toDerived>().m_data; + \returns Returns the packet/frame counter of a packet + */ + uint16_t XsDataPacket_packetCounter(const XsDataPacket* thisPtr) + { + auto it = MAP.find(XDI_PacketCounter); + if (it != MAP.end()) + return it->second->toDerived>().m_data; - it = MAP.find(XDI_FrameRange); - if (it != MAP.end()) - return static_cast(static_cast(it->second->toDerived().m_data.last())); + it = MAP.find(XDI_FrameRange); + if (it != MAP.end()) + return static_cast(static_cast(it->second->toDerived().m_data.last())); - return 0; -} + return 0; + } -/*! \brief Check if data item contains a packet counter - \returns true if this packet contains a packet counter -*/ -int XsDataPacket_containsPacketCounter(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_PacketCounter) || + /*! \brief Check if data item contains a packet counter + \returns true if this packet contains a packet counter + */ + int XsDataPacket_containsPacketCounter(const XsDataPacket* thisPtr) + { + return genericContains(thisPtr, XDI_PacketCounter) || genericContains(thisPtr, XDI_FrameRange); -} - -/*! \brief Add/update packet counter data for the item - \param counter The new data to set -*/ -void XsDataPacket_setPacketCounter(XsDataPacket* thisPtr, uint16_t counter) -{ - GenericSimple::set(thisPtr, counter, XDI_PacketCounter); - MAP.erase(XDI_FrameRange); -} - -/*! \brief Return the fine sample time of a packet - - \returns Returns the fine sample time of a packet -*/ -uint32_t XsDataPacket_sampleTimeFine(const XsDataPacket* thisPtr) -{ - return GenericSimple::get(thisPtr, XDI_SampleTimeFine); -} - -/*! \brief Check if data item XsDataPacket_contains a sample time fine - \returns true if this packet XsDataPacket_contains a sample time fine -*/ -int XsDataPacket_containsSampleTimeFine(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_SampleTimeFine); -} + } -/*! \brief Add/update sample tine fine data for the item - \param counter The new data to set -*/ -void XsDataPacket_setSampleTimeFine(XsDataPacket* thisPtr, uint32_t counter) -{ - GenericSimple::set(thisPtr, counter, XDI_SampleTimeFine); - auto it = MAP.find(XDI_SampleTime64); - if (it != MAP.end()) + /*! \brief Add/update packet counter data for the item + \param counter The new data to set + */ + void XsDataPacket_setPacketCounter(XsDataPacket* thisPtr, uint16_t counter) { - auto& var = it->second->toDerived>(); - var.m_data = coarseFactor*(var.m_data/coarseFactor) + (counter%coarseFactor); + GenericSimple::set(thisPtr, counter, XDI_PacketCounter); + MAP.erase(XDI_FrameRange); } -} - -/*! \return Return the coarse sample time of a packet -*/ -uint32_t XsDataPacket_sampleTimeCoarse(const XsDataPacket* thisPtr) -{ - return GenericSimple::get(thisPtr, XDI_SampleTimeCoarse); -} -/*! \brief Check if data item XsDataPacket_contains a sample time coarse - \returns true if this packet XsDataPacket_contains a sample time coarse -*/ -int XsDataPacket_containsSampleTimeCoarse(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_SampleTimeCoarse); -} + /*! \brief Return the fine sample time of a packet -/*! \brief Add/update sample tine coarse data for the item - \param counter The new data to set -*/ -void XsDataPacket_setSampleTimeCoarse(XsDataPacket* thisPtr, uint32_t counter) -{ - GenericSimple::set(thisPtr, counter, XDI_SampleTimeCoarse); - auto it = MAP.find(XDI_SampleTime64); - if (it != MAP.end()) + \returns Returns the fine sample time of a packet + */ + uint32_t XsDataPacket_sampleTimeFine(const XsDataPacket* thisPtr) { - auto& var = it->second->toDerived>(); - var.m_data = (coarseFactor * counter) + (var.m_data % coarseFactor); + return GenericSimple::get(thisPtr, XDI_SampleTimeFine); } -} -/*! \brief Return the full 64-bit sample time of a packet, combined from the fine and coarse sample times or received directly from the device. The 64-bit sample time runs at 10kHz. - \returns the full 64-bit sample time of a packet -*/ -uint64_t XsDataPacket_sampleTime64(const XsDataPacket* thisPtr) -{ - auto it = MAP.find(XDI_SampleTime64); - if (it != MAP.end()) - return it->second->toDerived>().m_data; - - uint64_t rv = 0; - - bool hasSampletimeCoarse = false; - it = MAP.find(XDI_SampleTimeCoarse); - if (it != MAP.end()) + /*! \brief Check if data item XsDataPacket_contains a sample time fine + \returns true if this packet XsDataPacket_contains a sample time fine + */ + int XsDataPacket_containsSampleTimeFine(const XsDataPacket* thisPtr) { - hasSampletimeCoarse = true; - rv += ((uint64_t) it->second->toDerived>().m_data) * coarseFactor; + return genericContains(thisPtr, XDI_SampleTimeFine); } - it = MAP.find(XDI_SampleTimeFine); - if (it != MAP.end()) + /*! \brief Add/update sample tine fine data for the item + \param counter The new data to set + */ + void XsDataPacket_setSampleTimeFine(XsDataPacket* thisPtr, uint32_t counter) { - uint32_t sampletimeFine = it->second->toDerived>().m_data; - if (hasSampletimeCoarse) - sampletimeFine = sampletimeFine % coarseFactor; - rv += sampletimeFine; + GenericSimple::set(thisPtr, counter, XDI_SampleTimeFine); + auto it = MAP.find(XDI_SampleTime64); + if (it != MAP.end()) + { + auto& var = it->second->toDerived>(); + var.m_data = coarseFactor * (var.m_data / coarseFactor) + (counter % coarseFactor); + } } - return rv; -} - -/*! \brief Check if data item XsDataPacket contains a full 64-bit sample time - \returns true if this packet XsDataPacket contains both a fine and coarse sample time -*/ -int XsDataPacket_containsSampleTime64(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_SampleTime64) || - (genericContains(thisPtr, XDI_SampleTimeCoarse) && genericContains(thisPtr, XDI_SampleTimeFine)); -} + /*! \return Return the coarse sample time of a packet + */ + uint32_t XsDataPacket_sampleTimeCoarse(const XsDataPacket* thisPtr) + { + return GenericSimple::get(thisPtr, XDI_SampleTimeCoarse); + } -/*! \brief Add/update sample tine coarse data for the item - \param counter The new data to set -*/ -void XsDataPacket_setSampleTime64(XsDataPacket* thisPtr, uint64_t counter) -{ - GenericSimple::set(thisPtr, counter, XDI_SampleTime64); - GenericSimple::set(thisPtr, (uint32_t) (counter / coarseFactor), XDI_SampleTimeCoarse); - GenericSimple::set(thisPtr, (uint32_t) (counter % coarseFactor), XDI_SampleTimeFine); -} + /*! \brief Check if data item XsDataPacket_contains a sample time coarse + \returns true if this packet XsDataPacket_contains a sample time coarse + */ + int XsDataPacket_containsSampleTimeCoarse(const XsDataPacket* thisPtr) + { + return genericContains(thisPtr, XDI_SampleTimeCoarse); + } -/*! \brief The utc time component of a data item. - \param returnVal : The XsTimeInfo to return the requested data in - \returns An XsTimeInfo containing the utc time value -*/ -XsTimeInfo* XsDataPacket_utcTime(const XsDataPacket* thisPtr, XsTimeInfo* returnVal) -{ - return genericGet(thisPtr, returnVal, XDI_UtcTime); -} + /*! \brief Add/update sample tine coarse data for the item + \param counter The new data to set + */ + void XsDataPacket_setSampleTimeCoarse(XsDataPacket* thisPtr, uint32_t counter) + { + GenericSimple::set(thisPtr, counter, XDI_SampleTimeCoarse); + auto it = MAP.find(XDI_SampleTime64); + if (it != MAP.end()) + { + auto& var = it->second->toDerived>(); + var.m_data = (coarseFactor * counter) + (var.m_data % coarseFactor); + } + } -/*! \brief Check if data item contains utc time data - \returns true if this packet contains utc time data -*/ -int XsDataPacket_containsUtcTime(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_UtcTime); -} + /*! \brief Return the full 64-bit sample time of a packet, combined from the fine and coarse sample times or received directly from the device. The 64-bit sample time runs at 10kHz. + \returns the full 64-bit sample time of a packet + */ + uint64_t XsDataPacket_sampleTime64(const XsDataPacket* thisPtr) + { + auto it = MAP.find(XDI_SampleTime64); + if (it != MAP.end()) + return it->second->toDerived>().m_data; -/*! \brief Add/update utc time data for the item - \param data The new data to set -*/ -void XsDataPacket_setUtcTime(XsDataPacket* thisPtr, const XsTimeInfo* data) -{ - genericSet(thisPtr, data, XDI_UtcTime); -} + uint64_t rv = 0; -/*! \brief The free acceleration component of a data item. - \details Free acceleration is the acceleration with the local gravity vector subtracted. - \param returnVal : An XsVector to put the requested in + bool hasSampletimeCoarse = false; + it = MAP.find(XDI_SampleTimeCoarse); + if (it != MAP.end()) + { + hasSampletimeCoarse = true; + rv += ((uint64_t) it->second->toDerived>().m_data) * coarseFactor; + } - \returns An XsVector containing the gravity acceleration -*/ -XsVector* XsDataPacket_freeAcceleration(const XsDataPacket* thisPtr, XsVector* returnVal) -{ - return genericGet(thisPtr, returnVal, XDI_FreeAcceleration); -} + it = MAP.find(XDI_SampleTimeFine); + if (it != MAP.end()) + { + uint32_t sampletimeFine = it->second->toDerived>().m_data; + if (hasSampletimeCoarse) + sampletimeFine = sampletimeFine % coarseFactor; + rv += sampletimeFine; + } -/*! \brief Check if data item contains free acceleration - \details Free acceleration is the acceleration with the local gravity vector subtracted. - \returns true if this packet contains free acceleration -*/ -int XsDataPacket_containsFreeAcceleration(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_FreeAcceleration); -} + return rv; + } -/*! \brief Add/update free acceleration data for the item - \details Free acceleration is the acceleration with the local gravity vector subtracted. - \param g A 3-component vector containing the new free acceleration -*/ -void XsDataPacket_setFreeAcceleration(XsDataPacket* thisPtr, const XsVector* g) -{ - genericSet(thisPtr, g, XDI_FreeAcceleration); -} + /*! \brief Check if data item XsDataPacket contains a full 64-bit sample time + \returns true if this packet XsDataPacket contains both a fine and coarse sample time + */ + int XsDataPacket_containsSampleTime64(const XsDataPacket* thisPtr) + { + return genericContains(thisPtr, XDI_SampleTime64) || + (genericContains(thisPtr, XDI_SampleTimeCoarse) && genericContains(thisPtr, XDI_SampleTimeFine)); + } -/*! \brief Returns the frame range contained in the datapacket + /*! \brief Add/update sample tine coarse data for the item + \param counter The new data to set + */ + void XsDataPacket_setSampleTime64(XsDataPacket* thisPtr, uint64_t counter) + { + GenericSimple::set(thisPtr, counter, XDI_SampleTime64); + GenericSimple::set(thisPtr, (uint32_t)(counter / coarseFactor), XDI_SampleTimeCoarse); + GenericSimple::set(thisPtr, (uint32_t)(counter % coarseFactor), XDI_SampleTimeFine); + } - \param returnVal : The XsRange object that will get the range from the packet + /*! \brief The utc time component of a data item. + \param returnVal : The XsTimeInfo to return the requested data in + \returns An XsTimeInfo containing the utc time value + */ + XsTimeInfo* XsDataPacket_utcTime(const XsDataPacket* thisPtr, XsTimeInfo* returnVal) + { + return genericGet(thisPtr, returnVal, XDI_UtcTime); + } - \returns Returns an XsRange object with the range from the packet -*/ -XsRange* XsDataPacket_frameRange(const XsDataPacket* thisPtr, XsRange* returnVal) -{ - return genericGet(thisPtr, returnVal, XDI_FrameRange); -} + /*! \brief Check if data item contains utc time data + \returns true if this packet contains utc time data + */ + int XsDataPacket_containsUtcTime(const XsDataPacket* thisPtr) + { + return genericContains(thisPtr, XDI_UtcTime); + } -/*! \brief Returns whether the datapacket contains a framerange + /*! \brief Add/update utc time data for the item + \param data The new data to set + */ + void XsDataPacket_setUtcTime(XsDataPacket* thisPtr, const XsTimeInfo* data) + { + genericSet(thisPtr, data, XDI_UtcTime); + } - \returns Whether the datapacket contains a framerange -*/ -int XsDataPacket_containsFrameRange(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_FrameRange); -} + /*! \brief The free acceleration component of a data item. + \details Free acceleration is the acceleration with the local gravity vector subtracted. + \param returnVal : An XsVector to put the requested in -/*! \brief Sets or updates the frame range in the datapacket + \returns An XsVector containing the gravity acceleration + */ + XsVector* XsDataPacket_freeAcceleration(const XsDataPacket* thisPtr, XsVector* returnVal) + { + return genericGet(thisPtr, returnVal, XDI_FreeAcceleration); + } - \param r : The XsRange object that should be added to the packet -*/ -void XsDataPacket_setFrameRange(XsDataPacket* thisPtr, const XsRange* r) -{ - genericSet(thisPtr, r, XDI_FrameRange); - MAP.erase(XDI_PacketCounter); -} + /*! \brief Check if data item contains free acceleration + \details Free acceleration is the acceleration with the local gravity vector subtracted. + \returns true if this packet contains free acceleration + */ + int XsDataPacket_containsFreeAcceleration(const XsDataPacket* thisPtr) + { + return genericContains(thisPtr, XDI_FreeAcceleration); + } -/*! \brief Returns the rssi value contained in the datapacket + /*! \brief Add/update free acceleration data for the item + \details Free acceleration is the acceleration with the local gravity vector subtracted. + \param g A 3-component vector containing the new free acceleration + */ + void XsDataPacket_setFreeAcceleration(XsDataPacket* thisPtr, const XsVector* g) + { + genericSet(thisPtr, g, XDI_FreeAcceleration); + } - \returns Returns the rssi value contained in the datapacket -*/ -int XsDataPacket_rssi(const XsDataPacket* thisPtr) -{ - return (int) (int8_t) GenericSimple::get(thisPtr, XDI_Rssi, (uint8_t) XS_RSSI_UNKNOWN); -} + /*! \brief Returns the frame range contained in the datapacket -/*! \brief Returns whether the datapacket contains an rssi value + \param returnVal : The XsRange object that will get the range from the packet - \returns Whether the datapacket contains an rssi value -*/ -int XsDataPacket_containsRssi(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_Rssi); -} + \returns Returns an XsRange object with the range from the packet + */ + XsRange* XsDataPacket_frameRange(const XsDataPacket* thisPtr, XsRange* returnVal) + { + return genericGet(thisPtr, returnVal, XDI_FrameRange); + } -/*! \brief Sets or updates the rssi value in the datapacket + /*! \brief Returns whether the datapacket contains a framerange - \param r : The rssi value that should be added to the packet -*/ -void XsDataPacket_setRssi(XsDataPacket* thisPtr, int r) -{ - GenericSimple::set(thisPtr, (uint8_t)(int8_t)r, XDI_Rssi); -} + \returns Whether the datapacket contains a framerange + */ + int XsDataPacket_containsFrameRange(const XsDataPacket* thisPtr) + { + return genericContains(thisPtr, XDI_FrameRange); + } -/*! \brief Returns a struct with RawGnssPvtData - \param returnVal The object to store the requested data in - \return a struct with RawGnssPvtData -*/ -XsRawGnssPvtData* XsDataPacket_rawGnssPvtData(const XsDataPacket* thisPtr, XsRawGnssPvtData* returnVal) -{ - return genericGet(thisPtr, returnVal, XDI_GnssPvtData); -} + /*! \brief Sets or updates the frame range in the datapacket -/*! \brief Returns 1 if data item contains RawGnssPvtData, 0 otherwise - \returns true if this XsDataPacket contains Raw GNSS data -*/ -int XsDataPacket_containsRawGnssPvtData(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_GnssPvtData); -} + \param r : The XsRange object that should be added to the packet + */ + void XsDataPacket_setFrameRange(XsDataPacket* thisPtr, const XsRange* r) + { + genericSet(thisPtr, r, XDI_FrameRange); + MAP.erase(XDI_PacketCounter); + } -/*! \brief Sets or updates the RawGnssPvtData value in the datapacket - \param r The new value that should be added to the packet -*/ -void XsDataPacket_setRawGnssPvtData(XsDataPacket* thisPtr, const XsRawGnssPvtData* r) -{ - genericSet(thisPtr, r, XDI_GnssPvtData); -} + /*! \brief Returns the rssi value contained in the datapacket -/*! \brief Returns the timestamp of a PVT Pulse + \returns Returns the rssi value contained in the datapacket + */ + int XsDataPacket_rssi(const XsDataPacket* thisPtr) + { + return (int)(int8_t) GenericSimple::get(thisPtr, XDI_Rssi, (uint8_t) XS_RSSI_UNKNOWN); + } - \returns Returns the timestamp of a PVT Pulse -*/ -uint32_t XsDataPacket_gnssPvtPulse(const XsDataPacket* thisPtr) -{ - return GenericSimple::get(thisPtr, XDI_GnssPvtPulse); -} + /*! \brief Returns whether the datapacket contains an rssi value -/*! \brief Check if data item XsDataPacket_contains a pvt pulse - \returns true if this packet XsDataPacket_contains a spvt pulse -*/ -int XsDataPacket_containsGnssPvtPulse(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_GnssPvtPulse); -} + \returns Whether the datapacket contains an rssi value + */ + int XsDataPacket_containsRssi(const XsDataPacket* thisPtr) + { + return genericContains(thisPtr, XDI_Rssi); + } -/*! \brief Add/update pvt pulse timestamp data for the item - \param counter The new data to set -*/ -void XsDataPacket_setGnssPvtPulse(XsDataPacket* thisPtr, uint32_t counter) -{ - GenericSimple::set(thisPtr, counter, XDI_GnssPvtPulse); -} + /*! \brief Sets or updates the rssi value in the datapacket -/*! \brief Returns the age of the GNSS data (in samples) - \returns the age of the GNSS data -*/ -uint8_t XsDataPacket_gnssAge(const XsDataPacket* thisPtr) -{ - return GenericSimple::get(thisPtr, XDI_GnssAge, 255); -} + \param r : The rssi value that should be added to the packet + */ + void XsDataPacket_setRssi(XsDataPacket* thisPtr, int r) + { + GenericSimple::set(thisPtr, (uint8_t)(int8_t)r, XDI_Rssi); + } -/*! \brief Returns 1 if data item contains GnssAge, 0 otherwise - \returns true if this XsDataPacket containts GNSS age data -*/ -int XsDataPacket_containsGnssAge(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_GnssAge); -} + /*! \brief Returns a struct with RawGnssPvtData + \param returnVal The object to store the requested data in + \return a struct with RawGnssPvtData + */ + XsRawGnssPvtData* XsDataPacket_rawGnssPvtData(const XsDataPacket* thisPtr, XsRawGnssPvtData* returnVal) + { + return genericGet(thisPtr, returnVal, XDI_GnssPvtData); + } -/*! \brief Sets or updates the GnssAge value in the datapacket - \param age The new value that should be added to the packet -*/ -void XsDataPacket_setGnssAge(XsDataPacket* thisPtr, uint8_t age) -{ - GenericSimple::set(thisPtr, age, XDI_GnssAge); -} + /*! \brief Returns 1 if data item contains RawGnssPvtData, 0 otherwise + \returns true if this XsDataPacket contains Raw GNSS data + */ + int XsDataPacket_containsRawGnssPvtData(const XsDataPacket* thisPtr) + { + return genericContains(thisPtr, XDI_GnssPvtData); + } -/*! \brief Returns a struct with RawGnssSatInfo - \param returnVal The object to store the requested data in - \return a struct with RawGnssSatInfo -*/ -XsRawGnssSatInfo* XsDataPacket_rawGnssSatInfo(const XsDataPacket* thisPtr, XsRawGnssSatInfo* returnVal) -{ - return genericGet(thisPtr, returnVal, XDI_GnssSatInfo); -} + /*! \brief Sets or updates the RawGnssPvtData value in the datapacket + \param r The new value that should be added to the packet + */ + void XsDataPacket_setRawGnssPvtData(XsDataPacket* thisPtr, const XsRawGnssPvtData* r) + { + genericSet(thisPtr, r, XDI_GnssPvtData); + } -/*! \brief Returns 1 if data item contains RawGnssPvtData, 0 otherwise - \returns true if this XsDataPacket containts raw GNSS Sat Info -*/ -int XsDataPacket_containsRawGnssSatInfo(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_GnssSatInfo); -} + /*! \brief Returns the timestamp of a PVT Pulse -/*! \brief Sets or updates the RawGnssSatInfo value in the datapacket - \param data The new value that should be added to the packet -*/ -void XsDataPacket_setRawGnssSatInfo(XsDataPacket* thisPtr, const XsRawGnssSatInfo* data) -{ - genericSet(thisPtr, data, XDI_GnssSatInfo); -} + \returns Returns the timestamp of a PVT Pulse + */ + uint32_t XsDataPacket_gnssPvtPulse(const XsDataPacket* thisPtr) + { + return GenericSimple::get(thisPtr, XDI_GnssPvtPulse); + } -/*! \brief Merge the data items from \a other into the packet + /*! \brief Check if data item XsDataPacket_contains a pvt pulse + \returns true if this packet XsDataPacket_contains a spvt pulse + */ + int XsDataPacket_containsGnssPvtPulse(const XsDataPacket* thisPtr) + { + return genericContains(thisPtr, XDI_GnssPvtPulse); + } - \details The data items contained in XsDataPacket \a other will be added to - this packet. Items that are already present will be - overwritten depending on the value of \a overwrite. + /*! \brief Add/update pvt pulse timestamp data for the item + \param counter The new data to set + */ + void XsDataPacket_setGnssPvtPulse(XsDataPacket* thisPtr, uint32_t counter) + { + GenericSimple::set(thisPtr, counter, XDI_GnssPvtPulse); + } - \param other The XsDataPacket to read the data items from - \param overwrite When set to true, existing items will be overwritten, otherwise they will not be modified + /*! \brief Returns the age of the GNSS data (in samples) + \returns the age of the GNSS data + */ + uint8_t XsDataPacket_gnssAge(const XsDataPacket* thisPtr) + { + return GenericSimple::get(thisPtr, XDI_GnssAge, 255); + } - \returns Returns the updated data packet -*/ -XsDataPacket* XsDataPacket_merge(XsDataPacket* thisPtr, const XsDataPacket* other, int overwrite) -{ - detach(thisPtr); - bool over = !!overwrite; - thisPtr->d->merge(*other->d, over); + /*! \brief Returns 1 if data item contains GnssAge, 0 otherwise + \returns true if this XsDataPacket containts GNSS age data + */ + int XsDataPacket_containsGnssAge(const XsDataPacket* thisPtr) + { + return genericContains(thisPtr, XDI_GnssAge); + } - // do additional handling for 'unique' items - auto keepOne = [&](XsDataIdentifier id1, XsDataIdentifier id2) + /*! \brief Sets or updates the GnssAge value in the datapacket + \param age The new value that should be added to the packet + */ + void XsDataPacket_setGnssAge(XsDataPacket* thisPtr, uint8_t age) { - if (genericContains(thisPtr, id1) && - genericContains(thisPtr, id2)) - { - bool gc = genericContains(other, id1); - if ((gc && !over) || (!gc && over)) // logical xor does not exist in C++, write it explicitly - MAP.erase(id1); - else - MAP.erase(id2); - } - }; + GenericSimple::set(thisPtr, age, XDI_GnssAge); + } - keepOne(XDI_Quaternion, XDI_RotationMatrix); - keepOne(XDI_Quaternion, XDI_EulerAngles); - keepOne(XDI_EulerAngles, XDI_RotationMatrix); - keepOne(XDI_PacketCounter, XDI_FrameRange); - keepOne(XDI_SampleTime64, XDI_SampleTimeCoarse); - keepOne(XDI_SampleTime64, XDI_SampleTimeFine); + /*! \brief Returns a struct with RawGnssSatInfo + \param returnVal The object to store the requested data in + \return a struct with RawGnssSatInfo + */ + XsRawGnssSatInfo* XsDataPacket_rawGnssSatInfo(const XsDataPacket* thisPtr, XsRawGnssSatInfo* returnVal) + { + return genericGet(thisPtr, returnVal, XDI_GnssSatInfo); + } - if (over) + /*! \brief Returns 1 if data item contains RawGnssPvtData, 0 otherwise + \returns true if this XsDataPacket containts raw GNSS Sat Info + */ + int XsDataPacket_containsRawGnssSatInfo(const XsDataPacket* thisPtr) { - thisPtr->m_deviceId = other->m_deviceId; - thisPtr->m_toa = other->m_toa; - thisPtr->m_packetId = other->m_packetId; - thisPtr->m_etos = other->m_etos; + return genericContains(thisPtr, XDI_GnssSatInfo); } - return thisPtr; -} + /*! \brief Sets or updates the RawGnssSatInfo value in the datapacket + \param data The new value that should be added to the packet + */ + void XsDataPacket_setRawGnssSatInfo(XsDataPacket* thisPtr, const XsRawGnssSatInfo* data) + { + genericSet(thisPtr, data, XDI_GnssSatInfo); + } -/*! \brief Overwrite the contents of the XsDataPacket with the contents of the supplied XsMessage - \param msg The XsMessage to read from - \note The packet is cleared before inserting new items -*/ -void XsDataPacket_setMessage(XsDataPacket* thisPtr, const XsMessage* msg) -{ - XsDataPacket_clear(thisPtr, XDI_None); + /*! \brief Merge the data items from \a other into the packet - XsSize offset = 0; - XsSize sz = msg->getDataSize(); + \details The data items contained in XsDataPacket \a other will be added to + this packet. Items that are already present will be + overwritten depending on the value of \a overwrite. - while (offset+3 <= sz) // minimum size of an item is 2(ID) + 1(size) + 0(minimum size) + \param other The XsDataPacket to read the data items from + \param overwrite When set to true, existing items will be overwritten, otherwise they will not be modified + + \returns Returns the updated data packet + */ + XsDataPacket* XsDataPacket_merge(XsDataPacket* thisPtr, const XsDataPacket* other, int overwrite) { - XsDataIdentifier id = static_cast(XsMessage_getDataShort(msg, offset)); - XsSize itemSize = XsMessage_getDataByte(msg, offset+2); - if (offset + itemSize + 3 > sz) - break; // the item is corrupt + detach(thisPtr); + bool over = !!overwrite; + thisPtr->d->merge(*other->d, over); + + // do additional handling for 'unique' items + auto keepOne = [&](XsDataIdentifier id1, XsDataIdentifier id2) + { + if (genericContains(thisPtr, id1) && + genericContains(thisPtr, id2)) + { + bool gc = genericContains(other, id1); + if ((gc && !over) || (!gc && over)) // logical xor does not exist in C++, write it explicitly + MAP.erase(id1); + else + MAP.erase(id2); + } + }; + + keepOne(XDI_Quaternion, XDI_RotationMatrix); + keepOne(XDI_Quaternion, XDI_EulerAngles); + keepOne(XDI_EulerAngles, XDI_RotationMatrix); + keepOne(XDI_PacketCounter, XDI_FrameRange); + keepOne(XDI_SampleTime64, XDI_SampleTimeCoarse); + keepOne(XDI_SampleTime64, XDI_SampleTimeFine); - Variant* var = createVariant(id); - if (var) + if (over) { - itemSize = var->readFromMessage(*msg, offset+3, itemSize); - MAP.insert(id, var); + thisPtr->m_deviceId = other->m_deviceId; + thisPtr->m_toa = other->m_toa; + thisPtr->m_packetId = other->m_packetId; + thisPtr->m_etos = other->m_etos; } - offset += 3 + itemSize; // never use var->sizeInMsg() here, since it _may_ differ + + return thisPtr; } - if (offset < sz) + + /*! \brief Overwrite the contents of the XsDataPacket with the contents of the supplied XsMessage + \param msg The XsMessage to read from + \note The packet is cleared before inserting new items + */ + void XsDataPacket_setMessage(XsDataPacket* thisPtr, const XsMessage* msg) { - // if we get here then we detected some kind of corruption - // so we should not trust anything we read XsDataPacket_clear(thisPtr, XDI_None); - return; - } -} -/*! \brief Write the contents of the XsDataPacket to an XsMessage in MtData2 format - \details The function will clear the message and add all items in the XsDataPacket as a standard MtData2 message. - \param msg The message to write to -*/ -void XsDataPacket_toMessage(const XsDataPacket* thisPtr, XsMessage* msg) -{ - msg->resizeData(0); // clear the data part while leaving header intact - msg->setMessageId(XMID_MtData2); + XsSize offset = 0; + XsSize sz = msg->getDataSize(); - XsSize offset = 0; - msg->resizeData(2048); // prevent constant message resizing by pre-allocating a large message and later reducing its size - for (auto const& i : MAP) - { - XsSize sz = i.second->sizeInMsg(); - if (sz < 255) + while (offset + 3 <= sz) // minimum size of an item is 2(ID) + 1(size) + 0(minimum size) { - msg->setDataShort((uint16_t) i.second->dataId(), offset); - msg->setDataByte((uint8_t)sz, offset+2); - i.second->writeToMessage(*msg, offset+3); - offset += 3+sz; + XsDataIdentifier id = static_cast(XsMessage_getDataShort(msg, offset)); + XsSize itemSize = XsMessage_getDataByte(msg, offset + 2); + if (offset + itemSize + 3 > sz) + break; // the item is corrupt + + Variant* var = createVariant(id); + if (var) + { + itemSize = var->readFromMessage(*msg, offset + 3, itemSize); + MAP.insert(id, var); + } + offset += 3 + itemSize; // never use var->sizeInMsg() here, since it _may_ differ } - else + if (offset < sz) + { + // if we get here then we detected some kind of corruption + // so we should not trust anything we read + XsDataPacket_clear(thisPtr, XDI_None); + return; + } + } + + /*! \brief Write the contents of the XsDataPacket to an XsMessage in MtData2 format + \details The function will clear the message and add all items in the XsDataPacket as a standard MtData2 message. + \param msg The message to write to + */ + void XsDataPacket_toMessage(const XsDataPacket* thisPtr, XsMessage* msg) + { + msg->resizeData(0); // clear the data part while leaving header intact + msg->setMessageId(XMID_MtData2); + + XsSize offset = 0; + msg->resizeData(2048); // prevent constant message resizing by pre-allocating a large message and later reducing its size + for (auto const& i : MAP) { - XsSize sz2 = sz; - XsSize offset2 = offset; - while (sz2 >= 255) + XsSize sz = i.second->sizeInMsg(); + if (sz < 255) + { + msg->setDataShort((uint16_t) i.second->dataId(), offset); + msg->setDataByte((uint8_t)sz, offset + 2); + i.second->writeToMessage(*msg, offset + 3); + offset += 3 + sz; + } + else { + XsSize sz2 = sz; + XsSize offset2 = offset; + while (sz2 >= 255) + { + msg->setDataShort((uint16_t) i.second->dataId(), offset2); + msg->setDataByte((uint8_t)255, offset2 + 2); + offset2 += 258; + sz2 -= 255; + } msg->setDataShort((uint16_t) i.second->dataId(), offset2); - msg->setDataByte((uint8_t)255, offset2+2); - offset2 += 258; - sz2 -= 255; + msg->setDataByte((uint8_t)sz2, offset2 + 2); // note that this size may be 0 + i.second->writeToMessage(*msg, offset + 3); // individual write functions should takke extended size into account + offset = offset2 + 3 + sz2; } - msg->setDataShort((uint16_t) i.second->dataId(), offset2); - msg->setDataByte((uint8_t)sz2, offset2+2); // note that this size may be 0 - i.second->writeToMessage(*msg, offset+3); // individual write functions should takke extended size into account - offset = offset2+3+sz2; } + msg->resizeData(offset); } - msg->resizeData(offset); -} -/*! \brief Returns the Full Snapshot part of the XsDataPacket - \details Full Snapshot is an internal format used by Xsens devices for high accuracy data transfer. - In most cases XDA processing will remove this item from the XsDataPacket and replace it with items that - are more directly usable. - \param returnVal The object to store the requested data in. This must be a properly constructed object. - \returns The supplied \a returnVal, filled with the requested data or cleared if it was not available -*/ -XsSnapshot* XsDataPacket_fullSnapshot(const XsDataPacket* thisPtr, XsSnapshot* returnVal) -{ - genericGet(thisPtr, returnVal, XDI_FullSnapshot); - if (!returnVal->m_deviceId.isValid() && thisPtr->m_deviceId.isValid()) - returnVal->m_deviceId = thisPtr->m_deviceId; - return returnVal; -} + /*! \brief Returns the Full Snapshot part of the XsDataPacket + \details Full Snapshot is an internal format used by Xsens devices for high accuracy data transfer. + In most cases XDA processing will remove this item from the XsDataPacket and replace it with items that + are more directly usable. + \param returnVal The object to store the requested data in. This must be a properly constructed object. + \returns The supplied \a returnVal, filled with the requested data or cleared if it was not available + */ + XsSnapshot* XsDataPacket_fullSnapshot(const XsDataPacket* thisPtr, XsSnapshot* returnVal) + { + genericGet(thisPtr, returnVal, XDI_FullSnapshot); + if (!returnVal->m_deviceId.isValid() && thisPtr->m_deviceId.isValid()) + returnVal->m_deviceId = thisPtr->m_deviceId; + return returnVal; + } -/*! \brief Returns true if the XsDataPacket contains Full Snapshot data - \returns true if the XsDataPacket contains Full Snapshot data -*/ -int XsDataPacket_containsFullSnapshot(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_FullSnapshot); -} + /*! \brief Returns true if the XsDataPacket contains Full Snapshot data + \returns true if the XsDataPacket contains Full Snapshot data + */ + int XsDataPacket_containsFullSnapshot(const XsDataPacket* thisPtr) + { + return genericContains(thisPtr, XDI_FullSnapshot); + } -/*! \brief Sets the Awinda Snapshot part of the XsDataPacket - \param data The new data to set - \param retransmission When non-zero, the item is marked as a retransmitted packet -*/ -void XsDataPacket_setFullSnapshot(XsDataPacket* thisPtr, XsSnapshot const * data, int retransmission) -{ - genericSet(thisPtr, data, XDI_FullSnapshot | (retransmission ? XDI_RetransmissionFlag : XDI_None)); -} + /*! \brief Sets the Awinda Snapshot part of the XsDataPacket + \param data The new data to set + \param retransmission When non-zero, the item is marked as a retransmitted packet + */ + void XsDataPacket_setFullSnapshot(XsDataPacket* thisPtr, XsSnapshot const* data, int retransmission) + { + genericSet(thisPtr, data, XDI_FullSnapshot | (retransmission ? XDI_RetransmissionFlag : XDI_None)); + } -/*! \brief Returns the Awinda Snapshot part of the XsDataPacket - \details Awinda Snapshot is an internal format used by Xsens devices for high accuracy data trasnfer. - In most cases XDA processing will remove this item from the XsDataPacket and replace it with items that - are more directly usable. - \param returnVal The object to store the requested data in. This must be a properly constructed object. - \returns The supplied \a returnVal, filled with the requested data or cleared if it was not available -*/ -XsSnapshot* XsDataPacket_awindaSnapshot(const XsDataPacket* thisPtr, XsSnapshot* returnVal) -{ - return genericGet(thisPtr, returnVal, XDI_AwindaSnapshot); -} + /*! \brief Returns the Awinda Snapshot part of the XsDataPacket + \details Awinda Snapshot is an internal format used by Xsens devices for high accuracy data trasnfer. + In most cases XDA processing will remove this item from the XsDataPacket and replace it with items that + are more directly usable. + \param returnVal The object to store the requested data in. This must be a properly constructed object. + \returns The supplied \a returnVal, filled with the requested data or cleared if it was not available + */ + XsSnapshot* XsDataPacket_awindaSnapshot(const XsDataPacket* thisPtr, XsSnapshot* returnVal) + { + return genericGet(thisPtr, returnVal, XDI_AwindaSnapshot); + } -/*! \brief Returns true if the XsDataPacket contains Awinda Snapshot data - \returns true if the XsDataPacket contains Awinda Snapshot data -*/ -int XsDataPacket_containsAwindaSnapshot(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_AwindaSnapshot); -} + /*! \brief Returns true if the XsDataPacket contains Awinda Snapshot data + \returns true if the XsDataPacket contains Awinda Snapshot data + */ + int XsDataPacket_containsAwindaSnapshot(const XsDataPacket* thisPtr) + { + return genericContains(thisPtr, XDI_AwindaSnapshot); + } -/*! \brief Sets the Awinda Snapshot part of the XsDataPacket - \param data The new data to set - \param retransmission When non-zero, the item is marked as a retransmitted packet -*/ -void XsDataPacket_setAwindaSnapshot(XsDataPacket* thisPtr, XsSnapshot const * data, int retransmission) -{ - genericSet(thisPtr, data, XDI_AwindaSnapshot | (retransmission ? XDI_RetransmissionFlag : XDI_None)); -} + /*! \brief Sets the Awinda Snapshot part of the XsDataPacket + \param data The new data to set + \param retransmission When non-zero, the item is marked as a retransmitted packet + */ + void XsDataPacket_setAwindaSnapshot(XsDataPacket* thisPtr, XsSnapshot const* data, int retransmission) + { + genericSet(thisPtr, data, XDI_AwindaSnapshot | (retransmission ? XDI_RetransmissionFlag : XDI_None)); + } -/*! \brief Returns true if the contained Awinda Snapshot is marked as a retransmission - \returns true if the XsDataPacket contains Awinda Snapshot data and it is marked as a retransmission -*/ -int XsDataPacket_isAwindaSnapshotARetransmission(const XsDataPacket* thisPtr) -{ - if (!thisPtr->d) - return false; - auto it = MAP.find(XDI_AwindaSnapshot); - if (it == MAP.end()) - return false; - return (it->second->dataId() & XDI_RetransmissionMask) == XDI_RetransmissionFlag; -} + /*! \brief Returns true if the contained Awinda Snapshot is marked as a retransmission + \returns true if the XsDataPacket contains Awinda Snapshot data and it is marked as a retransmission + */ + int XsDataPacket_isAwindaSnapshotARetransmission(const XsDataPacket* thisPtr) + { + if (!thisPtr->d) + return false; + auto it = MAP.find(XDI_AwindaSnapshot); + if (it == MAP.end()) + return false; + return (it->second->dataId() & XDI_RetransmissionMask) == XDI_RetransmissionFlag; + } -/*! \brief Returns the Glove Snapshot part of the XsDataPacket - \details Glove Snapshot is an internal format used by Xsens devices for high accuracy data transfer. - In most cases XDA processing will remove this item from the XsDataPacket and replace it with items that - are more directly usable. - \param returnVal The object to store the requested data in. This must be a properly constructed object. - \param hand Which hand to get data for, must be either XHI_LeftHand or XHI_RightHand - \returns The supplied \a returnVal, filled with the requested data or cleared if it was not available -*/ -XsGloveSnapshot* XsDataPacket_gloveSnapshot(const XsDataPacket* thisPtr, XsGloveSnapshot* returnVal, XsHandId hand) -{ - switch (hand) - { - case XHI_LeftHand: - return genericGet(thisPtr, returnVal, XDI_GloveSnapshotLeft); - case XHI_RightHand: - return genericGet(thisPtr, returnVal, XDI_GloveSnapshotRight); - case XHI_Unknown: - default: - memset(returnVal, 0, sizeof(XsGloveSnapshot)); - return returnVal; + /*! \brief Returns the Glove Snapshot part of the XsDataPacket + \details Glove Snapshot is an internal format used by Xsens devices for high accuracy data transfer. + In most cases XDA processing will remove this item from the XsDataPacket and replace it with items that + are more directly usable. + \param returnVal The object to store the requested data in. This must be a properly constructed object. + \param hand Which hand to get data for, must be either XHI_LeftHand or XHI_RightHand + \returns The supplied \a returnVal, filled with the requested data or cleared if it was not available + */ + XsGloveSnapshot* XsDataPacket_gloveSnapshot(const XsDataPacket* thisPtr, XsGloveSnapshot* returnVal, XsHandId hand) + { + switch (hand) + { + case XHI_LeftHand: + return genericGet(thisPtr, returnVal, XDI_GloveSnapshotLeft); + case XHI_RightHand: + return genericGet(thisPtr, returnVal, XDI_GloveSnapshotRight); + case XHI_Unknown: + default: + memset(returnVal, 0, sizeof(XsGloveSnapshot)); + return returnVal; + } } -} -/*! \brief Returns true if the XsDataPacket contains Glove Snapshot data - \param hand Which hand to get data for, must be either XHI_LeftHand or XHI_RightHand for a particular side or XHI_Unknown for any side - \returns true if the XsDataPacket contains Glove Snapshot data -*/ -int XsDataPacket_containsGloveSnapshot(const XsDataPacket* thisPtr, XsHandId hand) -{ - switch (hand) + /*! \brief Returns true if the XsDataPacket contains Glove Snapshot data + \param hand Which hand to get data for, must be either XHI_LeftHand or XHI_RightHand for a particular side or XHI_Unknown for any side + \returns true if the XsDataPacket contains Glove Snapshot data + */ + int XsDataPacket_containsGloveSnapshot(const XsDataPacket* thisPtr, XsHandId hand) { - case XHI_LeftHand: - return genericContains(thisPtr, XDI_GloveSnapshotLeft); - case XHI_RightHand: - return genericContains(thisPtr, XDI_GloveSnapshotRight); - case XHI_Unknown: - return genericContains(thisPtr, XDI_GloveSnapshotLeft) || genericContains(thisPtr, XDI_GloveSnapshotRight); - default: - return false; + switch (hand) + { + case XHI_LeftHand: + return genericContains(thisPtr, XDI_GloveSnapshotLeft); + case XHI_RightHand: + return genericContains(thisPtr, XDI_GloveSnapshotRight); + case XHI_Unknown: + return genericContains(thisPtr, XDI_GloveSnapshotLeft) || genericContains(thisPtr, XDI_GloveSnapshotRight); + default: + return false; + } } -} -/*! \brief Sets the Glove Snapshot part of the XsDataPacket - \param data The new data to set - \param retransmission When non-zero, the item is marked as a retransmitted packet - \param hand Which hand to get data for, must be either XHI_LeftHand or XHI_RightHand -*/ -void XsDataPacket_setGloveSnapshot(XsDataPacket* thisPtr, XsGloveSnapshot const * data, int retransmission, XsHandId hand) -{ - switch (hand) - { - case XHI_LeftHand: - genericSet(thisPtr, data, XDI_GloveSnapshotLeft | (retransmission ? XDI_RetransmissionFlag : XDI_None)); - break; - case XHI_RightHand: - genericSet(thisPtr, data, XDI_GloveSnapshotRight | (retransmission ? XDI_RetransmissionFlag : XDI_None)); - break; - case XHI_Unknown: - default: - break; + /*! \brief Sets the Glove Snapshot part of the XsDataPacket + \param data The new data to set + \param retransmission When non-zero, the item is marked as a retransmitted packet + \param hand Which hand to get data for, must be either XHI_LeftHand or XHI_RightHand + */ + void XsDataPacket_setGloveSnapshot(XsDataPacket* thisPtr, XsGloveSnapshot const* data, int retransmission, XsHandId hand) + { + switch (hand) + { + case XHI_LeftHand: + genericSet(thisPtr, data, XDI_GloveSnapshotLeft | (retransmission ? XDI_RetransmissionFlag : XDI_None)); + break; + case XHI_RightHand: + genericSet(thisPtr, data, XDI_GloveSnapshotRight | (retransmission ? XDI_RetransmissionFlag : XDI_None)); + break; + case XHI_Unknown: + default: + break; + } } -} -/*! \brief Converts input vector \a input with data identifier \a id to output XsVector \a returnVal */ -static void convertRawVector(XsUShortVector const& input, XsDataIdentifier id, XsVector& returnVal) -{ - XsReal (*caster)(uint16_t) = unsigned_cast; - returnVal.setSize(3); - if ((id & XDI_DataFormatMask) == XDI_RawSigned) - caster = signed_cast; - for (XsSize i = 0; i < 3; i++) - returnVal[i] = caster(input[i]); -} + /*! \brief Converts input vector \a input with data identifier \a id to output XsVector \a returnVal */ + static void convertRawVector(XsUShortVector const& input, XsDataIdentifier id, XsVector& returnVal) + { + XsReal(*caster)(uint16_t) = unsigned_cast; + returnVal.setSize(3); + if ((id & XDI_DataFormatMask) == XDI_RawSigned) + caster = signed_cast; + for (XsSize i = 0; i < 3; i++) + returnVal[i] = caster(input[i]); + } -/*! \returns the data component specified by \a id and \a field, converted to floating point values - \param[in] id The data identifier of the data component - \param[in] field Pointer to a member in XsScrData, pointing to the data to retrieve - \param[out] returnVal the returned floating point values -*/ -static XsVector* convertedVector(const XsDataPacket* thisPtr, XsVector* returnVal, XsDataIdentifier id, XsUShortVector XsScrData::* field) -{ - assert(returnVal); + /*! \returns the data component specified by \a id and \a field, converted to floating point values + \param[in] id The data identifier of the data component + \param[in] field Pointer to a member in XsScrData, pointing to the data to retrieve + \param[out] returnVal the returned floating point values + */ + static XsVector* convertedVector(const XsDataPacket* thisPtr, XsVector* returnVal, XsDataIdentifier id, XsUShortVector XsScrData::* field) + { + assert(returnVal); - XsUShortVector tmp; + XsUShortVector tmp; - auto it = MAP.find(XDI_RawAccGyrMagTemp); - if (it != MAP.end()) - tmp = it->second->toDerived().m_data.*field; - else - { - it = MAP.find(id); + auto it = MAP.find(XDI_RawAccGyrMagTemp); if (it != MAP.end()) - tmp = it->second->toDerived().m_data; - } + tmp = it->second->toDerived().m_data.*field; + else + { + it = MAP.find(id); + if (it != MAP.end()) + tmp = it->second->toDerived().m_data; + } - convertRawVector(tmp, it->second->dataId(), *returnVal); - return returnVal; -} + convertRawVector(tmp, it->second->dataId(), *returnVal); + return returnVal; + } -/*! \brief The raw accelerometer component of a data item, converted to floating point values. + /*! \brief The raw accelerometer component of a data item, converted to floating point values. - \param returnVal : An XsVector to put the requested data in + \param returnVal : An XsVector to put the requested data in - \returns A XsVector containing the x, y and z axis values in that order -*/ -XsVector* XsDataPacket_rawAccelerationConverted(const XsDataPacket* thisPtr, XsVector* returnVal) -{ - return convertedVector(thisPtr, returnVal, XDI_RawAcc, &XsScrData::m_acc); -} + \returns A XsVector containing the x, y and z axis values in that order + */ + XsVector* XsDataPacket_rawAccelerationConverted(const XsDataPacket* thisPtr, XsVector* returnVal) + { + return convertedVector(thisPtr, returnVal, XDI_RawAcc, &XsScrData::m_acc); + } -/*! \brief The raw gyroscope component of a data item, converted to floating point values. + /*! \brief The raw gyroscope component of a data item, converted to floating point values. - \param returnVal : An XsVector to put the requested data in + \param returnVal : An XsVector to put the requested data in - \returns A XsVector containing the x, y and z axis values in that order -*/ -XsVector* XsDataPacket_rawGyroscopeDataConverted(const XsDataPacket* thisPtr, XsVector* returnVal) -{ - return convertedVector(thisPtr, returnVal, XDI_RawGyr, &XsScrData::m_gyr); -} + \returns A XsVector containing the x, y and z axis values in that order + */ + XsVector* XsDataPacket_rawGyroscopeDataConverted(const XsDataPacket* thisPtr, XsVector* returnVal) + { + return convertedVector(thisPtr, returnVal, XDI_RawGyr, &XsScrData::m_gyr); + } -/*! \brief The raw magnetometer component of a data item, converted to floating point values. + /*! \brief The raw magnetometer component of a data item, converted to floating point values. - \param returnVal : An XsVector to put the requested data in + \param returnVal : An XsVector to put the requested data in - \returns A XsVector containing the x, y and z axis values in that order -*/ -XsVector* XsDataPacket_rawMagneticFieldConverted(const XsDataPacket* thisPtr, XsVector* returnVal) -{ - return convertedVector(thisPtr, returnVal, XDI_RawMag, &XsScrData::m_mag); -} + \returns A XsVector containing the x, y and z axis values in that order + */ + XsVector* XsDataPacket_rawMagneticFieldConverted(const XsDataPacket* thisPtr, XsVector* returnVal) + { + return convertedVector(thisPtr, returnVal, XDI_RawMag, &XsScrData::m_mag); + } -/*! \brief The raw gyroscope temperature component of a data item, converted to floating point values. + /*! \brief The raw gyroscope temperature component of a data item, converted to floating point values. - \param returnVal : An XsVector to put the requested data in + \param returnVal : An XsVector to put the requested data in - \returns A XsVector containing the x, y and z axis values in that order -*/ -XsVector* XsDataPacket_rawGyroscopeTemperatureDataConverted(const XsDataPacket* thisPtr, XsVector* returnVal) -{ - assert(returnVal); - auto it = MAP.find(XDI_RawGyroTemp); - XsUShortVector tmp; - if (it != MAP.end()) - tmp = it->second->toDerived().m_data; - convertRawVector(tmp, it->second->dataId(), *returnVal); - return returnVal; -} + \returns A XsVector containing the x, y and z axis values in that order + */ + XsVector* XsDataPacket_rawGyroscopeTemperatureDataConverted(const XsDataPacket* thisPtr, XsVector* returnVal) + { + assert(returnVal); + auto it = MAP.find(XDI_RawGyroTemp); + XsUShortVector tmp; + if (it != MAP.end()) + tmp = it->second->toDerived().m_data; + convertRawVector(tmp, it->second->dataId(), *returnVal); + return returnVal; + } -/*! \brief Returns the raw blob part of the XsDataPacket - \param returnVal The object to store the requested data in. This must be a properly constructed object. - \returns The supplied \a returnVal, filled with the requested data or cleared if it was not available -*/ -XsByteArray* XsDataPacket_rawBlob(const XsDataPacket* thisPtr, XsByteArray* returnVal) -{ - return genericGet(thisPtr, returnVal, XDI_RawBlob); -} + /*! \brief Returns the raw blob part of the XsDataPacket + \param returnVal The object to store the requested data in. This must be a properly constructed object. + \returns The supplied \a returnVal, filled with the requested data or cleared if it was not available + */ + XsByteArray* XsDataPacket_rawBlob(const XsDataPacket* thisPtr, XsByteArray* returnVal) + { + return genericGet(thisPtr, returnVal, XDI_RawBlob); + } -/*! \brief Returns true if the XsDataPacket contains raw blob data - \returns true if the XsDataPacket contains raw blob data -*/ -int XsDataPacket_containsRawBlob(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_RawBlob); -} + /*! \brief Returns true if the XsDataPacket contains raw blob data + \returns true if the XsDataPacket contains raw blob data + */ + int XsDataPacket_containsRawBlob(const XsDataPacket* thisPtr) + { + return genericContains(thisPtr, XDI_RawBlob); + } -/*! \brief Sets the raw blob part of the XsDataPacket - \param data The new data to set -*/ -void XsDataPacket_setRawBlob(XsDataPacket* thisPtr, const XsByteArray *data) -{ - genericSet(thisPtr, data, XDI_RawBlob); -} + /*! \brief Sets the raw blob part of the XsDataPacket + \param data The new data to set + */ + void XsDataPacket_setRawBlob(XsDataPacket* thisPtr, const XsByteArray* data) + { + genericSet(thisPtr, data, XDI_RawBlob); + } -/*! \brief Returns AccelerationHR - \param returnVal : An XsVector to put the requested in - \returns A XsVector containing the x, y and z axis values in that order -*/ -XsVector* XsDataPacket_accelerationHR(const XsDataPacket* thisPtr, XsVector* returnVal) -{ - return genericGet(thisPtr, returnVal, XDI_AccelerationHR); -} + /*! \brief Returns AccelerationHR + \param returnVal : An XsVector to put the requested in + \returns A XsVector containing the x, y and z axis values in that order + */ + XsVector* XsDataPacket_accelerationHR(const XsDataPacket* thisPtr, XsVector* returnVal) + { + return genericGet(thisPtr, returnVal, XDI_AccelerationHR); + } -/*! \brief Check if data item contains AccelerationHR - \returns true if this packet contains AccelerationHR -*/ -int XsDataPacket_containsAccelerationHR(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_AccelerationHR); -} + /*! \brief Check if data item contains AccelerationHR + \returns true if this packet contains AccelerationHR + */ + int XsDataPacket_containsAccelerationHR(const XsDataPacket* thisPtr) + { + return genericContains(thisPtr, XDI_AccelerationHR); + } -/*! \brief Add/update the AccelerationHR for the item - \param vec A 3-component vector containing AccelerationHR -*/ -void XsDataPacket_setAccelerationHR(XsDataPacket* thisPtr, const XsVector* vec) -{ - genericSet(thisPtr, vec, XDI_AccelerationHR | XDI_SubFormatDouble); -} + /*! \brief Add/update the AccelerationHR for the item + \param vec A 3-component vector containing AccelerationHR + */ + void XsDataPacket_setAccelerationHR(XsDataPacket* thisPtr, const XsVector* vec) + { + genericSet(thisPtr, vec, XDI_AccelerationHR | XDI_SubFormatDouble); + } -/*! \brief Returns RateOfTurnHR - \param returnVal : An XsVector to put the requested in - \returns A XsVector containing the x, y and z axis values in that order -*/ -XsVector* XsDataPacket_rateOfTurnHR(const XsDataPacket* thisPtr, XsVector* returnVal) -{ - return genericGet(thisPtr, returnVal, XDI_RateOfTurnHR); -} + /*! \brief Returns RateOfTurnHR + \param returnVal : An XsVector to put the requested in + \returns A XsVector containing the x, y and z axis values in that order + */ + XsVector* XsDataPacket_rateOfTurnHR(const XsDataPacket* thisPtr, XsVector* returnVal) + { + return genericGet(thisPtr, returnVal, XDI_RateOfTurnHR); + } -/*! \brief Check if data item contains RateOfTurnHR - \returns true if this packet contains RateOfTurnHR -*/ -int XsDataPacket_containsRateOfTurnHR(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_RateOfTurnHR); -} + /*! \brief Check if data item contains RateOfTurnHR + \returns true if this packet contains RateOfTurnHR + */ + int XsDataPacket_containsRateOfTurnHR(const XsDataPacket* thisPtr) + { + return genericContains(thisPtr, XDI_RateOfTurnHR); + } -/*! \brief Add/update the RateOfTurnHR for the item - \param vec A 3-component vector containing RateOfTurnHR -*/ -void XsDataPacket_setRateOfTurnHR(XsDataPacket* thisPtr, const XsVector* vec) -{ - genericSet(thisPtr, vec, XDI_RateOfTurnHR | XDI_SubFormatDouble); -} + /*! \brief Add/update the RateOfTurnHR for the item + \param vec A 3-component vector containing RateOfTurnHR + */ + void XsDataPacket_setRateOfTurnHR(XsDataPacket* thisPtr, const XsVector* vec) + { + genericSet(thisPtr, vec, XDI_RateOfTurnHR | XDI_SubFormatDouble); + } -/*! \brief Returns the number of private data items for all XsDataPacket combined currently in memory - \details This is an internal value and should not be used for any other purpose. - \return The current number of items. -*/ -int XsDataPacket_privateCount() -{ - return DataPacketPrivate::creationDiff(); -} + /*! \brief Returns the number of private data items for all XsDataPacket combined currently in memory + \details This is an internal value and should not be used for any other purpose. + \return The current number of items. + */ + int XsDataPacket_privateCount() + { + return DataPacketPrivate::creationDiff(); + } } // extern "C" diff --git a/lib/xspublic/xstypes/xsdatapacket.h b/lib/xspublic/xstypes/xsdatapacket.h index f918bfe..1398a7c 100644 --- a/lib/xspublic/xstypes/xsdatapacket.h +++ b/lib/xspublic/xstypes/xsdatapacket.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -92,7 +92,7 @@ #include "xshandid.h" #ifndef XSNOEXPORT -#define XSNOEXPORT + #define XSNOEXPORT #endif //AUTO namespace xstypes { @@ -101,8 +101,7 @@ struct XsDataPacket; struct XSNOEXPORT DataPacketPrivate; #ifdef __cplusplus -extern "C" -{ +extern "C" { #endif #ifndef __cplusplus typedef struct XsDataPacket XsDataPacket; @@ -260,25 +259,25 @@ XSTYPES_DLL_API int XsDataPacket_containsGnssPvtPulse(const XsDataPacket* thisPt XSTYPES_DLL_API void XsDataPacket_setGnssPvtPulse(XsDataPacket* thisPtr, uint32_t counter); XSTYPES_DLL_API XsDataPacket* XsDataPacket_merge(XsDataPacket* thisPtr, const XsDataPacket* other, int overwrite); -XSTYPES_DLL_API void XsDataPacket_setTriggerIndication(XsDataPacket* thisPtr, XsDataIdentifier triggerId, const XsTriggerIndicationData * triggerIndicationData); +XSTYPES_DLL_API void XsDataPacket_setTriggerIndication(XsDataPacket* thisPtr, XsDataIdentifier triggerId, const XsTriggerIndicationData* triggerIndicationData); XSTYPES_DLL_API XsTriggerIndicationData* XsDataPacket_triggerIndication(const XsDataPacket* thisPtr, XsDataIdentifier triggerId, XsTriggerIndicationData* returnVal); XSTYPES_DLL_API int XsDataPacket_containsTriggerIndication(const XsDataPacket* thisPtr, XsDataIdentifier triggerId); XSTYPES_DLL_API void XsDataPacket_toMessage(const XsDataPacket* thisPtr, XsMessage* msg); -XSTYPES_DLL_API void XsDataPacket_setAwindaSnapshot(XsDataPacket* thisPtr, XsSnapshot const * data, int retransmission); +XSTYPES_DLL_API void XsDataPacket_setAwindaSnapshot(XsDataPacket* thisPtr, XsSnapshot const* data, int retransmission); XSTYPES_DLL_API XsSnapshot* XsDataPacket_awindaSnapshot(const XsDataPacket* thisPtr, XsSnapshot* returnVal); XSTYPES_DLL_API int XsDataPacket_containsAwindaSnapshot(const XsDataPacket* thisPtr); XSTYPES_DLL_API int XsDataPacket_isAwindaSnapshotARetransmission(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setFullSnapshot(XsDataPacket* thisPtr, XsSnapshot const * data, int retransmission); +XSTYPES_DLL_API void XsDataPacket_setFullSnapshot(XsDataPacket* thisPtr, XsSnapshot const* data, int retransmission); XSTYPES_DLL_API XsSnapshot* XsDataPacket_fullSnapshot(const XsDataPacket* thisPtr, XsSnapshot* returnVal); XSTYPES_DLL_API int XsDataPacket_containsFullSnapshot(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setGloveSnapshot(XsDataPacket* thisPtr, XsGloveSnapshot const * data, int retransmission, XsHandId hand); +XSTYPES_DLL_API void XsDataPacket_setGloveSnapshot(XsDataPacket* thisPtr, XsGloveSnapshot const* data, int retransmission, XsHandId hand); XSTYPES_DLL_API XsGloveSnapshot* XsDataPacket_gloveSnapshot(const XsDataPacket* thisPtr, XsGloveSnapshot* returnVal, XsHandId hand); XSTYPES_DLL_API int XsDataPacket_containsGloveSnapshot(const XsDataPacket* thisPtr, XsHandId hand); -XSTYPES_DLL_API void XsDataPacket_setRawBlob(XsDataPacket* thisPtr, const XsByteArray * data); +XSTYPES_DLL_API void XsDataPacket_setRawBlob(XsDataPacket* thisPtr, const XsByteArray* data); XSTYPES_DLL_API XsByteArray* XsDataPacket_rawBlob(const XsDataPacket* thisPtr, XsByteArray* returnVal); XSTYPES_DLL_API int XsDataPacket_containsRawBlob(const XsDataPacket* thisPtr); @@ -295,11 +294,12 @@ XSTYPES_DLL_API int XsDataPacket_privateCount(); #ifdef __cplusplus } // extern "C" #ifndef XSENS_NO_STL -#include // only needed for simplifiedContents() + #include // only needed for simplifiedContents() #endif #endif -struct XsDataPacket { +struct XsDataPacket +{ #ifdef __cplusplus /*! \brief Default constructor, initializes empty data packet or from the supplied \a msg \param msg Either 0 to create an empty object or a pointer to a valid %XsMessage containing @@ -812,7 +812,7 @@ struct XsDataPacket { } /*! \brief \copybrief XsDataPacket_storedDeviceId(const XsDataPacket*, XsDeviceId*) - \returns the device ID stored in this packet + \returns the device ID stored in this packet */ inline XsDeviceId storedDeviceId(void) const { @@ -833,7 +833,7 @@ struct XsDataPacket { } /*! \brief \copybrief XsDataPacket_storedLocationId(const XsDataPacket*) - \returns the location ID stored in this packet + \returns the location ID stored in this packet */ inline uint16_t storedLocationId(void) const { @@ -1343,7 +1343,7 @@ struct XsDataPacket { /*! \copydoc XsDataPacket_setFullSnapshot(XsDataPacket*, XsSnapshot const *, int) */ XSNOEXPORT inline void setFullSnapshot(XsSnapshot const& data, bool retransmission) { - XsDataPacket_setFullSnapshot(this, &data, retransmission?1:0); + XsDataPacket_setFullSnapshot(this, &data, retransmission ? 1 : 0); } /*! \brief \copybrief XsDataPacket_awindaSnapshot */ XSNOEXPORT inline XsSnapshot awindaSnapshot(void) const @@ -1361,7 +1361,7 @@ struct XsDataPacket { /*! \brief \copybrief XsDataPacket_setAwindaSnapshot */ XSNOEXPORT inline void setAwindaSnapshot(XsSnapshot const& raw, bool retransmission) { - XsDataPacket_setAwindaSnapshot(this, &raw, retransmission?1:0); + XsDataPacket_setAwindaSnapshot(this, &raw, retransmission ? 1 : 0); } /*! \brief \copybrief XsDataPacket_containsAwindaSnapshot(const XsDataPacket*) */ @@ -1392,7 +1392,7 @@ struct XsDataPacket { /*! \copydoc XsDataPacket_merge(XsDataPacket*, const XsDataPacket*, int) */ inline XsDataPacket& merge(const XsDataPacket& other, bool overwrite = true) { - return *XsDataPacket_merge(this, &other, overwrite?1:0); + return *XsDataPacket_merge(this, &other, overwrite ? 1 : 0); } /*! \brief Set the time of arrival of the data packet @@ -1441,7 +1441,7 @@ struct XsDataPacket { } /*! \copydoc XsDataPacket_setTriggerIndication(XsDataPacket*, XsDataIdentifier, const XsTriggerIndicationData *) */ - void setTriggerIndication(XsDataIdentifier triggerId, const XsTriggerIndicationData &triggerIndicationData) + void setTriggerIndication(XsDataIdentifier triggerId, const XsTriggerIndicationData& triggerIndicationData) { XsDataPacket_setTriggerIndication(this, triggerId, &triggerIndicationData); } @@ -1478,7 +1478,7 @@ struct XsDataPacket { } /*! \copydoc XsDataPacket_setRawBlob(XsDataPacket*, const XsByteArray *) */ - inline void setRawBlob(const XsByteArray &data) + inline void setRawBlob(const XsByteArray& data) { XsDataPacket_setRawBlob(this, &data); } @@ -1532,7 +1532,7 @@ struct XsDataPacket { } #endif -//protected: + //protected: /*! \privatesection */ #endif // __cplusplus struct XSNOEXPORT DataPacketPrivate* d; diff --git a/lib/xspublic/xstypes/xsdatapacketptr.h b/lib/xspublic/xstypes/xsdatapacketptr.h index 84d39a5..c7266f5 100644 --- a/lib/xspublic/xstypes/xsdatapacketptr.h +++ b/lib/xspublic/xstypes/xsdatapacketptr.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xstypes/xsdatapacketptrarray.c b/lib/xspublic/xstypes/xsdatapacketptrarray.c index cefa886..15cc5e2 100644 --- a/lib/xspublic/xstypes/xsdatapacketptrarray.c +++ b/lib/xspublic/xstypes/xsdatapacketptrarray.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -106,7 +106,8 @@ void zeroDataPacketPtr(XsDataPacketPtr* a) } //! \brief Descriptor for XsDataPacketPtrArray -XsArrayDescriptor const g_xsDataPacketPtrArrayDescriptor = { +XsArrayDescriptor const g_xsDataPacketPtrArrayDescriptor = +{ sizeof(XsDataPacketPtr), XSEXPCASTITEMSWAP swapDataPacketPtr, // swap XSEXPCASTITEMMAKE zeroDataPacketPtr, // construct diff --git a/lib/xspublic/xstypes/xsdatapacketptrarray.h b/lib/xspublic/xstypes/xsdatapacketptrarray.h index 8811494..848bc18 100644 --- a/lib/xspublic/xstypes/xsdatapacketptrarray.h +++ b/lib/xspublic/xstypes/xsdatapacketptrarray.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -88,41 +88,42 @@ XSTYPES_DLL_API void XsDataPacketPtrArray_construct(XsDataPacketPtrArray* thisPt #endif #ifdef __cplusplus -struct XsDataPacketPtrArray : public XsArrayImpl { - //! \brief Constructs an XsDataPacketPtrArray - inline explicit XsDataPacketPtrArray(XsSize sz = 0, XsDataPacketPtr const* src = 0) - : ArrayImpl(sz, src) + struct XsDataPacketPtrArray : public XsArrayImpl { - } + //! \brief Constructs an XsDataPacketPtrArray + inline explicit XsDataPacketPtrArray(XsSize sz = 0, XsDataPacketPtr const* src = 0) + : ArrayImpl(sz, src) + { + } - //! \brief Constructs an XsDataPacketPtrArray as a copy of \a other - inline XsDataPacketPtrArray(XsDataPacketPtrArray const& other) - : ArrayImpl(other) - { - } + //! \brief Constructs an XsDataPacketPtrArray as a copy of \a other + inline XsDataPacketPtrArray(XsDataPacketPtrArray const& other) + : ArrayImpl(other) + { + } - //! \brief Constructs an XsDataPacketPtrArray that references the data supplied in \a ref - inline explicit XsDataPacketPtrArray(XsDataPacketPtr* ref, XsSize sz, XsDataFlags flags /* = XSDF_None */) - : ArrayImpl(ref, sz, flags) - { - } + //! \brief Constructs an XsDataPacketPtrArray that references the data supplied in \a ref + inline explicit XsDataPacketPtrArray(XsDataPacketPtr* ref, XsSize sz, XsDataFlags flags /* = XSDF_None */) + : ArrayImpl(ref, sz, flags) + { + } #ifndef SWIG - /*! \brief Swap the contents the \a first and \a second array */ - friend void swap(XsDataPacketPtrArray& first, XsDataPacketPtrArray& second) - { - first.swap(second); - } + /*! \brief Swap the contents the \a first and \a second array */ + friend void swap(XsDataPacketPtrArray& first, XsDataPacketPtrArray& second) + { + first.swap(second); + } #endif #ifndef XSENS_NOITERATOR - //! \brief Constructs an XsDataPacketPtrArray with the array bound by the supplied iterators \a beginIt and \a endIt - template - inline XsDataPacketPtrArray(Iterator beginIt, Iterator endIt) - : ArrayImpl(beginIt, endIt) - { - } + //! \brief Constructs an XsDataPacketPtrArray with the array bound by the supplied iterators \a beginIt and \a endIt + template + inline XsDataPacketPtrArray(Iterator beginIt, Iterator endIt) + : ArrayImpl(beginIt, endIt) + { + } #endif -}; + }; #endif #endif diff --git a/lib/xspublic/xstypes/xsdebugcounters.cpp b/lib/xspublic/xstypes/xsdebugcounters.cpp index b72b6e4..71d4fe6 100644 --- a/lib/xspublic/xstypes/xsdebugcounters.cpp +++ b/lib/xspublic/xstypes/xsdebugcounters.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -75,83 +75,82 @@ volatile std::atomic_int XsMatrix_freeCount_value(0); volatile std::atomic_int XsArray_allocCount_value(0); volatile std::atomic_int XsArray_freeCount_value(0); -extern "C" -{ - -int XsVector_resetDebugCounts(void) -{ - return XsVector_allocCount_value.exchange(0) + XsVector_freeCount_value.exchange(0); -} - -int XsVector_allocCount(void) -{ - return XsVector_allocCount_value.load(); -} - -int XsVector_freeCount(void) -{ - return XsVector_freeCount_value.load(); -} - -int XsVector_incAllocCount(void) -{ - return ++XsVector_allocCount_value; -} - -int XsVector_incFreeCount(void) -{ - return ++XsVector_freeCount_value; -} - -int XsMatrix_resetDebugCounts(void) -{ - return XsMatrix_allocCount_value.exchange(0) + XsMatrix_freeCount_value.exchange(0); -} - -int XsMatrix_allocCount(void) -{ - return XsMatrix_allocCount_value.load(); -} - -int XsMatrix_freeCount(void) -{ - return XsMatrix_freeCount_value.load(); -} - -int XsMatrix_incAllocCount(void) -{ - return ++XsMatrix_allocCount_value; -} - -int XsMatrix_incFreeCount(void) -{ - return ++XsMatrix_freeCount_value; -} - -int XsArray_resetDebugCounts(void) -{ - return XsArray_allocCount_value.exchange(0) + XsArray_freeCount_value.exchange(0); -} - -int XsArray_allocCount(void) -{ - return XsArray_allocCount_value.load(); -} - -int XsArray_freeCount(void) -{ - return XsArray_freeCount_value.load(); -} - -int XsArray_incAllocCount(void) -{ - return ++XsArray_allocCount_value; -} - -int XsArray_incFreeCount(void) -{ - return ++XsArray_freeCount_value; -} +extern "C" { + + int XsVector_resetDebugCounts(void) + { + return XsVector_allocCount_value.exchange(0) + XsVector_freeCount_value.exchange(0); + } + + int XsVector_allocCount(void) + { + return XsVector_allocCount_value.load(); + } + + int XsVector_freeCount(void) + { + return XsVector_freeCount_value.load(); + } + + int XsVector_incAllocCount(void) + { + return ++XsVector_allocCount_value; + } + + int XsVector_incFreeCount(void) + { + return ++XsVector_freeCount_value; + } + + int XsMatrix_resetDebugCounts(void) + { + return XsMatrix_allocCount_value.exchange(0) + XsMatrix_freeCount_value.exchange(0); + } + + int XsMatrix_allocCount(void) + { + return XsMatrix_allocCount_value.load(); + } + + int XsMatrix_freeCount(void) + { + return XsMatrix_freeCount_value.load(); + } + + int XsMatrix_incAllocCount(void) + { + return ++XsMatrix_allocCount_value; + } + + int XsMatrix_incFreeCount(void) + { + return ++XsMatrix_freeCount_value; + } + + int XsArray_resetDebugCounts(void) + { + return XsArray_allocCount_value.exchange(0) + XsArray_freeCount_value.exchange(0); + } + + int XsArray_allocCount(void) + { + return XsArray_allocCount_value.load(); + } + + int XsArray_freeCount(void) + { + return XsArray_freeCount_value.load(); + } + + int XsArray_incAllocCount(void) + { + return ++XsArray_allocCount_value; + } + + int XsArray_incFreeCount(void) + { + return ++XsArray_freeCount_value; + } } // extern "C" diff --git a/lib/xspublic/xstypes/xsdebugcounters.h b/lib/xspublic/xstypes/xsdebugcounters.h index 5e03953..b3a0732 100644 --- a/lib/xspublic/xstypes/xsdebugcounters.h +++ b/lib/xspublic/xstypes/xsdebugcounters.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -96,23 +96,68 @@ extern int XsArray_incFreeCount(void); #else -inline static int XsVector_resetDebugCounts(void) { return 0; } -inline static int XsVector_allocCount(void) { return 0; } -inline static int XsVector_freeCount(void) { return 0; } -inline static int XsVector_incAllocCount(void) { return 0; } -inline static int XsVector_incFreeCount(void) { return 0; } - -inline static int XsMatrix_resetDebugCounts(void) { return 0; } -inline static int XsMatrix_allocCount(void) { return 0; } -inline static int XsMatrix_freeCount(void) { return 0; } -inline static int XsMatrix_incAllocCount(void) { return 0; } -inline static int XsMatrix_incFreeCount(void) { return 0; } - -inline static int XsArray_resetDebugCounts(void) { return 0; } -inline static int XsArray_allocCount(void) { return 0; } -inline static int XsArray_freeCount(void) { return 0; } -inline static int XsArray_incAllocCount(void) { return 0; } -inline static int XsArray_incFreeCount(void) { return 0; } +inline static int XsVector_resetDebugCounts(void) +{ + return 0; +} +inline static int XsVector_allocCount(void) +{ + return 0; +} +inline static int XsVector_freeCount(void) +{ + return 0; +} +inline static int XsVector_incAllocCount(void) +{ + return 0; +} +inline static int XsVector_incFreeCount(void) +{ + return 0; +} + +inline static int XsMatrix_resetDebugCounts(void) +{ + return 0; +} +inline static int XsMatrix_allocCount(void) +{ + return 0; +} +inline static int XsMatrix_freeCount(void) +{ + return 0; +} +inline static int XsMatrix_incAllocCount(void) +{ + return 0; +} +inline static int XsMatrix_incFreeCount(void) +{ + return 0; +} + +inline static int XsArray_resetDebugCounts(void) +{ + return 0; +} +inline static int XsArray_allocCount(void) +{ + return 0; +} +inline static int XsArray_freeCount(void) +{ + return 0; +} +inline static int XsArray_incAllocCount(void) +{ + return 0; +} +inline static int XsArray_incFreeCount(void) +{ + return 0; +} #endif diff --git a/lib/xspublic/xstypes/xsdevicecapabilities.c b/lib/xspublic/xstypes/xsdevicecapabilities.c index 9b3be14..e197e93 100644 --- a/lib/xspublic/xstypes/xsdevicecapabilities.c +++ b/lib/xspublic/xstypes/xsdevicecapabilities.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -69,7 +69,7 @@ */ /*! \addtogroup cinterface C Interface -@{ + @{ */ /*! \brief Test if the given \a XsDeviceCapabilities is valid. diff --git a/lib/xspublic/xstypes/xsdevicecapabilities.h b/lib/xspublic/xstypes/xsdevicecapabilities.h index 235bea6..7f2cbe5 100644 --- a/lib/xspublic/xstypes/xsdevicecapabilities.h +++ b/lib/xspublic/xstypes/xsdevicecapabilities.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xstypes/xsdeviceid.c b/lib/xspublic/xstypes/xsdeviceid.c index 48c33c1..34ca945 100644 --- a/lib/xspublic/xstypes/xsdeviceid.c +++ b/lib/xspublic/xstypes/xsdeviceid.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -78,7 +78,7 @@ */ /*! \returns The legacy bit used to identify legacy or new XsDeviceId format */ -uint64_t XsDeviceId_legacyBit(const struct XsDeviceId *thisPtr) +uint64_t XsDeviceId_legacyBit(const struct XsDeviceId* thisPtr) { (void)thisPtr; return XS_DID64_BIT; @@ -87,7 +87,7 @@ uint64_t XsDeviceId_legacyBit(const struct XsDeviceId *thisPtr) /*! \brief Test if the device ID represents a legacy device identification \returns true if this XsDeviceId represents a legacy device identification */ -int XsDeviceId_isLegacyDeviceId(const struct XsDeviceId *thisPtr) +int XsDeviceId_isLegacyDeviceId(const struct XsDeviceId* thisPtr) { return ((thisPtr->m_deviceId & XS_DID64_BIT) == 0); } @@ -98,9 +98,7 @@ int XsDeviceId_isLegacyDeviceId(const struct XsDeviceId *thisPtr) int XsDeviceId_isMtiX(const struct XsDeviceId* thisPtr) { if (XsDeviceId_isLegacyDeviceId(thisPtr)) - { return ((thisPtr->m_deviceId & XS_DID_TYPEH_MASK) == XS_DID_TYPEH_MT_X_MPU); - } else { if (memcmp(thisPtr->m_productCode, "MTi-", 4) != 0) @@ -116,9 +114,7 @@ int XsDeviceId_isMtiX(const struct XsDeviceId* thisPtr) int XsDeviceId_isMtiX0(const struct XsDeviceId* thisPtr) { if (XsDeviceId_isLegacyDeviceId(thisPtr)) - { return ((thisPtr->m_deviceId & XS_DID_TYPEH_MASK) == XS_DID_TYPEH_MT_X0); - } else { if (memcmp(thisPtr->m_productCode, "MTi-", 4) != 0) @@ -134,18 +130,14 @@ int XsDeviceId_isMtiX0(const struct XsDeviceId* thisPtr) int XsDeviceId_isMtiX00(const struct XsDeviceId* thisPtr) { if (XsDeviceId_isLegacyDeviceId(thisPtr)) - { return ((thisPtr->m_deviceId & XS_DID_TYPEH_MASK) == XS_DID_TYPEH_MT_X00); - } else { if (memcmp(thisPtr->m_productCode, "MTi-", 4) != 0) return 0; int deviceFamily = atoi(&thisPtr->m_productCode[4]); if ((deviceFamily != 0) && (deviceFamily >= 100 && deviceFamily <= 300)) - { return 1; - } else if (memcmp(thisPtr->m_productCode, "MTi-G-", 6) == 0) { deviceFamily = atoi(&thisPtr->m_productCode[6]); @@ -164,7 +156,7 @@ int XsDeviceId_isMtigX00(const struct XsDeviceId* thisPtr) if (XsDeviceId_isLegacyDeviceId(thisPtr)) { return ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_700) && - ((thisPtr->m_deviceId &~XS_DID_TYPEL_COMM_MASK) < XS_DID_MK4TYPE_MT_710_RANGE_START); + ((thisPtr->m_deviceId & ~XS_DID_TYPEL_COMM_MASK) < XS_DID_MK4TYPE_MT_710_RANGE_START); } else { @@ -202,9 +194,7 @@ int XsDeviceId_isMtigX10(const struct XsDeviceId* thisPtr) int XsDeviceId_isMti6X0(const struct XsDeviceId* thisPtr) { if (XsDeviceId_isLegacyDeviceId(thisPtr)) - { return 0; - } else { if (memcmp(thisPtr->m_productCode, "MTi-", 4) != 0) @@ -221,9 +211,7 @@ int XsDeviceId_isMti6X0(const struct XsDeviceId* thisPtr) int XsDeviceId_isMti8X0(const struct XsDeviceId* thisPtr) { if (XsDeviceId_isLegacyDeviceId(thisPtr)) - { return 0; - } else { if (memcmp(thisPtr->m_productCode, "MTi-", 4) != 0) @@ -240,9 +228,7 @@ int XsDeviceId_isMti8X0(const struct XsDeviceId* thisPtr) int XsDeviceId_isMti3X0(const struct XsDeviceId* thisPtr) { if (XsDeviceId_isLegacyDeviceId(thisPtr)) - { return ((thisPtr->m_deviceId & XS_DID_TYPEH_MASK) == XS_DID_TYPEH_MT_3X0); - } else return 0; } @@ -268,15 +254,44 @@ int XsDeviceId_isGlove(const struct XsDeviceId* thisPtr) } } +/*! \brief Return the side the device should be worn on. + \details Currently this only applies to the Xsens Glove product line. For other devices XHI_Unknown will be returned. + \return The hand id of the provided Glove XsDeviceId, XHI_Unknown if the device ID does not represent a Glove +*/ +XsHandId XsDeviceId_side(struct XsDeviceId const* thisPtr) +{ + if (XsDeviceId_isLegacyDeviceId(thisPtr)) + { + if (thisPtr->m_deviceId == XS_DID_GLOVEMASTER_LEFT) + return XHI_LeftHand; + if (thisPtr->m_deviceId == XS_DID_GLOVEMASTER_RIGHT) + return XHI_RightHand; + return XHI_Unknown; + } + else + { + if (memcmp(thisPtr->m_productCode, "Glove", 5) != 0) + return XHI_Unknown; + + switch (thisPtr->m_productCode[8]) + { + case 'L': + return XHI_LeftHand; + case 'R': + return XHI_RightHand; + default: + return XHI_Unknown; + } + } +} + /*! \brief Test if the device ID represents a Dot series device \returns true if this XsDeviceId represents a Dot series device */ int XsDeviceId_isDot(struct XsDeviceId const* thisPtr) { if (XsDeviceId_isLegacyDeviceId(thisPtr)) - { return 0; - } else { if (memcmp(thisPtr->m_productCode, "XS-", 3) != 0) @@ -328,13 +343,9 @@ int XsDeviceId_isMtw(const struct XsDeviceId* thisPtr) int XsDeviceId_isMtw2(const struct XsDeviceId* thisPtr) { if (XsDeviceId_isLegacyDeviceId(thisPtr)) - { return XS_DID_MTW2(thisPtr->m_deviceId); - } else - { return (memcmp(thisPtr->m_productCode, "MTw2", 4) == 0); - } } /*! \brief Test if this device ID represents an MTx @@ -351,13 +362,9 @@ int XsDeviceId_isMtx(const struct XsDeviceId* thisPtr) int XsDeviceId_isMtx2(const struct XsDeviceId* thisPtr) { if (XsDeviceId_isLegacyDeviceId(thisPtr)) - { return XS_DID_MTX2(thisPtr->m_deviceId); - } else - { return (memcmp(thisPtr->m_productCode, "MTx2", 4) == 0); - } } /*! \brief Test if this device ID represents a bodypack (any version) device. @@ -401,13 +408,11 @@ int XsDeviceId_isWirelessMaster(const struct XsDeviceId* thisPtr) if (XsDeviceId_isLegacyDeviceId(thisPtr)) { return ((thisPtr->m_deviceId & XS_DID_TYPEH_MASK) == XS_DID_TYPEH_AWINDAMASTER) && - !XsDeviceId_isBodyPack(thisPtr) && - !XsDeviceId_isSyncStationX(thisPtr); + !XsDeviceId_isBodyPack(thisPtr) && + !XsDeviceId_isSyncStationX(thisPtr); } else - { return (memcmp(thisPtr->m_productCode, "AW-", 3) == 0); - } } /*! \brief Test if this device ID represents an Awinda device. @@ -448,13 +453,9 @@ int XsDeviceId_isAwindaXOem(const struct XsDeviceId* thisPtr) int XsDeviceId_isAwinda2(const struct XsDeviceId* thisPtr) { if (XsDeviceId_isLegacyDeviceId(thisPtr)) - { return XS_DID_AWINDA2(thisPtr->m_deviceId); - } else - { return (memcmp(thisPtr->m_productCode, "AW-", 3) == 0); - } } /*! \brief Test if this device ID represents an Awinda2 Station. @@ -463,13 +464,9 @@ int XsDeviceId_isAwinda2(const struct XsDeviceId* thisPtr) int XsDeviceId_isAwinda2Station(const struct XsDeviceId* thisPtr) { if (XsDeviceId_isLegacyDeviceId(thisPtr)) - { return XS_DID_AWINDA2_STATION(thisPtr->m_deviceId); - } else - { return memcmp(thisPtr->m_productCode, "AW-A2", 5) == 0; - } } /*! \brief Test if this device ID represents an Awinda2 Dongle. @@ -478,13 +475,9 @@ int XsDeviceId_isAwinda2Station(const struct XsDeviceId* thisPtr) int XsDeviceId_isAwinda2Dongle(const struct XsDeviceId* thisPtr) { if (XsDeviceId_isLegacyDeviceId(thisPtr)) - { return XS_DID_AWINDA2_DONGLE(thisPtr->m_deviceId); - } else - { return memcmp(thisPtr->m_productCode, "AW-DNG2", 7) == 0; - } } /*! \brief Test if this device ID represents an Awinda2 OEM board. @@ -517,15 +510,9 @@ int XsDeviceId_isSyncStation2(const struct XsDeviceId* thisPtr) int XsDeviceId_isHilDevice(const struct XsDeviceId* thisPtr) { if (XsDeviceId_isLegacyDeviceId(thisPtr)) - { return 0; - } else - { - if (strcmp(thisPtr->m_productCode, "HILDEVICE") != 0) - return 0; - return 1; - } + return strcmp(thisPtr->m_productCode, "HILDEVICE") == 0; } /*! \brief Test if this device ID represents an IMU. @@ -536,9 +523,9 @@ int XsDeviceId_isImu(const struct XsDeviceId* thisPtr) if (XsDeviceId_isLegacyDeviceId(thisPtr)) { return (((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_1_MPU) || - ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_10) || - ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_100) || - ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_310)); + ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_10) || + ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_100) || + ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_310)); } else { @@ -564,9 +551,9 @@ int XsDeviceId_isVru(const struct XsDeviceId* thisPtr) if (XsDeviceId_isLegacyDeviceId(thisPtr)) { return (((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_2_MPU) || - ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_20) || - ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_200) || - ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_320)); + ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_20) || + ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_200) || + ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_320)); } else { @@ -592,9 +579,9 @@ int XsDeviceId_isAhrs(const struct XsDeviceId* thisPtr) if (XsDeviceId_isLegacyDeviceId(thisPtr)) { return (((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_3_MPU) || - ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_30) || - ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_300) || - ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_330)); + ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_30) || + ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_300) || + ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_330)); } else { @@ -620,7 +607,7 @@ int XsDeviceId_isGnss(const struct XsDeviceId* thisPtr) if (XsDeviceId_isLegacyDeviceId(thisPtr)) { return (((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_700) || - ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_7_MPU)); + ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_7_MPU)); } else { @@ -629,13 +616,9 @@ int XsDeviceId_isGnss(const struct XsDeviceId* thisPtr) int deviceFamily = atoi(&thisPtr->m_productCode[4]); if (deviceFamily == 7) - { return 1; - } else if ((deviceFamily == 670) || (deviceFamily == 680) || (deviceFamily == 870) || (deviceFamily == 880)) - { return 1; - } else { if (memcmp(thisPtr->m_productCode, "MTi-G-", 6) != 0) @@ -653,23 +636,14 @@ int XsDeviceId_isGnss(const struct XsDeviceId* thisPtr) int XsDeviceId_isRtk(const struct XsDeviceId* thisPtr) { if (XsDeviceId_isLegacyDeviceId(thisPtr)) - { return 0; - } else { if (memcmp(thisPtr->m_productCode, "MTi-", 4) != 0) return 0; int deviceFamily = atoi(&thisPtr->m_productCode[4]); - if ((deviceFamily == 680) || (deviceFamily == 880)) - { - return 1; - } - else - { - return 0; - } + return ((deviceFamily == 680) || (deviceFamily == 880)); } } @@ -678,8 +652,7 @@ int XsDeviceId_isRtk(const struct XsDeviceId* thisPtr) */ int XsDeviceId_isContainerDevice(const struct XsDeviceId* thisPtr) { - return XsDeviceId_isBodyPack(thisPtr) || - XsDeviceId_isWirelessMaster(thisPtr); + return XsDeviceId_isBodyPack(thisPtr) || XsDeviceId_isWirelessMaster(thisPtr); } /*! \brief Test if this device ID represents an MT device (any Mti, Mtig, Mtx or Mtw) @@ -746,8 +719,12 @@ void XsDeviceId_toString(const XsDeviceId* thisPtr, XsString* str) } else { - char device[17]; - XsSize n = (XsSize) (ptrdiff_t) sprintf(device, "%010" PRINTF_INT64_MODIFIER "X", thisPtr->m_deviceId); + XsSize n; + char device[23]; + if (thisPtr->m_subDevice) + n = (XsSize)(ptrdiff_t) sprintf(device, "%010" PRINTF_INT64_MODIFIER "X/%hu", thisPtr->m_deviceId, (unsigned short int) thisPtr->m_subDevice); + else + n = (XsSize)(ptrdiff_t) sprintf(device, "%010" PRINTF_INT64_MODIFIER "X", thisPtr->m_deviceId); XsString_assign(str, n, device); } } @@ -755,19 +732,23 @@ void XsDeviceId_toString(const XsDeviceId* thisPtr, XsString* str) /*! \brief Read a device ID from the supplied string. \param str The string to interpret */ -void XsDeviceId_fromString(XsDeviceId* thisPtr, const XsString *str) +void XsDeviceId_fromString(XsDeviceId* thisPtr, const XsString* str) { uint64_t tmp = 0; - int count = 0; + uint16_t sub = 0; if (!thisPtr || !str || !str->m_data) return; if (isalpha(str->m_data[0])) return; - int result = sscanf(str->m_data, "%" PRINTF_INT64_MODIFIER "x%n", &tmp, &count); - if (result == 1) + int result = sscanf(str->m_data, "%" PRINTF_INT64_MODIFIER "x/%hu", &tmp, &sub); + if (result >= 1) thisPtr->m_deviceId = tmp; + if (result >= 2) + thisPtr->m_subDevice = sub; + else + thisPtr->m_subDevice = 0; } /*! \brief Get a string with a readable representation of this device ID. Either full or as a type @@ -784,16 +765,16 @@ void XsDeviceId_toDeviceTypeString(const XsDeviceId* thisPtr, XsString* str, int char device[50]; XsSize n; if (thisPtr->m_hardwareVersion == 0) - n = (XsSize) (ptrdiff_t) sprintf(device, "%s%s%08X.%08X", deviceType.m_productCode, deviceType.m_productCode[0] ? "_" : "", (uint32_t)deviceType.m_deviceId, thisPtr->m_productVariant); + n = (XsSize)(ptrdiff_t) sprintf(device, "%s%s%08X.%08X", deviceType.m_productCode, deviceType.m_productCode[0] ? "_" : "", (uint32_t)deviceType.m_deviceId, thisPtr->m_productVariant); else - n = (XsSize) (ptrdiff_t) sprintf(device, "%s%s%08X.%08X.%d_%d", deviceType.m_productCode, deviceType.m_productCode[0] ? "_" : "", (uint32_t)deviceType.m_deviceId, thisPtr->m_productVariant, (uint8_t)((thisPtr->m_hardwareVersion & 0xFF00) >> 8), (uint8_t)(thisPtr->m_hardwareVersion & 0xFF)); + n = (XsSize)(ptrdiff_t) sprintf(device, "%s%s%08X.%08X.%d_%d", deviceType.m_productCode, deviceType.m_productCode[0] ? "_" : "", (uint32_t)deviceType.m_deviceId, thisPtr->m_productVariant, (uint8_t)((thisPtr->m_hardwareVersion & 0xFF00) >> 8), (uint8_t)(thisPtr->m_hardwareVersion & 0xFF)); XsString_assign(str, n, device); } /*! \brief Read a device ID from the supplied string. \param str The string to interpret */ -void XsDeviceId_fromDeviceTypeString(XsDeviceId* thisPtr, const XsString *str) +void XsDeviceId_fromDeviceTypeString(XsDeviceId* thisPtr, const XsString* str) { uint32_t id = 0; int hwRevH = 0, hwRevL = 0; @@ -944,7 +925,7 @@ void XsDeviceId_typeName(XsDeviceId const* thisPtr, XsString* str) XsString_assignCharArray(str, "MTi-200"); else if (XsDeviceId_isMtMk4_300(thisPtr)) XsString_assignCharArray(str, "MTi-300"); - else if ( XsDeviceId_isMtMk4_400(thisPtr)) + else if (XsDeviceId_isMtMk4_400(thisPtr)) XsString_assignCharArray(str, "MTi-400"); else if (XsDeviceId_isMtMk4_500(thisPtr)) XsString_assignCharArray(str, "MTi-500"); @@ -956,6 +937,8 @@ void XsDeviceId_typeName(XsDeviceId const* thisPtr, XsString* str) XsString_assignCharArray(str, "MTi-G-800"); else if (XsDeviceId_isMtMk4_900(thisPtr)) XsString_assignCharArray(str, "MTi-G-900"); + else if (XsDeviceId_isGlove(thisPtr)) + XsString_assignCharArray(str, "Glove"); else XsString_assignCharArray(str, "Unknown"); } @@ -1044,9 +1027,9 @@ void XsDeviceId_deviceTypeMask(struct XsDeviceId const* thisPtr, int detailed, s else if (XsDeviceId_isMtMk4(thisPtr)) type->m_deviceId = (XS_DID_TYPEH_MASK | (detailed ? (XS_DID_GPH_MASK | XS_DID_GPL_MASK | XS_DID_TYPEL_MK5) : 0)); else if (XsDeviceId_isAwindaX(thisPtr)) - type->m_deviceId = (XS_DID_TYPEH_MASK | (detailed ? (XS_DID_GPH_MASK | XS_DID_GPL_MASK | XS_DID_TYPEL_MASK) : 0)); + type->m_deviceId = (XS_DID_TYPEH_MASK | (detailed ? (XS_DID_GPH_MASK | XS_DID_GPL_MASK) : 0)); else if (XsDeviceId_isSyncStationX(thisPtr)) - type->m_deviceId = (XS_DID_TYPEH_MASK | (detailed ? (XS_DID_GPH_MASK | XS_DID_GPL_MASK | XS_DID_TYPEL_MASK) : 0)); + type->m_deviceId = (XS_DID_TYPEH_MASK | (detailed ? (XS_DID_GPH_MASK | XS_DID_GPL_MASK) : 0)); else if (XsDeviceId_isMtw(thisPtr) || XsDeviceId_isMtx(thisPtr)) type->m_deviceId = (XS_DID_TYPE_MASK | (detailed ? (XS_DID_GPH_MASK | XS_DID_GPL_MASK) : 0)); else if (thisPtr->m_deviceId == XS_DID_ABMCLOCKMASTER) @@ -1071,8 +1054,8 @@ void XsDeviceId_deviceTypeMask(struct XsDeviceId const* thisPtr, int detailed, s int XsDeviceId_isMtMk4(const struct XsDeviceId* thisPtr) { return (XsDeviceId_isMtMk4_X(thisPtr) || - XsDeviceId_isMtMk4_X0(thisPtr) || - XsDeviceId_isMtMk4_X00(thisPtr)); + XsDeviceId_isMtMk4_X0(thisPtr) || + XsDeviceId_isMtMk4_X00(thisPtr)); } /*! \brief Test if this device ID represents an MTMk4 1 series. diff --git a/lib/xspublic/xstypes/xsdeviceid.h b/lib/xspublic/xstypes/xsdeviceid.h index 4296cc9..df72762 100644 --- a/lib/xspublic/xstypes/xsdeviceid.h +++ b/lib/xspublic/xstypes/xsdeviceid.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -68,7 +68,9 @@ #include "xstypesconfig.h" #include "pstdint.h" #include "xsstring.h" +#include "xshandid.h" #ifdef __cplusplus +#include extern "C" { #endif @@ -76,7 +78,7 @@ extern "C" { #define XSDEVICEID_PRODUCT_CODE_INIT "\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0" #ifndef __cplusplus -#define XSDEVICEID_INITIALIZER { 0, XSDEVICEID_PRODUCT_CODE_INIT, 0, 0 } +#define XSDEVICEID_INITIALIZER { 0, XSDEVICEID_PRODUCT_CODE_INIT, 0, 0, 0 } #endif struct XsDeviceId; @@ -111,6 +113,7 @@ XSTYPES_DLL_API int XsDeviceId_isSyncStationX(struct XsDeviceId const* thisPtr); XSTYPES_DLL_API int XsDeviceId_isSyncStation2(struct XsDeviceId const* thisPtr); XSTYPES_DLL_API int XsDeviceId_isHilDevice(struct XsDeviceId const* thisPtr); XSTYPES_DLL_API int XsDeviceId_isGlove(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API XsHandId XsDeviceId_side(struct XsDeviceId const* thisPtr); XSTYPES_DLL_API int XsDeviceId_isDot(struct XsDeviceId const* thisPtr); XSTYPES_DLL_API int XsDeviceId_isRugged(struct XsDeviceId const* thisPtr); @@ -189,26 +192,38 @@ XSTYPES_DLL_API int XsDeviceId_isMtMk5_710(struct XsDeviceId const* thisPtr); struct XsDeviceId { #ifdef __cplusplus - /*! \brief Constructor that creates an XsDeviceId from the supplied \a productcode, \a hardwareVersion, \a productVariant and \a serialNumber */ - inline XsDeviceId(const char* productCode, uint16_t hardwareVersion, uint32_t productVariant, uint64_t serialNumber) + /*! \brief Constructor that creates an XsDeviceId from the supplied \a productcode, \a hardwareVersion, \a productVariant, \a serialNumber and optional \a subDevice */ + inline XsDeviceId(const char* productCode, uint16_t hardwareVersion, uint32_t productVariant, uint64_t serialNumber, uint16_t subDevice = 0) : m_deviceId(serialNumber) - , m_hardwareVersion(hardwareVersion) , m_productVariant(productVariant) + , m_hardwareVersion(hardwareVersion) + , m_subDevice(subDevice) { memset(m_productCode, 0, sizeof(m_productCode)); if (productCode) - strcpy(m_productCode, productCode); + { + XSENS_MSC_WARNING_SUPPRESS(4996) + std::strncpy(m_productCode, productCode, XSDEVICEID_PRODUCT_CODE_LEN); + } } /*! \brief Constructor that creates an XsDeviceId from the supplied \a deviceId */ inline XsDeviceId(uint64_t serialNumber = 0) : m_deviceId(serialNumber) - , m_hardwareVersion(0) , m_productVariant(0) + , m_hardwareVersion(0) + , m_subDevice(0) { memset(m_productCode, 0, sizeof(m_productCode)); } + /*! \brief Constructor that creates a sub-device XsDeviceId from the supplied \a parent ID and \a subDevice */ + inline explicit XsDeviceId(XsDeviceId const& parent, uint16_t subDevice) + { + memcpy(static_cast(this), static_cast(&parent), sizeof(XsDeviceId)); + m_subDevice = subDevice; + } + /*! \brief Constructor that creates an XsDeviceId from the supplied XsDeviceId \a other */ inline XsDeviceId(const XsDeviceId& other) { @@ -253,6 +268,14 @@ struct XsDeviceId { return m_hardwareVersion; } + /*! \brief Returns the sub device value + \details This value can be ignored for most device types, except for combined products such as the Xsens Glove + \return The sub device index: 0 = the whole device, 1+ = a sub-device + */ + inline uint16_t subDevice() const + { + return m_subDevice; + } /*! \brief Returns the 32-bit device serial number, which may be 0 if the device has a 64-bit serial number */ inline uint32_t legacyDeviceId() const { @@ -299,10 +322,20 @@ struct XsDeviceId return 0 != XsDeviceId_isMti8X0(this); } /*! \brief \copybrief XsDeviceId_isGlove(const struct XsDeviceId*) */ - inline bool isGlove () const + inline bool isGlove() const { return 0 != XsDeviceId_isGlove(this); } + /*! \brief \copybrief XsDeviceId_side(const struct XsDeviceId*) */ + inline XsHandId side() const + { + return XsDeviceId_side(this); + } + /*! \brief \copybrief XsDeviceId_isDot(const struct XsDeviceId*) */ + inline bool isDot() const + { + return 0 != XsDeviceId_isDot(this); + } /*! \brief \copybrief XsDeviceId_isRugged(const struct XsDeviceId*) */ inline bool isRugged() const { @@ -548,18 +581,22 @@ struct XsDeviceId inline bool operator==(const XsDeviceId& other) const { if (isLegacyDeviceId() || other.isLegacyDeviceId()) - return toInt() == other.toInt(); + return toInt() == other.toInt(); // there are no legacy devices with a sub-device else { return (toInt() == other.toInt() && m_productVariant == other.m_productVariant && m_hardwareVersion == other.m_hardwareVersion && + m_subDevice == other.m_subDevice && (strcmp(m_productCode, other.m_productCode) == 0) - ); + ); } } /*! \brief Returns true if the \a other deviceId does not match this deviceId */ - inline bool operator!=(const XsDeviceId& other) const { return !(*this == other); } + inline bool operator!=(const XsDeviceId& other) const + { + return !(*this == other); + } /*! \brief Returns true if this deviceId is less than the \a other deviceId */ inline bool operator<(const XsDeviceId& other) const { @@ -591,7 +628,7 @@ struct XsDeviceId else if (toInt() > other.toInt()) return false; else - return false; + return m_subDevice < other.m_subDevice; } } } @@ -617,10 +654,12 @@ struct XsDeviceId return false; else { - if (toInt() > other.toInt()) + if (toInt() < other.toInt()) + return true; + else if (toInt() > other.toInt()) return false; else - return true; + return m_subDevice <= other.m_subDevice; } } } @@ -657,7 +696,7 @@ struct XsDeviceId else if (toInt() < other.toInt()) return false; else - return false; + return m_subDevice > other.m_subDevice; } } } @@ -683,10 +722,12 @@ struct XsDeviceId return false; else { - if (toInt() < other.toInt()) + if (toInt() > other.toInt()) + return true; + else if (toInt() < other.toInt()) return false; else - return true; + return m_subDevice >= other.m_subDevice; } } } @@ -694,176 +735,178 @@ struct XsDeviceId } -//============================================================================================================ -//============================================================================================================ -//==== Deprecated methods follow ===== -//============================================================================================================ -//============================================================================================================ + //============================================================================================================ + //============================================================================================================ + //==== Deprecated methods follow ===== + //============================================================================================================ + //============================================================================================================ + XSDEPRECATED_START /*! \brief \copybrief XsDeviceId_isMtMk4(const struct XsDeviceId*) */ - inline bool isMtMk4() const + XSDEPRECATED inline bool isMtMk4() const { return 0 != XsDeviceId_isMtMk4(this); } /*! \brief \copybrief XsDeviceId_isMtMk4_X(const struct XsDeviceId*) */ - inline bool isMtMk4_X() const + XSDEPRECATED inline bool isMtMk4_X() const { return 0 != XsDeviceId_isMtMk4_X(this); } /*! \brief \copybrief XsDeviceId_isMtMk4_1(const struct XsDeviceId*) */ - inline bool isMtMk4_1() const + XSDEPRECATED inline bool isMtMk4_1() const { return 0 != XsDeviceId_isMtMk4_1(this); } /*! \brief \copybrief XsDeviceId_isMtMk4_2(const struct XsDeviceId*) */ - inline bool isMtMk4_2() const + XSDEPRECATED inline bool isMtMk4_2() const { return 0 != XsDeviceId_isMtMk4_2(this); } /*! \brief \copybrief XsDeviceId_isMtMk4_3(const struct XsDeviceId*) */ - inline bool isMtMk4_3() const + XSDEPRECATED inline bool isMtMk4_3() const { return 0 != XsDeviceId_isMtMk4_3(this); } /*! \brief \copybrief XsDeviceId_isMtMk4_7(const struct XsDeviceId*) */ - inline bool isMtMk4_7() const + XSDEPRECATED inline bool isMtMk4_7() const { return 0 != XsDeviceId_isMtMk4_7(this); } /*! \brief \copybrief XsDeviceId_isMtMk4_X0(const struct XsDeviceId*) */ - inline bool isMtMk4_X0() const + XSDEPRECATED inline bool isMtMk4_X0() const { return 0 != XsDeviceId_isMtMk4_X0(this); } /*! \brief \copybrief XsDeviceId_isMtMk4_10(const struct XsDeviceId*) */ - inline bool isMtMk4_10() const + XSDEPRECATED inline bool isMtMk4_10() const { return 0 != XsDeviceId_isMtMk4_10(this); } /*! \brief \copybrief XsDeviceId_isMtMk4_20(const struct XsDeviceId*) */ - inline bool isMtMk4_20() const + XSDEPRECATED inline bool isMtMk4_20() const { return 0 != XsDeviceId_isMtMk4_20(this); } /*! \brief \copybrief XsDeviceId_isMtMk4_30(const struct XsDeviceId*) */ - inline bool isMtMk4_30() const + XSDEPRECATED inline bool isMtMk4_30() const { return 0 != XsDeviceId_isMtMk4_30(this); } /*! \brief \copybrief XsDeviceId_isMtMk4_X00(const struct XsDeviceId*) */ - inline bool isMtMk4_X00() const + XSDEPRECATED inline bool isMtMk4_X00() const { return 0 != XsDeviceId_isMtMk4_X00(this); } /*! \brief \copybrief XsDeviceId_isMtMk4_100(const struct XsDeviceId*) */ - inline bool isMtMk4_100() const + XSDEPRECATED inline bool isMtMk4_100() const { return 0 != XsDeviceId_isMtMk4_100(this); } /*! \brief \copybrief XsDeviceId_isMtMk4_200(const struct XsDeviceId*) */ - inline bool isMtMk4_200() const + XSDEPRECATED inline bool isMtMk4_200() const { return 0 != XsDeviceId_isMtMk4_200(this); } /*! \brief \copybrief XsDeviceId_isMtMk4_300(const struct XsDeviceId*) */ - inline bool isMtMk4_300() const + XSDEPRECATED inline bool isMtMk4_300() const { return 0 != XsDeviceId_isMtMk4_300(this); } /*! \brief \copybrief XsDeviceId_isMtMk4_400(const struct XsDeviceId*) */ - inline bool isMtMk4_400() const + XSDEPRECATED inline bool isMtMk4_400() const { return 0 != XsDeviceId_isMtMk4_400(this); } /*! \brief \copybrief XsDeviceId_isMtMk4_500(const struct XsDeviceId*) */ - inline bool isMtMk4_500() const + XSDEPRECATED inline bool isMtMk4_500() const { return 0 != XsDeviceId_isMtMk4_500(this); } /*! \brief \copybrief XsDeviceId_isMtMk4_600(const struct XsDeviceId*) */ - inline bool isMtMk4_600() const + XSDEPRECATED inline bool isMtMk4_600() const { return 0 != XsDeviceId_isMtMk4_600(this); } /*! \brief \copybrief XsDeviceId_isMtMk4_700(const struct XsDeviceId*) */ - inline bool isMtMk4_700() const + XSDEPRECATED inline bool isMtMk4_700() const { return 0 != XsDeviceId_isMtMk4_700(this); } /*! \brief \copybrief XsDeviceId_isMtMk4_710(const struct XsDeviceId*) */ - inline bool isMtMk4_710() const + XSDEPRECATED inline bool isMtMk4_710() const { return 0 != XsDeviceId_isMtMk4_710(this); } /*! \brief \copybrief XsDeviceId_isMtMk4_800(const struct XsDeviceId*) */ - inline bool isMtMk4_800() const + XSDEPRECATED inline bool isMtMk4_800() const { return 0 != XsDeviceId_isMtMk4_800(this); } /*! \brief \copybrief XsDeviceId_isMtMk4_900(const struct XsDeviceId*) */ - inline bool isMtMk4_900() const + XSDEPRECATED inline bool isMtMk4_900() const { return 0 != XsDeviceId_isMtMk4_900(this); } /*! \brief \copybrief XsDeviceId_isMtMk5(const struct XsDeviceId*) */ - inline bool isMtMk5() const + XSDEPRECATED inline bool isMtMk5() const { return 0 != XsDeviceId_isMtMk5(this); } /*! \brief \copybrief XsDeviceId_isMtMk5_X0(const struct XsDeviceId*) */ - inline bool isMtMk5_X0() const + XSDEPRECATED inline bool isMtMk5_X0() const { return 0 != XsDeviceId_isMtMk5_X0(this); } /*! \brief \copybrief XsDeviceId_isMtMk5_10(const struct XsDeviceId*) */ - inline bool isMtMk5_10() const + XSDEPRECATED inline bool isMtMk5_10() const { return 0 != XsDeviceId_isMtMk5_10(this); } /*! \brief \copybrief XsDeviceId_isMtMk5_20(const struct XsDeviceId*) */ - inline bool isMtMk5_20() const + XSDEPRECATED inline bool isMtMk5_20() const { return 0 != XsDeviceId_isMtMk5_20(this); } /*! \brief \copybrief XsDeviceId_isMtMk5_30(const struct XsDeviceId*) */ - inline bool isMtMk5_30() const + XSDEPRECATED inline bool isMtMk5_30() const { return 0 != XsDeviceId_isMtMk5_30(this); } /*! \brief \copybrief XsDeviceId_isMtMk5_X00(const struct XsDeviceId*) */ - inline bool isMtMk5_X00() const + XSDEPRECATED inline bool isMtMk5_X00() const { return 0 != XsDeviceId_isMtMk5_X00(this); } /*! \brief \copybrief XsDeviceId_isMtMk5_100(const struct XsDeviceId*) */ - inline bool isMtMk5_100() const + XSDEPRECATED inline bool isMtMk5_100() const { return 0 != XsDeviceId_isMtMk5_100(this); } /*! \brief \copybrief XsDeviceId_isMtMk5_200(const struct XsDeviceId*) */ - inline bool isMtMk5_200() const + XSDEPRECATED inline bool isMtMk5_200() const { return 0 != XsDeviceId_isMtMk5_200(this); } /*! \brief \copybrief XsDeviceId_isMtMk5_300(const struct XsDeviceId*) */ - inline bool isMtMk5_300() const + XSDEPRECATED inline bool isMtMk5_300() const { return 0 != XsDeviceId_isMtMk5_300(this); } /*! \brief \copybrief XsDeviceId_isMtMk5_710(const struct XsDeviceId*) */ - inline bool isMtMk5_710() const + XSDEPRECATED inline bool isMtMk5_710() const { return 0 != XsDeviceId_isMtMk5_710(this); } -//============================================================================================================ + //============================================================================================================ private: #endif - uint64_t m_deviceId; //!< The serialnumber of a device - char m_productCode[24]; //!< The productcode of a device - uint16_t m_hardwareVersion; //!< The hardware version of a device + uint64_t m_deviceId; //!< The serialnumber of a device + char m_productCode[XSDEVICEID_PRODUCT_CODE_LEN]; //!< The productcode of a device uint32_t m_productVariant; //!< The product variant of a device + uint16_t m_hardwareVersion; //!< The hardware version of a device + uint16_t m_subDevice; //!< The index of a sub-device, ie a finger in an Xsens Glove. 1-based, 0 = the whole device (default) }; typedef struct XsDeviceId XsDeviceId; @@ -884,6 +927,30 @@ inline XsString& operator<<(XsString& o, XsDeviceId const& xd) return o; } +/*! \brief Use this function to strip the trailing spaces from an internal product code + \param pc The product code string to convert + \param maxSz The maximum number of characters to convert + \return A std::string representation of the supplied string, without trailing spaces +*/ +static std::string makeNiceProductCode(char const* pc, int maxSz = 20) +{ + if (maxSz <= 0 && pc) + maxSz = 20; + std::string result(pc ? pc : "", (unsigned int) maxSz); + std::string::size_type thingy = result.find(" "); + if (thingy != std::string::npos && (int) thingy < maxSz) + result.erase(result.begin() + (unsigned)thingy, result.end()); + return result; +} +/*! \brief Use this function to strip the trailing spaces from an internal product code + \param pc The product code string to convert + \param maxSz The maximum number of characters to convert + \return A std::string representation of the supplied string, without trailing spaces +*/ +inline static std::string makeNiceProductCode(uint8_t const* pc, int maxSz = 20) +{ + return makeNiceProductCode((char const*) pc, maxSz); +} #endif #endif diff --git a/lib/xspublic/xstypes/xsdeviceidarray.c b/lib/xspublic/xstypes/xsdeviceidarray.c index 0f5dfcc..721f942 100644 --- a/lib/xspublic/xstypes/xsdeviceidarray.c +++ b/lib/xspublic/xstypes/xsdeviceidarray.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -93,7 +93,8 @@ int compareDeviceIds(XsDeviceId const* a, XsDeviceId const* b) } //! \brief Descriptor for XsDeviceIdArray -XsArrayDescriptor const g_xsDeviceIdArrayDescriptor = { +XsArrayDescriptor const g_xsDeviceIdArrayDescriptor = +{ sizeof(XsDeviceId), XSEXPCASTITEMSWAP XsDeviceId_swap, // swap XSEXPCASTITEMMAKE initDeviceId, // construct diff --git a/lib/xspublic/xstypes/xsdeviceidarray.h b/lib/xspublic/xstypes/xsdeviceidarray.h index 05c8516..d82ab99 100644 --- a/lib/xspublic/xstypes/xsdeviceidarray.h +++ b/lib/xspublic/xstypes/xsdeviceidarray.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -85,16 +85,17 @@ XSTYPES_DLL_API void XsDeviceIdArray_construct(XsDeviceIdArray* thisPtr, XsSize #ifdef __cplusplus } // extern "C" -struct XsDeviceIdArray : public XsArrayImpl { +struct XsDeviceIdArray : public XsArrayImpl +{ //! \brief Constructs an XsDeviceIdArray inline explicit XsDeviceIdArray(XsSize sz = 0, XsDeviceId const* src = 0) - : ArrayImpl(sz, src) + : ArrayImpl(sz, src) { } //! \brief Constructs an XsDeviceIdArray as a copy of \a other inline XsDeviceIdArray(XsDeviceIdArray const& other) - : ArrayImpl(other) + : ArrayImpl(other) { } diff --git a/lib/xspublic/xstypes/xsdeviceoptionflag.h b/lib/xspublic/xstypes/xsdeviceoptionflag.h index d50ffa3..d577fc1 100644 --- a/lib/xspublic/xstypes/xsdeviceoptionflag.h +++ b/lib/xspublic/xstypes/xsdeviceoptionflag.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -66,7 +66,7 @@ #define XSDEVICEOPTIONFLAG_H /*! \addtogroup enums Global enumerations -@{ + @{ */ /*! \brief Used to enable or disable some device options \sa XsDevice::setDeviceOptionFlags @@ -98,25 +98,25 @@ typedef enum XsDeviceOptionFlag XsDeviceOptionFlag; //! \brief Logical OR operator for XsDeviceOptionFlag values inline XsDeviceOptionFlag operator | (XsDeviceOptionFlag a, XsDeviceOptionFlag b) { - return (XsDeviceOptionFlag) ((int)a | (int)b); + return (XsDeviceOptionFlag)((int)a | (int)b); } //! \brief Logical AND operator for XsDeviceOptionFlag values inline XsDeviceOptionFlag operator & (XsDeviceOptionFlag a, XsDeviceOptionFlag b) { - return (XsDeviceOptionFlag) ((int)a & (int)b); + return (XsDeviceOptionFlag)((int)a & (int)b); } //! \brief Logical XOR operator for XsDeviceOptionFlag values inline XsDeviceOptionFlag operator ^ (XsDeviceOptionFlag a, XsDeviceOptionFlag b) { - return (XsDeviceOptionFlag) ((int)a ^ (int)b); + return (XsDeviceOptionFlag)((int)a ^ (int)b); } //! \brief Logical NEG operator for XsDeviceOptionFlag values -inline XsDeviceOptionFlag operator ~ (XsDeviceOptionFlag a) +inline XsDeviceOptionFlag operator ~(XsDeviceOptionFlag a) { - return (XsDeviceOptionFlag) (~(int)a); + return (XsDeviceOptionFlag)(~(int)a); } #endif diff --git a/lib/xspublic/xstypes/xsdid.h b/lib/xspublic/xstypes/xsdid.h index 8aff36c..57070db 100644 --- a/lib/xspublic/xstypes/xsdid.h +++ b/lib/xspublic/xstypes/xsdid.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -95,6 +95,7 @@ #define XS_DID_TYPEL_OEM 0x00020000 #define XS_DID_TYPEL_SYNCSTATION 0x00030000 #define XS_DID_TYPEL_BUS_MASTER 0x00040000 +#define XS_DID_TYPEL_DONGLE_ANT 0x00010000 //Note: this is the same range as XS_DID_TYPEL_DONGLE #define XS_DID_TYPEL_RS232 0x00000000 #define XS_DID_TYPEL_RS422 0x00010000 @@ -133,6 +134,7 @@ #define XS_DID_TYPE_AWINDA XS_DID_TYPEH_AWINDAMASTER #define XS_DID_TYPE_AWINDA_STATION (XS_DID_TYPEH_AWINDAMASTER | XS_DID_TYPEL_STATION) #define XS_DID_TYPE_AWINDA_DONGLE (XS_DID_TYPEH_AWINDAMASTER | XS_DID_TYPEL_DONGLE) +#define XS_DID_TYPE_AWINDA_DONGLE_ANT (XS_DID_TYPEH_AWINDAMASTER | XS_DID_TYPEL_DONGLE_ANT) #define XS_DID_TYPE_AWINDA_OEM (XS_DID_TYPEH_AWINDAMASTER | XS_DID_TYPEL_OEM) #define XS_DID_TYPE_SYNCSTATION (XS_DID_TYPEH_AWINDAMASTER | XS_DID_TYPEL_SYNCSTATION) @@ -185,7 +187,7 @@ #define XS_DID_GLOVETYPE_LEFT 0x00000001 #define XS_DID_GLOVETYPE_RIGHT 0x00000002 -#define XS_DID_GLOVEMASTER_UNKNOWN ( XS_DID_GLOVEMASTER | XS_DID_GLOVETYPE_UNKNOWN ) +#define XS_DID_GLOVEMASTER_UNKNOWN ( XS_DID_GLOVEMASTER | XS_DID_GLOVETYPE_UNKNOWN ) #define XS_DID_GLOVEMASTER_LEFT ( XS_DID_GLOVEMASTER | XS_DID_GLOVETYPE_LEFT ) #define XS_DID_GLOVEMASTER_RIGHT ( XS_DID_GLOVEMASTER | XS_DID_GLOVETYPE_RIGHT ) diff --git a/lib/xspublic/xstypes/xsens_compat.h b/lib/xspublic/xstypes/xsens_compat.h index 7a81f11..bb35bd2 100644 --- a/lib/xspublic/xstypes/xsens_compat.h +++ b/lib/xspublic/xstypes/xsens_compat.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xstypes/xseuler.c b/lib/xspublic/xstypes/xseuler.c index df2e01f..8887235 100644 --- a/lib/xspublic/xstypes/xseuler.c +++ b/lib/xspublic/xstypes/xseuler.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -114,9 +114,9 @@ void XsEuler_fromQuaternion(XsEuler* thisPtr, const XsQuaternion* quat) dphi = XsMath_two * (sqw + quat->m_z * quat->m_z) - XsMath_one; dpsi = XsMath_two * (sqw + quat->m_x * quat->m_x) - XsMath_one; - thisPtr->m_x = XsMath_rad2deg(atan2(XsMath_two*(quat->m_y*quat->m_z + quat->m_w*quat->m_x), dphi)); - thisPtr->m_y = -XsMath_rad2deg(XsMath_asinClamped(XsMath_two*(quat->m_x*quat->m_z - quat->m_w*quat->m_y))); - thisPtr->m_z = XsMath_rad2deg(atan2(XsMath_two*(quat->m_x*quat->m_y + quat->m_w*quat->m_z), dpsi)); + thisPtr->m_x = XsMath_rad2deg(atan2(XsMath_two * (quat->m_y * quat->m_z + quat->m_w * quat->m_x), dphi)); + thisPtr->m_y = -XsMath_rad2deg(XsMath_asinClamped(XsMath_two * (quat->m_x * quat->m_z - quat->m_w * quat->m_y))); + thisPtr->m_z = XsMath_rad2deg(atan2(XsMath_two * (quat->m_x * quat->m_y + quat->m_w * quat->m_z), dpsi)); } /*! @} */ diff --git a/lib/xspublic/xstypes/xseuler.h b/lib/xspublic/xstypes/xseuler.h index f941885..a0f844e 100644 --- a/lib/xspublic/xstypes/xseuler.h +++ b/lib/xspublic/xstypes/xseuler.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -89,7 +89,8 @@ XSTYPES_DLL_API void XsEuler_fromQuaternion(struct XsEuler* thisPtr, const struc } // extern "C" #endif -struct XsEuler { +struct XsEuler +{ #ifdef __cplusplus //! \brief Constructor that creates an Euler object with all angles 0 inline XsEuler() : m_x(XsMath_zero), m_y(XsMath_zero), m_z(XsMath_zero) {} @@ -118,14 +119,14 @@ struct XsEuler { //! \brief Returns the \a index'th euler angle in the object inline XsReal operator[](XsSize index) const { - assert (index <= 2); + assert(index <= 2); return m_data[index]; } //! \brief Returns a reference to the \a index'th euler angle in the object - inline XsReal &operator[](XsSize index) + inline XsReal& operator[](XsSize index) { - assert (index <= 2); + assert(index <= 2); return m_data[index]; } @@ -163,37 +164,58 @@ struct XsEuler { } //! \brief Returns the roll or x value - inline XsReal roll() const { return m_roll; } + inline XsReal roll() const + { + return m_roll; + } //! \brief Returns the pitch or y value - inline XsReal pitch() const { return m_pitch; } + inline XsReal pitch() const + { + return m_pitch; + } //! \brief Returns the yaw or z value - inline XsReal yaw() const { return m_yaw; } + inline XsReal yaw() const + { + return m_yaw; + } //! \brief Returns the x or roll value - inline XsReal x() const { return m_x; } + inline XsReal x() const + { + return m_x; + } //! \brief Returns the y or pitch value - inline XsReal y() const { return m_y; } + inline XsReal y() const + { + return m_y; + } //! \brief Returns the z or yaw value - inline XsReal z() const { return m_z; } + inline XsReal z() const + { + return m_z; + } //! \brief Returns true if the values of this and \a other are within \a tolerance of each other inline bool isEqual(const XsEuler& other, XsReal tolerance) const { return fabs(m_x - other.m_x) <= tolerance && - fabs(m_y - other.m_y) <= tolerance && - fabs(m_z - other.m_z) <= tolerance; + fabs(m_y - other.m_y) <= tolerance && + fabs(m_z - other.m_z) <= tolerance; } private: #endif - union { - struct { + union + { + struct + { XsReal m_x; //!< Stores the x component of the euler triplet XsReal m_y; //!< Stores the y component of the euler triplet XsReal m_z; //!< Stores the z component of the euler triplet }; - struct { + struct + { XsReal m_roll; //!< Stores the roll component of the euler triplet XsReal m_pitch; //!< Stores the pitch component of the euler triplet XsReal m_yaw; //!< Stores the yaw component of the euler triplet diff --git a/lib/xspublic/xstypes/xsexception.h b/lib/xspublic/xstypes/xsexception.h index f812298..c1e673c 100644 --- a/lib/xspublic/xstypes/xsexception.h +++ b/lib/xspublic/xstypes/xsexception.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -74,7 +74,8 @@ /*! \brief Exception class for Xsens public libraries. Inherits from std::exception */ -class XsException : public std::exception { +class XsException : public std::exception +{ public: //! \brief Copy constructor XsException(XsException const& e) @@ -90,26 +91,12 @@ class XsException : public std::exception { \param err The error code that the exception should report \param description A description of the error. The constructor prefixes this with a textual description of the error code unless prefix is false. - \param prefix Whether to prefix the description with a textual description of the error code or not (default is yes) */ - XsException(XsResultValue err, XsString const& description, bool prefix = true) + XsException(XsResultValue err, XsString const& description) : std::exception() , m_code(err) , m_description(description) { - if (prefix && (m_code != XRV_OK)) - { - char codeString[16]; - sprintf(codeString, "%d: ", (int) m_code); - XsString rv(codeString); - rv << XsResultValue_toString(m_code); - if (!m_description.empty()) - { - rv << ". "; - rv.append(m_description); - } - m_description.swap(rv); - } } /*! \brief Initializing constructor @@ -161,7 +148,19 @@ class XsException : public std::exception { XsString m_description; //!< The supplied description, possibly prefixed with a description of the error code }; +#include +inline static std::ostream& operator<< (std::ostream& os, const XsException& ex) +{ + if (ex.code() == XRV_OK) + os << "XRV_OK"; + else + os << "XRV " << ex.code(); + if (ex.code() != XRV_OK && !ex.text().empty()) + os << ": " << ex.text(); + return os; +} + #endif #endif -#endif // __cplusplus guard +#endif diff --git a/lib/xspublic/xstypes/xsfile.c b/lib/xspublic/xstypes/xsfile.c index 8ce686e..b98b95a 100644 --- a/lib/xspublic/xstypes/xsfile.c +++ b/lib/xspublic/xstypes/xsfile.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -66,18 +66,18 @@ #include "xsstring.h" #include #ifndef _WIN32 -# include // close -# include // ioctl -# include // open, O_RDWR -# include // strcpy -# include -# include -# include -# include + #include // close + #include // ioctl + #include // open, O_RDWR + #include // strcpy + #include + #include + #include + #include #else -# include -# include -# include + #include + #include + #include #endif // helper @@ -97,9 +97,7 @@ static FILE* openFile(const struct XsString* filename, const struct XsString* mo void XsFile_destruct(struct XsFile* thisPtr) { if (thisPtr->m_handle != NULL) - { (void)XsFile_close(thisPtr); - } } /*! \relates XsFile @@ -108,7 +106,7 @@ void XsFile_destruct(struct XsFile* thisPtr) \param writeOnly passing 0 will create the file using "w+b", any other value will create it using "wb" \returns XRV_OK if the file was opened, an error otherwise */ -XsResultValue XsFile_create(struct XsFile *thisPtr, const struct XsString* filename, int writeOnly) +XsResultValue XsFile_create(struct XsFile* thisPtr, const struct XsString* filename, int writeOnly) { XsString mode; XsString_construct(&mode); @@ -150,7 +148,7 @@ XsResultValue XsFile_create(struct XsFile *thisPtr, const struct XsString* filen \param writeOnly passing 0 will create the file using "w+t", any other value will create it using "wt" \returns XRV_OK if the file was opened, an error otherwise */ -XsResultValue XsFile_createText(struct XsFile *thisPtr, const struct XsString* filename, int writeOnly) +XsResultValue XsFile_createText(struct XsFile* thisPtr, const struct XsString* filename, int writeOnly) { XsString mode; XsString_construct(&mode); @@ -183,7 +181,7 @@ XsResultValue XsFile_createText(struct XsFile *thisPtr, const struct XsString* f \param readOnly passing 0 will open the file "r+b" (read/update), any other value will create it using "rb" (read) \returns XRV_OK if the file was opened, an error otherwise */ -XsResultValue XsFile_open(struct XsFile *thisPtr, const struct XsString* filename, int readOnly) +XsResultValue XsFile_open(struct XsFile* thisPtr, const struct XsString* filename, int readOnly) { XsString mode; XsString_construct(&mode); @@ -200,7 +198,12 @@ XsResultValue XsFile_open(struct XsFile *thisPtr, const struct XsString* filenam XsString_destruct(&mode); if (thisPtr->m_handle == NULL) - return XRV_OUTPUTCANNOTBEOPENED; + { + if (readOnly) + return XRV_INPUTCANNOTBEOPENED; + else + return XRV_OUTPUTCANNOTBEOPENED; + } else return XRV_OK; } @@ -211,7 +214,7 @@ XsResultValue XsFile_open(struct XsFile *thisPtr, const struct XsString* filenam \param readOnly passing 0 will open the file "r+t" (read/update), any other value will create it using "rt" (read) \returns XRV_OK if the file was opened, an error otherwise */ -XsResultValue XsFile_openText(struct XsFile *thisPtr, const struct XsString* filename, int readOnly) +XsResultValue XsFile_openText(struct XsFile* thisPtr, const struct XsString* filename, int readOnly) { XsString mode; XsString_construct(&mode); @@ -251,7 +254,7 @@ static FILE* openFile(const struct XsString* filename, const struct XsString* mo { return _wfopen(filenameW, modeW); } - __except(EXCEPTION_EXECUTE_HANDLER) + __except (EXCEPTION_EXECUTE_HANDLER) { return NULL; } @@ -266,7 +269,7 @@ static FILE* openFile(const struct XsString* filename, const struct XsString* mo \param mode Mode to reopen the file with \returns 0 if a file is open, another value otherwise */ -XsResultValue XsFile_reopen(struct XsFile *thisPtr, const struct XsString* filename, const struct XsString* mode) +XsResultValue XsFile_reopen(struct XsFile* thisPtr, const struct XsString* filename, const struct XsString* mode) { #ifdef _WIN32 wchar_t filenameW[XS_MAX_FILENAME_LENGTH]; @@ -290,7 +293,7 @@ XsResultValue XsFile_reopen(struct XsFile *thisPtr, const struct XsString* filen \brief Checks if a file is open \returns 0 if a file is open, another value otherwise */ -int XsFile_isOpen(const struct XsFile *thisPtr) +int XsFile_isOpen(const struct XsFile* thisPtr) { return (thisPtr->m_handle != NULL) ? 0 : 1; } @@ -317,7 +320,7 @@ int XsFile_exists(const struct XsString* filename) \brief Closes the file \returns XRV_OK if the file was closed properly, XRV_ENDOFFILE otherwise */ -XsResultValue XsFile_close(struct XsFile *thisPtr) +XsResultValue XsFile_close(struct XsFile* thisPtr) { int rv; @@ -342,7 +345,7 @@ XsResultValue XsFile_close(struct XsFile *thisPtr) \returns XRV_OK if the flushing was successful, an XRV_ERROR otherwise */ -XsResultValue XsFile_flush(struct XsFile *thisPtr) +XsResultValue XsFile_flush(struct XsFile* thisPtr) { return fflush(thisPtr->m_handle) ? XRV_ERROR : XRV_OK; } @@ -352,7 +355,7 @@ XsResultValue XsFile_flush(struct XsFile *thisPtr) \param fileSize The new size for the file \returns XRV_OK if the file was truncated, an error otherwise */ -XsResultValue XsFile_truncate(struct XsFile *thisPtr, XsFilePos fileSize) +XsResultValue XsFile_truncate(struct XsFile* thisPtr, XsFilePos fileSize) { return XsFile_resize(thisPtr, fileSize); } @@ -362,7 +365,7 @@ XsResultValue XsFile_truncate(struct XsFile *thisPtr, XsFilePos fileSize) \param fileSize The new size for the file \returns XRV_OK if the file was resized, an error otherwise */ -XsResultValue XsFile_resize(struct XsFile *thisPtr, XsFilePos fileSize) +XsResultValue XsFile_resize(struct XsFile* thisPtr, XsFilePos fileSize) { #ifdef _WIN32 int rv = _chsize_s(_fileno(thisPtr->m_handle), fileSize); @@ -371,18 +374,18 @@ XsResultValue XsFile_resize(struct XsFile *thisPtr, XsFilePos fileSize) #endif if (rv != 0) { - switch(errno) + switch (errno) { - case EACCES: - return XRV_BUSY; - case EBADF: - return XRV_ACCESSDENIED; - case ENOSPC: - return XRV_OUTOFMEMORY; - case EINVAL: - return XRV_INVALIDPARAM; - default: - return XRV_ERROR; + case EACCES: + return XRV_BUSY; + case EBADF: + return XRV_ACCESSDENIED; + case ENOSPC: + return XRV_OUTOFMEMORY; + case EINVAL: + return XRV_INVALIDPARAM; + default: + return XRV_ERROR; } } else @@ -406,12 +409,12 @@ XsResultValue XsFile_erase(const struct XsString* filename) { switch (errno) { - case EACCES: - return XRV_READONLY; - case ENOENT: - return XRV_NOTFOUND; - default: - return XRV_ERROR; + case EACCES: + return XRV_READONLY; + case ENOENT: + return XRV_NOTFOUND; + default: + return XRV_ERROR; } } else @@ -425,7 +428,7 @@ XsResultValue XsFile_erase(const struct XsString* filename) \param count Number of elements to read \returns Total number of elements successfully read */ -XsFilePos XsFile_read(struct XsFile *thisPtr, void *destination, XsFilePos size, XsFilePos count) +XsFilePos XsFile_read(struct XsFile* thisPtr, void* destination, XsFilePos size, XsFilePos count) { return (XsFilePos) fread(destination, (size_t) size, (size_t) count, thisPtr->m_handle); } @@ -437,7 +440,7 @@ XsFilePos XsFile_read(struct XsFile *thisPtr, void *destination, XsFilePos size, \param count Number of elements to write \returns Total number of elements successfully written */ -XsFilePos XsFile_write(struct XsFile *thisPtr, const void *source, XsFilePos size, XsFilePos count) +XsFilePos XsFile_write(struct XsFile* thisPtr, const void* source, XsFilePos size, XsFilePos count) { return (XsFilePos) fwrite(source, (size_t) size, (size_t) count, thisPtr->m_handle); } @@ -446,7 +449,7 @@ XsFilePos XsFile_write(struct XsFile *thisPtr, const void *source, XsFilePos siz \brief Gets and returns the next byte from a file \returns The byte read from the stream or -1 in case of failure */ -int XsFile_getc(struct XsFile *thisPtr) +int XsFile_getc(struct XsFile* thisPtr) { return fgetc(thisPtr->m_handle); } @@ -456,7 +459,7 @@ int XsFile_getc(struct XsFile *thisPtr) \param character The caharacter to write \returns XRV_OK on success, XRV_ERROR otehrwise */ -XsResultValue XsFile_putc(struct XsFile *thisPtr, int character) +XsResultValue XsFile_putc(struct XsFile* thisPtr, int character) { return fputc(character, thisPtr->m_handle) == EOF ? XRV_ERROR : XRV_OK; } @@ -467,7 +470,7 @@ XsResultValue XsFile_putc(struct XsFile *thisPtr, int character) \param num The size of the destination buffer \returns A pointer to the read string on success, NULL on failure */ -char* XsFile_gets(struct XsFile *thisPtr, char *str, int num) +char* XsFile_gets(struct XsFile* thisPtr, char* str, int num) { return fgets(str, num, thisPtr->m_handle); } @@ -477,7 +480,7 @@ char* XsFile_gets(struct XsFile *thisPtr, char *str, int num) \param str The null terminated c-string to write \returns XRV_OK on success, XRV_ERROR otehrwise */ -XsResultValue XsFile_puts(struct XsFile *thisPtr, const char *str) +XsResultValue XsFile_puts(struct XsFile* thisPtr, const char* str) { return fputs(str, thisPtr->m_handle) != EOF ? XRV_OK : XRV_ERROR; } @@ -487,7 +490,7 @@ XsResultValue XsFile_puts(struct XsFile *thisPtr, const char *str) \param offset Position in the file to move to, relative to the start of the file \returns XRV_OK if the seek was successful */ -XsResultValue XsFile_seek(struct XsFile *thisPtr, XsFilePos offset) +XsResultValue XsFile_seek(struct XsFile* thisPtr, XsFilePos offset) { #ifdef _WIN32 return _fseeki64(thisPtr->m_handle, offset, SEEK_SET) ? XRV_ERROR : XRV_OK; @@ -501,7 +504,7 @@ XsResultValue XsFile_seek(struct XsFile *thisPtr, XsFilePos offset) \param offset Position in the file to move to, relative to the end of the file \returns XRV_OK if the seek was successful */ -XsResultValue XsFile_seek_r(struct XsFile *thisPtr, XsFilePos offset) +XsResultValue XsFile_seek_r(struct XsFile* thisPtr, XsFilePos offset) { #ifdef _WIN32 return _fseeki64(thisPtr->m_handle, offset, SEEK_END) ? XRV_ERROR : XRV_OK; @@ -549,9 +552,7 @@ XsResultValue XsFile_fullPath(const struct XsString* filename, struct XsString* { XsResultValue result = XRV_OK; if (fullPath == NULL) - { result = XRV_NULLPTR; - } else { #ifdef _WIN32 @@ -567,7 +568,7 @@ XsResultValue XsFile_fullPath(const struct XsString* filename, struct XsString* // based on the assumption that this doesn't concern the serial port, handle // it the same way using realpath(). Apparently realpath() doesn't require a // maximum length. One would possibly want to write a wrapper for it. - char fullpath[XS_MAX_FILENAME_LENGTH*2]; + char fullpath[XS_MAX_FILENAME_LENGTH * 2]; if (realpath(filename->m_data, fullpath) == NULL) result = XRV_ERROR; else @@ -582,7 +583,7 @@ XsResultValue XsFile_fullPath(const struct XsString* filename, struct XsString* \param line The result of the read \returns XRV_OK if a line could be read */ -XsResultValue XsFile_getline(struct XsFile *thisPtr, struct XsString *line) +XsResultValue XsFile_getline(struct XsFile* thisPtr, struct XsString* line) { int b; XsResultValue ok = XRV_ENDOFFILE; @@ -607,7 +608,7 @@ XsResultValue XsFile_getline(struct XsFile *thisPtr, struct XsString *line) /*! \relates XsFile \returns The file handle */ -FILE* XsFile_handle(struct XsFile *thisPtr) +FILE* XsFile_handle(struct XsFile* thisPtr) { return thisPtr->m_handle; } diff --git a/lib/xspublic/xstypes/xsfile.h b/lib/xspublic/xstypes/xsfile.h index 25be69a..38b1649 100644 --- a/lib/xspublic/xstypes/xsfile.h +++ b/lib/xspublic/xstypes/xsfile.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -73,12 +73,13 @@ #include "xsstring.h" #ifdef _MSC_VER -# define XS_MAX_FILENAME_LENGTH 512 + #define XS_MAX_FILENAME_LENGTH 512 #else -# define XS_MAX_FILENAME_LENGTH PATH_MAX + #define XS_MAX_FILENAME_LENGTH PATH_MAX #endif #ifdef __cplusplus +#include "xsbytearray.h" extern "C" { #endif #ifndef __cplusplus @@ -89,27 +90,27 @@ struct XsFile; XSTYPES_DLL_API void XsFile_destruct(struct XsFile* thisPtr); -XSTYPES_DLL_API XsResultValue XsFile_create(struct XsFile *thisPtr, const struct XsString* filename, int writeOnly); -XSTYPES_DLL_API XsResultValue XsFile_createText(struct XsFile *thisPtr, const struct XsString* filename, int writeOnly); -XSTYPES_DLL_API XsResultValue XsFile_open(struct XsFile *thisPtr, const struct XsString* filename, int readOnly); -XSTYPES_DLL_API XsResultValue XsFile_openText(struct XsFile *thisPtr, const struct XsString* filename, int readOnly); -XSTYPES_DLL_API XsResultValue XsFile_reopen(struct XsFile *thisPtr, const struct XsString* filename, const struct XsString* mode); -XSTYPES_DLL_API int XsFile_isOpen(const struct XsFile *thisPtr); -XSTYPES_DLL_API XsResultValue XsFile_close(struct XsFile *thisPtr); +XSTYPES_DLL_API XsResultValue XsFile_create(struct XsFile* thisPtr, const struct XsString* filename, int writeOnly); +XSTYPES_DLL_API XsResultValue XsFile_createText(struct XsFile* thisPtr, const struct XsString* filename, int writeOnly); +XSTYPES_DLL_API XsResultValue XsFile_open(struct XsFile* thisPtr, const struct XsString* filename, int readOnly); +XSTYPES_DLL_API XsResultValue XsFile_openText(struct XsFile* thisPtr, const struct XsString* filename, int readOnly); +XSTYPES_DLL_API XsResultValue XsFile_reopen(struct XsFile* thisPtr, const struct XsString* filename, const struct XsString* mode); +XSTYPES_DLL_API int XsFile_isOpen(const struct XsFile* thisPtr); +XSTYPES_DLL_API XsResultValue XsFile_close(struct XsFile* thisPtr); XSTYPES_DLL_API int XsFile_exists(const struct XsString* filename); -XSTYPES_DLL_API XsResultValue XsFile_flush(struct XsFile *thisPtr); -XSTYPES_DLL_API XsResultValue XsFile_truncate(struct XsFile *thisPtr, XsFilePos fileSize); -XSTYPES_DLL_API XsResultValue XsFile_resize(struct XsFile *thisPtr, XsFilePos fileSize); +XSTYPES_DLL_API XsResultValue XsFile_flush(struct XsFile* thisPtr); +XSTYPES_DLL_API XsResultValue XsFile_truncate(struct XsFile* thisPtr, XsFilePos fileSize); +XSTYPES_DLL_API XsResultValue XsFile_resize(struct XsFile* thisPtr, XsFilePos fileSize); XSTYPES_DLL_API XsResultValue XsFile_erase(const struct XsString* filename); -XSTYPES_DLL_API XsFilePos XsFile_read(struct XsFile *thisPtr, void *destination, XsFilePos size, XsFilePos count); -XSTYPES_DLL_API XsFilePos XsFile_write(struct XsFile *thisPtr, const void *source, XsFilePos size, XsFilePos count); -XSTYPES_DLL_API int XsFile_getc(struct XsFile *thisPtr); -XSTYPES_DLL_API XsResultValue XsFile_putc(struct XsFile *thisPtr, int character); -XSTYPES_DLL_API char* XsFile_gets(struct XsFile *thisPtr, char *str, int num); -XSTYPES_DLL_API XsResultValue XsFile_puts(struct XsFile *thisPtr, const char *str); +XSTYPES_DLL_API XsFilePos XsFile_read(struct XsFile* thisPtr, void* destination, XsFilePos size, XsFilePos count); +XSTYPES_DLL_API XsFilePos XsFile_write(struct XsFile* thisPtr, const void* source, XsFilePos size, XsFilePos count); +XSTYPES_DLL_API int XsFile_getc(struct XsFile* thisPtr); +XSTYPES_DLL_API XsResultValue XsFile_putc(struct XsFile* thisPtr, int character); +XSTYPES_DLL_API char* XsFile_gets(struct XsFile* thisPtr, char* str, int num); +XSTYPES_DLL_API XsResultValue XsFile_puts(struct XsFile* thisPtr, const char* str); XSTYPES_DLL_API XsResultValue XsFile_seek(struct XsFile* thisPtr, XsFilePos offset); XSTYPES_DLL_API XsResultValue XsFile_seek_r(struct XsFile* thisPtr, XsFilePos offset); @@ -119,15 +120,16 @@ XSTYPES_DLL_API int XsFile_eof(struct XsFile const* thisPtr); XSTYPES_DLL_API XsResultValue XsFile_error(struct XsFile const* thisPtr); XSTYPES_DLL_API XsResultValue XsFile_fullPath(const struct XsString* filename, struct XsString* fullPath); -XSTYPES_DLL_API XsResultValue XsFile_getline(struct XsFile *thisPtr, struct XsString *line); +XSTYPES_DLL_API XsResultValue XsFile_getline(struct XsFile* thisPtr, struct XsString* line); -XSTYPES_DLL_API FILE* XsFile_handle(struct XsFile *thisPtr); +XSTYPES_DLL_API FILE* XsFile_handle(struct XsFile* thisPtr); #ifdef __cplusplus } // extern "C" #endif -struct XsFile { +struct XsFile +{ #ifdef __cplusplus /*! \brief Default constructor, creates an empty file object */ explicit inline XsFile() @@ -146,31 +148,31 @@ struct XsFile { } /*! \brief \copybrief XsFile_create */ - inline XsResultValue create(const XsString &filename, bool writeOnly) + inline XsResultValue create(const XsString& filename, bool writeOnly) { return XsFile_create(this, &filename, writeOnly ? 1 : 0); } /*! \brief \copybrief XsFile_createText */ - inline XsResultValue createText(const XsString &filename, bool writeOnly) + inline XsResultValue createText(const XsString& filename, bool writeOnly) { return XsFile_createText(this, &filename, writeOnly ? 1 : 0); } /*! \brief \copybrief XsFile_open */ - inline XsResultValue open(const XsString &fileName, bool readOnly) + inline XsResultValue open(const XsString& fileName, bool readOnly) { return XsFile_open(this, &fileName, readOnly ? 1 : 0); } /*! \brief \copybrief XsFile_openText */ - inline XsResultValue openText(const XsString &fileName, bool readOnly) + inline XsResultValue openText(const XsString& fileName, bool readOnly) { return XsFile_openText(this, &fileName, readOnly ? 1 : 0); } /*! \brief \copybrief XsFile_openText */ - inline XsResultValue reopen(const XsString &fileName, const XsString &mode) + inline XsResultValue reopen(const XsString& fileName, const XsString& mode) { return XsFile_reopen(this, &fileName, &mode); } @@ -182,7 +184,7 @@ struct XsFile { } /*! \brief \copybrief XsFile_exists */ - static inline bool exists(const XsString &fileName) + static inline bool exists(const XsString& fileName) { return (XsFile_exists(&fileName) == 0); } @@ -218,17 +220,23 @@ struct XsFile { } /*! \brief \copybrief XsFile_read */ - inline XsFilePos read(void *destination, XsFilePos size, XsFilePos count) + inline XsFilePos read(void* destination, XsFilePos size, XsFilePos count) { return XsFile_read(this, destination, size, count); } /*! \brief \copybrief XsFile_write */ - inline XsFilePos write(const void *source, XsFilePos size, XsFilePos count) + inline XsFilePos write(const void* source, XsFilePos size, XsFilePos count) { return XsFile_write(this, source, size, count); } + /*! \brief Write contents of \a source to the file as raw bytes */ + inline XsFilePos write(XsByteArray const& source) + { + return XsFile_write(this, source.data(), 1, source.size()); + } + /*! \brief \copybrief XsFile_getc */ inline int getc() { @@ -242,13 +250,13 @@ struct XsFile { } /*! \brief \copybrief XsFile_gets */ - inline char* gets(char *destination, int maxCount) + inline char* gets(char* destination, int maxCount) { return XsFile_gets(this, destination, maxCount); } /*! \brief \copybrief XsFile_puts */ - inline XsResultValue puts(const char *source) + inline XsResultValue puts(const char* source) { return XsFile_puts(this, source); } @@ -274,7 +282,7 @@ struct XsFile { /*! \brief \copybrief XsFile_eof */ inline bool eof() const { - return (0!=XsFile_eof(this)); + return (0 != XsFile_eof(this)); } /*! \brief \copybrief XsFile_error */ @@ -284,7 +292,7 @@ struct XsFile { } /*! \brief \copybrief XsFile_fullPath */ - static XsResultValue fullPath(const XsString &filename, XsString &fullPath) + static XsResultValue fullPath(const XsString& filename, XsString& fullPath) { return XsFile_fullPath(&filename, &fullPath); } diff --git a/lib/xspublic/xstypes/xsfilepos.h b/lib/xspublic/xstypes/xsfilepos.h index 26fef4a..97c862a 100644 --- a/lib/xspublic/xstypes/xsfilepos.h +++ b/lib/xspublic/xstypes/xsfilepos.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -83,25 +83,25 @@ #include #ifdef _WIN32 -#ifndef _PSTDINT_H_INCLUDED -# include "pstdint.h" -#endif -typedef __int64 XsFilePos; -#ifndef HANDLE -# include -#endif -typedef HANDLE XsIoHandle; + #ifndef _PSTDINT_H_INCLUDED + #include "pstdint.h" + #endif + typedef __int64 XsFilePos; + #ifndef HANDLE + #include + #endif + typedef HANDLE XsIoHandle; #else -#include -/* off_t is practically guaranteed not to be 64 bits on non64 bit systems. - We'd better explicitly use __off64_t to be sure of it's size. -*/ -#if defined(__off64_t_defined) -typedef __off64_t XsFilePos; -#else -typedef int64_t XsFilePos; -#endif -typedef int32_t XsIoHandle; + #include + /* off_t is practically guaranteed not to be 64 bits on non64 bit systems. + We'd better explicitly use __off64_t to be sure of it's size. + */ + #if defined(__off64_t_defined) + typedef __off64_t XsFilePos; + #else + typedef int64_t XsFilePos; + #endif + typedef int32_t XsIoHandle; #endif typedef FILE XsFileHandle; diff --git a/lib/xspublic/xstypes/xsfilterprofile.c b/lib/xspublic/xstypes/xsfilterprofile.c index 6e4d803..6e21456 100644 --- a/lib/xspublic/xstypes/xsfilterprofile.c +++ b/lib/xspublic/xstypes/xsfilterprofile.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -76,9 +76,9 @@ /*! \brief Converts filter profile version and type information to string \param out The string to write to */ -void XsFilterProfile_toString(XsFilterProfile const* thisPtr, XsString *out) +void XsFilterProfile_toString(XsFilterProfile const* thisPtr, XsString* out) { - char outData[3+1+3+1+1+2*XS_LEN_FILTERPROFILELABEL_TERM]; + char outData[3 + 1 + 3 + 1 + 1 + 2 * XS_LEN_FILTERPROFILELABEL_TERM]; sprintf(outData, "%d.%d %s", thisPtr->m_type, thisPtr->m_version, thisPtr->m_label); XsString_assignCharArray(out, outData); } diff --git a/lib/xspublic/xstypes/xsfilterprofile.h b/lib/xspublic/xstypes/xsfilterprofile.h index baa3ef8..e187843 100644 --- a/lib/xspublic/xstypes/xsfilterprofile.h +++ b/lib/xspublic/xstypes/xsfilterprofile.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -77,7 +77,7 @@ extern "C" { #endif struct XsFilterProfile; -XSTYPES_DLL_API void XsFilterProfile_toString(struct XsFilterProfile const* thisPtr, XsString *out); +XSTYPES_DLL_API void XsFilterProfile_toString(struct XsFilterProfile const* thisPtr, XsString* out); XSTYPES_DLL_API int XsFilterProfile_empty(struct XsFilterProfile const* thisPtr); XSTYPES_DLL_API void XsFilterProfile_swap(struct XsFilterProfile* a, struct XsFilterProfile* b); #ifdef __cplusplus @@ -150,25 +150,46 @@ struct XsFilterProfile } /*! \brief The filter profile type */ - inline uint8_t type() const { return m_type; } + inline uint8_t type() const + { + return m_type; + } /*! \brief The filter profile version */ - inline uint8_t version() const { return m_version; } + inline uint8_t version() const + { + return m_version; + } /*! \brief The filter profile name */ - inline const char* label() const { return m_label; } + inline const char* label() const + { + return m_label; + } /*! \brief The filter profile kind */ - inline const char* kind() const { return m_kind; } + inline const char* kind() const + { + return m_kind; + } /*! \brief The filter type this filter profile is for */ - inline char filterType() const { return m_filterType; } + inline char filterType() const + { + return m_filterType; + } /*! \brief The major version of the compatible filter */ - inline uint8_t filterMajor() const { return m_filterMajor; } + inline uint8_t filterMajor() const + { + return m_filterMajor; + } /*! \brief The minor version of the compatible filter */ - inline uint8_t filterMinor() const { return m_filterMinor; } + inline uint8_t filterMinor() const + { + return m_filterMinor; + } /*! \brief Set the type of the filter profile to \a type_ \param type_ the new type of the filter profile @@ -192,13 +213,11 @@ struct XsFilterProfile inline void setLabel(const char* label_) { if (!label_ || label_[0] == 0) - { m_label[0] = 0; - } else { int i = 0; - for (; i < 2 * (XS_LEN_FILTERPROFILELABEL_TERM-1); ++i) + for (; i < 2 * (XS_LEN_FILTERPROFILELABEL_TERM - 1); ++i) { if (label_[i] == '\0' || label_[i] == ' ') break; @@ -209,18 +228,16 @@ struct XsFilterProfile } /*! \brief Set the kind of filter profile \a kind_ - \param kind_ the new kind of filter profile + \param kind_ the new kind of filter profile */ inline void setKind(const char* kind_) { if (!kind_ || kind_[0] == 0) - { m_kind[0] = 0; - } else { int i = 0; - for (; i < XS_LEN_FILTERPROFILELABEL_TERM-1; ++i) + for (; i < XS_LEN_FILTERPROFILELABEL_TERM - 1; ++i) { if (kind_[i] == '\0' || kind_[i] == ' ') break; diff --git a/lib/xspublic/xstypes/xsfilterprofilearray.c b/lib/xspublic/xstypes/xsfilterprofilearray.c index 02b1375..88add5a 100644 --- a/lib/xspublic/xstypes/xsfilterprofilearray.c +++ b/lib/xspublic/xstypes/xsfilterprofilearray.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -95,15 +95,15 @@ void copyFilterProfile(XsFilterProfile* to, XsFilterProfile const* from) int compareFilterProfile(XsFilterProfile const* a, XsFilterProfile const* b) { if (a->m_filterType != b->m_filterType) - return (a->m_filterType < b->m_filterType)?-1:1; + return (a->m_filterType < b->m_filterType) ? -1 : 1; if (a->m_type != b->m_type) - return (a->m_type < b->m_type)?-1:1; + return (a->m_type < b->m_type) ? -1 : 1; if (a->m_filterMajor != b->m_filterMajor) - return (a->m_filterMajor < b->m_filterMajor)?-1:1; + return (a->m_filterMajor < b->m_filterMajor) ? -1 : 1; if (a->m_filterMinor != b->m_filterMinor) - return (a->m_filterMinor < b->m_filterMinor)?-1:1; + return (a->m_filterMinor < b->m_filterMinor) ? -1 : 1; if (a->m_version != b->m_version) - return (a->m_version < b->m_version)?-1:1; + return (a->m_version < b->m_version) ? -1 : 1; return 0; } @@ -114,7 +114,8 @@ void zeroFilterProfile(XsFilterProfile* a) } //! \brief Descriptor for XsFilterProfileArray -XsArrayDescriptor const g_xsFilterProfileArrayDescriptor = { +XsArrayDescriptor const g_xsFilterProfileArrayDescriptor = +{ sizeof(XsFilterProfile), XSEXPCASTITEMSWAP swapFilterProfile, // swap XSEXPCASTITEMMAKE zeroFilterProfile, // construct diff --git a/lib/xspublic/xstypes/xsfilterprofilearray.h b/lib/xspublic/xstypes/xsfilterprofilearray.h index 2e95306..a81c1a6 100644 --- a/lib/xspublic/xstypes/xsfilterprofilearray.h +++ b/lib/xspublic/xstypes/xsfilterprofilearray.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -89,41 +89,42 @@ XSTYPES_DLL_API void XsFilterProfileArray_construct(XsFilterProfileArray* thisPt #endif #ifdef __cplusplus -struct XsFilterProfileArray : public XsArrayImpl { - //! \brief Constructs an XsFilterProfileArray - inline explicit XsFilterProfileArray(XsSize sz = 0, XsFilterProfile const* src = 0) - : ArrayImpl(sz, src) + struct XsFilterProfileArray : public XsArrayImpl { - } - - //! \brief Constructs an XsFilterProfileArray as a copy of \a other - inline XsFilterProfileArray(XsFilterProfileArray const& other) - : ArrayImpl(other) - { - } - - //! \brief Constructs an XsFilterProfileArray that references the data supplied in \a ref - inline explicit XsFilterProfileArray(XsFilterProfile* ref, XsSize sz, XsDataFlags flags /* = XSDF_None */) - : ArrayImpl(ref, sz, flags) - { - } + //! \brief Constructs an XsFilterProfileArray + inline explicit XsFilterProfileArray(XsSize sz = 0, XsFilterProfile const* src = 0) + : ArrayImpl(sz, src) + { + } + + //! \brief Constructs an XsFilterProfileArray as a copy of \a other + inline XsFilterProfileArray(XsFilterProfileArray const& other) + : ArrayImpl(other) + { + } + + //! \brief Constructs an XsFilterProfileArray that references the data supplied in \a ref + inline explicit XsFilterProfileArray(XsFilterProfile* ref, XsSize sz, XsDataFlags flags /* = XSDF_None */) + : ArrayImpl(ref, sz, flags) + { + } #ifndef SWIG - /*! \brief Swap the contents the \a first and \a second array */ - friend void swap(XsFilterProfileArray& first, XsFilterProfileArray& second) - { - first.swap(second); - } + /*! \brief Swap the contents the \a first and \a second array */ + friend void swap(XsFilterProfileArray& first, XsFilterProfileArray& second) + { + first.swap(second); + } #endif #ifndef XSENS_NOITERATOR - //! \brief Constructs an XsFilterProfileArray with the array bound by the supplied iterators \a beginIt and \a endIt - template - inline XsFilterProfileArray(Iterator beginIt, Iterator endIt) - : ArrayImpl(beginIt, endIt) - { - } + //! \brief Constructs an XsFilterProfileArray with the array bound by the supplied iterators \a beginIt and \a endIt + template + inline XsFilterProfileArray(Iterator beginIt, Iterator endIt) + : ArrayImpl(beginIt, endIt) + { + } #endif -}; + }; #endif #endif diff --git a/lib/xspublic/xstypes/xsfilterprofilekind.h b/lib/xspublic/xstypes/xsfilterprofilekind.h index 68a29ae..ebb597a 100644 --- a/lib/xspublic/xstypes/xsfilterprofilekind.h +++ b/lib/xspublic/xstypes/xsfilterprofilekind.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xstypes/xsfloatmath.h b/lib/xspublic/xstypes/xsfloatmath.h index 4b33b60..e3e791e 100644 --- a/lib/xspublic/xstypes/xsfloatmath.h +++ b/lib/xspublic/xstypes/xsfloatmath.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xstypes/xsglovedata.c b/lib/xspublic/xstypes/xsglovedata.c index 393ca1a..ec85768 100644 --- a/lib/xspublic/xstypes/xsglovedata.c +++ b/lib/xspublic/xstypes/xsglovedata.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -139,7 +139,7 @@ void XsFingerData_construct(struct XsFingerData* thisPtr) } /*! \relates XsFingerData -\brief Destruct an %XsFingerData object + \brief Destruct an %XsFingerData object */ void XsFingerData_destruct(struct XsFingerData* thisPtr) { diff --git a/lib/xspublic/xstypes/xsglovedata.h b/lib/xspublic/xstypes/xsglovedata.h index 17a95d5..77dcd2c 100644 --- a/lib/xspublic/xstypes/xsglovedata.h +++ b/lib/xspublic/xstypes/xsglovedata.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -78,8 +78,8 @@ extern "C" { #ifndef __cplusplus #define XSFINGERDATA_INITIALIZER { XSQUATERNION_INITIALIZER, XSVECTOR3_INITIALIZER, XSVECTOR3_INITIALIZER, 0, 0, 0} #define XSGLOVEDATA_INITIALIZER { XSFINGERDATA_INITIALIZER, XSFINGERDATA_INITIALIZER, XSFINGERDATA_INITIALIZER, XSFINGERDATA_INITIALIZER, XSFINGERDATA_INITIALIZER, XSFINGERDATA_INITIALIZER, \ - XSFINGERDATA_INITIALIZER, XSFINGERDATA_INITIALIZER, XSFINGERDATA_INITIALIZER, XSFINGERDATA_INITIALIZER, XSFINGERDATA_INITIALIZER, XSFINGERDATA_INITIALIZER, \ - 0 ,0 ,0 ,0 } + XSFINGERDATA_INITIALIZER, XSFINGERDATA_INITIALIZER, XSFINGERDATA_INITIALIZER, XSFINGERDATA_INITIALIZER, XSFINGERDATA_INITIALIZER, XSFINGERDATA_INITIALIZER, \ + 0 ,0 ,0 ,0 } #endif XSTYPES_DLL_API void XsFingerData_construct(struct XsFingerData* thisPtr); @@ -222,7 +222,7 @@ struct XsGloveData } //! \brief Construct an initialized object - inline XsGloveData(const uint16_t frameNumber, const uint16_t validSampleFlags, const XsFingerData *fingerData) + inline XsGloveData(const uint16_t frameNumber, const uint16_t validSampleFlags, const XsFingerData* fingerData) : m_frameNumber(frameNumber) , m_validSampleFlags(validSampleFlags) { @@ -269,7 +269,7 @@ struct XsGloveData inline bool operator == (const XsGloveData& other) const { if (m_frameNumber != other.m_frameNumber || - m_validSampleFlags != other.m_validSampleFlags ) + m_validSampleFlags != other.m_validSampleFlags) return false; for (int i = 0; i < XSFINGERSEGMENTCOUNT; ++i) @@ -298,7 +298,7 @@ struct XsGloveData { XsGloveData_swap(this, &other); } - + /*! \brief Swap the contents of \a first with \a second */ friend void swap(XsGloveData& first, XsGloveData& second) { diff --git a/lib/xspublic/xstypes/xsglovesnapshot.h b/lib/xspublic/xstypes/xsglovesnapshot.h index 2418e65..91e7cab 100644 --- a/lib/xspublic/xstypes/xsglovesnapshot.h +++ b/lib/xspublic/xstypes/xsglovesnapshot.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -75,8 +75,8 @@ extern "C" { #endif #ifndef __cplusplus #define XSGLOVESNAPSHOT_INITIALIZER {0, 0, 0, 0, \ - XSFINGERSNAPSHOT_INITIALIZER, XSFINGERSNAPSHOT_INITIALIZER, XSFINGERSNAPSHOT_INITIALIZER, XSFINGERSNAPSHOT_INITIALIZER, XSFINGERSNAPSHOT_INITIALIZER, XSFINGERSNAPSHOT_INITIALIZER, \ - XSFINGERSNAPSHOT_INITIALIZER, XSFINGERSNAPSHOT_INITIALIZER, XSFINGERSNAPSHOT_INITIALIZER, XSFINGERSNAPSHOT_INITIALIZER, XSFINGERSNAPSHOT_INITIALIZER, XSFINGERSNAPSHOT_INITIALIZER} + XSFINGERSNAPSHOT_INITIALIZER, XSFINGERSNAPSHOT_INITIALIZER, XSFINGERSNAPSHOT_INITIALIZER, XSFINGERSNAPSHOT_INITIALIZER, XSFINGERSNAPSHOT_INITIALIZER, XSFINGERSNAPSHOT_INITIALIZER, \ + XSFINGERSNAPSHOT_INITIALIZER, XSFINGERSNAPSHOT_INITIALIZER, XSFINGERSNAPSHOT_INITIALIZER, XSFINGERSNAPSHOT_INITIALIZER, XSFINGERSNAPSHOT_INITIALIZER, XSFINGERSNAPSHOT_INITIALIZER} #define XSFINGERSNAPSHOT_INITIALIZER {0,0,0, 0,0,0, 0,0,0, 0, 0, 0} #endif @@ -84,22 +84,13 @@ extern "C" { } // extern "C" #endif -#ifdef _MSC_VER -#pragma pack(push, 1) -#ifndef PACK_POST -#define PACK_POST -#endif -#else -#ifndef PACK_POST -#define PACK_POST __attribute__((__packed__)) -#endif -#endif - #include /* pow */ +XS_PACKED_STRUCT_START /*! \brief int24 */ -struct int24_t { +struct int24_t +{ uint8_t m_vals[3]; /*!< \brief The data*/ #ifdef __cplusplus @@ -116,13 +107,14 @@ struct int24_t { #ifdef SWIG }; #else -} PACK_POST; +} XS_PACKED_STRUCT; #endif typedef struct int24_t int24_t; /*! \brief A container for Finger Snapshot data */ -struct XsFingerSnapshot { +struct XsFingerSnapshot +{ int24_t m_iQ[3]; /*!< \brief The integrated orientation */ int32_t m_iV[3]; /*!< \brief The integrated velocity */ int16_t m_mag[3]; /*!< \brief The magnetic field */ @@ -132,7 +124,7 @@ struct XsFingerSnapshot { #ifdef SWIG }; #else -} PACK_POST; +} XS_PACKED_STRUCT; #endif typedef struct XsFingerSnapshot XsFingerSnapshot; @@ -154,12 +146,10 @@ struct XsGloveSnapshot #ifdef SWIG }; #else -} PACK_POST; +} XS_PACKED_STRUCT; #endif typedef struct XsGloveSnapshot XsGloveSnapshot; -#ifdef _MSC_VER -#pragma pack(pop) -#endif +XS_PACKED_STRUCT_END #endif diff --git a/lib/xspublic/xstypes/xshandid.h b/lib/xspublic/xstypes/xshandid.h index 8735529..d9e08da 100644 --- a/lib/xspublic/xstypes/xshandid.h +++ b/lib/xspublic/xstypes/xshandid.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xstypes/xsinforequest.h b/lib/xspublic/xstypes/xsinforequest.h index bf4f1f1..a355cf7 100644 --- a/lib/xspublic/xstypes/xsinforequest.h +++ b/lib/xspublic/xstypes/xsinforequest.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -74,7 +74,8 @@ \details These values are used by the XsDevice::requestInfo function and XsCallback::onInfoResponse functions. */ -enum XsInfoRequest { +enum XsInfoRequest +{ XIR_BatteryLevel = 0, //!< Request battery level XIR_GnssSvInfo, //!< Request Gnss satellite vehicle information }; diff --git a/lib/xspublic/xstypes/xsint64array.c b/lib/xspublic/xstypes/xsint64array.c index 75b9a30..43928cb 100644 --- a/lib/xspublic/xstypes/xsint64array.c +++ b/lib/xspublic/xstypes/xsint64array.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -97,7 +97,8 @@ int compareInt64(int64_t const* a, int64_t const* b) } //! \brief Descriptor for XsInt64Array -XsArrayDescriptor const g_xsInt64ArrayDescriptor = { +XsArrayDescriptor const g_xsInt64ArrayDescriptor = +{ sizeof(int64_t), XSEXPCASTITEMSWAP swapInt64, // swap 0, // construct diff --git a/lib/xspublic/xstypes/xsint64array.h b/lib/xspublic/xstypes/xsint64array.h index 90ac882..0ae990e 100644 --- a/lib/xspublic/xstypes/xsint64array.h +++ b/lib/xspublic/xstypes/xsint64array.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -85,16 +85,17 @@ XSTYPES_DLL_API void XsInt64Array_construct(XsInt64Array* thisPtr, XsSize count, #ifdef __cplusplus } // extern "C" -struct XsInt64Array : public XsArrayImpl { +struct XsInt64Array : public XsArrayImpl +{ //! \brief Constructs an XsInt64Array inline explicit XsInt64Array(XsSize sz = 0, int64_t const* src = 0) - : ArrayImpl(sz, src) + : ArrayImpl(sz, src) { } //! \brief Constructs an XsInt64Array as a copy of \a other inline XsInt64Array(XsInt64Array const& other) - : ArrayImpl(other) + : ArrayImpl(other) { } diff --git a/lib/xspublic/xstypes/xsintarray.c b/lib/xspublic/xstypes/xsintarray.c index a0af50c..513c216 100644 --- a/lib/xspublic/xstypes/xsintarray.c +++ b/lib/xspublic/xstypes/xsintarray.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -98,7 +98,8 @@ int compareInt(int const* a, int const* b) //! \brief Descriptor for XsIntArray -XsArrayDescriptor const g_xsIntArrayDescriptor = { +XsArrayDescriptor const g_xsIntArrayDescriptor = +{ sizeof(int), XSEXPCASTITEMSWAP swapInt, // swap 0, // construct diff --git a/lib/xspublic/xstypes/xsintarray.h b/lib/xspublic/xstypes/xsintarray.h index 1c0bec6..33a1428 100644 --- a/lib/xspublic/xstypes/xsintarray.h +++ b/lib/xspublic/xstypes/xsintarray.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -85,16 +85,17 @@ XSTYPES_DLL_API void XsIntArray_construct(XsIntArray* thisPtr, XsSize count, int #ifdef __cplusplus } // extern "C" -struct XsIntArray : public XsArrayImpl { +struct XsIntArray : public XsArrayImpl +{ //! \brief Constructs an XsIntArray inline explicit XsIntArray(XsSize sz = 0, int const* src = 0) - : ArrayImpl(sz, src) + : ArrayImpl(sz, src) { } //! \brief Constructs an XsIntArray as a copy of \a other inline XsIntArray(XsIntArray const& other) - : ArrayImpl(other) + : ArrayImpl(other) { } diff --git a/lib/xspublic/xstypes/xslibraryloader.c b/lib/xspublic/xstypes/xslibraryloader.c index 39be97c..b764e32 100644 --- a/lib/xspublic/xstypes/xslibraryloader.c +++ b/lib/xspublic/xstypes/xslibraryloader.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -66,9 +66,9 @@ #include "xsstring.h" #ifdef __GNUC__ -#include + #include #elif defined(_MSC_VER) -#include + #include #endif /*! \brief Dynamically load a library @@ -85,7 +85,7 @@ int XsLibraryLoader_load(XsLibraryLoader* thisp, const XsString* libraryName) return 0; thisp->m_handle = dlopen(libraryName->m_data, RTLD_LAZY); #elif defined(_MSC_VER) - wchar_t *libraryNameW; + wchar_t* libraryNameW; XsSize required; if (XsLibraryLoader_isLoaded(thisp)) return 0; @@ -126,7 +126,7 @@ void* XsLibraryLoader_resolve(const XsLibraryLoader* thisp, const char* function */ int XsLibraryLoader_unload(XsLibraryLoader* thisp) { - void *handle = thisp->m_handle; + void* handle = thisp->m_handle; thisp->m_handle = NULL; if (handle) { @@ -159,14 +159,14 @@ void XsLibraryLoader_getErrorString(XsString* error) #elif defined(_MSC_VER) LPTSTR errorText = NULL; (void)FormatMessageW(FORMAT_MESSAGE_FROM_SYSTEM - |FORMAT_MESSAGE_ALLOCATE_BUFFER - |FORMAT_MESSAGE_IGNORE_INSERTS, - NULL, - GetLastError(), - MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT), - (LPTSTR)&errorText, - 0, - NULL); + | FORMAT_MESSAGE_ALLOCATE_BUFFER + | FORMAT_MESSAGE_IGNORE_INSERTS, + NULL, + GetLastError(), + MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT), + (LPTSTR)&errorText, + 0, + NULL); XsString_assignWCharArray(error, errorText); LocalFree(errorText); diff --git a/lib/xspublic/xstypes/xslibraryloader.h b/lib/xspublic/xstypes/xslibraryloader.h index 8a30f72..14f9398 100644 --- a/lib/xspublic/xstypes/xslibraryloader.h +++ b/lib/xspublic/xstypes/xslibraryloader.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -79,7 +79,7 @@ typedef struct XsLibraryLoader XsLibraryLoader; #endif XSTYPES_DLL_API int XsLibraryLoader_load(XsLibraryLoader* thisp, const XsString* libraryName); -XSTYPES_DLL_API void* XsLibraryLoader_resolve(const XsLibraryLoader* thisp, const char *functionName); +XSTYPES_DLL_API void* XsLibraryLoader_resolve(const XsLibraryLoader* thisp, const char* functionName); XSTYPES_DLL_API int XsLibraryLoader_unload(XsLibraryLoader* thisp); XSTYPES_DLL_API int XsLibraryLoader_isLoaded(const XsLibraryLoader* thisp); XSTYPES_DLL_API void XsLibraryLoader_getErrorString(XsString* error); @@ -90,7 +90,8 @@ XSTYPES_DLL_API void XsLibraryLoader_getErrorString(XsString* error); /*! \brief The Xsens dynamic library loader base class */ -struct XsLibraryLoader { +struct XsLibraryLoader +{ #ifdef __cplusplus public: /*! \brief Create a library loader */ @@ -109,8 +110,8 @@ struct XsLibraryLoader { } /*! \brief Load the library - \param[in] libraryName the name of the library to load - \return true if the library could be loaded, false otherwise + \param[in] libraryName the name of the library to load + \return true if the library could be loaded, false otherwise */ inline bool load(const XsString& libraryName) { @@ -119,7 +120,7 @@ struct XsLibraryLoader { /*! \brief Return true if a library has been loaded - \return true if a library has been loaded, false otherwise + \return true if a library has been loaded, false otherwise */ inline bool isLoaded() const { @@ -128,10 +129,10 @@ struct XsLibraryLoader { /*! \brief Resolve a function from the library - \param[in] functionName the name of the function to resolve - \return a pointer to the resolved function, NULL if nothing could be resolved + \param[in] functionName the name of the function to resolve + \return a pointer to the resolved function, NULL if nothing could be resolved */ - inline void* resolve(const char *functionName) const + inline void* resolve(const char* functionName) const { return XsLibraryLoader_resolve(this, functionName); } @@ -145,10 +146,10 @@ struct XsLibraryLoader { /*! \brief Return a string describing the error that occurred - Use this function after a function returned with an error to - receive some extra information about what went wrong. + Use this function after a function returned with an error to + receive some extra information about what went wrong. - \returns a string describing the error that occurred + \returns a string describing the error that occurred */ inline static XsString errorString() { diff --git a/lib/xspublic/xstypes/xsmalloc.c b/lib/xspublic/xstypes/xsmalloc.c index ddebabe..3cb832a 100644 --- a/lib/xspublic/xstypes/xsmalloc.c +++ b/lib/xspublic/xstypes/xsmalloc.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -68,32 +68,32 @@ #include "xsmalloc.h" #if !(defined __ICCARM__) && !(defined _ADI_COMPILER) && !defined(__APPLE__) && !defined(__CRCC__) && !(defined(__arm__) && defined(__ARMCC_VERSION)) -#include + #include #endif #include #ifdef XSENS_ASSERT_MALLOC -#include -#undef XSENS_ASSERT_MALLOC -#define XSENS_ASSERT_MALLOC(x) assert(x) + #include + #undef XSENS_ASSERT_MALLOC + #define XSENS_ASSERT_MALLOC(x) assert(x) #else -#define XSENS_ASSERT_MALLOC(x) + #define XSENS_ASSERT_MALLOC(x) #endif #if !(defined __ICCARM__) && !(defined _ADI_COMPILER) && defined(XSENS_DEBUG) -//#define TRACK_ALLOCS 32 + //#define TRACK_ALLOCS 32 #endif #ifdef TRACK_ALLOCS -int lastAllocIdx = -1; -void* lastAllocs[TRACK_ALLOCS]; -int lastFreeIdx = -1; -void* lastFrees[TRACK_ALLOCS]; - -int lastAlignedAllocIdx = -1; -void* lastAlignedAllocs[TRACK_ALLOCS]; -int lastAlignedFreeIdx = -1; -void* lastAlignedFrees[TRACK_ALLOCS]; + int lastAllocIdx = -1; + void* lastAllocs[TRACK_ALLOCS]; + int lastFreeIdx = -1; + void* lastFrees[TRACK_ALLOCS]; + + int lastAlignedAllocIdx = -1; + void* lastAlignedAllocs[TRACK_ALLOCS]; + int lastAlignedFreeIdx = -1; + void* lastAlignedFrees[TRACK_ALLOCS]; #endif #ifndef _MSC_VER @@ -126,7 +126,7 @@ void* xsMalloc(size_t sz) #ifdef TRACK_ALLOCS void* ptr = malloc(sz); XSENS_ASSERT_MALLOC(ptr); - lastAllocIdx = (lastAllocIdx + 1) & (TRACK_ALLOCS-1); + lastAllocIdx = (lastAllocIdx + 1) & (TRACK_ALLOCS - 1); lastAllocs[lastAllocIdx] = ptr; return ptr; #else @@ -140,12 +140,12 @@ void* xsMalloc(size_t sz) void* xsRealloc(void* ptr, size_t sz) { #ifdef TRACK_ALLOCS - lastFreeIdx = (lastFreeIdx + 1) & (TRACK_ALLOCS-1); + lastFreeIdx = (lastFreeIdx + 1) & (TRACK_ALLOCS - 1); lastFrees[lastFreeIdx] = ptr; ptr = realloc(ptr, sz); XSENS_ASSERT_MALLOC(ptr); - lastAllocIdx = (lastAllocIdx + 1) & (TRACK_ALLOCS-1); + lastAllocIdx = (lastAllocIdx + 1) & (TRACK_ALLOCS - 1); lastAllocs[lastAllocIdx] = ptr; return ptr; #else @@ -159,7 +159,7 @@ void* xsRealloc(void* ptr, size_t sz) void xsFree(void* ptr) { #ifdef TRACK_ALLOCS - lastFreeIdx = (lastFreeIdx + 1) & (TRACK_ALLOCS-1); + lastFreeIdx = (lastFreeIdx + 1) & (TRACK_ALLOCS - 1); lastFrees[lastFreeIdx] = ptr; #endif free(ptr); @@ -171,7 +171,7 @@ void* xsAlignedMalloc(size_t sz) #ifdef TRACK_ALLOCS void* ptr = _aligned_malloc(sz, 16); XSENS_ASSERT_MALLOC(ptr); - lastAlignedAllocIdx = (lastAlignedAllocIdx + 1) & (TRACK_ALLOCS-1); + lastAlignedAllocIdx = (lastAlignedAllocIdx + 1) & (TRACK_ALLOCS - 1); lastAlignedAllocs[lastAlignedAllocIdx] = ptr; return ptr; #else @@ -185,12 +185,12 @@ void* xsAlignedMalloc(size_t sz) void* xsAlignedRealloc(void* ptr, size_t sz) { #ifdef TRACK_ALLOCS - lastFreeIdx = (lastAlignedFreeIdx + 1) & (TRACK_ALLOCS-1); + lastFreeIdx = (lastAlignedFreeIdx + 1) & (TRACK_ALLOCS - 1); lastAlignedFrees[lastAlignedFreeIdx] = ptr; ptr = _aligned_realloc(ptr, sz, 16); XSENS_ASSERT_MALLOC(ptr); - lastAlignedAllocIdx = (lastAlignedAllocIdx + 1) & (TRACK_ALLOCS-1); + lastAlignedAllocIdx = (lastAlignedAllocIdx + 1) & (TRACK_ALLOCS - 1); lastAlignedAllocs[lastAlignedAllocIdx] = ptr; return ptr; #else @@ -204,7 +204,7 @@ void* xsAlignedRealloc(void* ptr, size_t sz) void xsAlignedFree(void* ptr) { #ifdef TRACK_ALLOCS - lastAlignedFreeIdx = (lastAlignedFreeIdx + 1) & (TRACK_ALLOCS-1); + lastAlignedFreeIdx = (lastAlignedFreeIdx + 1) & (TRACK_ALLOCS - 1); lastAlignedFrees[lastAlignedFreeIdx] = ptr; #endif _aligned_free(ptr); diff --git a/lib/xspublic/xstypes/xsmalloc.h b/lib/xspublic/xstypes/xsmalloc.h index 1c303da..a2573bb 100644 --- a/lib/xspublic/xstypes/xsmalloc.h +++ b/lib/xspublic/xstypes/xsmalloc.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xstypes/xsmath.c b/lib/xspublic/xstypes/xsmath.c index 52f76f6..fe57a17 100644 --- a/lib/xspublic/xstypes/xsmath.c +++ b/lib/xspublic/xstypes/xsmath.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -68,8 +68,8 @@ // The file may be used again later and too many projects depend on its presence to be able to easily remove it #ifdef _MSC_VER -/*! \cond NODOXYGEN */ -// this is just here to prevent compiler warnings about this file being empty -int haveObsoleteXsMath = 0; -/*! \endcond */ + /*! \cond NODOXYGEN */ + // this is just here to prevent compiler warnings about this file being empty + int haveObsoleteXsMath = 0; + /*! \endcond */ #endif diff --git a/lib/xspublic/xstypes/xsmath.h b/lib/xspublic/xstypes/xsmath.h index 2ec45e8..dea0ddc 100644 --- a/lib/xspublic/xstypes/xsmath.h +++ b/lib/xspublic/xstypes/xsmath.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -73,27 +73,36 @@ #include "xsfloatmath.h" #if defined(SQUISHCOCO) -#define XSMATHCONST static const -#define XSMATHINLINE2 static -#define XSMATHINLINE static + #define XSMATHCONST static const + #define XSMATHINLINE2 static + #define XSMATHINLINE static + #define WANTXSMATH2 1 #else // normal operation -#if defined( __cplusplus) -#if defined(__ADSP21000__) -#define XSMATHCONST static const -#define XSMATHINLINE inline static -#elif defined(__ANDROID_API__) || defined(__APPLE__) -#define XSMATHCONST constexpr -#define XSMATHINLINE inline static -#else -#define XSMATHCONST constexpr -#define XSMATHINLINE inline static constexpr -#endif -#define XSMATHINLINE2 inline static -#else -#define XSMATHCONST static const -#define XSMATHINLINE static -#define XSMATHINLINE2 static -#endif + #if defined( __cplusplus) + #if defined(__ADSP21000__) + #define XSMATHCONST static const + #define XSMATHINLINE inline static + #define WANTXSMATH2 0 + #elif defined(__ANDROID_API__) || defined(__APPLE__) + #define XSMATHCONST static constexpr + #define XSMATHINLINE inline static + #define WANTXSMATH2 1 + #elif defined(__AVR32__) + #define XSMATHCONST static const + #define XSMATHINLINE inline static + #define WANTXSMATH2 0 + #else + #define XSMATHCONST static constexpr + #define XSMATHINLINE inline static constexpr + #define WANTXSMATH2 1 + #endif + #define XSMATHINLINE2 inline static + #else + #define XSMATHCONST static const + #define XSMATHINLINE static + #define XSMATHINLINE2 static + #define WANTXSMATH2 0 + #endif #endif /*! \namespace XsMath @@ -165,14 +174,14 @@ XSMATHCONST XsReal XsMath_twoPi = 6.28318530717958647692528676655900576839433879 //! \brief sqrt(2) XSMATHCONST XsReal XsMath_sqrt2 = 1.4142135623730950488016887242097; //! \brief sqrt(0.5) -XSMATHCONST XsReal XsMath_sqrtHalf = 0.5*1.4142135623730950488016887242097; +XSMATHCONST XsReal XsMath_sqrtHalf = 0.5 * 1.4142135623730950488016887242097; #ifdef XSENS_SINGLE_PRECISION -//! \brief infinity value -XSMATHCONST XsReal XsMath_infinity = FLT_MAX; + //! \brief infinity value + XSMATHCONST XsReal XsMath_infinity = FLT_MAX; #else -//! \brief infinity value -XSMATHCONST XsReal XsMath_infinity = DBL_MAX; + //! \brief infinity value + XSMATHCONST XsReal XsMath_infinity = DBL_MAX; #endif /*! \brief Returns asin(\a x) for -1 < x < 1 @@ -200,21 +209,21 @@ XSMATHINLINE XsReal XsMath_deg2rad(XsReal degrees) */ XSMATHINLINE XsReal XsMath_pow2(XsReal a) { - return a*a; + return a * a; } /*! \brief Returns \a a to the power of 3 */ XSMATHINLINE XsReal XsMath_pow3(XsReal a) { - return a*a*a; + return a * a * a; } /*! \brief Returns \a a to the power of 5 */ XSMATHINLINE XsReal XsMath_pow5(XsReal a) { - return XsMath_pow2(a)*XsMath_pow3(a); + return XsMath_pow2(a) * XsMath_pow3(a); } /*! \brief Returns non-zero if \a x is finite @@ -224,22 +233,22 @@ XSMATHINLINE2 int XsMath_isFinite(XsReal x) #ifdef _MSC_VER switch (_fpclass(x)) { - case _FPCLASS_SNAN: - case _FPCLASS_QNAN: - case _FPCLASS_NINF: - case _FPCLASS_PINF: - return 0; + case _FPCLASS_SNAN: + case _FPCLASS_QNAN: + case _FPCLASS_NINF: + case _FPCLASS_PINF: + return 0; - case _FPCLASS_NN: - case _FPCLASS_ND: - case _FPCLASS_NZ: - case _FPCLASS_PZ: - case _FPCLASS_PD: - case _FPCLASS_PN: - return 1; + case _FPCLASS_NN: + case _FPCLASS_ND: + case _FPCLASS_NZ: + case _FPCLASS_PZ: + case _FPCLASS_PD: + case _FPCLASS_PN: + return 1; - default: - return _finite(x); + default: + return _finite(x); } #elif defined(isfinite) || defined(__APPLE__) return isfinite(x); @@ -262,34 +271,34 @@ XSMATHINLINE2 int XsMath_isFinite(XsReal x) */ XSMATHINLINE2 int32_t XsMath_floatToLong(float d) { - return (d >= 0) ? (int32_t) floorf(d+0.5f) : (int32_t) ceilf(d-0.5f); + return (d >= 0) ? (int32_t) floorf(d + 0.5f) : (int32_t) ceilf(d - 0.5f); } /*! \brief Returns \a d integer converted from a single precision floating point value */ XSMATHINLINE2 int64_t XsMath_floatToInt64(float d) { - return (d >= 0) ? (int64_t) floorf(d+0.5f) : (int64_t) ceilf(d-0.5f); + return (d >= 0) ? (int64_t) floorf(d + 0.5f) : (int64_t) ceilf(d - 0.5f); } /*! \brief Returns \a d integer converted from a double precision floating point value */ XSMATHINLINE2 int32_t XsMath_doubleToLong(double d) { - return (d >= 0) ? (int32_t) floor(d+0.5) : (int32_t) ceil(d-0.5); + return (d >= 0) ? (int32_t) floor(d + 0.5) : (int32_t) ceil(d - 0.5); } /*! \brief Returns \a d integer converted from a double precision floating point value */ XSMATHINLINE2 int64_t XsMath_doubleToInt64(double d) { - return (d >= 0) ? (int64_t) floor(d+0.5) : (int64_t) ceil(d-0.5); + return (d >= 0) ? (int64_t) floor(d + 0.5) : (int64_t) ceil(d - 0.5); } #ifdef __cplusplus -#ifndef XSMATH2_H -#include "xsmath2.h" -#endif + #ifndef XSMATH2_H + #include "xsmath2.h" + #endif #endif /*! @} */ diff --git a/lib/xspublic/xstypes/xsmath2.h b/lib/xspublic/xstypes/xsmath2.h index 0324c19..d90ef37 100644 --- a/lib/xspublic/xstypes/xsmath2.h +++ b/lib/xspublic/xstypes/xsmath2.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -66,115 +66,116 @@ #define XSMATH2_H #ifndef XSMATH_H -#include "xsmath.h" + #include "xsmath.h" #endif #ifdef __cplusplus -namespace XsMath { +namespace XsMath +{ #ifdef __GNUC__ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wunused-variable" + #pragma GCC diagnostic push + #pragma GCC diagnostic ignored "-Wunused-variable" #endif - //! \brief The value e - XSMATHCONST XsReal e = XsMath_e; - //! \brief The value pi - XSMATHCONST XsReal pi = XsMath_pi; - //! \brief A really small value - XSMATHCONST XsReal tinyValue = XsMath_tinyValue; - //! \brief A convincingly large number - XSMATHCONST XsReal hugeValue = XsMath_hugeValue; - //! \brief A value related to the precision of floating point arithmetic (2.2204460492503131e-016) - XSMATHCONST XsReal epsilon = XsMath_epsilon; - /*! \brief Square root of epsilon - \sa epsilon - */ - XSMATHCONST XsReal sqrtEpsilon = XsMath_sqrtEpsilon; - //! \brief Value that represents the subnormal number in floating point wizardry - XSMATHCONST XsReal denormalized = XsMath_denormalized; - /*! \brief Square root of denormalized - \sa denormalized - */ - XSMATHCONST XsReal sqrtDenormalized = XsMath_sqrtDenormalized; - //! \brief Value to convert radians to degrees by multiplication - XSMATHCONST XsReal rad2degValue = XsMath_rad2degValue; - //! \brief Value to convert degrees to radians by multiplication - XSMATHCONST XsReal deg2radValue = XsMath_deg2radValue; - //! \brief 0 - XSMATHCONST XsReal zero = XsMath_zero; - //! \brief 0.25 - XSMATHCONST XsReal pt25 = XsMath_pt25; - //! \brief 0.5 - XSMATHCONST XsReal pt5 = XsMath_pt5; - //! \brief -0.5 - XSMATHCONST XsReal minusPt5 = XsMath_minusPt5; - //! \brief 1 - XSMATHCONST XsReal one = XsMath_one; - //! \brief -1 - XSMATHCONST XsReal minusOne = XsMath_minusOne; - //! \brief 2 - XSMATHCONST XsReal two = XsMath_two; - //! \brief 4 - XSMATHCONST XsReal four = XsMath_four; - //! \brief -2 - XSMATHCONST XsReal minusTwo = XsMath_minusTwo; - //! \brief -pi/2 - XSMATHCONST XsReal minusHalfPi = XsMath_minusHalfPi; - //! \brief pi/2 - XSMATHCONST XsReal halfPi = XsMath_halfPi; - //! \brief 2*pi - XSMATHCONST XsReal twoPi = XsMath_twoPi; - //! \brief sqrt(2) - XSMATHCONST XsReal sqrt2 = XsMath_sqrt2; - //! \brief sqrt(0.5) - XSMATHCONST XsReal sqrtHalf = XsMath_sqrtHalf; - //! \brief infinity - XSMATHCONST XsReal infinity = XsMath_infinity; +//! \brief The value e +XSMATHCONST XsReal e = XsMath_e; +//! \brief The value pi +XSMATHCONST XsReal pi = XsMath_pi; +//! \brief A really small value +XSMATHCONST XsReal tinyValue = XsMath_tinyValue; +//! \brief A convincingly large number +XSMATHCONST XsReal hugeValue = XsMath_hugeValue; +//! \brief A value related to the precision of floating point arithmetic (2.2204460492503131e-016) +XSMATHCONST XsReal epsilon = XsMath_epsilon; +/*! \brief Square root of epsilon + \sa epsilon +*/ +XSMATHCONST XsReal sqrtEpsilon = XsMath_sqrtEpsilon; +//! \brief Value that represents the subnormal number in floating point wizardry +XSMATHCONST XsReal denormalized = XsMath_denormalized; +/*! \brief Square root of denormalized + \sa denormalized +*/ +XSMATHCONST XsReal sqrtDenormalized = XsMath_sqrtDenormalized; +//! \brief Value to convert radians to degrees by multiplication +XSMATHCONST XsReal rad2degValue = XsMath_rad2degValue; +//! \brief Value to convert degrees to radians by multiplication +XSMATHCONST XsReal deg2radValue = XsMath_deg2radValue; +//! \brief 0 +XSMATHCONST XsReal zero = XsMath_zero; +//! \brief 0.25 +XSMATHCONST XsReal pt25 = XsMath_pt25; +//! \brief 0.5 +XSMATHCONST XsReal pt5 = XsMath_pt5; +//! \brief -0.5 +XSMATHCONST XsReal minusPt5 = XsMath_minusPt5; +//! \brief 1 +XSMATHCONST XsReal one = XsMath_one; +//! \brief -1 +XSMATHCONST XsReal minusOne = XsMath_minusOne; +//! \brief 2 +XSMATHCONST XsReal two = XsMath_two; +//! \brief 4 +XSMATHCONST XsReal four = XsMath_four; +//! \brief -2 +XSMATHCONST XsReal minusTwo = XsMath_minusTwo; +//! \brief -pi/2 +XSMATHCONST XsReal minusHalfPi = XsMath_minusHalfPi; +//! \brief pi/2 +XSMATHCONST XsReal halfPi = XsMath_halfPi; +//! \brief 2*pi +XSMATHCONST XsReal twoPi = XsMath_twoPi; +//! \brief sqrt(2) +XSMATHCONST XsReal sqrt2 = XsMath_sqrt2; +//! \brief sqrt(0.5) +XSMATHCONST XsReal sqrtHalf = XsMath_sqrtHalf; +//! \brief infinity +XSMATHCONST XsReal infinity = XsMath_infinity; #ifdef __GNUC__ -#pragma GCC diagnostic pop + #pragma GCC diagnostic pop #endif - //! \brief \copybrief XsMath_asinClamped - XSMATHINLINE XsReal asinClamped(XsReal x) - { - return XsMath_asinClamped(x); - } +//! \brief \copybrief XsMath_asinClamped +XSMATHINLINE XsReal asinClamped(XsReal x) +{ + return XsMath_asinClamped(x); +} - //! \brief \copybrief XsMath_rad2deg - XSMATHINLINE XsReal rad2deg(XsReal radians) - { - return XsMath_rad2deg(radians); - } +//! \brief \copybrief XsMath_rad2deg +XSMATHINLINE XsReal rad2deg(XsReal radians) +{ + return XsMath_rad2deg(radians); +} - //! \brief \copybrief XsMath_deg2rad - XSMATHINLINE XsReal deg2rad(XsReal degrees) - { - return XsMath_deg2rad(degrees); - } +//! \brief \copybrief XsMath_deg2rad +XSMATHINLINE XsReal deg2rad(XsReal degrees) +{ + return XsMath_deg2rad(degrees); +} - //! \brief \copybrief XsMath_pow2 - XSMATHINLINE XsReal pow2(XsReal a) - { - return XsMath_pow2(a); - } +//! \brief \copybrief XsMath_pow2 +XSMATHINLINE XsReal pow2(XsReal a) +{ + return XsMath_pow2(a); +} - //! \brief \copybrief XsMath_pow3 - XSMATHINLINE XsReal pow3(XsReal a) - { - return XsMath_pow3(a); - } +//! \brief \copybrief XsMath_pow3 +XSMATHINLINE XsReal pow3(XsReal a) +{ + return XsMath_pow3(a); +} - //! \brief \copybrief XsMath_doubleToLong - XSMATHINLINE2 int32_t doubleToLong(double d) - { - return XsMath_doubleToLong(d); - } +//! \brief \copybrief XsMath_doubleToLong +XSMATHINLINE2 int32_t doubleToLong(double d) +{ + return XsMath_doubleToLong(d); +} #ifndef XSENS_NO_INT64 - //! \brief \copybrief XsMath_doubleToInt64 - XSMATHINLINE2 int64_t doubleToInt64(double d) - { - return XsMath_doubleToInt64(d); - } +//! \brief \copybrief XsMath_doubleToInt64 +XSMATHINLINE2 int64_t doubleToInt64(double d) +{ + return XsMath_doubleToInt64(d); +} #endif } // namespace #endif diff --git a/lib/xspublic/xstypes/xsmatrix.c b/lib/xspublic/xstypes/xsmatrix.c index b098f50..30468ce 100644 --- a/lib/xspublic/xstypes/xsmatrix.c +++ b/lib/xspublic/xstypes/xsmatrix.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -71,6 +71,10 @@ #include "xsquaternion.h" #include +#ifdef __ICCARM__ + #pragma diag_suppress=Pa039 +#endif + /*! \class XsMatrix \brief A class that represents a matrix of real numbers */ @@ -96,7 +100,7 @@ void XsMatrix_ref(XsMatrix* thisPtr, XsSize rows, XsSize cols, XsSize stride, Xs void XsMatrix_construct(XsMatrix* thisPtr, XsSize rows, XsSize cols, XsSize stride, const XsReal* src, XsSize srcStride) { XsSize r; - XsSize size = rows*stride; + XsSize size = rows * stride; if (stride == 0) { @@ -106,7 +110,7 @@ void XsMatrix_construct(XsMatrix* thisPtr, XsSize rows, XsSize cols, XsSize stri if (size) { // init to size - XsReal* data = (XsReal*) xsMathMalloc(size*sizeof(XsReal)); + XsReal* data = (XsReal*) xsMathMalloc(size * sizeof(XsReal)); assert(data); *((XsReal**) &thisPtr->m_data) = data; XsMatrix_incAllocCount(); @@ -121,17 +125,17 @@ void XsMatrix_construct(XsMatrix* thisPtr, XsSize rows, XsSize cols, XsSize stri if (src && size) { if (srcStride == 0 || srcStride == stride) - memcpy(thisPtr->m_data, src, size*sizeof(XsReal)); + memcpy(thisPtr->m_data, src, size * sizeof(XsReal)); else { for (r = 0; r < rows; ++r) { #if XSREAL_ALLOWS_MEMCPY - memcpy(thisPtr->m_data + r*stride, src + r*srcStride, cols*sizeof(XsReal)); + memcpy(thisPtr->m_data + r * stride, src + r * srcStride, cols * sizeof(XsReal)); #else XsSize c; for (c = 0; c < cols; ++c) - thisPtr->m_data[r*stride+c] = src[r*srcStride + c]; + thisPtr->m_data[r * stride + c] = src[r * srcStride + c]; #endif } } @@ -142,7 +146,7 @@ void XsMatrix_construct(XsMatrix* thisPtr, XsSize rows, XsSize cols, XsSize stri void XsMatrix_assign(XsMatrix* thisPtr, XsSize rows, XsSize cols, XsSize stride, const XsReal* src, XsSize srcStride) { XsSize r; - XsSize size = rows*stride; + XsSize size = rows * stride; if (thisPtr->m_flags & XSDF_FixedSize) { @@ -172,13 +176,13 @@ void XsMatrix_assign(XsMatrix* thisPtr, XsSize rows, XsSize cols, XsSize stride, stride = cols; size = rows * stride; } - if (size > thisPtr->m_rows*thisPtr->m_stride || thisPtr->m_rows == 0) + if (size > thisPtr->m_rows * thisPtr->m_stride || thisPtr->m_rows == 0) { XsMatrix_destruct(thisPtr); if (size) { // init to size - XsReal* data = (XsReal*) xsMathMalloc(size*sizeof(XsReal)); + XsReal* data = (XsReal*) xsMathMalloc(size * sizeof(XsReal)); assert(data); *((XsReal**) &thisPtr->m_data) = data; *((XsSize*) &thisPtr->m_flags) = XSDF_Managed; @@ -192,18 +196,18 @@ void XsMatrix_assign(XsMatrix* thisPtr, XsSize rows, XsSize cols, XsSize stride, } if (src && size) { - if (srcStride == 0 || srcStride == stride) - memcpy(thisPtr->m_data, src, size*sizeof(XsReal)); + if (stride == cols && (srcStride == 0 || srcStride == stride)) + memcpy(thisPtr->m_data, src, size * sizeof(XsReal)); else { for (r = 0; r < rows; ++r) { #if XSREAL_ALLOWS_MEMCPY - memcpy(thisPtr->m_data + r*stride, src + r*srcStride, cols*sizeof(XsReal)); + memcpy(thisPtr->m_data + r * stride, src + r * srcStride, cols * sizeof(XsReal)); #else XsSize c; for (c = 0; c < cols; ++c) - thisPtr->m_data[r*stride+c] = src[r*srcStride + c]; + thisPtr->m_data[r * stride + c] = src[r * srcStride + c]; #endif } } @@ -245,18 +249,18 @@ void XsMatrix_setZero(XsMatrix* thisPtr) { #if XSREAL_ALLOWS_MEMCPY if (thisPtr->m_stride == thisPtr->m_cols) - memset(thisPtr->m_data, 0, sizeof(XsReal)*thisPtr->m_rows*thisPtr->m_cols); + memset(thisPtr->m_data, 0, sizeof(XsReal)*thisPtr->m_rows * thisPtr->m_cols); else { XsSize r; for (r = 0; r < thisPtr->m_rows; ++r) - memset(thisPtr->m_data + r*thisPtr->m_stride, 0, sizeof(XsReal) * thisPtr->m_cols); + memset(thisPtr->m_data + r * thisPtr->m_stride, 0, sizeof(XsReal) * thisPtr->m_cols); } #else - XsSize r,c,stride = thisPtr->m_stride; + XsSize r, c, stride = thisPtr->m_stride; for (r = 0; r < thisPtr->m_rows; ++r) for (c = 0; c < thisPtr->m_cols; ++c) - thisPtr->m_data[r*stride+c] = XsMath_zero; + thisPtr->m_data[r * stride + c] = XsMath_zero; #endif } @@ -272,12 +276,12 @@ int XsMatrix_empty(const XsMatrix* thisPtr) */ void XsMatrix_multiplyScalar(const XsMatrix* thisPtr, XsReal scalar, XsMatrix* dest) { - XsSize r,c,stride = thisPtr->m_stride, stride2; + XsSize r, c, stride = thisPtr->m_stride, stride2; XsMatrix_assign(dest, thisPtr->m_rows, thisPtr->m_cols, 0, 0, 0); stride2 = dest->m_stride; for (r = 0; r < thisPtr->m_rows; ++r) for (c = 0; c < thisPtr->m_cols; ++c) - dest->m_data[r*stride2+c] = thisPtr->m_data[r*stride+c] * scalar; + dest->m_data[r * stride2 + c] = thisPtr->m_data[r * stride + c] * scalar; } /*! \relates XsMatrix @@ -321,18 +325,18 @@ void XsMatrix_fromQuaternion(XsMatrix* thisPtr, const XsQuaternion* quat) return; } - q00 = quat->m_w*quat->m_w; - q11 = quat->m_x*quat->m_x; - q22 = quat->m_y*quat->m_y; - q33 = quat->m_z*quat->m_z; + q00 = quat->m_w * quat->m_w; + q11 = quat->m_x * quat->m_x; + q22 = quat->m_y * quat->m_y; + q33 = quat->m_z * quat->m_z; - q01 = quat->m_w*quat->m_x; - q02 = quat->m_w*quat->m_y; - q03 = quat->m_w*quat->m_z; + q01 = quat->m_w * quat->m_x; + q02 = quat->m_w * quat->m_y; + q03 = quat->m_w * quat->m_z; - q12 = quat->m_x*quat->m_y; - q13 = quat->m_x*quat->m_z; - q23 = quat->m_y*quat->m_z; + q12 = quat->m_x * quat->m_y; + q13 = quat->m_x * quat->m_z; + q23 = quat->m_y * quat->m_z; XsMatrix_assign(thisPtr, 3, 3, 3, 0, 0); @@ -358,7 +362,7 @@ void XsMatrix_fromQuaternion(XsMatrix* thisPtr, const XsQuaternion* quat) void XsMatrix_swap(XsMatrix* a, XsMatrix* b) { #ifdef __ICCARM__ - #pragma diag_suppress=Pe370 +#pragma diag_suppress=Pe370 #endif XsMatrix tmp; if ((!a->m_data || (a->m_flags & XSDF_Managed)) && (!b->m_data || (b->m_flags & XSDF_Managed))) @@ -388,8 +392,8 @@ void XsMatrix_swap(XsMatrix* a, XsMatrix* b) assert(a->m_data && b->m_data && a->m_rows == b->m_rows && a->m_cols == b->m_cols); for (r = 0; r < a->m_rows; ++r) { - XsReal* aa = a->m_data + r*a->m_stride*sizeof(XsReal); - XsReal* bb = b->m_data + r*b->m_stride*sizeof(XsReal); + XsReal* aa = a->m_data + r * a->m_stride; + XsReal* bb = b->m_data + r * b->m_stride; for (c = 0; c < a->m_cols; ++c, ++aa, ++bb) { v = *aa; diff --git a/lib/xspublic/xstypes/xsmatrix.h b/lib/xspublic/xstypes/xsmatrix.h index d391008..a07cfba 100644 --- a/lib/xspublic/xstypes/xsmatrix.h +++ b/lib/xspublic/xstypes/xsmatrix.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -102,10 +102,11 @@ XSTYPES_DLL_API void XsMatrix_swap(XsMatrix* a, XsMatrix* b); } // extern "C" #endif #ifndef XSENS_NO_PACK -#pragma pack(push, 1) + #pragma pack(push, 1) #endif -struct XsMatrix { -XSCPPPROTECTED +struct XsMatrix +{ + XSCPPPROTECTED XsReal* const m_data; //!< Contained data const XsSize m_rows; //!< Number of rows in the matrix const XsSize m_cols; //!< Number of columns in the matrix @@ -113,8 +114,16 @@ XSCPPPROTECTED const XsSize m_flags; //!< Flags for data management #ifdef __cplusplus + +#ifdef __ICCARM__ +#pragma diag_suppress=Pa039 +#endif + //! \brief Return the data management flags of the matrix. - inline XsSize flags() const { return m_flags; } + inline XsSize flags() const + { + return m_flags; + } public: /*! \brief Initialize an XsMatrix object with the specified number of \a rows and \a cols */ inline explicit XsMatrix(XsSize rows = 0, XsSize cols = 0, XsSize strde = 0, const XsReal* dat = 0) @@ -125,7 +134,7 @@ XSCPPPROTECTED , m_flags(0) { if (rows && cols) - XsMatrix_construct(this, rows, cols, strde?strde:cols, dat, 0); + XsMatrix_construct(this, rows, cols, strde ? strde : cols, dat, 0); } /*! \brief Initialize an XsMatrix object from the \a other XsMatrix */ @@ -139,7 +148,7 @@ XSCPPPROTECTED XsMatrix_copy(this, &other); } -#if !defined(SWIG) && !defined(__ADSP21000__) +#if !defined(SWIG) && !defined(__ADSP21000__) && !defined(__AVR32__) /*! \brief Move-construct an XsMatrix object from the \a other XsMatrix */ inline XsMatrix(XsMatrix&& other) : m_data(0) @@ -338,23 +347,28 @@ XSCPPPROTECTED { XsMatrix_swap(this, &b); } - + /*! \brief swaps the contents \a first with that of \a second */ - friend void swap(XsMatrix& first, XsMatrix& second) - { + friend void swap(XsMatrix& first, XsMatrix& second) + { first.swap(second); } + +#ifdef __ICCARM__ +#pragma diag_default=Pa039 +#endif + #endif }; #ifndef XSENS_NO_PACK -#pragma pack(pop) + #pragma pack(pop) #endif #ifdef __cplusplus //! \brief Multiplies all values in the matrix \a m by \a scalar -inline XsMatrix operator *(XsReal scalar, const XsMatrix &m) +inline XsMatrix operator *(XsReal scalar, const XsMatrix& m) { - return (m*scalar); + return (m * scalar); } #endif diff --git a/lib/xspublic/xstypes/xsmatrix3x3.c b/lib/xspublic/xstypes/xsmatrix3x3.c index 3348f0d..1f1990a 100644 --- a/lib/xspublic/xstypes/xsmatrix3x3.c +++ b/lib/xspublic/xstypes/xsmatrix3x3.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -87,12 +87,12 @@ void XsMatrix3x3_assign(XsMatrix3x3* thisPtr, const XsReal* src, XsSize srcStrid if (src) { if (srcStride == 0 || srcStride == 3) - memcpy(thisPtr->m_matrix.m_data, src, 3*3*sizeof(XsReal)); + memcpy(thisPtr->m_matrix.m_data, src, 3 * 3 * sizeof(XsReal)); else { for (r = 0; r < 3; ++r) for (c = 0; c < 3; ++c) - thisPtr->m_matrix.m_data[r*3+c] = src[r*srcStride + c]; + thisPtr->m_matrix.m_data[r * 3 + c] = src[r * srcStride + c]; } } } diff --git a/lib/xspublic/xstypes/xsmatrix3x3.h b/lib/xspublic/xstypes/xsmatrix3x3.h index 42cb36b..aac7782 100644 --- a/lib/xspublic/xstypes/xsmatrix3x3.h +++ b/lib/xspublic/xstypes/xsmatrix3x3.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -84,54 +84,56 @@ XSTYPES_DLL_API void XsMatrix3x3_copy(XsMatrix* copy, XsMatrix3x3 const* src); #ifdef __cplusplus } // extern "C" - /* This is allowed since the C standard says that no padding appears before the first member of a struct. - Basically we're defining a union between a C++ inherited type and a C encapsulated type. +/* This is allowed since the C standard says that no padding appears before the first member of a struct. + Basically we're defining a union between a C++ inherited type and a C encapsulated type. */ -struct XsMatrix3x3 : public XsMatrix { -XSCPPPROTECTED +struct XsMatrix3x3 : public XsMatrix +{ + XSCPPPROTECTED #endif #ifndef __cplusplus -struct XsMatrix3x3 { - struct XsMatrix m_matrix; //!< The underlying XsMatrix + struct XsMatrix3x3 + { + struct XsMatrix m_matrix; //!< The underlying XsMatrix #endif - XsReal XSCCONST m_fixedData[9]; //!< Fixed storage for the elements of the matrix + XsReal XSCCONST m_fixedData[9]; //!< Fixed storage for the elements of the matrix #ifdef __cplusplus -public: - //! \brief Constructs an XsMatrix3x3 - XsMatrix3x3() : XsMatrix(m_fixedData, 3, 3, 3, XSDF_FixedSize) - { - } + public: + //! \brief Constructs an XsMatrix3x3 + XsMatrix3x3() : XsMatrix(m_fixedData, 3, 3, 3, XSDF_FixedSize) + { + } - //! \brief Constructs an XsMatrix3x3 from an \a other XsMatrix - XsMatrix3x3(const XsMatrix& other) : XsMatrix(other, m_fixedData, 3, 3, 3, XSDF_FixedSize) - { - } + //! \brief Constructs an XsMatrix3x3 from an \a other XsMatrix + XsMatrix3x3(const XsMatrix& other) : XsMatrix(other, m_fixedData, 3, 3, 3, XSDF_FixedSize) + { + } - //! \brief Constructs an XsMatrix3x3 from an \a other XsMatrix - XsMatrix3x3(const XsMatrix3x3& other) : XsMatrix(other, m_fixedData, 3, 3, 3, XSDF_FixedSize) - { - } + //! \brief Constructs an XsMatrix3x3 from an \a other XsMatrix + XsMatrix3x3(const XsMatrix3x3& other) : XsMatrix(other, m_fixedData, 3, 3, 3, XSDF_FixedSize) + { + } - //! \brief Constructs an XsMatrix3x3 from a set of values - XsMatrix3x3(XsReal r1c1, XsReal r1c2, XsReal r1c3, - XsReal r2c1, XsReal r2c2, XsReal r2c3, - XsReal r3c1, XsReal r3c2, XsReal r3c3) : XsMatrix(m_fixedData, 3, 3, 3, XSDF_FixedSize) - { - m_fixedData[0] = r1c1; - m_fixedData[1] = r1c2; - m_fixedData[2] = r1c3; - m_fixedData[3] = r2c1; - m_fixedData[4] = r2c2; - m_fixedData[5] = r2c3; - m_fixedData[6] = r3c1; - m_fixedData[7] = r3c2; - m_fixedData[8] = r3c3; - } -// using XsMatrix::operator=; -// using XsMatrix::operator[]; + //! \brief Constructs an XsMatrix3x3 from a set of values + XsMatrix3x3(XsReal r1c1, XsReal r1c2, XsReal r1c3, + XsReal r2c1, XsReal r2c2, XsReal r2c3, + XsReal r3c1, XsReal r3c2, XsReal r3c3) : XsMatrix(m_fixedData, 3, 3, 3, XSDF_FixedSize) + { + m_fixedData[0] = r1c1; + m_fixedData[1] = r1c2; + m_fixedData[2] = r1c3; + m_fixedData[3] = r2c1; + m_fixedData[4] = r2c2; + m_fixedData[5] = r2c3; + m_fixedData[6] = r3c1; + m_fixedData[7] = r3c2; + m_fixedData[8] = r3c3; + } + // using XsMatrix::operator=; + // using XsMatrix::operator[]; #endif -}; + }; #if 0 } // for Qt lupdate parser #endif diff --git a/lib/xspublic/xstypes/xsmessage.c b/lib/xspublic/xstypes/xsmessage.c index 48b3f82..c13be51 100644 --- a/lib/xspublic/xstypes/xsmessage.c +++ b/lib/xspublic/xstypes/xsmessage.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -97,8 +97,8 @@ #define XS_LEN_PRODUCTCODE 20 #define XS_LEN_PROCESSINGFLAGS 2 #define XS_LEN_XMPWROFF 0 -#define XS_LEN_OUTPUTMODE 2 -#define XS_LEN_OUTPUTSETTINGS 4 +#define XS_LEN_OUTPUTMODE 2 +#define XS_LEN_OUTPUTSETTINGS 4 #define XS_LEN_OUTPUTSKIPFACTOR 2 #define XS_LEN_SYNCINMODE 2 #define XS_LEN_SYNCINSKIPFACTOR 2 @@ -112,8 +112,8 @@ #define XS_LEN_OBJECTALIGNMENT 36 #define XS_LEN_XMERRORMODE 2 #define XS_LEN_BUFFERSIZE 2 -#define XS_LEN_HEADING 4 -#define XS_LEN_MAGNETICFIELD 12 +#define XS_LEN_HEADING 4 +#define XS_LEN_MAGNETICFIELD 12 #define XS_LEN_LOCATIONID 2 #define XS_LEN_EXTOUTPUTMODE 2 #define XS_LEN_INITTRACKMODE 2 @@ -161,21 +161,26 @@ #pragma pack(push, 1) // little endian -union Itypes { +union Itypes +{ int64_t i64; - struct { - int32_t i1,i0; + struct + { + int32_t i1, i0; } i32; - struct { - int16_t s3,s2,s1,s0; + struct + { + int16_t s3, s2, s1, s0; } i16; - struct { - signed char b7,b6,b5,b4,b3,b2,b1,b0; + struct + { + signed char b7, b6, b5, b4, b3, b2, b1, b0; } i8; double d; - struct { - float f1,f0; + struct + { + float f1, f0; } f32; }; #pragma pack(pop) @@ -201,34 +206,34 @@ uint8_t byteSum(const uint8_t* buffer, XsSize count) } /*! \brief Get the buffer at offset \a offset */ -static inline uint8_t *XsMessage_dataAtOffset(XsMessage *thisPtr, XsSize offset) +static inline uint8_t* XsMessage_dataAtOffset(XsMessage* thisPtr, XsSize offset) { XsMessageHeader* hdr; assert(thisPtr->m_message.m_data); assert(offset < XsMessage_dataSize(thisPtr)); - hdr = (XsMessageHeader*) (void*) thisPtr->m_message.m_data; + hdr = (XsMessageHeader*)(void*) thisPtr->m_message.m_data; if (hdr->m_length != 255) return hdr->m_datlen.m_data + offset; return hdr->m_datlen.m_extended.m_data + offset; } /*! \brief return the const data at offset \a offset */ -static inline const uint8_t *XsMessage_cdataAtOffset(XsMessage const *thisPtr, XsSize offset) +static inline const uint8_t* XsMessage_cdataAtOffset(XsMessage const* thisPtr, XsSize offset) { return XsMessage_dataAtOffset((XsMessage*)thisPtr, offset); } /*! \brief Make sure the data buffer is large enough to hold a new data item of \a sizeofValue */ -static inline void XsMessage_ensureDataSize(XsMessage *thisPtr, XsSize offset, XsSize sizeofValue) +static inline void XsMessage_ensureDataSize(XsMessage* thisPtr, XsSize offset, XsSize sizeofValue) { if (XsMessage_dataSize(thisPtr) < offset + sizeofValue) XsMessage_resizeData(thisPtr, offset + sizeofValue); } /*! \brief Update the message checksum with the passed value */ -static inline void XsMessage_updateChecksumWithValue(XsMessage *thisPtr, const void *value, XsSize sizeofValue, XsSize offset) +static inline void XsMessage_updateChecksumWithValue(XsMessage* thisPtr, const void* value, XsSize sizeofValue, XsSize offset) { if (thisPtr->m_autoUpdateChecksum) { @@ -238,36 +243,39 @@ static inline void XsMessage_updateChecksumWithValue(XsMessage *thisPtr, const v } /*! \brief Swap the endianness based on the data size */ -static inline void swapEndian(void *data, const XsSize size) +static inline void swapEndian(void* data, const XsSize size) { - switch (size) - { - case sizeof(char): - break; - case sizeof(uint16_t): - { - uint16_t i16; - memcpy((void*)&i16, data, sizeof(uint16_t)); - i16 = swapEndian16(i16); - memcpy(data, (void*)&i16, sizeof(uint16_t)); - } break; - case sizeof(uint32_t): - { - uint32_t i32; - memcpy((void*)&i32, data, sizeof(uint32_t)); - i32 = swapEndian32(i32); - memcpy(data, (void*)&i32, sizeof(uint32_t)); - } break; - case sizeof(uint64_t): - { - uint64_t i64; - memcpy((void*)&i64, data, sizeof(uint64_t)); - i64 = swapEndian64(i64); - memcpy(data, (void*)&i64, sizeof(uint64_t)); - } break; - default: - assert(0); - } + switch (size) + { + case sizeof(char): + break; + case sizeof(uint16_t): + { + uint16_t i16; + memcpy((void*)&i16, data, sizeof(uint16_t)); + i16 = swapEndian16(i16); + memcpy(data, (void*)&i16, sizeof(uint16_t)); + break; + } + case sizeof(uint32_t): + { + uint32_t i32; + memcpy((void*)&i32, data, sizeof(uint32_t)); + i32 = swapEndian32(i32); + memcpy(data, (void*)&i32, sizeof(uint32_t)); + break; + } + case sizeof(uint64_t): + { + uint64_t i64; + memcpy((void*)&i64, data, sizeof(uint64_t)); + i64 = swapEndian64(i64); + memcpy(data, (void*)&i64, sizeof(uint64_t)); + break; + } + default: + assert(0); + } } /*! \brief Get data of size \a size at \a offset, and put it byteswapped into \a value @@ -275,7 +283,7 @@ static inline void swapEndian(void *data, const XsSize size) \param size The size of the message \param offset The offset of the message */ -void XsMessage_getEndianCorrectData(XsMessage const* thisPtr, void *value, XsSize size, XsSize offset) +void XsMessage_getEndianCorrectData(XsMessage const* thisPtr, void* value, XsSize size, XsSize offset) { memcpy(value, (void const*) XsMessage_cdataAtOffset(thisPtr, offset), size); swapEndian(value, size); @@ -286,7 +294,7 @@ void XsMessage_getEndianCorrectData(XsMessage const* thisPtr, void *value, XsSiz \param size The size of the message \param offset The offset of the message */ -void XsMessage_setEndianCorrectData(XsMessage *thisPtr, void const *value, XsSize size, XsSize offset) +void XsMessage_setEndianCorrectData(XsMessage* thisPtr, void const* value, XsSize size, XsSize offset) { void* dest; XsMessage_ensureDataSize(thisPtr, offset, size); @@ -312,7 +320,7 @@ void XsMessage_constructSized(XsMessage* thisPtr, XsSize dataSize) XsByteArray_construct(&thisPtr->m_message, msgSize, 0); memset(thisPtr->m_message.m_data, 0, msgSize); - hdr = (XsMessageHeader*) (void*) thisPtr->m_message.m_data; + hdr = (XsMessageHeader*)(void*) thisPtr->m_message.m_data; hdr->m_preamble = XS_PREAMBLE; hdr->m_messageId = 0; hdr->m_busId = XS_BID_MASTER; @@ -321,15 +329,15 @@ void XsMessage_constructSized(XsMessage* thisPtr, XsSize dataSize) { hdr->m_length = (uint8_t) dataSize; *((uint8_t**) &thisPtr->m_checksum) = &hdr->m_datlen.m_data[dataSize]; - thisPtr->m_checksum[0] = (uint8_t)-(int8_t)(uint8_t)dataSize; + thisPtr->m_checksum[0] = (uint8_t) - (int8_t)(uint8_t)dataSize; } else { hdr->m_length = XS_EXTLENCODE; - hdr->m_datlen.m_extended.m_length.m_high = (uint8_t) (dataSize >> 8); + hdr->m_datlen.m_extended.m_length.m_high = (uint8_t)(dataSize >> 8); hdr->m_datlen.m_extended.m_length.m_low = (uint8_t) dataSize; *((uint8_t**) &thisPtr->m_checksum) = &hdr->m_datlen.m_extended.m_data[dataSize]; - thisPtr->m_checksum[0] = (uint8_t) -(hdr->m_datlen.m_extended.m_length.m_high + hdr->m_datlen.m_extended.m_length.m_low + XS_EXTLENCODE); + thisPtr->m_checksum[0] = (uint8_t) - (hdr->m_datlen.m_extended.m_length.m_high + hdr->m_datlen.m_extended.m_length.m_low + XS_EXTLENCODE); } thisPtr->m_checksum[0] -= hdr->m_busId; } @@ -354,7 +362,7 @@ void XsMessage_copyConstruct(XsMessage* thisPtr, XsMessage const* src) XsSize dataSize; XsArray_copyConstruct(&thisPtr->m_message, &src->m_message); thisPtr->m_autoUpdateChecksum = src->m_autoUpdateChecksum; - hdr = (XsMessageHeader*) (void*) thisPtr->m_message.m_data; + hdr = (XsMessageHeader*)(void*) thisPtr->m_message.m_data; dataSize = XsMessage_dataSize(thisPtr); if (dataSize >= 255) *((uint8_t**) &thisPtr->m_checksum) = &hdr->m_datlen.m_extended.m_data[dataSize]; @@ -380,7 +388,7 @@ void XsMessage_assign(XsMessage* thisPtr, XsSize dataSize) void XsMessage_load(XsMessage* thisPtr, XsSize msgSize, unsigned char const* src) { XsByteArray_construct(&thisPtr->m_message, msgSize, src); - *((uint8_t**) &thisPtr->m_checksum) = &thisPtr->m_message.m_data[XsMessage_getTotalMessageSize(thisPtr)-1]; + *((uint8_t**) &thisPtr->m_checksum) = &thisPtr->m_message.m_data[XsMessage_getTotalMessageSize(thisPtr) - 1]; } /*! \brief This function clears the data in the message @@ -398,7 +406,7 @@ void XsMessage_destruct(XsMessage* thisPtr) void XsMessage_copy(XsMessage* copy, XsMessage const* thisPtr) { XsArray_copy(©->m_message, &thisPtr->m_message); - *((uint8_t**) ©->m_checksum) = ©->m_message.m_data[XsMessage_getTotalMessageSize(copy)-1]; + *((uint8_t**) ©->m_checksum) = ©->m_message.m_data[XsMessage_getTotalMessageSize(copy) - 1]; copy->m_autoUpdateChecksum = thisPtr->m_autoUpdateChecksum; } @@ -412,7 +420,7 @@ XsSize XsMessage_dataSize(XsMessage const* thisPtr) if (!thisPtr->m_message.m_data) return 0; - hdr = (XsMessageHeader const*) (void const*) thisPtr->m_message.m_data; + hdr = (XsMessageHeader const*)(void const*) thisPtr->m_message.m_data; if (hdr->m_length == 255) return (((XsSize) hdr->m_datlen.m_extended.m_length.m_high) << 8) + hdr->m_datlen.m_extended.m_length.m_low; else @@ -457,7 +465,7 @@ XsSize XsMessage_getTotalMessageSize(XsMessage const* thisPtr) if (!thisPtr->m_message.m_data) return 0; - hdr = (XsMessageHeader*) (void*) thisPtr->m_message.m_data; + hdr = (XsMessageHeader*)(void*) thisPtr->m_message.m_data; if (hdr->m_length == 255) return (((XsSize) hdr->m_datlen.m_extended.m_length.m_high) << 8) + hdr->m_datlen.m_extended.m_length.m_low + 7; else @@ -550,7 +558,7 @@ double XsMessage_getDataF1220(XsMessage const* thisPtr, XsSize offset) Itypes rv; tmp = (int32_t) XsMessage_getDataLong(thisPtr, offset); - rv.d = ((double) tmp)/1048576.0; + rv.d = ((double) tmp) / 1048576.0; rv.i64 = (rv.i64 & ~1LL) | (tmp & 1); return rv.d; } @@ -567,7 +575,7 @@ double XsMessage_getDataFP1632(XsMessage const* thisPtr, XsSize offset) Itypes fp, rv; fpfrac = (int32_t) XsMessage_getDataLong(thisPtr, offset); - fpint = (int16_t) XsMessage_getDataShort(thisPtr, offset+4); + fpint = (int16_t) XsMessage_getDataShort(thisPtr, offset + 4); fp.i32.i0 = fpint; fp.i32.i1 = fpfrac; @@ -661,7 +669,7 @@ void XsMessage_setDataF1220(XsMessage* thisPtr, double value, XsSize offset) uint32_t val; fp.d = value; - val = (uint32_t) (int32_t) (value*1048576.0); + val = (uint32_t)(int32_t)(value * 1048576.0); XsMessage_setDataLong(thisPtr, (val & ~1UL) | (fp.i64 & 1), offset); } @@ -680,7 +688,7 @@ void XsMessage_setDataFP1632(XsMessage* thisPtr, double value, XsSize offset) uint32_t b; fp.d = value; - b = (uint32_t) (fp.i64 & 1); + b = (uint32_t)(fp.i64 & 1); dexp = ((fp.i32.i0 & (0x7fffffffL)) >> 20) - 1023; if (dexp <= 14) @@ -689,7 +697,7 @@ void XsMessage_setDataFP1632(XsMessage* thisPtr, double value, XsSize offset) if (value < 0) fp.i64 = -fp.i64; if (dexp > -32) - fp.i64 = fp.i64 >> (20-dexp); // 52-32 - exponent + fp.i64 = fp.i64 >> (20 - dexp); // 52-32 - exponent else fp.i64 = fp.i64 >> 52; // this could be optimized? fpint = fp.i16.s1; @@ -699,7 +707,7 @@ void XsMessage_setDataFP1632(XsMessage* thisPtr, double value, XsSize offset) { if (value < 0) { - fpint = ((int16_t) (uint16_t) (0x8000)); + fpint = ((int16_t)(uint16_t)(0x8000)); fpfrac = 0; } else @@ -710,7 +718,7 @@ void XsMessage_setDataFP1632(XsMessage* thisPtr, double value, XsSize offset) } XsMessage_setDataLong(thisPtr, (fpfrac & ~1L) | b, offset); - XsMessage_setDataShort(thisPtr, (uint16_t) fpint, offset+(XsSize)4); + XsMessage_setDataShort(thisPtr, (uint16_t) fpint, offset + (XsSize)4); } /*! \brief Puts \a size number of bytes from \a buffer into the message at \a offset @@ -787,36 +795,36 @@ static float convertToFloat(double d) \param offset offset in the data buffer from where to start reading. \param numValues number of values to be read */ -void XsMessage_getDataFPValuesById(XsMessage const* thisPtr, XsDataIdentifier dataIdentifier, double *dest, XsSize offset, XsSize numValues) +void XsMessage_getDataFPValuesById(XsMessage const* thisPtr, XsDataIdentifier dataIdentifier, double* dest, XsSize offset, XsSize numValues) { XsSize i; - for (i=0; im_message.m_data[i]; @@ -997,13 +1005,13 @@ int XsMessage_isChecksumOk(XsMessage const* thisPtr) */ XsMessageHeader* XsMessage_getHeader(XsMessage* thisPtr) { - return (XsMessageHeader*) (void*) thisPtr->m_message.m_data; + return (XsMessageHeader*)(void*) thisPtr->m_message.m_data; } /*! \brief \returns a const pointer to the message header */ const XsMessageHeader* XsMessage_getConstHeader(XsMessage const* thisPtr) { - return (const XsMessageHeader*) (void*) thisPtr->m_message.m_data; + return (const XsMessageHeader*)(void*) thisPtr->m_message.m_data; } /*! \brief Test if this message is empty @@ -1011,7 +1019,7 @@ const XsMessageHeader* XsMessage_getConstHeader(XsMessage const* thisPtr) */ int XsMessage_empty(XsMessage const* thisPtr) { - XsMessageHeader* hdr = (XsMessageHeader*) (void*) thisPtr->m_message.m_data; + XsMessageHeader* hdr = (XsMessageHeader*)(void*) thisPtr->m_message.m_data; if (!hdr) return 1; return (hdr->m_messageId == 0 && hdr->m_busId == XS_BID_MASTER); @@ -1083,7 +1091,7 @@ void XsMessage_setBusId(XsMessage* thisPtr, uint8_t busId) if (!thisPtr->m_message.m_data) XsMessage_construct(thisPtr); - hdr = (XsMessageHeader*) (void*) thisPtr->m_message.m_data; + hdr = (XsMessageHeader*)(void*) thisPtr->m_message.m_data; if (thisPtr->m_autoUpdateChecksum) thisPtr->m_checksum[0] += hdr->m_busId; hdr->m_busId = busId; @@ -1101,7 +1109,7 @@ void XsMessage_setMessageId(XsMessage* thisPtr, enum XsXbusMessageId msgId) if (!thisPtr->m_message.m_data) XsMessage_construct(thisPtr); - hdr = (XsMessageHeader*) (void*) thisPtr->m_message.m_data; + hdr = (XsMessageHeader*)(void*) thisPtr->m_message.m_data; if (thisPtr->m_autoUpdateChecksum) thisPtr->m_checksum[0] += hdr->m_messageId; hdr->m_messageId = (uint8_t) msgId; @@ -1125,10 +1133,10 @@ void XsMessage_insertData(XsMessage* thisPtr, XsSize count, XsSize offset) return; oldSize = XsMessage_dataSize(thisPtr); - newSize = oldSize+count; + newSize = oldSize + count; - if (newSize < offset+count) - newSize = offset+count; + if (newSize < offset + count) + newSize = offset + count; XsArray_swap(&thisPtr->m_message, &old); XsMessage_constructSized(thisPtr, newSize); @@ -1165,7 +1173,7 @@ void XsMessage_insertData(XsMessage* thisPtr, XsSize count, XsSize offset) newData[index] = 0; } for (; index < oldSize; ++index) - newData[index+count] = oldData[index]; + newData[index + count] = oldData[index]; if (thisPtr->m_autoUpdateChecksum) thisPtr->m_checksum[0] -= byteSum(oldData, oldSize); @@ -1194,7 +1202,7 @@ void XsMessage_deleteData(XsMessage* thisPtr, XsSize count, XsSize offset) XsMessage_resizeData(thisPtr, offset); return; } - newSize = oldSize-count; + newSize = oldSize - count; XsArray_swap(&thisPtr->m_message, &old); XsMessage_constructSized(thisPtr, newSize); @@ -1221,7 +1229,7 @@ void XsMessage_deleteData(XsMessage* thisPtr, XsSize count, XsSize offset) for (index = 0; index < offset; ++index) newData[index] = oldData[index]; for (; index < newSize; ++index) - newData[index] = oldData[index+count]; + newData[index] = oldData[index + count]; if (thisPtr->m_autoUpdateChecksum) thisPtr->m_checksum[0] -= byteSum(newData, newSize); @@ -1276,11 +1284,11 @@ void XsMessage_toHexString(XsMessage const* thisPtr, XsSize maxBytes, XsString* if (maxBytes) { - XsString_resize(resultValue, maxBytes*3-1); + XsString_resize(resultValue, maxBytes * 3 - 1); s = (char*) resultValue->m_data; - for (i = 0; i < maxBytes-1; ++i) - sprintf(s+(i*3), "%02X ", (unsigned int) ((uint8_t const*) thisPtr->m_message.m_data)[i]); - sprintf(s+((maxBytes-1)*3), "%02X", (unsigned int) ((uint8_t const*) thisPtr->m_message.m_data)[maxBytes-1]); + for (i = 0; i < maxBytes - 1; ++i) + sprintf(s + (i * 3), "%02X ", (unsigned int)((uint8_t const*) thisPtr->m_message.m_data)[i]); + sprintf(s + ((maxBytes - 1) * 3), "%02X", (unsigned int)((uint8_t const*) thisPtr->m_message.m_data)[maxBytes - 1]); } else XsString_resize(resultValue, 0); diff --git a/lib/xspublic/xstypes/xsmessage.h b/lib/xspublic/xstypes/xsmessage.h index 837eb03..1af9549 100644 --- a/lib/xspublic/xstypes/xsmessage.h +++ b/lib/xspublic/xstypes/xsmessage.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -119,7 +119,7 @@ XSTYPES_DLL_API void XsMessage_setDataBuffer(XsMessage* thisPtr, const uint8_t* XSTYPES_DLL_API uint8_t XsMessage_computeChecksum(XsMessage const* thisPtr); XSTYPES_DLL_API void XsMessage_recomputeChecksum(XsMessage* thisPtr); XSTYPES_DLL_API int XsMessage_isChecksumOk(XsMessage const* thisPtr); -XSTYPES_DLL_API XsMessageHeader* XsMessage_getHeader(XsMessage* ); +XSTYPES_DLL_API XsMessageHeader* XsMessage_getHeader(XsMessage*); XSTYPES_DLL_API const XsMessageHeader* XsMessage_getConstHeader(XsMessage const* thisPtr); XSTYPES_DLL_API int XsMessage_empty(XsMessage const* thisPtr); XSTYPES_DLL_API void XsMessage_resizeData(XsMessage* thisPtr, XsSize newSize); @@ -128,14 +128,14 @@ XSTYPES_DLL_API void XsMessage_setMessageId(XsMessage* thisPtr, XsXbusMessageId XSTYPES_DLL_API void XsMessage_insertData(XsMessage* thisPtr, XsSize count, XsSize offset); XSTYPES_DLL_API void XsMessage_deleteData(XsMessage* thisPtr, XsSize count, XsSize offset); XSTYPES_DLL_API uint8_t XsMessage_getFPValueSize(XsDataIdentifier id); -XSTYPES_DLL_API void XsMessage_getDataFPValuesById(XsMessage const* thisPtr, XsDataIdentifier dataIdentifier, double *dest, XsSize offset, XsSize numValues); -XSTYPES_DLL_API void XsMessage_setDataFPValuesById(XsMessage* thisPtr, XsDataIdentifier dataIdentifier, double const *data, XsSize offset, XsSize numValues); -XSTYPES_DLL_API void XsMessage_getDataRealValuesById(XsMessage const* thisPtr, XsDataIdentifier dataIdentifier, XsReal *dest, XsSize offset, XsSize numValues); -XSTYPES_DLL_API void XsMessage_setDataRealValuesById(XsMessage* thisPtr, XsDataIdentifier dataIdentifier, XsReal const *data, XsSize offset, XsSize numValues); +XSTYPES_DLL_API void XsMessage_getDataFPValuesById(XsMessage const* thisPtr, XsDataIdentifier dataIdentifier, double* dest, XsSize offset, XsSize numValues); +XSTYPES_DLL_API void XsMessage_setDataFPValuesById(XsMessage* thisPtr, XsDataIdentifier dataIdentifier, double const* data, XsSize offset, XsSize numValues); +XSTYPES_DLL_API void XsMessage_getDataRealValuesById(XsMessage const* thisPtr, XsDataIdentifier dataIdentifier, XsReal* dest, XsSize offset, XsSize numValues); +XSTYPES_DLL_API void XsMessage_setDataRealValuesById(XsMessage* thisPtr, XsDataIdentifier dataIdentifier, XsReal const* data, XsSize offset, XsSize numValues); XSTYPES_DLL_API int XsMessage_compare(XsMessage const* a, XsMessage const* b); XSTYPES_DLL_API void XsMessage_toHexString(XsMessage const* thisPtr, XsSize maxBytes, XsString* resultValue); -XSTYPES_DLL_API void XsMessage_getEndianCorrectData(XsMessage const* thisPtr, void *value, XsSize size, XsSize offset); -XSTYPES_DLL_API void XsMessage_setEndianCorrectData(XsMessage *thisPtr, void const *value, XsSize size, XsSize offset); +XSTYPES_DLL_API void XsMessage_getEndianCorrectData(XsMessage const* thisPtr, void* value, XsSize size, XsSize offset); +XSTYPES_DLL_API void XsMessage_setEndianCorrectData(XsMessage* thisPtr, void const* value, XsSize size, XsSize offset); #ifdef __cplusplus } // extern "C" @@ -162,31 +162,26 @@ XSTYPES_DLL_API void XsMessage_setEndianCorrectData(XsMessage *thisPtr, void con ////////////////////////////////////////////////////////////////////////////////////////// // different alignment commands for gcc / MSVS, the structure needs to be 1-byte aligned. -#ifdef _MSC_VER - #pragma pack(push, 1) - #ifndef PACK_POST - #define PACK_POST - #endif -#else - #ifndef PACK_POST - #define PACK_POST __attribute__((__packed__)) - #endif -#endif +XS_PACKED_STRUCT_START /*! \brief A message header \details This structure is used to interpret the header of a message. */ -struct XsMessageHeader { +struct XsMessageHeader +{ uint8_t m_preamble; //!< \brief The message preamble (always 0xFA) uint8_t m_busId; //!< \brief The bus ID \sa XS_BID_MASTER XS_BID_BROADCAST XS_BID_MT uint8_t m_messageId; //!< \brief The message ID \sa XsXbusMessageId uint8_t m_length; //!< \brief The length of the message \details A length of 255 means extended length is used /*! Contains optional extended length of message and first byte of data buffer */ - union LengthData { + union LengthData + { /*! Contains extended length information and first byte of data buffer if normal length is 255 */ - struct ExtendedLength { + struct ExtendedLength + { /*! The high and low byte of the extended length */ - struct ExtendedParts { + struct ExtendedParts + { uint8_t m_high; //!< \brief High byte of extended length uint8_t m_low; //!< \brief Low byte of extended length } m_length; //!< \brief Extended length, only valid if normal length is 255 @@ -197,16 +192,15 @@ struct XsMessageHeader { #ifdef SWIG }; #else -} PACK_POST; +} XS_PACKED_STRUCT; #endif -#ifdef _MSC_VER - #pragma pack(pop) -#endif +XS_PACKED_STRUCT_END ////////////////////////////////////////////////////////////////////////////////////////// //! \brief Structure for storing a single message. -struct XsMessage { +struct XsMessage +{ #ifdef __cplusplus /*! \brief Create a XsMessage object with the given data length and message Id. @@ -250,21 +244,21 @@ struct XsMessage { : m_autoUpdateChecksum(1) , m_checksum(0) { - XsSize szm = source.size()/2; + XsSize szm = source.size() / 2; XsSize szmcc = szm + (computeChecksum ? 1 : 0); XsByteArray tmp(szmcc); auto tonibble = [](char a) -> uint8_t { if (a >= '0' && a <= '9') - return (uint8_t) (a-'0'); + return (uint8_t)(a - '0'); if (a >= 'a' && a <= 'f') - return (uint8_t) (10+a-'a'); + return (uint8_t)(10 + a - 'a'); if (a >= 'A' && a <= 'F') - return (uint8_t) (10+a-'A'); + return (uint8_t)(10 + a - 'A'); return 0; }; for (XsSize i = 0; i < szm; ++i) - tmp[i] = tonibble(source[i*2])*(uint8_t)16+tonibble(source[i*2+1]); //lint !e734 + tmp[i] = tonibble(source[i * 2]) * (uint8_t)16 + tonibble(source[i * 2 + 1]); //lint !e734 XsMessage_load(this, tmp.size(), tmp.data()); if (computeChecksum) XsMessage_recomputeChecksum(this); @@ -480,7 +474,7 @@ struct XsMessage { /*! \copydoc XsMessage_setDataDouble */ - inline void setDataDouble(const double value, XsSize offset=0) + inline void setDataDouble(const double value, XsSize offset = 0) { XsMessage_setDataDouble(this, value, offset); } @@ -561,7 +555,7 @@ struct XsMessage { } /*! \copydoc XsMessage_getDataFPValuesById */ - inline void getDataFPValue(XsDataIdentifier dataIdentifier, double *dest, XsSize offset = 0, XsSize numValues = 1) const + inline void getDataFPValue(XsDataIdentifier dataIdentifier, double* dest, XsSize offset = 0, XsSize numValues = 1) const { XsMessage_getDataFPValuesById(this, dataIdentifier, dest, offset, numValues); } @@ -581,7 +575,7 @@ struct XsMessage { } /*! \copydoc XsMessage_setDataFPValuesById */ - inline void setDataFPValue(XsDataIdentifier dataIdentifier, const double *data, XsSize offset = 0, XsSize numValues = 1) + inline void setDataFPValue(XsDataIdentifier dataIdentifier, const double* data, XsSize offset = 0, XsSize numValues = 1) { XsMessage_setDataFPValuesById(this, dataIdentifier, data, offset, numValues); } @@ -634,7 +628,7 @@ struct XsMessage { { (void) id; for (int i = 0; i < numValues; ++i) - XsMessage_getEndianCorrectData(this, &data[i], sizeof(T), offset+((unsigned int)i)*sizeof(T)); + XsMessage_getEndianCorrectData(this, &data[i], sizeof(T), offset + ((unsigned int)i)*sizeof(T)); } /*! \brief Write data of type T to the message @@ -649,7 +643,7 @@ struct XsMessage { { (void) id; for (int i = 0; i < numValues; ++i) - XsMessage_setEndianCorrectData(this, &data[i], sizeof(T), offset+((unsigned int)i)*sizeof(T)); + XsMessage_setEndianCorrectData(this, &data[i], sizeof(T), offset + ((unsigned int)i)*sizeof(T)); } /*! \brief Return the number of bytes that \a numValues items of type T will require in a message @@ -670,7 +664,7 @@ struct XsMessage { { XsSize sz = XsMessage_getTotalMessageSize(this); if (sz) - *const_cast(&m_checksum) = &m_message[sz-1]; + *const_cast(&m_checksum) = &m_message[sz - 1]; else *const_cast(&m_checksum) = 0; } @@ -724,8 +718,8 @@ inline int XsMessage::sizeInMsg(XsDataIdentifier id, int numValues) #endif // some macros to help when constructing/parsing messages -#define swapEndian16(src) (((uint16_t)(src) >> 8) | ((uint16_t)(src) << 8)) -#define swapEndian32(src) (((uint32_t)(src) >> 24) | (((uint32_t)(src) >> 8) & 0xFF00) | (((uint32_t)(src) << 8) & 0xFF0000) | ((uint32_t)(src) << 24)) -#define swapEndian64(src) (((src >> 56) & 0xFFULL) | ((src >> 40) & 0xFF00ULL) | ((src >> 24) & 0xFF0000ULL) | ((src >> 8) & 0xFF000000ULL) | ((src << 8) & 0xFF00000000ULL) | ((src << 24) & 0xFF0000000000ULL) | ((src << 40) & 0xFF000000000000ULL) | ((src << 56))) +#define swapEndian16(src) (uint16_t)(((uint16_t)(src) >> 8) | ((uint16_t)(src) << 8)) +#define swapEndian32(src) (uint32_t)(((uint32_t)(src) >> 24) | (((uint32_t)(src) >> 8) & 0xFF00) | (((uint32_t)(src) << 8) & 0xFF0000) | ((uint32_t)(src) << 24)) +#define swapEndian64(src) (uint64_t)(((src >> 56) & 0xFFULL) | ((src >> 40) & 0xFF00ULL) | ((src >> 24) & 0xFF0000ULL) | ((src >> 8) & 0xFF000000ULL) | ((src << 8) & 0xFF00000000ULL) | ((src << 24) & 0xFF0000000000ULL) | ((src << 40) & 0xFF000000000000ULL) | ((src << 56))) #endif diff --git a/lib/xspublic/xstypes/xsmessagearray.c b/lib/xspublic/xstypes/xsmessagearray.c index d4b94d2..e1e23d6 100644 --- a/lib/xspublic/xstypes/xsmessagearray.c +++ b/lib/xspublic/xstypes/xsmessagearray.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -71,7 +71,8 @@ */ //! \brief Descriptor for XsMessageArray -XsArrayDescriptor const g_xsMessageArrayDescriptor = { +XsArrayDescriptor const g_xsMessageArrayDescriptor = +{ sizeof(XsMessage), XSEXPCASTITEMSWAP XsMessage_swap, XSEXPCASTITEMMAKE XsMessage_construct, diff --git a/lib/xspublic/xstypes/xsmessagearray.h b/lib/xspublic/xstypes/xsmessagearray.h index 828b202..c74e0b2 100644 --- a/lib/xspublic/xstypes/xsmessagearray.h +++ b/lib/xspublic/xstypes/xsmessagearray.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -88,16 +88,17 @@ XSTYPES_DLL_API void XsMessageArray_construct(XsMessageArray* thisPtr, XsSize co #ifdef __cplusplus } // extern "C" -struct XsMessageArray : public XsArrayImpl { +struct XsMessageArray : public XsArrayImpl +{ //! \brief Constructs an XsMessageArray inline explicit XsMessageArray(XsSize sz = 0, XsMessage const* src = 0) - : ArrayImpl(sz, src) + : ArrayImpl(sz, src) { } //! \brief Constructs an XsMessageArray as a copy of \a other inline XsMessageArray(XsMessageArray const& other) - : ArrayImpl(other) + : ArrayImpl(other) { } diff --git a/lib/xspublic/xstypes/xsmfmresultvalue.h b/lib/xspublic/xstypes/xsmfmresultvalue.h index 37c5bbe..9e34f5c 100644 --- a/lib/xspublic/xstypes/xsmfmresultvalue.h +++ b/lib/xspublic/xstypes/xsmfmresultvalue.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -71,7 +71,10 @@ enum XsMfmResultValue MRV_Bad, MRV_Acceptable, MRV_Good, - MRV_Processing + MRV_Processing, + MRV_Skipped, + + MRV_Unknown // initial value }; -#endif \ No newline at end of file +#endif diff --git a/lib/xspublic/xstypes/xsoption.h b/lib/xspublic/xstypes/xsoption.h index c132873..863817f 100644 --- a/lib/xspublic/xstypes/xsoption.h +++ b/lib/xspublic/xstypes/xsoption.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -73,8 +73,9 @@ available other data. XsOptions can be logically ORed together */ //AUTO namespace xstypes { -enum XsOption { - XSO_None = 0 //!< No option +enum XsOption +{ + XSO_None = 0 //!< No option , XSO_Calibrate = 0x0001 //!< Compute calibrated inertial data from raw data and temperature , XSO_Orientation = 0x0002 //!< Compute orientation, the orientation is typically only computed in one stream. If not specified the system will decide: when reading from file it will use XSO_OrientationInBufferedStream, otherwise XSO_OrientationInLiveStream. @@ -90,9 +91,8 @@ enum XsOption { , XSO_SkipDataBundling = 0x2000 //!< When set, the onAll...DataAvailable callbacks will not be called by the master device. This prevents some internal buffering. , XSO_ExpectNoRetransmissionsInFile = 0x4000 //!< When set and reading a file, missing data is immediately treated as unavailable. The default behaviour is to read further in the file to see if the data was retransmitted. , XSO_Reserved = 0x8000 //!< Reserved for internal use - - , XSO_All = 0xFFFF //!< All options, note that setting 'all options' is not valid, but it is useful for clearing all options }; +#define XSO_All ((XsOption)0xFFFF) //!< All options, note that setting 'all options' is not valid, but it is useful for clearing all options /*! @} */ typedef enum XsOption XsOption; @@ -103,22 +103,22 @@ typedef enum XsOption XsOption; //! \brief Logical OR operator for XsOption values inline XsOption operator | (XsOption a, XsOption b) { - return (XsOption) ((int)a | (int)b); + return (XsOption)((int)a | (int)b); } //! \brief Logical AND operator for XsOption values inline XsOption operator & (XsOption a, XsOption b) { - return (XsOption) ((int)a & (int)b); + return (XsOption)((int)a & (int)b); } //! \brief Logical XOR operator for XsOption values inline XsOption operator ^ (XsOption a, XsOption b) { - return (XsOption) ((int)a ^ (int)b); + return (XsOption)((int)a ^ (int)b); } //! \brief Logical NEG operator for XsOption values -inline XsOption operator ~ (XsOption a) +inline XsOption operator ~(XsOption a) { - return (XsOption) (~(int)a); + return (XsOption)(~(int)a); } //! \brief Return the option with mutually exclusive values 'fixed' inline XsOption XsOption_purify(XsOption a) diff --git a/lib/xspublic/xstypes/xsoutputconfiguration.c b/lib/xspublic/xstypes/xsoutputconfiguration.c index 754b2d8..1a503e8 100644 --- a/lib/xspublic/xstypes/xsoutputconfiguration.c +++ b/lib/xspublic/xstypes/xsoutputconfiguration.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xstypes/xsoutputconfiguration.h b/lib/xspublic/xstypes/xsoutputconfiguration.h index 907a441..cfff5c7 100644 --- a/lib/xspublic/xstypes/xsoutputconfiguration.h +++ b/lib/xspublic/xstypes/xsoutputconfiguration.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -93,7 +93,8 @@ XSTYPES_DLL_API void XsOutputConfiguration_swap(struct XsOutputConfiguration* a, itself to its maximum frequency for the data type. If it is 0xFFFF and reported by the device, the data has no maximum frequency, but is sent along with appropriate packets (ie. packet counter) */ -struct XsOutputConfiguration { +struct XsOutputConfiguration +{ XsDataIdentifier m_dataIdentifier; //!< The data identifier uint16_t m_frequency; //!< The frequency diff --git a/lib/xspublic/xstypes/xsoutputconfigurationarray.c b/lib/xspublic/xstypes/xsoutputconfigurationarray.c index e61ba3d..03bc820 100644 --- a/lib/xspublic/xstypes/xsoutputconfigurationarray.c +++ b/lib/xspublic/xstypes/xsoutputconfigurationarray.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -102,7 +102,8 @@ int compareXsOutputConfiguration(XsOutputConfiguration const* a, XsOutputConfigu //! \brief Descriptor for XsOutputConfigurationArray -XsArrayDescriptor const g_xsOutputConfigurationArrayDescriptor = { +XsArrayDescriptor const g_xsOutputConfigurationArrayDescriptor = +{ sizeof(XsOutputConfiguration), XSEXPCASTITEMSWAP swapXsOutputConfiguration, // swap 0, // construct diff --git a/lib/xspublic/xstypes/xsoutputconfigurationarray.h b/lib/xspublic/xstypes/xsoutputconfigurationarray.h index 6853da2..fa85751 100644 --- a/lib/xspublic/xstypes/xsoutputconfigurationarray.h +++ b/lib/xspublic/xstypes/xsoutputconfigurationarray.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -87,16 +87,17 @@ XSTYPES_DLL_API void XsOutputConfigurationArray_construct(XsOutputConfigurationA #ifdef __cplusplus } // extern "C" -struct XsOutputConfigurationArray : public XsArrayImpl { +struct XsOutputConfigurationArray : public XsArrayImpl +{ //! \brief Constructs an XsOutputConfigurationArray inline explicit XsOutputConfigurationArray(XsSize sz = 0, XsOutputConfiguration const* src = 0) - : ArrayImpl(sz, src) + : ArrayImpl(sz, src) { } //! \brief Constructs an XsOutputConfigurationArray as a copy of \a other inline XsOutputConfigurationArray(XsOutputConfigurationArray const& other) - : ArrayImpl(other) + : ArrayImpl(other) { } diff --git a/lib/xspublic/xstypes/xsplatform.h b/lib/xspublic/xstypes/xsplatform.h index 307aa27..b69bcfb 100644 --- a/lib/xspublic/xstypes/xsplatform.h +++ b/lib/xspublic/xstypes/xsplatform.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -68,16 +68,16 @@ #include "xstypesconfig.h" #ifdef _WIN32 -/// microsoft / windows -#include + /// microsoft / windows + #include -#define XsIoHandle HANDLE + #define XsIoHandle HANDLE #else -/// gcc / linux -#include + /// gcc / linux + #include -typedef int32_t XsIoHandle; + typedef int32_t XsIoHandle; #endif diff --git a/lib/xspublic/xstypes/xsportinfo.c b/lib/xspublic/xstypes/xsportinfo.c index 9283340..1aa0bad 100644 --- a/lib/xspublic/xstypes/xsportinfo.c +++ b/lib/xspublic/xstypes/xsportinfo.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -65,7 +65,7 @@ #include "xsportinfo.h" #include #include // strlen -#include // atoi +#include // atoi /*! \class XsPortInfo \brief Contains a descriptor for opening a communication port to an Xsens device. @@ -106,17 +106,17 @@ int XsPortInfo_portNumber(const struct XsPortInfo* thisPtr) if (XsPortInfo_empty(thisPtr)) return 0; - for (i = 0; i < strlen(thisPtr->m_portName); i++) { - if (isdigit(thisPtr->m_portName[i])) { + for (i = 0; i < strlen(thisPtr->m_portName); i++) + { + if (isdigit(thisPtr->m_portName[i])) return atoi(&thisPtr->m_portName[i]); - } } return 0; } /*! \relates XsPortInfo \brief Returns true if this port info object contains a USB device - */ +*/ int XsPortInfo_isUsb(const struct XsPortInfo* thisPtr) { #ifdef XSENS_WINDOWS @@ -127,23 +127,41 @@ int XsPortInfo_isUsb(const struct XsPortInfo* thisPtr) } /*! - * \relates XsPortInfo - * \brief Returns true if this port info object contains a network device - */ + \relates XsPortInfo + \brief Returns true if this port info object contains a bluetooth device +*/ +int XsPortInfo_isBluetooth(const struct XsPortInfo* thisPtr) +{ + return strncmp("BT:", thisPtr->m_portName, 3) == 0; +} + +/*! + \relates XsPortInfo + \brief Returns true if this port info object contains a network device +*/ int XsPortInfo_isNetwork(const struct XsPortInfo* thisPtr) { return strncmp("NET:", thisPtr->m_portName, 4) == 0; } /*! - * \relates XsPortInfo - * \brief Returns the network service name of this port - */ + \relates XsPortInfo + \brief Returns the network service name of this port +*/ const char* XsPortInfo_networkServiceName(const struct XsPortInfo* thisPtr) { return &thisPtr->m_portName[4]; } +/*! + \relates XsPortInfo + \brief Returns the bluetooth address +*/ +const char* XsPortInfo_bluetoothAddress(const struct XsPortInfo* thisPtr) +{ + return &thisPtr->m_portName[3]; +} + /*! \relates XsPortInfo \brief The usb bus \returns Returns the Usb bus number @@ -185,6 +203,7 @@ void XsPortInfo_swap(struct XsPortInfo* a, struct XsPortInfo* b) int i; char c; XsPortLinesOptions pLineOpts; + uint16_t u; XsBaudRate t = a->m_baudrate; a->m_baudrate = b->m_baudrate; @@ -202,6 +221,14 @@ void XsPortInfo_swap(struct XsPortInfo* a, struct XsPortInfo* b) pLineOpts = a->m_linesOptions; a->m_linesOptions = b->m_linesOptions; b->m_linesOptions = pLineOpts; + + u = a->m_vid; + a->m_vid = b->m_vid; + b->m_vid = u; + + u = a->m_pid; + a->m_pid = b->m_pid; + b->m_pid = u; } /*! @} */ diff --git a/lib/xspublic/xstypes/xsportinfo.h b/lib/xspublic/xstypes/xsportinfo.h index c546393..11f9662 100644 --- a/lib/xspublic/xstypes/xsportinfo.h +++ b/lib/xspublic/xstypes/xsportinfo.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -80,17 +80,19 @@ extern "C" { #endif #ifndef __cplusplus - typedef struct XsPortInfo XsPortInfo; +typedef struct XsPortInfo XsPortInfo; #else - struct XsPortInfo; +struct XsPortInfo; #endif XSTYPES_DLL_API void XsPortInfo_clear(XsPortInfo* thisPtr); XSTYPES_DLL_API int XsPortInfo_empty(XsPortInfo const* thisPtr); XSTYPES_DLL_API int XsPortInfo_portNumber(XsPortInfo const* thisPtr); XSTYPES_DLL_API int XsPortInfo_isUsb(XsPortInfo const* thisPtr); +XSTYPES_DLL_API int XsPortInfo_isBluetooth(XsPortInfo const* thisPtr); XSTYPES_DLL_API int XsPortInfo_isNetwork(XsPortInfo const* thisPtr); XSTYPES_DLL_API const char* XsPortInfo_networkServiceName(XsPortInfo const* thisPtr); +XSTYPES_DLL_API const char* XsPortInfo_bluetoothAddress(XsPortInfo const* thisPtr); XSTYPES_DLL_API int XsPortInfo_usbBus(XsPortInfo const* thisPtr); XSTYPES_DLL_API int XsPortInfo_usbAddress(XsPortInfo const* thisPtr); XSTYPES_DLL_API void XsPortInfo_swap(XsPortInfo* a, struct XsPortInfo* b); @@ -100,8 +102,8 @@ XSTYPES_DLL_API void XsPortInfo_swap(XsPortInfo* a, struct XsPortInfo* b); #endif #if defined(_MSC_VER) -#pragma warning(push) -#pragma warning(disable : 4996) + #pragma warning(push) + #pragma warning(disable : 4996) #endif typedef enum XsPortLinesOptions @@ -123,7 +125,8 @@ typedef enum XsPortLinesOptions XPLO_RtsCtsFlowControl = (1 << 6) } XsPortLinesOptions; -struct XsPortInfo { +struct XsPortInfo +{ #ifdef __cplusplus /*! \brief Default constructor, creates an empty port info object */ XsPortInfo() @@ -133,7 +136,7 @@ struct XsPortInfo { , m_vid(0) , m_pid(0) { - m_portName[0] = '\0'; + memset(m_portName, '\0', 256); } /*! \brief Named constructor, initializes the object to the supplied \a portname and optional \a baudRate @@ -148,10 +151,9 @@ struct XsPortInfo { , m_vid(0) , m_pid(0) { + memset(m_portName, '\0', 256); if (portname.size() < 255) strcpy(m_portName, portname.c_str()); - else - m_portName[0] = '\0'; } #ifndef XSENS_NO_PORT_NUMBERS @@ -168,6 +170,7 @@ struct XsPortInfo { , m_vid(0) , m_pid(0) { + memset(m_portName, 0, 256); sprintf(m_portName, "COM%d", portNr); } #endif @@ -185,16 +188,28 @@ struct XsPortInfo { } /*! \brief greater than operator, used for sorting the list. */ - inline bool operator > (const XsPortInfo& p) const { return strcmp(m_portName, p.m_portName) > 0; } + inline bool operator > (const XsPortInfo& p) const + { + return strcmp(m_portName, p.m_portName) > 0; + } /*! \brief less than operator, used for sorting the list. */ - inline bool operator < (const XsPortInfo& p) const { return strcmp(m_portName, p.m_portName) < 0; } + inline bool operator < (const XsPortInfo& p) const + { + return strcmp(m_portName, p.m_portName) < 0; + } /*! \brief equality operator, used for finding items in a list. */ - inline bool operator == (const XsPortInfo& p) const { return strcmp(m_portName, p.m_portName) == 0; } + inline bool operator == (const XsPortInfo& p) const + { + return strcmp(m_portName, p.m_portName) == 0; + } /*! \brief equality operator, used for finding items in a list. */ - inline bool operator == (const char *port) const { return strcmp(m_portName, port) == 0; } + inline bool operator == (const char* port) const + { + return strcmp(m_portName, port) == 0; + } /*! \copydoc XsPortInfo_portNumber */ inline int portNumber() const @@ -229,6 +244,12 @@ struct XsPortInfo { return XsPortInfo_isUsb(this) != 0; } + /*! \brief \copybrief XsPortInfo_isBluetooth */ + inline bool isBluetooth() const + { + return XsPortInfo_isBluetooth(this) != 0; + } + /*! \brief \copybrief XsPortInfo_isNetwork */ inline bool isNetwork() const { @@ -241,6 +262,12 @@ struct XsPortInfo { return XsString(XsPortInfo_networkServiceName(this)); } + /*! \copydoc XsPortInfo_bluetoothAddress */ + inline XsString bluetoothAddress() const + { + return XsString(XsPortInfo_bluetoothAddress(this)); + } + /*! \copydoc XsPortInfo_usbBus */ inline int usbBus() const { @@ -322,25 +349,25 @@ struct XsPortInfo { }; #if defined(_MSC_VER) -#pragma warning(pop) + #pragma warning(pop) #endif #if defined(__cplusplus) && !defined(XSENS_NO_STL) #include namespace std { - template - basic_ostream<_CharT, _Traits>& operator<<(basic_ostream<_CharT, _Traits>& o, XsPortInfo const& xpi) - { - if (xpi.isUsb()) - o << "usb "; - o << "port " << xpi.portName(); - if (xpi.baudrate() != XBR_Invalid) - o << " at " << xpi.baudrate() << " bps"; - if (xpi.deviceId() != 0) - o << " (" << xpi.deviceId() << ")"; - return o; - } +template +basic_ostream<_CharT, _Traits>& operator<<(basic_ostream<_CharT, _Traits>& o, XsPortInfo const& xpi) +{ + if (xpi.isUsb()) + o << "usb "; + o << "port " << xpi.portName(); + if (xpi.baudrate() != XBR_Invalid) + o << " at " << xpi.baudrate() << " bps"; + if (xpi.deviceId() != 0) + o << " (" << xpi.deviceId() << ")"; + return o; +} } #endif diff --git a/lib/xspublic/xstypes/xsportinfoarray.c b/lib/xspublic/xstypes/xsportinfoarray.c index adf6a8e..6c3a3ba 100644 --- a/lib/xspublic/xstypes/xsportinfoarray.c +++ b/lib/xspublic/xstypes/xsportinfoarray.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -76,11 +76,20 @@ void copyPortInfo(XsPortInfo* dest, XsPortInfo const* src) int comparePortInfo(XsPortInfo const* a, XsPortInfo const* b) { - return memcmp(a, b, sizeof(XsPortInfo)); + if ((memcmp(&a->m_deviceId, &b->m_deviceId, sizeof(XsDeviceId)) == 0) && + (memcmp(&a->m_portName, &b->m_portName, sizeof(a->m_portName)) == 0) && //lint !e545 Taking this address is no problem here + (a->m_baudrate == b->m_baudrate) && + (a->m_linesOptions == b->m_linesOptions) && + (a->m_vid == b->m_vid) && + (a->m_pid == b->m_pid)) + return 0; + else + return -1; } //! \brief Descriptor for XsPortInfoArray -XsArrayDescriptor const g_xsPortInfoArrayDescriptor = { +XsArrayDescriptor const g_xsPortInfoArrayDescriptor = +{ sizeof(XsPortInfo), XSEXPCASTITEMSWAP XsPortInfo_swap, // swap XSEXPCASTITEMMAKE XsPortInfo_clear, // construct diff --git a/lib/xspublic/xstypes/xsportinfoarray.h b/lib/xspublic/xstypes/xsportinfoarray.h index 3f31a22..84acacf 100644 --- a/lib/xspublic/xstypes/xsportinfoarray.h +++ b/lib/xspublic/xstypes/xsportinfoarray.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -87,16 +87,17 @@ XSTYPES_DLL_API void XsPortInfoArray_construct(XsPortInfoArray* thisPtr, XsSize #ifdef __cplusplus } // extern "C" -struct XsPortInfoArray : public XsArrayImpl { +struct XsPortInfoArray : public XsArrayImpl +{ //! \brief Constructs an XsPortInfoArray inline explicit XsPortInfoArray(XsSize sz = 0, XsPortInfo const* src = 0) - : ArrayImpl(sz, src) + : ArrayImpl(sz, src) { } //! \brief Constructs an XsPortInfoArray as a copy of \a other inline XsPortInfoArray(XsPortInfoArray const& other) - : ArrayImpl(other) + : ArrayImpl(other) { } diff --git a/lib/xspublic/xstypes/xspressure.h b/lib/xspublic/xstypes/xspressure.h index 6ec94e9..4f7d951 100644 --- a/lib/xspublic/xstypes/xspressure.h +++ b/lib/xspublic/xstypes/xspressure.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -70,15 +70,24 @@ /*! \brief Pressure data. \details Contains the pressure data and the pressure age */ -struct XsPressure { +struct XsPressure +{ #ifdef __cplusplus + /*! \brief Create an empty XsPressure + */ + explicit XsPressure() + : m_pressure(0) + , m_pressureAge(0xFFu) + { + } + /*! \brief Create an XsPressure \param pressure the pressure \param age the pressure age */ - explicit XsPressure(double pressure = 0, uint8_t age = 0) : - m_pressure(pressure), - m_pressureAge(age) + explicit XsPressure(double pressure, uint8_t age = 0) + : m_pressure(pressure) + , m_pressureAge(age) { } @@ -95,7 +104,7 @@ struct XsPressure { \param other the pressure carrier to copy from \return this */ - inline XsPressure const & operator=(XsPressure const& other) + inline XsPressure const& operator=(XsPressure const& other) { m_pressure = other.m_pressure; m_pressureAge = other.m_pressureAge; @@ -110,6 +119,16 @@ struct XsPressure { { return other.m_pressure == m_pressure && other.m_pressureAge == m_pressureAge; } + + /*! \brief Return true if this is not equal to \a other + \param other the pressure carrier to compare against + \return true if both XsPressures differ + */ + inline bool operator!=(XsPressure const& other) const + { + return other.m_pressure != m_pressure || other.m_pressureAge != m_pressureAge; + } + #endif double m_pressure; //!< Pressure in Pascal uint8_t m_pressureAge; //!< Age of pressure data in samples diff --git a/lib/xspublic/xstypes/xsprotocol.h b/lib/xspublic/xstypes/xsprotocol.h index 4e1f1cb..fd0fd8a 100644 --- a/lib/xspublic/xstypes/xsprotocol.h +++ b/lib/xspublic/xstypes/xsprotocol.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -69,10 +69,12 @@ @{ */ //! Protocol types, used for MTi6x0 devices. -enum XsProtocol { +enum XsProtocol +{ XP_None = 0, //!< None XP_Xbus = 1, //!< The Xsens Xbus protocol XP_Nmea = 5, //!< The NMEA protocol + XP_Rtcm = 6, //!< RTCM protocol for RTK }; /*! @} */ typedef enum XsProtocol XsProtocol; diff --git a/lib/xspublic/xstypes/xsquaternion.c b/lib/xspublic/xstypes/xsquaternion.c index fa584c6..e2770ca 100644 --- a/lib/xspublic/xstypes/xsquaternion.c +++ b/lib/xspublic/xstypes/xsquaternion.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -122,8 +122,8 @@ void XsQuaternion_inverse(const XsQuaternion* thisPtr, XsQuaternion* dest) */ XsReal XsQuaternion_normalized(const XsQuaternion* thisPtr, XsQuaternion* dest) { - XsReal divisor, length = sqrt(thisPtr->m_w*thisPtr->m_w + thisPtr->m_x*thisPtr->m_x + thisPtr->m_y*thisPtr->m_y + thisPtr->m_z*thisPtr->m_z); - divisor = XsMath_one/length; + XsReal divisor, length = sqrt(thisPtr->m_w * thisPtr->m_w + thisPtr->m_x * thisPtr->m_x + thisPtr->m_y * thisPtr->m_y + thisPtr->m_z * thisPtr->m_z); + divisor = XsMath_one / length; if (thisPtr->m_w < 0) divisor = -divisor; @@ -178,13 +178,13 @@ void XsQuaternion_fromRotationMatrix(XsQuaternion* thisPtr, const XsMatrix* ori) XsReal trace; // Trace of matrix XsReal s; - if (!XsMatrix_dimensionsMatch(ori, 3,3)) + if (!XsMatrix_dimensionsMatch(ori, 3, 3)) { XsQuaternion_destruct(thisPtr); return; } -// XsQuaternion result; + // XsQuaternion result; // Calculate trace of matrix // T = 4 - 4x^2 - 4y^2 - 4z^2 @@ -199,12 +199,12 @@ void XsQuaternion_fromRotationMatrix(XsQuaternion* thisPtr, const XsMatrix* ori) // Test if (T > 0.0000001) to avoid large distortions! // If the trace of the matrix is equal to zero, then identify // which major diagonal element has the greatest value - if (trace*trace >= XsMath_tinyValue) + if (trace * trace >= XsMath_tinyValue) { s = XsMath_two * sqrt(trace); thisPtr->m_w = XsMath_pt25 * s; - s = XsMath_one/s; + s = XsMath_one / s; thisPtr->m_x = (XsMatrix_value(ori, 1, 2) - XsMatrix_value(ori, 2, 1)) * s; thisPtr->m_y = (XsMatrix_value(ori, 2, 0) - XsMatrix_value(ori, 0, 2)) * s; thisPtr->m_z = (XsMatrix_value(ori, 0, 1) - XsMatrix_value(ori, 1, 0)) * s; @@ -215,7 +215,7 @@ void XsQuaternion_fromRotationMatrix(XsQuaternion* thisPtr, const XsMatrix* ori) s = XsMath_two * sqrt(trace); thisPtr->m_x = XsMath_pt25 * s; - s = XsMath_one/s; + s = XsMath_one / s; thisPtr->m_w = (XsMatrix_value(ori, 1, 2) - XsMatrix_value(ori, 2, 1)) * s; thisPtr->m_y = (XsMatrix_value(ori, 0, 1) + XsMatrix_value(ori, 1, 0)) * s; thisPtr->m_z = (XsMatrix_value(ori, 2, 0) + XsMatrix_value(ori, 0, 2)) * s; @@ -226,7 +226,7 @@ void XsQuaternion_fromRotationMatrix(XsQuaternion* thisPtr, const XsMatrix* ori) s = XsMath_two * sqrt(trace); thisPtr->m_y = XsMath_pt25 * s; - s = XsMath_one/s; + s = XsMath_one / s; thisPtr->m_w = (XsMatrix_value(ori, 2, 0) - XsMatrix_value(ori, 0, 2)) * s; thisPtr->m_x = (XsMatrix_value(ori, 0, 1) + XsMatrix_value(ori, 1, 0)) * s; thisPtr->m_z = (XsMatrix_value(ori, 1, 2) + XsMatrix_value(ori, 2, 1)) * s; @@ -237,7 +237,7 @@ void XsQuaternion_fromRotationMatrix(XsQuaternion* thisPtr, const XsMatrix* ori) s = XsMath_two * sqrt(trace); thisPtr->m_z = XsMath_pt25 * s; - s = XsMath_one/s; + s = XsMath_one / s; thisPtr->m_w = (XsMatrix_value(ori, 0, 1) - XsMatrix_value(ori, 1, 0)) * s; thisPtr->m_x = (XsMatrix_value(ori, 2, 0) + XsMatrix_value(ori, 0, 2)) * s; thisPtr->m_y = (XsMatrix_value(ori, 1, 2) + XsMatrix_value(ori, 2, 1)) * s; @@ -343,9 +343,9 @@ int XsQuaternion_compare(XsQuaternion const* thisPtr, XsQuaternion const* other, XsReal XsQuaternion_dotProduct(XsQuaternion const* thisPtr, XsQuaternion const* other) { return (thisPtr->m_w * other->m_w) + - (thisPtr->m_x * other->m_x) + - (thisPtr->m_y * other->m_y) + - (thisPtr->m_z * other->m_z); + (thisPtr->m_x * other->m_x) + + (thisPtr->m_y * other->m_y) + + (thisPtr->m_z * other->m_z); } /*! @} */ diff --git a/lib/xspublic/xstypes/xsquaternion.h b/lib/xspublic/xstypes/xsquaternion.h index ceb89bd..4930dca 100644 --- a/lib/xspublic/xstypes/xsquaternion.h +++ b/lib/xspublic/xstypes/xsquaternion.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -99,10 +99,13 @@ XSTYPES_DLL_API XsReal XsQuaternion_dotProduct(XsQuaternion const* thisPtr, XsQu } // extern "C" #endif -struct XsQuaternion { -XSCPPPROTECTED - union { - struct { +struct XsQuaternion +{ + XSCPPPROTECTED + union + { + struct + { XsReal m_w; //!< Stores the w component of the quaternion XsReal m_x; //!< Stores the x component of the quaternion XsReal m_y; //!< Stores the y component of the quaternion @@ -284,29 +287,65 @@ XSCPPPROTECTED XsQuaternion_multiply(this, &other, this); } + /*! \brief Multiplication of \a a quaternion with \a b quaternion + \param a The first quaternion to multiply with (left side of multiplication) + \param b The second quaternion to multiply with (right side of multiplication) + \details Result is stored in this quaternion. + Explicit function instead of *= operator needed for java interface, which does not have *= operator + \sa XsQuaternion_multiply() + */ + inline void multiply(const XsQuaternion& a, const XsQuaternion& b) + { + XsQuaternion_multiply(&a, &b, this); + } + //! \brief Return the w component of the quaternion - inline XsReal w() const { return m_w; } + inline XsReal w() const + { + return m_w; + } //! \brief Return the x component of the quaternion - inline XsReal x() const { return m_x; } + inline XsReal x() const + { + return m_x; + } //! \brief Return the y component of the quaternion - inline XsReal y() const { return m_y; } + inline XsReal y() const + { + return m_y; + } //! \brief Return the z component of the quaternion - inline XsReal z() const { return m_z; } + inline XsReal z() const + { + return m_z; + } //! \brief Return a reference to the w component of the quaternion - inline XsReal& w() { return m_w; } + inline XsReal& w() + { + return m_w; + } //! \brief Return a reference to the x component of the quaternion - inline XsReal& x() { return m_x; } + inline XsReal& x() + { + return m_x; + } //! \brief Return a reference to the y component of the quaternion - inline XsReal& y() { return m_y; } + inline XsReal& y() + { + return m_y; + } //! \brief Return a reference to the z component of the quaternion - inline XsReal& z() { return m_z; } + inline XsReal& z() + { + return m_z; + } //! \brief Swap the contents with \a other inline void swap(XsQuaternion& other) { XsQuaternion_swap(this, &other); } - + //! \brief Swap the contents of \a first with that of \a second friend void swap(XsQuaternion& first, XsQuaternion& second) { @@ -317,9 +356,9 @@ XSCPPPROTECTED inline bool operator ==(const XsQuaternion& other) const { return m_w == other.m_w && - m_x == other.m_x && - m_y == other.m_y && - m_z == other.m_z; + m_x == other.m_x && + m_y == other.m_y && + m_z == other.m_z; } //! \brief Returns true if the fields of this and \a other are within \a tolerance of each otehr @@ -338,13 +377,13 @@ XSCPPPROTECTED #ifdef __cplusplus //! \brief Return the negated version of the Quaternion \a q (w,-x,-y,-z) -inline XsQuaternion operator-(const XsQuaternion &q) +inline XsQuaternion operator-(const XsQuaternion& q) { return XsQuaternion(q.w(), -q.x(), -q.y(), -q.z()); } //! \brief Multiply \a lhs by \a rhs and return the result -inline XsQuaternion operator *(const XsQuaternion& lhs, const XsQuaternion &rhs) +inline XsQuaternion operator *(const XsQuaternion& lhs, const XsQuaternion& rhs) { XsQuaternion tmp(lhs); tmp *= rhs; diff --git a/lib/xspublic/xstypes/xsquaternionarray.c b/lib/xspublic/xstypes/xsquaternionarray.c index 00ac8a7..fa02a75 100644 --- a/lib/xspublic/xstypes/xsquaternionarray.c +++ b/lib/xspublic/xstypes/xsquaternionarray.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -73,14 +73,15 @@ /*! \copydoc XsArrayDescriptor::itemCompare -\note Specialization for int*/ + \note Specialization for int*/ int compareQuat(XsQuaternion const* a, XsQuaternion const* b) { return XsQuaternion_compare(a, b, 0.0); } //! \brief Descriptor for XsQuaternionArray -XsArrayDescriptor const g_xsQuaternionArrayDescriptor = { +XsArrayDescriptor const g_xsQuaternionArrayDescriptor = +{ sizeof(XsQuaternion), XSEXPCASTITEMSWAP XsQuaternion_swap, 0, //construct diff --git a/lib/xspublic/xstypes/xsquaternionarray.h b/lib/xspublic/xstypes/xsquaternionarray.h index c09bcad..a463437 100644 --- a/lib/xspublic/xstypes/xsquaternionarray.h +++ b/lib/xspublic/xstypes/xsquaternionarray.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -90,16 +90,17 @@ XSTYPES_DLL_API void XsQuaternionArray_construct(XsQuaternionArray* thisPtr, XsS #endif #ifdef __cplusplus -struct XsQuaternionArray : public XsArrayImpl { +struct XsQuaternionArray : public XsArrayImpl +{ //! \brief Constructs an XsQuaternionArray inline explicit XsQuaternionArray(XsSize sz = 0, XsQuaternion const* src = 0) - : ArrayImpl(sz, src) + : ArrayImpl(sz, src) { } //! \brief Constructs an XsQuaternionArray as a copy of \a other inline XsQuaternionArray(XsQuaternionArray const& other) - : ArrayImpl(other) + : ArrayImpl(other) { } diff --git a/lib/xspublic/xstypes/xsrange.c b/lib/xspublic/xstypes/xsrange.c index 8c9dc10..e155d41 100644 --- a/lib/xspublic/xstypes/xsrange.c +++ b/lib/xspublic/xstypes/xsrange.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -66,7 +66,7 @@ /*! \class XsRange \brief A class whose objects can be used to store a range. It provides method to - check whether a value is inside the range. + check whether a value is inside the range. */ /*! \addtogroup cinterface C Interface @@ -74,7 +74,7 @@ */ /*! \relates XsRange \brief Get the number of values in the range. - \note The range is *inclusive* [first, last] instead of [first, last>. So count [1, 2] = 2 + \note The range is *inclusive* [first, last] instead of [first, last>. So count [1, 2] = 2 \returns The number of values in the range (inclusive) */ int XsRange_count(const XsRange* thisPtr) @@ -85,7 +85,7 @@ int XsRange_count(const XsRange* thisPtr) } /*! \relates XsRange \brief Get the number of values in the range. - \note The range is *exclusive* [first, last> instead of [first, last]. So interval [1, 2] = 1 + \note The range is *exclusive* [first, last> instead of [first, last]. So interval [1, 2] = 1 \returns The number of values in the range (exclusive) */ int XsRange_interval(const XsRange* thisPtr) diff --git a/lib/xspublic/xstypes/xsrange.h b/lib/xspublic/xstypes/xsrange.h index 3212276..13fc89d 100644 --- a/lib/xspublic/xstypes/xsrange.h +++ b/lib/xspublic/xstypes/xsrange.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -86,7 +86,8 @@ XSTYPES_DLL_API int XsRange_empty(const struct XsRange* thisPtr); } // extern "C" #endif -struct XsRange { +struct XsRange +{ #ifdef __cplusplus //! \brief Constructs a range starting at \a f and ending at \a l. Default values are 0 and -1 respectively. inline explicit XsRange(int f = 0, int l = -1) diff --git a/lib/xspublic/xstypes/xsrawgnsspvtdata.h b/lib/xspublic/xstypes/xsrawgnsspvtdata.h index d25631d..13ca446 100644 --- a/lib/xspublic/xstypes/xsrawgnsspvtdata.h +++ b/lib/xspublic/xstypes/xsrawgnsspvtdata.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -78,11 +78,57 @@ #include #endif -// The valid flag from the PVT package has the following fields, x is the valid flag. +// The valid flag from the PVT package has the following fields, x is the valid flag: #define GNSS_PVT_VALID_DATE(x) (0x01 & x) // UTC date is valid #define GNSS_PVT_VALID_TIME(x) (0x02 & x) // UTC time of day is valid #define GNSS_PVT_VALID_RESOLVE(x) (0x04 & x) // UTC time of day has been fully resolved +// The flag from the PVT package has the following fields, x is the valid flag: +#define GNSS_PVT_FLAGS_GNSS_FIX(x) ((0x03 << 0) & x) +#define GNSS_PVT_FLAGS_POWER_SAVE_MODE_STATE(x) ((0x07 << 2) & x) +#define GNSS_PVT_FLAGS_HEADING_VEHICLE_VALID(x) ((0x01 << 5) & x) +#define GNSS_PVT_FLAGS_CARRIER_SOLUTION(x) ((0x03 << 6) & x) + +#define GNSS_PVT_FLAGS_GNSS_FIX_NONE (0x00) +#define GNSS_PVT_FLAGS_GNSS_FIX_SINGLE (0x01) +#define GNSS_PVT_FLAGS_GNSS_FIX_DIFFERENTIAL (0x03) + +#define GNSS_PVT_FLAGS_CARRIER_SOLUTION_NONE (0x00 << 6) +#define GNSS_PVT_FLAGS_CARRIER_SOLUTION_FLOATING (0x01 << 6) +#define GNSS_PVT_FLAGS_CARRIER_SOLUTION_FIXED (0x02 << 6) + +// Default accuracy deviation when not provided by GNSS receiver +#define GNSS_ACCURACY_DEFAULT_DEVIATION (10000000) + +/*! \addtogroup enums Global enumerations + @{ +*/ + +/*! \enum XsPvtDataUtcFlags + * \brief Define the validity flags for the UTC time +*/ +enum XsPvtDataUtcFlags +{ + XPDUF_ValidDate = 0x01, + XPDUF_ValidTime = 0x02, + XPDUF_FullyResolved = 0x04, + XPDUF_ValidMag = 0x08 +}; + +/*! \enum XsPvtDataQualityIndicator + * \brief Defines the quality indicator flags of the GNSS Fix type +*/ +enum XsPvtDataQualityIndicator +{ + XPDQI_NoFix = 0, + XPDQI_DeadReckiningOnly = 1, + XPDQI_2DFix = 2, + XPDQI_3DFix = 3, + XPDQI_GnssAndDeadReck = 4, + XPDQI_TimeOnlyFix = 5 +}; +/*! @} */ + /*! \brief A container for GNSS Position Velocity and Time data */ struct XsRawGnssPvtData @@ -113,8 +159,9 @@ struct XsRawGnssPvtData uint8_t m_flags; /*!< Fix Status Flags bit(0) = Set if there is a valid fix (i.e. within DOP & accuracy masks) bit(1) = Set if differential corrections were applied - bit(2..4) = Reserved (Ignore) + bit(4..2) = Power save mode state bit(5) = Set if heading of vehicle is valid + bit(7..6) = Carrier phase range solution status: 0 = none, 1 = floating, 2 = fixed */ uint8_t m_numSv; //!< Number of satellites used in Nav Solution diff --git a/lib/xspublic/xstypes/xsrawgnsssatinfo.h b/lib/xspublic/xstypes/xsrawgnsssatinfo.h index ce3241f..2846144 100644 --- a/lib/xspublic/xstypes/xsrawgnsssatinfo.h +++ b/lib/xspublic/xstypes/xsrawgnsssatinfo.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -68,22 +68,22 @@ #include "pstdint.h" #ifdef _MSC_VER -#pragma pack(push, 1) + #pragma pack(push, 1) #endif #ifndef __cplusplus #define XSSATINFO_INITIALIZER { 0, 0, 0, 0 } #define XSRAWGNSSSATINFO_INITIALIZER { 0, 0, 0, 0, 0, \ - XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, \ - XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, \ - XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, \ - XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, \ - XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, \ - XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER } + XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, \ + XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, \ + XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, \ + XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, \ + XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, \ + XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER, XSSATINFO_INITIALIZER } #endif /*! \addtogroup enums Global enumerations - @{ + @{ */ /*! \brief Xsens Satellite Info Flags */ @@ -113,10 +113,10 @@ typedef enum XsSatInfoFlags XsSatInfoFlags; //! \brief A container for information of one GNSS satellite struct XsSatInfo { - uint8_t m_gnssId; //!< GNSS identifier - uint8_t m_svId; //!< Satellite identifier - uint8_t m_cno; //!< Carrier to noise ratio (signals strength) - uint8_t m_flags; /*!< + uint8_t m_gnssId; //!< GNSS identifier + uint8_t m_svId; //!< Satellite identifier + uint8_t m_cno; //!< Carrier to noise ratio (signals strength) + uint8_t m_flags; /*!< bits[0..2] : Signal quality indicator 0 = No signal 1 = Searching signal @@ -131,7 +131,7 @@ struct XsSatInfo 2 = unhealthy bits[6] : Is set to 1 when differential correction data is available bits[7] : reserved - */ +*/ }; typedef struct XsSatInfo XsSatInfo; @@ -170,7 +170,7 @@ struct XsRawGnssSatInfo typedef struct XsRawGnssSatInfo XsRawGnssSatInfo; #ifdef _MSC_VER -#pragma pack(pop) + #pragma pack(pop) #endif #endif diff --git a/lib/xspublic/xstypes/xsresetmethod.h b/lib/xspublic/xstypes/xsresetmethod.h index ac70ee0..3932348 100644 --- a/lib/xspublic/xstypes/xsresetmethod.h +++ b/lib/xspublic/xstypes/xsresetmethod.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -71,7 +71,8 @@ //AUTO namespace xstypes { /*! \brief Orientation reset type. */ -enum XsResetMethod { +enum XsResetMethod +{ XRM_StoreAlignmentMatrix = 0, //!< \brief Store the current object alignment matrix to persistent memory XRM_Heading = 1, //!< \brief Reset the heading (yaw) XRM_Object = 3, //!< \brief Reset the attitude (roll, pitch), same as XRM_Inclination diff --git a/lib/xspublic/xstypes/xsresultvalue.c b/lib/xspublic/xstypes/xsresultvalue.c index 29cddda..1b393a3 100644 --- a/lib/xspublic/xstypes/xsresultvalue.c +++ b/lib/xspublic/xstypes/xsresultvalue.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -77,131 +77,239 @@ */ const char* XsResultValue_toString(XsResultValue result) { - switch(result) + switch (result) { - case XRV_OK: return "No error"; + case XRV_OK: + return "No error"; // Communication protocol - case XRV_NOBUS: return "Bus has no power"; - case XRV_BUSNOTREADY: return "Bus not ready for measurement"; - case XRV_INVALIDPERIOD: return "Period is invalid"; - case XRV_INVALIDMSG: return "Message is invalid"; - case XRV_INITBUSFAIL1: return "A slave did not respond to WaitForSetBID"; - case XRV_INITBUSFAIL2: return "An incorrect answer received after WaitForSetBID"; - case XRV_INITBUSFAIL3: return "After four bus-scans still undetected Motion Trackers"; - case XRV_SETBIDFAIL1: return "No reply to SetBID message during SetBID procedure"; - case XRV_SETBIDFAIL2: return "Other than SetBIDAck received"; - case XRV_MEASUREMENTFAIL1: return "Timer overflow - period too short to collect all data from Motion Trackers"; - case XRV_MEASUREMENTFAIL2: return "Motion Tracker responds with other than SlaveData message"; - case XRV_MEASUREMENTFAIL3: return "Total bytes of data of Motion Trackers incl sample counter exceeds 255 bytes"; - case XRV_MEASUREMENTFAIL4: return "Timer overflow during measurement - MT does not respond within measurement period. Increase period (lower update rate)"; - case XRV_MEASUREMENTFAIL5: return "Timer overflow during measurement - MT response was not received within measurement period. Increase period (lower update rate) or use fewer Motion Trackers"; - case XRV_MEASUREMENTFAIL6: return "No correct response from Motion Tracker during measurement"; - case XRV_TIMEROVERFLOW: return "Timer overflow during measurement"; - case XRV_BAUDRATEINVALID: return "Baudrate does not comply with valid range"; - case XRV_INVALIDPARAM: return "Invalid parameter supplied"; - case XRV_MEASUREMENTFAIL7: return "TX PC Buffer is full"; - case XRV_MEASUREMENTFAIL8: return "TX PC Buffer overflow, cannot fit full message"; - case XRV_WIRELESSFAIL: return "Wireless communication system failed"; - case XRV_DEVICEERROR: return "The device generated an error, try updating the firmware"; - case XRV_DATAOVERFLOW: return "Data overflow"; - case XRV_BUFFEROVERFLOW: return "Sample buffer overflow during communication outage"; - case XRV_EXTTRIGGERERROR: return "The external trigger is not behaving as configured"; - case XRV_SAMPLESTREAMERROR: return "Sample stream detected an error in ordering input data"; - case XRV_POWER_DIP: return "A power dip has been detected and recovered from"; - case XRV_POWER_OVERCURRENT: return "A current limiter has been activated"; - case XRV_OVERHEATING: return "Device temperature exceded operational limits"; - case XRV_BATTERYLOW: return "Battery level reached lower limit"; - case XRV_INVALIDFILTERPROFILE: return "Specified filter profile ID is not available on the device or the user is trying to duplicate an existing filter profile type"; - case XRV_INVALIDSTOREDSETTINGS: return "The settings stored in the device's non volatile memory are invalid"; - case XRV_ACCESSDENIED: return "Request for control of the device was denied"; - case XRV_FILEERROR: return "Failure reading, writing, opening or closing a file"; - case XRV_OUTPUTCONFIGERROR: return "Erroneous output configuration, device can not go to measurement"; + case XRV_NOBUS: + return "Bus has no power"; + case XRV_BUSNOTREADY: + return "Bus not ready for measurement"; + case XRV_INVALIDPERIOD: + return "Period is invalid"; + case XRV_INVALIDMSG: + return "Message is invalid"; + case XRV_INITBUSFAIL1: + return "A slave did not respond to WaitForSetBID"; + case XRV_INITBUSFAIL2: + return "An incorrect answer received after WaitForSetBID"; + case XRV_INITBUSFAIL3: + return "After four bus-scans still undetected Motion Trackers"; + case XRV_SETBIDFAIL1: + return "No reply to SetBID message during SetBID procedure"; + case XRV_SETBIDFAIL2: + return "Other than SetBIDAck received"; + case XRV_MEASUREMENTFAIL1: + return "Timer overflow - period too short to collect all data from Motion Trackers"; + case XRV_MEASUREMENTFAIL2: + return "Motion Tracker responds with other than SlaveData message"; + case XRV_MEASUREMENTFAIL3: + return "Total bytes of data of Motion Trackers incl sample counter exceeds 255 bytes"; + case XRV_MEASUREMENTFAIL4: + return "Timer overflow during measurement - MT does not respond within measurement period. Increase period (lower update rate)"; + case XRV_MEASUREMENTFAIL5: + return "Timer overflow during measurement - MT response was not received within measurement period. Increase period (lower update rate) or use fewer Motion Trackers"; + case XRV_MEASUREMENTFAIL6: + return "No correct response from Motion Tracker during measurement"; + case XRV_TIMEROVERFLOW: + return "Timer overflow during measurement"; + case XRV_BAUDRATEINVALID: + return "Baudrate does not comply with valid range"; + case XRV_INVALIDPARAM: + return "Invalid parameter supplied"; + case XRV_MEASUREMENTFAIL7: + return "TX PC Buffer is full"; + case XRV_MEASUREMENTFAIL8: + return "TX PC Buffer overflow, cannot fit full message"; + case XRV_WIRELESSFAIL: + return "Wireless communication system failed"; + case XRV_DEVICEERROR: + return "The device generated an error, try updating the firmware"; + case XRV_DATAOVERFLOW: + return "Data overflow"; + case XRV_BUFFEROVERFLOW: + return "Sample buffer overflow during communication outage"; + case XRV_EXTTRIGGERERROR: + return "The external trigger is not behaving as configured"; + case XRV_SAMPLESTREAMERROR: + return "Sample stream detected an error in ordering input data"; + case XRV_POWER_DIP: + return "A power dip has been detected and recovered from"; + case XRV_POWER_OVERCURRENT: + return "A current limiter has been activated"; + case XRV_OVERHEATING: + return "Device temperature exceded operational limits"; + case XRV_BATTERYLOW: + return "Battery level reached lower limit"; + case XRV_INVALIDFILTERPROFILE: + return "Specified filter profile ID is not available on the device or the user is trying to duplicate an existing filter profile type"; + case XRV_INVALIDSTOREDSETTINGS: + return "The settings stored in the device's non volatile memory are invalid"; + case XRV_ACCESSDENIED: + return "Request for control of the device was denied"; + case XRV_FILEERROR: + return "Failure reading, writing, opening or closing a file"; + case XRV_OUTPUTCONFIGERROR: + return "Erroneous output configuration, device can not go to measurement"; + case XRV_FILE_SYSTEM_CORRUPT: + return "The internal file system of the device has become corrupt"; // XDA / XME / etc - case XRV_ERROR: return "Generic error"; - case XRV_NOTIMPLEMENTED: return "Operation not implemented"; - case XRV_TIMEOUT: return "Timeout occurred, some data received"; - case XRV_TIMEOUTNODATA: return "Timeout occurred, no data was received"; - case XRV_CHECKSUMFAULT: return "Checksum fault"; - case XRV_OUTOFMEMORY: return "Out of memory"; - case XRV_NOTFOUND: return "Requested item was not found"; - case XRV_UNEXPECTEDMSG: return "Unexpected message received"; - case XRV_INVALIDID: return "Invalid id supplied"; - case XRV_INVALIDOPERATION: return "Invalid operation"; - case XRV_INSUFFICIENTSPACE: return "Insufficient buffer space available"; - case XRV_INPUTCANNOTBEOPENED: return "Could not open input device"; - case XRV_OUTPUTCANNOTBEOPENED: return "Could not open output device"; - case XRV_ALREADYOPEN: return "A device is already open"; - case XRV_ENDOFFILE: return "End of file reached"; - case XRV_COULDNOTREADSETTINGS: return "A required settings file could not be opened or is missing some data"; - case XRV_NODATA: return "No data available"; - case XRV_READONLY: return "Tried to change a read-only value"; - case XRV_NULLPTR: return "Invalid NULL pointer supplied"; - case XRV_INSUFFICIENTDATA: return "Insufficient data supplied"; - case XRV_BUSY: return "Busy processing, try again later"; - case XRV_INVALIDINSTANCE: return "Invalid instance called, because of an invalid or missing license"; - case XRV_DATACORRUPT: return "Data-source corrupt"; - - case XRV_READINITFAILED: return "Failure during reading of settings. File may be old or corrupt."; - case XRV_NOXMFOUND: return "Could not find any MVN-compatible hardware, check connections and LEDs"; - case XRV_DEVICECOUNTZERO: return "No xsens devices found, check connections"; - case XRV_MTLOCATIONINVALID: return "One or more sensors are not where they were expected, check locations"; - case XRV_INSUFFICIENTMTS: return "Not enough sensors were found, check connections"; - case XRV_INITFUSIONFAILED: return "Failure during initialization of Fusion Engine. Source file may be old or corrupt."; - case XRV_OTHER: return "Something else was received than was requested"; - - case XRV_NOFILEOPEN: return "No file open"; - case XRV_NOPORTOPEN: return "No serial port open"; - case XRV_NOFILEORPORTOPEN: return "No file or serial port open"; - case XRV_PORTNOTFOUND: return "A required port could not be found"; - case XRV_INITPORTFAILED: return "The low-level port handler failed to initialize"; - case XRV_CALIBRATIONFAILED: return "A calibration routine failed"; - - case XRV_CONFIGCHECKFAIL: return "Failure during device configuration"; - case XRV_ALREADYDONE: return "This once only operation has already been performed"; - - case XRV_SYNC_SINGLE_SLAVE: return "The single connected device is configured as a slave"; - case XRV_SYNC_SECOND_MASTER: return "More than one master was detected"; - case XRV_SYNC_NO_SYNC: return "A device was detected that was neither master nor slave"; - case XRV_SYNC_NO_MASTER: return "No master detected"; - case XRV_SYNC_DATA_MISSING: return "A device is not sending enough data, check synchronization cables"; - - case XRV_VERSION_TOO_LOW: return "The version of the object is too low for the requested operation"; - case XRV_VERSION_PROBLEM: return "The object has an unrecognized version, so it's not safe to perform the operation"; - - case XRV_ABORTED: return "The process was aborted by an external event"; - case XRV_UNSUPPORTED: return "The requested functionality is not supported by the device"; - - case XRV_PACKETCOUNTERMISSED: return "A packet counter value was missed"; - - case XRV_MEASUREMENTFAILED: return "Failed to start measurement"; - case XRV_STARTRECORDINGFAILED: return "A device could not start recording"; - case XRV_STOPRECORDINGFAILED: return "A device could not stop recording"; - - case XRV_RADIO_CHANNEL_IN_USE: return "Detected another system using the selected radio channel"; - case XRV_UNEXPECTED_DISCONNECT: return "Motion tracker disconnected unexpectedly"; - case XRV_TOO_MANY_CONNECTED_TRACKERS: return "Too many motion trackers connected"; - case XRV_GOTOCONFIGFAILED: return "Failed to go to config mode"; - case XRV_OUTOFRANGE: return "Device has gone out of range"; - case XRV_BACKINRANGE: return "Device is back in range, resuming normal operation"; - case XRV_EXPECTED_DISCONNECT: return "The device was disconnected"; - - case XRV_RESTORE_COMMUNICATION_FAILED: return "Restore communication failed"; - case XRV_RESTORE_COMMUNICATION_STOPPED: return "Restore communication was stopped"; - - case XRV_EXPECTED_CONNECT: return "The device connected"; - case XRV_IN_USE: return "Requested connection already in use"; - case XRV_PERFORMANCE_WARNING: return "The system running the application can't fully keep up with the incoming data"; - case XRV_PERFORMANCE_OK: return "The system running the application can keep up with the incoming data again"; - - case XRV_SHUTTINGDOWN: return "The device is shutting down"; - case XRV_GNSSCONFIGURATIONERROR:return "A configuration item was refused by the GNSS module"; - case XRV_GNSSCOMMTIMEOUT: return "The communication with the GNSS module timed out"; - case XRV_GNSSERROR: return "Communication between the device and the GNSS module failed"; - case XRV_DEVICE_NOT_CALIBRATED: return "The EMTS of the device does not contain calibration data"; - - default: return "!!Undefined Result Value!!"; + case XRV_ERROR: + return "Generic error"; + case XRV_NOTIMPLEMENTED: + return "Operation not implemented"; + case XRV_TIMEOUT: + return "Timeout occurred, some data received"; + case XRV_TIMEOUTNODATA: + return "Timeout occurred, no data was received"; + case XRV_CHECKSUMFAULT: + return "Checksum fault"; + case XRV_OUTOFMEMORY: + return "Out of memory"; + case XRV_NOTFOUND: + return "Requested item was not found"; + case XRV_UNEXPECTEDMSG: + return "Unexpected message received"; + case XRV_INVALIDID: + return "Invalid id supplied"; + case XRV_INVALIDOPERATION: + return "Invalid operation"; + case XRV_INSUFFICIENTSPACE: + return "Insufficient buffer space available"; + case XRV_INPUTCANNOTBEOPENED: + return "Could not open input device"; + case XRV_OUTPUTCANNOTBEOPENED: + return "Could not open output device"; + case XRV_ALREADYOPEN: + return "A device is already open"; + case XRV_ENDOFFILE: + return "End of file reached"; + case XRV_COULDNOTREADSETTINGS: + return "A required settings file could not be opened or is missing some data"; + case XRV_NODATA: + return "No data available"; + case XRV_READONLY: + return "Tried to change a read-only value"; + case XRV_NULLPTR: + return "Invalid NULL pointer supplied"; + case XRV_INSUFFICIENTDATA: + return "Insufficient data supplied"; + case XRV_BUSY: + return "Busy processing, try again later"; + case XRV_INVALIDINSTANCE: + return "Invalid instance called, because of an invalid or missing license"; + case XRV_DATACORRUPT: + return "Data-source corrupt"; + + case XRV_READINITFAILED: + return "Failure during reading of settings. File may be old or corrupt."; + case XRV_NOXMFOUND: + return "Could not find any MVN-compatible hardware, check connections and LEDs"; + case XRV_DEVICECOUNTZERO: + return "No xsens devices found, check connections"; + case XRV_MTLOCATIONINVALID: + return "One or more sensors are not where they were expected, check locations"; + case XRV_INSUFFICIENTMTS: + return "Not enough sensors were found, check connections"; + case XRV_INITFUSIONFAILED: + return "Failure during initialization of Fusion Engine. Source file may be old or corrupt."; + case XRV_OTHER: + return "Something else was received than was requested"; + + case XRV_NOFILEOPEN: + return "No file open"; + case XRV_NOPORTOPEN: + return "No serial port open"; + case XRV_NOFILEORPORTOPEN: + return "No file or serial port open"; + case XRV_PORTNOTFOUND: + return "A required port could not be found"; + case XRV_INITPORTFAILED: + return "The low-level port handler failed to initialize"; + case XRV_CALIBRATIONFAILED: + return "A calibration routine failed"; + + case XRV_CONFIGCHECKFAIL: + return "Failure during device configuration"; + case XRV_ALREADYDONE: + return "This once only operation has already been performed"; + + case XRV_SYNC_SINGLE_SLAVE: + return "The single connected device is configured as a slave"; + case XRV_SYNC_SECOND_MASTER: + return "More than one master was detected"; + case XRV_SYNC_NO_SYNC: + return "A device was detected that was neither master nor slave"; + case XRV_SYNC_NO_MASTER: + return "No master detected"; + case XRV_SYNC_DATA_MISSING: + return "A device is not sending enough data, check synchronization cables"; + + case XRV_VERSION_TOO_LOW: + return "The version of the object is too low for the requested operation"; + case XRV_VERSION_PROBLEM: + return "The object has an unrecognized version, so it's not safe to perform the operation"; + + case XRV_ABORTED: + return "The process was aborted by an external event"; + case XRV_UNSUPPORTED: + return "The requested functionality is not supported by the device"; + + case XRV_PACKETCOUNTERMISSED: + return "A packet counter value was missed"; + + case XRV_MEASUREMENTFAILED: + return "Failed to start measurement"; + case XRV_STARTRECORDINGFAILED: + return "A device could not start recording"; + case XRV_STOPRECORDINGFAILED: + return "A device could not stop recording"; + + case XRV_RADIO_CHANNEL_IN_USE: + return "Detected another system using the selected radio channel"; + case XRV_UNEXPECTED_DISCONNECT: + return "Motion tracker disconnected unexpectedly"; + case XRV_TOO_MANY_CONNECTED_TRACKERS: + return "Too many motion trackers connected"; + case XRV_GOTOCONFIGFAILED: + return "Failed to go to config mode"; + case XRV_OUTOFRANGE: + return "Device has gone out of range"; + case XRV_BACKINRANGE: + return "Device is back in range, resuming normal operation"; + case XRV_EXPECTED_DISCONNECT: + return "The device was disconnected"; + + case XRV_RESTORE_COMMUNICATION_FAILED: + return "Restore communication failed"; + case XRV_RESTORE_COMMUNICATION_STOPPED: + return "Restore communication was stopped"; + + case XRV_EXPECTED_CONNECT: + return "The device connected"; + case XRV_IN_USE: + return "Requested connection already in use"; + case XRV_PERFORMANCE_WARNING: + return "The system running the application can't fully keep up with the incoming data"; + case XRV_PERFORMANCE_OK: + return "The system running the application can keep up with the incoming data again"; + + case XRV_SHUTTINGDOWN: + return "The device is shutting down"; + case XRV_GNSSCONFIGURATIONERROR: + return "A configuration item was refused by the GNSS module"; + case XRV_GNSSCOMMTIMEOUT: + return "The communication with the GNSS module timed out"; + case XRV_GNSSERROR: + return "Communication between the device and the GNSS module failed"; + case XRV_DEVICE_NOT_CALIBRATED: + return "The EMTS of the device does not contain calibration data"; + + default: + return "!!Undefined Result Value!!"; } } diff --git a/lib/xspublic/xstypes/xsresultvalue.h b/lib/xspublic/xstypes/xsresultvalue.h index fe1d76a..1fd6dde 100644 --- a/lib/xspublic/xstypes/xsresultvalue.h +++ b/lib/xspublic/xstypes/xsresultvalue.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -79,7 +79,8 @@ extern "C" { \details These values are used to signal success or specific failures of functions \sa XsResultValue_toString */ -enum XsResultValue { +enum XsResultValue +{ // general OK XRV_OK = 0, //!< 0: Operation was performed successfully @@ -119,6 +120,7 @@ enum XsResultValue { XRV_ACCESSDENIED = 51, //!< 51: Request for control of the device was denied XRV_FILEERROR = 52, //!< 52: Failure reading, writing, opening or closing a file XRV_OUTPUTCONFIGERROR = 53, //!< 53: Erroneous output configuration, device can not go to measurement + XRV_FILE_SYSTEM_CORRUPT = 54, //!< 54: The internal file system of the device has become corrupt // CMT / XDA / XME / etc XRV_ERROR = 256, //!< 256: A generic error occurred @@ -225,7 +227,7 @@ typedef enum XsDeviceErrorType XsDeviceErrorType; ////////////////////////////////////////////////////////////////////////////////////////// -/* \brief Retrieve a string corresponding to the given result code. +/*! \brief Retrieve a string corresponding to the given result code. This function uses the XsResultText list to return a string with the relevant result code in a textual format. If the supplied result code is invalid the diff --git a/lib/xspublic/xstypes/xsrssi.c b/lib/xspublic/xstypes/xsrssi.c index 4b3a966..9f3adb8 100644 --- a/lib/xspublic/xstypes/xsrssi.c +++ b/lib/xspublic/xstypes/xsrssi.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -94,7 +94,9 @@ XSTYPES_DLL_API int XsRssi_unknown(void) */ XSTYPES_DLL_API int XsRssi_unbiased(int raw) { - return raw + XS_RSSI_MAX; + if (raw < 0) + return raw + XS_RSSI_MAX; + return XS_RSSI_MAX; } /*! @} */ diff --git a/lib/xspublic/xstypes/xsrssi.h b/lib/xspublic/xstypes/xsrssi.h index c07e01f..967cad6 100644 --- a/lib/xspublic/xstypes/xsrssi.h +++ b/lib/xspublic/xstypes/xsrssi.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -80,25 +80,26 @@ XSTYPES_DLL_API int XsRssi_unbiased(int raw); #ifdef __cplusplus } // extern "C" -namespace XsRssi { - /*! \brief The bias on biased RSSI values. */ - static const int bias = XS_RSSI_MAX; +namespace XsRssi +{ +/*! \brief The bias on biased RSSI values. */ +static const int bias = XS_RSSI_MAX; - /*! \brief The maximum RSSI value. */ - static const int max = XS_RSSI_MAX; - /*! \brief The RSSI value that was reserved for when the RSSI is unknown. */ - static const int unknown = XS_RSSI_UNKNOWN; +/*! \brief The maximum RSSI value. */ +static const int max = XS_RSSI_MAX; +/*! \brief The RSSI value that was reserved for when the RSSI is unknown. */ +static const int unknown = XS_RSSI_UNKNOWN; - /*! \brief The maximum unbiased RSSI value. */ - static const int maxUnbiased = XS_RSSI_MAX + XS_RSSI_MAX; - /*! \brief The RSSI value that was reserved for when the RSSI is unknown (unbiased). */ - static const int unknownUnbiased = XS_RSSI_UNKNOWN + XS_RSSI_MAX; +/*! \brief The maximum unbiased RSSI value. */ +static const int maxUnbiased = XS_RSSI_MAX + XS_RSSI_MAX; +/*! \brief The RSSI value that was reserved for when the RSSI is unknown (unbiased). */ +static const int unknownUnbiased = XS_RSSI_UNKNOWN + XS_RSSI_MAX; - /*! \copydoc XsRssi_unbiased */ - inline int unbiased(int raw) - { - return XsRssi_unbiased(raw); - } +/*! \copydoc XsRssi_unbiased */ +inline int unbiased(int raw) +{ + return XsRssi_unbiased(raw); +} } #endif diff --git a/lib/xspublic/xstypes/xsscrdata.h b/lib/xspublic/xstypes/xsscrdata.h index 5ac5300..76092a8 100644 --- a/lib/xspublic/xstypes/xsscrdata.h +++ b/lib/xspublic/xstypes/xsscrdata.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -69,14 +69,15 @@ #include "xsushortvector.h" #ifndef __cplusplus -#define XSSCRDATA_INITIALIZER {XSUSHORTVECTOR_INITIALIZER, XSUSHORTVECTOR_INITIALIZER, XSUSHORTVECTOR_INITIALIZER, {0, 0, 0, 0}} + #define XSSCRDATA_INITIALIZER {XSUSHORTVECTOR_INITIALIZER, XSUSHORTVECTOR_INITIALIZER, XSUSHORTVECTOR_INITIALIZER, {0, 0, 0, 0}} #endif /*! \brief Container for raw sensor measurement data \details This structure contains raw measurement data from the sensors on the device. This data is unscaled, the bias has not been subtracted and no error correction has been applied. */ -struct XsScrData { +struct XsScrData +{ XsUShortVector m_acc; //!< The raw accelerometer data XsUShortVector m_gyr; //!< The raw gyroscope data XsUShortVector m_mag; //!< The raw magnetometer data diff --git a/lib/xspublic/xstypes/xssdidata.c b/lib/xspublic/xstypes/xssdidata.c index 3a2a952..be8091f 100644 --- a/lib/xspublic/xstypes/xssdidata.c +++ b/lib/xspublic/xstypes/xssdidata.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xstypes/xssdidata.h b/lib/xspublic/xstypes/xssdidata.h index ccb02f2..1e79c6d 100644 --- a/lib/xspublic/xstypes/xssdidata.h +++ b/lib/xspublic/xstypes/xssdidata.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -153,7 +153,7 @@ struct XsSdiData inline bool operator == (const XsSdiData& other) const { return m_orientationIncrement == other.m_orientationIncrement && - m_velocityIncrement == other.m_velocityIncrement; + m_velocityIncrement == other.m_velocityIncrement; } private: diff --git a/lib/xspublic/xstypes/xssensorranges.cpp b/lib/xspublic/xstypes/xssensorranges.cpp index 1ecae51..baeb08f 100644 --- a/lib/xspublic/xstypes/xssensorranges.cpp +++ b/lib/xspublic/xstypes/xssensorranges.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -67,181 +67,213 @@ extern "C" { -/* Find the A in hardware info of the product code - -Returns a pointer to the A. -*/ -static const char *findHardwareType(const char *productCode) -{ - if (findHardwareManufacturer(productCode) != HMT_MT) - return nullptr; - - const char *A = strchr(productCode, 'A'); - if (!A) - return nullptr; - const char *G = strchr(A, 'G'); - if (!G) - return nullptr; - assert(G - A <= 3); - return A; -} + /* Find the A in hardware info of the product code -/*! \brief Return the hardware manufacturer from \a productCode -*/ -HardwareManufacturerType findHardwareManufacturerC(const XsString* productCode) -{ - if (strstr(productCode->c_str(), "MT") != nullptr) - return HMT_MT; + Returns a pointer to the A. + */ + static const char* findHardwareType(const char* productCode) + { + if (findHardwareManufacturer(productCode) != HMT_MT) + return nullptr; - return HMT_None; -} + const char* A = strchr(productCode, 'A'); + if (!A) + return nullptr; + const char* G = strchr(A, 'G'); + if (!G) + return nullptr; + assert(G - A <= 3); + return A; + } -/*! \brief Return the hardware type from \a productCode -*/ -void findHardwareTypeC(const XsString* productCode, XsString* resultValue) -{ - if (!resultValue) - return; - const char *hwt = findHardwareType(productCode->c_str()); - if (!hwt) - resultValue->clear(); - else - *resultValue = hwt; -} + /*! \brief Return the hardware manufacturer from \a productCode + */ + HardwareManufacturerType findHardwareManufacturerC(const XsString* productCode) + { + if (strstr(productCode->c_str(), "MT") != nullptr) + return HMT_MT; -/* Return the accelerometer range field */ -static char accelerometerRangeField(const char *productCode) -{ - const char *hwi = findHardwareType(productCode); - if (!hwi) - return 0; - return *(hwi + 1); -} + return HMT_None; + } -/* Return the gyroscope range field */ -static char gyroscopeRangeField(const char *productCode) -{ - const char *hwi = findHardwareType(productCode); - if (!hwi) - return 0; - const char *G = strchr(hwi, 'G'); - if (!G) - return 0; - return *(G + 1); -} + /*! \brief Return the hardware type from \a productCode + */ + void findHardwareTypeC(const XsString* productCode, XsString* resultValue) + { + if (!resultValue) + return; + const char* hwt = findHardwareType(productCode->c_str()); + if (!hwt) + resultValue->clear(); + else + *resultValue = hwt; + } -/*! \brief The accelerometer range from product code \a productCode -*/ -double accelerometerRangeC(const XsString *productCode, int32_t hwVersionMajor) -{ - switch (findHardwareManufacturerC(productCode)) + /* Return the accelerometer range field */ + static char accelerometerRangeField(const char* productCode) { - case HardwareManufacturerType::HMT_MT: - switch(accelerometerRangeField(productCode->c_str())) - { - case '1': return 100.0; - case '2': return 20.0; - case '3': return 17.0; - case '5': return 50.0; - case '6': return 60.0; - case '7': return 160.0; - case '8': + const char* hwi = findHardwareType(productCode); + if (!hwi) + return 0; + return *(hwi + 1); + } + + /* Return the gyroscope range field */ + static char gyroscopeRangeField(const char* productCode) + { + const char* hwi = findHardwareType(productCode); + if (!hwi) + return 0; + const char* G = strchr(hwi, 'G'); + if (!G) + return 0; + return *(G + 1); + } + + /*! \brief The accelerometer range from product code \a productCode + */ + double accelerometerRangeC(const XsString* productCode, int32_t hwVersionMajor) + { + switch (findHardwareManufacturerC(productCode)) { - if (hwVersionMajor < 3) - return 180.0; - else - return 200.0; - } - default: return 10000.0; - } + case HardwareManufacturerType::HMT_MT: + switch (accelerometerRangeField(productCode->c_str())) + { + case '1': + return 100.0; + case '2': + return 20.0; + case '3': + return 17.0; + case '5': + return 50.0; + case '6': + return 60.0; + case '7': + return 160.0; + case '8': + { + if (hwVersionMajor < 3) + return 180.0; + else + return 200.0; + } + default: + return 10000.0; + } - default: - return 10000.0; + default: + return 10000.0; + } } -} -/*! \brief The actual accelerometer range from product code \a productCode + /*! \brief The actual accelerometer range from product code \a productCode -This is a measured value and possibly larger than what accelerometerRange() returns. -*/ -double actualAccelerometerRangeC(const XsString *productCode, int32_t hwVersionMajor) -{ - switch (findHardwareManufacturerC(productCode)) + This is a measured value and possibly larger than what accelerometerRange() returns. + */ + double actualAccelerometerRangeC(const XsString* productCode, int32_t hwVersionMajor) { - case HardwareManufacturerType::HMT_MT: - switch(accelerometerRangeField(productCode->c_str())) - { - case '1': return 100.0; - case '2': return 20.0; - case '3': return 17.0; - case '5': return 50.0; - case '6': return 60.0; - case '7': return 160.0; - case '8': + switch (findHardwareManufacturerC(productCode)) { - if (hwVersionMajor < 3) - return 180.0; - else - return 200.0; - } - default: return 10000.0; - } + case HardwareManufacturerType::HMT_MT: + switch (accelerometerRangeField(productCode->c_str())) + { + case '1': + return 100.0; + case '2': + return 20.0; + case '3': + return 17.0; + case '5': + return 50.0; + case '6': + return 60.0; + case '7': + return 160.0; + case '8': + { + if (hwVersionMajor < 3) + return 180.0; + else + return 200.0; + } + default: + return 10000.0; + } - default: - return 10000.0; + default: + return 10000.0; + } } -} -/*! \brief The gyroscope range from product code \a productCode -*/ -double gyroscopeRangeC(const XsString *productCode) -{ - switch (findHardwareManufacturerC(productCode)) + /*! \brief The gyroscope range from product code \a productCode + */ + double gyroscopeRangeC(const XsString* productCode) { - case HardwareManufacturerType::HMT_MT: - switch(gyroscopeRangeField(productCode->c_str())) + switch (findHardwareManufacturerC(productCode)) { - case '0': return 1000.0; - case '1': return 150.0; - case '2': return 1200.0; - case '3': return 300.0; - case '4': return 450.0; - case '5': return 2500.0; - case '6': return 1800.0; - case '9': return 900.0; - default: return 10000.0; - } + case HardwareManufacturerType::HMT_MT: + switch (gyroscopeRangeField(productCode->c_str())) + { + case '0': + return 1000.0; + case '1': + return 150.0; + case '2': + return 1200.0; + case '3': + return 300.0; + case '4': + return 450.0; + case '5': + return 2500.0; + case '6': + return 1800.0; + case '9': + return 900.0; + default: + return 10000.0; + } - default: - return 10000.0; + default: + return 10000.0; + } } -} -/*! \brief The actual gyroscope range from product code \a productCode + /*! \brief The actual gyroscope range from product code \a productCode -This is a measured value and possibly larger than what gyroscopeRange() returns. -*/ -double actualGyroscopeRangeC(const XsString *productCode) -{ - switch (findHardwareManufacturerC(productCode)) + This is a measured value and possibly larger than what gyroscopeRange() returns. + */ + double actualGyroscopeRangeC(const XsString* productCode) { - case HardwareManufacturerType::HMT_MT: - switch(gyroscopeRangeField(productCode->c_str())) + switch (findHardwareManufacturerC(productCode)) { - case '0': return 1000.0; - case '1': return 180.0; - case '2': return 1700.0; - case '3': return 420.0; - case '4': return 450.0; - case '5': return 2500.0; - case '6': return 2000.0; - case '9': return 1080.0; - default: return 10000.0; - } + case HardwareManufacturerType::HMT_MT: + switch (gyroscopeRangeField(productCode->c_str())) + { + case '0': + return 1000.0; + case '1': + return 180.0; + case '2': + return 1700.0; + case '3': + return 420.0; + case '4': + return 450.0; + case '5': + return 2500.0; + case '6': + return 2000.0; + case '9': + return 1080.0; + default: + return 10000.0; + } - default: - return 10000.0; + default: + return 10000.0; + } } -} } diff --git a/lib/xspublic/xstypes/xssensorranges.h b/lib/xspublic/xstypes/xssensorranges.h index 20f0075..d7aa78c 100644 --- a/lib/xspublic/xstypes/xssensorranges.h +++ b/lib/xspublic/xstypes/xssensorranges.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -79,41 +79,41 @@ enum HardwareManufacturerType }; typedef enum HardwareManufacturerType HardwareManufacturerType; -XSTYPES_DLL_API void findHardwareTypeC(const XsString *productCode, XsString* resultValue); -XSTYPES_DLL_API HardwareManufacturerType findHardwareManufacturerC(const XsString *productCode); +XSTYPES_DLL_API void findHardwareTypeC(const XsString* productCode, XsString* resultValue); +XSTYPES_DLL_API HardwareManufacturerType findHardwareManufacturerC(const XsString* productCode); -XSTYPES_DLL_API double accelerometerRangeC(const XsString *productCode, int32_t hwVersionMajor); -XSTYPES_DLL_API double gyroscopeRangeC(const XsString *productCode); +XSTYPES_DLL_API double accelerometerRangeC(const XsString* productCode, int32_t hwVersionMajor); +XSTYPES_DLL_API double gyroscopeRangeC(const XsString* productCode); -XSTYPES_DLL_API double actualAccelerometerRangeC(const XsString *productCode, int32_t hwVersionMajor); -XSTYPES_DLL_API double actualGyroscopeRangeC(const XsString *productCode); +XSTYPES_DLL_API double actualAccelerometerRangeC(const XsString* productCode, int32_t hwVersionMajor); +XSTYPES_DLL_API double actualGyroscopeRangeC(const XsString* productCode); #ifdef __cplusplus } -inline static XsString findHardwareType(const XsString &productCode) +inline static XsString findHardwareType(const XsString& productCode) { XsString rv; findHardwareTypeC(&productCode, &rv); return rv; } -inline static HardwareManufacturerType findHardwareManufacturer(const XsString &productCode) +inline static HardwareManufacturerType findHardwareManufacturer(const XsString& productCode) { return findHardwareManufacturerC(&productCode); } -inline static double accelerometerRange(const XsString &productCode, int32_t hwVersionMajor) +inline static double accelerometerRange(const XsString& productCode, int32_t hwVersionMajor) { return accelerometerRangeC(&productCode, hwVersionMajor); } -inline static double gyroscopeRange(const XsString &productCode) +inline static double gyroscopeRange(const XsString& productCode) { return gyroscopeRangeC(&productCode); } -inline static double actualAccelerometerRange(const XsString &productCode, int32_t hwVersionMajor) +inline static double actualAccelerometerRange(const XsString& productCode, int32_t hwVersionMajor) { return actualAccelerometerRangeC(&productCode, hwVersionMajor); } -inline static double actualGyroscopeRange(const XsString &productCode) +inline static double actualGyroscopeRange(const XsString& productCode) { return actualGyroscopeRangeC(&productCode); } diff --git a/lib/xspublic/xstypes/xssimpleversion.c b/lib/xspublic/xstypes/xssimpleversion.c index befaabc..d8b3ff8 100644 --- a/lib/xspublic/xstypes/xssimpleversion.c +++ b/lib/xspublic/xstypes/xssimpleversion.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -65,14 +65,14 @@ #include "xssimpleversion.h" #ifdef _WIN32 -#include + #include #else -#ifdef __GNUC__ -#include -#endif -#include -#include -#include + #ifdef __GNUC__ + #include + #endif + #include + #include + #include #endif /*! \class XsSimpleVersion @@ -123,7 +123,7 @@ void XsSimpleVersion_osVersion(XsSimpleVersion* thisPtr) if (!fetched) { #ifdef _WIN32 - NTSTATUS(WINAPI *RtlGetVersion)(LPOSVERSIONINFOEXW); + NTSTATUS(WINAPI * RtlGetVersion)(LPOSVERSIONINFOEXW); OSVERSIONINFOEXW osInfo; *(FARPROC*)&RtlGetVersion = GetProcAddress(GetModuleHandleA("ntdll"), "RtlGetVersion"); if (RtlGetVersion) diff --git a/lib/xspublic/xstypes/xssimpleversion.h b/lib/xspublic/xstypes/xssimpleversion.h index 4ff2bde..9bed2c4 100644 --- a/lib/xspublic/xstypes/xssimpleversion.h +++ b/lib/xspublic/xstypes/xssimpleversion.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -136,26 +136,35 @@ struct XsSimpleVersion return true; else if (m_major > other.m_major) return false; + + if (m_minor < other.m_minor) + return true; + else if (m_minor > other.m_minor) + return false; + + if (m_revision < other.m_revision) + return true; else - { - if (m_minor < other.m_minor) - return true; - else if (m_minor > other.m_minor) - return false; - else - { - if (m_revision < other.m_revision) - return true; - else - return false; - } - } + return false; } /*! \brief Test if the \a other version is lower or equal than this. */ inline bool operator <= (const XsSimpleVersion& other) const { - return (*this == other) || (*this < other); + if (m_major < other.m_major) + return true; + else if (m_major > other.m_major) + return false; + + if (m_minor < other.m_minor) + return true; + else if (m_minor > other.m_minor) + return false; + + if (m_revision < other.m_revision) + return true; + else + return m_revision == other.m_revision; } /*! \brief Test if the \a other version is higher than this. */ @@ -167,7 +176,7 @@ struct XsSimpleVersion /*! \brief Test if the \a other version is higher or equal than this. */ inline bool operator >= (const XsSimpleVersion& other) const { - return (*this == other) || (*this > other); + return !(*this < other); } //! \brief \copybrief XsSimpleVersion_empty @@ -177,16 +186,31 @@ struct XsSimpleVersion } //! \brief Return the \e major part of the version - inline int major() const { return (int) m_major; } + inline int major() const + { + return (int) m_major; + } //! \brief Return the \e minor part of the version - inline int minor() const { return (int) m_minor; } + inline int minor() const + { + return (int) m_minor; + } //! \brief Return the \e revision part of the version - inline int revision() const { return (int) m_revision; } + inline int revision() const + { + return (int) m_revision; + } //! \brief \copybrief XsSimpleVersion_osVersion inline static XsSimpleVersion osVersion() { - static XsSimpleVersion rv = []() { XsSimpleVersion rv; XsSimpleVersion_osVersion(&rv); return rv; }(); + static XsSimpleVersion rv = []() + { + XsSimpleVersion rv; + XsSimpleVersion_osVersion(&rv); + return rv; + } + (); return rv; } diff --git a/lib/xspublic/xstypes/xssnapshot.c b/lib/xspublic/xstypes/xssnapshot.c index ee66aca..543e1e9 100644 --- a/lib/xspublic/xstypes/xssnapshot.c +++ b/lib/xspublic/xstypes/xssnapshot.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xstypes/xssnapshot.h b/lib/xspublic/xstypes/xssnapshot.h index a68681b..e34a196 100644 --- a/lib/xspublic/xstypes/xssnapshot.h +++ b/lib/xspublic/xstypes/xssnapshot.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -113,7 +113,7 @@ struct XsSnapshot m_baro != other.m_baro || m_status != other.m_status || m_accClippingCounter != other.m_accClippingCounter || - m_gyrClippingCounter != other.m_gyrClippingCounter|| + m_gyrClippingCounter != other.m_gyrClippingCounter || m_type != other.m_type) return false; @@ -126,7 +126,7 @@ struct XsSnapshot } if (m_type == ST_Full) { - if (m_iQ[3] != other.m_iQ[3] || m_timestamp != other.m_timestamp ) + if (m_iQ[3] != other.m_iQ[3] || m_timestamp != other.m_timestamp) return false; } return true; diff --git a/lib/xspublic/xstypes/xssocket.c b/lib/xspublic/xstypes/xssocket.c index d8c5eea..916722b 100644 --- a/lib/xspublic/xstypes/xssocket.c +++ b/lib/xspublic/xstypes/xssocket.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -92,20 +92,20 @@ static int closesocket(SOCKET s) } #endif #if defined(__FreeBSD__) || defined(BSD) || defined(__APPLE__) || defined(__linux__) -# define USE_GETIFADDRS 1 -# include + #define USE_GETIFADDRS 1 + #include #endif #include // MSG_NOSIGNAL is linux stuff #ifndef MSG_NOSIGNAL -#define MSG_NOSIGNAL 0 + #define MSG_NOSIGNAL 0 #endif #include "xssocket.h" #ifndef PEEKBUFSIZE -#define PEEKBUFSIZE 32768 + #define PEEKBUFSIZE 32768 #endif /* the socket data */ @@ -138,13 +138,13 @@ static int socketIsUsable(SOCKET s) /*! - * \brief Get the IP address of \a remote. - * \param[in] remote the socket address info of the remote peer - * \param[in,out] address XsString to return the host IP address in. - */ -static void getRemoteHostAddress(const struct sockaddr_storage *remote, XsString *address) + \brief Get the IP address of \a remote. + \param[in] remote the socket address info of the remote peer + \param[in,out] address XsString to return the host IP address in. +*/ +static void getRemoteHostAddress(const struct sockaddr_storage* remote, XsString* address) { - void *src; + void* src; socklen_t length; switch (remote->ss_family) @@ -164,7 +164,7 @@ static void getRemoteHostAddress(const struct sockaddr_storage *remote, XsString XsString_erase(address, 0, address->m_size); } -/* Update the last result of thisPtr to retval, if thisPtr is non-null +/* Update the last result of thisPtr to retval, if thisPtr is non-null Returns retval for easy use in return statements. */ @@ -178,7 +178,7 @@ static XsResultValue setLastResult(XsSocket* thisPtr, XsResultValue retval, int return retval; } -/* Translate a platform error into an XsResultValue +/* Translate a platform error into an XsResultValue If thisPtr is non-null, the socket's last result will be updated. */ @@ -192,130 +192,130 @@ static XsResultValue translateAndReturnSocketError(XsSocket* thisPtr, int functi err = WSAGetLastError(); switch (err) { - case WSA_INVALID_HANDLE: - case WSA_INVALID_PARAMETER: - case WSAEINVAL: - result = XRV_INVALIDPARAM; - break; - case WSA_NOT_ENOUGH_MEMORY: - result = XRV_OUTOFMEMORY; - break; - case WSA_OPERATION_ABORTED: - result = XRV_ABORTED; - break; - case WSAEMSGSIZE: - result = XRV_BUFFEROVERFLOW; - break; - case WSAETIMEDOUT: - result = XRV_TIMEOUTNODATA; - break; - case WSAEACCES: - case WSAEADDRINUSE: - result = XRV_IN_USE; - break; - case WSA_IO_INCOMPLETE: - case WSA_IO_PENDING: - case WSAEINTR: - case WSAEBADF: - case WSAEFAULT: - case WSAEMFILE: - case WSAEWOULDBLOCK: - case WSAEINPROGRESS: - case WSAEALREADY: - case WSAENOTSOCK: - case WSAEDESTADDRREQ: - case WSAEPROTOTYPE: - case WSAENOPROTOOPT: - case WSAEPROTONOSUPPORT: - case WSAESOCKTNOSUPPORT: - case WSAEOPNOTSUPP: - case WSAEPFNOSUPPORT: - case WSAEAFNOSUPPORT: - case WSAEADDRNOTAVAIL: - case WSAENETDOWN: - case WSAENETUNREACH: - case WSAENETRESET: - case WSAECONNABORTED: - case WSAECONNRESET: - case WSAENOBUFS: - case WSAEISCONN: - case WSAENOTCONN: - case WSAESHUTDOWN: - case WSAETOOMANYREFS: - case WSAECONNREFUSED: - case WSAELOOP: - case WSAENAMETOOLONG: - case WSAEHOSTDOWN: - case WSAEHOSTUNREACH: - case WSAENOTEMPTY: - case WSAEPROCLIM: - case WSAEUSERS: - case WSAEDQUOT: - case WSAESTALE: - case WSAEREMOTE: - case WSASYSNOTREADY: - case WSAVERNOTSUPPORTED: - case WSANOTINITIALISED: - case WSAEDISCON: - case WSAENOMORE: - case WSAECANCELLED: - case WSAEINVALIDPROCTABLE: - case WSAEINVALIDPROVIDER: - case WSAEPROVIDERFAILEDINIT: - case WSASYSCALLFAILURE: - case WSASERVICE_NOT_FOUND: - case WSATYPE_NOT_FOUND: - case WSA_E_NO_MORE: - case WSA_E_CANCELLED: - case WSAEREFUSED: - case WSAHOST_NOT_FOUND: - case WSATRY_AGAIN: - case WSANO_RECOVERY: - case WSANO_DATA: - default: - result = XRV_OTHER; - break; + case WSA_INVALID_HANDLE: + case WSA_INVALID_PARAMETER: + case WSAEINVAL: + result = XRV_INVALIDPARAM; + break; + case WSA_NOT_ENOUGH_MEMORY: + result = XRV_OUTOFMEMORY; + break; + case WSA_OPERATION_ABORTED: + result = XRV_ABORTED; + break; + case WSAEMSGSIZE: + result = XRV_BUFFEROVERFLOW; + break; + case WSAETIMEDOUT: + result = XRV_TIMEOUTNODATA; + break; + case WSAEACCES: + case WSAEADDRINUSE: + result = XRV_IN_USE; + break; + case WSA_IO_INCOMPLETE: + case WSA_IO_PENDING: + case WSAEINTR: + case WSAEBADF: + case WSAEFAULT: + case WSAEMFILE: + case WSAEWOULDBLOCK: + case WSAEINPROGRESS: + case WSAEALREADY: + case WSAENOTSOCK: + case WSAEDESTADDRREQ: + case WSAEPROTOTYPE: + case WSAENOPROTOOPT: + case WSAEPROTONOSUPPORT: + case WSAESOCKTNOSUPPORT: + case WSAEOPNOTSUPP: + case WSAEPFNOSUPPORT: + case WSAEAFNOSUPPORT: + case WSAEADDRNOTAVAIL: + case WSAENETDOWN: + case WSAENETUNREACH: + case WSAENETRESET: + case WSAECONNABORTED: + case WSAECONNRESET: + case WSAENOBUFS: + case WSAEISCONN: + case WSAENOTCONN: + case WSAESHUTDOWN: + case WSAETOOMANYREFS: + case WSAECONNREFUSED: + case WSAELOOP: + case WSAENAMETOOLONG: + case WSAEHOSTDOWN: + case WSAEHOSTUNREACH: + case WSAENOTEMPTY: + case WSAEPROCLIM: + case WSAEUSERS: + case WSAEDQUOT: + case WSAESTALE: + case WSAEREMOTE: + case WSASYSNOTREADY: + case WSAVERNOTSUPPORTED: + case WSANOTINITIALISED: + case WSAEDISCON: + case WSAENOMORE: + case WSAECANCELLED: + case WSAEINVALIDPROCTABLE: + case WSAEINVALIDPROVIDER: + case WSAEPROVIDERFAILEDINIT: + case WSASYSCALLFAILURE: + case WSASERVICE_NOT_FOUND: + case WSATYPE_NOT_FOUND: + case WSA_E_NO_MORE: + case WSA_E_CANCELLED: + case WSAEREFUSED: + case WSAHOST_NOT_FOUND: + case WSATRY_AGAIN: + case WSANO_RECOVERY: + case WSANO_DATA: + default: + result = XRV_OTHER; + break; } #else err = errno; switch (err) { - case EROFS: - result = XRV_READONLY; - break; - case EACCES: - result = XRV_INPUTCANNOTBEOPENED; - break; - case EADDRINUSE: - result = XRV_ALREADYOPEN; - break; - case EBADF: - case EINVAL: - case ENOTDIR: - case EFAULT: - result = XRV_INVALIDPARAM; - break; - case ENAMETOOLONG: - result = XRV_DATAOVERFLOW; - break; - case ENOTSOCK: - result = XRV_UNSUPPORTED; - break; - case EADDRNOTAVAIL: - case ENOENT: - result = XRV_NOTFOUND; - break; - case ELOOP: - case ENOMEM: - result = XRV_OUTOFMEMORY; - break; - case ETIME: - case ETIMEDOUT: - result = XRV_TIMEOUTNODATA; - break; - default: - result = XRV_OTHER; - break; + case EROFS: + result = XRV_READONLY; + break; + case EACCES: + result = XRV_INPUTCANNOTBEOPENED; + break; + case EADDRINUSE: + result = XRV_ALREADYOPEN; + break; + case EBADF: + case EINVAL: + case ENOTDIR: + case EFAULT: + result = XRV_INVALIDPARAM; + break; + case ENAMETOOLONG: + result = XRV_DATAOVERFLOW; + break; + case ENOTSOCK: + result = XRV_UNSUPPORTED; + break; + case EADDRNOTAVAIL: + case ENOENT: + result = XRV_NOTFOUND; + break; + case ELOOP: + case ENOMEM: + result = XRV_OUTOFMEMORY; + break; + case ETIME: + case ETIMEDOUT: + result = XRV_TIMEOUTNODATA; + break; + default: + result = XRV_OTHER; + break; } #endif return setLastResult(thisPtr, result, err); @@ -326,7 +326,7 @@ static void translateSocketError(XsSocket* thisPtr, int functionResult) (void)translateAndReturnSocketError(thisPtr, functionResult); } -/* Initialize the socket +/* Initialize the socket This function performs some basic initialization on the socket */ @@ -337,7 +337,7 @@ static void XsSocket_initialize(XsSocket* thisPtr, XsDataFlags flags) thisPtr->d->m_sd = INVALID_SOCKET; thisPtr->d->m_flags = flags; #ifdef _WIN32 - (void)WSAStartup(MAKEWORD(2,0), &thisPtr->d->m_sockData); + (void)WSAStartup(MAKEWORD(2, 0), &thisPtr->d->m_sockData); #endif } @@ -360,13 +360,13 @@ void XsSocket_create(XsSocket* thisPtr, enum NetworkLayerProtocol ip, enum IpPro thisPtr->d->m_ipProtocol = protocol; } -/* Create a socket from a native socket +/* Create a socket from a native socket Usually we expect that theirinfo and infolen are filled in. If theirInfo is NULL though, we will fetch the information from the socket. Doing so by default would add a possible extra point of failure. */ -void XsSocket_createFromNativeSocket(XsSocket* thisPtr, SOCKET nativeSocket, struct sockaddr const *theirInfo, socklen_t infolen, XsDataFlags flags) +void XsSocket_createFromNativeSocket(XsSocket* thisPtr, SOCKET nativeSocket, struct sockaddr const* theirInfo, socklen_t infolen, XsDataFlags flags) { XsSocket_initialize(thisPtr, flags); thisPtr->d->m_sd = nativeSocket; @@ -386,39 +386,39 @@ void XsSocket_createFromNativeSocket(XsSocket* thisPtr, SOCKET nativeSocket, str switch (thisPtr->d->m_remoteAddr.ss_family) { - case PF_INET6: - thisPtr->d->m_ipVersion = NLP_IPV6; - break; - case PF_INET: - thisPtr->d->m_ipVersion = NLP_IPV4; - break; - default: - // IRDA and the likes. do we need it? - break; + case PF_INET6: + thisPtr->d->m_ipVersion = NLP_IPV6; + break; + case PF_INET: + thisPtr->d->m_ipVersion = NLP_IPV4; + break; + default: + // IRDA and the likes. do we need it? + break; } } /*! - * \brief Create a socket from a native file descriptor. - * \param[in] sockfd the file descriptor of the underlying socket - * \param[in] flags flags to inicate if the underlying socket should be managed by this object - * \relates XsSocket - * - * If the socket should be closed when this XsSocket is destroyed then the flags - * should be set to XSDF_Managed. - */ + \brief Create a socket from a native file descriptor. + \param[in] sockfd the file descriptor of the underlying socket + \param[in] flags flags to inicate if the underlying socket should be managed by this object + \relates XsSocket + + If the socket should be closed when this XsSocket is destroyed then the flags + should be set to XSDF_Managed. +*/ void XsSocket_createFromFileDescriptor(XsSocket* thisPtr, int sockfd, XsDataFlags flags) { XsSocket_createFromNativeSocket(thisPtr, sockfd, NULL, 0, flags); } /*! \brief Return the native file descriptor - * - * The native socket descriptor returned from this function should only be - * passed to third party libraries. However, it is possible to select, read and - * write on it, if you know what you're doing. Do remember that this object - * still manages the lifetime of the file descriptor. - */ + + The native socket descriptor returned from this function should only be + passed to third party libraries. However, it is possible to select, read and + write on it, if you know what you're doing. Do remember that this object + still manages the lifetime of the file descriptor. +*/ XSOCKET XsSocket_nativeDescriptor(XsSocket const* thisPtr) { return thisPtr->d->m_sd; @@ -459,9 +459,7 @@ void XsSocket_destroy(XsSocket* thisPtr) if (thisPtr->d) { if ((thisPtr->d->m_flags & XSDF_Managed) != 0) - { (void)XsSocket_close(thisPtr); - } free(thisPtr->d); thisPtr->d = NULL; #ifdef _WIN32 @@ -484,7 +482,7 @@ void XsSocket_destroy(XsSocket* thisPtr) If only one of canRead or canWrite points to non-null, a positive non-zero return value already indicates the filled in value is set to non-zero. */ -int XsSocket_select(XsSocket* thisPtr, int mstimeout, int *canRead, int *canWrite) +int XsSocket_select(XsSocket* thisPtr, int mstimeout, int* canRead, int* canWrite) { fd_set readfd; fd_set writefd; @@ -503,34 +501,34 @@ int XsSocket_select(XsSocket* thisPtr, int mstimeout, int *canRead, int *canWrit if (canWrite) *canWrite = 0; - timeout.tv_sec = mstimeout/1000; - timeout.tv_usec = (mstimeout%1000) * 1000; + timeout.tv_sec = mstimeout / 1000; + timeout.tv_usec = (mstimeout % 1000) * 1000; rv = select(FD_SETSIZE, (canRead ? &readfd : NULL), - (canWrite ? &writefd : NULL), - &errorfd, mstimeout >= 0 ? &timeout : NULL); + (canWrite ? &writefd : NULL), + &errorfd, mstimeout >= 0 ? &timeout : NULL); switch (rv) { - case -1: - translateSocketError(thisPtr, rv); - break; - case 0: - (void)setLastResult(thisPtr, XRV_TIMEOUT, 0); - break; - default: - if (FD_ISSET(thisPtr->d->m_sd, &errorfd)) - { - (void)setLastResult(thisPtr, XRV_ERROR, 0); - rv = -1; + case -1: + translateSocketError(thisPtr, rv); break; - } + case 0: + (void)setLastResult(thisPtr, XRV_TIMEOUT, 0); + break; + default: + if (FD_ISSET(thisPtr->d->m_sd, &errorfd)) + { + (void)setLastResult(thisPtr, XRV_ERROR, 0); + rv = -1; + break; + } - if (canRead) - *canRead = FD_ISSET(thisPtr->d->m_sd, &readfd); - if (canWrite) - *canWrite = FD_ISSET(thisPtr->d->m_sd, &writefd); - break; + if (canRead) + *canRead = FD_ISSET(thisPtr->d->m_sd, &readfd); + if (canWrite) + *canWrite = FD_ISSET(thisPtr->d->m_sd, &writefd); + break; } return rv; } @@ -588,7 +586,7 @@ int XsSocket_readFrom(XsSocket* thisPtr, void* dest, XsSize size, XsString* host if (!dest) return peekPendingDataSize(thisPtr); - rv = recvfrom(thisPtr->d->m_sd, dest, (int)size, 0, (struct sockaddr *)&sender, &l); + rv = recvfrom(thisPtr->d->m_sd, dest, (int)size, 0, (struct sockaddr*)&sender, &l); if (hostname) getRemoteHostAddress(&sender, hostname); @@ -638,7 +636,7 @@ int XsSocket_readFrom2ByteArray(XsSocket* thisPtr, XsByteArray* dest, XsString* if (!dest) return peekPendingDataSize(thisPtr); - rv = recvfrom(thisPtr->d->m_sd, thisPtr->d->m_peekBuf, PEEKBUFSIZE, 0, (struct sockaddr *)&sender, &l); + rv = recvfrom(thisPtr->d->m_sd, thisPtr->d->m_peekBuf, PEEKBUFSIZE, 0, (struct sockaddr*)&sender, &l); if (rv <= 0) { translateSocketError(thisPtr, rv); @@ -678,7 +676,7 @@ int XsSocket_write(XsSocket* thisPtr, const void* data, XsSize size) /* Return non-zero if the hostname is actually an IPv4 address */ int isIPv4Address(XsString const* hostname) { - char *c; + char* c; int expectNum = 1; int expectDot = 0; int numbersFound = 0; @@ -706,9 +704,7 @@ int isIPv4Address(XsString const* hostname) expectNum = 0; } else - { return 0; - } } return 1; } @@ -731,7 +727,7 @@ void XsSocket_fixupHostname(XsSocket const* thisPtr, XsString* hostname) typedef int (*lookupTestFunction)(XsSocket* thisPtr, SOCKET currentSocket, struct addrinfo const* info); -/* Do a lookup of the given hostname and port +/* Do a lookup of the given hostname and port This is an internal function that centralizes the lookup code. @@ -746,10 +742,10 @@ typedef int (*lookupTestFunction)(XsSocket* thisPtr, SOCKET currentSocket, struc return OK or NOTFOUND */ static XsResultValue XsSocket_internalLookup(XsSocket* thisPtr, const XsString* hostname, uint16_t port, - int hints_flags, lookupTestFunction tester, - struct sockaddr* info, socklen_t* addrlen) + int hints_flags, lookupTestFunction tester, + struct sockaddr* info, socklen_t* addrlen) { - struct addrinfo *lookupInfo, *p; + struct addrinfo* lookupInfo, *p; SOCKET s; char gaport[7]; struct addrinfo hints; @@ -758,9 +754,15 @@ static XsResultValue XsSocket_internalLookup(XsSocket* thisPtr, const XsString* switch (thisPtr->d->m_ipVersion) { - case NLP_IPV6: hints.ai_family = AF_INET6; break; - case NLP_IPV4: hints.ai_family = AF_INET; break; - case NLP_IPVX: hints.ai_family = AF_UNSPEC; break; + case NLP_IPV6: + hints.ai_family = AF_INET6; + break; + case NLP_IPV4: + hints.ai_family = AF_INET; + break; + case NLP_IPVX: + hints.ai_family = AF_UNSPEC; + break; } hints.ai_socktype = (thisPtr->d->m_ipProtocol == IP_UDP) ? SOCK_DGRAM : SOCK_STREAM; hints.ai_flags = hints_flags; @@ -780,33 +782,31 @@ static XsResultValue XsSocket_internalLookup(XsSocket* thisPtr, const XsString* XsString_destruct(&host); } else - { ret = getaddrinfo(NULL, gaport, &hints, &lookupInfo); - } if (ret) { switch (ret) { - case EAI_BADFLAGS: - return setLastResult(thisPtr, XRV_INVALIDPARAM, -1); - case EAI_AGAIN: - return translateAndReturnSocketError(thisPtr, EAGAIN); - case EAI_FAIL: - return setLastResult(thisPtr, XRV_ERROR, -1); - case EAI_MEMORY: - return setLastResult(thisPtr, XRV_INSUFFICIENTSPACE, -1); - //case EAI_NODATA: - // return setLastResult(thisPtr, XRV_TIMEOUTNODATA, -1); - case EAI_NONAME: - return setLastResult(thisPtr, XRV_INSUFFICIENTDATA, -1); - //case EAI_ADDRFAMILY: - case EAI_SERVICE: - case EAI_FAMILY: - case EAI_SOCKTYPE: - return setLastResult(thisPtr, XRV_UNSUPPORTED, -1); - default: - return translateAndReturnSocketError(thisPtr, ret); + case EAI_BADFLAGS: + return setLastResult(thisPtr, XRV_INVALIDPARAM, -1); + case EAI_AGAIN: + return translateAndReturnSocketError(thisPtr, EAGAIN); + case EAI_FAIL: + return setLastResult(thisPtr, XRV_ERROR, -1); + case EAI_MEMORY: + return setLastResult(thisPtr, XRV_INSUFFICIENTSPACE, -1); + //case EAI_NODATA: + // return setLastResult(thisPtr, XRV_TIMEOUTNODATA, -1); + case EAI_NONAME: + return setLastResult(thisPtr, XRV_INSUFFICIENTDATA, -1); + //case EAI_ADDRFAMILY: + case EAI_SERVICE: + case EAI_FAMILY: + case EAI_SOCKTYPE: + return setLastResult(thisPtr, XRV_UNSUPPORTED, -1); + default: + return translateAndReturnSocketError(thisPtr, ret); } } @@ -906,9 +906,9 @@ int XsSocket_enableBroadcasts(XsSocket* thisPtr, int enable) } #ifdef USE_GETIFADDRS -static uint32_t SockAddrToUint32(struct sockaddr * a) +static uint32_t SockAddrToUint32(struct sockaddr* a) { - return ((a)&&(a->sa_family == AF_INET)) ? ntohl(((struct sockaddr_in *)a)->sin_addr.s_addr) : 0; + return ((a) && (a->sa_family == AF_INET)) ? ntohl(((struct sockaddr_in*)a)->sin_addr.s_addr) : 0; } #endif @@ -945,16 +945,16 @@ int XsSocket_broadcast(XsSocket* thisPtr, const void* data, XsSize size, uint16_ do { // Windows XP style implementation - MIB_IPADDRTABLE * ipTable = NULL; + MIB_IPADDRTABLE* ipTable = NULL; { ULONG bufLen = 0; - for (int i=0; i<5; i++) + for (int i = 0; i < 5; i++) { DWORD ipRet = GetIpAddrTable(ipTable, &bufLen, FALSE); if (ipRet == ERROR_INSUFFICIENT_BUFFER) { free(ipTable); // in case we had previously allocated it - ipTable = (MIB_IPADDRTABLE *) malloc(bufLen); + ipTable = (MIB_IPADDRTABLE*) malloc(bufLen); } else if (ipRet == NO_ERROR) break; @@ -969,16 +969,16 @@ int XsSocket_broadcast(XsSocket* thisPtr, const void* data, XsSize size, uint16_ if (ipTable) { - IP_ADAPTER_INFO * pAdapterInfo = NULL; + IP_ADAPTER_INFO* pAdapterInfo = NULL; { ULONG bufLen = 0; - for (int i=0; i<5; i++) + for (int i = 0; i < 5; i++) { DWORD apRet = GetAdaptersInfo(pAdapterInfo, &bufLen); if (apRet == ERROR_BUFFER_OVERFLOW) { free(pAdapterInfo); // in case we had previously allocated it - pAdapterInfo = (IP_ADAPTER_INFO *) malloc(bufLen); + pAdapterInfo = (IP_ADAPTER_INFO*) malloc(bufLen); } else if (apRet == ERROR_SUCCESS) break; @@ -991,12 +991,12 @@ int XsSocket_broadcast(XsSocket* thisPtr, const void* data, XsSize size, uint16_ } } - for (DWORD i=0; idwNumEntries; i++) + for (DWORD i = 0; i < ipTable->dwNumEntries; i++) { const MIB_IPADDRROW* row = &ipTable->table[i]; uint32_t ad = ntohl(row->dwAddr) | ~ntohl(row->dwMask); char bcastAddr[32]; - sprintf(bcastAddr, "%u.%u.%u.%u", (ad >> 24)&0xFF, (ad >> 16) & 0xFF, (ad >> 8) & 0xFF, ad & 0xFF); + sprintf(bcastAddr, "%u.%u.%u.%u", (ad >> 24) & 0xFF, (ad >> 16) & 0xFF, (ad >> 8) & 0xFF, ad & 0xFF); inet_pton(AF_INET, bcastAddr, &addr.sin_addr); sent = sendto(thisPtr->d->m_sd, data, (int)size, MSG_NOSIGNAL, (struct sockaddr const*) &addr, sizeof(addr)); if (sent < 0) @@ -1011,21 +1011,20 @@ int XsSocket_broadcast(XsSocket* thisPtr, const void* data, XsSize size, uint16_ free(pAdapterInfo); free(ipTable); } - } - while (0); + } while (0); #elif defined(USE_GETIFADDRS) // BSD-style implementation - struct ifaddrs * ifap; + struct ifaddrs* ifap; if (getifaddrs(&ifap) == 0) { - struct ifaddrs * p = ifap; + struct ifaddrs* p = ifap; while (p) { uint32_t bcastAddr = (uint32_t)SockAddrToUint32(p->ifa_dstaddr); if (bcastAddr > 0) { char bcastAddrStr[32]; - sprintf(bcastAddrStr, "%u.%u.%u.%u", (bcastAddr >> 24)&0xFF, (bcastAddr >> 16) & 0xFF, (bcastAddr >> 8) & 0xFF, bcastAddr & 0xFF); + sprintf(bcastAddrStr, "%u.%u.%u.%u", (bcastAddr >> 24) & 0xFF, (bcastAddr >> 16) & 0xFF, (bcastAddr >> 8) & 0xFF, bcastAddr & 0xFF); inet_pton(AF_INET, bcastAddrStr, &addr.sin_addr); sent = (int)sendto(thisPtr->d->m_sd, data, size, MSG_NOSIGNAL, (struct sockaddr const*) &addr, sizeof(addr)); if (sent < 0) @@ -1081,10 +1080,10 @@ void XsSocket_flush(XsSocket* thisPtr) \returns a pointer to a newly created socket for the new connection. NULL on error. \relates XsSocket - */ +*/ XsSocket* XsSocket_accept(XsSocket* thisPtr, int mstimeout) { - XsSocket *ns; + XsSocket* ns; struct sockaddr_storage theirInfo; socklen_t infoLength = sizeof(struct sockaddr_storage); SOCKET sd; @@ -1106,7 +1105,7 @@ XsSocket* XsSocket_accept(XsSocket* thisPtr, int mstimeout) } } - sd = accept(thisPtr->d->m_sd, (struct sockaddr *)&theirInfo, &infoLength); + sd = accept(thisPtr->d->m_sd, (struct sockaddr*)&theirInfo, &infoLength); if (!socketIsUsable(sd)) return NULL; @@ -1145,23 +1144,23 @@ void XsSocket_free(XsSocket* thisPtr) } /*! \brief Changes the value of a socket option - * \param[in] option the socket option to change - * \param[in] valuePtr poins to the value the option must be set to - * \param[in] valueSize the size of the value \a valuePtr points to - * \return an XsResultValue indicating the result of the operation, possibly pointing towards a cause - * \relates XsSocket - */ -XsResultValue XsSocket_setSocketOption(XsSocket *thisPtr, enum XsSocketOption option, void* valuePtr, int valueSize) + \param[in] option the socket option to change + \param[in] valuePtr poins to the value the option must be set to + \param[in] valueSize the size of the value \a valuePtr points to + \return an XsResultValue indicating the result of the operation, possibly pointing towards a cause + \relates XsSocket +*/ +XsResultValue XsSocket_setSocketOption(XsSocket* thisPtr, enum XsSocketOption option, void* valuePtr, int valueSize) { int res; int nativeOption; #ifdef _WIN32 - const char *valPtr = (const char*)valuePtr; + const char* valPtr = (const char*)valuePtr; #else - const void *valPtr = valuePtr; + const void* valPtr = valuePtr; #endif - switch(option) + switch (option) { case XSO_ReuseAddress: nativeOption = SO_REUSEADDR; @@ -1182,7 +1181,7 @@ XsResultValue XsSocket_setSocketOption(XsSocket *thisPtr, enum XsSocketOption op } -/* test if we can bind to info +/* test if we can bind to info keeps the bind alive after leaving the function */ @@ -1191,9 +1190,9 @@ static int binder(XsSocket* thisPtr, SOCKET currentSocket, struct addrinfo const int res; int yesval = 1; #ifdef _WIN32 - const char *yes = (const char*)&yesval; + const char* yes = (const char*)&yesval; #else - const void *yes = &yesval; + const void* yes = &yesval; #endif (void)currentSocket; @@ -1231,7 +1230,7 @@ XsResultValue XsSocket_bind(XsSocket* thisPtr, const XsString* hostname, uint16_ if (thisPtr->d->m_ipVersion == NLP_IPV4) { - struct sockaddr_in *sin = (struct sockaddr_in*)&s; + struct sockaddr_in* sin = (struct sockaddr_in*)&s; sin->sin_family = AF_INET; sin->sin_port = htons(port); sin->sin_addr.s_addr = INADDR_ANY; @@ -1239,7 +1238,7 @@ XsResultValue XsSocket_bind(XsSocket* thisPtr, const XsString* hostname, uint16_ } else { - struct sockaddr_in6 *sin6 = (struct sockaddr_in6*)&s; + struct sockaddr_in6* sin6 = (struct sockaddr_in6*)&s; sin6->sin6_family = AF_INET6; sin6->sin6_port = htons(port); sin6->sin6_addr = in6addr_any; @@ -1263,7 +1262,7 @@ XsResultValue XsSocket_listen(XsSocket* thisPtr, int maxPending) return translateAndReturnSocketError(thisPtr, r); } -/* Connect to info +/* Connect to info Keep the connection alive */ @@ -1310,18 +1309,18 @@ int XsSocket_isUsable(const XsSocket* thisPtr) } /*! - * \brief Get the numeric IP address of remote host of this socket. - * \param[in,out] address XsString to return the remote host IP address in - */ -void XsSocket_getRemoteAddress(const XsSocket* thisPtr, XsString *address) + \brief Get the numeric IP address of remote host of this socket. + \param[in,out] address XsString to return the remote host IP address in +*/ +void XsSocket_getRemoteAddress(const XsSocket* thisPtr, XsString* address) { getRemoteHostAddress(&thisPtr->d->m_remoteAddr, address); } /*! - * \brief Return the system error code of the last socket operation - * \return The error code - */ + \brief Return the system error code of the last socket operation + \return The error code +*/ int XsSocket_getLastSystemError(const XsSocket* thisPtr) { return thisPtr->d->m_lastSystemError; diff --git a/lib/xspublic/xstypes/xssocket.h b/lib/xspublic/xstypes/xssocket.h index fde6c00..418e48d 100644 --- a/lib/xspublic/xstypes/xssocket.h +++ b/lib/xspublic/xstypes/xssocket.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -75,31 +75,34 @@ struct XsSocketInterface; typedef struct XsSocket XsSocket; /*! \brief the protocol on top of IP */ -enum IpProtocol { +enum IpProtocol +{ IP_UDP, /*!< \brief The UDP/IP protocol */ IP_TCP /*!< \brief The TCP/IP protocol */ }; /*! \brief The network layer protocol, or IP address family */ -enum NetworkLayerProtocol { +enum NetworkLayerProtocol +{ NLP_IPV4, /*!< \brief IPv4 address family */ NLP_IPV6, /*!< \brief IPv6 address family */ NLP_IPVX /*!< \brief any IP address family */ }; -enum XsSocketOption { +enum XsSocketOption +{ XSO_ReuseAddress, /*!< \brief Allow bind to reuse a local port (using different local addresses) */ XSO_ReusePort /*!< \brief Allow bind to reuse the exact local address */ }; #ifdef WIN32 -# if defined(_WIN64) - typedef unsigned __int64 XSOCKET; -# else - typedef unsigned int XSOCKET; -# endif + #if defined(_WIN64) + typedef unsigned __int64 XSOCKET; + #else + typedef unsigned int XSOCKET; + #endif #else -typedef int XSOCKET; + typedef int XSOCKET; #endif #ifdef __cplusplus @@ -118,7 +121,7 @@ XSTYPES_DLL_API XsResultValue XsSocket_close(XsSocket* thisPtr); XSTYPES_DLL_API XSOCKET XsSocket_nativeDescriptor(XsSocket const* thisPtr); -XSTYPES_DLL_API int XsSocket_select(XsSocket* thisPtr, int mstimeout, int *canRead, int *canWrite); +XSTYPES_DLL_API int XsSocket_select(XsSocket* thisPtr, int mstimeout, int* canRead, int* canWrite); XSTYPES_DLL_API int XsSocket_read(XsSocket* thisPtr, void* dest, XsSize size, int timeout); XSTYPES_DLL_API int XsSocket_readFrom(XsSocket* thisPtr, void* dest, XsSize size, XsString* hostname, uint16_t* port, int timeout); XSTYPES_DLL_API int XsSocket_read2ByteArray(XsSocket* thisPtr, XsByteArray* dest, int timeout); @@ -140,7 +143,7 @@ XSTYPES_DLL_API XsResultValue XsSocket_listen(XsSocket* thisPtr, int maxPending) XSTYPES_DLL_API XsResultValue XsSocket_connect(XsSocket* thisPtr, const XsString* host, uint16_t port); XSTYPES_DLL_API int XsSocket_isUsable(const XsSocket* thisPtr); -XSTYPES_DLL_API void XsSocket_getRemoteAddress(const XsSocket* thisPtr, XsString *address); +XSTYPES_DLL_API void XsSocket_getRemoteAddress(const XsSocket* thisPtr, XsString* address); XSTYPES_DLL_API int XsSocket_getLastSystemError(const XsSocket* thisPtr); XSTYPES_DLL_API int XsSocket_enableBroadcasts(XsSocket* thisPtr, int enable); XSTYPES_DLL_API int XsSocket_broadcast(XsSocket* thisPtr, const void* buffer, XsSize size, uint16_t port); @@ -322,7 +325,7 @@ struct XsSocket } /*! \brief \copybrief XsSocket_flush - */ + */ void flush() { XsSocket_flush(this); @@ -334,7 +337,7 @@ struct XsSocket \param[in,out] canWrite if not null then will be set to non-zero to indicate that the socket can be written to. \returns -1 on error, 0 on timeout, or a positive number otherwise */ - int select(int mstimeout, int *canRead, int *canWrite) + int select(int mstimeout, int* canRead, int* canWrite) { return XsSocket_select(this, mstimeout, canRead, canWrite); } @@ -353,16 +356,16 @@ struct XsSocket } /*! \brief Allocate memory for a socket */ - inline void* operator new(size_t) + inline void* operator new (size_t) { - XsSocket *s = XsSocket_allocate(); + XsSocket* s = XsSocket_allocate(); if (!s) throw std::bad_alloc(); return s; } /*! \brief De-allocate the socket */ - inline void operator delete(void* p) noexcept + inline void operator delete (void* p) noexcept { XsSocket_free(reinterpret_cast(p)); } @@ -497,7 +500,7 @@ struct XsSocket } private: #endif - struct XsSocketPrivate *d; + struct XsSocketPrivate* d; }; #endif diff --git a/lib/xspublic/xstypes/xsstatusflag.h b/lib/xspublic/xstypes/xsstatusflag.h index 1dd0142..0d30baf 100644 --- a/lib/xspublic/xstypes/xsstatusflag.h +++ b/lib/xspublic/xstypes/xsstatusflag.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -78,39 +78,41 @@ enum XsStatusFlag { - XSF_SelfTestOk = 0x01 //!< Is set when the self test result was ok - ,XSF_OrientationValid = 0x02 //!< Is set when the computed orientation is valid. The orientation may be invalid during startup or when the sensor data is clipping during violent (for the device) motion - ,XSF_GpsValid = 0x04 //!< Is set when the device has a GPS receiver and the receiver says that there is a GPS position fix. - - ,XSF_NoRotationMask = 0x18 //!< If all of these flags are set, the No Rotation algorithm is running - ,XSF_NoRotationAborted = 0x10 //!< If only this flag is set (out of the XSF_NoRotationMask) then the No Rotation algorithm was aborted - ,XSF_NoRotationSamplesRejected = 0x08 //!< If only this flag is set (out of the XSF_NoRotationMask) then the No Rotation algorithm is running but has rejected samples - ,XSF_NoRotationRunningNormally = 0x18 //!< If all these flags are set (out of the XSF_NoRotationMask) then the No Rotation algorithm is running normally - - ,XSF_RepresentativeMotion = 0x20 //!< Indicates if the In-Run Compass Calibration is doing the representative motion analysis - - ,XSF_ExternalClockSynced = 0x40 //!< Indicates whether the internal clock is synced with an external clock (Either GNNS or custom provided clock sync) - - ,XSF_ClipAccX = 0x00000100 - ,XSF_ClipAccY = 0x00000200 - ,XSF_ClipAccZ = 0x00000400 - ,XSF_ClipGyrX = 0x00000800 - ,XSF_ClipGyrY = 0x00001000 - ,XSF_ClipGyrZ = 0x00002000 - ,XSF_ClipMagX = 0x00004000 - ,XSF_ClipMagY = 0x00008000 - ,XSF_ClipMagZ = 0x00010000 - - ,XSF_Retransmitted = 0x00040000 //!< When set Indicates the sample was received as a retransmission - ,XSF_ClippingDetected = 0x00080000 //!< When set Indicates clipping has occurred - ,XSF_Interpolated = 0x00100000 //!< When set Indicates the sample is an interpolation between other samples - ,XSF_SyncIn = 0x00200000 //!< When set indicates a sync-in event has been triggered - ,XSF_SyncOut = 0x00400000 //!< When set Indicates a sync-out event has been generated - - ,XSF_FilterMode = 0x03800000 //!< Mask for the 3 bit filter mode field - ,XSF_HaveGnssTimePulse = 0x04000000 //!< Indicates that the 1PPS GNSS time pulse is present - - ,XSF_RtkStatus = 0x18000000 //!< Mask for 2 bit RTK status field 00: No RTK; 01: RTK floating; 10: RTK fixed + XSF_SelfTestOk = 0x01 //!< Is set when the self test result was ok + , XSF_OrientationValid = 0x02 //!< Is set when the computed orientation is valid. The orientation may be invalid during startup or when the sensor data is clipping during violent (for the device) motion + , XSF_GpsValid = 0x04 //!< Is set when the device has a GPS receiver and the receiver says that there is a GPS position fix. + + , XSF_NoRotationMask = 0x18 //!< If all of these flags are set, the No Rotation algorithm is running + , XSF_NoRotationAborted = 0x10 //!< If only this flag is set (out of the XSF_NoRotationMask) then the No Rotation algorithm was aborted because of movement of the device + , XSF_NoRotationSamplesRejected = 0x08 //!< If only this flag is set (out of the XSF_NoRotationMask) then the No Rotation algorithm is running but has rejected samples + , XSF_NoRotationRunningNormally = 0x18 //!< If all these flags are set (out of the XSF_NoRotationMask) then the No Rotation algorithm is running normally + + , XSF_RepresentativeMotion = 0x20 //!< Indicates if the In-Run Compass Calibration is doing the representative motion analysis + + , XSF_ExternalClockSynced = 0x40 //!< Indicates whether the internal clock is synced with an external clock (Either GNNS or custom provided clock sync) + + , XSF_FilterInputStart = 0x80 //!< Marks that the corresponding data is the first data fed to the (onboard) filter. + + , XSF_ClipAccX = 0x00000100 + , XSF_ClipAccY = 0x00000200 + , XSF_ClipAccZ = 0x00000400 + , XSF_ClipGyrX = 0x00000800 + , XSF_ClipGyrY = 0x00001000 + , XSF_ClipGyrZ = 0x00002000 + , XSF_ClipMagX = 0x00004000 + , XSF_ClipMagY = 0x00008000 + , XSF_ClipMagZ = 0x00010000 + + , XSF_Retransmitted = 0x00040000 //!< When set Indicates the sample was received as a retransmission + , XSF_ClippingDetected = 0x00080000 //!< When set Indicates clipping has occurred + , XSF_Interpolated = 0x00100000 //!< When set Indicates the sample is an interpolation between other samples + , XSF_SyncIn = 0x00200000 //!< When set indicates a sync-in event has been triggered + , XSF_SyncOut = 0x00400000 //!< When set Indicates a sync-out event has been generated + + , XSF_FilterMode = 0x03800000 //!< Mask for the 3 bit filter mode field + , XSF_HaveGnssTimePulse = 0x04000000 //!< Indicates that the 1PPS GNSS time pulse is present + + , XSF_RtkStatus = 0x18000000 //!< Mask for 2 bit RTK status field 00: No RTK; 01: RTK floating; 10: RTK fixed }; /*! \brief Status flag bit offsets @@ -119,37 +121,38 @@ enum XsStatusFlag offsets. \sa XsStatusFlag */ -enum XsStatusFlagOffset { - XSFO_OffsetSelfTestOk = 0 - ,XSFO_OffsetOrientationValid = 1 - ,XSFO_OffsetGpsValid = 2 - ,XSFO_OffsetNoRotation = 3 - - ,XSFO_OffsetRepresentativeMotion = 5 - ,XSFO_OffsetExternalClockSynced = 6 - - ,XSFO_OffsetClipAccX = 8 - ,XSFO_OffsetClipAccY = 9 - ,XSFO_OffsetClipAccZ = 10 - ,XSFO_OffsetClipGyrX = 11 - ,XSFO_OffsetClipGyrY = 12 - ,XSFO_OffsetClipGyrZ = 13 - ,XSFO_OffsetClipMagX = 14 - ,XSFO_OffsetClipMagY = 15 - ,XSFO_OffsetClipMagZ = 16 - - ,XSFO_Retransmitted = 18 - ,XSFO_ClippingDetected = 19 - ,XSFO_Interpolated = 20 - ,XSFO_SyncIn = 21 - ,XSFO_SyncOut = 22 - - ,XSFO_FilterMode = 23 // bits 23 -> 23 + XSFO_FilterModeNrOfBits - 1 - ,XSFO_FilterModeNrOfBits = 3 // note: bit 26 is reserved for future use - - ,XSFO_HaveGnssTimePulse = 26 - ,XSFO_RtkStatus = 27 - ,XSFO_RtkStatusNrOfBits = 2 +enum XsStatusFlagOffset +{ + XSFO_OffsetSelfTestOk = 0 + , XSFO_OffsetOrientationValid = 1 + , XSFO_OffsetGpsValid = 2 + , XSFO_OffsetNoRotation = 3 + + , XSFO_OffsetRepresentativeMotion = 5 + , XSFO_OffsetExternalClockSynced = 6 + + , XSFO_OffsetClipAccX = 8 + , XSFO_OffsetClipAccY = 9 + , XSFO_OffsetClipAccZ = 10 + , XSFO_OffsetClipGyrX = 11 + , XSFO_OffsetClipGyrY = 12 + , XSFO_OffsetClipGyrZ = 13 + , XSFO_OffsetClipMagX = 14 + , XSFO_OffsetClipMagY = 15 + , XSFO_OffsetClipMagZ = 16 + + , XSFO_Retransmitted = 18 + , XSFO_ClippingDetected = 19 + , XSFO_Interpolated = 20 + , XSFO_SyncIn = 21 + , XSFO_SyncOut = 22 + + , XSFO_FilterMode = 23 // bits 23 -> 23 + XSFO_FilterModeNrOfBits - 1 + , XSFO_FilterModeNrOfBits = 3 // note: bit 26 is reserved for future use + + , XSFO_HaveGnssTimePulse = 26 + , XSFO_RtkStatus = 27 + , XSFO_RtkStatusNrOfBits = 2 }; /*! @} */ @@ -234,7 +237,8 @@ class XsStatus */ inline XsStatus const& operator = (XsStatus const& a) { - m_status = a.m_status; return *this; + m_status = a.m_status; + return *this; } /*! \brief Assignment operator diff --git a/lib/xspublic/xstypes/xsstring.c b/lib/xspublic/xstypes/xsstring.c index cc84b81..180fac0 100644 --- a/lib/xspublic/xstypes/xsstring.c +++ b/lib/xspublic/xstypes/xsstring.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -68,7 +68,7 @@ #include #if defined(WIN32) -#include + #include #endif /*! \struct XsString @@ -100,7 +100,7 @@ void copyChar(char* to, char const* from) \note Specialization for char*/ int compareChar(void const* a, void const* b) { - if (*(char*)a < *(char*)b) + if (*(char*)a < * (char*)b) return -1; if (*(char*)a > *(char*)b) return 1; @@ -108,7 +108,8 @@ int compareChar(void const* a, void const* b) } //! \brief Descriptor for XsInt64Array -XsArrayDescriptor const g_xsStringDescriptor = { +XsArrayDescriptor const g_xsStringDescriptor = +{ sizeof(char), // const size_t itemSize; //!< \protected Size of a single data element XSEXPCASTITEMSWAP swapChar, // void (*itemSwap)(void* a, void* b); 0, // void (*itemConstruct)(void* e); @@ -141,13 +142,13 @@ void XsString_destruct(XsString* thisPtr) void XsString_assign(XsString* thisPtr, XsSize count, const char* src) { if (!count && src) - count = (XsSize)strlen(src)+1; + count = (XsSize)strlen(src) + 1; if (src) { - if (src[count-1]) + if (src[count - 1]) { - XsArray_assign(thisPtr, count+1, 0); + XsArray_assign(thisPtr, count + 1, 0); memcpy(thisPtr->m_data, src, count); thisPtr->m_data[count] = 0; } @@ -158,7 +159,7 @@ void XsString_assign(XsString* thisPtr, XsSize count, const char* src) { if (count) { - XsArray_assign(thisPtr, count+1, 0); + XsArray_assign(thisPtr, count + 1, 0); memset(thisPtr->m_data, ' ', count); thisPtr->m_data[count] = 0; } @@ -182,14 +183,14 @@ void XsString_assignWCharArray(XsString* thisPtr, const wchar_t* src) if (src) { #ifdef WIN32 - int unicodeLength = lstrlenW( src ); // Convert all UNICODE characters + int unicodeLength = lstrlenW(src); // Convert all UNICODE characters int required = WideCharToMultiByte(CP_UTF8, 0, src, unicodeLength, NULL, 0, NULL, NULL); if (required != -1 && required > 0) { - XsSize reqv = (XsSize)(ptrdiff_t)required+1; + XsSize reqv = (XsSize)(ptrdiff_t)required + 1; if (reqv > thisPtr->m_reserved) XsArray_reserve(thisPtr, reqv); - WideCharToMultiByte(CP_UTF8, 0, src, unicodeLength, thisPtr->m_data, required+1, NULL, NULL); + WideCharToMultiByte(CP_UTF8, 0, src, unicodeLength, thisPtr->m_data, required + 1, NULL, NULL); thisPtr->m_data[required] = '\0'; *((XsSize*) &thisPtr->m_size) = reqv; return; @@ -198,10 +199,10 @@ void XsString_assignWCharArray(XsString* thisPtr, const wchar_t* src) size_t required = wcstombs(0, src, 0); if (required != (size_t) -1 && required > 0) { - if ((XsSize)required+1 > thisPtr->m_reserved) - XsArray_reserve(thisPtr, required+1); - wcstombs(thisPtr->m_data, src, required+1); - *((XsSize*) &thisPtr->m_size) = required+1; + if ((XsSize)required + 1 > thisPtr->m_reserved) + XsArray_reserve(thisPtr, required + 1); + wcstombs(thisPtr->m_data, src, required + 1); + *((XsSize*) &thisPtr->m_size) = required + 1; return; } #endif @@ -216,7 +217,7 @@ XsSize XsString_copyToWCharArray(const XsString* thisPtr, wchar_t* dest, XsSize #ifdef WIN32 return (XsSize)(ptrdiff_t) MultiByteToWideChar(CP_UTF8, 0, thisPtr->m_data, (int)(ptrdiff_t) thisPtr->m_size, dest, (int)(ptrdiff_t) size); #else - return mbstowcs(dest, thisPtr->m_data, size) + (dest?0:1); + return mbstowcs(dest, thisPtr->m_data, size) + (dest ? 0 : 1); #endif } @@ -242,10 +243,10 @@ void XsString_push_backWChar(XsString* thisPtr, wchar_t c) void XsString_resize(XsString* thisPtr, XsSize count) { XsSize sz = thisPtr->m_size; - XsArray_resize(thisPtr, count?count+1:0); + XsArray_resize(thisPtr, count ? count + 1 : 0); if (count) { - for (;sz < count; ++sz) + for (; sz < count; ++sz) thisPtr->m_data[sz] = ' '; thisPtr->m_data[count] = 0; } @@ -258,13 +259,13 @@ void XsString_append(XsString* thisPtr, XsString const* other) if (other && other->m_size > 1) { // remove terminating null from this and append arrays - XsArray_erase(thisPtr, thisPtr->m_size-1, 1); + XsArray_erase(thisPtr, thisPtr->m_size - 1, 1); XsArray_append(thisPtr, other); if (thisPtr == other) { // add terminating null again static const char nullChar = 0; - XsArray_insert(thisPtr, (XsSize) -1, 1, &nullChar); + XsArray_insert(thisPtr, (XsSize) - 1, 1, &nullChar); } } } @@ -277,7 +278,7 @@ void XsString_erase(XsString* thisPtr, XsSize index, XsSize count) if (index + count >= thisPtr->m_size) { if (index) - XsArray_erase(thisPtr, index, (thisPtr->m_size-1)-index); + XsArray_erase(thisPtr, index, (thisPtr->m_size - 1) - index); else XsArray_erase(thisPtr, 0, thisPtr->m_size); } @@ -294,27 +295,26 @@ void XsString_push_back(XsString* thisPtr, char c) if (!sz) sz = 1; XsString_resize(thisPtr, sz); - thisPtr->m_data[sz-1] = c; + thisPtr->m_data[sz - 1] = c; } -uint8_t const * advanceUtf8(const uint8_t* p) +uint8_t const* advanceUtf8(const uint8_t* p) { if ((*p & 0xC0) != 0xC0) ++p; - else - if (*p & 0x20) - if (*p & 0x10) - if (*p & 0x08) - if (*p & 0x04) - p += 6; - else - p += 5; + else if (*p & 0x20) + if (*p & 0x10) + if (*p & 0x08) + if (*p & 0x04) + p += 6; else - p += 4; + p += 5; else - p += 3; + p += 4; else - p += 2; + p += 3; + else + p += 2; return p; } @@ -322,10 +322,10 @@ uint8_t const * advanceUtf8(const uint8_t* p) \details http://en.wikipedia.org/wiki/Utf-8#Description \returns the number of characters in a UTF-8 encoded string */ -XsSize XsString_utf8Len(XsString const * thisPtr) +XsSize XsString_utf8Len(XsString const* thisPtr) { XsSize count = 0; - uint8_t const * p = (uint8_t const*) thisPtr->m_data; + uint8_t const* p = (uint8_t const*) thisPtr->m_data; if (!thisPtr || !thisPtr->m_data) return 0; @@ -355,7 +355,7 @@ int32_t shiftUtf8(int32_t t, uint8_t const* p, int bytes) wchar_t XsString_utf8At(XsString const* thisPtr, XsSize index) { int32_t t = 0; - uint8_t const * p = (uint8_t const*) thisPtr->m_data; + uint8_t const* p = (uint8_t const*) thisPtr->m_data; if (!thisPtr || !thisPtr->m_data) return 0; @@ -373,20 +373,19 @@ wchar_t XsString_utf8At(XsString const* thisPtr, XsSize index) if ((*p & 0xC0) != 0xC0) t = (*p & 0x7F); - else - if (*p & 0x20) - if (*p & 0x10) - if (*p & 0x08) - if (*p & 0x04) - t = shiftUtf8(p[0] & 0x01, p+1, 5); - else - t = shiftUtf8(p[0] & 0x03, p+1, 4); + else if (*p & 0x20) + if (*p & 0x10) + if (*p & 0x08) + if (*p & 0x04) + t = shiftUtf8(p[0] & 0x01, p + 1, 5); else - t = shiftUtf8(p[0] & 0x07, p+1, 3); + t = shiftUtf8(p[0] & 0x03, p + 1, 4); else - t = shiftUtf8(p[0] & 0x0F, p+1, 2); + t = shiftUtf8(p[0] & 0x07, p + 1, 3); else - t = shiftUtf8(p[0] & 0x1F, p+1, 1); + t = shiftUtf8(p[0] & 0x0F, p + 1, 2); + else + t = shiftUtf8(p[0] & 0x1F, p + 1, 1); return (wchar_t) t; } #endif @@ -396,7 +395,7 @@ wchar_t XsString_utf8At(XsString const* thisPtr, XsSize index) \param caseSensitive Whether to compare case sensitive or not \return true when the string ends with the given string */ -int XsString_endsWith(XsString const * thisPtr, XsString const* other, int caseSensitive) +int XsString_endsWith(XsString const* thisPtr, XsString const* other, int caseSensitive) { const char* left; const char* right; @@ -428,7 +427,7 @@ int XsString_endsWith(XsString const * thisPtr, XsString const* other, int caseS \param caseSensitive Whether to compare case sensitive or not \return true when the string starts with the given string */ -int XsString_startsWith(XsString const * thisPtr, XsString const* other, int caseSensitive) +int XsString_startsWith(XsString const* thisPtr, XsString const* other, int caseSensitive) { const char* left = thisPtr->m_data; const char* right = other->m_data; @@ -458,7 +457,7 @@ int XsString_startsWith(XsString const * thisPtr, XsString const* other, int cas \param offset when not null, this will be filled with the offset at which \a other was found \return true when the string contains the given string */ -int XsString_contains(XsString const * thisPtr, XsString const* other, int caseSensitive, XsSize* offset) +int XsString_contains(XsString const* thisPtr, XsString const* other, int caseSensitive, XsSize* offset) { XsSize offsetI = 0; if (!offset) @@ -470,9 +469,9 @@ int XsString_contains(XsString const * thisPtr, XsString const* other, int caseS return 1; // we can never find a bigger string than our own string - while (thisPtr->m_size-*offset >= other->m_size) + while (thisPtr->m_size - *offset >= other->m_size) { - const char* left = thisPtr->m_data+*offset; + const char* left = thisPtr->m_data + *offset; const char* right = other->m_data; if (caseSensitive) for (; *left == *right && *right; ++left, ++right); @@ -484,20 +483,20 @@ int XsString_contains(XsString const * thisPtr, XsString const* other, int caseS ++*offset; } - *offset = (XsSize)-1; + *offset = (XsSize) - 1; return 0; } /*! \brief Returns true when the supplied string is empty \return true when the string is empty */ -int XsString_empty(XsString const * thisPtr) +int XsString_empty(XsString const* thisPtr) { if (!thisPtr) return 1; if (!thisPtr->m_size || (thisPtr->m_flags & XSDF_Empty)) return 1; - return !(thisPtr->m_size-1); + return !(thisPtr->m_size - 1); } /*! \brief Sorts the string @@ -506,7 +505,7 @@ int XsString_empty(XsString const * thisPtr) void XsString_sort(XsString* thisPtr) { if (thisPtr->m_size > 2) - qsort(thisPtr->m_data, thisPtr->m_size-1, sizeof(char), compareChar); + qsort(thisPtr->m_data, thisPtr->m_size - 1, sizeof(char), compareChar); } /*! \brief Reverses the contents of the string @@ -519,9 +518,9 @@ void XsString_reverse(XsString* thisPtr) char* data, tmp, *right; if (thisPtr->m_size > 2) { - half = (int) ((thisPtr->m_size-1) >> 1); + half = (int)((thisPtr->m_size - 1) >> 1); data = (char*) thisPtr->m_data; - right = data + thisPtr->m_size-2; + right = data + thisPtr->m_size - 2; for (i = 0; i < half; ++i) { tmp = data[i]; @@ -535,7 +534,7 @@ void XsString_reverse(XsString* thisPtr) \param needle The string to find \return The offset of \a needle or -1 if it was not found */ -int XsString_findSubStr(XsString const* thisPtr, XsString const* needle) +ptrdiff_t XsString_findSubStr(XsString const* thisPtr, XsString const* needle) { XsSize offset, i, end, endN; if (!thisPtr) // no string to search in @@ -546,7 +545,7 @@ int XsString_findSubStr(XsString const* thisPtr, XsString const* needle) return -1; end = thisPtr->m_size - needle->m_size; - endN = needle->m_size-1; + endN = needle->m_size - 1; for (offset = 0; offset <= end; ++offset) { @@ -554,7 +553,7 @@ int XsString_findSubStr(XsString const* thisPtr, XsString const* needle) if (thisPtr->m_data[offset + i] != needle->m_data[i]) break; if (i == endN) - return (int) offset; // found! + return (ptrdiff_t) offset; // found! } // not found return -1; @@ -595,7 +594,7 @@ void XsString_replaceAll(XsString* thisPtr, XsString const* src, XsString const* XsString_construct(&sub); end = thisPtr->m_size - src->m_size; - endN = src->m_size-1; + endN = src->m_size - 1; start = 0; for (offset = 0; offset <= end;) @@ -627,3 +626,34 @@ void XsString_replaceAll(XsString* thisPtr, XsString const* src, XsString const* XsString_destruct(&sub); XsString_destruct(&rv); } + +/*! \brief Fills thisPtr with a copy of \a source with all its leading and trailing whitespace removed + \param source The original string to use as source. +*/ +void XsString_trimmed(XsString* thisPtr, XsString const* source) +{ + if (!source || XsString_empty(source)) + XsString_destruct(thisPtr); + else + { + XsSize start = 0, end = source->m_size - 1; + while (start < end) + { + if (isspace(source->m_data[start])) + ++start; + else + break; + } + while (start < end) + { + if (isspace(source->m_data[end - 1])) + --end; + else + break; + } + if (start < end) + XsString_assign(thisPtr, end - start, &source->m_data[start]); + else + XsString_destruct(thisPtr); + } +} diff --git a/lib/xspublic/xstypes/xsstring.h b/lib/xspublic/xstypes/xsstring.h index 324035d..1d896bd 100644 --- a/lib/xspublic/xstypes/xsstring.h +++ b/lib/xspublic/xstypes/xsstring.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -68,7 +68,7 @@ #include "xstypesconfig.h" #include "xsarray.h" #ifndef XSENS_NO_WCHAR -#include + #include #endif // XSENS_NO_WCHAR #include @@ -99,16 +99,17 @@ XSTYPES_DLL_API void XsString_resize(XsString* thisPtr, XsSize count); XSTYPES_DLL_API void XsString_append(XsString* thisPtr, XsString const* other); XSTYPES_DLL_API void XsString_erase(XsString* thisPtr, XsSize index, XsSize count); XSTYPES_DLL_API void XsString_push_back(XsString* thisPtr, char c); -XSTYPES_DLL_API XsSize XsString_utf8Len(XsString const * thisPtr); -XSTYPES_DLL_API int XsString_endsWith(XsString const * thisPtr, XsString const* other, int caseSensitive); -XSTYPES_DLL_API int XsString_startsWith(XsString const * thisPtr, XsString const* other, int caseSensitive); -XSTYPES_DLL_API int XsString_contains(XsString const * thisPtr, XsString const* other, int caseSensitive, XsSize* offset); -XSTYPES_DLL_API int XsString_empty(XsString const * thisPtr); +XSTYPES_DLL_API XsSize XsString_utf8Len(XsString const* thisPtr); +XSTYPES_DLL_API int XsString_endsWith(XsString const* thisPtr, XsString const* other, int caseSensitive); +XSTYPES_DLL_API int XsString_startsWith(XsString const* thisPtr, XsString const* other, int caseSensitive); +XSTYPES_DLL_API int XsString_contains(XsString const* thisPtr, XsString const* other, int caseSensitive, XsSize* offset); +XSTYPES_DLL_API int XsString_empty(XsString const* thisPtr); XSTYPES_DLL_API void XsString_sort(XsString* thisPtr); XSTYPES_DLL_API void XsString_reverse(XsString* thisPtr); -XSTYPES_DLL_API int XsString_findSubStr(XsString const* thisPtr, XsString const* needle); +XSTYPES_DLL_API ptrdiff_t XsString_findSubStr(XsString const* thisPtr, XsString const* needle); XSTYPES_DLL_API void XsString_mid(XsString* thisPtr, XsString const* source, XsSize start, XsSize count); XSTYPES_DLL_API void XsString_replaceAll(XsString* thisPtr, XsString const* src, XsString const* dst); +XSTYPES_DLL_API void XsString_trimmed(XsString* thisPtr, XsString const* source); #ifndef XSENS_NO_WCHAR XSTYPES_DLL_API XsSize XsString_copyToWCharArray(const XsString* thisPtr, wchar_t* dest, XsSize size); @@ -127,7 +128,7 @@ XSTYPES_DLL_API void XsString_push_backWChar(XsString* thisPtr, wchar_t c); #endif #ifdef __cplusplus -/* We need some special template implementations for strings to keep them 0-terminated +/* We need some special template implementations for strings to keep them 0-terminated */ // this typedef is not _always_ interpreted correctly by doxygen, hence the occasional function where we're NOT using it. typedef XsArrayImpl XsStringType; @@ -137,16 +138,16 @@ typedef XsArrayImpl XsStringType; \returns The number of items currently in the array \sa reserved \sa setSize \sa resize \sa utf8Len */ -template<> inline XsSize XsStringType::size() const +template<> inline XsSize XsStringType::size() const noexcept { - return m_size?m_size-1:0; + return m_size ? m_size - 1 : 0; } //! \copydoc XsArray_reserve template<> inline void XsStringType::reserve(XsSize count) { if (count) - XsArray_reserve(this, count+1); + XsArray_reserve(this, count + 1); else XsArray_reserve(this, 0); } @@ -154,14 +155,7 @@ template<> inline void XsStringType::reserve(XsSize count) //! \brief Returns the reserved space in number of items template<> inline XsSize XsStringType::reserved() const { - return m_reserved?m_reserved-1:0; -} - -/*! \brief indexed data access operator */ -template<> inline char& XsStringType::operator[] (XsSize index) -{ - assert(index < size()); - return *ptrAt(m_data, (ptrdiff_t) index); + return m_reserved ? m_reserved - 1 : 0; } /*! \brief Removes \a count items from the array starting at \a index. \param index The index of the first item to remove. \param count The number of items to remove. */ @@ -227,10 +221,11 @@ template<> inline void XsArrayImpl::append XsString_append((XsString*) this, (XsString const*) &other); } -struct XsString : public XsStringType { +struct XsString : public XsStringType +{ //! \brief Constructs an XsString inline explicit XsString(XsSize sz = 0, char const* src = 0) - : XsStringType() + : XsStringType() { if (sz || src) XsString_assign(this, sz, src); @@ -239,7 +234,7 @@ struct XsString : public XsStringType { #ifndef SWIG //! \brief Constructs an XsString as a copy of \a other inline XsString(const XsStringType& other) - : XsStringType(other) + : XsStringType(other) { } #else @@ -251,7 +246,7 @@ struct XsString : public XsStringType { //! \brief Constructs an XsInt64Array that references the data supplied in \a ref inline explicit XsString(char* ref, XsSize sz, XsDataFlags flags /* = XSDF_None */) - : XsStringType(ref, sz+1, flags) + : XsStringType(ref, sz + 1, flags) { } #ifndef XSENS_NOITERATOR @@ -287,7 +282,7 @@ struct XsString : public XsStringType { : XsStringType() { if (!src.empty()) - XsString_assign(this, (XsSize)src.size()+1, src.c_str()); + XsString_assign(this, (XsSize)src.size() + 1, src.c_str()); } //! \brief Construct an XsString from a std::wstring @@ -310,7 +305,7 @@ struct XsString : public XsStringType { { return begin().operator ->(); } - catch(...) + catch (...) { return const_cast(&nullChar); } @@ -330,7 +325,7 @@ struct XsString : public XsStringType { { return begin().operator ->(); } - catch(...) + catch (...) { return &nullChar; } @@ -341,7 +336,7 @@ struct XsString : public XsStringType { #ifndef XSENS_NO_STL //! \brief Return the string as a std::string - std::string toStdString() const + inline std::string toStdString() const { if (empty()) return std::string(); @@ -353,7 +348,7 @@ struct XsString : public XsStringType { XsString operator + (XsString const& other) const { XsString tmp; - tmp.reserve(size()+other.size()); + tmp.reserve(size() + other.size()); tmp.append(*this); tmp.append(other); return tmp; @@ -367,7 +362,7 @@ struct XsString : public XsStringType { return std::wstring(); size_t s = XsString_copyToWCharArray(this, NULL, 0); std::wstring w; - w.resize(s-1); + w.resize(s - 1); (void) XsString_copyToWCharArray(this, &w[0], (XsSize)s); return w; } @@ -382,18 +377,31 @@ struct XsString : public XsStringType { /*! \endcond */ //! \brief Return true if the contents of \a str are identical to this string - bool operator == (char const* str) const + inline bool operator == (char const* str) const { - if (!str) return empty(); + if (!str) + return empty(); return !strcmp(c_str(), str); } + //! \brief Return true if the contents of \a str are identical to this string + inline bool operator == (XsString const& str) const + { + return XsStringType::operator ==(str); + } + //! \brief Return true if the contents of \a str differ from this string inline bool operator != (char const* str) const { return !(*this == str); } + //! \brief Return true if the contents of \a str differ from this string + inline bool operator != (XsString const& str) const + { + return !(*this == str); + } + //! \brief Append a character array to the string in a stream-like way inline XsString& operator << (char const* str) { @@ -409,7 +417,8 @@ struct XsString : public XsStringType { inline XsString& operator << (int i) { char buffer[32]; - append(XsString(buffer, (XsSize) (ptrdiff_t) std::sprintf(buffer, "%d", i), XSDF_None)); + XSENS_MSC_WARNING_SUPPRESS(4996) + append(XsString(buffer, (XsSize)(ptrdiff_t) std::sprintf(buffer, "%d", i), XSDF_None)); return *this; } @@ -421,7 +430,7 @@ struct XsString : public XsStringType { } //! \brief Return true if the contents if \a str are greater then this string - inline bool operator < (const XsString &str) const + inline bool operator < (const XsString& str) const { #ifdef XSENS_NO_STL return (strcmp(c_str(), str.c_str()) < 0); @@ -431,7 +440,7 @@ struct XsString : public XsStringType { } //! \brief Return true if the contents if \a str are less then this string - inline bool operator > (const XsString &str) const + inline bool operator > (const XsString& str) const { #ifdef XSENS_NO_STL return (strcmp(c_str(), str.c_str()) > 0); @@ -442,7 +451,7 @@ struct XsString : public XsStringType { #ifndef SWIG /*! \brief Swap the contents the \a first and \a second array */ - friend void swap(XsString& first, XsString& second) + friend inline void swap(XsString& first, XsString& second) { first.swap(second); } @@ -505,7 +514,7 @@ struct XsString : public XsStringType { */ inline bool endsWith(XsString const& other, bool caseSensitive = false) const { - return 0 != XsString_endsWith(this, &other, caseSensitive?1:0); + return 0 != XsString_endsWith(this, &other, caseSensitive ? 1 : 0); } /*! \brief Returns whether this string starts with \a other (case-insensitive!) @@ -515,7 +524,7 @@ struct XsString : public XsStringType { */ inline bool startsWith(XsString const& other, bool caseSensitive = false) const { - return 0 != XsString_startsWith(this, &other, caseSensitive?1:0); + return 0 != XsString_startsWith(this, &other, caseSensitive ? 1 : 0); } /*! \brief Returns whether this string contains \a other (case-insensitive!) @@ -526,7 +535,7 @@ struct XsString : public XsStringType { */ inline bool contains(XsString const& other, bool caseSensitive = false, XsSize* offset = 0) const { - return 0 != XsString_contains(this, &other, caseSensitive?1:0, offset); + return 0 != XsString_contains(this, &other, caseSensitive ? 1 : 0, offset); } #ifndef XSENS_NO_WCHAR @@ -566,7 +575,7 @@ struct XsString : public XsStringType { \param needle The string to find \return The offset of \a needle or -1 if it was not found */ - int findSubStr(XsString const& needle) const + inline ptrdiff_t findSubStr(XsString const& needle) const { return XsString_findSubStr(this, &needle); } @@ -577,7 +586,7 @@ struct XsString : public XsStringType { \param count The maximum number of characters to copy \return The requested substring */ - XsString mid(XsSize start, XsSize count) const + inline XsString mid(XsSize start, XsSize count) const { XsString rv; XsString_mid(&rv, this, start, count); @@ -588,7 +597,7 @@ struct XsString : public XsStringType { \param src Substring to search for \param dst Substring to use as replacement */ - void replaceAll(XsString const& src, XsString const& dst) + inline void replaceAll(XsString const& src, XsString const& dst) { XsString_replaceAll(this, &src, &dst); } @@ -597,25 +606,51 @@ struct XsString : public XsStringType { \param src Substring to search for \param dst Substring to use as replacement */ - XsString replacedAll(XsString const& src, XsString const& dst) const + inline XsString replacedAll(XsString const& src, XsString const& dst) const { XsString rv(*this); XsString_replaceAll(&rv, &src, &dst); return rv; } + + /*! \brief Remove all whitespace from start and end of the string + */ + inline void trim() + { + XsString rv; + XsString_trimmed(&rv, this); + swap(rv); + } + + /*! \brief Returns an XsString based on this with all whitespace at start and end of the string removed + */ + inline XsString trimmed() const + { + XsString rv; + XsString_trimmed(&rv, this); + return rv; + } + + /*! \brief Returns the last character in the XsString or \0 if the string is empty */ + inline char last() const + { + if (size()) + return at(size() - 1); + return 0; + } + }; #ifndef XSENS_NO_STL namespace std { - template - basic_ostream<_CharT, _Traits>& operator<<(basic_ostream<_CharT, _Traits>& o, XsString const& xs) - { - return (o << xs.toStdString()); - } +template +basic_ostream<_CharT, _Traits>& operator<<(basic_ostream<_CharT, _Traits>& o, XsString const& xs) +{ + return (o << xs.toStdString()); +} } #endif #endif - #endif diff --git a/lib/xspublic/xstypes/xsstringarray.c b/lib/xspublic/xstypes/xsstringarray.c index eff2b85..4fdc401 100644 --- a/lib/xspublic/xstypes/xsstringarray.c +++ b/lib/xspublic/xstypes/xsstringarray.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -71,7 +71,8 @@ */ //! \brief Descriptor for XsStringArray -XsArrayDescriptor const g_xsStringArrayDescriptor = { +XsArrayDescriptor const g_xsStringArrayDescriptor = +{ sizeof(XsString), XSEXPCASTITEMSWAP XsArray_swap, XSEXPCASTITEMMAKE XsString_construct, @@ -120,10 +121,10 @@ void XsStringArray_fromSplicedString(struct XsStringArray* thisPtr, struct XsStr { if (newIdx != idx) { - XsString_assign(&s, (XsSize)(newIdx-idx), idx); + XsString_assign(&s, (XsSize)(newIdx - idx), idx); XsArray_insert(thisPtr, thisPtr->m_size, 1, &s); } - idx = newIdx+1; + idx = newIdx + 1; newIdx = strpbrk(idx, sep); } if (*idx) @@ -145,17 +146,17 @@ void XsStringArray_join(struct XsStringArray const* thisPtr, struct XsString* re { // determine required buffer size XsSize i; - XsSize chars = (thisPtr->m_size ? (thisPtr->m_size-1) : 0) * (separator->m_size ? separator->m_size-1 : 0); + XsSize chars = (thisPtr->m_size ? (thisPtr->m_size - 1) : 0) * (separator->m_size ? separator->m_size - 1 : 0); for (i = 0; i < thisPtr->m_size; ++i) { XsSize sz = ((const XsString*)XsArray_at(thisPtr, i))->m_size; - chars += (sz ? sz-1 : 0); + chars += (sz ? sz - 1 : 0); } XsArray_destruct(result); if (chars) { - XsArray_reserve(result, chars+1); + XsArray_reserve(result, chars + 1); for (i = 0; i < thisPtr->m_size; ++i) { const XsString* s = (const XsString*)XsArray_at(thisPtr, i); diff --git a/lib/xspublic/xstypes/xsstringarray.h b/lib/xspublic/xstypes/xsstringarray.h index 145bb5a..77f799f 100644 --- a/lib/xspublic/xstypes/xsstringarray.h +++ b/lib/xspublic/xstypes/xsstringarray.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -90,16 +90,17 @@ XSTYPES_DLL_API void XsStringArray_join(struct XsStringArray const* thisPtr, str #ifdef __cplusplus } // extern "C" -struct XsStringArray : public XsArrayImpl { +struct XsStringArray : public XsArrayImpl +{ //! \brief Constructs an XsStringArray inline explicit XsStringArray(XsSize sz = 0, XsString const* src = 0) - : ArrayImpl(sz, src) + : ArrayImpl(sz, src) { } //! \brief Constructs an XsStringArray as a copy of \a other inline XsStringArray(XsStringArray const& other) - : ArrayImpl(other) + : ArrayImpl(other) { } @@ -155,7 +156,7 @@ struct XsStringArray : public XsArrayImpl::find(needle); diff --git a/lib/xspublic/xstypes/xsstringoutputtype.h b/lib/xspublic/xstypes/xsstringoutputtype.h index be63d1f..9e90e62 100644 --- a/lib/xspublic/xstypes/xsstringoutputtype.h +++ b/lib/xspublic/xstypes/xsstringoutputtype.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -69,23 +69,24 @@ @{ */ //! String output types -enum XsStringOutputType { - XSOT_None = 0x0000 - ,XSOT_HCHDM = 0x0001 //!< NMEA string with Magnetic Heading - ,XSOT_HCHDG = 0x0002 //!< NMEA string with Heading and Magnetic Variation - ,XSOT_TSS2 = 0x0004 //!< Proprietry string with Heading, Heave, Roll and Pitch - ,XSOT_PHTRO = 0x0008 //!< Proprietry NMEA string with Pitch and Roll - ,XSOT_PRDID = 0x0010 //!< Proprietry NMEA string with Pitch, Roll and Heading - ,XSOT_EM1000 = 0x0020 //!< Binary format suitable for use with Simrad EM1000 mulitibeam sounders with Roll, Pitch, Heave and Heading - ,XSOT_PSONCMS = 0x0040 //!< NMEA string with Xsens Compass Motion Sensor information - ,XSOT_HCMTW = 0x0080 //!< NMEA string with (water) Temperature - ,XSOT_HEHDT = 0x0100 //!< NMEA string with True Heading - ,XSOT_HEROT = 0x0200 //!< NMEA string with Rate of Turn - ,XSOT_GPGGA = 0x0400 //!< NMEA string with Global Positioning system fix data - ,XSOT_PTCF = 0x0800 //!< NMEA string with motion data - ,XSOT_XSVEL = 0x1000 //!< Proprietry NMEA string with velocity data - ,XSOT_GPZDA = 0x2000 //!< NMEA string with date and time - ,XSOT_GPRMC = 0x4000 //!< NMEA string with recommended minimum specific GPS/Transit data +enum XsStringOutputType +{ + XSOT_None = 0x0000 + , XSOT_HCHDM = 0x0001 //!< NMEA string with Magnetic Heading + , XSOT_HCHDG = 0x0002 //!< NMEA string with Heading and Magnetic Variation + , XSOT_TSS2 = 0x0004 //!< Proprietry string with Heading, Heave, Roll and Pitch + , XSOT_PHTRO = 0x0008 //!< Proprietry NMEA string with Pitch and Roll + , XSOT_PRDID = 0x0010 //!< Proprietry NMEA string with Pitch, Roll and Heading + , XSOT_EM1000 = 0x0020 //!< Binary format suitable for use with Simrad EM1000 mulitibeam sounders with Roll, Pitch, Heave and Heading + , XSOT_PSONCMS = 0x0040 //!< NMEA string with Xsens Compass Motion Sensor information + , XSOT_HCMTW = 0x0080 //!< NMEA string with (water) Temperature + , XSOT_HEHDT = 0x0100 //!< NMEA string with True Heading + , XSOT_HEROT = 0x0200 //!< NMEA string with Rate of Turn + , XSOT_GPGGA = 0x0400 //!< NMEA string with Global Positioning system fix data + , XSOT_PTCF = 0x0800 //!< NMEA string with motion data + , XSOT_XSVEL = 0x1000 //!< Proprietry NMEA string with velocity data + , XSOT_GPZDA = 0x2000 //!< NMEA string with date and time + , XSOT_GPRMC = 0x4000 //!< NMEA string with recommended minimum specific GPS/Transit data }; /*! @} */ typedef enum XsStringOutputType XsStringOutputType; diff --git a/lib/xspublic/xstypes/xsstringoutputtypearray.c b/lib/xspublic/xstypes/xsstringoutputtypearray.c index c6789a6..34b2b9c 100644 --- a/lib/xspublic/xstypes/xsstringoutputtypearray.c +++ b/lib/xspublic/xstypes/xsstringoutputtypearray.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -102,7 +102,8 @@ int compareStringOutputType(XsStringOutputType const* a, XsStringOutputType cons } //! \brief Descriptor for XsStringOutputTypeArray -XsArrayDescriptor const g_xsStringOutputTypeArrayDescriptor = { +XsArrayDescriptor const g_xsStringOutputTypeArrayDescriptor = +{ sizeof(XsStringOutputType), XSEXPCASTITEMSWAP swapStringOutputType, // swap 0, // construct diff --git a/lib/xspublic/xstypes/xsstringoutputtypearray.h b/lib/xspublic/xstypes/xsstringoutputtypearray.h index 84e9bec..3b9faa6 100644 --- a/lib/xspublic/xstypes/xsstringoutputtypearray.h +++ b/lib/xspublic/xstypes/xsstringoutputtypearray.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -86,42 +86,42 @@ XSTYPES_DLL_API void XsStringOutputTypeArray_construct(XsStringOutputTypeArray* #endif #ifdef __cplusplus -struct XsStringOutputTypeArray : public XsArrayImpl -{ - //! \brief Constructs an XsStringOutputTypeArray - inline explicit XsStringOutputTypeArray(XsSize sz = 0, XsStringOutputType const* src = 0) - : ArrayImpl(sz, src) + struct XsStringOutputTypeArray : public XsArrayImpl { - } + //! \brief Constructs an XsStringOutputTypeArray + inline explicit XsStringOutputTypeArray(XsSize sz = 0, XsStringOutputType const* src = 0) + : ArrayImpl(sz, src) + { + } - //! \brief Constructs an XsStringOutputTypeArray as a copy of \a other - inline XsStringOutputTypeArray(XsStringOutputTypeArray const& other) - : ArrayImpl(other) - { - } + //! \brief Constructs an XsStringOutputTypeArray as a copy of \a other + inline XsStringOutputTypeArray(XsStringOutputTypeArray const& other) + : ArrayImpl(other) + { + } - //! \brief Constructs an XsStringOutputTypeArray that references the data supplied in \a ref - inline explicit XsStringOutputTypeArray(XsStringOutputType* ref, XsSize sz, XsDataFlags flags /* = XSDF_None */) - : ArrayImpl(ref, sz, flags) - { - } + //! \brief Constructs an XsStringOutputTypeArray that references the data supplied in \a ref + inline explicit XsStringOutputTypeArray(XsStringOutputType* ref, XsSize sz, XsDataFlags flags /* = XSDF_None */) + : ArrayImpl(ref, sz, flags) + { + } #ifndef SWIG - /*! \brief Swap the contents the \a first and \a second array */ - friend void swap(XsStringOutputTypeArray& first, XsStringOutputTypeArray& second) - { - first.swap(second); - } + /*! \brief Swap the contents the \a first and \a second array */ + friend void swap(XsStringOutputTypeArray& first, XsStringOutputTypeArray& second) + { + first.swap(second); + } #endif #ifndef XSENS_NOITERATOR - //! \brief Constructs an XsStringOutputTypeArray with the array bound by the supplied iterators \a beginIt and \a endIt - template - inline XsStringOutputTypeArray(Iterator beginIt, Iterator endIt) - : ArrayImpl(beginIt, endIt) - { - } + //! \brief Constructs an XsStringOutputTypeArray with the array bound by the supplied iterators \a beginIt and \a endIt + template + inline XsStringOutputTypeArray(Iterator beginIt, Iterator endIt) + : ArrayImpl(beginIt, endIt) + { + } #endif -}; + }; #endif #endif diff --git a/lib/xspublic/xstypes/xsstringstreaming.h b/lib/xspublic/xstypes/xsstringstreaming.h index 59da8cf..e474923 100644 --- a/lib/xspublic/xstypes/xsstringstreaming.h +++ b/lib/xspublic/xstypes/xsstringstreaming.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -75,39 +75,39 @@ #include namespace std { - template - basic_ostream<_CharT, _Traits>& operator<<(basic_ostream<_CharT, _Traits>& o, XsVector const& xv) - { - o << "V<" << xv.size() << ">("; - for (XsSize i = 0; i < xv.size() - 1; i++) - o << xv[i] << ", "; - return (o << xv[xv.size() - 1] << ")"); - } +template +basic_ostream<_CharT, _Traits>& operator<<(basic_ostream<_CharT, _Traits>& o, XsVector const& xv) +{ + o << "V<" << xv.size() << ">("; + for (XsSize i = 0; i < xv.size() - 1; i++) + o << xv[i] << ", "; + return (o << xv[xv.size() - 1] << ")"); +} - template - basic_ostream<_CharT, _Traits>& operator<<(basic_ostream<_CharT, _Traits>& o, XsMatrix const& xm) +template +basic_ostream<_CharT, _Traits>& operator<<(basic_ostream<_CharT, _Traits>& o, XsMatrix const& xm) +{ + o << "M<" << xm.rows() << "," << xm.cols() << ">("; + for (XsSize r = 0; r < xm.rows(); ++r) { - o << "M<" << xm.rows() << "," << xm.cols() << ">("; - for (XsSize r = 0; r < xm.rows(); ++r) - { - for (XsSize c = 0; c < xm.cols() - 1; ++c) - o << xm[r][c] << ", "; - o << xm[r][xm.cols()-1]; - if (r < xm.rows() - 1) - o << "\n"; - } - o << ")"; - return o; + for (XsSize c = 0; c < xm.cols() - 1; ++c) + o << xm[r][c] << ", "; + o << xm[r][xm.cols() - 1]; + if (r < xm.rows() - 1) + o << "\n"; } + o << ")"; + return o; +} - template - basic_ostream<_CharT, _Traits>& operator<<(basic_ostream<_CharT, _Traits>& o, XsQuaternion const& xq) - { - o << "Q("; - for (int i = 0; i < 3; i++) - o << xq[i] << ", "; - return (o << xq[3] << ")"); - } +template +basic_ostream<_CharT, _Traits>& operator<<(basic_ostream<_CharT, _Traits>& o, XsQuaternion const& xq) +{ + o << "Q("; + for (int i = 0; i < 3; i++) + o << xq[i] << ", "; + return (o << xq[3] << ")"); +} } #endif @@ -146,7 +146,7 @@ inline XsString& operator<<(XsString& o, XsMatrix const& xm) o << "\t"; for (XsSize c = 0; c < xm.cols() - 1; ++c) o << xm[r][c] << ", "; - o << xm[r][xm.cols()-1]; + o << xm[r][xm.cols() - 1]; if (r < xm.rows() - 1) o << "\n"; } diff --git a/lib/xspublic/xstypes/xssyncfunction.h b/lib/xspublic/xstypes/xssyncfunction.h index 28dd019..be5eb0d 100644 --- a/lib/xspublic/xstypes/xssyncfunction.h +++ b/lib/xspublic/xstypes/xssyncfunction.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xstypes/xssyncline.h b/lib/xspublic/xstypes/xssyncline.h index 0f9de47..d26e26e 100644 --- a/lib/xspublic/xstypes/xssyncline.h +++ b/lib/xspublic/xstypes/xssyncline.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xstypes/xssyncpolarity.h b/lib/xspublic/xstypes/xssyncpolarity.h index e28d6ed..fbca461 100644 --- a/lib/xspublic/xstypes/xssyncpolarity.h +++ b/lib/xspublic/xstypes/xssyncpolarity.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xstypes/xssyncrole.h b/lib/xspublic/xstypes/xssyncrole.h index 00b8a24..8f58dfd 100644 --- a/lib/xspublic/xstypes/xssyncrole.h +++ b/lib/xspublic/xstypes/xssyncrole.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -70,7 +70,8 @@ */ /*! \brief Synchronization roles */ -enum XsSyncRole { +enum XsSyncRole +{ XSR_Slave, XSR_None, XSR_MasterSlave, diff --git a/lib/xspublic/xstypes/xssyncsetting.c b/lib/xspublic/xstypes/xssyncsetting.c index c470fb4..47cbba4 100644 --- a/lib/xspublic/xstypes/xssyncsetting.c +++ b/lib/xspublic/xstypes/xssyncsetting.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -75,15 +75,15 @@ int XsSyncSetting_isInput(const XsSyncSetting* thisPtr) { switch (thisPtr->m_line) { - case XSL_In1: - case XSL_In2: - case XSL_Bi1In: - case XSL_ClockIn: - case XSL_CtsIn: - return 1; + case XSL_In1: + case XSL_In2: + case XSL_Bi1In: + case XSL_ClockIn: + case XSL_CtsIn: + return 1; - default: - return 0; + default: + return 0; } } @@ -93,14 +93,14 @@ int XsSyncSetting_isOutput(const XsSyncSetting* thisPtr) { switch (thisPtr->m_line) { - case XSL_Out1: - case XSL_Out2: - case XSL_Bi1Out: - case XSL_RtsOut: - return 1; + case XSL_Out1: + case XSL_Out2: + case XSL_Bi1Out: + case XSL_RtsOut: + return 1; - default: - return 0; + default: + return 0; } } @@ -123,31 +123,49 @@ int XsSyncSetting_compare(const struct XsSyncSetting* a, const struct XsSyncSett { assert(a && b); - if (a->m_line < b->m_line) return -1; - if (b->m_line < a->m_line) return 1; - if (a->m_function < b->m_function) return -1; - if (b->m_function < a->m_function) return 1; - if (a->m_polarity < b->m_polarity) return -1; - if (b->m_polarity < a->m_polarity) return 1; + if (a->m_line < b->m_line) + return -1; + if (b->m_line < a->m_line) + return 1; + if (a->m_function < b->m_function) + return -1; + if (b->m_function < a->m_function) + return 1; + if (a->m_polarity < b->m_polarity) + return -1; + if (b->m_polarity < a->m_polarity) + return 1; if (XsSyncSetting_isOutput(a)) { // only relevant for output triggers, ignored for inputs since inputs always trigger on an edge - if (a->m_pulseWidth < b->m_pulseWidth) return -1; - if (b->m_pulseWidth < a->m_pulseWidth) return 1; + if (a->m_pulseWidth < b->m_pulseWidth) + return -1; + if (b->m_pulseWidth < a->m_pulseWidth) + return 1; } - if (a->m_offset < b->m_offset) return -1; - if (b->m_offset < a->m_offset) return 1; - if (a->m_skipFirst < b->m_skipFirst) return -1; - if (b->m_skipFirst < a->m_skipFirst) return 1; - if (a->m_skipFactor < b->m_skipFactor) return -1; - if (b->m_skipFactor < a->m_skipFactor) return 1; + if (a->m_offset < b->m_offset) + return -1; + if (b->m_offset < a->m_offset) + return 1; + if (a->m_skipFirst < b->m_skipFirst) + return -1; + if (b->m_skipFirst < a->m_skipFirst) + return 1; + if (a->m_skipFactor < b->m_skipFactor) + return -1; + if (b->m_skipFactor < a->m_skipFactor) + return 1; if (a->m_function == XSF_ClockBiasEstimation) { - if (a->m_clockPeriod < b->m_clockPeriod) return -1; - if (b->m_clockPeriod < a->m_clockPeriod) return 1; + if (a->m_clockPeriod < b->m_clockPeriod) + return -1; + if (b->m_clockPeriod < a->m_clockPeriod) + return 1; } - if (a->m_triggerOnce < b->m_triggerOnce) return -1; - if (b->m_triggerOnce < a->m_triggerOnce) return 1; + if (a->m_triggerOnce < b->m_triggerOnce) + return -1; + if (b->m_triggerOnce < a->m_triggerOnce) + return 1; return 0; } diff --git a/lib/xspublic/xstypes/xssyncsetting.h b/lib/xspublic/xstypes/xssyncsetting.h index 49dca83..780db1e 100644 --- a/lib/xspublic/xstypes/xssyncsetting.h +++ b/lib/xspublic/xstypes/xssyncsetting.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -92,7 +92,8 @@ XSTYPES_DLL_API int XsSyncSetting_compare(const struct XsSyncSetting* a, const s #endif /*! \brief A structure for storing all xsens sync settings */ -struct XsSyncSetting { +struct XsSyncSetting +{ XsSyncLine m_line; /*!< The sync lines enabled. \see XsSyncLine. */ XsSyncFunction m_function; /*!< The action to be performed, when an input sync line changes \see XsSyncFunction. */ XsSyncPolarity m_polarity; /*!< The edge on which the action is performed, \see XsSyncPolarity. */ @@ -106,14 +107,14 @@ struct XsSyncSetting { #ifdef __cplusplus //! \brief Default constructor, initializes to the given (default) settings explicit XsSyncSetting(XsSyncLine line = XSL_Invalid - , XsSyncFunction function = XSF_Invalid - , XsSyncPolarity polarity = XSP_RisingEdge - , uint32_t pulseWidth = 1000 - , int32_t offset = 0 - , uint16_t skipFirst = 0 - , uint16_t skipFactor = 0 - , uint16_t clockPeriod = 0 - , uint8_t triggerOnce = 0) + , XsSyncFunction function = XSF_Invalid + , XsSyncPolarity polarity = XSP_RisingEdge + , uint32_t pulseWidth = 1000 + , int32_t offset = 0 + , uint16_t skipFirst = 0 + , uint16_t skipFactor = 0 + , uint16_t clockPeriod = 0 + , uint8_t triggerOnce = 0) : m_line(line) , m_function(function) , m_polarity(polarity) diff --git a/lib/xspublic/xstypes/xssyncsettingarray.c b/lib/xspublic/xstypes/xssyncsettingarray.c index fbd7248..72aecb6 100644 --- a/lib/xspublic/xstypes/xssyncsettingarray.c +++ b/lib/xspublic/xstypes/xssyncsettingarray.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -94,7 +94,8 @@ void zeroSyncSetting(XsSyncSetting* a) } //! \brief Descriptor for XsSyncSettingArray -XsArrayDescriptor const g_xsSyncSettingArrayDescriptor = { +XsArrayDescriptor const g_xsSyncSettingArrayDescriptor = +{ sizeof(XsSyncSetting), XSEXPCASTITEMSWAP XsSyncSetting_swap, XSEXPCASTITEMMAKE zeroSyncSetting, // construct diff --git a/lib/xspublic/xstypes/xssyncsettingarray.h b/lib/xspublic/xstypes/xssyncsettingarray.h index 626e228..094f96d 100644 --- a/lib/xspublic/xstypes/xssyncsettingarray.h +++ b/lib/xspublic/xstypes/xssyncsettingarray.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -87,16 +87,17 @@ XSTYPES_DLL_API void XsSyncSettingArray_construct(XsSyncSettingArray* thisPtr, X #ifdef __cplusplus } // extern "C" -struct XsSyncSettingArray : public XsArrayImpl { +struct XsSyncSettingArray : public XsArrayImpl +{ //! \brief Constructs an XsSyncSettingArray inline explicit XsSyncSettingArray(XsSize sz = 0, XsSyncSetting const* src = 0) - : ArrayImpl(sz, src) + : ArrayImpl(sz, src) { } //! \brief Constructs an XsSyncSettingArray as a copy of \a other inline XsSyncSettingArray(XsSyncSettingArray const& other) - : ArrayImpl(other) + : ArrayImpl(other) { } diff --git a/lib/xspublic/xstypes/xsthread.c b/lib/xspublic/xstypes/xsthread.c index 297ac59..1a0f2f9 100644 --- a/lib/xspublic/xstypes/xsthread.c +++ b/lib/xspublic/xstypes/xsthread.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -100,9 +100,9 @@ void XSTYPES_DLL_API xsNameThisThread(const char* threadName) __try { - RaiseException( MS_VC_EXCEPTION, 0, sizeof(info)/sizeof(ULONG_PTR), (ULONG_PTR*)&info ); + RaiseException(MS_VC_EXCEPTION, 0, sizeof(info) / sizeof(ULONG_PTR), (ULONG_PTR*)&info); } - __except(EXCEPTION_EXECUTE_HANDLER) + __except (EXCEPTION_EXECUTE_HANDLER) { } } @@ -112,16 +112,16 @@ void XSTYPES_DLL_API xsNameThisThread(const char* threadName) #else #include #ifdef __APPLE__ -inline static int pthread_setname_np2 (pthread_t __target_thread, const char *__name) +inline static int pthread_setname_np2(pthread_t __target_thread, const char* __name) { (void) __target_thread; return pthread_setname_np(__name); } #else /* Set thread name visible in the kernel and its interfaces. */ -extern int pthread_setname_np (pthread_t __target_thread, const char *__name); +extern int pthread_setname_np(pthread_t __target_thread, const char* __name); -inline static int pthread_setname_np2 (pthread_t __target_thread, const char *__name) +inline static int pthread_setname_np2(pthread_t __target_thread, const char* __name) { return pthread_setname_np(__target_thread, __name); } @@ -145,7 +145,7 @@ void XSTYPES_DLL_API xsNameThisThread(const char* threadName) { char dup[16]; strncpy(dup, threadName, 11); - strncpy(dup+11, threadName + strlen(threadName)-4, 4); + strncpy(dup + 11, threadName + strlen(threadName) - 4, 4); dup[15] = 0; pthread_setname_np2(xsGetCurrentThreadId(), dup); } @@ -153,7 +153,7 @@ void XSTYPES_DLL_API xsNameThisThread(const char* threadName) /*! @} */ -pthread_t XSTYPES_DLL_API xsStartThread(void *(func)(void *), void *param, void *pid) +pthread_t XSTYPES_DLL_API xsStartThread(void* (func)(void*), void* param, void* pid) { (void)pid; pthread_t thread; diff --git a/lib/xspublic/xstypes/xsthread.h b/lib/xspublic/xstypes/xsthread.h index 0c8565e..9ce2130 100644 --- a/lib/xspublic/xstypes/xsthread.h +++ b/lib/xspublic/xstypes/xsthread.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -67,12 +67,12 @@ #include "xstime.h" #if defined(XSENS_DEBUG) && defined(_MSC_VER) -#pragma warning (disable: 4985) -#include + #pragma warning (disable: 4985) + #include #endif #ifndef __GNUC__ -#pragma warning(disable: 4127) + #pragma warning(disable: 4127) #endif #ifdef __cplusplus @@ -80,116 +80,118 @@ extern "C" { #endif #ifdef XSENS_WINDOWS - #ifndef WINVER // Allow use of features specific to Windows XP or later. - #define WINVER 0x0502 // Change this to the appropriate value to target other versions of Windows. - #endif - - #ifndef _WIN32_WINNT // Allow use of features specific to Windows XP or later. - #define _WIN32_WINNT 0x0502 // Change this to the appropriate value to target other versions of Windows. - #endif - - #ifndef _WIN32_WINDOWS // Allow use of features specific to Windows 98 or later. - #define _WIN32_WINDOWS 0x0410 // Change this to the appropriate value to target Windows Me or later. - #endif - - #ifndef _WIN32_IE // Allow use of features specific to IE 6.0 or later. - #define _WIN32_IE 0x0600 // Change this to the appropriate value to target other versions of IE. - #endif - - #include - - /*! \addtogroup enums Global enumerations - @{ - */ - /*! \brief Thread priorities for xsSetThreadPriority() and xsGetThreadPriority() - */ - enum XsThreadPriority { - XS_THREAD_PRIORITY_LOWEST = THREAD_PRIORITY_IDLE, - XS_THREAD_PRIORITY_LOWER = THREAD_PRIORITY_LOWEST, - XS_THREAD_PRIORITY_LOW = THREAD_PRIORITY_BELOW_NORMAL, - XS_THREAD_PRIORITY_NORMAL = THREAD_PRIORITY_NORMAL, - XS_THREAD_PRIORITY_HIGH = THREAD_PRIORITY_ABOVE_NORMAL, - XS_THREAD_PRIORITY_HIGHER = THREAD_PRIORITY_HIGHEST, - XS_THREAD_PRIORITY_HIGHEST = THREAD_PRIORITY_TIME_CRITICAL - }; - /*! @} */ - - // The components of the type of a thread function - #define XSENS_THREAD_RETURN DWORD - #define XSENS_THREAD_TYPE WINAPI - #define XSENS_THREAD_PARAM LPVOID - - #define XSENS_INVALID_THREAD INVALID_HANDLE_VALUE - - /*! \brief Release the remainder of the timeslice so other operations can run. - On Windows this is done using Sleep(0), since this is the most reliable method. - SwitchToThread can cause delays since it does not allow the thread to resume on a different core. - Sleep http://msdn.microsoft.com/en-us/library/ms686298%28v=vs.85%29.aspx - SwitchToThread http://msdn.microsoft.com/en-us/library/ms686352%28v=vs.85%29.aspx - */ - #define xsYield() Sleep(0) - - //! A handle for a thread - typedef HANDLE XsThread; +#ifndef WINVER // Allow use of features specific to Windows XP or later. +#define WINVER 0x0502 // Change this to the appropriate value to target other versions of Windows. +#endif + +#ifndef _WIN32_WINNT // Allow use of features specific to Windows XP or later. +#define _WIN32_WINNT 0x0502 // Change this to the appropriate value to target other versions of Windows. +#endif + +#ifndef _WIN32_WINDOWS // Allow use of features specific to Windows 98 or later. +#define _WIN32_WINDOWS 0x0410 // Change this to the appropriate value to target Windows Me or later. +#endif + +#ifndef _WIN32_IE // Allow use of features specific to IE 6.0 or later. +#define _WIN32_IE 0x0600 // Change this to the appropriate value to target other versions of IE. +#endif + +#include + +/*! \addtogroup enums Global enumerations + @{ +*/ +/*! \brief Thread priorities for xsSetThreadPriority() and xsGetThreadPriority() +*/ +enum XsThreadPriority +{ + XS_THREAD_PRIORITY_LOWEST = THREAD_PRIORITY_IDLE, + XS_THREAD_PRIORITY_LOWER = THREAD_PRIORITY_LOWEST, + XS_THREAD_PRIORITY_LOW = THREAD_PRIORITY_BELOW_NORMAL, + XS_THREAD_PRIORITY_NORMAL = THREAD_PRIORITY_NORMAL, + XS_THREAD_PRIORITY_HIGH = THREAD_PRIORITY_ABOVE_NORMAL, + XS_THREAD_PRIORITY_HIGHER = THREAD_PRIORITY_HIGHEST, + XS_THREAD_PRIORITY_HIGHEST = THREAD_PRIORITY_TIME_CRITICAL +}; +/*! @} */ + +// The components of the type of a thread function +#define XSENS_THREAD_RETURN DWORD +#define XSENS_THREAD_TYPE WINAPI +#define XSENS_THREAD_PARAM LPVOID + +#define XSENS_INVALID_THREAD INVALID_HANDLE_VALUE + +/*! \brief Release the remainder of the timeslice so other operations can run. + On Windows this is done using Sleep(0), since this is the most reliable method. + SwitchToThread can cause delays since it does not allow the thread to resume on a different core. + Sleep http://msdn.microsoft.com/en-us/library/ms686298%28v=vs.85%29.aspx + SwitchToThread http://msdn.microsoft.com/en-us/library/ms686352%28v=vs.85%29.aspx +*/ +#define xsYield() Sleep(0) + +//! A handle for a thread +typedef HANDLE XsThread; #ifdef __cplusplus - typedef ::DWORD XsThreadId; +typedef ::DWORD XsThreadId; #else - typedef DWORD XsThreadId; +typedef DWORD XsThreadId; #endif - //! Start a function as a thread - #define xsStartThread(func,param,pid) CreateThread(NULL,0,(LPTHREAD_START_ROUTINE) func,param,0,pid) +//! Start a function as a thread +#define xsStartThread(func,param,pid) CreateThread(NULL,0,(LPTHREAD_START_ROUTINE) func,param,0,pid) - XSTYPES_DLL_API void xsNameThisThread(const char* threadName); +XSTYPES_DLL_API void xsNameThisThread(const char* threadName); - #define xsGetCurrentThreadId() GetCurrentThreadId() - #define xsSuspendThread(thrd) SuspendThread(thrd) - #define xsResumeThread(thrd) ResumeThread(thrd) - #define xsSetThreadPriority(thrd,prio) SetThreadPriority(thrd,prio) - #define xsGetThreadPriority(thrd) GetThreadPriority(thrd) +#define xsGetCurrentThreadId() GetCurrentThreadId() +#define xsSuspendThread(thrd) SuspendThread(thrd) +#define xsResumeThread(thrd) ResumeThread(thrd) +#define xsSetThreadPriority(thrd,prio) SetThreadPriority(thrd,prio) +#define xsGetThreadPriority(thrd) GetThreadPriority(thrd) #else #include #include #include - /*! \addtogroup enums Global enumerations - @{ - */ - /*! \brief Thread priorities for xsSetThreadPriority() and xsGetThreadPriority() - */ - enum XsThreadPriority { - XS_THREAD_PRIORITY_LOWEST = 0, //THREAD_PRIORITY_IDLE, - XS_THREAD_PRIORITY_LOWER = 1, //THREAD_PRIORITY_LOWEST, - XS_THREAD_PRIORITY_LOW = 2, //THREAD_PRIORITY_BELOW_NORMAL, - XS_THREAD_PRIORITY_NORMAL = 3, //THREAD_PRIORITY_NORMAL, - XS_THREAD_PRIORITY_HIGH = 4, //THREAD_PRIORITY_ABOVE_NORMAL, - XS_THREAD_PRIORITY_HIGHER = 5, //THREAD_PRIORITY_HIGHEST, - XS_THREAD_PRIORITY_HIGHEST = 6 //THREAD_PRIORITY_TIME_CRITICAL - }; - /*! @} */ - - // The components of the type of a thread function - #define XSENS_THREAD_RETURN void* // DWORD - #define XSENS_THREAD_TYPE // WINAPI - #define XSENS_THREAD_PARAM void* // LPVOID - - #define XSENS_INVALID_THREAD 0 // INVALID_HANDLE_VALUE - - //! Release the remainder of the timeslice so other operations can run. - #define xsYield() sched_yield() - - XSTYPES_DLL_API void xsNameThisThread(const char* threadName); - //! A handle for a thread - typedef pthread_t XsThread; - typedef pthread_t XsThreadId; - - //! Start a function as a thread - pthread_t xsStartThread(void *(func)(void *), void *param, void *pid); - #define xsGetCurrentThreadId() pthread_self() - #define xsSuspendThread(thrd) - #define xsResumeThread(thrd) - #define xsSetThreadPriority(thrd,prio) +/*! \addtogroup enums Global enumerations + @{ +*/ +/*! \brief Thread priorities for xsSetThreadPriority() and xsGetThreadPriority() +*/ +enum XsThreadPriority +{ + XS_THREAD_PRIORITY_LOWEST = 0, //THREAD_PRIORITY_IDLE, + XS_THREAD_PRIORITY_LOWER = 1, //THREAD_PRIORITY_LOWEST, + XS_THREAD_PRIORITY_LOW = 2, //THREAD_PRIORITY_BELOW_NORMAL, + XS_THREAD_PRIORITY_NORMAL = 3, //THREAD_PRIORITY_NORMAL, + XS_THREAD_PRIORITY_HIGH = 4, //THREAD_PRIORITY_ABOVE_NORMAL, + XS_THREAD_PRIORITY_HIGHER = 5, //THREAD_PRIORITY_HIGHEST, + XS_THREAD_PRIORITY_HIGHEST = 6 //THREAD_PRIORITY_TIME_CRITICAL +}; +/*! @} */ + +// The components of the type of a thread function +#define XSENS_THREAD_RETURN void* // DWORD +#define XSENS_THREAD_TYPE // WINAPI +#define XSENS_THREAD_PARAM void* // LPVOID + +#define XSENS_INVALID_THREAD 0 // INVALID_HANDLE_VALUE + +//! Release the remainder of the timeslice so other operations can run. +#define xsYield() sched_yield() + +XSTYPES_DLL_API void xsNameThisThread(const char* threadName); +//! A handle for a thread +typedef pthread_t XsThread; +typedef pthread_t XsThreadId; + +//! Start a function as a thread +pthread_t xsStartThread(void* (func)(void*), void* param, void* pid); +#define xsGetCurrentThreadId() pthread_self() +#define xsSuspendThread(thrd) +#define xsResumeThread(thrd) +#define xsSetThreadPriority(thrd,prio) #endif // _WIN32 diff --git a/lib/xspublic/xstypes/xstime.c b/lib/xspublic/xstypes/xstime.c index dc34d57..bcd2546 100644 --- a/lib/xspublic/xstypes/xstime.c +++ b/lib/xspublic/xstypes/xstime.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -69,14 +69,14 @@ #include #ifdef _WIN32 -# include -# include "xssimpleversion.h" + #include + #include "xssimpleversion.h" #else -# include -# include -# include -# include -# include + #include + #include + #include + #include + #include #endif #include @@ -85,10 +85,10 @@ */ //! The number of seconds in a normal day -const XsTimeStamp XsTime_secPerDay = { 60*60*24LL }; +const XsTimeStamp XsTime_secPerDay = { 60 * 60 * 24LL }; //! The number of milliseconds in a normal day -const XsTimeStamp XsTime_milliSecPerDay = { 60*60*24*1000LL }; +const XsTimeStamp XsTime_milliSecPerDay = { 60 * 60 * 24 * 1000LL }; //! The maximum positive value of an XsTimeStamp value const XsTimeStamp XsTime_timeStampMax = { 0x7FFFFFFFFFFFFFFFLL }; @@ -101,11 +101,11 @@ const XsTimeStamp XsTime_timeStampMax = { 0x7FFFFFFFFFFFFFFFLL }; clock_gettime() is not available on Apple/Darwin platforms. This function helps maintaining compatibility with Linux code. - */ +*/ #ifndef CLOCK_REALTIME -#define CLOCK_REALTIME 0 + #define CLOCK_REALTIME 0 #endif -static int clock_gettime(int clk_id, struct timespec *tp) +static int clock_gettime(int clk_id, struct timespec* tp) { (void)clk_id; struct timeval now; @@ -134,7 +134,7 @@ static int clock_gettime(int clk_id, struct timespec *tp) uint32_t XsTime_getTimeOfDay(struct tm* date_, time_t* secs_) { #ifdef _WIN32 - typedef void(WINAPI *GetSystemTimeType)(LPFILETIME); + typedef void(WINAPI * GetSystemTimeType)(LPFILETIME); static GetSystemTimeType getSystemTime = 0; static int havePreciseFileTime = 0; if (getSystemTime == 0) @@ -159,13 +159,13 @@ uint32_t XsTime_getTimeOfDay(struct tm* date_, time_t* secs_) FILETIME now; __time64_t tin; getSystemTime(&now); - t = (int64_t) (((((uint64_t) now.dwHighDateTime << 32) | now.dwLowDateTime)/10000) - 11644473600000); - tin = t/1000; + t = (int64_t)(((((uint64_t) now.dwHighDateTime << 32) | now.dwLowDateTime) / 10000) - 11644473600000); + tin = t / 1000; if (date_ != NULL) - _localtime64_s(date_,&tin); + _localtime64_s(date_, &tin); if (secs_ != NULL) *secs_ = (time_t) tin; - return (uint32_t) (t % (XsTime_secPerDay.m_msTime*1000)); + return (uint32_t)(t % (XsTime_secPerDay.m_msTime * 1000)); } else { @@ -180,7 +180,7 @@ uint32_t XsTime_getTimeOfDay(struct tm* date_, time_t* secs_) __time64_t tin; GetSystemTimeAsFileTime(&now); - t = (int64_t) (((((uint64_t) now.dwHighDateTime << 32) | now.dwLowDateTime)/10000) - 11644473600000); + t = (int64_t)(((((uint64_t) now.dwHighDateTime << 32) | now.dwLowDateTime) / 10000) - 11644473600000); if (QueryPerformanceCounter(&pc)) { @@ -194,9 +194,9 @@ uint32_t XsTime_getTimeOfDay(struct tm* date_, time_t* secs_) startTimeSysTime = t; } - tNow = startTimeSysTime + (1000*(pc.QuadPart - startTimePerfCount))/perfCountFreq; //lint !e414 + tNow = startTimeSysTime + (1000 * (pc.QuadPart - startTimePerfCount)) / perfCountFreq; //lint !e414 - if (t > tNow || (tNow-t) > CORRECTION_DELTA_MS) + if (t > tNow || (tNow - t) > CORRECTION_DELTA_MS) { startTimePerfCount = pc.QuadPart; startTimeSysTime = t; @@ -205,25 +205,25 @@ uint32_t XsTime_getTimeOfDay(struct tm* date_, time_t* secs_) t = tNow; } - tin = t/1000; + tin = t / 1000; if (date_ != NULL) - _localtime64_s(date_,&tin); + _localtime64_s(date_, &tin); if (secs_ != NULL) *secs_ = (time_t) tin; - return (uint32_t) (t % (XsTime_secPerDay.m_msTime*1000)); + return (uint32_t)(t % (XsTime_secPerDay.m_msTime * 1000)); } #else struct timespec tp; clock_gettime(CLOCK_REALTIME, &tp); // compile with -lrt if (date_ != NULL) - localtime_r(&tp.tv_sec,date_); + localtime_r(&tp.tv_sec, date_); if (secs_ != NULL) secs_[0] = tp.tv_sec; // 86400 = 24*60*60 = secs in a day, this gives us the seconds since midnight - return (uint32_t)((1000 * (tp.tv_sec % XsTime_secPerDay.m_msTime)) + (tp.tv_nsec/1000000)); + return (uint32_t)((1000 * (tp.tv_sec % XsTime_secPerDay.m_msTime)) + (tp.tv_nsec / 1000000)); #endif } @@ -231,7 +231,7 @@ uint32_t XsTime_getTimeOfDay(struct tm* date_, time_t* secs_) \param date : if non-zero the local (!) date and time is stored in the tm struct this parameter points to \returns The UTC date and time as seconds since 1970 */ -int64_t XsTime_getDateTime(struct tm *date) +int64_t XsTime_getDateTime(struct tm* date) { #ifdef _WIN32 __time64_t t; @@ -242,9 +242,9 @@ int64_t XsTime_getDateTime(struct tm *date) #else time_t t; time(&t); - if(date != 0) + if (date != 0) { - struct tm *result = localtime(&t); + struct tm* result = localtime(&t); memcpy(date, result, sizeof(struct tm)); } return (int64_t)t; @@ -253,25 +253,32 @@ int64_t XsTime_getDateTime(struct tm *date) /*! \brief Retrieves the date as string representation The format is YYYYMMDD - so 25 dec 2010 is stored as an array dest[8] = {'2', '0', '1', '0', '1', '2', '2', '5' } + so 25 dec 2010 is stored as an array dest[9] = { '2', '0', '1', '0', '1', '2', '2', '5' } \param dest : A pointer to an array of at least (!) 8 bytes \param date : If date is non-zero this date is converted, otherwise the current date is retrieved and used) */ void XsTime_getDateAsString(char* dest, const struct tm* date) { struct tm dt; - int year, month; + unsigned int year, month; - if(date != 0) + if (date != 0) dt = *date; else XsTime_getDateTime(&dt); - year = dt.tm_year + 1900; - month = dt.tm_mon + 1; + year = (unsigned int) (dt.tm_year + 1900); + month = (unsigned int) (dt.tm_mon + 1); char tmpDest[9]; - snprintf(tmpDest, 9, "%04d%02d%02d", year, month, dt.tm_mday); +#if defined(__GNUC__) && !defined(__APPLE__) && !defined(__ANDROID_API__) + #pragma GCC diagnostic push + #pragma GCC diagnostic ignored "-Wformat-truncation" // valid date values will always fit properly +#endif + snprintf(tmpDest, 9, "%04u%02u%02u", year, month, (unsigned int) dt.tm_mday); +#if defined(__GNUC__) && !defined(__APPLE__) && !defined(__ANDROID_API__) + #pragma GCC diagnostic pop +#endif memcpy(dest, tmpDest, 8); } @@ -307,7 +314,7 @@ void XsTime_getDateAsWString(wchar_t* dest, const struct tm* date) struct tm dt; int year, month; - if(date != 0) + if (date != 0) dt = *date; else XsTime_getDateTime(&dt); @@ -370,8 +377,7 @@ void XsTime_udelay(uint64_t us) do { QueryPerformanceCounter(&stop); - } - while (start.QuadPart + ((double)(us) * countPerMicroSecond) > stop.QuadPart); + } while (start.QuadPart + ((double)(us) * countPerMicroSecond) > stop.QuadPart); #else struct timespec ts; int ret = -1; @@ -401,7 +407,7 @@ int64_t XsTime_timeStampNow(XsTimeStamp* now) now = &tmp; now->m_msTime = (long long) XsTime_getTimeOfDay(NULL, &s); - now->m_msTime = (now->m_msTime % 1000) + (((long long)s)*1000); + now->m_msTime = (now->m_msTime % 1000) + (((long long)s) * 1000); return now->m_msTime; } @@ -424,8 +430,8 @@ void XsTime_initializeTime() GetSystemTimeAsFileTime(&now); FileTimeToLocalFileTime(&now, &nowloc); - tutc = (int64_t) (((((uint64_t) now.dwHighDateTime << 32) | now.dwLowDateTime)/10000) - 11644473600000); - tloc = (int64_t) (((((uint64_t) nowloc.dwHighDateTime << 32) | nowloc.dwLowDateTime)/10000) - 11644473600000); + tutc = (int64_t)(((((uint64_t) now.dwHighDateTime << 32) | now.dwLowDateTime) / 10000) - 11644473600000); + tloc = (int64_t)(((((uint64_t) nowloc.dwHighDateTime << 32) | nowloc.dwLowDateTime) / 10000) - 11644473600000); #else struct timespec tp; diff --git a/lib/xspublic/xstypes/xstime.h b/lib/xspublic/xstypes/xstime.h index ff17a2d..3207b0b 100644 --- a/lib/xspublic/xstypes/xstime.h +++ b/lib/xspublic/xstypes/xstime.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -67,7 +67,7 @@ #include "xstypesconfig.h" #ifdef _WIN32 -#include + #include #endif #include @@ -84,7 +84,7 @@ XSTYPES_DLL_API extern const XsTimeStamp XsTime_milliSecPerDay; XSTYPES_DLL_API extern const XsTimeStamp XsTime_timeStampMax; XSTYPES_DLL_API uint32_t XsTime_getTimeOfDay(struct tm* date_, time_t* secs_); -XSTYPES_DLL_API int64_t XsTime_getDateTime(struct tm * date); +XSTYPES_DLL_API int64_t XsTime_getDateTime(struct tm* date); XSTYPES_DLL_API void XsTime_getDateAsString(char* dest, struct tm const* date); XSTYPES_DLL_API void XsTime_getTimeAsString(char* dest, struct tm const* time); XSTYPES_DLL_API void XsTime_getDateAsWString(wchar_t* dest, struct tm const* date); @@ -99,94 +99,95 @@ XSTYPES_DLL_API int64_t XsTime_localToUtc(); #ifdef __cplusplus } // extern "C" -namespace XsTime { +namespace XsTime +{ #ifdef __GNUC__ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wunused-variable" + #pragma GCC diagnostic push + #pragma GCC diagnostic ignored "-Wunused-variable" #endif - /*! \brief The number of seconds in a day */ - static const XsTimeStamp& secPerDay = XsTime_secPerDay; - /*! \brief The number of milliseconds in a day */ - static const XsTimeStamp& milliSecPerDay = XsTime_milliSecPerDay; - /*! \brief The maximum possible timestamp */ - static const XsTimeStamp& timeStampMax = XsTime_timeStampMax; +/*! \brief The number of seconds in a day */ +static const XsTimeStamp& secPerDay = XsTime_secPerDay; +/*! \brief The number of milliseconds in a day */ +static const XsTimeStamp& milliSecPerDay = XsTime_milliSecPerDay; +/*! \brief The maximum possible timestamp */ +static const XsTimeStamp& timeStampMax = XsTime_timeStampMax; #ifdef __GNUC__ -#pragma GCC diagnostic pop + #pragma GCC diagnostic pop #endif - //! \copydoc XsTime_getTimeOfDay - inline uint32_t getTimeOfDay(tm* date_ = NULL, time_t* secs_ = NULL) - { - return XsTime_getTimeOfDay(date_ , secs_); - } - - //! \copydoc XsTime_getDateTime - inline int64_t getDateTime(tm * date = 0) - { - return XsTime_getDateTime(date); - } - - //! \copydoc XsTime_getDateAsString - inline void getDateAsString(char* dest, tm const* date = 0) - { - XsTime_getDateAsString(dest, date); - } - - //! \copydoc XsTime_getTimeAsString - inline void getTimeAsString(char* dest, tm const* date = 0) - { - XsTime_getTimeAsString(dest, date); - } - - /*! \brief Returns the date as a readable string - \param date to convert to string - \returns The date as a readable string - \sa XsTime_getDateAsWString - */ - inline XsString getDateAsString(tm const* date = 0) - { - wchar_t wcharBuf[9]; - XsTime_getDateAsWString(wcharBuf, date); - wcharBuf[8] = 0; - return XsString(wcharBuf); - } - - /*! \brief Returns the time as a readable string - \param time to convert to string - \returns The time as a readable string - \sa XsTime_getTimeAsWString - */ - inline XsString getTimeAsString(tm const* time = 0) - { - wchar_t wcharBuf[9]; - XsTime_getTimeAsWString(wcharBuf, time); - wcharBuf[8] = 0; - return XsString(wcharBuf); - } - - //! \copydoc XsTime_msleep - inline void msleep(uint32_t ms) noexcept - { - XsTime_msleep(ms); - } - - //! \copydoc XsTime_udelay - inline void udelay(uint64_t us) noexcept - { - XsTime_udelay(us); - } - - //! \copydoc XsTime_initializeTime - inline void initializeTime() - { - XsTime_initializeTime(); - } - - //! \copydoc XsTime_timeStampNow - inline int64_t timeStampNow(XsTimeStamp* now = 0) - { - return XsTime_timeStampNow(now); - } +//! \copydoc XsTime_getTimeOfDay +inline uint32_t getTimeOfDay(tm* date_ = NULL, time_t* secs_ = NULL) +{ + return XsTime_getTimeOfDay(date_, secs_); +} + +//! \copydoc XsTime_getDateTime +inline int64_t getDateTime(tm* date = 0) +{ + return XsTime_getDateTime(date); +} + +//! \copydoc XsTime_getDateAsString +inline void getDateAsString(char* dest, tm const* date = 0) +{ + XsTime_getDateAsString(dest, date); +} + +//! \copydoc XsTime_getTimeAsString +inline void getTimeAsString(char* dest, tm const* date = 0) +{ + XsTime_getTimeAsString(dest, date); +} + +/*! \brief Returns the date as a readable string + \param date to convert to string + \returns The date as a readable string + \sa XsTime_getDateAsWString +*/ +inline XsString getDateAsString(tm const* date = 0) +{ + wchar_t wcharBuf[9]; + XsTime_getDateAsWString(wcharBuf, date); + wcharBuf[8] = 0; + return XsString(wcharBuf); +} + +/*! \brief Returns the time as a readable string + \param time to convert to string + \returns The time as a readable string + \sa XsTime_getTimeAsWString +*/ +inline XsString getTimeAsString(tm const* time = 0) +{ + wchar_t wcharBuf[9]; + XsTime_getTimeAsWString(wcharBuf, time); + wcharBuf[8] = 0; + return XsString(wcharBuf); +} + +//! \copydoc XsTime_msleep +inline void msleep(uint32_t ms) noexcept +{ + XsTime_msleep(ms); +} + +//! \copydoc XsTime_udelay +inline void udelay(uint64_t us) noexcept +{ + XsTime_udelay(us); +} + +//! \copydoc XsTime_initializeTime +inline void initializeTime() +{ + XsTime_initializeTime(); +} + +//! \copydoc XsTime_timeStampNow +inline int64_t timeStampNow(XsTimeStamp* now = 0) +{ + return XsTime_timeStampNow(now); +} } #endif diff --git a/lib/xspublic/xstypes/xstimeinfo.c b/lib/xspublic/xstypes/xstimeinfo.c index c964382..6f0533f 100644 --- a/lib/xspublic/xstypes/xstimeinfo.c +++ b/lib/xspublic/xstypes/xstimeinfo.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -76,7 +76,7 @@ /*! \relates XsTimeInfo \brief Returns the current UTC time */ -void XsTimeInfo_currentTime(struct XsTimeInfo * thisPtr) +void XsTimeInfo_currentTime(struct XsTimeInfo* thisPtr) { XsTimeStamp timeStamp; @@ -90,7 +90,7 @@ void XsTimeInfo_currentTime(struct XsTimeInfo * thisPtr) /*! \relates XsTimeInfo \brief Returns the current local time */ -void XsTimeInfo_currentLocalTime(struct XsTimeInfo * thisPtr) +void XsTimeInfo_currentLocalTime(struct XsTimeInfo* thisPtr) { XsTimeStamp timeStamp; @@ -105,7 +105,7 @@ void XsTimeInfo_currentLocalTime(struct XsTimeInfo * thisPtr) /*! \relates XsTimeInfo \brief Removes the local time information, making the object pure UTC */ -void XsTimeInfo_makeUtc(struct XsTimeInfo * thisPtr) +void XsTimeInfo_makeUtc(struct XsTimeInfo* thisPtr) { if (thisPtr->m_utcOffset) { diff --git a/lib/xspublic/xstypes/xstimeinfo.h b/lib/xspublic/xstypes/xstimeinfo.h index 83e824e..1460f69 100644 --- a/lib/xspublic/xstypes/xstimeinfo.h +++ b/lib/xspublic/xstypes/xstimeinfo.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -69,6 +69,7 @@ #include "pstdint.h" #ifdef __cplusplus +#include extern "C" { #endif #define XSTIMEINFO_INITIALIZER { 0, 0 ,0, 0, 0, 0, 0, 0, 0} @@ -126,14 +127,14 @@ struct XsTimeInfo inline bool operator == (const XsTimeInfo& other) const { return m_nano == other.m_nano && - m_year == other.m_year && - m_month == other.m_month && - m_day == other.m_day && - m_hour == other.m_hour && - m_minute == other.m_minute && - m_second == other.m_second && - m_valid == other.m_valid && - m_utcOffset == other.m_utcOffset; + m_year == other.m_year && + m_month == other.m_month && + m_day == other.m_day && + m_hour == other.m_hour && + m_minute == other.m_minute && + m_second == other.m_second && + m_valid == other.m_valid && + m_utcOffset == other.m_utcOffset; } /*! \brief Removes the local time information, making the object pure UTC */ diff --git a/lib/xspublic/xstypes/xstimestamp.c b/lib/xspublic/xstypes/xstimestamp.c index ba585d9..2e8fff8 100644 --- a/lib/xspublic/xstypes/xstimestamp.c +++ b/lib/xspublic/xstypes/xstimestamp.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -66,7 +66,7 @@ #include "xstime.h" #include "xstimeinfo.h" #ifndef XSENS_NO_STL -#include "xsstring.h" + #include "xsstring.h" #endif #include #include @@ -89,42 +89,42 @@ void XsTimeStamp_setMilliSecondTime(XsTimeStamp* thisPtr, int64_t t) /*! \relates XsTimeStamp \brief Get the time of day component of the stored timestamp in seconds as a double precision value. */ double XsTimeStamp_timeOfDay(const XsTimeStamp* thisPtr) { - return (double)(thisPtr->m_msTime % (24*60*60*1000))*0.001; + return (double)(thisPtr->m_msTime % (24 * 60 * 60 * 1000)) * 0.001; } /*! \relates XsTimeStamp \brief Returns the number of seconds elapsed since the epoch as stored in the XsTimeStamp */ int64_t XsTimeStamp_secondTime(const XsTimeStamp* thisPtr) { - return thisPtr->m_msTime/1000; + return thisPtr->m_msTime / 1000; } /*! \relates XsTimeStamp \brief Returns the millisecond part of the time (in the range 0-999) */ int32_t XsTimeStamp_milliSecondPart(const XsTimeStamp* thisPtr) { - return (int32_t) (thisPtr->m_msTime % 1000); + return (int32_t)(thisPtr->m_msTime % 1000); } /*! \relates XsTimeStamp \brief Returns the seconds part of the time (in the range 0-59) */ int32_t XsTimeStamp_secondPart(const XsTimeStamp* thisPtr) { - return (int32_t) ((thisPtr->m_msTime/(1000))%60); + return (int32_t)((thisPtr->m_msTime / (1000)) % 60); } /*! \relates XsTimeStamp \brief Returns the minutes part of the time (in the range 0-59) */ int32_t XsTimeStamp_minutePart(const XsTimeStamp* thisPtr) { - return (int32_t) ((thisPtr->m_msTime/(60*1000))%60); + return (int32_t)((thisPtr->m_msTime / (60 * 1000)) % 60); } /*! \relates XsTimeStamp \brief Returns the hours part of the time (in the range 0-23) */ int32_t XsTimeStamp_hourPart(const XsTimeStamp* thisPtr) { - return (int32_t) ((thisPtr->m_msTime/(60*60*1000))%24); + return (int32_t)((thisPtr->m_msTime / (60 * 60 * 1000)) % 24); } /*! \relates XsTimeStamp @@ -201,7 +201,7 @@ void XsTimeStamp_toTimeInfo(struct XsTimeStamp const* thisPtr, struct XsTimeInfo return; } #elif (defined(__arm__) && defined(__ARMCC_VERSION)) - #warning Function is not thread-safe for this platform/toolchain +#warning Function is not thread-safe for this platform/toolchain time_t t; t = (time_t)(thisPtr->m_msTime / 1000); struct tm* tmUtcPtr = gmtime(&t); @@ -228,7 +228,7 @@ void XsTimeStamp_toTimeInfo(struct XsTimeStamp const* thisPtr, struct XsTimeInfo info->m_month = (uint8_t) tmUtc.tm_mon + 1; info->m_nano = (uint32_t)((thisPtr->m_msTime % 1000) * 1e6); info->m_second = (uint8_t) tmUtc.tm_sec; - info->m_year = (uint16_t) (tmUtc.tm_year + 1900); + info->m_year = (uint16_t)(tmUtc.tm_year + 1900); info->m_valid = 1; info->m_utcOffset = 0; } @@ -253,7 +253,7 @@ void XsTimeStamp_toString(struct XsTimeStamp const* thisPtr, struct XsString* re , (int) info.m_hour , (int) info.m_minute , (int) info.m_second - , (int) (thisPtr->m_msTime % 1000)); + , (int)(thisPtr->m_msTime % 1000)); XsString_assign(result, 23, buffer); } else @@ -308,7 +308,7 @@ void XsTimeStamp_localToUtcTime2(struct XsTimeStamp const* thisPtr, struct XsTim */ void XsTimeStamp_utcToLocalTime_ms(struct XsTimeStamp const* thisPtr, struct XsTimeStamp* local, int64_t utcOffset) { - local->m_msTime = thisPtr->m_msTime - (thisPtr->m_msTime ? utcOffset: 0LL); + local->m_msTime = thisPtr->m_msTime - (thisPtr->m_msTime ? utcOffset : 0LL); } /*! \brief Convert the supplied time from (assumed) local time to UTC, using the offset in \a utcOffset diff --git a/lib/xspublic/xstypes/xstimestamp.h b/lib/xspublic/xstypes/xstimestamp.h index ba6f9dc..5fefb31 100644 --- a/lib/xspublic/xstypes/xstimestamp.h +++ b/lib/xspublic/xstypes/xstimestamp.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -70,7 +70,7 @@ #ifdef __cplusplus #ifndef XSENS_NO_STL -#include "xsstring.h" + #include "xsstring.h" #endif #include "xstimeinfo.h" extern "C" { @@ -112,7 +112,8 @@ XSTYPES_DLL_API void XsTimeStamp_localToUtcTime_ms(struct XsTimeStamp const* thi /*! \struct XsTimeStamp \brief Class for managing timestamps in a unified way. */ -struct XsTimeStamp { +struct XsTimeStamp +{ #ifdef __cplusplus /*! \brief Construct a timestamp with \a t as the time in milliseconds. */ inline XsTimeStamp(int64_t t = 0) : m_msTime(t) {} @@ -121,7 +122,7 @@ struct XsTimeStamp { inline XsTimeStamp(int t) : m_msTime(t) {} /*! \brief Construct a timestamp with \a t as the time in seconds. */ - inline explicit XsTimeStamp(double t) : m_msTime((int64_t)(t*1000.0)) {} + inline explicit XsTimeStamp(double t) : m_msTime((int64_t)(t * 1000.0)) {} /*! \brief Construct a copy of \a other. */ inline XsTimeStamp(const XsTimeStamp& other) : m_msTime(other.m_msTime) {} @@ -165,113 +166,169 @@ struct XsTimeStamp { /*! \brief Get the stored time as milliseconds. */ inline int64_t msTime(void) const - { return m_msTime; } + { + return m_msTime; + } /*! \brief Set the stored time to \a t milliseconds. */ inline void setMsTime(int64_t t) - { m_msTime = t; } + { + m_msTime = t; + } /*! \brief Get the time of day component of the stored timestamp in seconds as a double precision value. */ inline double timeOfDay() const - { return XsTimeStamp_timeOfDay(this); } + { + return XsTimeStamp_timeOfDay(this); + } /*! \brief Get the time of day component of the stored timestamp in milliseconds */ inline int64_t msTimeOfDay() const - { return m_msTime % (24*60*60*1000); } + { + return m_msTime % (24 * 60 * 60 * 1000); + } /*! \brief Return the time as seconds */ inline double secTime() const - { return ((double)m_msTime)*0.001; } + { + return ((double)m_msTime) * 0.001; + } /*! \brief Set the time as seconds */ inline void setSecTime(double t) - { m_msTime = (int64_t) (t*1000.0); } + { + m_msTime = (int64_t)(t * 1000.0); + } /*! \brief Get the sum of the current and the given \a other timestamp. \param other The value to add to this \returns The added timestamp values */ inline XsTimeStamp operator + (const XsTimeStamp& other) const - { return XsTimeStamp(m_msTime + other.m_msTime); } + { + return XsTimeStamp(m_msTime + other.m_msTime); + } /*! \brief Get the current minus the given \a other timestamp. \param other The value to subtract from this \returns The subtracted timestamp values */ inline XsTimeStamp operator - (const XsTimeStamp& other) const - { return XsTimeStamp(m_msTime - other.m_msTime); } + { + return XsTimeStamp(m_msTime - other.m_msTime); + } /*! \brief Get the result of adding the given \a other timestamp to the current timestamp. */ inline XsTimeStamp& operator += (const XsTimeStamp& other) - { m_msTime += other.m_msTime; return *this; } + { + m_msTime += other.m_msTime; + return *this; + } /*! \brief Get the result of subtracting the given \a other timestamp from the current timestamp. */ inline XsTimeStamp& operator -= (const XsTimeStamp& d) - { m_msTime -= d.m_msTime; return *this; } + { + m_msTime -= d.m_msTime; + return *this; + } /*! \brief Test if the given \a other timestamp is smaller than the current timestamp. */ - inline bool operator < (const XsTimeStamp& other) const - { return m_msTime < other.m_msTime; } + inline bool operator < (const XsTimeStamp& other) const + { + return m_msTime < other.m_msTime; + } /*! \brief Test if the given \a other timestamp is smaller than or equal to the current timestamp. */ inline bool operator <= (const XsTimeStamp& other) const - { return m_msTime <= other.m_msTime; } + { + return m_msTime <= other.m_msTime; + } /*! \brief Test if the given \a other timestamp is equal to the current timestamp. */ inline bool operator == (const XsTimeStamp& other) const - { return m_msTime == other.m_msTime; } + { + return m_msTime == other.m_msTime; + } /*! \brief Test if the given \a other timestamp is larger than the current timestamp. */ - inline bool operator > (const XsTimeStamp& other) const - { return m_msTime > other.m_msTime; } + inline bool operator > (const XsTimeStamp& other) const + { + return m_msTime > other.m_msTime; + } /*! \brief Test if the given \a other timestamp is larger than or equal to the current timestamp. */ inline bool operator >= (const XsTimeStamp& other) const - { return m_msTime >= other.m_msTime; } + { + return m_msTime >= other.m_msTime; + } /*! \brief Test if the given \a other timestamp is not equal to the current timestamp. */ inline bool operator != (const XsTimeStamp& other) const - { return m_msTime != other.m_msTime; } + { + return m_msTime != other.m_msTime; + } /*! \brief Test if the given \a other is smaller than the current timestamp. */ - inline bool operator < (int other) const - { return m_msTime < other; } + inline bool operator < (int other) const + { + return m_msTime < other; + } /*! \brief Test if the given \a other is smaller than or equal to the current timestamp. */ inline bool operator <= (int other) const - { return m_msTime <= other; } + { + return m_msTime <= other; + } /*! \brief Test if the given \a other is equal to the current timestamp. */ inline bool operator == (int other) const - { return m_msTime == other; } + { + return m_msTime == other; + } /*! \brief Test if the given \a other is larger than the current timestamp. */ - inline bool operator > (int other) const - { return m_msTime > other; } + inline bool operator > (int other) const + { + return m_msTime > other; + } /*! \brief Test if the given \a other is larger than or equal to the current timestamp. */ inline bool operator >= (int other) const - { return m_msTime >= other; } + { + return m_msTime >= other; + } /*! \brief Test if the given \a other is not equal to the current timestamp. */ inline bool operator != (int other) const - { return m_msTime != other; } + { + return m_msTime != other; + } /*! \brief Returns the number of seconds elapsed since the epoch as stored in the XsTimeStamp */ inline int64_t secondTime() const - { return m_msTime/1000; } + { + return m_msTime / 1000; + } /*! \brief Returns the millisecond part of the time (in the range 0-999) */ inline int32_t milliSecondPart() const - { return (int32_t) (m_msTime % 1000); } + { + return (int32_t)(m_msTime % 1000); + } /*! \brief Returns the seconds part of the time (in the range 0-59) */ inline int32_t secondPart() const - { return (int32_t) ((m_msTime/(1000))%60); } + { + return (int32_t)((m_msTime / (1000)) % 60); + } /*! \brief Returns the minutes part of the time (in the range 0-59) */ inline int32_t minutePart() const - { return (int32_t) ((m_msTime/(60*1000))%60); } + { + return (int32_t)((m_msTime / (60 * 1000)) % 60); + } /*! \brief Returns the hours part of the time (in the range 0-23) */ inline int32_t hourPart() const - { return (int32_t) ((m_msTime/(60*60*1000))%24); } + { + return (int32_t)((m_msTime / (60 * 60 * 1000)) % 24); + } /*! \brief Returns the current time in ms since the epoch (Jan 1st 1970) */ inline static XsTimeStamp now() @@ -303,19 +360,29 @@ struct XsTimeStamp { /*! \brief Increment the timestamp by one ms, prefix */ XsTimeStamp& operator++() - { ++m_msTime; return *this; } + { + ++m_msTime; + return *this; + } /*! \brief Increment the timestamp by one ms, postfix */ XsTimeStamp operator++(int) - { return XsTimeStamp(m_msTime++); } + { + return XsTimeStamp(m_msTime++); + } /*! \brief Decrement the timestamp by one ms, prefix */ XsTimeStamp& operator--() - { --m_msTime; return *this; } + { + --m_msTime; + return *this; + } /*! \brief Decrement the timestamp by one ms, postfix */ XsTimeStamp operator--(int) - { return XsTimeStamp(m_msTime--); } + { + return XsTimeStamp(m_msTime--); + } /*! \brief Convert the supplied time from (assumed) UTC to local time, using the system's local time zone knowledge \returns The converted time diff --git a/lib/xspublic/xstypes/xstriggerindicationdata.c b/lib/xspublic/xstypes/xstriggerindicationdata.c index 5d0ddaf..7799bc2 100644 --- a/lib/xspublic/xstypes/xstriggerindicationdata.c +++ b/lib/xspublic/xstypes/xstriggerindicationdata.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xstypes/xstriggerindicationdata.h b/lib/xspublic/xstypes/xstriggerindicationdata.h index 5d5ed3d..21fa6dc 100644 --- a/lib/xspublic/xstypes/xstriggerindicationdata.h +++ b/lib/xspublic/xstypes/xstriggerindicationdata.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -86,7 +86,8 @@ XSTYPES_DLL_API int XsTriggerIndicationData_valid(const struct XsTriggerIndicati /*! \brief Data for a trigger indication message */ -struct XsTriggerIndicationData { +struct XsTriggerIndicationData +{ uint8_t m_line; //!< The line number uint8_t m_polarity; //!< The polarity uint32_t m_timestamp; //!< The timestamp @@ -100,7 +101,7 @@ struct XsTriggerIndicationData { \param[in] frameNumber Frame number */ explicit XsTriggerIndicationData(uint8_t line = 0, uint8_t polarity = 0, uint32_t timestamp = 0, uint16_t frameNumber = 0) - : m_line(line), m_polarity(polarity), m_timestamp(timestamp), m_frameNumber(frameNumber) + : m_line(line), m_polarity(polarity), m_timestamp(timestamp), m_frameNumber(frameNumber) {} /*! \brief \copybrief XsTriggerIndicationData_destruct */ @@ -119,9 +120,9 @@ struct XsTriggerIndicationData { inline bool operator == (XsTriggerIndicationData const& other) const { return m_line == other.m_line && - m_polarity == other.m_polarity && - m_timestamp == other.m_timestamp && - m_frameNumber == other.m_frameNumber; + m_polarity == other.m_polarity && + m_timestamp == other.m_timestamp && + m_frameNumber == other.m_frameNumber; } #endif }; diff --git a/lib/xspublic/xstypes/xstypedefs.c b/lib/xspublic/xstypes/xstypedefs.c index 2c384e4..10cb431 100644 --- a/lib/xspublic/xstypes/xstypedefs.c +++ b/lib/xspublic/xstypes/xstypedefs.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -75,9 +75,9 @@ \note This function is NOT reentrant, multiple simultaneous calls may cause crashes. Also, later calls will invalidate the results of earlier calls. */ -const char *XsDataFlags_toString(XsDataFlags f) +const char* XsDataFlags_toString(XsDataFlags f) { - static char rv[4*20]; + static char rv[4 * 20]; if (f == XSDF_None) return "XSDF_None"; diff --git a/lib/xspublic/xstypes/xstypedefs.h b/lib/xspublic/xstypes/xstypedefs.h index a902bd2..c5feae5 100644 --- a/lib/xspublic/xstypes/xstypedefs.h +++ b/lib/xspublic/xstypes/xstypedefs.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -126,6 +126,13 @@ inline const char* toString(XsDataFlags s) return XsDataFlags_toString(s); } #endif +#ifdef SWIG + // note that there is no XSDEPRECATED_END, deprecated definitions are expected to be the last definitions in a list + #define XSDEPRECATED_START private: /* deprecated start */ +#else + // note that there is no XSDEPRECATED_END, deprecated definitions are expected to be the last definitions in a list + #define XSDEPRECATED_START /* deprecated start */ +#endif #ifndef __cplusplus // define BOOL, TRUE and FALSE #ifndef BOOL @@ -139,21 +146,70 @@ inline const char* toString(XsDataFlags s) #ifndef FALSE #define FALSE (0) #endif - #define XSFALLTHROUGH - #define XSNORETURN + #define XSFALLTHROUGH /* fallthrough */ + #define XSNORETURN /* noreturn */ + #define XSDEPRECATED /* deprecated */ #else #if __cplusplus >= 201703L - #define XSFALLTHROUGH [[fallthrough]] - #define XSNORETURN [[noreturn]] - #elif defined(__GNUC__) - #define XSFALLTHROUGH [[fallthrough]] - #define XSNORETURN [[noreturn]] + #define XSFALLTHROUGH [[fallthrough]] /* FALLTHRU fallthrough */ + #define XSNORETURN [[noreturn]] /* noreturn */ + #define XSDEPRECATED [[deprecated]] /* deprecated */ #else - #define XSFALLTHROUGH // FALLTHRU - #define XSNORETURN + #if defined(__GNUC__) + #define XSFALLTHROUGH [[fallthrough]] /* FALLTHRU fallthrough */ + #define XSNORETURN [[noreturn]] /* noreturn */ + #else + #define XSFALLTHROUGH /* FALLTHRU fallthrough */ + #define XSNORETURN /* noreturn */ + #endif + #if __cplusplus >= 201402L + #define XSDEPRECATED [[deprecated]] /* deprecated */ + #else + #define XSDEPRECATED /* deprecated */ + #endif #endif #endif // __cplusplus #define XS_ENUM_TO_STR_CASE(value) case value: return #value +// different alignment commands for gcc / MSVS for structures that need to be 1-byte aligned. +#if defined (SWIG) + #define XS_PACKED_STRUCT_START + #define XS_PACKED_STRUCT_END +#elif defined (_MSC_VER) + #define XS_PACKED_STRUCT_START __pragma(pack(push, 1)); + #define XS_PACKED_STRUCT_END __pragma(pack(pop)); +#elif defined (__ICCARM__) + #define XS_PACKED_STRUCT_START _Pragma("pack(push, 1)"); + #define XS_PACKED_STRUCT_END _Pragma("pack(pop)"); +#elif defined (__ICC8051__) + #define XS_PACKED_STRUCT_START + #define XS_PACKED_STRUCT_END +#elif defined (__GNUC__) + #define XS_PACKED_STRUCT_START + #define XS_PACKED_STRUCT_END +#elif (defined(__arm__) && defined(__ARMCC_VERSION)) + #define XS_PACKED_STRUCT_START + #define XS_PACKED_STRUCT_END +#elif (defined(__ADSP21000__)) + #define XS_PACKED_STRUCT_START + #define XS_PACKED_STRUCT_END +#else + #error "Structure packing macros not defined for this compiler" + #define XS_PACKED_STRUCT_START + #define XS_PACKED_STRUCT_END +#endif + +#ifdef __GNUC__ + #define XS_PACKED_STRUCT __attribute__((__packed__)) +#else + #define XS_PACKED_STRUCT +#endif + +#if defined (__ICCARM__) + #define XS_PACKED_POINTER __packed +#else + #define XS_PACKED_POINTER +#endif + #endif diff --git a/lib/xspublic/xstypes/xstypesconfig.h b/lib/xspublic/xstypes/xstypesconfig.h index fbc682c..b83f3dc 100644 --- a/lib/xspublic/xstypes/xstypesconfig.h +++ b/lib/xspublic/xstypes/xstypesconfig.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -73,125 +73,129 @@ // generic preprocessor defines //http://support.microsoft.com/kb/155196 -#define __STR2__(x) #x -#define __STR1__(x) __STR2__(x) -#define __LOC__ __FILE__ "(" __STR1__(__LINE__)") : WARNING: " +#define XSTRINGIFY2(s) #s +#define XSTRINGIFY(s) XSTRINGIFY2(s) +#define __LOC__ __FILE__ "(" XSTRINGIFY(__LINE__)") : WARNING: " // make sure both _WIN32 and WIN32 are defined if either of them is. #if defined(_WIN32) || defined(_M_IX86) -# ifndef WIN32 -# define WIN32 -# endif -# define XSENS_WINDOWS + #ifndef WIN32 + #define WIN32 + #endif + #define XSENS_WINDOWS + // suggested by https://www.fluentcpp.com/2019/08/30/how-to-disable-a-warning-in-cpp/ + #define XSENS_MSC_WARNING_SUPPRESS(...) __pragma(warning(suppress: __VA_ARGS__)) +#else + #define XSENS_MSC_WARNING_SUPPRESS(...) #endif #ifdef WIN32 -# ifndef _WIN32 -# define _WIN32 -# define XSENS_WINDOWS -# endif + #ifndef _WIN32 + #define _WIN32 + #define XSENS_WINDOWS + #endif #endif #if !defined(_WIN32) && defined(_MSC_VER) && !defined(_WIN64) -# define _WIN64 + #define _WIN64 #endif // make things as secure as possible without modifying the code... #ifndef _CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES -#define _CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES 1 + #define _CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES 1 #endif #ifndef _CRT_SECURE_NO_WARNINGS -#define _CRT_SECURE_NO_WARNINGS + #define _CRT_SECURE_NO_WARNINGS #endif #ifndef __cplusplus -// make sure that const-correctness is enforced -#ifdef _MSC_VER -#pragma warning(error : 4090) -#endif -#define XSCCONST const -#define XSCPPPROTECTED + // make sure that const-correctness is enforced + #ifdef _MSC_VER + #pragma warning(error : 4090) + #endif + #define XSCCONST const + #define XSCPPPROTECTED #else -#define XSCCONST -#define XSCPPPROTECTED protected: + #define XSCCONST + #define XSCPPPROTECTED protected: #endif #ifdef __GNUC__ -# include -# if __WORDSIZE == 64 -# define XSENS_64BIT -# else -# define XSENS_32BIT -# endif -# ifndef XSENS_PFSHARED -# define XSENS_PFSHARED ".so" -# define XSENS_PFPRE "lib" -# endif + #include + #if __WORDSIZE == 64 + #define XSENS_64BIT + #else + #define XSENS_32BIT + #endif + #ifndef XSENS_PFSHARED + #define XSENS_PFSHARED ".so" + #define XSENS_PFPRE "lib" + #endif #else -# ifndef XSENS_PFSHARED -# define XSENS_PFSHARED ".dll" -# define XSENS_PFPRE "" -# endif + #ifndef XSENS_PFSHARED + #define XSENS_PFSHARED ".dll" + #define XSENS_PFPRE "" + #endif #endif #if defined(_WIN64) || defined(_M_X64) || defined(_M_IA64) -# ifndef XSENS_64BIT -# define XSENS_64BIT -# endif -# ifndef XSENS_WINDOWS -# define XSENS_WINDOWS -# endif -# ifndef _WIN64 -# define _WIN64 -# endif -# ifndef WIN64 -# define WIN64 -# endif -# ifndef XSENS_PFBITS -# ifdef __GNUC__ -# define XSENS_PFBITS "" -# else -# define XSENS_PFBITS "64" -# endif -# endif + #ifndef XSENS_64BIT + #define XSENS_64BIT + #endif + #ifndef XSENS_WINDOWS + #define XSENS_WINDOWS + #endif + #ifndef _WIN64 + #define _WIN64 + #endif + #ifndef WIN64 + #define WIN64 + #endif + #ifndef XSENS_PFBITS + #ifdef __GNUC__ + #define XSENS_PFBITS "" + #else + #define XSENS_PFBITS "64" + #endif + #endif #else -# ifndef XSENS_32BIT -# define XSENS_32BIT -# endif -# ifndef XSENS_PFBITS -# ifdef __GNUC__ -# define XSENS_PFBITS "" -# else -# define XSENS_PFBITS "32" -# endif -# endif + #ifndef XSENS_32BIT + #define XSENS_32BIT + #endif + #ifndef XSENS_PFBITS + #ifdef __GNUC__ + #define XSENS_PFBITS "" + #else + #define XSENS_PFBITS "32" + #endif + #endif #endif // all xsens libraries should use unicode #ifndef UNICODE -#define UNICODE + #define UNICODE #endif // use XSENS_32BIT and XSENS_64BIT to check for 32/64 bit builds in your application // on non-windows systems these should be defined in this file /* -Configuration | Runtime | DebInfo | Defines ---------------+--------------------------------------- -Debug | MDd | Yes | XSENS_DEBUG;_DEBUG -RelWithDeb | MD | Yes | XSENS_DEBUG;XSENS_RELEASE;_DEBUG -Release | MD | No | XSENS_RELEASE;NDEBUG - -The common way to setup configuration-dependent defines: -#if defined(XSENS_DEBUG) + Configuration | Runtime | DebInfo | Defines + --------------+--------------------------------------- + Debug | MDd | Yes | XSENS_DEBUG;_DEBUG + RelWithDeb | MD | Yes | XSENS_DEBUG;XSENS_RELEASE;_DEBUG + Release | MD | No | XSENS_RELEASE;NDEBUG + + The common way to setup configuration-dependent defines: + #if defined(XSENS_DEBUG) //// Debug or RelWithDeb build #if defined(XSENS_RELEASE) //// RelWithDeb build #else //// Debug build #endif -#else + #else //// Release build -#endif + #endif */ #if defined(XSENS_DEBUG) @@ -205,7 +209,6 @@ The common way to setup configuration-dependent defines: #if !defined(QT_DEBUG) && !defined(QT_NO_DEBUG) #define QT_DEBUG 1 #endif - #define XSENS_CONFIG 0 #ifndef XSENS_PFCONF #ifdef __GNUC__ #define XSENS_PFCONF "" @@ -218,7 +221,6 @@ The common way to setup configuration-dependent defines: #if !defined(QT_DEBUG) && !defined(QT_NO_DEBUG) #define QT_NO_DEBUG 1 #endif - #define XSENS_CONFIG 1 #ifndef XSENS_PFCONF #ifdef __GNUC__ #define XSENS_PFCONF "" @@ -229,10 +231,13 @@ The common way to setup configuration-dependent defines: #endif #else //// Release build + #if !defined(XSENS_RELEASE) + #define XSENS_RELEASE + #endif + #if !defined(QT_DEBUG) && !defined(QT_NO_DEBUG) #define QT_NO_DEBUG 1 #endif - #define XSENS_CONFIG 3 #ifndef NDEBUG #ifndef KEEP_ASSERTS #define NDEBUG // make sure assertions and other debug options are compiled away by MSVC @@ -251,33 +256,32 @@ The common way to setup configuration-dependent defines: // more generic preprocessor defines // required for gnu c++ compiler versions due to difference in attribute declarations #if defined(__AVR32__) -# define __cdecl -# define __stdcall + #define __cdecl + #define __stdcall #elif defined(_ADI_COMPILER) -# define __cdecl -# define __stdcall + #define __cdecl + #define __stdcall #elif defined(__GNUC__) && !defined(HAVE_CDECL) -# if !defined(__cdecl) -# if defined(__x86_64__) -# define __cdecl -# else -# define __cdecl __attribute__((cdecl)) -# endif -# endif -# if !defined(__stdcall) -# if defined(__x86_64__) -# define __stdcall -# else -# define __stdcall __attribute__((stdcall)) -#endif -# endif + #if !defined(__cdecl) + #if defined(__x86_64__) + #define __cdecl + #else + #define __cdecl __attribute__((cdecl)) + #endif + #endif + #if !defined(__stdcall) + #if defined(__x86_64__) + #define __stdcall + #else + #define __stdcall __attribute__((stdcall)) + #endif + #endif #elif defined(IAR_ARM_CM3_NAMESPACE) -# define __cdecl -# define __stdcall -# define XSENS_32BIT + #define __cdecl + #define __stdcall + #define XSENS_32BIT #endif - ////////////////////////////////////////////////// // generic preprocessor defines @@ -285,39 +289,28 @@ The common way to setup configuration-dependent defines: // on non-windows systems these should be defined #ifndef XSTYPES_DLL_API -# ifdef XSTYPES_DLL_EXPORT -# ifdef _WIN32 -//# pragma message("XSTYPES_DLL_API export in xstypesconfig.h") -# define XSTYPES_DLL_API __declspec(dllexport) -# else -//# pragma message("XSTYPES_DLL_API linux export in xstypesconfig.h") -# define XSTYPES_DLL_API __attribute__((visibility("default"))) -# endif -# else // ifdef XSTYPES_DLL_EXPORT -# ifdef XSTYPES_STATIC_LIB -//# pragma message("XSTYPES_DLL_API static in xstypesconfig.h") -# define XSTYPES_DLL_API -# else -# ifdef _WIN32 -//# pragma message("XSTYPES_DLL_API import in xstypesconfig.h") -# define XSTYPES_DLL_API __declspec(dllimport) -# else -//# pragma message("XSTYPES_DLL_API import/static for linux in xstypesconfig.h") -# define XSTYPES_DLL_API -# endif -# endif -# endif // ifdef XSTYPES_DLL_EXPORT - else + #ifdef XSTYPES_DLL_EXPORT + #ifdef _WIN32 + //#pragma message("XSTYPES_DLL_API export in xstypesconfig.h") + #define XSTYPES_DLL_API __declspec(dllexport) + #else + //#pragma message("XSTYPES_DLL_API linux export in xstypesconfig.h") + #define XSTYPES_DLL_API __attribute__((visibility("default"))) + #endif + #else // ifdef XSTYPES_DLL_EXPORT + #ifdef XSTYPES_STATIC_LIB + //#pragma message("XSTYPES_DLL_API static in xstypesconfig.h") + #define XSTYPES_DLL_API + #elif defined(_WIN32) + //#pragma message("XSTYPES_DLL_API import in xstypesconfig.h") + #define XSTYPES_DLL_API __declspec(dllimport) + #else + //#pragma message("XSTYPES_DLL_API import/static for linux in xstypesconfig.h") + #define XSTYPES_DLL_API + #endif + #endif // ifdef XSTYPES_DLL_EXPORT - else #endif // ifndef XSTYPES_DLL_API -#if XSENS_CONFIG < 3 // anything except full release builds -////////////////////////////////////////////////// -// stuff for debugging - -#else -// non-debug stuff -// -#endif - #include // since this is (almost) always required and it does not conflict with pstdint, include the file here @@ -325,23 +318,20 @@ The common way to setup configuration-dependent defines: #include "xstypedefs.h" #ifndef XSNOCOMEXPORT -#define XSNOCOMEXPORT + #define XSNOCOMEXPORT #endif -#define XSTRINGIFY2(s) #s -#define XSTRINGIFY(s) XSTRINGIFY2(s) - #ifdef XSENS_DEBUG -#define XSDEBUGLINE(a) a -#define XSNODEBUGLINE(...) + #define XSDEBUGLINE(a) a + #define XSNODEBUGLINE(...) #else -#define XSDEBUGLINE(...) -#define XSNODEBUGLINE(a) a + #define XSDEBUGLINE(...) + #define XSNODEBUGLINE(a) a #endif #if defined(_MSC_VER) && !defined(__cplusplus) && !defined(inline) -/* MSVC doesn't know the inline keyword in C mode, but it does know __inline */ -#define inline __inline + /* MSVC doesn't know the inline keyword in C mode, but it does know __inline */ + #define inline __inline #endif #endif diff --git a/lib/xspublic/xstypes/xstypesdynlib.c b/lib/xspublic/xstypes/xstypesdynlib.c index 0ca9086..585fdf5 100644 --- a/lib/xspublic/xstypes/xstypesdynlib.c +++ b/lib/xspublic/xstypes/xstypesdynlib.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -63,25 +63,25 @@ // #ifdef _WIN32 -# ifndef WINVER // Allow use of features specific to Windows XP or later. -# define WINVER 0x0501 // Change this to the appropriate value to target other versions of Windows. -# endif + #ifndef WINVER // Allow use of features specific to Windows XP or later. + #define WINVER 0x0501 // Change this to the appropriate value to target other versions of Windows. + #endif -# ifndef _WIN32_WINNT // Allow use of features specific to Windows XP or later. -# define _WIN32_WINNT 0x0501 // Change this to the appropriate value to target other versions of Windows. -# endif + #ifndef _WIN32_WINNT // Allow use of features specific to Windows XP or later. + #define _WIN32_WINNT 0x0501 // Change this to the appropriate value to target other versions of Windows. + #endif -# ifndef _WIN32_WINDOWS // Allow use of features specific to Windows 98 or later. -# define _WIN32_WINDOWS 0x0501 // Change this to the appropriate value to target Windows Me or later. -# endif + #ifndef _WIN32_WINDOWS // Allow use of features specific to Windows 98 or later. + #define _WIN32_WINDOWS 0x0501 // Change this to the appropriate value to target Windows Me or later. + #endif -# ifndef _WIN32_IE // Allow use of features specific to IE 6.0 or later. -# define _WIN32_IE 0x0600 // Change this to the appropriate value to target other versions of IE. -# endif -# ifndef WIN32_LEAN_AND_MEAN -# define WIN32_LEAN_AND_MEAN // Exclude rarely-used stuff from Windows headers -# endif -# include + #ifndef _WIN32_IE // Allow use of features specific to IE 6.0 or later. + #define _WIN32_IE 0x0600 // Change this to the appropriate value to target other versions of IE. + #endif + #ifndef WIN32_LEAN_AND_MEAN + #define WIN32_LEAN_AND_MEAN // Exclude rarely-used stuff from Windows headers + #endif + #include #else #endif #include "xstypesdynlib.h" @@ -89,14 +89,14 @@ #include "xsstring.h" #ifdef _MANAGED -#pragma managed(push, off) + #pragma managed(push, off) #endif // The so_* functions are also used by programs that use xstypes as a static library // When the function contents change, check if this functionality is also required in these programs void #ifdef __GNUC__ -__attribute__((constructor(65535))) + __attribute__((constructor(65535))) #endif so_init(void) { @@ -105,7 +105,7 @@ so_init(void) void #ifdef __GNUC__ -__attribute__((destructor)) + __attribute__((destructor)) #endif so_fini(void) { @@ -113,30 +113,30 @@ so_fini(void) #ifdef _WIN32 HMODULE g_hModule = 0; -BOOL APIENTRY DllMain( HMODULE hModule, - DWORD ul_reason_for_call, - LPVOID lpvReserved - ) +BOOL APIENTRY DllMain(HMODULE hModule, + DWORD ul_reason_for_call, + LPVOID lpvReserved +) { g_hModule = hModule; switch (ul_reason_for_call) { - case DLL_PROCESS_ATTACH: - so_init(); - break; - case DLL_THREAD_ATTACH: - break; - case DLL_THREAD_DETACH: - break; - case DLL_PROCESS_DETACH: - // The lpvReserved parameter to DllMain is NULL if the DLL is being unloaded because of a call to FreeLibrary, it's non NULL if the DLL is being unloaded due to process termination. - // in case of process termination, all the threads have already been destroyed and we'll just assume that windows will clean us up - if (lpvReserved == NULL) - so_fini(); - break; - default: - break; + case DLL_PROCESS_ATTACH: + so_init(); + break; + case DLL_THREAD_ATTACH: + break; + case DLL_THREAD_DETACH: + break; + case DLL_PROCESS_DETACH: + // The lpvReserved parameter to DllMain is NULL if the DLL is being unloaded because of a call to FreeLibrary, it's non NULL if the DLL is being unloaded due to process termination. + // in case of process termination, all the threads have already been destroyed and we'll just assume that windows will clean us up + if (lpvReserved == NULL) + so_fini(); + break; + default: + break; } return TRUE; } @@ -164,5 +164,5 @@ XSTYPES_DLL_API void xstypesPath(XsString* path) #endif } #ifdef _MANAGED -#pragma managed(pop) + #pragma managed(pop) #endif diff --git a/lib/xspublic/xstypes/xstypesdynlib.h b/lib/xspublic/xstypes/xstypesdynlib.h index 0f3c1f3..93b0a9a 100644 --- a/lib/xspublic/xstypes/xstypesdynlib.h +++ b/lib/xspublic/xstypes/xstypesdynlib.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xstypes/xstypesinfo.h b/lib/xspublic/xstypes/xstypesinfo.h index fa9a392..2c9d568 100644 --- a/lib/xspublic/xstypes/xstypesinfo.h +++ b/lib/xspublic/xstypes/xstypesinfo.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -70,8 +70,8 @@ struct XsVersion; #ifdef __cplusplus -#include "xsversion.h" -extern "C" + #include "xsversion.h" + extern "C" #endif XSTYPES_DLL_API void xsTypesVersion(struct XsVersion* version); #define XsTypesInfoGetVersion(a) xsTypesVersion(a) diff --git a/lib/xspublic/xstypes/xsushortvector.h b/lib/xspublic/xstypes/xsushortvector.h index 5820bb0..f65b372 100644 --- a/lib/xspublic/xstypes/xsushortvector.h +++ b/lib/xspublic/xstypes/xsushortvector.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -66,15 +66,16 @@ #define XSUSHORTVECTOR_H #ifdef __cplusplus -#include "xstypedefs.h" + #include "xstypedefs.h" #endif #ifndef __cplusplus -#define XSUSHORTVECTOR_INITIALIZER {{0,0,0}} + #define XSUSHORTVECTOR_INITIALIZER {{0,0,0}} #endif /*! \brief A vector containing 3 short values. */ -struct XsUShortVector { +struct XsUShortVector +{ #ifdef __cplusplus //! \brief Constructor that creates the vector with all components set to 0 inline XsUShortVector() @@ -124,8 +125,8 @@ struct XsUShortVector { inline bool operator == (const XsUShortVector& other) const { return m_data[0] == other.m_data[0] && - m_data[1] == other.m_data[1] && - m_data[2] == other.m_data[2]; + m_data[1] == other.m_data[1] && + m_data[2] == other.m_data[2]; } //! \brief Return the size of the vector (always 3) diff --git a/lib/xspublic/xstypes/xsutctime.c b/lib/xspublic/xstypes/xsutctime.c index d4e56ea..8f5b443 100644 --- a/lib/xspublic/xstypes/xsutctime.c +++ b/lib/xspublic/xstypes/xsutctime.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/lib/xspublic/xstypes/xsutctime.h b/lib/xspublic/xstypes/xsutctime.h index 6727a83..e679ccb 100644 --- a/lib/xspublic/xstypes/xsutctime.h +++ b/lib/xspublic/xstypes/xsutctime.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -68,8 +68,7 @@ #include "xstimeinfo.h" #ifdef __cplusplus -extern "C" -{ +extern "C" { #endif #ifndef __cplusplus #define XSUTCTIME_INITIALIZER { 0, 0 ,0, 0, 0, 0, 0, 0, 0} @@ -77,7 +76,7 @@ extern "C" // for backwards compatibility only, use XsTimeInfo instead #define XsUtcTime XsTimeInfo -XSTYPES_DLL_API void XsUtcTime_currentTime(struct XsUtcTime * now); +XSTYPES_DLL_API void XsUtcTime_currentTime(struct XsUtcTime* now); #ifdef __cplusplus } // extern "C" diff --git a/lib/xspublic/xstypes/xsvector.c b/lib/xspublic/xstypes/xsvector.c index c40ae42..42fccd7 100644 --- a/lib/xspublic/xstypes/xsvector.c +++ b/lib/xspublic/xstypes/xsvector.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -75,6 +75,10 @@ #define realSwap(a,b) { XsReal t = *a; *a = *b; *b = t; } +#ifdef __ICCARM__ + #pragma diag_suppress=Pa039 +#endif + /*! \class XsVector \brief A class that represents a vector of real numbers */ @@ -86,7 +90,7 @@ //! \relates XsVector \brief Initialize the %XsVector to refer to the supplied buffer void XsVector_ref(XsVector* thisPtr, XsSize sz, XsReal* buffer, XsDataFlags flags) { - assert(sz==0 || buffer != 0); + assert(sz == 0 || buffer != 0); *((XsReal**) &thisPtr->m_data) = buffer; *((XsSize*) &thisPtr->m_size) = sz; *((XsSize*) &thisPtr->m_flags) = flags; @@ -98,7 +102,7 @@ void XsVector_construct(XsVector* thisPtr, XsSize sz, const XsReal* src) if (sz) { // init to size - XsReal* data = (XsReal*) xsMathMalloc(sz*sizeof(XsReal)); + XsReal* data = (XsReal*) xsMathMalloc(sz * sizeof(XsReal)); assert(data); *((XsReal**) &thisPtr->m_data) = data; XsVector_incAllocCount(); @@ -108,7 +112,7 @@ void XsVector_construct(XsVector* thisPtr, XsSize sz, const XsReal* src) *((XsSize*) &thisPtr->m_flags) = XSDF_Managed; *((XsSize*) &thisPtr->m_size) = sz; if (src && sz) - memcpy(thisPtr->m_data, src, sz*sizeof(XsReal)); + memcpy(thisPtr->m_data, src, sz * sizeof(XsReal)); } /*! \relates XsVector @@ -118,7 +122,7 @@ void XsVector_construct(XsVector* thisPtr, XsSize sz, const XsReal* src) */ void XsVector_assign(XsVector* thisPtr, XsSize sz, const XsReal* src) { - if (thisPtr->m_flags == XSDF_FixedSize) + if (thisPtr->m_flags & XSDF_FixedSize) { if (sz == 0) { @@ -135,7 +139,7 @@ void XsVector_assign(XsVector* thisPtr, XsSize sz, const XsReal* src) if (sz) { // init to size - XsReal* data = (XsReal*) xsMathMalloc(sz*sizeof(XsReal)); + XsReal* data = (XsReal*) xsMathMalloc(sz * sizeof(XsReal)); assert(data); *((XsReal**) &thisPtr->m_data) = data; *((XsSize*) &thisPtr->m_flags) = XSDF_Managed; @@ -144,7 +148,7 @@ void XsVector_assign(XsVector* thisPtr, XsSize sz, const XsReal* src) } *((XsSize*) &thisPtr->m_size) = sz; if (src && sz) - memcpy(thisPtr->m_data, src, sz*sizeof(XsReal)); + memcpy(thisPtr->m_data, src, sz * sizeof(XsReal)); } //! \relates XsVector \brief Release and clear the contents of the vector @@ -188,8 +192,8 @@ XsReal XsVector_dotProduct(const XsVector* a, const XsVector* b) XsReal r = XsMath_zero; assert(a->m_size == b->m_size); - for (i = a->m_size; i--; ) - r += a->m_data[i]*b->m_data[i]; + for (i = a->m_size; i--;) + r += a->m_data[i] * b->m_data[i]; return r; } @@ -270,7 +274,7 @@ void XsVector_angularVelocityFromQuaternion(XsVector* thisPtr, XsReal deltaT, co XsVector_assign(thisPtr, 3, &quat->m_data[1]); a = XsVector_cartesianLength(thisPtr); - XsVector_multiplyScalar(thisPtr, (a > XsMath_tinyValue) ? (XsMath_two*asin(a)/(a*deltaT)) : (XsMath_two/deltaT), thisPtr); + XsVector_multiplyScalar(thisPtr, (a > XsMath_tinyValue) ? (XsMath_two * asin(a) / (a * deltaT)) : (XsMath_two / deltaT), thisPtr); } /*! \relates XsVector \brief Swap the contents of \a a and \a b @@ -282,7 +286,7 @@ void XsVector_angularVelocityFromQuaternion(XsVector* thisPtr, XsReal deltaT, co void XsVector_swap(XsVector* a, XsVector* b) { #ifdef __ICCARM__ - #pragma diag_suppress=Pe370 +#pragma diag_suppress=Pe370 #endif XSLISTSWAP3(XsReal, XsVector, realSwap) } @@ -301,9 +305,11 @@ int XsVector_equal(const struct XsVector* a, const struct XsVector* b) return 0; if (XsVector_empty(a) && XsVector_empty(b)) return 1; + if (XsVector_empty(a) || XsVector_empty(b)) + return 0; if (a->m_size != b->m_size) return 0; - return memcmp(a->m_data, b->m_data, a->m_size*sizeof(XsReal)) == 0; //lint !e2499 memcmp is a perfect comparison here + return memcmp(a->m_data, b->m_data, a->m_size * sizeof(XsReal)) == 0; //lint !e2499 memcmp is a perfect comparison here } /*! \relates XsVector @@ -323,6 +329,8 @@ int XsVector_compare(const struct XsVector* thisPtr, const struct XsVector* that return 0; if (XsVector_empty(thisPtr) && XsVector_empty(thatPtr)) return 1; + if (XsVector_empty(thisPtr) || XsVector_empty(thatPtr)) + return 0; if (thisPtr->m_size != thatPtr->m_size) return 0; diff --git a/lib/xspublic/xstypes/xsvector.h b/lib/xspublic/xstypes/xsvector.h index 4850f9a..571628c 100644 --- a/lib/xspublic/xstypes/xsvector.h +++ b/lib/xspublic/xstypes/xsvector.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -75,6 +75,11 @@ struct XsQuaternion; #ifdef __cplusplus #include #include +#if __cplusplus >= 201103L && !defined(XSENS_HAVE_TYPE_TRAITS) +#include +#define XSENS_HAVE_TYPE_TRAITS +#endif + extern "C" { #endif #ifndef __cplusplus @@ -103,17 +108,26 @@ XSTYPES_DLL_API int XsVector_compare(const XsVector* thisPtr, const XsVector* th } // extern "C" #endif #ifndef XSENS_NO_PACK -#pragma pack(push,1) + #pragma pack(push,1) #endif -struct XsVector { -XSCPPPROTECTED +struct XsVector +{ + XSCPPPROTECTED XsReal* const m_data; //!< \protected Points to contained data buffer const XsSize m_size; //!< \protected Size of contained data buffer in elements const XsSize m_flags; //!< \protected Flags for data management #ifdef __cplusplus - //! \brief Return the data management flags of the vector. - inline XsSize flags() const { return m_flags; } + +#ifdef __ICCARM__ +#pragma diag_suppress=Pa039 +#endif + + //! \brief Return the data management flags of the vector. + inline XsSize flags() const + { + return m_flags; + } public: //! \brief Initialize a vector, empty or using the data in the supplied \a sz and \a src inline explicit XsVector(XsSize sz = 0, const XsReal* src = 0) @@ -160,7 +174,7 @@ XSCPPPROTECTED XsVector_angularVelocityFromQuaternion(this, deltaT, &quat); } -#if !defined(SWIG) && !defined(__ADSP21000__) +#if !defined(SWIG) && !defined(__ADSP21000__) && !defined(__AVR32__) //! \brief Move-construct a vector using the supplied \a other vector inline XsVector(XsVector&& other) : m_data(0) @@ -258,6 +272,29 @@ XSCPPPROTECTED m_data[index] = val; } +#ifndef SWIG + //! \brief Returns the \a index'th item in the vector + template + inline XsReal operator[](J index) const + { +#ifdef XSENS_HAVE_TYPE_TRAITS + static_assert(std::is_integral::value || std::is_enum::value, "Integral index required."); +#endif + assert(static_cast(index) < m_size); + return m_data[index]; + } + + //! \brief Returns a reference the \a index'th item in the vector + template + inline XsReal& operator[](J index) + { +#ifdef XSENS_HAVE_TYPE_TRAITS + static_assert(std::is_integral::value || std::is_enum::value, "Integral index required."); +#endif + assert(static_cast(index) < m_size); + return m_data[index]; + } +#else //! \brief Returns the \a index'th item in the vector inline XsReal operator[](XsSize index) const { @@ -271,9 +308,10 @@ XSCPPPROTECTED assert(index < m_size); return m_data[index]; } +#endif //! \brief \copybrief XsVector_dotProduct - inline XsReal dotProduct(const XsVector &v) const + inline XsReal dotProduct(const XsVector& v) const { return XsVector_dotProduct(this, &v); } @@ -357,8 +395,8 @@ XSCPPPROTECTED \param other the vector to compare with \param epsilon the maximum difference between individual values \returns true if the vectors are equal within \a epsilon - */ - bool isEqual(const XsVector &other, XsReal epsilon) const + */ + bool isEqual(const XsVector& other, XsReal epsilon) const { return 0 != XsVector_compare(this, &other, epsilon); } @@ -397,9 +435,9 @@ XSCPPPROTECTED { XsVector_swap(this, &b); } - + /*! \brief swap the contents of \a first and \a second */ - friend void swap(XsVector& first, XsVector& second) + friend void swap(XsVector& first, XsVector& second) { first.swap(second); } @@ -414,7 +452,7 @@ XSCPPPROTECTED for (XsSize i = 0; i < size(); ++i) tmp[i] = (*this)[i]; for (XsSize i = 0; i < other.size(); ++i) - tmp[i+size()] = other[i]; + tmp[i + size()] = other[i]; swap(tmp); } @@ -434,20 +472,25 @@ XSCPPPROTECTED XsSize half = sz >> 1; --sz; for (XsSize i = 0; i < half; ++i) - swap(operator[](i), operator[](sz-i)); + swap(operator[](i), operator[](sz - i)); } #endif + +#ifdef __ICCARM__ +#pragma diag_default=Pa039 +#endif + #endif }; #ifndef XSENS_NO_PACK -#pragma pack(pop) + #pragma pack(pop) #endif #ifdef __cplusplus //! \brief Multiplies all values in the vector \a v by \a scalar -inline XsVector operator *(XsReal scalar, const XsVector &v) +inline XsVector operator *(XsReal scalar, const XsVector& v) { - return v*scalar; + return v * scalar; } #endif diff --git a/lib/xspublic/xstypes/xsvector3.c b/lib/xspublic/xstypes/xsvector3.c index 57ea328..b839f36 100644 --- a/lib/xspublic/xstypes/xsvector3.c +++ b/lib/xspublic/xstypes/xsvector3.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -78,14 +78,13 @@ void XsVector3_construct(XsVector3* thisPtr, const XsReal* src) { XsVector_ref(&thisPtr->m_vector, 3, (XsReal*) thisPtr->m_fixedData, XSDF_FixedSize); if (src) - memcpy((XsReal*) thisPtr->m_fixedData, src, 3*sizeof(XsReal)); + memcpy((XsReal*) thisPtr->m_fixedData, src, 3 * sizeof(XsReal)); } /*! \relates XsVector3 \brief Init the %XsVector3 and copy the data from \a src into the vector if \a src is not null */ void XsVector3_assign(XsVector3* thisPtr, const XsReal* src) { - if (src) - memcpy((XsReal*) thisPtr->m_fixedData, (XsReal*) src, 3*sizeof(XsReal)); + XsVector_assign(&thisPtr->m_vector, 3, src); } /*! \relates XsVector3 \brief Frees the XsVector3 */ diff --git a/lib/xspublic/xstypes/xsvector3.h b/lib/xspublic/xstypes/xsvector3.h index c042fc1..8bbb171 100644 --- a/lib/xspublic/xstypes/xsvector3.h +++ b/lib/xspublic/xstypes/xsvector3.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -85,13 +85,15 @@ XSTYPES_DLL_API void XsVector3_copy(XsVector* copy, XsVector3 const* src); #endif #ifdef __cplusplus -/* This is allowed since the C standard says that no padding appears before the first member of a struct. +/* This is allowed since the C standard says that no padding appears before the first member of a struct. Basically we're defining a union between a C++ inherited type and a C encapsulated type. */ -struct XsVector3 : public XsVector { -XSCPPPROTECTED +struct XsVector3 : public XsVector +{ + XSCPPPROTECTED #else -struct XsVector3 { +struct XsVector3 +{ XsVector m_vector; //!< The underlying vector #endif XsReal m_fixedData[3]; //!< Fixed size storage for the components in the vector @@ -144,6 +146,7 @@ struct XsVector3 { m_data[0] = x; m_data[1] = y; m_data[2] = z; + const_cast(m_flags) &= ~XSDF_Empty; } // using XsVector::operator[]; #endif diff --git a/lib/xspublic/xstypes/xsversion.c b/lib/xspublic/xstypes/xsversion.c index c546525..302bab4 100644 --- a/lib/xspublic/xstypes/xsversion.c +++ b/lib/xspublic/xstypes/xsversion.c @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -95,15 +95,15 @@ void XsVersion_toString(const XsVersion* thisPtr, XsString* version) if (thisPtr->m_extra.m_size != 0) { const char space = ' '; - XsArray_insert(version, version->m_size-1, 1, &space); + XsArray_insert(version, version->m_size - 1, 1, &space); XsString_append(version, &thisPtr->m_extra); } } /*! - * \relates XsVersion - * \brief Get a string with the version expressed in a readable format. - */ + \relates XsVersion + \brief Get a string with the version expressed in a readable format. +*/ void XsVersion_toSimpleString(const XsVersion* thisPtr, XsString* version) { char buffer[256]; @@ -112,9 +112,9 @@ void XsVersion_toSimpleString(const XsVersion* thisPtr, XsString* version) } /*! - * \relates XsVersion - * \brief Set the version to the values in the string - */ + \relates XsVersion + \brief Set the version to the values in the string +*/ void XsVersion_fromString(XsVersion* thisPtr, const XsString* version) { int major = 0; @@ -155,8 +155,8 @@ void XsVersion_fromString(XsVersion* thisPtr, const XsString* version) } /*! \relates XsVersion - * \brief Create a XsVersion a XsSimpleVersion, \a simpleVersion. - */ + \brief Create a XsVersion a XsSimpleVersion, \a simpleVersion. +*/ void XsVersion_fromSimpleVersion(XsVersion* thisPtr, const XsSimpleVersion* simpleVersion) { thisPtr->m_major = simpleVersion->m_major; @@ -168,8 +168,8 @@ void XsVersion_fromSimpleVersion(XsVersion* thisPtr, const XsSimpleVersion* simp } /*! \relates XsVersion - * \brief Create a XsSimpleVersion (\a version) from a XsVersion. - */ + \brief Create a XsSimpleVersion (\a version) from a XsVersion. +*/ void XsVersion_toSimpleVersion(const XsVersion* thisPtr, XsSimpleVersion* simpleVersion) { simpleVersion->m_major = (uint8_t)(int8_t)thisPtr->m_major; diff --git a/lib/xspublic/xstypes/xsversion.h b/lib/xspublic/xstypes/xsversion.h index 21ccbfc..3a0a415 100644 --- a/lib/xspublic/xstypes/xsversion.h +++ b/lib/xspublic/xstypes/xsversion.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -92,7 +92,8 @@ XSTYPES_DLL_API void XsVersion_toSimpleString(const XsVersion* thisPtr, XsString } // extern "C" #endif -struct XsVersion { +struct XsVersion +{ #ifdef __cplusplus //! \brief Constructs a version object using the supplied parameters or an empty version object if no parameters are given. explicit XsVersion(int maj, int min, int rev, int build, const XsString& extra) @@ -121,7 +122,7 @@ struct XsVersion { } //! \brief Constructs a version object based upon the version contained by \a simpleVersion. - explicit XsVersion(const XsSimpleVersion& simpleVersion) + XsVersion(const XsSimpleVersion& simpleVersion) { XsVersion_fromSimpleVersion(this, &simpleVersion); } @@ -149,7 +150,7 @@ struct XsVersion { } /*! \brief Test if the \a other version is equal to this. The comparison involves the entire object.*/ - inline bool isEqual (const XsVersion& other) const + inline bool isEqual(const XsVersion& other) const { return (*this == other) && (m_build == other.m_build) && (m_extra == other.m_extra) && (m_reposVersion == other.m_reposVersion); } @@ -157,16 +158,13 @@ struct XsVersion { /*! \brief Test if the \a other version is equal to this. The comparison involves only the version numbers (major, minor and revision). */ inline bool operator == (const XsVersion& other) const { - if (m_major == other.m_major && m_minor == other.m_minor && m_revision == other.m_revision) - return true; - - return false; + return m_major == other.m_major && m_minor == other.m_minor && m_revision == other.m_revision; } /*! \brief Test if the \a other version is NOT equal to this. The comparison involves only the version numbers (major, minor and revision). */ inline bool operator != (const XsVersion& other) const { - return !(*this == other); + return m_major != other.m_major || m_minor != other.m_minor || m_revision != other.m_revision; } /*! \brief Test if the \a other version is lower than this. The comparison involves only the version numbers (major, minor and revision). */ @@ -176,26 +174,35 @@ struct XsVersion { return true; else if (m_major > other.m_major) return false; + + if (m_minor < other.m_minor) + return true; + else if (m_minor > other.m_minor) + return false; + + if (m_revision < other.m_revision) + return true; else - { - if (m_minor < other.m_minor) - return true; - else if (m_minor > other.m_minor) - return false; - else - { - if (m_revision < other.m_revision) - return true; - else - return false; - } - } + return false; } /*! \brief Test if the \a other version is lower or equal than this. The comparison involves only the version numbers (major, minor and revision). */ inline bool operator <= (const XsVersion& other) const { - return (*this == other) || (*this < other); + if (m_major < other.m_major) + return true; + else if (m_major > other.m_major) + return false; + + if (m_minor < other.m_minor) + return true; + else if (m_minor > other.m_minor) + return false; + + if (m_revision < other.m_revision) + return true; + else + return m_revision == other.m_revision; } /*! \brief Test if the \a other version is higher than this. The comparison involves only the version numbers (major, minor and revision). */ @@ -207,7 +214,7 @@ struct XsVersion { /*! \brief Test if the \a other version is higher or equal than this. The comparison involves only the version numbers (major, minor and revision). */ inline bool operator >= (const XsVersion& other) const { - return (*this == other) || (*this > other); + return !(*this < other); } //! \brief \copybrief XsVersion_empty @@ -241,30 +248,66 @@ struct XsVersion { } //! \brief Return the \e major part of the version - inline int major() const { return m_major; } + inline int major() const + { + return m_major; + } //! \brief Return the \e minor part of the version - inline int minor() const { return m_minor; } + inline int minor() const + { + return m_minor; + } //! \brief Return the \e revision part of the version - inline int revision() const { return m_revision; } + inline int revision() const + { + return m_revision; + } //! \brief Return the \e build number used for this build - inline int build() const { return m_build; } + inline int build() const + { + return m_build; + } //! \brief Return the \e source revision used for this build - inline int reposVersion() const { return m_reposVersion;} + inline int reposVersion() const + { + return m_reposVersion; + } //! \brief Return the extra part of the version. This may contain custom version details such as 'beta' or 'Mk4' to indicate the readiness and purpose of this version of the object. - inline const XsString& extra() const { return m_extra; } + inline const XsString& extra() const + { + return m_extra; + } //! \brief Set the \e major part of the version - inline void setMajor(int major) { m_major = major; } + inline void setMajor(int major) + { + m_major = major; + } //! \brief Set the \e minor part of the version - inline void setMinor(int minor) { m_minor = minor; } + inline void setMinor(int minor) + { + m_minor = minor; + } //! \brief Set the \e revision part of the version - inline void setRevision(int revision) { m_revision = revision; } + inline void setRevision(int revision) + { + m_revision = revision; + } //! \brief Set the \e build part of the version - inline void setBuild(int build) { m_build = build; } + inline void setBuild(int build) + { + m_build = build; + } //! \brief Set the \e reposVersion part of the version - inline void setReposVersion(int reposVersion) { m_reposVersion = reposVersion; } + inline void setReposVersion(int reposVersion) + { + m_reposVersion = reposVersion; + } //! \brief Set the \e extra part of the version. This may contain custom version details such as 'beta' or 'Mk4' to indicate the readiness and purpose of this version of the object. - inline void setExtra(const XsString& extra) { m_extra = extra; } + inline void setExtra(const XsString& extra) + { + m_extra = extra; + } private: #endif diff --git a/lib/xspublic/xstypes/xsxbusmessageid.h b/lib/xspublic/xstypes/xsxbusmessageid.h index 2301ef4..929cbbb 100644 --- a/lib/xspublic/xstypes/xsxbusmessageid.h +++ b/lib/xspublic/xstypes/xsxbusmessageid.h @@ -1,5 +1,5 @@ -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -31,7 +31,7 @@ // -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, @@ -70,7 +70,8 @@ */ /*! \brief Xsens Xbus Message Identifiers */ -enum XsXbusMessageId { +enum XsXbusMessageId +{ XMID_InvalidMessage = 0x00, // Config state messages @@ -141,10 +142,10 @@ enum XsXbusMessageId { XMID_SetOutputTrigger = 0x28, XMID_SetOutputTriggerAck = 0x29, - XMID_SetSyncStationMode = 0x2A, //!< Request the current sync station mode - XMID_SetSyncStationModeAck = 0x2B, //!< Message contains the current sync station mode - XMID_ReqSyncStationMode = 0x2A, //!< Set the current sync station mode - XMID_ReqSyncStationModeAck = 0x2B, //!< Acknowledge of setting the current sync station mode + XMID_SetSyncStationMode = 0x2A, //!< Request the current sync station mode + XMID_SetSyncStationModeAck = 0x2B, //!< Message contains the current sync station mode + XMID_ReqSyncStationMode = 0x2A, //!< Set the current sync station mode + XMID_ReqSyncStationModeAck = 0x2B, //!< Acknowledge of setting the current sync station mode XMID_SetSyncConfiguration = 0x2C, XMID_SetSyncConfigurationAck = 0x2D, @@ -191,6 +192,7 @@ enum XsXbusMessageId { // Wireless XMID_MasterIndication = 0x46, + XMID_XsbData = 0x47, // optional meta-data XMID_ReqOptionFlags = 0x48, XMID_ReqOptionFlagsAck = 0x49, @@ -238,13 +240,13 @@ enum XsXbusMessageId { XMID_FactoryTestSensorTiming = 0x60, XMID_FactoryTestSensorTimingResults = 0x61, - XMID_ReqAvailableFilterProfiles = 0x62, //!< Request the available filter profiles - XMID_AvailableFilterProfiles = 0x63, //!< Message contains the available filter profiles + XMID_ReqAvailableFilterProfiles = 0x62, //!< Request the available filter profiles + XMID_AvailableFilterProfiles = 0x63, //!< Message contains the available filter profiles - XMID_ReqFilterProfile = 0x64, //!< Request the current filter profile - XMID_ReqFilterProfileAck = 0x65, //!< Message contains the current filter profile - XMID_SetFilterProfile = 0x64, //!< Set the current filter profile - XMID_SetFilterProfileAck = 0x65, //!< Acknowledge of setting the current filter profile + XMID_ReqFilterProfile = 0x64, //!< Request the current filter profile + XMID_ReqFilterProfileAck = 0x65, //!< Message contains the current filter profile + XMID_SetFilterProfile = 0x64, //!< Set the current filter profile + XMID_SetFilterProfileAck = 0x65, //!< Acknowledge of setting the current filter profile XMID_ReqGravityMagnitude = 0x66, XMID_ReqGravityMagnitudeAck = 0x67, @@ -338,20 +340,20 @@ enum XsXbusMessageId { XMID_StoreFilterState = 0x8A, XMID_StoreFilterStateAck = 0x8B, - XMID_ReqPortConfig = 0x8C, - XMID_SetPortConfig = 0x8C, - XMID_PortConfig = 0x8D, - XMID_SetPortConfigAck = 0x8D, + XMID_ReqPortConfig = 0x8C, + XMID_SetPortConfig = 0x8C, + XMID_PortConfig = 0x8D, + XMID_SetPortConfigAck = 0x8D, XMID_ReqStringOutputType = 0x8E, XMID_ReqStringOutputTypeAck = 0x8F, XMID_SetStringOutputType = 0x8E, XMID_SetStringOutputTypeAck = 0x8F, - XMID_ReqStringOutputConfig = 0x8E, - XMID_ReqStringOutputConfigAck = 0x8F, - XMID_SetStringOutputConfig = 0x8E, - XMID_SetStringOutputConfigAck = 0x8F, + XMID_ReqStringOutputConfig = 0x8E, + XMID_ReqStringOutputConfigAck = 0x8F, + XMID_SetStringOutputConfig = 0x8E, + XMID_SetStringOutputConfigAck = 0x8F, XMID_ReqEmts = 0x90, XMID_EmtsData = 0x91, @@ -363,8 +365,11 @@ enum XsXbusMessageId { XMID_StoreEmts = 0x96, XMID_StoreEmtsAck = 0x97, - XMID_ClockSyncCommand = 0x9A, - XMID_ClockSyncCommandAck = 0x9B, + XMID_UnlockMaintenanceMode = 0x98, + XMID_UnlockMaintenanceModeAck = 0x99, + + XMID_ClockSyncCommand = 0x9A, + XMID_ClockSyncCommandAck = 0x9B, XMID_ReqActiveClockCorrection = 0x9C, XMID_ActiveClockCorrection = 0x9D, @@ -388,15 +393,15 @@ enum XsXbusMessageId { XMID_AdjustUtcTime = 0xA8, XMID_AdjustUtcTimeAck = 0xA9, - XMID_ReqManufacturerId = 0xAA, - XMID_SetManufacturerId = 0xAA, - XMID_ManufacturerId = 0xAB, - XMID_SetManufacturerIdAck = 0xAB, + XMID_ReqAssemblyId = 0xAA, + XMID_SetAssemblyId = 0xAA, + XMID_AssemblyId = 0xAB, + XMID_SetAssemblyIdAck = 0xAB, - XMID_ReqGnssReceiverSettings = 0xAC, - XMID_ReqGnssReceiverSettingsAck = 0xAD, - XMID_SetGnssReceiverSettings = 0xAC, - XMID_SetGnssReceiverSettingsAck = 0xAD, + XMID_ReqGnssReceiverSettings = 0xAC, + XMID_ReqGnssReceiverSettingsAck = 0xAD, + XMID_SetGnssReceiverSettings = 0xAC, + XMID_SetGnssReceiverSettingsAck = 0xAD, XMID_ReqAccessControlList = 0xAE, XMID_AccessControlList = 0xAF, @@ -452,7 +457,7 @@ enum XsXbusMessageId { XMID_ReqOutputConfiguration = 0xC0, XMID_ReqOutputConfigurationAck = 0xC1, XMID_SetOutputConfiguration = 0xC0, - XMID_SetOutputConfigurationAck = 0xC1, + XMID_SetOutputConfigurationAck = 0xC1, XMID_ReqOutputMode = 0xD0, XMID_ReqOutputModeAck = 0xD1, @@ -469,8 +474,8 @@ enum XsXbusMessageId { XMID_SetOutputSkipFactor = 0xD4, XMID_SetOutputSkipFactorAck = 0xD5, - XMID_SetInitialHeading = 0xD6, - XMID_SetInitialHeadingAck = 0xD7, + XMID_SetInitialHeading = 0xD6, + XMID_SetInitialHeadingAck = 0xD7, XMID_ReqErrorMode = 0xDA, XMID_ReqErrorModeAck = 0xDB, @@ -490,17 +495,17 @@ enum XsXbusMessageId { XMID_SetObjectAlignment = 0xE0, XMID_SetObjectAlignmentAck = 0xE1, - XMID_ForwardGnssData = 0xE2, - XMID_ForwardGnssDataAck = 0xE3, - - XMID_ReqCanConfig = 0xE6, - XMID_SetCanConfig = 0xE6, - XMID_CanConfig = 0xE7, - XMID_SetCanConfigAck = 0xE7, - XMID_ReqCanOutputConfig = 0xE8, - XMID_SetCanOutputConfig = 0xE8, - XMID_CanOutputConfig = 0xE9, - XMID_SetCanOutputConfigAck = 0xE9, + XMID_ForwardGnssData = 0xE2, + XMID_ForwardGnssDataAck = 0xE3, + + XMID_ReqCanConfig = 0xE6, + XMID_SetCanConfig = 0xE6, + XMID_CanConfig = 0xE7, + XMID_SetCanConfigAck = 0xE7, + XMID_ReqCanOutputConfig = 0xE8, + XMID_SetCanOutputConfig = 0xE8, + XMID_CanOutputConfig = 0xE9, + XMID_SetCanOutputConfigAck = 0xE9, XMID_ReqAlignmentRotation = 0xEC, XMID_ReqAlignmentRotationAck = 0xED, diff --git a/src/main.cpp b/src/main.cpp index c120334..95e9882 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -28,7 +28,7 @@ // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/src/messagepublishers/accelerationpublisher.h b/src/messagepublishers/accelerationpublisher.h index c958f27..c58b17b 100644 --- a/src/messagepublishers/accelerationpublisher.h +++ b/src/messagepublishers/accelerationpublisher.h @@ -28,7 +28,7 @@ // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/src/messagepublishers/angularvelocitypublisher.h b/src/messagepublishers/angularvelocitypublisher.h index 9ce0c31..da4401d 100644 --- a/src/messagepublishers/angularvelocitypublisher.h +++ b/src/messagepublishers/angularvelocitypublisher.h @@ -28,7 +28,7 @@ // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/src/messagepublishers/freeaccelerationpublisher.h b/src/messagepublishers/freeaccelerationpublisher.h index 5b1a860..c35cb9a 100644 --- a/src/messagepublishers/freeaccelerationpublisher.h +++ b/src/messagepublishers/freeaccelerationpublisher.h @@ -28,7 +28,7 @@ // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/src/messagepublishers/gnsspublisher.h b/src/messagepublishers/gnsspublisher.h index 4ab6cda..2c5c728 100644 --- a/src/messagepublishers/gnsspublisher.h +++ b/src/messagepublishers/gnsspublisher.h @@ -28,7 +28,7 @@ // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/src/messagepublishers/imupublisher.h b/src/messagepublishers/imupublisher.h index 2576f34..a5177a0 100644 --- a/src/messagepublishers/imupublisher.h +++ b/src/messagepublishers/imupublisher.h @@ -28,7 +28,7 @@ // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/src/messagepublishers/magneticfieldpublisher.h b/src/messagepublishers/magneticfieldpublisher.h index 1d2b6d3..15289be 100644 --- a/src/messagepublishers/magneticfieldpublisher.h +++ b/src/messagepublishers/magneticfieldpublisher.h @@ -28,7 +28,7 @@ // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/src/messagepublishers/orientationincrementspublisher.h b/src/messagepublishers/orientationincrementspublisher.h index fb5aebb..b78c42d 100644 --- a/src/messagepublishers/orientationincrementspublisher.h +++ b/src/messagepublishers/orientationincrementspublisher.h @@ -28,7 +28,7 @@ // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/src/messagepublishers/orientationpublisher.h b/src/messagepublishers/orientationpublisher.h index 0f2c360..24aafe1 100644 --- a/src/messagepublishers/orientationpublisher.h +++ b/src/messagepublishers/orientationpublisher.h @@ -28,7 +28,7 @@ // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/src/messagepublishers/packetcallback.h b/src/messagepublishers/packetcallback.h index f32db3f..28da27a 100644 --- a/src/messagepublishers/packetcallback.h +++ b/src/messagepublishers/packetcallback.h @@ -28,7 +28,7 @@ // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/src/messagepublishers/positionllapublisher.h b/src/messagepublishers/positionllapublisher.h index 320471a..dd7c02d 100644 --- a/src/messagepublishers/positionllapublisher.h +++ b/src/messagepublishers/positionllapublisher.h @@ -32,7 +32,7 @@ // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/src/messagepublishers/pressurepublisher.h b/src/messagepublishers/pressurepublisher.h index ce23cf7..bf4f2e0 100644 --- a/src/messagepublishers/pressurepublisher.h +++ b/src/messagepublishers/pressurepublisher.h @@ -28,7 +28,7 @@ // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/src/messagepublishers/temperaturepublisher.h b/src/messagepublishers/temperaturepublisher.h index 9f1ed6a..4c8cbc2 100644 --- a/src/messagepublishers/temperaturepublisher.h +++ b/src/messagepublishers/temperaturepublisher.h @@ -28,7 +28,7 @@ // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/src/messagepublishers/timereferencepublisher.h b/src/messagepublishers/timereferencepublisher.h index 5ffb701..4354849 100644 --- a/src/messagepublishers/timereferencepublisher.h +++ b/src/messagepublishers/timereferencepublisher.h @@ -28,7 +28,7 @@ // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/src/messagepublishers/transformpublisher.h b/src/messagepublishers/transformpublisher.h index e4189fd..ef51da1 100644 --- a/src/messagepublishers/transformpublisher.h +++ b/src/messagepublishers/transformpublisher.h @@ -28,7 +28,7 @@ // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/src/messagepublishers/twistpublisher.h b/src/messagepublishers/twistpublisher.h index f01100f..aeb5926 100644 --- a/src/messagepublishers/twistpublisher.h +++ b/src/messagepublishers/twistpublisher.h @@ -28,7 +28,7 @@ // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/src/messagepublishers/velocityincrementpublisher.h b/src/messagepublishers/velocityincrementpublisher.h index c0fc632..3f7aefa 100644 --- a/src/messagepublishers/velocityincrementpublisher.h +++ b/src/messagepublishers/velocityincrementpublisher.h @@ -28,7 +28,7 @@ // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/src/messagepublishers/velocitypublisher.h b/src/messagepublishers/velocitypublisher.h index 60e6c25..546160b 100644 --- a/src/messagepublishers/velocitypublisher.h +++ b/src/messagepublishers/velocitypublisher.h @@ -28,7 +28,7 @@ // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/src/xdacallback.cpp b/src/xdacallback.cpp index 87a918a..fdb87df 100644 --- a/src/xdacallback.cpp +++ b/src/xdacallback.cpp @@ -28,7 +28,7 @@ // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/src/xdacallback.h b/src/xdacallback.h index 73e9fb7..e3d5253 100644 --- a/src/xdacallback.h +++ b/src/xdacallback.h @@ -28,7 +28,7 @@ // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/src/xdainterface.cpp b/src/xdainterface.cpp index b4095a2..9a44f16 100644 --- a/src/xdainterface.cpp +++ b/src/xdainterface.cpp @@ -28,7 +28,7 @@ // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/src/xdainterface.h b/src/xdainterface.h index 238f998..81ec639 100644 --- a/src/xdainterface.h +++ b/src/xdainterface.h @@ -28,7 +28,7 @@ // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -// Copyright (c) 2003-2020 Xsens Technologies B.V. or subsidiaries worldwide. +// Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide. // All rights reserved. // // Redistribution and use in source and binary forms, with or without modification, diff --git a/urdf/MTi_1.urdf b/urdf/MTi_1.urdf index 735c50b..5db762a 100644 --- a/urdf/MTi_1.urdf +++ b/urdf/MTi_1.urdf @@ -4,7 +4,7 @@ - + diff --git a/urdf/MTi_10.urdf b/urdf/MTi_10.urdf index fa000fc..c44dd33 100644 --- a/urdf/MTi_10.urdf +++ b/urdf/MTi_10.urdf @@ -4,7 +4,7 @@ - + diff --git a/urdf/MTi_6xx.urdf b/urdf/MTi_6xx.urdf index 1221b9f..7a66003 100644 --- a/urdf/MTi_6xx.urdf +++ b/urdf/MTi_6xx.urdf @@ -4,7 +4,7 @@ - +