diff --git a/.gitignore b/.gitignore index e15fe6b..dba84ce 100644 --- a/.gitignore +++ b/.gitignore @@ -38,3 +38,7 @@ build*/ # Temporary file generated by the mtsdk make *.dpp Makefile + +# Clangd files +**/.cache +**/compile_commands.json diff --git a/CHANGELOG.md b/CHANGELOG.md index 1ac94ce..4f21ee0 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,14 +6,15 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ## [Unreleased] -## [0.2.2] - 2022-11-17 +Update Extern library to the latest XDA (2022.0.0) (https://github.com/robotology/yarp-device-xsensmt/pull/38). - Fix compatibility with YARP 3.8 (https://github.com/robotology/yarp-device-xsensmt/pull/36). +## [0.2.2] - 2022-11-17 +Fix compatibility with YARP 3.8 (https://github.com/robotology/yarp-device-xsensmt/pull/36). ## [0.2.1] - 2022-09-28 - Add parameter period to set the output frequency of XSens device (https://github.com/robotology/yarp-device-xsensmt/pull/34)/ +Add parameter period to set the output frequency of XSens device (https://github.com/robotology/yarp-device-xsensmt/pull/34)/ ## [0.2.0] - 2022-05-30 diff --git a/extern/CMakeLists.txt b/extern/CMakeLists.txt index c90cb28..9006bd0 100644 --- a/extern/CMakeLists.txt +++ b/extern/CMakeLists.txt @@ -1,19 +1,41 @@ -# Copyright (C) 2017-2018 iCub Facility, Istituto Italiano di Tecnologia +# Copyright (C) 2017-2023 Fondazione Istituto Italiano di Tecnologia # Authors: Silvio Traversaro # CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LICENSE -# The code contained in this directory is imported directly from the src_cpp example -# in the MT Software Suite, version 4.8.2, available from https://www.xsens.com/mt-software-suite/ +# The code contained in this directory is imported directly from the src_cpp example +# in the MT Software Suite, version 2022.0.0, available from https://www.movella.com/support/software-documentation/ # If you want to update the code to a new version of the MT Software Suite, run make in the mtsdk -# example directory and copy the relevant files from the src_example in this directory. +# example directory and copy the xspublic files from the public directory in this directory. +# Furthermore, you should update the CMake scripts options by inspecting the xspublic's Makefiles -# Globbing should be avoided in general, but this is a library that we use directly as provided by Xsens -file(GLOB xcommunication_SRCS ${CMAKE_CURRENT_SOURCE_DIR}/xcommunication/*.cpp ${CMAKE_CURRENT_SOURCE_DIR}/xcommunication/*.c) -file(GLOB xstypes_SRCS ${CMAKE_CURRENT_SOURCE_DIR}/xstypes/*.cpp ${CMAKE_CURRENT_SOURCE_DIR}/xstypes/*.c) -add_library(xsens_mt_software_suite STATIC deviceclass.cpp ${xcommunication_SRCS} ${xstypes_SRCS}) -target_include_directories(xsens_mt_software_suite PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} - ${CMAKE_CURRENT_SOURCE_DIR}/include - ${CMAKE_CURRENT_SOURCE_DIR}/include/xcommunication - ${CMAKE_CURRENT_SOURCE_DIR}/include/xsens) find_package (Threads) -target_link_libraries(xsens_mt_software_suite PUBLIC ${CMAKE_THREAD_LIBS_INIT} ${CMAKE_DL_LIBS}) + +# Globbing should be avoided in general, but this is a library that we use directly as provided by Xsens +file(GLOB xstypes_SRCS ${CMAKE_CURRENT_SOURCE_DIR}/xspublic/xstypes/*.cpp ${CMAKE_CURRENT_SOURCE_DIR}/xspublic/xstypes/*.c) +# See EXCLUDE argument in xstypes' Makefile +list(FILTER xstypes_SRCS EXCLUDE REGEX "xstypesdynlib") +add_library(xstypes STATIC ${xstypes_SRCS}) +target_include_directories(xstypes PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/xspublic) +target_include_directories(xstypes PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/xspublic/xstypes) +target_link_libraries(xstypes PUBLIC ${CMAKE_THREAD_LIBS_INIT} ${CMAKE_DL_LIBS}) + + +file(GLOB xscommon_SRCS ${CMAKE_CURRENT_SOURCE_DIR}/xspublic/xscommon/*.cpp ${CMAKE_CURRENT_SOURCE_DIR}/xspublic/xscommon/*.c) +add_library(xscommon STATIC ${xscommon_SRCS}) +target_include_directories(xscommon PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/xspublic) +target_include_directories(xscommon PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/xspublic/xscommon) +target_compile_options(xscommon PRIVATE "SHELL:-include xscommon_config.h" "SHELL:-include xstypes/xsens_compat.h") +target_link_libraries(xscommon PUBLIC xstypes) + + +file(GLOB xscontroller_SRCS ${CMAKE_CURRENT_SOURCE_DIR}/xspublic/xscontroller/*.cpp ${CMAKE_CURRENT_SOURCE_DIR}/xspublic/xscontroller/*.c) +add_library(xscontroller STATIC ${xscontroller_SRCS}) +target_include_directories(xscontroller PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/xspublic) +target_include_directories(xscontroller PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/xspublic/xscontroller) +target_compile_options(xscontroller PRIVATE "SHELL:-include xscontrollerconfig.h") +target_compile_definitions(xscontroller PRIVATE -DHAVE_JOURNALLER) +target_link_libraries(xscontroller PUBLIC xstypes xscommon) + + +add_library(xsens_mt_software_suite INTERFACE) +target_link_libraries(xsens_mt_software_suite INTERFACE xstypes xscommon xscontroller) diff --git a/extern/deviceclass.cpp b/extern/deviceclass.cpp deleted file mode 100644 index 05a35a4..0000000 --- a/extern/deviceclass.cpp +++ /dev/null @@ -1,243 +0,0 @@ -/* Copyright (c) 2003-2017 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. -*/ - -#include "deviceclass.h" -#include -#include -#include -#include - -DeviceClass::DeviceClass(void) - : m_streamInterface {} - , m_protocolManager { new SimpleProtocolManager } -{ - m_messageExtractor.reset(new MessageExtractor(m_protocolManager)); -} - -DeviceClass::~DeviceClass(void) -{ -} - -/*! \brief Open an IO device - \param portInfo The info to use for opening the port - \return True when successful -*/ -bool DeviceClass::openPort(const XsPortInfo& portInfo) -{ - if (portInfo.isUsb()) - m_streamInterface.reset(new UsbInterface); - else - m_streamInterface.reset(new SerialInterface); - - if (m_streamInterface->open(portInfo) != XRV_OK) - return false; - return true; -} - -/*! \brief Close an IO device -*/ -void DeviceClass::close() -{ - m_streamInterface->close(); -} - -/*! \brief Read available data from the open IO device - \details This function will attempt to read all available data from the open device (COM port - or USB port). - The function will read from the device, but it won't wait for data to become available. - \param raw A XsByteArray to where the read data will be stored. - \return Whether data has been read from the IO device -*/ -XsResultValue DeviceClass::readDataToBuffer(XsByteArray& raw) -{ - // always read data and append it to the cache before doing analysis - const int maxSz = 8192; - XsResultValue res = m_streamInterface->readData(maxSz, raw); - if (raw.size()) - return XRV_OK; - - return res; -} - -/*! \brief Read all messages from the buffered read data after adding new data supplied in \a rawIn - \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. - \param rawIn The buffered data in which to search for messages - \param messages The messages found in the data - \return The messages that were read. -*/ -XsResultValue DeviceClass::processBufferedData(XsByteArray& rawIn, std::deque& messages) -{ - return m_messageExtractor->processNewData(rawIn, messages); -} - -/*! \brief Wait for the requested XsXbusMessageId - \param xmid The message id to wait for - \param rcv The received message - \return Whether the requested message was found -*/ -bool DeviceClass::waitForMessage(XsXbusMessageId xmid, XsMessage& rcv) -{ - XsByteArray data; - std::deque msgs; - bool foundAck = false; - do - { - readDataToBuffer(data); - processBufferedData(data, msgs); - for (auto it = msgs.begin(); it != msgs.end(); ++it) - if ((*it).getMessageId() == xmid) - { - foundAck = true; - rcv = *it; - } - } while (!foundAck); - return foundAck; -} - -/*! \brief Write a message to the IO device - \param msg The message to write - \return Whether the message could be written -*/ -bool DeviceClass::writeMessage(const XsMessage& msg) -{ - XsByteArray raw; - if (ProtocolHandler::composeMessage(raw, msg) < 0) - return false; - - return (m_streamInterface->writeData(raw) == XRV_OK); -} - -/*! \brief Put a device in config mode - \return True when the device acknowledged config mode -*/ -bool DeviceClass::gotoConfig() -{ - XsMessage snd(XMID_GotoConfig, 0), rcv; - writeMessage(snd); - - return waitForMessage(XMID_GotoConfigAck, rcv); -} - -/*! \brief Put a device in measurement mode - \return True when the device acknowledged measurement mode -*/ -bool DeviceClass::gotoMeasurement() -{ - XsMessage snd(XMID_GotoMeasurement, 0), rcv; - writeMessage(snd); - - return waitForMessage(XMID_GotoMeasurementAck, rcv); -} - -/*! \brief Request the product code from a device - \return The product code when ok, otherwise an empty XsString -*/ -XsString DeviceClass::getProductCode() -{ - XsMessage snd(XMID_ReqProductCode, 0), rcv; - writeMessage(snd); - - if (waitForMessage(XMID_ProductCode,rcv)) - { - const char* pc = (const char*) rcv.getDataBuffer(0); - std::string result(pc?pc:"", rcv.getDataSize()); - std::string::size_type thingy = result.find(" "); - if (thingy < 20) - result.erase(result.begin() + thingy, result.end()); //lint !e534 - return XsString(result); - } - else - return XsString(); -} - -/*! \brief Request the device id from a device - \return The device id (XsDeviceId) when ok, otherwise an empty XsDeviceId -*/ -XsDeviceId DeviceClass::getDeviceId() -{ - XsMessage snd(XMID_ReqDid, 0), rcv; - writeMessage(snd); - - if (waitForMessage(XMID_DeviceId,rcv)) - { - return rcv.getDataLong(); - } - else - return XsDeviceId(); -} - -/*! \brief Set the device mode of a device (outputmode and outputsettings) - \param outputMode The XsOutputMode to set - \param outputSettings The XsOutputSettings to set - \return True when successful -*/ -bool DeviceClass::setDeviceMode(const XsOutputMode& outputMode, const XsOutputSettings& outputSettings) -{ - XsMessage sndOM(XMID_SetOutputMode), sndOS(XMID_SetOutputSettings), rcv; - - sndOM.resizeData(2); - sndOM.setDataShort((uint16_t) outputMode); - writeMessage(sndOM); - if (!waitForMessage(XMID_SetOutputModeAck, rcv)) - return false; - - XsMessage snd(XMID_SetOutputSettings); - snd.resizeData(4); - snd.setDataLong((uint32_t)outputSettings); - writeMessage(sndOS); - if (!waitForMessage(XMID_SetOutputSettingsAck, rcv)) - return false; - - return true; -} - -/*! \brief Set the output configuration of a device - \param config An array XsOutputConfigurationArray) containing the one or multiple XsOutputConfigurations - \return True when successful -*/ -bool DeviceClass::setOutputConfiguration(XsOutputConfigurationArray& config) -{ - XsMessage snd(XMID_SetOutputConfiguration, 4), rcv; - if (config.size() == 0) - { - snd.setDataShort((uint16_t)XDI_None, 0); - snd.setDataShort(0, 2); - } - else - { - for (XsSize i = 0; i < (XsSize) config.size(); ++i) - { - snd.setDataShort((uint16_t)config[i].m_dataIdentifier, i*4); - snd.setDataShort(config[i].m_frequency, i*4+2); - } - } - writeMessage(snd); - - return waitForMessage(XMID_SetOutputConfigurationAck, rcv); -} diff --git a/extern/deviceclass.h b/extern/deviceclass.h deleted file mode 100644 index 077765f..0000000 --- a/extern/deviceclass.h +++ /dev/null @@ -1,73 +0,0 @@ -/* Copyright (c) 2003-2017 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. -*/ - -#ifndef DEVICECLASS_H -#define DEVICECLASS_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -class StreamInterface; - -class DeviceClass -{ -public: - DeviceClass(void); - ~DeviceClass(void); - - bool openPort(const XsPortInfo& portInfo); - void close(); - - XsResultValue readDataToBuffer(XsByteArray& raw); - XsResultValue processBufferedData(XsByteArray& rawIn, std::deque& messages); - bool waitForMessage(XsXbusMessageId xmid, XsMessage& rcv); - bool writeMessage(const XsMessage& msg); - bool gotoConfig(); - bool gotoMeasurement(); - XsString getProductCode(); - XsDeviceId getDeviceId(); - bool setDeviceMode(const XsOutputMode& outputMode, const XsOutputSettings& outputSettings); - bool setOutputConfiguration(XsOutputConfigurationArray& config); - -private: - std::unique_ptr m_streamInterface; - std::shared_ptr m_protocolManager; - std::unique_ptr m_messageExtractor; - -}; - -#endif diff --git a/extern/include/xcommunication/enumerateusbdevices.h b/extern/include/xcommunication/enumerateusbdevices.h deleted file mode 100644 index 93b9774..0000000 --- a/extern/include/xcommunication/enumerateusbdevices.h +++ /dev/null @@ -1,36 +0,0 @@ -/* Copyright (c) 2003-2017 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. -*/ - -#ifndef XSENUMERATEUSBDEVICES_H -#define XSENUMERATEUSBDEVICES_H - -#include "xsens/xsportinfolist.h" - -bool xsEnumerateUsbDevices(XsPortInfoList& ports); - -#endif \ No newline at end of file diff --git a/extern/include/xcommunication/int_xsdatapacket.h b/extern/include/xcommunication/int_xsdatapacket.h deleted file mode 100644 index bea83fa..0000000 --- a/extern/include/xcommunication/int_xsdatapacket.h +++ /dev/null @@ -1,46 +0,0 @@ -/* Copyright (c) 2003-2017 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. -*/ - -#ifndef INT_XSDATAPACKET_H -#define INT_XSDATAPACKET_H - -struct XsDataPacket; -struct LegacyDataPacket; - -#ifdef __cplusplus -extern "C" -{ -#endif - -void XsDataPacket_assignFromLegacyDataPacket(struct XsDataPacket* thisPtr, struct LegacyDataPacket const* pack, int index); - -#ifdef __cplusplus -} // extern "C" -#endif - -#endif diff --git a/extern/include/xcommunication/iointerface.h b/extern/include/xcommunication/iointerface.h deleted file mode 100644 index 318ebf4..0000000 --- a/extern/include/xcommunication/iointerface.h +++ /dev/null @@ -1,142 +0,0 @@ -/* Copyright (c) 2003-2017 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. -*/ - -#ifndef IOINTERFACE_H -#define IOINTERFACE_H - -#include "xcommunicationconfig.h" -#include -#include -#include -#include -#include -#include -#include - -#include - -//! The default size of the serial read buffer in bytes -#define XS_DEFAULT_READ_BUFFER_SIZE (XS_MAXMSGLEN*8) -//! The default size of the serial write buffer in bytes -#define XS_DEFAULT_WRITE_BUFFER_SIZE XS_MAXMSGLEN - -#ifdef _MSC_VER -# define XS_MAX_FILENAME_LENGTH 512 -#else -# define XS_MAX_FILENAME_LENGTH PATH_MAX -#endif - -#ifndef _WIN32 -typedef int32_t XsIoHandle; -#else -#define XsIoHandle HANDLE -#endif - -struct XsPortInfo; - -struct IoInterface { -public: - /*! \brief Destructor */ - virtual ~IoInterface() {} - - /*! \brief Close the connection to the device. - \returns XRV_OK if the connection was closed successfully - */ - virtual XsResultValue close(void) = 0; - - /*! \brief Flush all data in the buffers to and from the device - \returns XRV_OK if the data was flushed successfully - */ - virtual XsResultValue flushData(void) = 0; - - /*! \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; - - /*! \brief Returns the last result value produced by this interface - \returns The last result value produced by this interface. - */ - virtual XsResultValue getLastResult(void) const = 0; - - /*! \brief Write the data contained in \a data to the device - \param data The data to write to the device. - \param written An optional %XsFilePos value that will receive the number of bytes that were - actually written. - \returns XRV_OK if the data was written successfully - */ - virtual XsResultValue writeData(const XsByteArray& data, XsFilePos* written = NULL) = 0; - - /*! \brief Read at most \a maxLength bytes from the device into \a data - \param maxLength The maximum number of bytes to read. Depending on the device type and timeout - settings, the function may return with less than this number of bytes read. - \param data A buffer that will contain the read data. - \returns XRV_OK if all data was read successfully, XRV_TIMEOUT if some data was read, but not - \a maxLength, XRV_TIMEOUTNODATA if no data was read at all. - */ - virtual XsResultValue readData(XsFilePos maxLength, XsByteArray& data) = 0; - - //! Options for flow control and stopbits which must be used when opening a port - 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) - }; - // 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 void cancelIo(void) const; - - // IOInterfaceFile overridable functions - virtual XsResultValue appendData(const XsByteArray& bdata); - virtual XsResultValue closeAndDelete(void); - virtual XsResultValue create(const XsString& filename); - virtual XsResultValue deleteData(XsFilePos start, XsFilePos length); - virtual XsResultValue find(const XsByteArray& needleV, XsFilePos& pos); - virtual XsFilePos getFileSize(void) const; - virtual XsResultValue getName(XsString& filename) const; - virtual XsFilePos getReadPosition(void) const; - virtual XsFilePos getWritePosition(void) const; - virtual XsResultValue insertData(XsFilePos start, const XsByteArray& data); - virtual bool isReadOnly(void) const; - virtual XsResultValue open(const XsString& filename, bool createNew, bool readOnly); - virtual XsResultValue setReadPosition(XsFilePos pos); - virtual XsResultValue setWritePosition(XsFilePos pos = -1); - - XSENS_DISABLE_COPY(IoInterface) -protected: - /*! \brief Constructor */ - IoInterface() - {} -}; -#endif // file guard diff --git a/extern/include/xcommunication/iointerfacefile.h b/extern/include/xcommunication/iointerfacefile.h deleted file mode 100644 index b0e8b28..0000000 --- a/extern/include/xcommunication/iointerfacefile.h +++ /dev/null @@ -1,106 +0,0 @@ -/* Copyright (c) 2003-2017 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. -*/ - -#ifndef IOINTERFACEFILE_H -#define IOINTERFACEFILE_H - -#include -#include "streaminterface.h" -#include - -/*! \brief The low-level file communication class. -*/ -class IoInterfaceFile : public IoInterface -{ -private: - XSENS_DISABLE_COPY(IoInterfaceFile) - -protected: - //! The file handle, also indicates if the file is open or not. - XsFile* m_handle; - //! Contains the size of the file - XsFilePos m_fileSize; - //! The last read position in the file - XsFilePos m_readPos; - //! The last write position in the file - XsFilePos m_writePos; - //! The last result of an operation - mutable XsResultValue m_lastResult; - //! 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. - - This value is used to check whether or not a seek is required to perform a - requested read or write operation. - */ - bool m_reading; - //! Indicates if the file was opened in read-only mode - bool m_readOnly; - - void gotoRead(); - void gotoWrite(); -public: - IoInterfaceFile(); - ~IoInterfaceFile(); - - // Function overrides - XsResultValue close() override; - XsResultValue closeFile(); - XsResultValue flushData() override; - bool isOpen() const override; - XsResultValue getLastResult() const 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); - - // Other functions - XsResultValue appendData(const XsByteArray& bdata); - XsResultValue closeAndDelete(); - XsResultValue create(const XsString& filename); - XsResultValue deleteData(XsFilePos start, XsFilePos length); - XsResultValue find(const XsByteArray& data, XsFilePos& pos); - XsFilePos getFileSize() const; - XsTimeStamp getFileDate() const; - XsResultValue getName(XsString& filename) const; - XsString getFileName() const; - XsFilePos getReadPosition() const; - XsFilePos getWritePosition() const; - XsResultValue insertData(XsFilePos start, const XsByteArray& data); - bool isReadOnly() const; - XsResultValue open(const XsString& filename, bool createNew, bool readOnly); - XsResultValue setReadPosition(XsFilePos pos); - XsResultValue setWritePosition(XsFilePos pos = -1); - XsResultValue reserve(XsFilePos minSize); - XsResultValue flushFileBuffers(); - - //! \brief The default file block size - static const XsFilePos m_fileBlockSize = 4096; -}; - -#endif diff --git a/extern/include/xcommunication/iprotocolhandler.h b/extern/include/xcommunication/iprotocolhandler.h deleted file mode 100644 index 31f20e7..0000000 --- a/extern/include/xcommunication/iprotocolhandler.h +++ /dev/null @@ -1,96 +0,0 @@ -/* Copyright (c) 2003-2017 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. -*/ - -#ifndef IPROTOCOLHANDLER_H -#define IPROTOCOLHANDLER_H - -#include "messagelocation.h" -#include -#include - -//-------------------------------------------------------------------------------- -/*! \brief Interface class for protocol handlers - \details Describes the interfaces of the protocol handling classes. The protocol handlers are - used to convert a binary data stream into XsMessage objects. -*/ -class IProtocolHandler -{ -public: - //! \brief Destructor - virtual ~IProtocolHandler() {} - - /*! \brief Find the first message in the \a raw byte stream - \details This function scans \a raw for a sequence of bytes that it can convert into an - %XsMessage object. It returns the location and total byte size of the message so that the - caller can remove those bytes from the stream. The return value can also describe that a - partial message has been found. Return values: - \li \a startpos >= 0 and \a size > 0: A full message with \a size has been found at \a startpos. - \li \a startpos >= 0 and \a size == 0: The start of a message has been found at \a startpos, but the size could not yet be determined. - \li \a startpos >= 0 and \a size < 0: The start of a message has been found at \a startpos, and the size of the full message is at least \a -size. - \li \a startpos < 0: No messages have been found. - - \param rcv If a message is read, it will be put in this object. - \param raw The raw byte stream to analyze. - \returns A %MessageLocation object that describes what was found. - */ - virtual MessageLocation findMessage(XsMessage& rcv, const XsByteArray& raw) const = 0; - - /*! \brief Returns the minimum size of a valid message - \details This value may differ for different protocols, but is always at least 1. - \returns The minimum size of a valid message for the protocol. - */ - virtual int minimumMessageSize() const = 0; - - /*! \brief Returns the maximum size of a valid message - \details This value may differ for different protocols. - \returns The maximum size of a valid message for the protocol. - */ - virtual int maximumMessageSize() const = 0; - - /*! \brief Returns the type of the protocol handler - \details Each protocol handler has a locally unique id that can be used for instantiation of - the correct protocol handler. - \returns The type id of the protocol handler. - */ - virtual int type() const = 0; - - /*! \brief Tells the protocol handler to ignore/expand its maximum message size - \details This is mostly used when reading from a file that is known to contain correct data. - Please note that the protocol handler can decide to ignore this call, which is what the default - implementation does. - \param ignore Set to true to ignore the maximum message size check. - */ - virtual void ignoreMaximumMessageSize(bool ignore) - { - (void) ignore; - } -}; - -//-------------------------------------------------------------------------------- - -#endif diff --git a/extern/include/xcommunication/iprotocolmanager.h b/extern/include/xcommunication/iprotocolmanager.h deleted file mode 100644 index 85e7a8c..0000000 --- a/extern/include/xcommunication/iprotocolmanager.h +++ /dev/null @@ -1,59 +0,0 @@ -/* Copyright (c) 2003-2017 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. -*/ - -#ifndef IPROTOCOLMANAGER_H -#define IPROTOCOLMANAGER_H - -#include -#include "messagelocation.h" - -/*! \brief Interface class for protocol manager -\details Describes the interfaces of a manager of different protocols \sa ProtocolHandler -*/ -class IProtocolManager -{ -public: - - //! \brief Destructor - virtual ~IProtocolManager() {} - - /*! \brief Will let all supported protocols attempt finding a message in the given raw data - \param rcv: If a message has been found it will be stored in this input variable - \param raw: The input raw byte array in which to look for a message - \returns A MessageLocation object describing the best possible found message \sa MessageLocation - */ - virtual MessageLocation findMessage(XsMessage& rcv, const XsByteArray& raw) = 0; - - /*! \brief Performs a sanity check on the given message - \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; -}; - -#endif diff --git a/extern/include/xcommunication/legacydatapacket.h b/extern/include/xcommunication/legacydatapacket.h deleted file mode 100644 index 6182eaa..0000000 --- a/extern/include/xcommunication/legacydatapacket.h +++ /dev/null @@ -1,367 +0,0 @@ -/* Copyright (c) 2003-2017 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. -*/ - -#ifndef LEGACYDATAPACKET_H -#define LEGACYDATAPACKET_H - -#include -#include -#include -#include -#include -#include - -//! Indicates that a data item is not available in the packet -#define XS_DATA_ITEM_NOT_AVAILABLE 65535 - -struct XsVector; -struct PacketInfo; -struct XsUShortVector; -struct XsScrData; -struct XsCalibratedData; -struct XsGpsPvtData; -struct XsPressure; -struct MtwSdiData; -struct XsQuaternion; -struct XsMatrix; -struct XsEuler; -struct XsAnalogInData; -struct XsUtcTime; - -//! \brief An MT timestamp (sample count) -typedef uint16_t XsMtTimeStamp; - -struct PacketFixedData; - -struct LegacyDataPacket -{ -public: - - LegacyDataPacket(); - LegacyDataPacket(XsSize count, bool xbus); - LegacyDataPacket(const LegacyDataPacket& pack); - ~LegacyDataPacket(); - - const LegacyDataPacket& operator = (const LegacyDataPacket& pack); - - XsSize itemCount(void) const; - void setItemCount(XsSize count); - - XsTimeStamp timeOfArrival(void) const; - void setTimeOfArrival(XsTimeStamp timeOfArrival); - - XsTimeStamp rtc(void) const; - void setRtc(XsTimeStamp rtc); - - int64_t largePacketCounter(void) const; - void setLargePacketCounter(int64_t sc); - - XsMessage message(void) const; - void setMessage(const XsMessage& message); - XsMessage originalMessage(void) const; - - PacketInfo packetInfo(int32_t index) const; - - bool setDataFormat(const XsDataFormat& format, int32_t index = 0); - bool setDataFormat(XsOutputMode outputMode, XsOutputSettings outputSettings, int32_t index = 0); - XsDataFormat dataFormat(int32_t index = 0) const; - - void setXbusSystem(bool xbus); - bool isXbusSystem(void) const; - - int32_t findDeviceId(XsDeviceId dev) const; - void setDeviceId(XsDeviceId deviceId, int32_t index); - XsDeviceId deviceId(int32_t index) const; - - uint16_t frameCounter() const; - - XsSize dataSize(int32_t index=0) const; - - void updateInfoList(); - - //PacketFixedData takePacketFixedData(); - //void putPacketFixedData(const PacketFixedData& data); - - uint16_t getFPValueSize(int32_t index) const; - - XsUShortVector rawAcceleration(int32_t index=0) const; - bool containsRawAcceleration(int32_t index=0) const; - bool setRawAcceleration(const XsUShortVector& vec, int32_t index=0); - /*! \brief Return the Raw Gyroscope component of a data item. - \param index The index of the item of which the data should be returned. - \returns The Raw Gyroscope component of a data item. - */ - XsUShortVector rawGyroscopeData(int32_t index=0) const; - //! Check if data item contains Raw Gyroscope data - bool containsRawGyroscopeData(int32_t index=0) const; - //! Add/update Raw Gyroscope data for the item - bool setRawGyroscopeData(const XsUShortVector& vec, int32_t index=0); - /*! \brief Return the Raw Magnetometer component of a data item. - \param index The index of the item of which the data should be returned. - \returns The Raw Magnetometer component of a data item. - */ - XsUShortVector rawMagneticField(int32_t index=0) const; - //! Check if data item contains Raw Magnetometer data - bool containsRawMagneticField(int32_t index=0) const; - //! Add/update Raw Magnetometer data for the item - bool setRawMagneticField(const XsUShortVector& vec, int32_t index=0); - /*! \brief Return the Raw Temperature component of a data item. - \param index The index of the item of which the data should be returned. - \param channel The Temperature channel to return - \returns The Raw Temperature component of a data item. - */ - uint16_t rawTemperature(int32_t index=0, int channel = 0) const; - //! Check if data item contains Raw Temperature data - bool containsRawTemperature(int32_t index=0, int channel = 0) const; - //! Add/update Raw Temperature data for the item - bool setRawTemperature(uint16_t temp, int32_t index=0, int channel = 0); - //! Returns the number of available Raw Temperature channels - int rawTemperatureChannelCount(int32_t index = 0) const; - /*! \brief Return the Raw Data component of a data item. - \param index The index of the item of which the data should be returned. - \returns The Raw Data component of a data item. - */ - XsScrData rawData(int32_t index=0) const; - //! Check if data item contains Raw Data - bool containsRawData(int32_t index=0) const; - //! Add/update Raw Data for the item - bool setRawData(const XsScrData& data, int32_t index=0); - /*! \brief Return the Gps PVT data component of a data item. - \param index The index of the item of which the data should be returned. - \returns The Gps PVT data component of a data item. - */ - XsGpsPvtData gpsPvtData(int32_t index=0) const; - //! Check if data item contains Gps PVT Data - bool containsGpsPvtData(int32_t index=0) const; - //! Add/update Gps PVT Data for the item - bool setGpsPvtData(const XsGpsPvtData& data, int32_t index=0); - - XsPressure pressure(int32_t index=0) const; - bool containsPressure(int32_t index=0) const; - bool setPressure(const XsPressure& data, int32_t index=0); - - MtwSdiData mtwSdiData(int32_t index=0) const; - bool containsMtwSdiData(int32_t index=0) const; - bool setMtwSdiData(const MtwSdiData& data, int32_t index=0); - - /*! \brief Return the Temperature component of a data item. - \param index The index of the item of which the data should be returned. - \param channel The Temperature channel to return - \returns The Temperature component of a data item. - */ - double temperature(int32_t index=0, int channel = 0) const; - //! Check if data item contains Temperature data - bool containsTemperature(int32_t index=0, int channel = 0) const; - //! Add/update Calibrated Accelerometer data for the item - bool setTemperature(double temp, int32_t index=0, int channel = 0); - //! Returns the number of temperature channels - int temperatureChannelCount(int32_t index = 0) const; - - /*! \brief Return the Calibrated Accelerometer component of a data item. - \param index The index of the item of which the data should be returned. - \returns The Calibrated Accelerometer component of a data item. - */ - XsVector calibratedAcceleration(int32_t index=0) const; - //! Check if data item contains Calibrated Accelerometer data - bool containsCalibratedAcceleration(int32_t index=0) const; - //! Add/update Calibrated Accelerometer data for the item - bool setCalibratedAcceleration(const XsVector& vec, int32_t index=0); - - /*! \brief Return the Calibrated Gyroscope component of a data item. - \param index The index of the item of which the data should be returned. - \returns The Calibrated Gyroscope component of a data item. - */ - XsVector calibratedGyroscopeData(int32_t index=0) const; - //! Check if data item contains Calibrated Gyroscope data - bool containsCalibratedGyroscopeData(int32_t index=0) const; - //! Add/update Calibrated Gyroscope data for the item - bool setCalibratedGyroscopeData(const XsVector& vec, int32_t index=0); - - /*! \brief Return the Calibrated Magnetometer component of a data item. - \param index The index of the item of which the data should be returned. - \returns The Calibrated Magnetometer component of a data item. - */ - XsVector calibratedMagneticField(int32_t index=0) const; - //! Check if data item contains Calibrated Magnetometer data - bool containsCalibratedMagneticField(int32_t index=0) const; - //! Add/update Calibrated Magnetometer data for the item - bool setCalibratedMagneticField(const XsVector& vec, int32_t index=0); - - XsCalibratedData calibratedData(int32_t index=0) const; - bool containsCalibratedData(int32_t index=0) const; - bool setCalibratedData(const XsCalibratedData& data, int32_t index=0); - /*! \brief Return the Orientation component of a data item as a Quaternion. - \param index The index of the item of which the data should be returned. - \returns The Orientation component of a data item as a Quaternion. - */ - XsQuaternion orientationQuaternion(int32_t index=0) const; - //! Check if data item contains Quaternion Orientation data - bool containsOrientationQuaternion(int32_t index=0) const; - //! Add/update Quaternion Orientation data for the item - bool setOrientationQuaternion(const XsQuaternion& data, int32_t index=0); - /*! \brief Return the Orientation component of a data item as Euler angles. - \param index The index of the item of which the data should be returned. - \returns The Orientation component of a data item as Euler angles. - */ - XsEuler orientationEuler(int32_t index=0) const; - //! Check if data item contains Euler Orientation data - bool containsOrientationEuler(int32_t index=0) const; - //! Add/update Euler Orientation data for the item - bool setOrientationEuler(const XsEuler& data, int32_t index=0); - /*! \brief Return the Orientation component of a data item as an Orientation Matrix. - \param index The index of the item of which the data should be returned. - \returns The Orientation component of a data item as an Orientation Matrix. - */ - XsMatrix orientationMatrix(int32_t index=0) const; - //! Check if data item contains Matrix Orientation data - bool containsOrientationMatrix(int32_t index=0) const; - //! Add/update Matrix Orientation data for the item - bool setOrientationMatrix(const XsMatrix& data, int32_t index=0); - - //! Check if data item contains Orientation Data of any kind - bool containsOrientation(int32_t index=0) const; - - /*! \brief Return the AnalogIn 1 component of a data item. - \param index The index of the item of which the data should be returned. - \returns The AnalogIn 1 component of a data item. - */ - XsAnalogInData analogIn1Data(int32_t index=0) const; - //! Check if data item contains AnalogIn 1 - bool containsAnalogIn1Data(int32_t index=0) const; - //! Add/update AnalogIn 1 for the item - bool setAnalogIn1Data(const XsAnalogInData& data, int32_t index=0); - - /*! \brief Return the AnalogIn 2 component of a data item. - \param index The index of the item of which the data should be returned. - \returns The AnalogIn 2 component of a data item. - */ - XsAnalogInData analogIn2Data(int32_t index=0) const; - //! Check if data item contains AnalogIn 2 - bool containsAnalogIn2Data(int32_t index=0) const; - //! Add/update AnalogIn 2 for the item - bool setAnalogIn2Data(const XsAnalogInData& data, int32_t index=0); - - /*! \brief Return the Position Lat Lon Alt component of a data item. - \param index The index of the item of which the data should be returned. - \returns The Position Lat Lon Alt component of a data item. - */ - XsVector positionLLA(int32_t index=0) const; - //! Check if data item contains Position Lat Lon Alt - bool containsPositionLLA(int32_t index=0) const; - //! Add/update Position Lat Lon Alt for the item - bool setPositionLLA(const XsVector& data, int32_t index=0); - - /*! \brief Return the Velocity component of a data item. - \param index The index of the item of which the data should be returned. - \returns The Velocity component of a data item. - */ - XsVector velocity(int32_t index=0) const; - //! Check if data item contains Velocity - bool containsVelocity(int32_t index=0) const; - //! Add/update Velocity for the item - bool setVelocity(const XsVector& data, int32_t index=0); - - /*! \brief Return the Status component of a data item. - \param index The index of the item of which the data should be returned. - \param[out] outIsDetailed pointer to boolean. After call, the boolean is true if the status is detailed - \returns The Status component of a data item. - */ - uint32_t status(int32_t index, bool* outIsDetailed) const; - //! Check if data item contains Status - bool containsStatus(int32_t index=0) const; - //! Check if data item contains detailed Status information - bool containsDetailedStatus(int32_t index=0) const; - //! Add/update Status information for the item - bool setStatus(const uint32_t data, int32_t index=0); - - /*! \brief Return the Sample Counter component of the packet. - \param index The index of the item of which the data should be returned. (ignored) - \returns The Sample Counter component of the packet. - */ - uint16_t packetCounter(int32_t index=0) const; - //! Check if data item contains Sample Counter - bool containsPacketCounter(int32_t index=0) const; - //! Add/update Sample Counter for all items - bool setPacketCounter(const uint16_t counter, int32_t index=0); - - /*! \brief Return the SampleTimeFine component of the packet. - \param index The index of the item of which the data should be returned. (ignored) - \returns The SampleTimeFine component of the packet. - */ - uint32_t sampleTimeFine(int32_t index = 0) const; - //! Check if data item contains SampleTimeFine - bool containsSampleTimeFine(int32_t index = 0) const; - //! Add/update SampleTimeFine for all items - bool setSampleTimeFine(const uint32_t counter, int32_t index = 0); - - /*! \brief Return the UTC Time component of the packet. - \param index The index of the item of which the data should be returned. (ignored) - \returns The UTC Time component of the packet. - */ - XsUtcTime utcTime(int32_t index=0) const; - //! Check if data item contains UTC Time - bool containsUtcTime(int32_t index=0) const; - //! Add/update UTC Time for all items - bool setUtcTime(const XsUtcTime& data, int32_t index=0); - - /*! \brief Return the Acc-G component of the packet. - \param index The index of the item of which the data should be returned. - \returns The Acc-G component of the packet. - */ - XsVector freeAcceleration(int32_t index=0) const; - //! Check if data item contains XKF-3 Acc-G data - bool containsFreeAcceleration(int32_t index=0) const; - //! Add/update XKF-3 Acc-G data for the item - bool setFreeAcceleration(const XsVector& g, int32_t index=0); - - /*! \brief Return the synhcronization time recorded by the station. - \param channelID The synchronization channel number, where the output signal is connected. - \returns The synhcronization time recorded by the station. - */ - XsTimeStamp triggerIndication(int channelID) const; - //! Check if data item contains indication time on the given line - bool containsTriggerIndication(int channelID = 0) const; - //! Add/update trigger indication time for the item to the given line - bool setTriggerIndication(int channelID, const XsTimeStamp &t); - - /*! \brief Returns a const pointer to the contained fixed data, mostly used for debugging */ - inline const PacketFixedData* fixedData() const { return m_fixedData; } - -private: - PacketFixedData* m_fixedData; //!< Fixed packet data - mutable XsDeviceId m_lastFoundId; //!< Last found deviceId, speeds up searches - mutable XsSize m_lastFoundIndex; //!< Index of last found deviceId, speeds up searches - XsMessage m_msg; //!< The message - XsTimeStamp m_rtc; //!< Sample time in ms, based on the sample counter - XsTimeStamp m_toa; //!< Time of arrival - XsTimeStamp m_packetId; //!< 64 bit sample counter - XsTimeStamp m_triggerIn1; //!< Trigger indication on line 1 \todo remove this - XsTimeStamp m_triggerIn2; //!< Trigger indication on line 2 \todo remove this -}; - -#endif // file guard diff --git a/extern/include/xcommunication/messageextractor.h b/extern/include/xcommunication/messageextractor.h deleted file mode 100644 index ebf4d04..0000000 --- a/extern/include/xcommunication/messageextractor.h +++ /dev/null @@ -1,55 +0,0 @@ -/* Copyright (c) 2003-2017 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. -*/ - -#ifndef MESSAGEEXTRACTOR_H -#define MESSAGEEXTRACTOR_H - -#include -#include -#include -#include -#include "iprotocolmanager.h" - -class MessageExtractor -{ -public: - MessageExtractor(std::shared_ptr protocolManager); - - XsResultValue processNewData(XsByteArray const& newData, std::deque &messages); - void clearBuffer(); - -private: - static const int m_maxIncompleteRetryCount; - XsByteArray m_buffer; - int m_retryTimeout; - std::shared_ptr m_protocolManager; -}; - - -#endif - diff --git a/extern/include/xcommunication/messagelocation.h b/extern/include/xcommunication/messagelocation.h deleted file mode 100644 index b2dc29f..0000000 --- a/extern/include/xcommunication/messagelocation.h +++ /dev/null @@ -1,76 +0,0 @@ -/* Copyright (c) 2003-2017 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. -*/ - -#ifndef MESSAGELOCATION_H -#define MESSAGELOCATION_H - -/*! \brief Stores the location of a message in a buffer using a start position and a size -*/ -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 - - /*! The offset of the first incomplete potentially eventually valid message that has been - skipped to reach the first valid and complete message at m_startPos. - If no valid complete message has been found (m_startPos == -1), m_incompletePos still - contains the offset of the first incomplete message. - invariant: m_incompletePos == -1 || m_startPos == -1 || m_incompletePos < m_startPos - */ - int m_incompletePos; - - /*! The expected size of the first incomplete potentially eventually valid message that has been - skipped to reach the first valid and complete message at m_startPos. - If a valid complete message has been found at a non-0 start position and m_incompletePos is not -1, - m_incompleteSize will contain the expected size of the incomplete message. - */ - int m_incompleteSize; - - /*! \brief Constructor, initializes by default to an invalid message - \param start The offset of the first byte of the message - \param size The size of the message - \param incompletePos The offset of the first byte of an incomplete but potentially eventually valid message - \param incompleteSize The expected size of the message at \a incompletePos, if that is not -1 - \sa isValid() \sa m_startPos \sa m_size \sa IProtocolHandler::findMessage - */ - MessageLocation(int start = -1, int size = 0, int incompletePos = -1, int incompleteSize = 0) - : m_startPos(start) - , m_size(size) - , m_incompletePos(incompletePos) - , m_incompleteSize(incompleteSize) - {} - - /*! \brief Returns whether the stored message information describes a valid message - */ - inline bool isValid() const - { - return m_startPos >= 0 && m_size > 0; - } -}; - -#endif diff --git a/extern/include/xcommunication/mtwsdidata.h b/extern/include/xcommunication/mtwsdidata.h deleted file mode 100644 index 708a6d0..0000000 --- a/extern/include/xcommunication/mtwsdidata.h +++ /dev/null @@ -1,69 +0,0 @@ -/* Copyright (c) 2003-2017 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. -*/ - -#ifndef MTWSDIDATA_H -#define MTWSDIDATA_H - -#include -#include -#include -#include - -struct XsRange; - -struct MtwSdiData -{ - XsDeviceId m_deviceId; //!< The ID of the device that generated the data - uint8_t m_timeSync; //!< Indicates if the time sync is in order (unused) - uint16_t m_firstFrameNumber; //!< The first frame number of the SDI interval. The time of the interval is [first, last) - uint16_t m_lastFrameNumber; //!< The last frame number of the SDI interval. The time of the interval is [first, last) - XsVector3 m_currentBias; //!< The gyroscope bias used during the SDI interval - XsQuaternion m_orientationIncrement; //!< The orientation increment (delta Q) over the interval - XsVector3 m_velocityIncrement; //!< The velocity increment (delta V) over the interval - bool m_aidingData; //!< reserved - double m_barometer; //!< The barometer value during the interval - XsVector3 m_magnetoMeter; //!< The magnetometer values during the interval - int8_t m_rssi; //!< The Received Signal Strength Indication (RSSI) of the message - - MtwSdiData(); - MtwSdiData(const MtwSdiData& other); - ~MtwSdiData(); - const MtwSdiData& operator=(const MtwSdiData& other); - - bool empty() const; - bool containsAidingData() const; - XsQuaternion orientationIncrement() const; - XsVector velocityIncrement() const; - double pressure() const; - XsVector magneticField() const; - XsVector currentBias() const; - XsRange frameRange() const; - double rssi() const; -}; - -#endif diff --git a/extern/include/xcommunication/packetfixeddata.h b/extern/include/xcommunication/packetfixeddata.h deleted file mode 100644 index 8a40991..0000000 --- a/extern/include/xcommunication/packetfixeddata.h +++ /dev/null @@ -1,135 +0,0 @@ -/* Copyright (c) 2003-2017 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. -*/ - -#ifndef PACKETFIXEDDATA_H -#define PACKETFIXEDDATA_H - -#include -#include - -#ifndef NOT_FOR_PUBLIC_RELEASE -struct XsControl; // required by DLL for supporting advanced features -#endif - -struct XsDataFormat; - -//! Indicates that a data item is not available in the packet -#define XS_DATA_ITEM_NOT_AVAILABLE 65535 - -/*! \brief Contains offset information about data in the packet - \details All items are initialized to \a XS_DATA_ITEM_NOT_AVAILABLE and set to their proper - value by LegacyDataPacket::updateInfoList and/or the LegacyDataPacket update functions. -*/ -struct PacketInfo { - uint16_t m_offset; //!< Offset of first data byte (whatever it is) - uint16_t m_rawData; //!< Offset of raw data - uint16_t m_rawAcc; //!< Offset of raw acceleration data - uint16_t m_rawGyr; //!< Offset of raw gyroscope data - uint16_t m_rawMag; //!< Offset of raw magnetometer data - uint16_t m_rawTemp; //!< Offset of raw temperature data. Note that usually only the first item is used. - uint16_t m_temp; //!< Offset of calibrated temperature data. Note that usually only the first item is used. - uint16_t m_rawGyroTemp[3]; //!< Offset of raw gyro temperature data - uint16_t m_gyroTemp[3]; //!< Offset of calibrated gyro temperature data - uint16_t m_calData; //!< Offset of calibrated data - uint16_t m_calAcc; //!< Offset of calibrated acceleration data - uint16_t m_calGyr; //!< Offset of calibrated gyroscope data - uint16_t m_calMag; //!< Offset of calibrated magnetometer data - uint16_t m_oriQuat; //!< Offset of orientation in quaternion format - uint16_t m_oriEul; //!< Offset of orientation in euler format - uint16_t m_oriMat; //!< Offset of orientation in matrix format - uint16_t m_analogIn1; //!< Offset of analog in channel 1 data - uint16_t m_analogIn2; //!< Offset of analog in channel 2 data - uint16_t m_posLLA; //!< Offset of Latitude-Longitude-Altitude position data - uint16_t m_velNEDorNWU; //!< Offset of velocity data - uint16_t m_status; //!< Offset of status data - uint16_t m_detailedStatus; //!< Offset of detailed status data - uint16_t m_sc; //!< Offset of sample counter - uint16_t m_utcTime; //!< Offset of UTC time data - uint16_t m_utcNano; //!< Offset of nanosecond part of UTC time - uint16_t m_utcYear; //!< Offset of year part of UTC time - uint16_t m_utcMonth; //!< Offset of month part of UTC time - uint16_t m_utcDay; //!< Offset of day part of UTC time - uint16_t m_utcHour; //!< Offset of hour part of UTC time - uint16_t m_utcMinute; //!< Offset of minute part of UTC time - uint16_t m_utcSecond; //!< Offset of second part of UTC time - uint16_t m_utcValid; //!< Offset of validity part of UTC time - uint16_t m_acc_g; //!< Offset of acceleration in global frame - uint16_t m_gpsPvtData; //!< Offset of GPS & pressure data - uint16_t m_gpsPvtPressure; //!< Offset of pressure data - uint16_t m_gpsPvtPressureAge; //!< Offset of pressure age - uint16_t m_gpsPvtGpsData; //!< Offset of raw GPS data - uint16_t m_gpsPvtItow; //!< Offset of raw GPS ITOW (Integer Time Of Week) data - uint16_t m_gpsPvtLatitude; //!< Offset of raw GPS latitude data - uint16_t m_gpsPvtLongitude; //!< Offset of raw GPS longitude data - uint16_t m_gpsPvtHeight; //!< Offset of raw GPS height data - uint16_t m_gpsPvtVeln; //!< Offset of raw GPS northward velocity data - uint16_t m_gpsPvtVele; //!< Offset of raw GPS eastward velocity data - uint16_t m_gpsPvtVeld; //!< Offset of raw GPS dowanward velocity data - uint16_t m_gpsPvtHacc; //!< Offset of raw GPS horizontal accuracy estimate data - uint16_t m_gpsPvtVacc; //!< Offset of raw GPS vertical accuracy estimate data - uint16_t m_gpsPvtSacc; //!< Offset of raw GPS speed accuracy estimate data - uint16_t m_gpsPvtGpsAge; //!< Offset of raw GPS age - uint16_t m_mtwSdiData; //!< Offset of MTw SDI data - uint16_t m_wClientId; //!< Offset of MTw SDI client ID data - uint16_t m_wTimeSync; //!< Offset of MTw SDI time sync data - uint16_t m_wFirstFrameNumber; //!< Offset of MTw SDI first frame number in interval data - uint16_t m_wLastFrameNumber; //!< Offset of MTw SDI last frame number in interval data - uint16_t m_wCurrentBias; //!< Offset of MTw SDI gyroscope bias data - uint16_t m_wOrientationIncrement; //!< Offset of MTw SDI orientation increment data - uint16_t m_wVelocityIncrement; //!< Offset of MTw SDI velocity increment data - uint16_t m_wAidingData; //!< Offset of MTw SDI aiding data - uint16_t m_wBaroMeter; //!< Offset of MTw SDI barometer data - uint16_t m_wMagnetoMeter; //!< Offset of MTw SDI magnetometer data - uint16_t m_wRssi; //!< Offset of MTw SDI RSSI data - uint16_t m_sampleTimeFine; //!< Offset of SampleTimeFine - uint16_t m_size; //!< Total size of the data - uint16_t m_doubleBoundary; //!< Boundary where the original data format is ignored and values are stored in double precision - - /*! \brief Default contructor, sets all values to XS_DATA_ITEM_NOT_AVAILABLE - */ - PacketInfo() { memset(this, 0xFF, sizeof(*this)); } -}; - -//! A structure containing fixed packet data, which should not change during a measurement for the same device -struct PacketFixedData -{ - PacketFixedData(); - PacketFixedData(XsSize count); - PacketFixedData(const PacketFixedData& pack); - ~PacketFixedData(); - - void operator = (const PacketFixedData& data); - - PacketInfo* m_infoList; //!< Contains information about data in the packet and the format of that data - XsDataFormat* m_formatList; //!< A list of the formats of the data items - XsDeviceId* m_idList; //!< A list of the device-ids in this packet - bool m_xm; //!< Indicates that xbus-formatting is used - XsSize m_itemCount; //!< The number of data items in the message -}; - -#endif // file guard diff --git a/extern/include/xcommunication/packetstamper.h b/extern/include/xcommunication/packetstamper.h deleted file mode 100644 index 2284099..0000000 --- a/extern/include/xcommunication/packetstamper.h +++ /dev/null @@ -1,57 +0,0 @@ -/* Copyright (c) 2003-2017 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. -*/ - -#ifndef PACKETSTAMPER_H -#define PACKETSTAMPER_H - -#include - -struct XsDataPacket; - -class PacketStamper -{ -public: - PacketStamper(); - void resetTosEstimation(); - - static const int64_t AWINDABOUNDARY; - static const int64_t MTSCBOUNDARY; - static const int64_t SC8BOUNDARY; - - static int64_t calculateLargePacketCounter(int64_t frameCounter, int64_t lastCounter, int64_t boundary); - static int64_t calculateLargeSampleTime(int64_t frameTime, int64_t lastTime); - int64_t stampPacket(XsDataPacket& pack, XsDataPacket& highest); - -private: - int64_t m_t0Est, m_t0New; - int64_t m_pid0Est, m_pid0New; - double m_tPerPidEst, m_tPerPidNew; - void estimateTos(XsDataPacket& pack); -}; - -#endif diff --git a/extern/include/xcommunication/protocolhandler.h b/extern/include/xcommunication/protocolhandler.h deleted file mode 100644 index c2cf506..0000000 --- a/extern/include/xcommunication/protocolhandler.h +++ /dev/null @@ -1,52 +0,0 @@ -/* Copyright (c) 2003-2017 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. -*/ - -#ifndef PROTOCOLHANDLER_H -#define PROTOCOLHANDLER_H - -#include "xcommunicationconfig.h" -#include "iprotocolhandler.h" - -class ProtocolHandler : public virtual IProtocolHandler -{ - bool m_ignoreMaxMsgSize; -public: - ProtocolHandler(); - virtual ~ProtocolHandler(); - - MessageLocation findMessage(XsMessage& rcv, const XsByteArray& raw) const override; - int minimumMessageSize() const override; - int maximumMessageSize() const override; - int type() const override; - static int composeMessage(XsByteArray& raw, const XsMessage& msg); - void ignoreMaximumMessageSize(bool ignore) override; - - XSENS_DISABLE_COPY(ProtocolHandler) -}; - -#endif diff --git a/extern/include/xcommunication/rx_tx_log.h b/extern/include/xcommunication/rx_tx_log.h deleted file mode 100644 index 63d6b1c..0000000 --- a/extern/include/xcommunication/rx_tx_log.h +++ /dev/null @@ -1,171 +0,0 @@ -/* Copyright (c) 2003-2017 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. -*/ - -#ifndef RX_TX_LOG_H -#define RX_TX_LOG_H -#ifdef LOG_RX_TX -#if defined(LOG_RX_TX_PER_STATE) || defined(LOG_RX_TX_UNIQUE) -#include -#include - -/*! \brief Helper function for making filename of log file unique -*/ -inline static void makeFilenameUnique(char* filename, char const* state) -{ -#if defined(LOG_RX_TX_PER_STATE) || defined(LOG_RX_TX_UNIQUE) - char basename[XS_MAX_FILENAME_LENGTH]; - strcpy(basename, filename); - basename[strlen(basename) - 4] = 0; // remove .log extension - sprintf(filename, "%s_%" PRINTF_INT64_MODIFIER "u%s.log", basename, XsTimeStamp::nowMs(), state); -#else - (void)filename; -#endif -} - - -#ifdef LOG_RX_TX_PER_STATE -inline static void checkStateRx(char* state, int length, XsByteArray const& data, XsFile& rx_log) -{ - state[0] = 0; - if (length >= 4) - { - // find preamble - int idx = 0; - while (idx < length-4) - { - if (data[idx] == 0xFA && (data[idx+1] == 0xFF || data[idx+1] == 0) && data[idx+3] == 0) - { - 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; - } - break; - } - ++idx; - } - } - if (rx_log.isOpen() && state[0] != 0) - { - rx_log.flush(); - rx_log.close(); - } -} -inline static void checkStateRx(char* state, int length, void const* data, XsFile& rx_log) -{ - XsByteArray tmp((uint8_t*) data, length); - checkStateRx(state, length, tmp, rx_log); -} - -#define CHECK_STATE_RX(length, data, logfile) \ - char state[16] = "";\ - checkStateRx(state, (int) (length), (data), (logfile)) - -#else - static const char state[1] = ""; - #define CHECK_STATE_RX(...) ((void)0) -#endif - -#ifdef LOG_RX_TX_PER_STATE -inline static void checkStateTx(char* state, int length, XsByteArray const& data, XsFile& tx_log) -{ - state[0] = 0; - if (length >= 4) - { - // find preamble - int idx = 0; - while (idx < length-4) - { - if (data[idx] == 0xFA && (data[idx+1] == 0xFF || data[idx+1] == 0) && data[idx+3] == 0) - { - 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; - } - break; - } - ++idx; - } - } - if (tx_log.isOpen() && state[0] != 0) - { - tx_log.flush(); - tx_log.close(); - } -} -inline static void checkStateTx(char* state, int length, void const* data, XsFile& tx_log) -{ - XsByteArray tmp((uint8_t*) data, length); - checkStateTx(state, length, tmp, tx_log); -} -#define CHECK_STATE_TX(length, data, logfile) \ - char state[16] = "";\ - checkStateTx(state, (int) (length), (data), (logfile)) - -#else - static const char state[1] = ""; - #define CHECK_STATE_TX(...) ((void)0) -#endif - - -#endif -#endif -#endif diff --git a/extern/include/xcommunication/serialinterface.h b/extern/include/xcommunication/serialinterface.h deleted file mode 100644 index f39dc0c..0000000 --- a/extern/include/xcommunication/serialinterface.h +++ /dev/null @@ -1,112 +0,0 @@ -/* Copyright (c) 2003-2017 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. -*/ - -#ifndef SERIALINTERFACE_H -#define SERIALINTERFACE_H - -#include "streaminterface.h" -#include -#include -#include -#include - -struct XsPortInfo; - -////////////////////////////////////////////////////////////////////////////////////////// -///////////////////////////////////////// SerialInterface ///////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////////////////// - -/*! \brief The low-level serial communication class. -*/ -class SerialInterface : public StreamInterface { -private: - XSENS_DISABLE_COPY(SerialInterface) - - void applyHwControlLinesOptions(PortOptions options, int portLinesOptions, int& p); - -#ifdef LOG_RX_TX - XsFile rx_log; - XsFile tx_log; -#endif - -protected: - //! 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. - uint32_t m_endTime; - //! The last result of an operation - mutable XsResultValue m_lastResult; - //! The opened COM port nr - uint16_t m_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 -public: - SerialInterface(); - virtual ~SerialInterface(); - - // Function overrides - XsResultValue close (void) override; - XsResultValue closeLive(void); - XsResultValue flushData (void) override; - bool isOpen (void) const; - XsResultValue getLastResult(void) const; - XsResultValue writeData (const XsByteArray& data, XsFilePos* written = 0) override; - XsResultValue readData(XsFilePos maxLength, XsByteArray& data) override; - - void cancelIo(void) const; - - // Other functions - XsResultValue escape(XsControlLine mask, XsControlLine state); - XsBaudRate getBaudrate(void) const; - XsIoHandle getHandle(void) const; - uint16_t getPortNumber (void) const; - void getPortName(XsString& portname) 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; -}; - -SerialInterface::PortOptions operator|(SerialInterface::PortOptions lhs, SerialInterface::PortOptions rhs); -SerialInterface::PortOptions operator&(SerialInterface::PortOptions lhs, SerialInterface::PortOptions rhs); -SerialInterface::PortOptions operator~(SerialInterface::PortOptions lhs); - - -#endif // file guard diff --git a/extern/include/xcommunication/simpleprotocolmanager.h b/extern/include/xcommunication/simpleprotocolmanager.h deleted file mode 100644 index ac30d35..0000000 --- a/extern/include/xcommunication/simpleprotocolmanager.h +++ /dev/null @@ -1,58 +0,0 @@ -/* Copyright (c) 2003-2017 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. -*/ - -#ifndef SIMPLEPROTOCOLMANAGER_H -#define SIMPLEPROTOCOLMANAGER_H - -#include "iprotocolmanager.h" -#include "protocolhandler.h" - -/*! \brief A very basic protocol manager - \details This protocol manager supports only the xbus protocol -*/ -class SimpleProtocolManager : public IProtocolManager -{ -public: - /*! \copydoc IProtocolManager::findMessage - */ - MessageLocation findMessage(XsMessage& rcv, const XsByteArray& raw) override - { - return m_protocolHandler.findMessage(rcv, raw); - } - - /*! \copydoc IProtocolManager::validateMessage - */ - bool validateMessage(XsMessage const& msg) const override - { - return msg.isChecksumOk(); - } -private: - ProtocolHandler m_protocolHandler; //!< Standard protocol handler; used for finding messages. -}; - -#endif diff --git a/extern/include/xcommunication/streaminterface.h b/extern/include/xcommunication/streaminterface.h deleted file mode 100644 index a251236..0000000 --- a/extern/include/xcommunication/streaminterface.h +++ /dev/null @@ -1,62 +0,0 @@ -/* Copyright (c) 2003-2017 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. -*/ - -#ifndef STREAMINTERFACE_H -#define STREAMINTERFACE_H - -#include "iointerface.h" -#include - -/*! \brief A stream interface - \details This class provides an interface for dealing with streaming I/O devices, such as a COM - port or a USB port. -*/ -class StreamInterface : public IoInterface -{ -public: - /*! \brief Destroy the stream interface - */ - inline ~StreamInterface() {} - - /*! \brief Set the read/write timeout to \a ms - A timeout of 0 means non-blocking operation of writeData() and readData(). - */ - virtual XsResultValue setTimeout(uint32_t ms) = 0; - - /*! \brief The timeout used for read/write operations - */ - virtual uint32_t getTimeout() const = 0; - - XSENS_DISABLE_COPY(StreamInterface) -protected: - /*! \brief Create a stream interface - */ - inline StreamInterface() {} -}; - -#endif diff --git a/extern/include/xcommunication/synclinegmt.h b/extern/include/xcommunication/synclinegmt.h deleted file mode 100644 index 0f496c8..0000000 --- a/extern/include/xcommunication/synclinegmt.h +++ /dev/null @@ -1,70 +0,0 @@ -/* Copyright (c) 2003-2017 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. -*/ - -#ifndef XSSYNCLINEGMT_H -#define XSSYNCLINEGMT_H - -#include - -/*! \addtogroup enums Global enumerations - @{ -*/ -/*! \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 - * FMT1000 series -*/ -enum SyncLineGmt -{ - XSLGMT_ClockIn = 0, //!< External clock sync \ref XSL_ClockIn - XSLGMT_GpsClockIn = 1, //!< GPS clock sync \ref XSL_GpsClockIn - XSLGMT_In = 2, //!< Sync in 1 \ref XSL_In1 - XSLGMT_BiIn = 3, //!< Bidirectional sync line, configured as input \ref XSL_Bi1In - XSLGMT_BiOut = 4, //!< Bidirectional sync line, configured as output \ref XSL_Bi1Out - XSLGMT_ReqData = 6, //!< Serial data sync option, use XMID_ReqData message id for this \ref XSL_ReqData - XSLGMT_Out = 7, //!< Sync out 1 \ref XSL_Out1 - - XSLGMT_Invalid -}; -/*! @} */ -typedef enum SyncLineGmt SyncLineGmt; - -#ifdef __cplusplus -extern "C" { -#endif - -XsSyncLine xslgmtToXsl(SyncLineGmt mk4Line); -SyncLineGmt xslToXslgmt(XsSyncLine line); - -#ifdef __cplusplus -} // extern "C" -#endif - -#endif diff --git a/extern/include/xcommunication/synclinemk4.h b/extern/include/xcommunication/synclinemk4.h deleted file mode 100644 index 791daab..0000000 --- a/extern/include/xcommunication/synclinemk4.h +++ /dev/null @@ -1,64 +0,0 @@ -/* Copyright (c) 2003-2017 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. -*/ - -#ifndef XSSYNCLINEMK4_H -#define XSSYNCLINEMK4_H - -#include - -/*! \addtogroup enums Global enumerations - @{ -*/ -/*! \brief Synchronization line identifiers for the Mk4 devices, only to be used directly in Xbus messages */ -enum SyncLineMk4 -{ - XSL4_ClockIn = 0, //!< External clock sync \ref XSL_ClockIn - XSL4_GpsClockIn = 1, //!< GPS clock sync \ref XSL_GpsClockIn - XSL4_In = 2, //!< Send data line \ref XSL_In1 - XSL4_BiIn = 3, //!< Bidirectional sync line, configured as input \ref XSL_Bi1In - XSL4_BiOut = 4, //!< Bidirectional sync line, configured as output \ref XSL_Bi1Out - XSL4_ExtTimepulseIn = 5,//!< External Timepulse input \ref XSL_ExtTimepulseIn - XSL4_ReqData = 6, //!< Serial data sync option, use XMID_ReqData message id for this \ref XSL_ReqData - - XSL4_Invalid -}; -/*! @} */ -typedef enum SyncLineMk4 SyncLineMk4; - -#ifdef __cplusplus -extern "C" { -#endif - -XsSyncLine xsl4ToXsl(SyncLineMk4 mk4Line); -SyncLineMk4 xslToXsl4(XsSyncLine line); - -#ifdef __cplusplus -} // extern "C" -#endif - -#endif diff --git a/extern/include/xcommunication/usbinterface.h b/extern/include/xcommunication/usbinterface.h deleted file mode 100644 index 494e59a..0000000 --- a/extern/include/xcommunication/usbinterface.h +++ /dev/null @@ -1,92 +0,0 @@ -/* Copyright (c) 2003-2017 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. -*/ - -#ifndef USBINTERFACE_H -#define USBINTERFACE_H - -#include - -#include -#include -#ifdef _WIN32 -# 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 -#endif - -#include "streaminterface.h" -#include - -struct XsPortInfo; - -class UsbInterfacePrivate; - -class UsbInterface : public StreamInterface { -public: - UsbInterface(); - ~UsbInterface(); - - XsResultValue open(const XsPortInfo &portInfo, XsFilePos readBufSize = 0, XsFilePos writeBufSize = 0, PortOptions = PO_XsensDefaults) override; - XsResultValue close(void); - XsResultValue closeUsb(void); - XsResultValue flushData (void); - - 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; - - void setRawIo(bool enable); - bool getRawIo(void); - - XsResultValue writeData(const XsByteArray& data, XsFilePos* written = NULL) override; - XsResultValue readData(XsFilePos maxLength, XsByteArray& data) override; - using IoInterface::waitForData; - - //lint -e1411 inherited definitions are also available (see above) - 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); - //lint +e1411 - - void getPortName(XsString& portname) const; - -private: - XSENS_DISABLE_COPY(UsbInterface) - UsbInterfacePrivate *d; -}; - -#endif // file guard diff --git a/extern/include/xcommunication/xcommunicationconfig.h b/extern/include/xcommunication/xcommunicationconfig.h deleted file mode 100644 index 36c016d..0000000 --- a/extern/include/xcommunication/xcommunicationconfig.h +++ /dev/null @@ -1,224 +0,0 @@ -/* Copyright (c) 2003-2017 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. -*/ - -#define XSENS_NO_AUTOLIB -// include this file in Visual Studio using C/C++->Advanced->Force Includes (the /FI option) -#ifndef XCOMMUNICATION_CONFIG_H -#define XCOMMUNICATION_CONFIG_H - -// define to build with Journaller -#ifndef HAVE_JOURNALLER -//#define HAVE_JOURNALLER -#endif - -////////////////////////////////////////////////// -// generic preprocessor defines - -// 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 -#endif - -#ifdef WIN32 -# ifndef _WIN32 -# define _WIN32 -# define XSENS_WINDOWS -# endif -#endif - -#ifdef _WIN32 -# define USE_WINUSB -#else -# 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 -#endif -#ifndef _CRT_SECURE_NO_WARNINGS -#define _CRT_SECURE_NO_WARNINGS -#endif - -#ifdef __GNUC__ -#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 -#else -# ifndef XSENS_32BIT -# define XSENS_32BIT -# endif -#endif - -// all xsens libraries should use unicode -#ifndef 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) - //// Debug or RelWithDeb build - #if defined(XSENS_RELEASE) - //// RelWithDeb build - #else - //// Debug build - #endif -#else - //// Release build -#endif -*/ - -#if defined(XSENS_DEBUG) && !defined(JLLOGLEVEL) - //// Debug or RelWithDeb build - //#define LOG_RX_TX // Lowest level byte receive and send (binary log) - //#define LOG_RX_TX_UNIQUE // Use unique file names for rx/tx logs - //#define LOG_RX_TX_FLUSH // Flush after each log operation (can cause hickups in timing, 300ms is not unheard of) - #define LOG_RX_TX_PER_STATE // Use new unique log file after switching to a new state (config/measurement/operational/recording) for rx/tx logs, override LOG_RX_TX_UNIQUE, automatically disabled if LOG_RX_TX is disabled - - #if defined(XSENS_RELEASE) - //// RelWithDeb build - #if !defined(QT_DEBUG) && !defined(QT_NO_DEBUG) - #define QT_NO_DEBUG 1 - #endif - #ifdef HAVE_JOURNALLER - //#define JLLOGLEVEL JLL_DEBUG - #define RUN_LL JLL_DEBUG // JLL_ALERT - #define DEBUG_LL JLL_ERROR - #endif - #else - //// Debug build - #if !defined(QT_DEBUG) && !defined(QT_NO_DEBUG) - #define QT_DEBUG 1 - #endif - #ifdef HAVE_JOURNALLER - //#define JLLOGLEVEL JLL_TRACE - #define RUN_LL JLL_DEBUG - #define DEBUG_LL JLL_ERROR - #endif - #endif -#else -# if !defined(JLLOGLEVEL) - //// Release build - #if !defined(QT_DEBUG) && !defined(QT_NO_DEBUG) - #define QT_NO_DEBUG 1 - #endif - #ifdef HAVE_JOURNALLER - //#define JLLOGLEVEL JLL_ALERT - #define RUN_LL JLL_ERROR - #define DEBUG_LL JLL_FATAL - #define JLNOLINEINFO - #endif -# endif -#endif - -#if defined(LOG_RX_TX_PER_STATE) && defined(LOG_RX_TX_UNIQUE) -#undef LOG_RX_TX_UNIQUE -#endif - -////////////////////////////////////////////////// -// more generic preprocessor defines -//! Add this macro to the start of a class definition to prevent automatic creation of copy functions -#define XSENS_DISABLE_COPY(className) \ -private: \ - className(const className &); \ - className &operator = (const className &); - -#ifdef __cplusplus -#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 -#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) - -#endif - -#endif - -#endif diff --git a/extern/include/xcommunication/xslibusb.h b/extern/include/xcommunication/xslibusb.h deleted file mode 100644 index 83f8bbd..0000000 --- a/extern/include/xcommunication/xslibusb.h +++ /dev/null @@ -1,126 +0,0 @@ -/* Copyright (c) 2003-2017 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. -*/ - -#ifndef XSLIBUSB -#define XSLIBUSB - -#ifdef HAVE_LIBUSB - -#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); - -struct XsLibraryLoader; - -class XsLibUsb -{ -public: - XsLibUsb(void); - ~XsLibUsb(void); - - 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; -private: - - 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); - } m_libUsb; - - XsLibraryLoader* m_libraryLoader; - - void tryLoadLibrary(); - void initLibrary(); -}; - -#endif // HAVE_LIBUSB - -#endif // file guard - diff --git a/extern/include/xcommunication/xsudev.h b/extern/include/xcommunication/xsudev.h deleted file mode 100644 index 102a7c5..0000000 --- a/extern/include/xcommunication/xsudev.h +++ /dev/null @@ -1,104 +0,0 @@ -/* Copyright (c) 2003-2017 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. -*/ - -#ifndef XSUDEV -#define XSUDEV - -struct udev; -struct udev_device; -struct udev_enumerate; -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); - -class XsUdev -{ -public: - XsUdev(void); - ~XsUdev(void); - - 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; - -private: - - 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); - } m_uDev; - - XsLibraryLoader* m_libraryLoader; - - void initLibrary(); -}; - -#endif // file guard - diff --git a/extern/include/xcommunication/xswinusb.h b/extern/include/xcommunication/xswinusb.h deleted file mode 100644 index 2eb3c2c..0000000 --- a/extern/include/xcommunication/xswinusb.h +++ /dev/null @@ -1,124 +0,0 @@ -/* Copyright (c) 2003-2017 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. -*/ - -#ifndef XSWINUSB -#define XSWINUSB - -#include "xcommunicationconfig.h" - -#ifdef USE_WINUSB - -#include -#include - -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); - -class XsWinUsb -{ -public: - XsWinUsb(void); - ~XsWinUsb(void); - - 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; - -private: - - 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_API; - - WINUSB_API m_winUsb; - XsLibraryLoader* m_libraryLoader; - - void initLibrary(); -}; - -#endif // USE_WINUSB - -#endif - diff --git a/extern/include/xsens/pstdint.h b/extern/include/xsens/pstdint.h deleted file mode 100644 index 4b52a7f..0000000 --- a/extern/include/xsens/pstdint.h +++ /dev/null @@ -1,869 +0,0 @@ -/* 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 - * - */ - -#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_. - */ - -#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 -#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) //lint !e553 __WORDSIZE should always be defined at this point -# 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(__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) - -#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) -//lint -e(417) -# 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. - */ - -#include -#include -#include - -#define glue3_aux(x,y,z) x ## y ## z -#define glue3(x,y,z) glue3_aux(x,y,z) - -#define DECLU(bits) glue3(uint,bits,_t) glue3(u,bits,=) glue3(UINT,bits,_C) (0); -#define DECLI(bits) glue3(int,bits,_t) glue3(i,bits,=) glue3(INT,bits,_C) (0); - -#define DECL(us,bits) glue3(DECL,us,) (bits) - -#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) -#ifdef INT64_MAX - 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); -#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); -#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); - - TESTUMAX(8); - TESTUMAX(16); - TESTUMAX(32); -#ifdef INT64_MAX - TESTUMAX(64); -#endif - - return EXIT_SUCCESS; -} - -#endif - -#if defined(_STDINT) && defined(_MSC_VER) -#define stdint_int64_defined -#endif - -#endif diff --git a/extern/include/xsens/xsanalogindata.h b/extern/include/xsens/xsanalogindata.h deleted file mode 100644 index 4c570d7..0000000 --- a/extern/include/xsens/xsanalogindata.h +++ /dev/null @@ -1,41 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSANALOGINDATA_H -#define XSANALOGINDATA_H - -#include "pstdint.h" - -/*! \brief Data from analog inputs from sensors. */ -struct XsAnalogInData { - uint16_t m_data; /*!< \brief The data */ -#ifdef __cplusplus - /*! \brief Construct a nulled analog data item */ - inline XsAnalogInData() : m_data(0) - {} - - /*! \brief Construct analog-data with value \a data */ - inline XsAnalogInData(uint16_t data) : m_data(data) - {} - - /*! \brief Equality comparison */ - inline bool operator == (XsAnalogInData const& other) const - { - return m_data == other.m_data; - } -#endif -}; -typedef struct XsAnalogInData XsAnalogInData; - -#endif diff --git a/extern/include/xsens/xsarray.h b/extern/include/xsens/xsarray.h deleted file mode 100644 index f78bbe1..0000000 --- a/extern/include/xsens/xsarray.h +++ /dev/null @@ -1,804 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSARRAY_H -#define XSARRAY_H - -#include "xstypesconfig.h" - -#define XSARRAY_DECL(T)\ - T* const m_data; /*!< Pointer to contained data buffer */\ - const XsSize m_size; /*!< Size of used data buffer in number of items */\ - const XsSize m_reserved; /*!< Size of allocated data buffer in number of items */\ - const int m_flags; /*!< Flags for data management */\ - XsArrayDescriptor const* const m_descriptor; /*!< Describes how to handle the items in the array */ - -#define XSARRAY_STRUCT(S,T) struct S { XSARRAY_DECL(T) } -#define XSARRAY_INITIALIZER(D) { 0, 0, 0, XSDF_Managed, D } - -/*! \cond XS_INTERNAL */ -typedef void (*XsArrayItemSwapFunc)(void*, void*); -typedef void (*XsArrayItemStructFunc)(void*); -typedef void (*XsArrayItemCopyFunc)(void*, void const*); -typedef int (*XsArrayItemCompareFunc)(void const*, void const*); //!< \brief Custom item compare function \return 0 means the items are equal, negative and positive values may be returned for sorting -typedef void (*XsArrayRawCopy)(void*, void const*, XsSize, XsSize); -#if defined(__GNUC__) || (defined(__arm__) && defined(__ARMCC_VERSION)) -#define XSEXPCASTITEMSWAP (XsArrayItemSwapFunc) -#define XSEXPCASTITEMMAKE (XsArrayItemStructFunc) -#define XSEXPCASTITEMCOPY (XsArrayItemCopyFunc) -#define XSEXPCASTITEMCOMP (XsArrayItemCompareFunc) -#define XSEXPCASTRAWCOPY (XsArrayRawCopy) -#else -#define XSEXPCASTITEMSWAP -#define XSEXPCASTITEMMAKE -#define XSEXPCASTITEMCOPY -#define XSEXPCASTITEMCOMP -#define XSEXPCASTRAWCOPY -#endif -/*! \endcond */ - -/*! \brief This object describes how to treat the data in an array. - \details Ususally there is one static instance per type of array that will be used by all - XsArrays of that type. -*/ -struct XsArrayDescriptor { -#ifndef __cplusplus - const -#else -protected: - template friend struct XsArrayImpl; -#endif - XsSize itemSize; //!< The size of an array item in bytes - void (*itemSwap)(void* a, void* b); //!< The function to use for swapping the data of two array items. \param a Pointer to first item to swap. \param b Pointer to second item to swap. - void (*itemConstruct)(void* e); //!< The function to use for constructing a new array item. May be 0 for simple types. \param e Pointer to item to construct. - void (*itemCopyConstruct)(void* e, void const* s); //!< The function to use for constructing a new array item with a source initializer. This may not be 0. \param e Pointer to item to construct. \param s Pointer to source item to copy from. - void (*itemDestruct)(void* e); //!< The function to use for destructing a array item. May be 0 for simple types. \param e Pointer to item to destruct. - void (*itemCopy)(void* to, void const* from); //!< The function to use for copying the data of \a from to \a to. \param to Pointer to item to copy to. \param from Pointer to item to copy from. - int (*itemCompare)(void const* a, void const* b); //!< The function to use for comparing two items. \param a Left hand side of comparison. \param b Right hand side of comparison. \returns The function will return 0 when the items are equal. When greater/less comparison is possible, the function should return < 0 if a < b and > 0 if a > b. - void (*rawCopy)(void* to, void const* from, XsSize count, XsSize iSize); //!< The function to use for copying the data of an array of \a from to \a to. \param to Pointer to array to copy to. \param from Pointer to array to copy from. \param count The number of items to copy. \param iSize The size of an individual item, should match the \a itemSize descriptor field. \note If this function pointer is 0, itemCopy will be used instead. \note XsArray_rawCopy can be used here for all types that can be safely memcpy'd. This typically means all means non-pointer types and structures containing only types matching these criteria. -}; -typedef struct XsArrayDescriptor XsArrayDescriptor; - -#ifdef __cplusplus -#include -#include -extern "C" { -#endif - -XSTYPES_DLL_API void XsArray_construct(void* thisPtr, XsArrayDescriptor const* const descriptor, XsSize count, void const* src); -XSTYPES_DLL_API void XsArray_copyConstruct(void* thisPtr, void const* src); -XSTYPES_DLL_API void XsArray_destruct(void* thisPtr); -XSTYPES_DLL_API void XsArray_assign(void* thisPtr, XsSize count, void const* src); -XSTYPES_DLL_API void XsArray_resize(void* thisPtr, XsSize count); -XSTYPES_DLL_API void XsArray_reserve(void* thisPtr, XsSize count); -XSTYPES_DLL_API void XsArray_copy(void* thisPtr, void const* src); -XSTYPES_DLL_API void XsArray_append(void* thisPtr, void const* other); -XSTYPES_DLL_API void XsArray_insert(void* thisPtr, XsSize index, XsSize count, void const* src); -XSTYPES_DLL_API void XsArray_erase(void* thisPtr, XsSize index, XsSize count); -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 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); -XSTYPES_DLL_API void XsArray_removeDuplicates(void* thisPtr); -XSTYPES_DLL_API void XsArray_removeDuplicatesPredicate(void* thisPtr, XsArrayItemCompareFunc predicate); -XSTYPES_DLL_API void XsArray_rawCopy(void* to, void const* from, XsSize count, XsSize iSize); -XSTYPES_DLL_API void XsArray_sort(void* thisPtr); -XSTYPES_DLL_API void XsArray_reverse(void* thisPtr); - -struct XsArray { - XSARRAY_DECL(void) -#ifdef __cplusplus - //! \copydoc XsArray_construct - inline XsArray(XsArrayDescriptor const* descriptor, XsSize count = 0, void const* src = 0) - : m_data(0) - , m_size(0) - , m_reserved(0) - , m_flags(0) - , m_descriptor(0) - { - XsArray_construct(this, descriptor, count, src); - } - - //! \copydoc XsArray_copyConstruct - inline XsArray(const XsArray& src) - : m_data(0) - , m_size(0) - , m_reserved(0) - , m_flags(0) - , m_descriptor(0) - { - XsArray_copyConstruct(this, &src); - } - - //! \brief Creates a array that references the data supplied in \a ref without allocating the data itself - inline explicit XsArray(XsArrayDescriptor const* descriptor, void* ref, XsSize count, XsDataFlags flags) - : m_data(ref) - , m_size(count) - , m_reserved(count) - , m_flags(flags) - , m_descriptor(descriptor) - { - } - - //! \brief Destructor - ~XsArray() - { - XsArray_destruct(this); - } - - /*! \brief Assignment operator - \details Copies the values in \a src into \a this - \param src The array to copy from - \return A reference to this - */ - inline XsArray const& operator=(const XsArray& src) - { - if (this != &src) - XsArray_copy(this, &src); - return *this; - } -#endif -}; - -typedef struct XsArray XsArray; - -#ifdef __cplusplus -} // extern "C" - -/*! \brief A C++ wrapper for XsArray, this is a template class for the actual specialized classes - \tparam T The type of the contained values - \tparam D The descriptor to use for this specific array implementation. This must be statically allocated and its lifetime must encompass the lifetime of objects that use it. - \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 { - //! \brief The contained type - typedef T value_type; - - //! \brief The type of size fields - typedef XsSize size_type; - - //! \brief A shorthand for the type of this specific implementation - typedef XsArrayImpl ArrayImpl; - - /*! \brief Construct an 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) - { - } - - //! \brief Constructs the XsArray as a copy of \a other - inline XsArrayImpl(ArrayImpl const& other) - : XsArray(other) - { - } -#ifndef XSENS_NOITERATOR - //! \brief Constructs the XsArray with a copy of the array bound by the supplied iterators \a beginIt and \a endIt - template - inline explicit XsArrayImpl(Iterator const& beginIt, Iterator const& endIt) - : XsArray(&D, 0, 0) - { - ptrdiff_t diff = endIt-beginIt; - if (diff > 0) - { - reserve((XsSize) diff); - for (Iterator it = beginIt; it != endIt; ++it) - push_back(*it); - } - } -#endif - //! \brief Creates the XsArray as a reference to the data supplied in \a ref - inline explicit XsArrayImpl(T* ref, XsSize sz, XsDataFlags flags = XSDF_None) - : XsArray(&D, ref, sz, flags) - { - } - - //! \copydoc XsArray_destruct - inline ~XsArrayImpl() - { - XsArray_destruct(this); - } - - //! \brief Clears the array \sa XsArray_destruct() - inline void clear() - { - XsArray_destruct(this); - } - -#ifndef SWIG - /*! \brief Tests \a other for equality according to \a predicate - \param other the array to compare against - \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); - } - - /*! \brief Tests \a other for equality according to \a predicate - \param other the array to compare against - \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); - } -#endif - - /*! \brief Tests \a other for equality - \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); - } - - /*! \brief Tests \a other for inequality - \param other the array to compare against - \returns true if the two arrays are not equal - \sa XsArray_compareArray - */ - inline bool operator != (ArrayImpl const& other) const - { - return !!XsArray_compare(this, &other); - } - - //! \copydoc XsArray_reserve - inline void reserve(XsSize count) - { - XsArray_reserve(this, count); - } - - //! \brief Returns the reserved space in number of items - inline XsSize reserved() const - { - return m_reserved; - } - - //! \brief Returns the XsArrayDescriptor describing the array - inline XsArrayDescriptor const& descriptor() const - { - return *m_descriptor; - } - -protected: -#ifndef XSENS_NOITERATOR - /*! \brief STL-style iterator */ - template - struct IteratorImplBase { - public: - //! \brief Difference between two items - typedef ptrdiff_t difference_type; - //! \brief The contained type - typedef T value_type; - //! \brief Type of pointer - typedef T* pointer; - //! \brief Type of reference - typedef T& reference; -#ifndef XSENS_NO_STL - //! \brief The category of this type of iterator (random access) - typedef std::random_access_iterator_tag iterator_category; -#endif - //! \brief The type of the inherited class that is actually this class - typedef Derived this_type; - //! \brief The direction of the iterator, +1 = forward, -1 = reverse - static const ptrdiff_t direction = F; - protected: - //! \brief Basic constructor - inline explicit IteratorImplBase(void* p = 0) : m_ptr((T*) p) {} - //! \brief Basic constructor - inline explicit IteratorImplBase(T* p) : m_ptr(p) {} - //! \brief Copy constructor - inline IteratorImplBase(this_type const& i) : m_ptr(i.m_ptr) {} - public: - //! \brief Assignment operator - inline this_type operator =(void* p) - { - m_ptr = (T*) p; - return this_type(m_ptr); - } - //! \brief Assignment operator - inline this_type operator =(T* p) - { - m_ptr = p; - return this_type(m_ptr); - } - //! \brief Assignment operator - inline this_type operator =(this_type const& i) - { - m_ptr = i.m_ptr; - return this_type(m_ptr); - } - //! \brief Prefix increment by one operator - inline this_type operator ++() - { - m_ptr = (T*) ptrAt(m_ptr, F); - return this_type(m_ptr); - } - //! \brief Postfix increment by one operator - inline this_type operator ++(int) - { - this_type p(m_ptr); - m_ptr = (T*) ptrAt(m_ptr, F); - return p; - } - //! \brief Prefix decrement by one operator - inline this_type operator --() - { - m_ptr = (T*) ptrAt(m_ptr, -F); - return this_type(m_ptr); - } - //! \brief Postfix decrement by one operator - inline this_type operator --(int) - { - this_type p(m_ptr); - m_ptr = (T*) ptrAt(m_ptr, -F); - return p; - } - //! \brief Increment by \a count operator - inline this_type operator +=(ptrdiff_t count) - { - m_ptr = ptrAt(m_ptr, F*count); - return this_type(m_ptr); - } - //! \brief Decrement by \a count operator - inline this_type operator -=(ptrdiff_t count) - { - m_ptr = ptrAt(m_ptr, -F*count); - return this_type(m_ptr); - } - //! \brief Addition by \a count operator - inline this_type operator +(ptrdiff_t count) const - { - 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)); - } - /*! \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 - difference may be negative if \a other is 'ahead' of this. The function takes the direction - of the iterator into consideration. - \param other The iterator to subtract from this. - \returns The difference between the two iterators. - */ - inline difference_type operator - (const this_type& other) const - { - 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; } - //! \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); } - //! \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); } - //! \brief Iterator comparison - 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); } - //! \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); } - //! \brief Dereferencing operator - inline R& operator *() const { return *(R*) ptr(); } - //! \brief Pointer operator - inline R* operator ->() const { return (R*) ptr(); } - //! \brief Access to internal pointer object, use should be avoided - inline T* ptr() const { return m_ptr; } - private: - //! \brief The internal pointer - T* m_ptr; - }; -#endif - -public: -#ifndef XSENS_NOITERATOR - /*! \brief A non-const iterator implementation */ - template - struct IteratorImpl : public IteratorImplBase > - { - private: - //! \brief A shorthand for the base object - typedef IteratorImplBase > ParentType; - public: - //! \brief Basic constructor - inline IteratorImpl(void* p = 0) : ParentType(p) {} - //! \brief Basic constructor - inline IteratorImpl(T* p) : ParentType(p) {} - //! \brief Copy constructor - inline IteratorImpl(const IteratorImpl& i) : ParentType(i) {} - }; - - /*! \brief A const iterator implementation */ - template - struct IteratorImplConst : public IteratorImplBase > - { - private: - //! \brief A shorthand for the base object - typedef IteratorImplBase > ParentType; - public: - //! \brief Basic constructor - inline IteratorImplConst(void* p = 0) : ParentType(p) {} - //! \brief Basic constructor - inline IteratorImplConst(T* p) : ParentType(p) {} - //! \brief Copy constructor - inline IteratorImplConst(IteratorImpl const& i) : ParentType(i.ptr()) {} - //! \brief Copy constructor - inline IteratorImplConst(IteratorImplConst const& i) : ParentType(i) {} - }; - - //! \brief STL-style mutable forward iterator - typedef IteratorImpl<1> iterator; - //! \brief STL-style mutable 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; - - /*! \brief STL-style const_iterator to the first data item in the array */ - 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(); } - - /*! \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(); } - /*! \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; } - - /*! \brief STL-style iterator to the first data item in the array */ - 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(); } - - /*! \brief STL-style reverse_iterator to the first data item in the reversed array */ - 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; } -#endif - /*! \brief indexed data access operator */ - inline T & operator[] (XsSize index) const - { - assert(index < m_size); - return *ptrAt(m_data, index); - } - /*! \brief indexed data access operator */ - inline T& operator[] (XsSize index) - { - assert(index < m_size); - 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 value). */ - inline T value(XsSize index) const - { -#ifdef XSENS_NO_EXCEPTIONS - assert(index >= m_size); -#else - if (index >= m_size) - throw std::out_of_range("index out of range"); -#endif - return *ptrAt(m_data, index); - } - /*! \brief Returns the first item in the array (by value). */ - inline T first() const - { -#ifdef XSENS_NO_EXCEPTIONS - assert(m_size); -#else - if (!m_size) - throw std::out_of_range("out of range"); -#endif - return *ptrAt(m_data, 0); - } - /*! \brief Returns the last item in the array (by value). */ - inline T last() const - { -#ifdef XSENS_NO_EXCEPTIONS - assert(m_size); -#else - if (!m_size) - throw std::out_of_range("out of range"); -#endif - 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 - { -#ifdef XSENS_NO_EXCEPTIONS - assert(index >= m_size); -#else - if (index >= m_size) - throw std::out_of_range("index out of range"); -#endif - return *ptrAt(m_data, 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) - { -#ifdef XSENS_NO_EXCEPTIONS - assert(index >= m_size); -#else - if (index >= m_size) - throw std::out_of_range("index out of range"); -#endif - return *ptrAt(m_data, index); - } - /*! \brief Insert \a item at \a index - \param item The item to insert - \param index The index to insert the item. When beyond the end of the array, the item is appended. - \sa XsArray_insert - */ - inline void insert(T const& item, XsSize index) - { - insert(&item, index, 1); - } - /*! \brief Insert \a item at \a index - \param items The items to insert - \param index The index to insert the items. When beyond the end of the array, the items are appended. - \param count The number of items to insert - \sa XsArray_insert - */ - inline void insert(T const* items, XsSize index, XsSize count) - { - XsArray_insert(this, index, count, items); - } - -#ifndef XSENS_NOITERATOR - /*! \brief Insert \a item before iterator \a it - \param item The item to insert - \param it The iterator before which to insert the item. - \sa XsArray_insert - */ - inline void insert(T const& item, const_iterator it) - { - insert(&item, indexOf(it), 1); - } - /*! \brief Insert \a item before iterator \a it - \param item The item to insert - \param it The iterator before which to insert the item. - \sa XsArray_insert - */ - inline void insert(T const& item, const_reverse_iterator it) - { - insert(&item, indexOf(it), 1); - } - - /*! \brief Insert \a item at \a index - \param items The items to insert - \param it The iterator before which to insert the item. - \param count The number of items to insert - \sa XsArray_insert - */ - inline void insert(T const* items, const_iterator it, XsSize count) - { - insert(items, indexOf(it), count); - } - /*! \brief Insert \a item at \a index - \param items The items to insert - \param it The iterator before which to insert the item. - \param count The number of items to insert - \sa XsArray_insert - */ - inline void insert(T const* items, const_reverse_iterator it, XsSize count) - { - insert(items, indexOf(it), count); - } -#endif - - /*! \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); - } - /*! \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); - else - 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) - { - insert(&item, 0, 1); - } - /*! \brief Removes \a count items from the start of the array \param count The number items to remove \sa XsArray_erase */ - inline void pop_front(XsSize count = 1) - { - erase(0, count); - } - /*! \brief Returns the number of items currently in the array - \returns The number of items currently in the array - \sa reserved \sa setSize \sa resize - */ - inline XsSize size() const - { - return m_size; - } - /*! \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. */ - inline void erase(XsSize index, XsSize count = 1) - { - XsArray_erase(this, index, count); - } -#ifndef XSENS_NOITERATOR - /*! \brief Removes the item at iterator \a it. \details \param it The item to remove. \returns An iterator pointing to the next item after the erased item. */ - inline iterator erase(iterator it) - { - XsSize idx = indexOf(it); - erase(idx, 1); - return (idx < size()) ? ptrAt(m_data, idx) : end(); - } - /*! \brief Removes the item at iterator \a it. \details \param it The item to remove. \returns An iterator pointing to the next item after the erased item. */ - inline reverse_iterator erase(reverse_iterator it) - { - XsSize idx = indexOf(it); - erase(idx, 1); - return (idx < size()) ? ptrAt(m_data, idx) : rend(); - } -#endif - /*! \copydoc XsArray_assign \sa XsArray_assign */ - inline void assign(XsSize count, T const* src) - { - XsArray_assign(this, count, src); - } - /*! \copydoc XsArray_resize \sa XsArray_resize */ - inline void resize(XsSize count) - { - XsArray_resize(this, count); - } - /*! \brief Set the size of the array to \a count. \details The contents of the array after this operation are undefined. \param count The desired new size fo the array. \sa XsArray_assign \sa reserve \sa resize */ - inline void setSize(XsSize count) - { - if (count != m_size) - XsArray_assign(this, count, 0); - } - /*! \copydoc XsArray_append \sa XsArray_append */ - inline void append(ArrayImpl const& other) - { - XsArray_append(this, &other); - } - /*! \brief Assignment operator, copies \a other into this, overwriting the old contents \param other The array to copy from \returns A reference to this \sa XsArray_copy */ - inline ArrayImpl& operator=(ArrayImpl const& other) - { - if (this != &other) - XsArray_copy(this, &other); - 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 - { - 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); } - - /*! \brief Return the inheriting object */ - 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*/ - inline void swap(ArrayImpl& other) - { - XsArray_swap(this, &other); - } - - /*! \brief Append \a item in a stream-like manner. - \details This allows for constructing XsArrays easily like this: - XsInt64Array array = XsInt64Array() << 1 << 2 << 3; - \param item The item to append to the array. - \returns A reference to the modified array - \sa push_back - */ -#ifndef XSENS_NOITERATOR - inline I& operator <<(T const& item) - { - push_back(item); - return inherited(); - } -#endif - - /*! \copydoc XsArray_find */ - inline int find(T const& needle) const - { - return XsArray_find(this, &needle); - } -#ifndef SWIG - /*! \copydoc XsArray_findPredicate */ - inline int findPredicate(T const& needle, XsArrayItemCompareFunc predicate) const - { - return XsArray_findPredicate(this, &needle, predicate); - } -#endif -#ifndef XSENS_NOITERATOR - /*! \brief Returns the linear index of \a it in the array - \param it The iterator to analyze - \returns The linear forward index of the item pointed to by \a it. If \a it points to before the - 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 - { - ptrdiff_t d = ((char const*) it.ptr() - (char const*) m_data); - if (d >= 0) - { - XsSize r = d/D.itemSize; - if (r <= size()) - return r; - return size(); - } - return 0; - } -#endif - - /*! \copydoc XsArray_removeDuplicates */ - inline void removeDuplicates() - { - XsArray_removeDuplicates(this); - } - - /*! \copydoc XsArray_removeDuplicatesPredicate */ - inline void removeDuplicatesPredicate(XsArrayItemCompareFunc predicate) - { - XsArray_removeDuplicatesPredicate(this, predicate); - } - - /*! \copydoc XsArray_sort */ - inline void sort() - { - XsArray_sort(this); - } - - /*! \copydoc XsArray_reverse */ - inline void reverse() - { - XsArray_reverse(this); - } - -private: - /*! \internal - \brief Generic pointer movement function - \details This function adds \a count items to \a ptr based on the size specified in \a descriptor - \param ptr The pointer to start from - \param count The number of items to move up or down. count may be negative - \returns The requested pointer. - \note The return value loses its constness, take care when using this function directly. In most cases - it should not be necessary to use this function in user code. - */ - inline static T* ptrAt(void const* ptr, ptrdiff_t count) - { - return (T*)(void*)(((char*)ptr)+count*D.itemSize); - } -}; -#endif - -#endif diff --git a/extern/include/xsens/xsatomicint.h b/extern/include/xsens/xsatomicint.h deleted file mode 100644 index 4367de2..0000000 --- a/extern/include/xsens/xsatomicint.h +++ /dev/null @@ -1,114 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSATOMICINT_H -#define XSATOMICINT_H - -#include "xstypesconfig.h" - -#ifdef XSENS_64BIT -typedef long long XsAtomicIntValue; -#else -typedef long XsAtomicIntValue; -#endif -struct XsAtomicInt; -#ifndef __cplusplus -#define XSATOMICINT_INITIALIZER { 0 } -#endif - -#ifdef _MSC_VER -#include - #ifdef XSENS_64BIT - #define atomicIncrement(a) InterlockedIncrement64(&a) - #define atomicDecrement(a) InterlockedDecrement64(&a) - #else - #define atomicIncrement(a) InterlockedIncrement(&a) - #define atomicDecrement(a) InterlockedDecrement(&a) - #endif -#else - #ifdef XSENS_64BIT - #define atomicIncrement(a) __sync_add_and_fetch(&a, 1) - #define atomicDecrement(a) __sync_sub_and_fetch(&a, 1) - #else - #define atomicIncrement(a) (++a) - #define atomicDecrement(a) (--a) - #endif -#endif - -/*! \relates XsAtomicInt \brief Increase the value by one unit (prefix notation). */ -#define XsAtomicInt_preIncrement(a) ((XsAtomicIntValue) atomicIncrement((a)->m_value)) - -/*! \relates XsAtomicInt \brief Decrease the value by one unit (prefix notation). */ -#define XsAtomicInt_preDecrement(a) ((XsAtomicIntValue) atomicDecrement((a)->m_value)) - -/*! \relates XsAtomicInt \brief Increase the value by one unit (postfix notation). */ -#define XsAtomicInt_postIncrement(a) ((XsAtomicIntValue) atomicIncrement((a)->m_value) - 1) - -/*! \relates XsAtomicInt \brief Decrease the value by one unit (postfix notation). */ -#define XsAtomicInt_postDecrement(a) ((XsAtomicIntValue) atomicDecrement((a)->m_value) + 1) - -/*! \class XsAtomicInt - \brief Wrapper class for easy use of XsAtomicIntValue values -*/ -struct XsAtomicInt { - volatile XsAtomicIntValue m_value; //!< The actual value - -#ifdef __cplusplus - /*! \brief Initialize the value. */ - XsAtomicInt(XsAtomicIntValue val = 0) - : m_value(val) - {} - - /*! \brief Set the value to the given \a val. */ - inline void setValue(XsAtomicIntValue val) - { - m_value = val; - } - - /*! \brief Get the current value. */ - inline XsAtomicIntValue value() const - { - return (XsAtomicIntValue) m_value; - } - - /*! \brief Increase the value by one unit (prefix). */ - inline XsAtomicIntValue operator++() - { - return XsAtomicInt_preIncrement(this); - } - - /*! \brief Decrease the value by one unit (prefix). */ - inline XsAtomicIntValue operator--() - { - return XsAtomicInt_preDecrement(this); - } - - /*! \brief Increase the value by one unit (postfix). */ - inline XsAtomicIntValue operator++(int) - { - return XsAtomicInt_postIncrement(this); - } - - /*! \brief Decrease the value by one unit (postfix). */ - inline XsAtomicIntValue operator--(int) - { - return XsAtomicInt_postDecrement(this); - } - -#endif // __cplusplus -}; - -typedef struct XsAtomicInt XsAtomicInt; - -#endif diff --git a/extern/include/xsens/xsbaud.h b/extern/include/xsens/xsbaud.h deleted file mode 100644 index 7de313e..0000000 --- a/extern/include/xsens/xsbaud.h +++ /dev/null @@ -1,85 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSBAUD_H -#define XSBAUD_H - -#include "xstypesconfig.h" - - -/*! \addtogroup enums Global enumerations - @{ -*/ - -#include "xsbaudcode.h" -#include "xsbaudrate.h" - -/*! @} */ - -typedef enum XsBaudCode XsBaudCode; -typedef enum XsBaudRate XsBaudRate; - -#ifdef __cplusplus -extern "C" { -#endif - -XSTYPES_DLL_API XsBaudRate XsBaud_codeToRate(XsBaudCode baudcode); -XSTYPES_DLL_API XsBaudCode XsBaud_rateToCode(XsBaudRate baudrate); -XSTYPES_DLL_API int XsBaud_rateToNumeric(XsBaudRate baudrate); -XSTYPES_DLL_API XsBaudRate XsBaud_numericToRate(int numeric); - -#ifdef __cplusplus -} // extern "C" - -/*! \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); - } -} - -#ifndef XSENS_NO_STL -#include - -namespace std { -template -basic_ostream<_CharT, _Traits>& operator<<(basic_ostream<_CharT, _Traits>& o, XsBaudRate const& xd) -{ - return (o << XsBaud::rateToNumeric(xd)); -} -} -#endif - -#endif - -#endif diff --git a/extern/include/xsens/xsbaudcode.h b/extern/include/xsens/xsbaudcode.h deleted file mode 100644 index 5e32656..0000000 --- a/extern/include/xsens/xsbaudcode.h +++ /dev/null @@ -1,44 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSBAUDCODE_H -#define XSBAUDCODE_H - -#include "xstypesconfig.h" - -/*! \brief Internal baud rate configuration codes -*/ - -enum XSNOCOMEXPORT XsBaudCode { - // Baudrate codes for SetBaudrate message - XBC_4k8 = 0x0B, //!< 4k8 (4800 bps) - XBC_9k6 = 0x09, //!< 9k6 (9600 bps) -// XBC_14k4 = 0x08, - XBC_19k2 = 0x07, //!< 19k2 (19200 bps) -// XBC_28k8 = 0x06, - XBC_38k4 = 0x05, //!< 38k4 (38400 bps) - XBC_57k6 = 0x04, //!< 57k6 (57600 bps) -// XBC_76k8 = 0x03, - XBC_115k2 = 0x02, //!< 115k2 (115200 bps) - XBC_230k4 = 0x01, //!< 230k4 (230400 bps) - XBC_460k8 = 0x00, //!< 460k8 (460800 bps) - XBC_921k6 = 0x0A, //!< 921k6 (921600 bps). Only usable from MTi/x FW 2.4.6 - XBC_921k6Legacy = 0x80, //!< 921k6 (921600 bps) - XBC_2MegaBaud = 0x0C, //!< 2000k0 (2000000 bps) - XBC_3_5MegaBaud = 0x0E, //!< 3500k0 (3500000 bps) - XBC_4MegaBaud = 0x0D, //!< 4000k0 (4000000 bps) - XBC_Invalid = 0xFF //!< Not a valid baud rate -}; - -#endif diff --git a/extern/include/xsens/xsbaudrate.h b/extern/include/xsens/xsbaudrate.h deleted file mode 100644 index 052bd7d..0000000 --- a/extern/include/xsens/xsbaudrate.h +++ /dev/null @@ -1,98 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSBAUDRATE_H -#define XSBAUDRATE_H - -#ifdef _WIN32 -# include -#else -# include -# ifndef B460800 -# undef B230400 -# define B230400 0010003 -# define B460800 0010004 -# define B921600 0010007 -# endif -#endif - -/*! \addtogroup enums Global enumerations - @{ -*/ -/*! \brief Communication speed. */ -#ifdef _WIN32 - -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_19k2 = CBR_19200, //!< 19k2 (19200 bps) -// XBR_28k8 = 28800, - XBR_38k4 = CBR_38400, //!< 38k4 (38400 bps) - XBR_57k6 = CBR_57600, //!< 57k6 (57600 bps) - XBR_115k2 = CBR_115200, //!< 115k2 (115200 bps) - XBR_230k4 = 230400, //!< 230k4 (230400 bps) - XBR_460k8 = 460800, //!< 460k8 (460800 bps) - XBR_921k6 = 921600, //!< 921k6 (921600 bps) - XBR_2000k = 2000000, //!< 2000k0 (2000000 bps) - XBR_3500k = 3500000, //!< 3500k0 (3500000 bps) - XBR_4000k = 4000000 //!< 4000k0 (4000000 bps) -}; - -#else - -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 -#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_19k2 = B19200, //!< 19k2 (19200 bps) -// XBR_28k8 = B28800, - XBR_38k4 = B38400, //!< 38k4 (38400 bps) - XBR_57k6 = B57600, //!< 57k6 (57600 bps) - XBR_115k2 = B115200, //!< 115k2 (115200 bps) - XBR_230k4 = B230400, //!< 230k4 (230400 bps) - XBR_460k8 = B460800, //!< 460k8 (460800 bps) - XBR_921k6 = B921600, //!< 921k6 (921600 bps) -#ifndef SWIG - XBR_2000k = B2000000, //!< 2000k0 (2000000 bps) - XBR_3500k = B3500000, //!< 3500k0 (3500000 bps) - XBR_4000k = B4000000 //!< 4000k0 (4000000 bps) -#else - XBR_2000k = 2000000, //!< 2000k0 (2000000 bps) - XBR_3500k = 3500000, //!< 3500k0 (3500000 bps) - XBR_4000k = 4000000 //!< 4000k0 (4000000 bps) -#endif -}; -/*! @} */ -typedef enum XsBaudRate XsBaudRate; -#endif - -#endif diff --git a/extern/include/xsens/xsbusid.h b/extern/include/xsens/xsbusid.h deleted file mode 100644 index 2117b0c..0000000 --- a/extern/include/xsens/xsbusid.h +++ /dev/null @@ -1,41 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSBUSID_H -#define XSBUSID_H - -/*! \addtogroup cinterface C Interface - @{ -*/ - -/*! \brief The bus identifier of the master device */ -#define XS_BID_MASTER 0xFF - -/*! \brief The bus broadcast bus identifier (all devices) */ -#define XS_BID_BROADCAST 0x00 - -/*! \brief The bus identifier for the first MT on the bus */ -#define XS_BID_MT 0x01 - -/*! \brief An invalid bus identifier - \note This value may change between API releases as it is not strictly part of the Xbus protocol but only used internally -*/ -#define XS_BID_INVALID 0xFD - -/*! \brief A dynamic bus identifier */ -#define XS_BID_DYNAMIC 0xFE - -/*! @} */ - -#endif diff --git a/extern/include/xsens/xsbytearray.h b/extern/include/xsens/xsbytearray.h deleted file mode 100644 index 9e10e79..0000000 --- a/extern/include/xsens/xsbytearray.h +++ /dev/null @@ -1,132 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSBYTEARRAY_H -#define XSBYTEARRAY_H - -#include "xsarray.h" -#include "pstdint.h" - -#ifdef __cplusplus -#include "xsstring.h" -extern "C" { -#endif - -extern XsArrayDescriptor const XSTYPES_DLL_API g_xsByteArrayDescriptor; - -#ifndef __cplusplus -#define XSBYTEARRAY_INITIALIZER XSARRAY_INITIALIZER(&g_xsByteArrayDescriptor) -XSARRAY_STRUCT(XsByteArray, uint8_t); -typedef struct XsByteArray XsByteArray; -XSTYPES_DLL_API void XsByteArray_construct(XsByteArray* thisPtr, XsSize count, uint8_t const* src); - -// obsolete: -#define XsByteArray_ref(thisPtr, sz, src, flags) XsArray_ref(thisPtr, sz, src, flags) -#define XsByteArray_assign(thisPtr, sz, src) XsArray_assign(thisPtr, sz, src) -#define XsByteArray_destruct(thisPtr) XsArray_destruct(thisPtr) -#define XsByteArray_copy(thisPtr, copy) XsArray_copy(copy, thisPtr) -#define XsByteArray_append(thisPtr, other) XsArray_append(thisPtr, other) -#define XsByteArray_popFront(thisPtr, count) XsArray_erase(thisPtr, 0, count) -#define XsByteArray_popBack(thisPtr, count) XsArray_erase(thisPtr, (XsSize)-1, count) -#define XsByteArray_fromString(str, copy) XsArray_assign(copy, str->m_size?str->m_size:1, str->m_size?str->m_data:"\0") -#define XsByteArray_swap(a, b) XsArray_swap(a, b) -#define XsByteArray_erase(thisPtr, index, count) XsArray_erase(thisPtr, index, count) - -#else -} // extern "C" -#endif - -#ifdef __cplusplus -struct XsByteArray : public XsArrayImpl { - //! \brief Constructs an XsByteArray - inline explicit XsByteArray(XsSize sz = 0, uint8_t const* src = 0) - : ArrayImpl(sz, src) - { - } - - //! \brief Constructs an XsByteArray as a copy of \a other - inline XsByteArray(XsByteArray const& other) - : ArrayImpl(other) - { - } - - //! \brief Constructs an XsByteArray that references the data supplied in \a ref - inline explicit XsByteArray(uint8_t* ref, XsSize sz, XsDataFlags flags = XSDF_None) - : ArrayImpl(ref, sz, flags) - { - } -#ifndef XSENS_NOITERATOR - //! \brief Constructs an XsByteArray with the array bound by the supplied iterators \a beginIt and \a endIt - template - inline XsByteArray(Iterator beginIt, Iterator endIt) - : ArrayImpl(beginIt, endIt) - { - } -#endif - //! \brief Constructs an XsByteArray as a copy of the supplied XsString, including the terminating 0 - inline XsByteArray(XsString const& src) - : ArrayImpl() - { - 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 ->(); } - - //! \brief Return a pointer to the internal data buffer - inline uint8_t const* data() const { return begin().operator ->(); } - - /*! \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 - POD type or structure with a trivial default constructor, since its contents will be completely - overwritten by a memcpy - \param offset The offset of the first byte to convert (in byte units, not in T units) - \returns The converted value - */ - template - inline T getValue(XsSize offset) const - { - assert(offset+sizeof(T) <= size()); - T tmp; - memcpy(&tmp, data()+offset, sizeof(T)); - return tmp; - } - - /*! \brief Append \a value T to the byte array - \param value The value to append to the array. The type T needs to be a POD structure or a primitive type - since it gets copied into the array using memcpy. - */ - template - inline void appendValue(T const& value) - { - XsSize offset = size(); - resize(offset + sizeof(T)); - memcpy(data()+offset, &value, sizeof(T)); - } - - /*! \brief Set \a value T in the byte array at byte position \a offset - \param value The value to append to the array. The type T needs to be a POD structure or a primitive type - since it gets copied into the array using memcpy. - \param offset The position of the first byte to write. The array will be enlarged if necessary - */ - template - inline void setValue(T const& value, XsSize offset) - { - if (size() < offset + sizeof(T)) - resize(offset + sizeof(T)); - memcpy(data()+offset, &value, sizeof(T)); - } -}; -#endif -#endif diff --git a/extern/include/xsens/xscalibrateddata.h b/extern/include/xsens/xscalibrateddata.h deleted file mode 100644 index 7344fbe..0000000 --- a/extern/include/xsens/xscalibrateddata.h +++ /dev/null @@ -1,74 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSCALIBRATEDDATA_H -#define XSCALIBRATEDDATA_H - -#include "xstypesconfig.h" -#include "xsvector3.h" - -struct XsCalibratedData; - -#ifdef __cplusplus -extern "C" { -#else -#define XSCALIBRATEDDATA_INITIALIZER {XSVECTOR3_INITIALIZER, XSVECTOR3_INITIALIZER, XSVECTOR3_INITIALIZER} -#endif - -XSTYPES_DLL_API void XsCalibratedData_construct(struct XsCalibratedData* thisPtr, const XsReal* acc, const XsReal* gyr, const XsReal* mag); -XSTYPES_DLL_API void XsCalibratedData_destruct(struct XsCalibratedData* thisPtr); - -#ifdef __cplusplus -} // extern "C" -#endif - -struct XsCalibratedData -{ - XsVector3 m_acc; //!< Accelerometer data - XsVector3 m_gyr; //!< Gyroscope data - XsVector3 m_mag; //!< Magnetometer data - -#ifdef __cplusplus - //! \brief Constructor \sa XsCalibratedData_construct - inline XsCalibratedData() - {} - - //! \brief Copy constructor, copies the values from \a other to this - inline XsCalibratedData(const XsCalibratedData& other) - : m_acc(other.m_acc) - , m_gyr(other.m_gyr) - , m_mag(other.m_mag) - { - } - - //! \brief Destructor - inline ~XsCalibratedData() - {} - - //! \brief Assignment operator, copies the values from \a other to this - inline const XsCalibratedData& operator = (const XsCalibratedData& other) - { - if (this != &other) - { - m_acc = other.m_acc; - m_gyr = other.m_gyr; - m_mag = other.m_mag; - } - return *this; - } -#endif -}; -typedef struct XsCalibratedData XsCalibratedData; - -#endif diff --git a/extern/include/xsens/xscontrolline.h b/extern/include/xsens/xscontrolline.h deleted file mode 100644 index 33b6513..0000000 --- a/extern/include/xsens/xscontrolline.h +++ /dev/null @@ -1,36 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSCONTROLLINE_H -#define XSCONTROLLINE_H - -/*! \addtogroup enums Global enumerations - @{ -*/ -/*! \brief Serial control lines. */ -enum XsControlLine { - XCL_DCD = 0x0001, //!< pin 1: Carrier Detect - XCL_RD = 0x0002, //!< pin 2: Received Data - XCL_TD = 0x0004, //!< pin 3: Transmitted Data - XCL_DTR = 0x0008, //!< pin 4: Data Terminal Ready - XCL_GND = 0x0010, //!< pin 5: Common Ground - XCL_DSR = 0x0020, //!< pin 6: Data Set Ready - XCL_RTS = 0x0040, //!< pin 7: Request To Send - XCL_CTS = 0x0080, //!< pin 8: Clear To Send - XCL_RI = 0x0100 //!< pin 9: Ring Indicator -}; -/*! @} */ -typedef enum XsControlLine XsControlLine; - -#endif diff --git a/extern/include/xsens/xscopy.h b/extern/include/xsens/xscopy.h deleted file mode 100644 index 25958c3..0000000 --- a/extern/include/xsens/xscopy.h +++ /dev/null @@ -1,47 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSCOPY_H -#define XSCOPY_H - -#define XSLISTCOPY(C) \ - if (copy == thisPtr)\ - {\ - return;\ - }\ - C##_assign(copy, thisPtr->m_size, thisPtr->m_data); - -#define XSLISTSWAP3(C, B, S) \ - if ((!a->m_data || (a->m_flags & XSDF_Managed)) && (!b->m_data || (b->m_flags & XSDF_Managed))) {\ - B tmp;\ - *((C**) &tmp.m_data) = a->m_data;\ - *((XsSize*) &tmp.m_size) = a->m_size;\ - *((int*) &tmp.m_flags) = a->m_flags;\ - *((C**) &a->m_data) = b->m_data;\ - *((XsSize*) &a->m_size) = b->m_size;\ - *((int*) &a->m_flags) = b->m_flags;\ - *((C**) &b->m_data) = tmp.m_data;\ - *((XsSize*) &b->m_size) = tmp.m_size;\ - *((int*) &b->m_flags) = tmp.m_flags;\ - } else { /* elementwise swap */ \ - XsSize i;\ - assert(a->m_size == b->m_size);\ - for (i = 0; i < a->m_size; ++i) S(&a->m_data[i], &b->m_data[i]);\ - } - -#define XSLISTSWAP2(C, B) XSLISTSWAP3(C, B, C##_swap) - -#define XSLISTSWAP(C) XSLISTSWAP2(C, C##List) - -#endif diff --git a/extern/include/xsens/xsdataformat.h b/extern/include/xsens/xsdataformat.h deleted file mode 100644 index 218d812..0000000 --- a/extern/include/xsens/xsdataformat.h +++ /dev/null @@ -1,57 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSDATAFORMAT_H -#define XSDATAFORMAT_H - -#include "xstypesconfig.h" -#include "xsoutputmode.h" -#include "xsoutputsettings.h" - -/*! \brief A structure for storing data formats. */ -struct XsDataFormat { - XsOutputMode m_outputMode; //!< The stored output mode - XsOutputSettings m_outputSettings; //!< The stored output settings - -#ifdef __cplusplus - /*! \brief Construct an XsDataFormat specifier - - \param mode the outputmode - \param settings the output settings - */ - inline explicit XsDataFormat(const XsOutputMode mode = XS_DEFAULT_OUTPUT_MODE, const XsOutputSettings settings = XS_DEFAULT_OUTPUT_SETTINGS) - : m_outputMode(mode), m_outputSettings(settings) {} - - //! Copy constructor - inline XsDataFormat(const XsDataFormat& other) : m_outputMode(other.m_outputMode), m_outputSettings(other.m_outputSettings) {} - - //! Assignment operator - inline const XsDataFormat& operator = (const XsDataFormat& other) - { - //lint --e{1529} assignment to self ok - m_outputMode = other.m_outputMode; - m_outputSettings = other.m_outputSettings; - return *this; - } - - //! Equality operator - inline bool operator == (const XsDataFormat& other) const - { - return m_outputMode == other.m_outputMode && m_outputSettings == other.m_outputSettings; - } -#endif -}; - -#endif - diff --git a/extern/include/xsens/xsdataidentifier.h b/extern/include/xsens/xsdataidentifier.h deleted file mode 100644 index 07ab15b..0000000 --- a/extern/include/xsens/xsdataidentifier.h +++ /dev/null @@ -1,166 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSDATAIDENTIFIER_H -#define XSDATAIDENTIFIER_H - -#define XDI_MAX_FREQUENCY ((uint16_t) 0xFFFF) - -////////////////////////////////////////////////////////////////////////////////////////// -/*! \addtogroup enums Global enumerations - @{ -*/ - -/*! \enum XsDataIdentifier - \brief Defines the data identifiers - - The list of standard data identifiers is shown below. - The last positions in the data identifier depends on the configuration of the data. - For example 0x4020 is 3D acceleration in single precision float format, - where 0x4022 is 3D acceleration in fixed point 16.32 format. - - Refer to the Low Level Communication Protocol for more details. -*/ -enum XsDataIdentifier -{ - XDI_None = 0x0000, //!< Empty datatype - XDI_TypeMask = 0xFE00, //!< Mask for checking the group which a dataidentifier belongs to, Eg. XDI_TimestampGroup or XDI_OrientationGroup - XDI_FullTypeMask = 0xFFF0, //!< Mask to get the type of data, without the data format - XDI_FullMask = 0xFFFF, //!< Complete mask to get entire data identifier - XDI_FormatMask = 0x01FF, //!< Mask for getting the data id without checking the group - XDI_DataFormatMask = 0x000F, //!< Mask for extracting just the data format /sa XDI_SubFormat - - XDI_SubFormatMask = 0x0003, //!< Determines, float, fp12.20, fp16.32, double output... (where applicable) - XDI_SubFormatFloat = 0x0000, //!< Floating point format - XDI_SubFormatFp1220 = 0x0001, //!< Fixed point 12.20 - XDI_SubFormatFp1632 = 0x0002, //!< Fixed point 16.32 - XDI_SubFormatDouble = 0x0003, //!< Double format - - XDI_TemperatureGroup = 0x0800, //!< Group for temperature outputs - XDI_Temperature = 0x0810, //!< Temperature - - XDI_TimestampGroup = 0x1000, //!< Group for time stamp related outputs - XDI_UtcTime = 0x1010, //!< Utc time from the GPS receiver - XDI_PacketCounter = 0x1020, //!< Packet counter, increments every packet - XDI_Itow = 0x1030, //!< Itow. Time Of Week from the GPS receiver - XDI_GpsAge = 0x1040, //!< Age of Gps sample \deprecated Replaced by XDI_GnssAge - XDI_GnssAge = 0x1040, //!< Gnss age from the GPS receiver - XDI_PressureAge = 0x1050, //!< Age of a pressure sample, in packet counts - XDI_SampleTimeFine = 0x1060, //!< Sample Time Fine - XDI_SampleTimeCoarse = 0x1070, //!< Sample Time Coarse - XDI_FrameRange = 0x1080, //!< Reserved \internal add for MTw (if needed) - XDI_PacketCounter8 = 0x1090, //!< 8 bit packet counter, wraps at 256 - XDI_SampleTime64 = 0x10A0, //!< 64 bit sample time - - XDI_OrientationGroup = 0x2000, //!< Group for orientation related outputs - XDI_CoordSysMask = 0x000C, //!< Mask for the coordinate system part of the orientation data identifier - XDI_CoordSysEnu = 0x0000, //!< East North Up orientation output - XDI_CoordSysNed = 0x0004, //!< North East Down orientation output - XDI_CoordSysNwu = 0x0008, //!< North West Up orientation output - XDI_Quaternion = 0x2010, //!< Orientation in quaternion format - XDI_RotationMatrix = 0x2020, //!< Orientation in rotation matrix format - XDI_EulerAngles = 0x2030, //!< Orientation in euler angles format - - XDI_PressureGroup = 0x3000, //!< Group for pressure related outputs - XDI_BaroPressure = 0x3010, //!< Pressure output recorded from the barometer - - XDI_AccelerationGroup = 0x4000, //!< Group for acceleration related outputs - XDI_DeltaV = 0x4010, //!< DeltaV SDI data output - XDI_Acceleration = 0x4020, //!< Acceleration output in m/s2 - XDI_FreeAcceleration = 0x4030, //!< Free acceleration output in m/s2 - XDI_AccelerationHR = 0x4040, //!< AccelerationHR output - - XDI_PositionGroup = 0x5000, //!< Group for position related outputs - XDI_AltitudeMsl = 0x5010, //!< Altitude at Mean Sea Level - XDI_AltitudeEllipsoid = 0x5020, //!< Altitude at ellipsoid - XDI_PositionEcef = 0x5030, //!< Position in earth-centered, earth-fixed format - XDI_LatLon = 0x5040, //!< Position in latitude, longitude - - XDI_SnapshotGroup = 0xC800, //!< Group for snapshot related outputs - XDI_RetransmissionMask = 0x0001, //!< Mask for the retransmission bit in the snapshot data - XDI_RetransmissionFlag = 0x0001, //!< Bit indicating if the snapshot if from a retransmission - XDI_AwindaSnapshot = 0xC810, //!< Awinda type snapshot - XDI_FullSnapshot = 0xC820, //!< Full snapshot - - XDI_GnssGroup = 0x7000, //!< Group for Gnss related outputs - XDI_GnssPvtData = 0x7010, //!< Gnss position, velocity and time data - XDI_GnssSatInfo = 0x7020, //!< Gnss satellite information - - XDI_AngularVelocityGroup = 0x8000, //!< Group for angular velocity related outputs - XDI_RateOfTurn = 0x8020, //!< Rate of turn data in rad/sec - XDI_DeltaQ = 0x8030, //!< DeltaQ SDI data - XDI_RateOfTurnHR = 0x8040, //!< Rate of turn HR data - - XDI_GpsGroup = 0x8800, //!< Group for GPS only related data \deprecated Replaced by XDI_GnssGroup - XDI_GpsDop = 0x8830, //!< Gps dilution of precision data \deprecated - XDI_GpsSol = 0x8840, //!< Gps navigation solution information \deprecated Replaced by XDI_GnssPvtData - XDI_GpsTimeUtc = 0x8880, //!< Gps time in UTC format \deprecated Replaced by XDI_GnssPvtData - XDI_GpsSvInfo = 0x88A0, //!< Gps satellite vehicle information \deprecated Replaced by XDI_GnssSatInfo - - XDI_RawSensorGroup = 0xA000, //!< Group for raw sensor data related outputs - XDI_RawUnsigned = 0x0000, //!< Tracker produces unsigned raw values, usually fixed behavior - XDI_RawSigned = 0x0001, //!< Tracker produces signed raw values, usually fixed behavior - XDI_RawAccGyrMagTemp = 0xA010, //!< Raw acceleration, gyroscope, magnetometer and temperature data - XDI_RawGyroTemp = 0xA020, //!< Raw gyroscope and temperature data - XDI_RawAcc = 0xA030, //!< Raw acceleration data - XDI_RawGyr = 0xA040, //!< Raw gyroscope data - XDI_RawMag = 0xA050, //!< Raw magnetometer data - XDI_RawDeltaQ = 0xA060, //!< Raw deltaQ SDI data - XDI_RawDeltaV = 0xA070, //!< Raw deltaV SDI data - XDI_RawBlob = 0xA080, //!< Raw blob data - - XDI_AnalogInGroup = 0xB000, //!< Group for analog in related outputs - XDI_AnalogIn1 = 0xB010, //!< Data containing adc data from analog in 1 line (if present) - XDI_AnalogIn2 = 0xB020, //!< Data containing adc data from analog in 2 line (if present) - - XDI_MagneticGroup = 0xC000, //!< Group for magnetometer related outputs - XDI_MagneticField = 0xC020, //!< Magnetic field data in a.u. - - XDI_VelocityGroup = 0xD000, //!< Group for velocity related outputs - XDI_VelocityXYZ = 0xD010, //!< Velocity in XYZ coordinate frame - - XDI_StatusGroup = 0xE000, //!< Group for status related outputs - XDI_StatusByte = 0xE010, //!< Status byte - XDI_StatusWord = 0xE020, //!< Status word - XDI_Rssi = 0xE040, //!< Rssi information - XDI_DeviceId = 0xE080, //!< DeviceId output - - XDI_IndicationGroup = 0x4800, //!< 0100.1000 -> bit reverse = 0001.0010 -> type 18 - XDI_TriggerIn1 = 0x4810, //!< Trigger in 1 indication - XDI_TriggerIn2 = 0x4820, //!< Trigger in 2 indication -}; -/*! @} */ - -typedef enum XsDataIdentifier XsDataIdentifier; - -#ifdef __cplusplus -inline XsDataIdentifier operator | (XsDataIdentifier a, XsDataIdentifier b) -{ - return (XsDataIdentifier) ((int) a | (int) b); -} - -inline XsDataIdentifier operator & (XsDataIdentifier a, XsDataIdentifier b) -{ - return (XsDataIdentifier) ((int) a & (int) b); -} - -inline XsDataIdentifier operator ~ (XsDataIdentifier a) -{ - return (XsDataIdentifier) ~((unsigned short)a); -} -#endif - -#include "xsdataidentifiervalue.h" - -#endif diff --git a/extern/include/xsens/xsdataidentifiervalue.h b/extern/include/xsens/xsdataidentifiervalue.h deleted file mode 100644 index 8f9b2ea..0000000 --- a/extern/include/xsens/xsdataidentifiervalue.h +++ /dev/null @@ -1,38 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSDATAIDENTIFIERVALUE_H -#define XSDATAIDENTIFIERVALUE_H - -#define XDI_MAX_FREQUENCY ((uint16_t) 0xFFFF) - -////////////////////////////////////////////////////////////////////////////////////////// -/*! \addtogroup enums Global enumerations - @{ -*/ - -/*! \enum XsDataIdentifierValue - \brief Defines some convenience values for use with the data identifiers - - Refer to the Low Level Communication Protocol for more details. -*/ -enum XsDataIdentifierValue -{ - XDIV_MaxFrequency = XDI_MAX_FREQUENCY, //!< Maximum / no frequency -}; -/*! @} */ - -typedef enum XsDataIdentifierValue XsDataIdentifierValue; - -#endif diff --git a/extern/include/xsens/xsdatapacket.h b/extern/include/xsens/xsdatapacket.h deleted file mode 100644 index 06b6ece..0000000 --- a/extern/include/xsens/xsdatapacket.h +++ /dev/null @@ -1,1437 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSDATAPACKET_H -#define XSDATAPACKET_H - -#include "xstypedefs.h" -#include "pstdint.h" -#include "xsmessage.h" -#include "xstimestamp.h" -#include "xsdataidentifier.h" -#include "xsushortvector.h" -#include "xsscrdata.h" -#include "xscalibrateddata.h" -#include "xsgpspvtdata.h" -#include "xspressure.h" -#include "xssdidata.h" -#include "xsvector.h" -#include "xsquaternion.h" -#include "xsmatrix.h" -#include "xseuler.h" -#include "xsanalogindata.h" -#include "xsutctime.h" -#include "xsrawgpsdop.h" -#include "xsrawgpssol.h" -#include "xsrawgpssvinfo.h" -#include "xsrawgpstimeutc.h" -#include "xsrawgnsspvtdata.h" -#include "xsrawgnsssatinfo.h" -#include "xsdeviceid.h" -#include "xsrange.h" -#include "xstriggerindicationdata.h" -#include "xssnapshot.h" - -#ifndef XSNOEXPORT -#define XSNOEXPORT -#endif - -struct XsDataPacket; -struct XSNOEXPORT DataPacketPrivate; - -#ifdef __cplusplus -extern "C" -{ -#else -typedef struct XsDataPacket XsDataPacket; -//#define XSDATAPACKET_INITIALIZER { 0, 0, XSDEVICEID_INITIALIZER, -1 } //Use XsDataPacket_construct in all cases because of dynamic initialization -#endif - -XSTYPES_DLL_API void XsDataPacket_construct(XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_destruct(XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_clear(XsDataPacket* thisPtr, XsDataIdentifier id); -XSTYPES_DLL_API void XsDataPacket_copy(XsDataPacket* copy, XsDataPacket const* src); -XSTYPES_DLL_API void XsDataPacket_swap(XsDataPacket* thisPtr, XsDataPacket* other); -XSTYPES_DLL_API int XsDataPacket_empty(const XsDataPacket* thisPtr); -XSTYPES_DLL_API int XsDataPacket_itemCount(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setMessage(XsDataPacket* thisPtr, const XsMessage* msg); -XSTYPES_DLL_API XsDataIdentifier XsDataPacket_dataFormat(const XsDataPacket* thisPtr, XsDataIdentifier id); -XSTYPES_DLL_API XsUShortVector* XsDataPacket_rawAcceleration(const XsDataPacket* thisPtr, XsUShortVector* returnVal); -XSTYPES_DLL_API XsVector* XsDataPacket_rawAccelerationConverted(const XsDataPacket* thisPtr, XsVector* returnVal); -XSTYPES_DLL_API int XsDataPacket_containsRawAcceleration(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setRawAcceleration(XsDataPacket* thisPtr, const XsUShortVector* vec); -XSTYPES_DLL_API XsUShortVector* XsDataPacket_rawGyroscopeData(const XsDataPacket* thisPtr, XsUShortVector* returnVal); -XSTYPES_DLL_API XsVector* XsDataPacket_rawGyroscopeDataConverted(const XsDataPacket* thisPtr, XsVector* returnVal); -XSTYPES_DLL_API int XsDataPacket_containsRawGyroscopeData(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setRawGyroscopeData(XsDataPacket* thisPtr, const XsUShortVector* vec); -XSTYPES_DLL_API XsUShortVector* XsDataPacket_rawGyroscopeTemperatureData(const XsDataPacket* thisPtr, XsUShortVector* returnVal); -XSTYPES_DLL_API XsVector* XsDataPacket_rawGyroscopeTemperatureDataConverted(const XsDataPacket* thisPtr, XsVector* returnVal); -XSTYPES_DLL_API int XsDataPacket_containsRawGyroscopeTemperatureData(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setRawGyroscopeTemperatureData(XsDataPacket* thisPtr, const XsUShortVector* vec); -XSTYPES_DLL_API XsUShortVector* XsDataPacket_rawMagneticField(const XsDataPacket* thisPtr, XsUShortVector* returnVal); -XSTYPES_DLL_API XsVector* XsDataPacket_rawMagneticFieldConverted(const XsDataPacket* thisPtr, XsVector* returnVal); -XSTYPES_DLL_API int XsDataPacket_containsRawMagneticField(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setRawMagneticField(XsDataPacket* thisPtr, const XsUShortVector* vec); -XSTYPES_DLL_API uint16_t XsDataPacket_rawTemperature(const XsDataPacket* thisPtr); -XSTYPES_DLL_API int XsDataPacket_containsRawTemperature(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setRawTemperature(XsDataPacket* thisPtr, uint16_t temp); -XSTYPES_DLL_API XsScrData* XsDataPacket_rawData(const XsDataPacket* thisPtr, XsScrData* returnVal); -XSTYPES_DLL_API int XsDataPacket_containsRawData(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setRawData(XsDataPacket* thisPtr, const XsScrData* data); -XSTYPES_DLL_API XsVector* XsDataPacket_calibratedAcceleration(const XsDataPacket* thisPtr, XsVector* returnVal); -XSTYPES_DLL_API int XsDataPacket_containsCalibratedAcceleration(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setCalibratedAcceleration(XsDataPacket* thisPtr, const XsVector* vec); -XSTYPES_DLL_API XsVector* XsDataPacket_calibratedGyroscopeData(const XsDataPacket* thisPtr, XsVector* returnVal); -XSTYPES_DLL_API int XsDataPacket_containsCalibratedGyroscopeData(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setCalibratedGyroscopeData(XsDataPacket* thisPtr, const XsVector* vec); -XSTYPES_DLL_API XsVector* XsDataPacket_calibratedMagneticField(const XsDataPacket* thisPtr, XsVector* returnVal); -XSTYPES_DLL_API int XsDataPacket_containsCalibratedMagneticField(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setCalibratedMagneticField(XsDataPacket* thisPtr, const XsVector* vec); -XSTYPES_DLL_API XsCalibratedData* XsDataPacket_calibratedData(const XsDataPacket* thisPtr, XsCalibratedData* returnVal); -XSTYPES_DLL_API int XsDataPacket_containsCalibratedData(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setCalibratedData(XsDataPacket* thisPtr, const XsCalibratedData* data); -XSTYPES_DLL_API XsQuaternion* XsDataPacket_orientationQuaternion(const XsDataPacket* thisPtr, XsQuaternion* returnVal, XsDataIdentifier coordinateSystem); -XSTYPES_DLL_API void XsDataPacket_setOrientationQuaternion(XsDataPacket* thisPtr, const XsQuaternion* data, XsDataIdentifier coordinateSystem); -XSTYPES_DLL_API XsEuler* XsDataPacket_orientationEuler(const XsDataPacket* thisPtr, XsEuler* returnVal, XsDataIdentifier coordinateSystem); -XSTYPES_DLL_API void XsDataPacket_setOrientationEuler(XsDataPacket* thisPtr, const XsEuler* data, XsDataIdentifier coordinateSystem); -XSTYPES_DLL_API XsMatrix* XsDataPacket_orientationMatrix(const XsDataPacket* thisPtr, XsMatrix* returnVal, XsDataIdentifier coordinateSystem); -XSTYPES_DLL_API void XsDataPacket_setOrientationMatrix(XsDataPacket* thisPtr, const XsMatrix* data, XsDataIdentifier coordinateSystem); -XSTYPES_DLL_API int XsDataPacket_containsOrientation(const XsDataPacket* thisPtr); -XSTYPES_DLL_API XsDataIdentifier XsDataPacket_orientationIdentifier(const XsDataPacket* thisPtr); -XSTYPES_DLL_API XsDataIdentifier XsDataPacket_coordinateSystemOrientation(const XsDataPacket* thisPtr); -XSTYPES_DLL_API XsSdiData* XsDataPacket_sdiData(const XsDataPacket* thisPtr, XsSdiData* returnVal); -XSTYPES_DLL_API int XsDataPacket_containsSdiData(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setSdiData(XsDataPacket* thisPtr, const XsSdiData* data); -XSTYPES_DLL_API XsDeviceId* XsDataPacket_storedDeviceId(const XsDataPacket* thisPtr, XsDeviceId* returnVal); -XSTYPES_DLL_API int XsDataPacket_containsStoredDeviceId(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setStoredDeviceId(XsDataPacket* thisPtr, const XsDeviceId* data); -XSTYPES_DLL_API uint32_t XsDataPacket_status(const XsDataPacket* thisPtr); -XSTYPES_DLL_API int XsDataPacket_containsStatus(const XsDataPacket* thisPtr); -XSTYPES_DLL_API int XsDataPacket_containsDetailedStatus(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setStatus(XsDataPacket* thisPtr, uint32_t data); -XSTYPES_DLL_API void XsDataPacket_setStatusByte(XsDataPacket* thisPtr, uint8_t data); -XSTYPES_DLL_API uint8_t XsDataPacket_packetCounter8(const XsDataPacket* thisPtr); -XSTYPES_DLL_API int XsDataPacket_containsPacketCounter8(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setPacketCounter8(XsDataPacket* thisPtr, uint8_t counter); -XSTYPES_DLL_API uint16_t XsDataPacket_packetCounter(const XsDataPacket* thisPtr); -XSTYPES_DLL_API int XsDataPacket_containsPacketCounter(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setPacketCounter(XsDataPacket* thisPtr, uint16_t counter); -XSTYPES_DLL_API uint32_t XsDataPacket_sampleTimeFine(const XsDataPacket* thisPtr); -XSTYPES_DLL_API int XsDataPacket_containsSampleTimeFine(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setSampleTimeFine(XsDataPacket* thisPtr, uint32_t counter); -XSTYPES_DLL_API uint32_t XsDataPacket_sampleTimeCoarse(const XsDataPacket* thisPtr); -XSTYPES_DLL_API int XsDataPacket_containsSampleTimeCoarse(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setSampleTimeCoarse(XsDataPacket* thisPtr, uint32_t counter); -XSTYPES_DLL_API uint64_t XsDataPacket_sampleTime64(const XsDataPacket* thisPtr); -XSTYPES_DLL_API int XsDataPacket_containsSampleTime64(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setSampleTime64(XsDataPacket* thisPtr, uint64_t counter); -XSTYPES_DLL_API XsVector* XsDataPacket_freeAcceleration(const XsDataPacket* thisPtr, XsVector* returnVal); -XSTYPES_DLL_API int XsDataPacket_containsFreeAcceleration(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setFreeAcceleration(XsDataPacket* thisPtr, const XsVector* g); -XSTYPES_DLL_API double XsDataPacket_temperature(const XsDataPacket* thisPtr); -XSTYPES_DLL_API int XsDataPacket_containsTemperature(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setTemperature(XsDataPacket* thisPtr, double temperature); -XSTYPES_DLL_API XsGpsPvtData* XsDataPacket_gpsPvtData(const XsDataPacket* thisPtr, XsGpsPvtData* returnVal); -XSTYPES_DLL_API int XsDataPacket_containsGpsPvtData(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setGpsPvtData(XsDataPacket* thisPtr, const XsGpsPvtData* data); -XSTYPES_DLL_API int XsDataPacket_containsPressure(const XsDataPacket* thisPtr); -XSTYPES_DLL_API XsPressure* XsDataPacket_pressure(const XsDataPacket* thisPtr, XsPressure* returnVal); -XSTYPES_DLL_API int XsDataPacket_containsPressureAge(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setPressure(XsDataPacket* thisPtr, const XsPressure* data); -XSTYPES_DLL_API XsAnalogInData* XsDataPacket_analogIn1Data(const XsDataPacket* thisPtr, XsAnalogInData* returnVal); -XSTYPES_DLL_API int XsDataPacket_containsAnalogIn1Data(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setAnalogIn1Data(XsDataPacket* thisPtr, const XsAnalogInData* data); -XSTYPES_DLL_API XsAnalogInData* XsDataPacket_analogIn2Data(const XsDataPacket* thisPtr, XsAnalogInData* returnVal); -XSTYPES_DLL_API int XsDataPacket_containsAnalogIn2Data(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setAnalogIn2Data(XsDataPacket* thisPtr, const XsAnalogInData* data); -XSTYPES_DLL_API XsVector* XsDataPacket_positionLLA(const XsDataPacket* thisPtr, XsVector* returnVal); -XSTYPES_DLL_API int XsDataPacket_containsPositionLLA(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setPositionLLA(XsDataPacket* thisPtr, const XsVector* data); -XSTYPES_DLL_API XsVector* XsDataPacket_latitudeLongitude(const XsDataPacket* thisPtr, XsVector* returnVal); -XSTYPES_DLL_API int XsDataPacket_containsLatitudeLongitude(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setLatitudeLongitude(XsDataPacket* thisPtr, const XsVector* data); -XSTYPES_DLL_API double XsDataPacket_altitude(const XsDataPacket* thisPtr); -XSTYPES_DLL_API int XsDataPacket_containsAltitude(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setAltitude(XsDataPacket* thisPtr, double data); -XSTYPES_DLL_API double XsDataPacket_altitudeMsl(const XsDataPacket* thisPtr); -XSTYPES_DLL_API int XsDataPacket_containsAltitudeMsl(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setAltitudeMsl(XsDataPacket* thisPtr, double data); -XSTYPES_DLL_API XsVector* XsDataPacket_velocity(const XsDataPacket* thisPtr, XsVector* returnVal, XsDataIdentifier coordinateSystem); -XSTYPES_DLL_API int XsDataPacket_containsVelocity(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setVelocity(XsDataPacket* thisPtr, const XsVector* data, XsDataIdentifier coordinateSystem); -XSTYPES_DLL_API XsDataIdentifier XsDataPacket_velocityIdentifier(const XsDataPacket* thisPtr); -XSTYPES_DLL_API XsDataIdentifier XsDataPacket_coordinateSystemVelocity(const XsDataPacket* thisPtr); -XSTYPES_DLL_API XsUtcTime* XsDataPacket_utcTime(const XsDataPacket* thisPtr, XsUtcTime* returnVal); -XSTYPES_DLL_API int XsDataPacket_containsUtcTime(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setUtcTime(XsDataPacket* thisPtr, const XsUtcTime* data); -XSTYPES_DLL_API XsRange* XsDataPacket_frameRange(const XsDataPacket* thisPtr, XsRange* returnVal); -XSTYPES_DLL_API int XsDataPacket_containsFrameRange(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setFrameRange(XsDataPacket* thisPtr, const XsRange* r); -XSTYPES_DLL_API int XsDataPacket_rssi(const XsDataPacket* thisPtr); -XSTYPES_DLL_API int XsDataPacket_containsRssi(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setRssi(XsDataPacket* thisPtr, int r); -XSTYPES_DLL_API XsRawGpsDop* XsDataPacket_rawGpsDop(const XsDataPacket* thisPtr, XsRawGpsDop* returnVal); -XSTYPES_DLL_API int XsDataPacket_containsRawGpsDop(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setRawGpsDop(XsDataPacket* thisPtr, const XsRawGpsDop* data); -XSTYPES_DLL_API XsRawGpsSol* XsDataPacket_rawGpsSol(const XsDataPacket* thisPtr, XsRawGpsSol* returnVal); -XSTYPES_DLL_API int XsDataPacket_containsRawGpsSol(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setRawGpsSol(XsDataPacket* thisPtr, const XsRawGpsSol* data); -XSTYPES_DLL_API XsRawGpsTimeUtc* XsDataPacket_rawGpsTimeUtc(const XsDataPacket* thisPtr, XsRawGpsTimeUtc* returnVal); -XSTYPES_DLL_API int XsDataPacket_containsRawGpsTimeUtc(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setRawGpsTimeUtc(XsDataPacket* thisPtr, const XsRawGpsTimeUtc* data); -XSTYPES_DLL_API XsRawGpsSvInfo* XsDataPacket_rawGpsSvInfo(const XsDataPacket* thisPtr, XsRawGpsSvInfo* returnVal); -XSTYPES_DLL_API int XsDataPacket_containsRawGpsSvInfo(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setRawGpsSvInfo(XsDataPacket* thisPtr, const XsRawGpsSvInfo* data); - -XSTYPES_DLL_API XsRawGnssPvtData* XsDataPacket_rawGnssPvtData(const XsDataPacket* thisPtr, XsRawGnssPvtData* returnVal); -XSTYPES_DLL_API int XsDataPacket_containsRawGnssPvtData(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setRawGnssPvtData(XsDataPacket* thisPtr, const XsRawGnssPvtData* r); -XSTYPES_DLL_API uint8_t XsDataPacket_gnssAge(const XsDataPacket* thisPtr); -XSTYPES_DLL_API int XsDataPacket_containsGnssAge(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setGnssAge(XsDataPacket* thisPtr, uint8_t age); -XSTYPES_DLL_API XsRawGnssSatInfo* XsDataPacket_rawGnssSatInfo(const XsDataPacket* thisPtr, XsRawGnssSatInfo* returnVal); -XSTYPES_DLL_API int XsDataPacket_containsRawGnssSatInfo(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setRawGnssSatInfo(XsDataPacket* thisPtr, const XsRawGnssSatInfo* r); - -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 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 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 XsSnapshot* XsDataPacket_fullSnapshot(const XsDataPacket* thisPtr, XsSnapshot* returnVal); -XSTYPES_DLL_API int XsDataPacket_containsFullSnapshot(const XsDataPacket* thisPtr); - -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); - -XSTYPES_DLL_API XsVector* XsDataPacket_accelerationHR(const XsDataPacket* thisPtr, XsVector* returnVal); -XSTYPES_DLL_API int XsDataPacket_containsAccelerationHR(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setAccelerationHR(XsDataPacket* thisPtr, const XsVector* vec); - -XSTYPES_DLL_API XsVector* XsDataPacket_rateOfTurnHR(const XsDataPacket* thisPtr, XsVector* returnVal); -XSTYPES_DLL_API int XsDataPacket_containsRateOfTurnHR(const XsDataPacket* thisPtr); -XSTYPES_DLL_API void XsDataPacket_setRateOfTurnHR(XsDataPacket* thisPtr, const XsVector* vec); - - -#ifdef __cplusplus -} // extern "C" -#ifndef XSENS_NO_STL -#include // only needed for simplifiedContents() -#endif -#endif - -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 - MTData2 data. - */ - inline explicit XsDataPacket(const XsMessage* msg = 0) - { - XsDataPacket_construct(this); - if (msg) - XsDataPacket_setMessage(this, msg); - } - - /*! \brief Copy constructor - \param pack The packet to copy from - */ - inline XsDataPacket(const XsDataPacket& pack) - { - XsDataPacket_construct(this); - *this = pack; - } - - //! \copydoc XsDataPacket_destruct - inline ~XsDataPacket() - { - XsDataPacket_destruct(this); - } - - /*! \brief Assignment operator - \param other The packet to copy from - \returns A reference to this %XsDataPacket - \sa XsDataPacket_copy - */ - inline const XsDataPacket& operator = (const XsDataPacket& other) - { - if (this != &other) - XsDataPacket_copy(this, &other); - return *this; - } - - /*! \copydoc XsDataPacket_swap(XsDataPacket*,XsDataPacket*)*/ - inline void swap(XsDataPacket& other) - { - XsDataPacket_swap(this, &other); - } - - /*! \copydoc XsDataPacket_clear(XsDataPacket*,XsDataIdentifier)*/ - inline void clear(XsDataIdentifier id = XDI_None) - { - XsDataPacket_clear(this, id); - } - - /*! \copydoc XsDataPacket_empty(const XsDataPacket*)*/ - inline bool empty(void) const - { - return 0 != XsDataPacket_empty(this); - } - - //! \brief Return the device ID associated with the data packet - inline XsDeviceId deviceId() const - { - return m_deviceId; - } - - /*! \copydoc XsDataPacket_itemCount(const XsDataPacket*)*/ - inline uint16_t itemCount() const - { - return static_cast(static_cast(XsDataPacket_itemCount(this))); - } - - //! \copydoc XsDataPacket_setMessage(XsDataPacket*, const XsMessage*) - inline void setMessage(const XsMessage& msg) - { - XsDataPacket_setMessage(this, &msg); - } - - /*! \brief Returns a const reference to the message that contains the data packet - */ - inline XsMessage toMessage() const - { - XsMessage msg; - XsDataPacket_toMessage(this, &msg); - return msg; - } - - /*! \brief Set the device ID associated with this data packet - \param id The device ID to set - */ - inline void setDeviceId(const XsDeviceId id) - { - m_deviceId = id; - } - - /*! \copydoc XsDataPacket_dataFormat(const XsDataPacket*, XsDataIdentifier) */ - inline XsDataIdentifier dataFormat(XsDataIdentifier id) const - { - return XsDataPacket_dataFormat(this, id); - } - - /*! \brief \copybrief XsDataPacket_rawAcceleration(const XsDataPacket*, XsUShortVector*) */ - inline XsUShortVector rawAcceleration(void) const - { - XsUShortVector returnVal; - return *XsDataPacket_rawAcceleration(this, &returnVal); - } - - /*! \brief \copybrief XsDataPacket_rawAccelerationConverted(const XsDataPacket*, XsVector*) */ - inline XsVector rawAccelerationConverted(void) const - { - XsVector returnVal; - return *XsDataPacket_rawAccelerationConverted(this, &returnVal); - } - - /*! \copydoc XsDataPacket_containsRawAcceleration(const XsDataPacket*) */ - inline bool containsRawAcceleration(void) const - { - return 0 != XsDataPacket_containsRawAcceleration(this); - } - - /*! \copydoc XsDataPacket_setRawAcceleration(XsDataPacket*, const XsUShortVector*) */ - inline void setRawAcceleration(const XsUShortVector& vec) - { - XsDataPacket_setRawAcceleration(this, &vec); - } - - /*! \brief \copybrief XsDataPacket_rawGyroscopeData(const XsDataPacket*, XsUShortVector*) */ - inline XsUShortVector rawGyroscopeData(void) const - { - XsUShortVector returnVal; - return *XsDataPacket_rawGyroscopeData(this, &returnVal); - } - - /*! \brief \copybrief XsDataPacket_rawGyroscopeDataConverted(const XsDataPacket*, XsVector*) */ - inline XsVector rawGyroscopeDataConverted(void) const - { - XsVector returnVal; - return *XsDataPacket_rawGyroscopeDataConverted(this, &returnVal); - } - - /*! \copydoc XsDataPacket_containsRawGyroscopeData(const XsDataPacket*) */ - inline bool containsRawGyroscopeData(void) const - { - return 0 != XsDataPacket_containsRawGyroscopeData(this); - } - - /*! \copydoc XsDataPacket_setRawGyroscopeData(XsDataPacket*, const XsUShortVector*) */ - inline void setRawGyroscopeData(const XsUShortVector& vec) - { - XsDataPacket_setRawGyroscopeData(this, &vec); - } - - /*! \brief \copybrief XsDataPacket_rawGyroscopeTemperatureData(const XsDataPacket*, XsUShortVector*) */ - inline XsUShortVector rawGyroscopeTemperatureData(void) const - { - XsUShortVector returnVal; - return *XsDataPacket_rawGyroscopeTemperatureData(this, &returnVal); - } - - /*! \brief \copybrief XsDataPacket_rawGyroscopeTemperatureDataConverted(const XsDataPacket*, XsVector*) */ - inline XsVector rawGyroscopeTemperatureDataConverted(void) const - { - XsVector returnVal; - return *XsDataPacket_rawGyroscopeTemperatureDataConverted(this, &returnVal); - } - - /*! \copydoc XsDataPacket_containsRawGyroscopeTemperatureData(const XsDataPacket*) */ - inline bool containsRawGyroscopeTemperatureData(void) const - { - return 0 != XsDataPacket_containsRawGyroscopeTemperatureData(this); - } - - /*! \copydoc XsDataPacket_setRawGyroscopeTemperatureData(XsDataPacket*, const XsUShortVector*) */ - inline void setRawGyroscopeTemperatureData(const XsUShortVector& vec) - { - XsDataPacket_setRawGyroscopeTemperatureData(this, &vec); - } - - /*! \brief \copybrief XsDataPacket_rawMagneticField(const XsDataPacket*, XsUShortVector*) */ - inline XsUShortVector rawMagneticField(void) const - { - XsUShortVector returnVal; - return *XsDataPacket_rawMagneticField(this, &returnVal); - } - - /*! \brief \copybrief XsDataPacket_rawMagneticFieldConverted(const XsDataPacket*, XsVector*) */ - inline XsVector rawMagneticFieldConverted(void) const - { - XsVector returnVal; - return *XsDataPacket_rawMagneticFieldConverted(this, &returnVal); - } - - /*! \copydoc XsDataPacket_containsRawMagneticField(const XsDataPacket*) */ - inline bool containsRawMagneticField(void) const - { - return 0 != XsDataPacket_containsRawMagneticField(this); - } - - /*! \copydoc XsDataPacket_setRawMagneticField(XsDataPacket*, const XsUShortVector*) */ - inline void setRawMagneticField(const XsUShortVector& vec) - { - XsDataPacket_setRawMagneticField(this, &vec); - } - - /*! \brief \copybrief XsDataPacket_rawTemperature(const XsDataPacket*) */ - inline uint16_t rawTemperature(void) const - { - return XsDataPacket_rawTemperature(this); - } - - /*! \copydoc XsDataPacket_containsRawTemperature(const XsDataPacket*) */ - inline bool containsRawTemperature(void) const - { - return 0 != XsDataPacket_containsRawTemperature(this); - } - - /*! \copydoc XsDataPacket_setRawTemperature(XsDataPacket*, uint16_t) */ - inline void setRawTemperature(uint16_t temp) - { - XsDataPacket_setRawTemperature(this, temp); - } - - /*! \brief \copybrief XsDataPacket_rawData(const XsDataPacket*, XsScrData*) - \return The raw data component of a data item. - */ - inline XsScrData rawData(void) const - { - XsScrData returnVal; - return *XsDataPacket_rawData(this, &returnVal); - } - - /*! \copydoc XsDataPacket_containsRawData(const XsDataPacket*) */ - inline bool containsRawData(void) const - { - return 0 != XsDataPacket_containsRawData(this); - } - - /*! \copydoc XsDataPacket_setRawData(XsDataPacket*, const XsScrData*) */ - inline void setRawData(const XsScrData& data) - { - XsDataPacket_setRawData(this, &data); - } - - /*! \brief \copybrief XsDataPacket_calibratedAcceleration(const XsDataPacket*, XsVector*) */ - inline XsVector calibratedAcceleration(void) const - { - XsVector returnVal; - return *XsDataPacket_calibratedAcceleration(this, &returnVal); - } - - /*! \copydoc XsDataPacket_containsCalibratedAcceleration(const XsDataPacket*) */ - inline bool containsCalibratedAcceleration(void) const - { - return 0 != XsDataPacket_containsCalibratedAcceleration(this); - } - - /*! \copydoc XsDataPacket_setCalibratedAcceleration(XsDataPacket*, const XsVector*) */ - inline void setCalibratedAcceleration(const XsVector& vec) - { - XsDataPacket_setCalibratedAcceleration(this, &vec); - } - - /*! \brief \copybrief XsDataPacket_calibratedGyroscopeData(const XsDataPacket*, XsVector*) */ - inline XsVector calibratedGyroscopeData(void) const - { - XsVector returnVal; - return *XsDataPacket_calibratedGyroscopeData(this, &returnVal); - } - - /*! \copydoc XsDataPacket_containsCalibratedGyroscopeData(const XsDataPacket*) */ - inline bool containsCalibratedGyroscopeData(void) const - { - return 0 != XsDataPacket_containsCalibratedGyroscopeData(this); - } - - /*! \copydoc XsDataPacket_setCalibratedGyroscopeData(XsDataPacket*, const XsVector*) */ - inline void setCalibratedGyroscopeData(const XsVector& vec) - { - XsDataPacket_setCalibratedGyroscopeData(this, &vec); - } - - /*! \brief \copybrief XsDataPacket_calibratedMagneticField(const XsDataPacket*, XsVector*) */ - inline XsVector calibratedMagneticField(void) const - { - XsVector returnVal; - return *XsDataPacket_calibratedMagneticField(this, &returnVal); - } - - /*! \copydoc XsDataPacket_containsCalibratedMagneticField(const XsDataPacket*) */ - inline bool containsCalibratedMagneticField(void) const - { - return 0 != XsDataPacket_containsCalibratedMagneticField(this); - } - - /*! \copydoc XsDataPacket_setCalibratedMagneticField(XsDataPacket*, const XsVector*) */ - inline void setCalibratedMagneticField(const XsVector& vec) - { - XsDataPacket_setCalibratedMagneticField(this, &vec); - } - - /*! \brief \copybrief XsDataPacket_calibratedData(const XsDataPacket*, XsCalibratedData*) */ - inline XsCalibratedData calibratedData(void) const - { - XsCalibratedData returnVal; - return *XsDataPacket_calibratedData(this, &returnVal); - } - - /*! \copydoc XsDataPacket_containsCalibratedData(const XsDataPacket*) */ - inline bool containsCalibratedData(void) const - { - return 0 != XsDataPacket_containsCalibratedData(this); - } - - /*! \copydoc XsDataPacket_setCalibratedData(XsDataPacket*, const XsCalibratedData*) */ - inline void setCalibratedData(const XsCalibratedData& data) - { - XsDataPacket_setCalibratedData(this, &data); - } - - /*! \brief \copybrief XsDataPacket_orientationQuaternion(const XsDataPacket*, XsQuaternion*, XsDataIdentifier) - \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 The requested data - */ - inline XsQuaternion orientationQuaternion(XsDataIdentifier coordinateSystem) const - { - XsQuaternion returnVal; - return *XsDataPacket_orientationQuaternion(this, &returnVal, coordinateSystem); - } - - /*! \brief Returns the orientation as a quaternion with the current coordinate system - */ - inline XsQuaternion orientationQuaternion() const - { - XsQuaternion returnVal; - return *XsDataPacket_orientationQuaternion(this, &returnVal, coordinateSystemOrientation()); - } - - /*! \copydoc XsDataPacket_setOrientationQuaternion(XsDataPacket*, const XsQuaternion*, XsDataIdentifier) */ - inline void setOrientationQuaternion(const XsQuaternion& data, XsDataIdentifier coordinateSystem) - { - XsDataPacket_setOrientationQuaternion(this, &data, coordinateSystem); - } - - /*! \brief \copybrief XsDataPacket_orientationEuler(const XsDataPacket*, XsEuler*, XsDataIdentifier) - \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 The requested data - */ - inline XsEuler orientationEuler(XsDataIdentifier coordinateSystem) const - { - XsEuler returnVal; - return *XsDataPacket_orientationEuler(this, &returnVal, coordinateSystem); - } - - /*! \brief Returns the orientation as an XsEuler with the current coordinate system*/ - inline XsEuler orientationEuler() const - { - XsEuler returnVal; - return *XsDataPacket_orientationEuler(this, &returnVal, coordinateSystemOrientation()); - } - - /*! \copydoc XsDataPacket_setOrientationEuler(XsDataPacket*, const XsEuler*, XsDataIdentifier) */ - inline void setOrientationEuler(const XsEuler& data, XsDataIdentifier coordinateSystem) - { - XsDataPacket_setOrientationEuler(this, &data, coordinateSystem); - } - - /*! \brief \copybrief XsDataPacket_orientationMatrix(const XsDataPacket*, XsMatrix*, XsDataIdentifier) - \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 The requested data - */ - inline XsMatrix orientationMatrix(XsDataIdentifier coordinateSystem) const - { - XsMatrix returnVal; - return *XsDataPacket_orientationMatrix(this, &returnVal, coordinateSystem); - } - - /*! \brief returns the orientation as an orientation matrix with the current coordinate system*/ - inline XsMatrix orientationMatrix() const - { - XsMatrix returnVal; - return *XsDataPacket_orientationMatrix(this, &returnVal, coordinateSystemOrientation()); - } - - /*! \copydoc XsDataPacket_setOrientationMatrix(XsDataPacket*, const XsMatrix*, XsDataIdentifier) */ - inline void setOrientationMatrix(const XsMatrix& data, XsDataIdentifier coordinateSystem) - { - XsDataPacket_setOrientationMatrix(this, &data, coordinateSystem); - } - - /*! \copydoc XsDataPacket_containsOrientation(const XsDataPacket*) */ - inline bool containsOrientation(void) const - { - return 0 != XsDataPacket_containsOrientation(this); - } - - /*! \copydoc XsDataPacket_orientationIdentifier(const XsDataPacket*) */ - inline XsDataIdentifier orientationIdentifier() const - { - return XsDataPacket_orientationIdentifier(this); - } - - /*! \copydoc XsDataPacket_coordinateSystemOrientation(const XsDataPacket*) */ - inline XsDataIdentifier coordinateSystemOrientation() const - { - return XsDataPacket_coordinateSystemOrientation(this); - } - - /*! \brief \copybrief XsDataPacket_sdiData(const XsDataPacket*, XsSdiData*) */ - inline XsSdiData sdiData(void) const - { - XsSdiData returnVal; - return *XsDataPacket_sdiData(this, &returnVal); - } - - /*! \copydoc XsDataPacket_containsSdiData(const XsDataPacket*) */ - inline bool containsSdiData(void) const - { - return 0 != XsDataPacket_containsSdiData(this); - } - - /*! \copydoc XsDataPacket_setSdiData(XsDataPacket*, const XsSdiData*) */ - inline void setSdiData(const XsSdiData& data) - { - XsDataPacket_setSdiData(this, &data); - } - - /*! \brief \copybrief XsDataPacket_storedDeviceId(const XsDataPacket*, XsDeviceId*) - \returns the device ID stored in this packet - */ - inline XsDeviceId storedDeviceId(void) const - { - XsDeviceId returnVal; - return *XsDataPacket_storedDeviceId(this, &returnVal); - } - - /*! \copydoc XsDataPacket_containsStoredDeviceId(const XsDataPacket*) */ - inline bool containsStoredDeviceId(void) const - { - return 0 != XsDataPacket_containsStoredDeviceId(this); - } - - /*! \copydoc XsDataPacket_setStoredDeviceId(XsDataPacket*, const XsDeviceId*) */ - inline void setStoredDeviceId(const XsDeviceId& data) - { - XsDataPacket_setStoredDeviceId(this, &data); - } - - /*! \brief \copybrief XsDataPacket_status(const XsDataPacket*) */ - inline uint32_t status(void) const - { - return XsDataPacket_status(this); - } - - /*! \copydoc XsDataPacket_containsStatus(const XsDataPacket*) */ - inline bool containsStatus(void) const - { - return 0 != XsDataPacket_containsStatus(this); - } - - /*! \copydoc XsDataPacket_containsDetailedStatus(const XsDataPacket*) */ - inline bool containsDetailedStatus(void) const - { - return 0 != XsDataPacket_containsDetailedStatus(this); - } - - /*! \copydoc XsDataPacket_setStatus(XsDataPacket*, uint32_t) */ - inline void setStatus(const uint32_t data) - { - XsDataPacket_setStatus(this, data); - } - - /*! \copydoc XsDataPacket_setStatusByte(XsDataPacket*, uint8_t) */ - inline void setStatusByte(const uint8_t data) - { - XsDataPacket_setStatusByte(this, data); - } - - /*! \brief \copybrief XsDataPacket_packetCounter8(const XsDataPacket*) */ - inline uint8_t packetCounter8(void) const - { - return XsDataPacket_packetCounter8(this); - } - - /*! \copydoc XsDataPacket_containsPacketCounter8(const XsDataPacket*) */ - inline bool containsPacketCounter8(void) const - { - return 0 != XsDataPacket_containsPacketCounter8(this); - } - - /*! \copydoc XsDataPacket_setPacketCounter8(XsDataPacket*, uint8_t) */ - inline void setPacketCounter8(uint8_t counter) - { - XsDataPacket_setPacketCounter8(this, counter); - } - - /*! \brief \copybrief XsDataPacket_packetCounter(const XsDataPacket* thisPtr) */ - inline uint16_t packetCounter(void) const - { - return XsDataPacket_packetCounter(this); - } - - /*! \copydoc XsDataPacket_containsPacketCounter(const XsDataPacket*) */ - inline bool containsPacketCounter(void) const - { - return 0 != XsDataPacket_containsPacketCounter(this); - } - - /*! \copydoc XsDataPacket_setPacketCounter(XsDataPacket*, uint16_t) */ - inline void setPacketCounter(uint16_t counter) - { - XsDataPacket_setPacketCounter(this, counter); - } - - /*! \brief \copybrief XsDataPacket_sampleTimeFine(const XsDataPacket*) */ - inline uint32_t sampleTimeFine(void) const - { - return XsDataPacket_sampleTimeFine(this); - } - - /*! \copydoc XsDataPacket_containsSampleTimeFine(const XsDataPacket*) */ - inline bool containsSampleTimeFine(void) const - { - return 0 != XsDataPacket_containsSampleTimeFine(this); - } - - /*! \copydoc XsDataPacket_setSampleTimeFine(XsDataPacket*, uint32_t) */ - inline void setSampleTimeFine(uint32_t counter) - { - XsDataPacket_setSampleTimeFine(this, counter); - } - - /*! \brief \copybrief XsDataPacket_sampleTimeCoarse(const XsDataPacket*) */ - inline uint32_t sampleTimeCoarse(void) const - { - return XsDataPacket_sampleTimeCoarse(this); - } - - /*! \copydoc XsDataPacket_containsSampleTimeCoarse(const XsDataPacket*) */ - inline bool containsSampleTimeCoarse(void) const - { - return 0 != XsDataPacket_containsSampleTimeCoarse(this); - } - - /*! \copydoc XsDataPacket_setSampleTimeCoarse(XsDataPacket*, uint32_t) */ - inline void setSampleTimeCoarse(uint32_t counter) - { - XsDataPacket_setSampleTimeCoarse(this, counter); - } - - /*! \brief \copybrief XsDataPacket_sampleTime64(const XsDataPacket*) */ - inline uint64_t sampleTime64(void) const - { - return XsDataPacket_sampleTime64(this); - } - - /*! \copydoc XsDataPacket_containsSampleTime64(const XsDataPacket*) */ - inline bool containsSampleTime64(void) const - { - return 0 != XsDataPacket_containsSampleTime64(this); - } - - /*! \copydoc XsDataPacket_setSampleTime64(XsDataPacket*, uint64_t) */ - inline void setSampleTime64(uint64_t counter) - { - XsDataPacket_setSampleTime64(this, counter); - } - - /*! \brief \copybrief XsDataPacket_freeAcceleration(const XsDataPacket*, XsVector*) */ - inline XsVector freeAcceleration(void) const - { - XsVector returnVal; - return *XsDataPacket_freeAcceleration(this, &returnVal); - } - - /*! \copydoc XsDataPacket_containsFreeAcceleration(const XsDataPacket*) */ - inline bool containsFreeAcceleration(void) const - { - return 0 != XsDataPacket_containsFreeAcceleration(this); - } - - /*! \copydoc XsDataPacket_setFreeAcceleration(XsDataPacket*, const XsVector*) */ - inline void setFreeAcceleration(const XsVector& g) - { - XsDataPacket_setFreeAcceleration(this, &g); - } - - /*! \brief \copybrief XsDataPacket_temperature(const XsDataPacket*) */ - inline double temperature(void) const - { - return XsDataPacket_temperature(this); - } - - /*! \copydoc XsDataPacket_containsTemperature(const XsDataPacket*) */ - inline bool containsTemperature(void) const - { - return 0 != XsDataPacket_containsTemperature(this); - } - - /*! \copydoc XsDataPacket_setTemperature(XsDataPacket*, double) */ - inline void setTemperature(double temperature) - { - XsDataPacket_setTemperature(this, temperature); - } - - /*! \brief \copybrief XsDataPacket_gpsPvtData(const XsDataPacket*, XsGpsPvtData*) */ - inline XsGpsPvtData gpsPvtData(void) const - { - XsGpsPvtData returnVal; - return *XsDataPacket_gpsPvtData(this, &returnVal); - } - - /*! \copydoc XsDataPacket_containsGpsPvtData(const XsDataPacket*) */ - inline bool containsGpsPvtData(void) const - { - return 0 != XsDataPacket_containsGpsPvtData(this); - } - - /*! \copydoc XsDataPacket_setGpsPvtData(XsDataPacket*, const XsGpsPvtData*) */ - inline void setGpsPvtData(const XsGpsPvtData& data) - { - XsDataPacket_setGpsPvtData(this, &data); - } - - /*! \brief \copybrief XsDataPacket_pressure(const XsDataPacket*, XsPressure*) */ - inline XsPressure pressure(void) const - { - XsPressure returnVal; - return *XsDataPacket_pressure(this, &returnVal); - } - - /*! \copydoc XsDataPacket_containsPressure(const XsDataPacket*) */ - inline bool containsPressure(void) const - { - return 0 != XsDataPacket_containsPressure(this); - } - - /*! \copydoc XsDataPacket_containsPressureAge(const XsDataPacket*) */ - inline bool containsPressureAge(void) const - { - return 0 != XsDataPacket_containsPressureAge(this); - } - - /*! \copydoc XsDataPacket_setPressure(XsDataPacket*, const XsPressure*) */ - inline void setPressure(const XsPressure& data) - { - XsDataPacket_setPressure(this, &data); - } - - /*! \brief \copybrief XsDataPacket_analogIn1Data(const XsDataPacket*, XsAnalogInData*) */ - inline XsAnalogInData analogIn1Data(void) const - { - XsAnalogInData returnVal; - return *XsDataPacket_analogIn1Data(this, &returnVal); - } - - /*! \copydoc XsDataPacket_containsAnalogIn1Data(const XsDataPacket*) */ - inline bool containsAnalogIn1Data(void) const - { - return 0 != XsDataPacket_containsAnalogIn1Data(this); - } - - /*! \copydoc XsDataPacket_setAnalogIn1Data(XsDataPacket*, const XsAnalogInData*) */ - inline void setAnalogIn1Data(const XsAnalogInData& data) - { - XsDataPacket_setAnalogIn1Data(this, &data); - } - - /*! \brief \copybrief XsDataPacket_analogIn2Data(const XsDataPacket*, XsAnalogInData*) */ - inline XsAnalogInData analogIn2Data(void) const - { - XsAnalogInData returnVal; - return *XsDataPacket_analogIn2Data(this, &returnVal); - } - - /*! \copydoc XsDataPacket_containsAnalogIn2Data(const XsDataPacket*) */ - inline bool containsAnalogIn2Data(void) const - { - return 0 != XsDataPacket_containsAnalogIn2Data(this); - } - - /*! \copydoc XsDataPacket_setAnalogIn2Data(XsDataPacket*, const XsAnalogInData*) */ - inline void setAnalogIn2Data(const XsAnalogInData& data) - { - XsDataPacket_setAnalogIn2Data(this, &data); - } - - /*! \brief \copybrief XsDataPacket_positionLLA(const XsDataPacket*, XsVector*) */ - inline XsVector positionLLA(void) const - { - XsVector returnVal; - return *XsDataPacket_positionLLA(this, &returnVal); - } - - /*! \copydoc XsDataPacket_containsPositionLLA(const XsDataPacket*) */ - inline bool containsPositionLLA(void) const - { - return 0 != XsDataPacket_containsPositionLLA(this); - } - - /*! \copydoc XsDataPacket_setPositionLLA(XsDataPacket*, const XsVector*) */ - inline void setPositionLLA(const XsVector& data) - { - XsDataPacket_setPositionLLA(this, &data); - } - - /*! \brief \copybrief XsDataPacket_latitudeLongitude(const XsDataPacket*, XsVector*) */ - inline XsVector latitudeLongitude(void) const - { - XsVector returnVal; - return *XsDataPacket_latitudeLongitude(this, &returnVal); - } - - /*! \copydoc XsDataPacket_containsLatitudeLongitude(const XsDataPacket*) */ - inline bool containsLatitudeLongitude(void) const - { - return 0 != XsDataPacket_containsLatitudeLongitude(this); - } - - /*! \copydoc XsDataPacket_setLatitudeLongitude(XsDataPacket*, const XsVector*) */ - inline void setLatitudeLongitude(const XsVector& data) - { - XsDataPacket_setLatitudeLongitude(this, &data); - } - - /*! \brief \copybrief XsDataPacket_altitude(const XsDataPacket*) */ - inline double altitude(void) const - { - return XsDataPacket_altitude(this); - } - - /*! \copydoc XsDataPacket_containsAltitude(const XsDataPacket*) */ - inline bool containsAltitude(void) const - { - return 0 != XsDataPacket_containsAltitude(this); - } - - /*! \copydoc XsDataPacket_setAltitude(XsDataPacket*, double) */ - inline void setAltitude(double data) - { - XsDataPacket_setAltitude(this, data); - } - - /*! \brief \copybrief XsDataPacket_altitudeMsl(const XsDataPacket*) */ - inline double altitudeMsl(void) const - { - return XsDataPacket_altitudeMsl(this); - } - - /*! \copydoc XsDataPacket_containsAltitudeMsl(const XsDataPacket*) */ - inline bool containsAltitudeMsl(void) const - { - return 0 != XsDataPacket_containsAltitudeMsl(this); - } - - /*! \copydoc XsDataPacket_setAltitudeMsl(XsDataPacket*, double) */ - inline void setAltitudeMsl(double data) - { - XsDataPacket_setAltitudeMsl(this, data); - } - - /*! \brief \copybrief XsDataPacket_velocity(const XsDataPacket*, XsVector*, XsDataIdentifier) - \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 - */ - inline XsVector velocity(XsDataIdentifier coordinateSystem) const - { - XsVector returnVal; - return *XsDataPacket_velocity(this, &returnVal, coordinateSystem); - } - - /*! \brief Returns the velocity with the current coordinate system*/ - inline XsVector velocity(void) const - { - XsVector returnVal; - return *XsDataPacket_velocity(this, &returnVal, coordinateSystemVelocity()); - } - - /*! \copydoc XsDataPacket_containsVelocity(const XsDataPacket*) */ - inline bool containsVelocity(void) const - { - return 0 != XsDataPacket_containsVelocity(this); - } - - /*! \copydoc XsDataPacket_setVelocity(XsDataPacket*, const XsVector*, XsDataIdentifier) */ - inline void setVelocity(const XsVector& data, XsDataIdentifier coordinateSystem) - { - XsDataPacket_setVelocity(this, &data, coordinateSystem); - } - - /*! \brief \copybrief XsDataPacket_velocityIdentifier(const XsDataPacket*) */ - inline XsDataIdentifier velocityIdentifier() const - { - return XsDataPacket_velocityIdentifier(this); - } - - /*! \copydoc XsDataPacket_coordinateSystemVelocity(const XsDataPacket*) */ - inline XsDataIdentifier coordinateSystemVelocity() const - { - return XsDataPacket_coordinateSystemVelocity(this); - } - - /*! \brief \copybrief XsDataPacket_utcTime(const XsDataPacket*, XsUtcTime*) - \returns An XsUtcTime containing the utc time value - */ - inline XsUtcTime utcTime(void) const - { - XsUtcTime returnVal; - return *XsDataPacket_utcTime(this, &returnVal); - } - - /*! \copydoc XsDataPacket_containsUtcTime(const XsDataPacket*) */ - inline bool containsUtcTime(void) const - { - return 0 != XsDataPacket_containsUtcTime(this); - } - - /*! \copydoc XsDataPacket_setUtcTime(XsDataPacket*, const XsUtcTime*) */ - inline void setUtcTime(const XsUtcTime& data) - { - XsDataPacket_setUtcTime(this, &data); - } - - /*! \brief \copybrief XsDataPacket_frameRange(const XsDataPacket*, XsRange*) */ - inline XsRange frameRange() const - { - XsRange returnVal; - return *XsDataPacket_frameRange(this, &returnVal); - } - - /*! \copydoc XsDataPacket_containsFrameRange(const XsDataPacket*) */ - inline bool containsFrameRange() const - { - return 0 != XsDataPacket_containsFrameRange(this); - } - - /*! \copydoc XsDataPacket_setFrameRange(XsDataPacket*, const XsRange*) */ - inline void setFrameRange(const XsRange& r) - { - XsDataPacket_setFrameRange(this, &r); - } - - /*! \brief \copybrief XsDataPacket_rssi(const XsDataPacket*) */ - inline int rssi() const - { - return XsDataPacket_rssi(this); - } - - /*! \copydoc XsDataPacket_containsRssi(const XsDataPacket*) */ - inline bool containsRssi() const - { - return 0 != XsDataPacket_containsRssi(this); - } - - /*! \copydoc XsDataPacket_setRssi(XsDataPacket* thisPtr, int) */ - inline void setRssi(int r) - { - XsDataPacket_setRssi(this, r); - } - - - /*! \brief \copybrief XsDataPacket_rawGpsDop(const XsDataPacket*, XsRawGpsDop*) */ - inline XsRawGpsDop rawGpsDop(void) const - { - XsRawGpsDop returnVal; - return *XsDataPacket_rawGpsDop(this, &returnVal); - } - - /*! \brief \copybrief XsDataPacket_containsRawGpsDop(const XsDataPacket*) */ - inline bool containsRawGpsDop(void) const - { - return 0 != XsDataPacket_containsRawGpsDop(this); - } - - /*! \copydoc XsDataPacket_setRawGpsDop(XsDataPacket*, const XsRawGpsDop*) */ - XSNOEXPORT inline void setRawGpsDop(XsRawGpsDop const& data) - { - XsDataPacket_setRawGpsDop(this, &data); - } - - /*! \brief \copybrief XsDataPacket_rawGpsSol(const XsDataPacket*, XsRawGpsSol*) */ - inline XsRawGpsSol rawGpsSol(void) const - { - XsRawGpsSol returnVal; - return *XsDataPacket_rawGpsSol(this, &returnVal); - } - - /*! \brief \copybrief XsDataPacket_containsRawGpsSol(const XsDataPacket*) */ - inline bool containsRawGpsSol(void) const - { - return 0 != XsDataPacket_containsRawGpsSol(this); - } - - /*! \copydoc XsDataPacket_setRawGpsSol(XsDataPacket*, const XsRawGpsSol*) */ - XSNOEXPORT inline void setRawGpsSol(XsRawGpsSol const& data) - { - XsDataPacket_setRawGpsSol(this, &data); - } - - /*! \brief \copybrief XsDataPacket_rawGpsTimeUtc(const XsDataPacket*, XsRawGpsTimeUtc*) */ - inline XsRawGpsTimeUtc rawGpsTimeUtc(void) const - { - XsRawGpsTimeUtc returnVal; - return *XsDataPacket_rawGpsTimeUtc(this, &returnVal); - } - - /*! \brief \copybrief XsDataPacket_containsRawGpsTimeUtc(const XsDataPacket*) */ - inline bool containsRawGpsTimeUtc(void) const - { - return 0 != XsDataPacket_containsRawGpsTimeUtc(this); - } - - /*! \copydoc XsDataPacket_setRawGpsTimeUtc(XsDataPacket*, const XsRawGpsTimeUtc*) */ - XSNOEXPORT inline void setRawGpsTimeUtc(XsRawGpsTimeUtc const& data) - { - XsDataPacket_setRawGpsTimeUtc(this, &data); - } - - /*! \brief \copybrief XsDataPacket_rawGpsSvInfo(const XsDataPacket*, XsRawGpsSvInfo*) */ - inline XsRawGpsSvInfo rawGpsSvInfo(void) const - { - XsRawGpsSvInfo returnVal; - return *XsDataPacket_rawGpsSvInfo(this, &returnVal); - } - - /*! \brief \copybrief XsDataPacket_containsRawGpsSvInfo(const XsDataPacket*) */ - inline bool containsRawGpsSvInfo(void) const - { - return 0 != XsDataPacket_containsRawGpsSvInfo(this); - } - - /*! \brief \copybrief XsDataPacket_containsRawGpsSvInfo(const XsDataPacket*) */ - XSNOEXPORT inline void setRawGpsSvInfo(XsRawGpsSvInfo const& raw) - { - XsDataPacket_setRawGpsSvInfo(this, &raw); - } - - /*! \brief \copybrief XsDataPacket_rawGnssPvtData(const XsDataPacket*, XsRawGnssPvtData*) - \return a struct with RawGnssPvtData - */ - inline XsRawGnssPvtData rawGnssPvtData(void) const - { - XsRawGnssPvtData returnVal; - return *XsDataPacket_rawGnssPvtData(this, &returnVal); - } - - /*! \brief \copybrief XsDataPacket_containsRawGnssPvtData(const XsDataPacket*) */ - inline bool containsRawGnssPvtData(void) const - { - return 0 != XsDataPacket_containsRawGnssPvtData(this); - } - - /*! \brief \copybrief XsDataPacket_setRawGnssPvtData */ - XSNOEXPORT inline void setRawGnssPvtData(XsRawGnssPvtData const& raw) - { - XsDataPacket_setRawGnssPvtData(this, &raw); - } - - /*! \brief \copybrief XsDataPacket_gnssAge(const XsDataPacket*) */ - inline uint8_t gnssAge(void) const - { - return XsDataPacket_gnssAge(this); - } - - /*! \brief \copybrief XsDataPacket_containsGnssAge(const XsDataPacket*) */ - inline bool containsGnssAge(void) const - { - return 0 != XsDataPacket_containsGnssAge(this); - } - - /*! \brief \copybrief XsDataPacket_setGnssAge(XsDataPacket*, uint8_t) */ - XSNOEXPORT inline void setGnssAge(uint8_t age) - { - XsDataPacket_setGnssAge(this, age); - } - - /*! \brief \copybrief XsDataPacket_rawGnssSatInfo(const XsDataPacket*, XsRawGnssSatInfo*) - \return a struct with RawGnssSatInfo - */ - inline XsRawGnssSatInfo rawGnssSatInfo(void) const - { - XsRawGnssSatInfo returnVal; - return *XsDataPacket_rawGnssSatInfo(this, &returnVal); - } - - /*! \brief \copybrief XsDataPacket_containsRawGnssSatInfo(const XsDataPacket*) */ - inline bool containsRawGnssSatInfo(void) const - { - return 0 != XsDataPacket_containsRawGnssSatInfo(this); - } - - /*! \copydoc XsDataPacket_setRawGnssSatInfo(XsDataPacket*, const XsRawGnssSatInfo*) */ - XSNOEXPORT inline void setRawGnssSatInfo(XsRawGnssSatInfo const& data) - { - XsDataPacket_setRawGnssSatInfo(this, &data); - } - /*! \brief \copybrief XsDataPacket_fullSnapshot(const XsDataPacket*, XsSnapshot*) */ - inline XsSnapshot fullSnapshot(void) const - { - XsSnapshot returnVal; - return *XsDataPacket_fullSnapshot(this, &returnVal); - } - - /*! \brief \copybrief XsDataPacket_containsFullSnapshot(const XsDataPacket*) */ - inline bool containsFullSnapshot(void) const - { - return 0 != XsDataPacket_containsFullSnapshot(this); - } - - /*! \copydoc XsDataPacket_setFullSnapshot(XsDataPacket*, XsSnapshot const *, int) */ - XSNOEXPORT inline void setFullSnapshot(XsSnapshot const& data, bool retransmission) - { - XsDataPacket_setFullSnapshot(this, &data, retransmission?1:0); - } - /*! \brief \copybrief XsDataPacket_awindaSnapshot */ - XSNOEXPORT inline XsSnapshot awindaSnapshot(void) const - { - XsSnapshot returnVal; - return *XsDataPacket_awindaSnapshot(this, &returnVal); - } - - /*! \brief \copybrief XsDataPacket_containsAwindaSnapshot(const XsDataPacket*) */ - XSNOEXPORT inline bool containsAwindaSnapshot(void) const - { - return 0 != XsDataPacket_containsAwindaSnapshot(this); - } - - /*! \brief \copybrief XsDataPacket_setAwindaSnapshot */ - XSNOEXPORT inline void setAwindaSnapshot(XsSnapshot const& raw, bool retransmission) - { - XsDataPacket_setAwindaSnapshot(this, &raw, retransmission?1:0); - } - - /*! \brief \copybrief XsDataPacket_containsAwindaSnapshot(const XsDataPacket*) */ - inline bool isAwindaSnapshotARetransmission(void) const - { - return 0 != XsDataPacket_isAwindaSnapshotARetransmission(this); - } - - /*! \copydoc XsDataPacket_merge(XsDataPacket*, const XsDataPacket*, int) */ - inline XsDataPacket& merge(const XsDataPacket& other, bool overwrite = true) - { - return *XsDataPacket_merge(this, &other, overwrite?1:0); - } - - /*! \brief Set the time of arrival of the data packet - \param t The time of arrival - */ - inline void setTimeOfArrival(XsTimeStamp t) - { - m_toa = t; - } - - /*! \brief Return the time of arrival of the data packet. Only valid for live streams. The behaviour for file streams is undefined and may change in the future. */ - inline XsTimeStamp timeOfArrival() const - { - return m_toa; - } - - /*! \brief Set the estimated time of sampling of the data packet - \param t The estimated time of sampling - */ - inline void setEstimatedTimeOfSampling(XsTimeStamp t) - { - m_etos = t; - } - - /*! \brief Return the estimated time of sampling of the data packet. Only valid for live streams. The behaviour for file streams is undefined and may change in the future. */ - inline XsTimeStamp estimatedTimeOfSampling() const - { - return m_etos; - } - - /*! \brief Set the packet ID of the data packet - \param t The packet ID to set - */ - inline void setPacketId(int64_t t) - { - m_packetId = t; - } - - /*! \brief Return the ID of the packet. - \details This ID is based on, depending on availability: (1) packet counter (2) sample time (3) arrival order - \returns The ID of the packet. - */ - inline int64_t packetId() const - { - return m_packetId; - } - - /*! \copydoc XsDataPacket_setTriggerIndication(XsDataPacket*, XsDataIdentifier, const XsTriggerIndicationData *) */ - void setTriggerIndication(XsDataIdentifier triggerId, const XsTriggerIndicationData &triggerIndicationData) - { - XsDataPacket_setTriggerIndication(this, triggerId, &triggerIndicationData); - } - - /*! \copydoc XsDataPacket_containsTriggerIndication(const XsDataPacket*, XsDataIdentifier) */ - inline bool containsTriggerIndication(XsDataIdentifier triggerId) const - { - return 0 != XsDataPacket_containsTriggerIndication(this, triggerId); - } - - /*! \brief Returns the trigger indication data of a packet - \details - 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) - \returns the trigger indication data of a packet - */ - XsTriggerIndicationData triggerIndication(XsDataIdentifier triggerId) - { - XsTriggerIndicationData returnVal; - return *XsDataPacket_triggerIndication(this, triggerId, &returnVal); - } - - /*! \brief \copybrief XsDataPacket_rawBlob(const XsDataPacket*, XsByteArray*) */ - inline XsByteArray rawBlob(void) const - { - XsByteArray returnVal; - return *XsDataPacket_rawBlob(this, &returnVal); - } - - /*! \brief \copybrief XsDataPacket_containsRawBlob(const XsDataPacket*) */ - inline bool containsRawBlob(void) const - { - return 0 != XsDataPacket_containsRawBlob(this); - } - - /*! \copydoc XsDataPacket_setRawBlob(XsDataPacket*, const XsByteArray *) */ - inline void setRawBlob(const XsByteArray &data) - { - XsDataPacket_setRawBlob(this, &data); - } - - /*! \brief \copybrief XsDataPacket_accelerationHR(const XsDataPacket*, XsVector*) */ - inline XsVector accelerationHR(void) const - { - XsVector returnVal; - return *XsDataPacket_accelerationHR(this, &returnVal); - } - - /*! \copydoc XsDataPacket_containsAccelerationHR(const XsDataPacket*) */ - inline bool containsAccelerationHR(void) const - { - return 0 != XsDataPacket_containsAccelerationHR(this); - } - - /*! \copydoc XsDataPacket_setAccelerationHR(XsDataPacket*, const XsVector*) */ - inline void setAccelerationHR(const XsVector& vec) - { - XsDataPacket_setAccelerationHR(this, &vec); - } - - /*! \brief \copybrief XsDataPacket_rateOfTurnHR(const XsDataPacket*, XsVector*) */ - inline XsVector rateOfTurnHR(void) const - { - XsVector returnVal; - return *XsDataPacket_rateOfTurnHR(this, &returnVal); - } - - /*! \copydoc XsDataPacket_containsRateOfTurnHR(const XsDataPacket*) */ - inline bool containsRateOfTurnHR(void) const - { - return 0 != XsDataPacket_containsRateOfTurnHR(this); - } - - /*! \copydoc XsDataPacket_setRateOfTurnHR(XsDataPacket*, const XsVector*) */ - inline void setRateOfTurnHR(const XsVector& vec) - { - XsDataPacket_setRateOfTurnHR(this, &vec); - } - -#ifndef XSENS_NO_STL - /*! \brief Return the contents of the XsDataPacket in a simplified form - \details This function is meant for debugging purposes only and should not be used in production code. - \return A STL map with the contents of the packet identified by XsDataIdentifier - */ - inline std::map simplifiedContents() const - { - return *((std::map*)(void*)d); - } -#endif - -//protected: - /*! \privatesection */ -#endif // __cplusplus - struct XSNOEXPORT DataPacketPrivate* d; - - XsDeviceId m_deviceId; //!< The device Id to which the message belongs - XsTimeStamp m_toa; //!< Time Of Arrival (live packets only) - int64_t m_packetId; //!< 64 bit packet id, based on, depending on availability: (1) packet counter (2) sample time (3) arrival order - XsTimeStamp m_etos; //!< Estimated Time of Sampling (live packets only) -}; - -#endif diff --git a/extern/include/xsens/xsdebugcounters.h b/extern/include/xsens/xsdebugcounters.h deleted file mode 100644 index da432f4..0000000 --- a/extern/include/xsens/xsdebugcounters.h +++ /dev/null @@ -1,40 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSDEBUGCOUNTERS_H -#define XSDEBUGCOUNTERS_H - -#include "xsatomicint.h" - -// These counters are only available in the non-public version of the XsTypes library -// For the public version, they will all always have a 0 value. - -#ifdef __cplusplus -extern "C" { -#endif - -extern XsAtomicInt XSTYPES_DLL_API XsVector_allocCount; -extern XsAtomicInt XSTYPES_DLL_API XsVector_freeCount; - -extern XsAtomicInt XSTYPES_DLL_API XsMatrix_allocCount; -extern XsAtomicInt XSTYPES_DLL_API XsMatrix_freeCount; - -extern XsAtomicInt XSTYPES_DLL_API XsArray_allocCount; -extern XsAtomicInt XSTYPES_DLL_API XsArray_freeCount; - -#ifdef __cplusplus -} // extern "C" -#endif - -#endif diff --git a/extern/include/xsens/xsdeviceid.h b/extern/include/xsens/xsdeviceid.h deleted file mode 100644 index b51e430..0000000 --- a/extern/include/xsens/xsdeviceid.h +++ /dev/null @@ -1,603 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSDEVICEID_H -#define XSDEVICEID_H - -#include "xstypesconfig.h" -#include "pstdint.h" -#include "xsstring.h" -#ifdef __cplusplus -extern "C" { -#else -#define XSDEVICEID_INITIALIZER { 0 } -#endif - -struct XsDeviceId; - -XSTYPES_DLL_API void XsDeviceId_toString(struct XsDeviceId const* thisPtr, XsString* str); -XSTYPES_DLL_API void XsDeviceId_fromString(struct XsDeviceId* thisPtr, XsString const* str); -XSTYPES_DLL_API int XsDeviceId_isValid(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isXbusMaster(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isBodyPack(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isXbusMasterMotionTracker(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isWirelessMaster(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isMtw1(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isMtw2(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isMtw(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isMt9c(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isMtig(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isLegacyMtig(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isAwinda1(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isAwinda2(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isAwinda(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isAwinda1Station(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isAwinda1Dongle(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isAwinda1Oem(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isAwinda2Station(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isAwinda2Dongle(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isAwinda2Oem(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isAwindaStation(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isAwindaDongle(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isAwindaOem(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isSyncStation(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isSyncStation1(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isSyncStation2(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isMtMk4(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isMtMk4_X(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isMtMk4_1(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isMtMk4_2(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isMtMk4_3(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isMtMk4_X0(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isMtMk4_10(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isMtMk4_20(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isMtMk4_30(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isMtMk4_X00(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isMtMk4_100(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isMtMk4_200(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isMtMk4_300(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isMtMk4_400(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isMtMk4_500(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isMtMk4_600(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isMtMk4_700(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isMtMk4_710(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isMtMk4_800(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isMtMk4_900(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isMtMk5(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isMtMk5_X0(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isMtMk5_10(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isMtMk5_20(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isMtMk5_30(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isMtMk5_X00(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isMtMk5_100(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isMtMk5_200(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isMtMk5_300(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isMtMk5_710(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isMtx(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isMtx2(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isFis1100EvalKit(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isFis2100EvalKit(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isFisEvalKit(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isFmt_X000(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isFmt1000(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isFmt1010(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isFmt1020(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isFmt1030(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isImu(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isVru(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isAhrs(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isContainerDevice(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_containsBroadcast(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API int XsDeviceId_isBroadcast(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API uint32_t XsDeviceId_broadcast(void); -XSTYPES_DLL_API void XsDeviceId_swap(struct XsDeviceId* a, struct XsDeviceId* b); -XSTYPES_DLL_API int XsDeviceId_contains(struct XsDeviceId const* a, struct XsDeviceId const* b); -XSTYPES_DLL_API int XsDeviceId_isType(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API void XsDeviceId_typeName(struct XsDeviceId const* thisPtr, XsString* str); -XSTYPES_DLL_API uint32_t XsDeviceId_type(struct XsDeviceId const* thisPtr); -XSTYPES_DLL_API uint32_t XsDeviceId_deviceType(struct XsDeviceId const* thisPtr, int detailed); -XSTYPES_DLL_API uint32_t XsDeviceId_deviceTypeMask(struct XsDeviceId const* thisPtr, int detailed); - -#ifdef __cplusplus -} // extern "C" -#endif - -struct XsDeviceId { -#ifdef __cplusplus - /*! \brief Constructor that creates an XsDeviceId from the supplied \a deviceId */ - inline XsDeviceId(uint32_t deviceId = 0) - : m_deviceId(deviceId) - { - } - /*! \brief Constructor that creates an XsDeviceId from the supplied XsDeviceId \a other */ - inline XsDeviceId(const XsDeviceId& other) - : m_deviceId(other.m_deviceId) - { - } - - /*! \brief Assign the \a other deviceId to this XsDeviceId */ - inline const XsDeviceId& operator=(const XsDeviceId& other) - { - //lint --e{1529} trivial assignment - m_deviceId = other.m_deviceId; - return *this; - } - /*! \brief Assign the \a deviceId to this XsDeviceId */ - inline const XsDeviceId& operator=(uint32_t deviceId) - { - m_deviceId = deviceId; - return *this; - } - - /*! \brief Returns the deviceId as an unsigned integer */ - inline uint32_t toInt() const - { - return m_deviceId; - } - - /*! \brief Returns the deviceId as an XsString */ - inline XsString toString() const - { - XsString tmp; - XsDeviceId_toString(this, &tmp); - return tmp; - } - - /*! \brief Fills the deviceId with the parsed value from the supplied string \a s - \param s The string containing the device ID - */ - inline void fromString(const XsString &s) - { - XsDeviceId_fromString(this, &s); - } - - /*! \brief \copybrief XsDeviceId_isValid(const struct XsDeviceId*) */ - inline bool isValid() const - { - return 0 != XsDeviceId_isValid(this); - } - /*! \brief \copybrief XsDeviceId_isXbusMaster(const struct XsDeviceId*) */ - inline bool isXbusMaster() const - { - return 0 != XsDeviceId_isXbusMaster(this); - } - /*! \brief \copybrief XsDeviceId_isAwinda1(const struct XsDeviceId*) */ - inline bool isAwinda1() const - { - return 0 != XsDeviceId_isAwinda1(this); - } - /*! \brief \copybrief XsDeviceId_isAwinda2(const struct XsDeviceId*) */ - inline bool isAwinda2() const - { - return 0 != XsDeviceId_isAwinda2(this); - } - /*! \brief \copybrief XsDeviceId_isAwinda(const struct XsDeviceId*) */ - inline bool isAwinda() const - { - return 0 != XsDeviceId_isAwinda(this); - } - /*! \brief \copybrief XsDeviceId_isXbusMasterMotionTracker(const struct XsDeviceId*) */ - inline bool isXbusMasterMotionTracker() const - { - return 0 != XsDeviceId_isXbusMasterMotionTracker(this); - } - /*! \brief \copybrief XsDeviceId_isMtw1(const struct XsDeviceId*) */ - inline bool isMtw1() const - { - return 0 != XsDeviceId_isMtw1(this); - } - /*! \brief \copybrief XsDeviceId_isMtw2(const struct XsDeviceId*) */ - inline bool isMtw2() const - { - return 0 != XsDeviceId_isMtw2(this); - } - /*! \brief \copybrief XsDeviceId_isMtw(const struct XsDeviceId*) */ - inline bool isMtw() const - { - return 0 != XsDeviceId_isMtw(this); - } - /*! \brief \copybrief XsDeviceId_isMt9c(const struct XsDeviceId*) */ - inline bool isMt9c() const - { - return 0 != XsDeviceId_isMt9c(this); - } - /*! \brief \copybrief XsDeviceId_isLegacyMtig(const struct XsDeviceId*) */ - inline bool isLegacyMtig() const - { - return 0 != XsDeviceId_isLegacyMtig(this); - } - /*! \brief \copybrief XsDeviceId_isMtig(const struct XsDeviceId*) */ - inline bool isMtig() const - { - return 0 != XsDeviceId_isMtig(this); - } - /*! \brief \copybrief XsDeviceId_isMtMk4(const struct XsDeviceId*) */ - inline bool isMtMk4() const - { - return 0 != XsDeviceId_isMtMk4(this); - } - /*! \brief \copybrief XsDeviceId_isMtMk4_X(const struct XsDeviceId*) */ - inline bool isMtMk4_X() const - { - return 0 != XsDeviceId_isMtMk4_X(this); - } - /*! \brief \copybrief XsDeviceId_isMtMk4_1(const struct XsDeviceId*) */ - inline bool isMtMk4_1() const - { - return 0 != XsDeviceId_isMtMk4_1(this); - } - /*! \brief \copybrief XsDeviceId_isMtMk4_2(const struct XsDeviceId*) */ - inline bool isMtMk4_2() const - { - return 0 != XsDeviceId_isMtMk4_2(this); - } - /*! \brief \copybrief XsDeviceId_isMtMk4_3(const struct XsDeviceId*) */ - inline bool isMtMk4_3() const - { - return 0 != XsDeviceId_isMtMk4_3(this); - } - /*! \brief \copybrief XsDeviceId_isMtMk4_X0(const struct XsDeviceId*) */ - inline bool isMtMk4_X0() const - { - return 0 != XsDeviceId_isMtMk4_X0(this); - } - /*! \brief \copybrief XsDeviceId_isMtMk4_10(const struct XsDeviceId*) */ - inline bool isMtMk4_10() const - { - return 0 != XsDeviceId_isMtMk4_10(this); - } - /*! \brief \copybrief XsDeviceId_isMtMk4_20(const struct XsDeviceId*) */ - inline bool isMtMk4_20() const - { - return 0 != XsDeviceId_isMtMk4_20(this); - } - /*! \brief \copybrief XsDeviceId_isMtMk4_30(const struct XsDeviceId*) */ - inline bool isMtMk4_30() const - { - return 0 != XsDeviceId_isMtMk4_30(this); - } - /*! \brief \copybrief XsDeviceId_isMtMk4_X00(const struct XsDeviceId*) */ - inline bool isMtMk4_X00() const - { - return 0 != XsDeviceId_isMtMk4_X00(this); - } - /*! \brief \copybrief XsDeviceId_isMtMk4_100(const struct XsDeviceId*) */ - inline bool isMtMk4_100() const - { - return 0 != XsDeviceId_isMtMk4_100(this); - } - /*! \brief \copybrief XsDeviceId_isMtMk4_200(const struct XsDeviceId*) */ - inline bool isMtMk4_200() const - { - return 0 != XsDeviceId_isMtMk4_200(this); - } - /*! \brief \copybrief XsDeviceId_isMtMk4_300(const struct XsDeviceId*) */ - inline bool isMtMk4_300() const - { - return 0 != XsDeviceId_isMtMk4_300(this); - } - /*! \brief \copybrief XsDeviceId_isMtMk4_400(const struct XsDeviceId*) */ - inline bool isMtMk4_400() const - { - return 0 != XsDeviceId_isMtMk4_400(this); - } - /*! \brief \copybrief XsDeviceId_isMtMk4_500(const struct XsDeviceId*) */ - inline bool isMtMk4_500() const - { - return 0 != XsDeviceId_isMtMk4_500(this); - } - /*! \brief \copybrief XsDeviceId_isMtMk4_600(const struct XsDeviceId*) */ - inline bool isMtMk4_600() const - { - return 0 != XsDeviceId_isMtMk4_600(this); - } - /*! \brief \copybrief XsDeviceId_isMtMk4_700(const struct XsDeviceId*) */ - inline bool isMtMk4_700() const - { - return 0 != XsDeviceId_isMtMk4_700(this); - } - /*! \brief \copybrief XsDeviceId_isMtMk4_710(const struct XsDeviceId*) */ - inline bool isMtMk4_710() const - { - return 0 != XsDeviceId_isMtMk4_710(this); - } - /*! \brief \copybrief XsDeviceId_isMtMk4_800(const struct XsDeviceId*) */ - inline bool isMtMk4_800() const - { - return 0 != XsDeviceId_isMtMk4_800(this); - } - /*! \brief \copybrief XsDeviceId_isMtMk4_900(const struct XsDeviceId*) */ - inline bool isMtMk4_900() const - { - return 0 != XsDeviceId_isMtMk4_900(this); - } - /*! \brief \copybrief XsDeviceId_isMtMk5(const struct XsDeviceId*) */ - inline bool isMtMk5() const - { - return 0 != XsDeviceId_isMtMk5(this); - } - /*! \brief \copybrief XsDeviceId_isMtMk5_X0(const struct XsDeviceId*) */ - inline bool isMtMk5_X0() const - { - return 0 != XsDeviceId_isMtMk5_X0(this); - } - /*! \brief \copybrief XsDeviceId_isMtMk5_10(const struct XsDeviceId*) */ - inline bool isMtMk5_10() const - { - return 0 != XsDeviceId_isMtMk5_10(this); - } - /*! \brief \copybrief XsDeviceId_isMtMk5_20(const struct XsDeviceId*) */ - inline bool isMtMk5_20() const - { - return 0 != XsDeviceId_isMtMk5_20(this); - } - /*! \brief \copybrief XsDeviceId_isMtMk5_30(const struct XsDeviceId*) */ - inline bool isMtMk5_30() const - { - return 0 != XsDeviceId_isMtMk5_30(this); - } - /*! \brief \copybrief XsDeviceId_isMtMk5_X00(const struct XsDeviceId*) */ - inline bool isMtMk5_X00() const - { - return 0 != XsDeviceId_isMtMk5_X00(this); - } - /*! \brief \copybrief XsDeviceId_isMtMk5_100(const struct XsDeviceId*) */ - inline bool isMtMk5_100() const - { - return 0 != XsDeviceId_isMtMk5_100(this); - } - /*! \brief \copybrief XsDeviceId_isMtMk5_200(const struct XsDeviceId*) */ - inline bool isMtMk5_200() const - { - return 0 != XsDeviceId_isMtMk5_200(this); - } - /*! \brief \copybrief XsDeviceId_isMtMk5_300(const struct XsDeviceId*) */ - inline bool isMtMk5_300() const - { - return 0 != XsDeviceId_isMtMk5_300(this); - } - /*! \brief \copybrief XsDeviceId_isMtMk5_710(const struct XsDeviceId*) */ - inline bool isMtMk5_710() const - { - return 0 != XsDeviceId_isMtMk5_710(this); - } - /*! \brief \copybrief XsDeviceId_isMtx2(const struct XsDeviceId*) */ - inline bool isMtx2() const - { - return 0 != XsDeviceId_isMtx2(this); - } - /*! \brief \copybrief XsDeviceId_isMtx(const struct XsDeviceId*) */ - inline bool isMtx() const - { - return 0 != XsDeviceId_isMtx(this); - } - /*! \brief \copybrief XsDeviceId_isWirelessMaster(const struct XsDeviceId*) */ - inline bool isWirelessMaster() const - { - return 0 != XsDeviceId_isWirelessMaster(this); - } - /*! \brief \copybrief XsDeviceId_isAwinda1Station(const struct XsDeviceId*) */ - inline bool isAwinda1Station() const - { - return 0 != XsDeviceId_isAwinda1Station(this); - } - /*! \brief \copybrief XsDeviceId_isAwinda1Dongle(const struct XsDeviceId*) */ - inline bool isAwinda1Dongle() const - { - return 0 != XsDeviceId_isAwinda1Dongle(this); - } - /*! \brief \copybrief XsDeviceId_isAwinda1Oem(const struct XsDeviceId*)*/ - inline bool isAwinda1Oem() const - { - return 0 != XsDeviceId_isAwinda1Oem(this); - } - /*! \brief \copybrief XsDeviceId_isAwinda2Station(const struct XsDeviceId*) */ - inline bool isAwinda2Station() const - { - return 0 != XsDeviceId_isAwinda2Station(this); - } - /*! \brief \copybrief XsDeviceId_isAwinda2Dongle(const struct XsDeviceId*) */ - inline bool isAwinda2Dongle() const - { - return 0 != XsDeviceId_isAwinda2Dongle(this); - } - /*! \brief \copybrief XsDeviceId_isAwinda2Oem(const struct XsDeviceId*) */ - inline bool isAwinda2Oem() const - { - return 0 != XsDeviceId_isAwinda2Oem(this); - } - /*! \brief \copybrief XsDeviceId_isAwindaStation(const struct XsDeviceId*) */ - inline bool isAwindaStation() const - { - return 0 != XsDeviceId_isAwindaStation(this); - } - /*! \brief \copybrief XsDeviceId_isAwindaDongle(const struct XsDeviceId*) */ - inline bool isAwindaDongle() const - { - return 0 != XsDeviceId_isAwindaDongle(this); - } - /*! \brief \copybrief XsDeviceId_isAwindaOem(const struct XsDeviceId*) */ - inline bool isAwindaOem() const - { - return 0 != XsDeviceId_isAwindaOem(this); - } - /*! \brief \copybrief XsDeviceId_isSyncStation(const struct XsDeviceId*) */ - inline bool isSyncStation() const - { - return 0 != XsDeviceId_isSyncStation(this); - } - /*! \brief \copybrief XsDeviceId_isBodyPack(const struct XsDeviceId*) */ - inline bool isBodyPack() const - { - return 0 != XsDeviceId_isBodyPack(this); - } - /*! \brief \copybrief XsDeviceId_isFis1100EvalKit(const struct XsDeviceId*) */ - inline bool isFis1100EvalKit() const - { - return 0 != XsDeviceId_isFis1100EvalKit(this); - } - /*! \brief \copybrief XsDeviceId_isFis2100EvalKit(const struct XsDeviceId*) */ - inline bool isFis2100EvalKit() const - { - return 0 != XsDeviceId_isFis2100EvalKit(this); - } - /*! \brief \copybrief XsDeviceId_isFisEvalKit(const struct XsDeviceId*) */ - inline bool isFisEvalKit() const - { - return 0 != XsDeviceId_isFisEvalKit(this); - } - /*! \brief \copybrief XsDeviceId_isFmt_X000(const struct XsDeviceId*) */ - inline bool isFmt_X000() const - { - return 0 != XsDeviceId_isFmt1000(this); - } - /*! \brief \copybrief XsDeviceId_isFmt1000(const struct XsDeviceId*) */ - inline bool isFmt1000() const - { - return 0 != XsDeviceId_isFmt1000(this); - } - /*! \brief \copybrief XsDeviceId_isFmt1010(const struct XsDeviceId*) */ - inline bool isFmt1010() const - { - return 0 != XsDeviceId_isFmt1010(this); - } - /*! \brief \copybrief XsDeviceId_isFmt1020(const struct XsDeviceId*) */ - inline bool isFmt1020() const - { - return 0 != XsDeviceId_isFmt1020(this); - } - /*! \brief \copybrief XsDeviceId_isFmt1030(const struct XsDeviceId*) */ - inline bool isFmt1030() const - { - return 0 != XsDeviceId_isFmt1030(this); - } - /*! \brief \copybrief XsDeviceId_isImu(const struct XsDeviceId*) */ - inline bool isImu() const - { - return 0 != XsDeviceId_isImu(this); - } - /*! \brief \copybrief XsDeviceId_isVru(const struct XsDeviceId*) */ - inline bool isVru() const - { - return 0 != XsDeviceId_isVru(this); - } - /*! \brief \copybrief XsDeviceId_isAhrs(const struct XsDeviceId*) */ - inline bool isAhrs() const - { - return 0 != XsDeviceId_isAhrs(this); - } - /*! \brief \copybrief XsDeviceId_isContainerDevice(const struct XsDeviceId*) */ - inline bool isContainerDevice() const - { - return 0 != XsDeviceId_isContainerDevice(this); - } - /*! \copydoc XsDeviceId_containsBroadcast(const XsDeviceId*) */ - inline bool containsBroadcast() const - { - return 0 != XsDeviceId_containsBroadcast(this); - } - /*! \copydoc XsDeviceId_isBroadcast(const XsDeviceId*) */ - inline bool isBroadcast() const - { - return 0 != XsDeviceId_isBroadcast(this); - } - - /*! \brief Returns true if the \a other deviceId matches this deviceId */ - inline bool operator==(const XsDeviceId& other) const { return m_deviceId == other.m_deviceId; } - /*! \brief Returns true if the \a other deviceId does not match this deviceId */ - inline bool operator!=(const XsDeviceId& other) const { return m_deviceId != other.m_deviceId; } - /*! \brief Returns true if this deviceId is less than the \a other deviceId */ - inline bool operator<(const XsDeviceId& other) const { return m_deviceId < other.m_deviceId; } - /*! \brief Returns true if this deviceId is less or equal to the \a other deviceId */ - inline bool operator<=(const XsDeviceId& other) const { return m_deviceId <= other.m_deviceId; } - /*! \brief Returns true if this deviceId is larger than the \a other deviceId */ - inline bool operator>(const XsDeviceId& other) const { return m_deviceId > other.m_deviceId; } - /*! \brief Returns true if this deviceId is larger or equal to the \a other deviceId */ - inline bool operator>=(const XsDeviceId& other) const { return m_deviceId >= other.m_deviceId; } - - /*! \brief Creates and returns a XsDeviceId representing the broadcast deviceId */ - inline static XsDeviceId broadcast() - { - return XsDeviceId(XsDeviceId_broadcast()); - } - - /*! \copydoc XsDeviceId_contains(XsDeviceId const*, XsDeviceId const*) */ - inline bool contains(const XsDeviceId& other) const - { - return 0 != XsDeviceId_contains(this, &other); - } - - /*! \brief Returns true if the ID is just a device type, not an actual device ID */ - inline bool isType() const - { - return 0 != XsDeviceId_isType(this); - } - - /*! \brief Returns the type of device identified by this id */ - inline uint32_t type() const - { - return XsDeviceId_type(this); - } - - /*! \brief Returns the (detailed) device type of this id - \param detailed Boolean whether detailed information is returned - \return The requested device type - */ - inline uint32_t deviceType(bool detailed = true) const - { - return XsDeviceId_deviceType(this, detailed ? 1 : 0); - } - - /*! \brief Returns the detailed device type mask of this id - \param detailed Boolean whether detailed information is returned - \return The requested device type mask - */ - inline uint32_t deviceTypeMask(bool detailed = true) const - { - return XsDeviceId_deviceTypeMask(this, detailed ? 1 : 0); - } - - /*! \brief Returns the name of the type of device identified by this id */ - inline XsString typeName() const - { - XsString rv; - XsDeviceId_typeName(this, &rv); - return rv; - } - -private: -#endif - uint32_t m_deviceId; //!< The actual device id -}; - -typedef struct XsDeviceId XsDeviceId; - -#if defined(__cplusplus) && !defined(XSENS_NO_STL) -namespace std { -template -basic_ostream<_CharT, _Traits>& operator<<(basic_ostream<_CharT, _Traits>& o, XsDeviceId const& xd) -{ - return (o << xd.toString()); -} -} - -inline XsString& operator<<(XsString& o, XsDeviceId const& xd) -{ - o.append(xd.toString()); - return o; -} - -#endif - -#endif diff --git a/extern/include/xsens/xsdeviceidarray.h b/extern/include/xsens/xsdeviceidarray.h deleted file mode 100644 index 0b8cf00..0000000 --- a/extern/include/xsens/xsdeviceidarray.h +++ /dev/null @@ -1,68 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSDEVICEIDARRAY_H -#define XSDEVICEIDARRAY_H - -#include "xsarray.h" -#include "xsdeviceid.h" - -#ifdef __cplusplus -extern "C" { -#endif - -extern XsArrayDescriptor const XSTYPES_DLL_API g_xsDeviceIdArrayDescriptor; - -#ifndef __cplusplus -#define XSDEVICEIDARRAY_INITIALIZER XSARRAY_INITIALIZER(&g_xsDeviceIdArrayDescriptor) -XSARRAY_STRUCT(XsDeviceIdArray, XsDeviceId); -typedef struct XsDeviceIdArray XsDeviceIdArray; - -XSTYPES_DLL_API void XsDeviceIdArray_construct(XsDeviceIdArray* thisPtr, XsSize count, XsDeviceId const* src); -#else -} // extern "C" -#endif - -#ifdef __cplusplus -struct XsDeviceIdArray : public XsArrayImpl { - //! \brief Constructs an XsDeviceIdArray - inline explicit XsDeviceIdArray(XsSize sz = 0, XsDeviceId const* src = 0) - : ArrayImpl(sz, src) - { - } - - //! \brief Constructs an XsDeviceIdArray as a copy of \a other - inline XsDeviceIdArray(XsDeviceIdArray const& other) - : ArrayImpl(other) - { - } - - //! \brief Constructs an XsDeviceIdArray that references the data supplied in \a ref - inline explicit XsDeviceIdArray(XsDeviceId* ref, XsSize sz, XsDataFlags flags = XSDF_None) - : ArrayImpl(ref, sz, flags) - { - } -#ifndef XSENS_NOITERATOR - //! \brief Constructs an XsDeviceIdArray with the array bound by the supplied iterators \a beginIt and \a endIt - template - inline XsDeviceIdArray(Iterator beginIt, Iterator endIt) - : ArrayImpl(beginIt, endIt) - { - } -#endif -}; -#endif - - -#endif diff --git a/extern/include/xsens/xsdeviceidlist.h b/extern/include/xsens/xsdeviceidlist.h deleted file mode 100644 index db9d37a..0000000 --- a/extern/include/xsens/xsdeviceidlist.h +++ /dev/null @@ -1,36 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSDEVICEIDLIST_H -#define XSDEVICEIDLIST_H - -#include "xsdeviceidarray.h" - -#define XsDeviceIdList XsDeviceIdArray - -#ifndef __cplusplus -// obsolete: -#define XSDEVICEIDLIST_INITIALIZER XSDEVICEIDARRAY_INITIALIZER -#define XsDeviceIdList_construct(thisPtr, sz, src) XsDeviceIdArray_construct(thisPtr, sz, src) -#define XsDeviceIdList_assign(thisPtr, sz, src) XsArray_assign(thisPtr, sz, src) -#define XsDeviceIdList_destruct(thisPtr) XsArray_destruct(thisPtr) -#define XsDeviceIdList_copy(thisPtr, copy) XsArray_copy(copy, thisPtr) -#define XsDeviceIdList_append(thisPtr, other) XsArray_append(thisPtr, other) -#define XsDeviceIdList_popFront(thisPtr, count) XsArray_erase(thisPtr, 0, count) -#define XsDeviceIdList_popBack(thisPtr, count) XsArray_erase(thisPtr, (XsSize)-1, count) -#define XsDeviceIdList_swap(a, b) XsArray_swap(a, b) -#define XsDeviceIdList_erase(thisPtr, index, count) XsArray_erase(thisPtr, index, count) - -#endif -#endif diff --git a/extern/include/xsens/xsdid.h b/extern/include/xsens/xsdid.h deleted file mode 100644 index 2352450..0000000 --- a/extern/include/xsens/xsdid.h +++ /dev/null @@ -1,189 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSDID_H -#define XSDID_H - -// DID Type (high nibble) -#define XS_DID_TYPEL_MASK 0x000F0000 -#define XS_DID_TYPEH_MASK 0x00F00000 -#define XS_DID_GPL_MASK 0x0F000000 -#define XS_DID_GPH_MASK 0xF0000000 -#define XS_DID_TYPE_MASK (XS_DID_TYPEH_MASK | XS_DID_TYPEL_MASK) -#define XS_DID_GP_MASK (XS_DID_GPH_MASK | XS_DID_GPL_MASK) -#define XS_DID_MK4TYPE_MASK (XS_DID_TYPEH_MASK | XS_DID_GPL_MASK) -#define XS_DID_ID_MASK 0x0000FFFF -#define XS_DID_FULLTYPE_MASK 0xFFFF0000 - -#define XS_DID_TYPEL_SHIFT 16 -#define XS_DID_TYPEH_SHIFT 20 -#define XS_DID_GPL_SHIFT 24 -#define XS_DID_GPH_SHIFT 28 - -#define XS_DID_TYPEH_INTERNAL 0x00000000 -#define XS_DID_TYPEH_XM 0x00100000 -#define XS_DID_TYPEH_AWINDAMASTER 0x00200000 -#define XS_DID_TYPEH_MTI_MTX 0x00300000 -#define XS_DID_TYPEH_MTIG 0x00500000 -#define XS_DID_TYPEH_MT_X0 0x00600000 -#define XS_DID_TYPEH_MT_X00 0x00700000 -#define XS_DID_TYPEH_MTX2_MTW2 0x00B00000 -#define XS_DID_TYPEH_BODYPACK 0x00A00000 -#define XS_DID_TYPEH_MT_X_MPU 0x00800000 -#define XS_DID_TYPEH_MT_X_FIS 0x00C00000 - -#define XS_DID_TYPEL_STATION 0x00000000 -#define XS_DID_TYPEL_DONGLE 0x00010000 -#define XS_DID_TYPEL_OEM 0x00020000 -#define XS_DID_TYPEL_SYNCSTATION 0x00030000 -#define XS_DID_TYPEL_BUS_MASTER 0x00040000 - -#define XS_DID_TYPEL_RS232 0x00000000 -#define XS_DID_TYPEL_RS422 0x00010000 -#define XS_DID_TYPEL_RS485XM 0x00020000 -#define XS_DID_TYPEL_RS485 0x00030000 -#define XS_DID_TYPEL_WIRELESS 0x00040000 -#define XS_DID_TYPEL_MULTI 0x00080000 -#define XS_DID_TYPEL_MK5 0x00080000 - -#define XS_DID_GPL_1 0x01000000 -#define XS_DID_GPL_2 0x02000000 -#define XS_DID_GPL_3 0x03000000 -#define XS_DID_GPL_10 0x01000000 -#define XS_DID_GPL_20 0x02000000 -#define XS_DID_GPL_30 0x03000000 -#define XS_DID_GPL_100 0x01000000 -#define XS_DID_GPL_200 0x02000000 -#define XS_DID_GPL_300 0x03000000 -#define XS_DID_GPL_400 0x04000000 -#define XS_DID_GPL_500 0x05000000 -#define XS_DID_GPL_600 0x06000000 -#define XS_DID_GPL_700 0x07000000 -#define XS_DID_GPL_800 0x08000000 -#define XS_DID_GPL_900 0x09000000 -#define XS_DID_GPL_IMU 0x01000000 -#define XS_DID_GPL_VRU 0x02000000 -#define XS_DID_GPL_AHRS 0x03000000 -#define XS_DID_GPL_AWINDA2 0x01000000 -#define XS_DID_GPL_AHRSGNSS XS_DID_GPL_400 -#define XS_DID_GPL_AHRSGNSSG XS_DID_GPL_500 -#define XS_DID_GPL_GNSSINS XS_DID_GPL_600 -#define XS_DID_GPL_GNSSINSG XS_DID_GPL_700 - -#define XS_DID_TYPE_XBUS_MASTER (XS_DID_TYPEH_XM | 0x00030000) - -#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_OEM (XS_DID_TYPEH_AWINDAMASTER | XS_DID_TYPEL_OEM) -#define XS_DID_TYPE_SYNCSTATION (XS_DID_TYPEH_AWINDAMASTER | XS_DID_TYPEL_SYNCSTATION) - -#define XS_DID_FULLTYPE_AWINDA1_STATION XS_DID_TYPE_AWINDA_STATION -#define XS_DID_FULLTYPE_AWINDA1_DONGLE XS_DID_TYPE_AWINDA_DONGLE -#define XS_DID_FULLTYPE_AWINDA1_OEM XS_DID_TYPE_AWINDA_OEM -#define XS_DID_FULLTYPE_SYNCSTATION1 XS_DID_TYPE_SYNCSTATION - -#define XS_DID_FULLTYPE_AWINDA2_STATION (XS_DID_GPL_AWINDA2 | XS_DID_TYPE_AWINDA_STATION) -#define XS_DID_FULLTYPE_AWINDA2_DONGLE (XS_DID_GPL_AWINDA2 | XS_DID_TYPE_AWINDA_DONGLE) -#define XS_DID_FULLTYPE_AWINDA2_OEM (XS_DID_GPL_AWINDA2 | XS_DID_TYPE_AWINDA_OEM) -#define XS_DID_FULLTYPE_SYNCSTATION2 (XS_DID_GPL_AWINDA2 | XS_DID_TYPE_SYNCSTATION) - -#define XS_DID_TYPE_MTW1 (XS_DID_TYPEH_MTI_MTX | XS_DID_TYPEL_WIRELESS) -#define XS_DID_TYPE_MTIX_RS232 (XS_DID_TYPEH_MTI_MTX | XS_DID_TYPEL_RS232) -#define XS_DID_TYPE_MTIX_RS422 (XS_DID_TYPEH_MTI_MTX | XS_DID_TYPEL_RS422) -#define XS_DID_TYPE_MTIX_RS485 (XS_DID_TYPEH_MTI_MTX | XS_DID_TYPEL_RS485) -#define XS_DID_TYPE_MTX_XBUS (XS_DID_TYPEH_MTI_MTX | XS_DID_TYPEL_RS485XM) -#define XS_DID_TYPE_MTX2 (XS_DID_TYPEH_MTX2_MTW2 | XS_DID_TYPEL_RS485XM) -#define XS_DID_TYPE_MTW2 (XS_DID_TYPEH_MTX2_MTW2 | XS_DID_TYPEL_WIRELESS) - -#define XS_DID_MK4TYPE_MT_1_FIS (XS_DID_TYPEH_MT_X_FIS | XS_DID_GPL_1) -#define XS_DID_MK4TYPE_MT_2_FIS (XS_DID_TYPEH_MT_X_FIS | XS_DID_GPL_2) -#define XS_DID_MK4TYPE_MT_3_FIS (XS_DID_TYPEH_MT_X_FIS | XS_DID_GPL_3) -#define XS_DID_MK4TYPE_MT_1_MPU (XS_DID_TYPEH_MT_X_MPU | XS_DID_GPL_1) -#define XS_DID_MK4TYPE_MT_2_MPU (XS_DID_TYPEH_MT_X_MPU | XS_DID_GPL_2) -#define XS_DID_MK4TYPE_MT_3_MPU (XS_DID_TYPEH_MT_X_MPU | XS_DID_GPL_3) -#define XS_DID_MK4TYPE_MT_10 (XS_DID_TYPEH_MT_X0 | XS_DID_GPL_10) -#define XS_DID_MK4TYPE_MT_20 (XS_DID_TYPEH_MT_X0 | XS_DID_GPL_20) -#define XS_DID_MK4TYPE_MT_30 (XS_DID_TYPEH_MT_X0 | XS_DID_GPL_30) -#define XS_DID_MK4TYPE_MT_100 (XS_DID_TYPEH_MT_X00 | XS_DID_GPL_100) -#define XS_DID_MK4TYPE_MT_200 (XS_DID_TYPEH_MT_X00 | XS_DID_GPL_200) -#define XS_DID_MK4TYPE_MT_300 (XS_DID_TYPEH_MT_X00 | XS_DID_GPL_300) -#define XS_DID_MK4TYPE_MT_400 (XS_DID_TYPEH_MT_X00 | XS_DID_GPL_400) -#define XS_DID_MK4TYPE_MT_500 (XS_DID_TYPEH_MT_X00 | XS_DID_GPL_500) -#define XS_DID_MK4TYPE_MT_600 (XS_DID_TYPEH_MT_X00 | XS_DID_GPL_600) -#define XS_DID_MK4TYPE_MT_700 (XS_DID_TYPEH_MT_X00 | XS_DID_GPL_700) -#define XS_DID_MK4TYPE_MT_800 (XS_DID_TYPEH_MT_X00 | XS_DID_GPL_800) -#define XS_DID_MK4TYPE_MT_900 (XS_DID_TYPEH_MT_X00 | XS_DID_GPL_900) - -#define XS_DID_MK5TYPE_MT_10 (XS_DID_TYPEH_MT_X0 | XS_DID_GPL_10 | XS_DID_TYPEL_MK5) -#define XS_DID_MK5TYPE_MT_20 (XS_DID_TYPEH_MT_X0 | XS_DID_GPL_20 | XS_DID_TYPEL_MK5) -#define XS_DID_MK5TYPE_MT_30 (XS_DID_TYPEH_MT_X0 | XS_DID_GPL_30 | XS_DID_TYPEL_MK5) -#define XS_DID_MK5TYPE_MT_100 (XS_DID_TYPEH_MT_X00 | XS_DID_GPL_100 | XS_DID_TYPEL_MK5) -#define XS_DID_MK5TYPE_MT_200 (XS_DID_TYPEH_MT_X00 | XS_DID_GPL_200 | XS_DID_TYPEL_MK5) -#define XS_DID_MK5TYPE_MT_300 (XS_DID_TYPEH_MT_X00 | XS_DID_GPL_300 | XS_DID_TYPEL_MK5) -#define XS_DID_MK5TYPE_MT_400 (XS_DID_TYPEH_MT_X00 | XS_DID_GPL_400 | XS_DID_TYPEL_MK5) -#define XS_DID_MK5TYPE_MT_500 (XS_DID_TYPEH_MT_X00 | XS_DID_GPL_500 | XS_DID_TYPEL_MK5) -#define XS_DID_MK5TYPE_MT_600 (XS_DID_TYPEH_MT_X00 | XS_DID_GPL_600 | XS_DID_TYPEL_MK5) -#define XS_DID_MK5TYPE_MT_700 (XS_DID_TYPEH_MT_X00 | XS_DID_GPL_700 | XS_DID_TYPEL_MK5) -#define XS_DID_MK5TYPE_MT_800 (XS_DID_TYPEH_MT_X00 | XS_DID_GPL_800 | XS_DID_TYPEL_MK5) -#define XS_DID_MK5TYPE_MT_900 (XS_DID_TYPEH_MT_X00 | XS_DID_GPL_900 | XS_DID_TYPEL_MK5) - -#define XS_DID_BROADCAST 0x80000000 -#define XS_DID_MASTER 0x00000000 -#define XS_DID_ABMCLOCKMASTER 0x00000100 - -#define XS_DID_INTERNAL(did) ((did & XS_DID_TYPEH_MASK) == XS_DID_TYPEH_INTERNAL) -#define XS_DID_XM(did) ((did & XS_DID_TYPEH_MASK) == XS_DID_TYPEH_XM) -#define XS_DID_NOXM(did) ((did & XS_DID_TYPEH_MASK) != XS_DID_TYPEH_XM) -#define XS_DID_WM(did) ((did & XS_DID_TYPEH_MASK) == XS_DID_TYPEH_AWINDAMASTER) -#define XS_DID_NOWM(did) ((did & XS_DID_TYPEH_MASK) != XS_DID_TYPEH_AWINDAMASTER) -#define XS_DID_WMMT(did) ((did & XS_DID_TYPE_MASK) == XS_DID_TYPE_MTW1) -#define XS_DID_NOWMMT(did) ((did & XS_DID_TYPE_MASK) != XS_DID_TYPE_MTW1) -#define XS_DID_XMMT(did) ((did & XS_DID_TYPE_MASK) == XS_DID_TYPE_MTX_XBUS) -#define XS_DID_NOXMMT(did) ((did & XS_DID_TYPE_MASK) != XS_DID_TYPE_MTX_XBUS) -#define XS_DID_MTIX(did) (((did & XS_DID_TYPEH_MASK) == XS_DID_TYPEH_MTI_MTX) && XS_DID_NOWMMT(did)) -#define XS_DID_MTIG(did) (((did & XS_DID_TYPEH_MASK) == XS_DID_TYPEH_MTIG) || ((did & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_700)) -#define XS_DID_MTW2(did) ((did & XS_DID_TYPE_MASK) == XS_DID_TYPE_MTW2) -#define XS_DID_MTX2(did) ((did & XS_DID_TYPE_MASK) == XS_DID_TYPE_MTX2) - -#define XS_DID_AWINDA1_STATION(did) ((did & XS_DID_FULLTYPE_MASK) == XS_DID_FULLTYPE_AWINDA1_STATION) -#define XS_DID_AWINDA1_DONGLE(did) ((did & XS_DID_FULLTYPE_MASK) == XS_DID_FULLTYPE_AWINDA1_DONGLE) -#define XS_DID_AWINDA1_OEM(did) ((did & XS_DID_FULLTYPE_MASK) == XS_DID_FULLTYPE_AWINDA1_OEM) - -#define XS_DID_AWINDA2_STATION(did) ((did & XS_DID_FULLTYPE_MASK) == XS_DID_FULLTYPE_AWINDA2_STATION) -#define XS_DID_AWINDA2_DONGLE(did) ((did & XS_DID_FULLTYPE_MASK) == XS_DID_FULLTYPE_AWINDA2_DONGLE) -#define XS_DID_AWINDA2_OEM(did) ((did & XS_DID_FULLTYPE_MASK) == XS_DID_FULLTYPE_AWINDA2_OEM) - -#define XS_DID_SYNCSTATION1(did) ((did & XS_DID_FULLTYPE_MASK) == XS_DID_FULLTYPE_SYNCSTATION1) -#define XS_DID_SYNCSTATION2(did) ((did & XS_DID_FULLTYPE_MASK) == XS_DID_FULLTYPE_SYNCSTATION2) -#define XS_DID_SYNCSTATION(did) (XS_DID_SYNCSTATION1(did) || XS_DID_SYNCSTATION2(did)) - -#define XS_DID_AWINDA1(did) (XS_DID_AWINDA1_STATION(did) || XS_DID_AWINDA1_DONGLE(did) || XS_DID_AWINDA1_OEM(did)) -#define XS_DID_AWINDA2(did) (XS_DID_AWINDA2_STATION(did) || XS_DID_AWINDA2_DONGLE(did) || XS_DID_AWINDA2_OEM(did)) - -#define XS_DID_BODYPACK(did) ((did & XS_DID_TYPEH_MASK) == XS_DID_TYPEH_BODYPACK) - -#define XS_DID_MK4TYPE_MT_710_RANGE_START (XS_DID_TYPEH_MT_X00 | XS_DID_GPL_700 | 0x1000) -#define XS_DID_MK5TYPE_RANGE_START 0x00002000 - -/* fairchild devices */ -#define XS_DID_TYPEH_FAIRCHILD_DEVICE 0x00F00000 -#define XS_DID_GPL_FIS1100EVK 0x00000000 -#define XS_DID_GPL_FIS2100EVK 0x01000000 -#define XS_DID_TYPE_FIS1100EVK (XS_DID_TYPEH_FAIRCHILD_DEVICE|XS_DID_GPL_FIS1100EVK) -#define XS_DID_TYPE_FIS2100EVK (XS_DID_TYPEH_FAIRCHILD_DEVICE|XS_DID_GPL_FIS2100EVK) -#define XS_DID_FIS1100EVK(did) ((did & (XS_DID_TYPEH_MASK | XS_DID_GPL_MASK)) == XS_DID_TYPE_FIS1100EVK) -#define XS_DID_FIS2100EVK(did) ((did & (XS_DID_TYPEH_MASK | XS_DID_GPL_MASK)) == XS_DID_TYPE_FIS2100EVK) - -#endif // file guard diff --git a/extern/include/xsens/xseuler.h b/extern/include/xsens/xseuler.h deleted file mode 100644 index e414832..0000000 --- a/extern/include/xsens/xseuler.h +++ /dev/null @@ -1,157 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSEULER_H -#define XSEULER_H - -#include "xsmath.h" -#include - -#ifdef __cplusplus -#include "xsvector.h" -extern "C" { -#else -#define XSEULER_INITIALIZER { { { XsMath_zero,XsMath_zero,XsMath_zero } } } -struct XsVector; -#endif - -struct XsEuler; -struct XsQuaternion; -struct XsMatrix; - -XSTYPES_DLL_API void XsEuler_destruct(struct XsEuler* thisPtr); -XSTYPES_DLL_API int XsEuler_empty(const struct XsEuler* thisPtr); -XSTYPES_DLL_API void XsEuler_fromQuaternion(struct XsEuler* thisPtr, const struct XsQuaternion* quat); - -#ifdef __cplusplus -} // extern "C" -#endif - -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) {} - //! \brief Constructor that creates an Euler object the specified angles - inline XsEuler(XsReal x_, XsReal y_, XsReal z_) : m_x(x_), m_y(y_), m_z(z_) {} - //! \brief Constructor that creates an Euler object from \a other - inline XsEuler(const XsEuler& other) : m_x(other.m_x), m_y(other.m_y), m_z(other.m_z) {} - //! \brief Constructor that creates an Euler object from \a other - inline explicit XsEuler(const XsVector& other) : m_x(other[0]), m_y(other[1]), m_z(other[2]) {} - - //! \brief \copybrief XsEuler_fromQuaternion - inline explicit XsEuler(const XsQuaternion& q) - { - XsEuler_fromQuaternion(this, &q); - } - - //! \brief Assigns the \a other XsEuler object to this one - inline XsEuler& operator=(const XsEuler& other) - { - //lint --e{1529} trivial assignment - m_x = other.m_x; - m_y = other.m_y; - m_z = other.m_z; - return *this; - } - - //! \brief Returns the \a index'th euler angle in the object - inline XsReal operator[](XsSize index) const - { - 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) - { - assert (index <= 2); - return m_data[index]; - } - - //! \brief Returns true if all angles in this object are zero - inline bool empty() const - { - return m_x == XsMath_zero && m_y == XsMath_zero && m_z == XsMath_zero; - } - - //! \brief Return a const pointer to the internal data - inline const XsReal* data() const - { - return m_data; - } - - //! \brief \copybrief XsEuler_fromQuaternion - inline XsEuler& fromQuaternion(const XsQuaternion& quat) - { - XsEuler_fromQuaternion(this, &quat); - return *this; - } - - /*! \brief Returns true if the values in \a other are exactly equal to this - */ - inline bool operator == (const XsEuler& other) const - { - return m_roll == other.m_roll && m_pitch == other.m_pitch && m_yaw == other.m_yaw; - } - - /*! \brief Returns true if the values in \a other are different from this - */ - inline bool operator != (const XsEuler& other) const - { - return m_roll != other.m_roll || m_pitch != other.m_pitch || m_yaw != other.m_yaw; - } - - //! \brief Returns the roll or x value - inline XsReal roll() const { return m_roll; } - //! \brief Returns the pitch or y value - inline XsReal pitch() const { return m_pitch; } - //! \brief Returns the yaw or z value - inline XsReal yaw() const { return m_yaw; } - - //! \brief Returns the x or roll value - inline XsReal x() const { return m_x; } - //! \brief Returns the y or pitch value - inline XsReal y() const { return m_y; } - //! \brief Returns the z or yaw value - 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 compare(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; - } - -private: -#endif - - 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 { - 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 - }; - XsReal m_data[3]; //!< Stores the euler triplet in an array of three elements - }; -}; - -typedef struct XsEuler XsEuler; - -#endif diff --git a/extern/include/xsens/xsexception.h b/extern/include/xsens/xsexception.h deleted file mode 100644 index 7864da6..0000000 --- a/extern/include/xsens/xsexception.h +++ /dev/null @@ -1,107 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifdef __cplusplus -#ifndef XSEXCEPTION_H -#define XSEXCEPTION_H - -#include -#include "xsresultvalue.h" -#include "xsstring.h" - -/*! \brief Exception class for Xsens public libraries. Inherits from std::exception -*/ -class XsException : public std::exception { -public: - //! \brief Copy constructor - XsException(XsException const& e) - : m_code(e.m_code) - , m_description(e.m_description) - { - } - - /*! \brief Initializing constructor - \details This constructor uses the value in \a err and the supplied \a description to create a full - text for when the user requests what() or text() - \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) - : 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 = rv; - } - } - - /*! \brief Initializing constructor - \param description A description of the error. - */ - explicit XsException(XsString const& description) - : m_code(XRV_ERROR) - , m_description(description) - { - } - - //! \brief Destructor - virtual ~XsException() throw() - { - } - - //! \brief Assignment operator, copies \a e to this - XsException& operator = (XsException const& e) - { - m_code = e.m_code; - m_description = e.m_description; - return *this; - } - - //! \brief Returns the error value supplied during construction - inline XsResultValue code() const throw() - { - return m_code; - } - - //! \brief Returns a description of the error that occurred as a char const* - inline char const* what() const throw() - { - return m_description.c_str(); - } - - //! \brief Returns a description of the error that occurred as a XsString - inline XsString const& text() const throw() - { - return m_description; - } - -private: - XsResultValue m_code; //!< The supplied error code - XsString m_description; //!< The supplied description, prefixed with a description of the error code -}; - -#endif -#endif // __cplusplus guard diff --git a/extern/include/xsens/xsfile.h b/extern/include/xsens/xsfile.h deleted file mode 100644 index 27ecff1..0000000 --- a/extern/include/xsens/xsfile.h +++ /dev/null @@ -1,269 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSFILE_H -#define XSFILE_H - -#include "xstypesconfig.h" -#include "pstdint.h" -#include -#include "xsresultvalue.h" -#include "xsfilepos.h" -#include "xsstring.h" - -#ifdef _MSC_VER -# define XS_MAX_FILENAME_LENGTH 512 -#else -# define XS_MAX_FILENAME_LENGTH PATH_MAX -#endif - -#ifdef __cplusplus -extern "C" { -#else -#define XSFILE_INITIALIZER { 0 } -#endif - -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 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, XsSize fileSize); -XSTYPES_DLL_API XsResultValue XsFile_resize(struct XsFile *thisPtr, XsSize fileSize); -XSTYPES_DLL_API XsResultValue XsFile_erase(const struct XsString* filename); - -XSTYPES_DLL_API XsSize XsFile_read(struct XsFile *thisPtr, void *destination, XsSize size, XsSize count); -XSTYPES_DLL_API XsSize XsFile_write(struct XsFile *thisPtr, const void *source, XsSize size, XsSize 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); -XSTYPES_DLL_API XsFilePos XsFile_tell(struct XsFile *thisPtr); - -XSTYPES_DLL_API int XsFile_eof(struct XsFile *thisPtr); -XSTYPES_DLL_API XsResultValue XsFile_error(struct XsFile *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 FILE* XsFile_handle(struct XsFile *thisPtr); - -#ifdef __cplusplus -} // extern "C" -#endif - -struct XsFile { -#ifdef __cplusplus - /*! \brief Default constructor, creates an empty file object */ - explicit inline XsFile() - : m_handle(NULL) - { - // Silence -Wunused-private-field - // This field is used in the C back-end, but - // some compilers may not notice this - (void)m_handle; - } - - /*! \brief \copybrief XsFile_destruct */ - inline ~XsFile() - { - XsFile_destruct(this); - } - - /*! \brief \copybrief XsFile_create */ - 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) - { - return XsFile_createText(this, &filename, writeOnly ? 1 : 0); - } - - /*! \brief \copybrief XsFile_open */ - 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) - { - return XsFile_openText(this, &fileName, readOnly ? 1 : 0); - } - - /*! \brief \copybrief XsFile_openText */ - inline XsResultValue reopen(const XsString &fileName, const XsString &mode) - { - return XsFile_reopen(this, &fileName, &mode); - } - - /*! \brief \copybrief XsFile_isOpen */ - inline bool isOpen() const - { - return (XsFile_isOpen(this) == 0); - } - - /*! \brief \copybrief XsFile_exists */ - static inline bool exists(const XsString &fileName) - { - return (XsFile_exists(&fileName) == 0); - } - - /*! \brief \copybrief XsFile_close */ - inline XsResultValue close() - { - return XsFile_close(this); - } - - /*! \brief \copybrief XsFile_flush */ - inline XsResultValue flush() - { - return XsFile_flush(this); - } - - /*! \brief \copybrief XsFile_truncate */ - inline XsResultValue truncate(XsSize fileSize) - { - return XsFile_truncate(this, fileSize); - } - - /*! \brief \copybrief XsFile_resize */ - inline XsResultValue resize(XsSize fileSize) - { - return XsFile_resize(this, fileSize); - } - - /*! \brief \copybrief XsFile_erase */ - static XsResultValue erase(const XsString& filename) - { - return XsFile_erase(&filename); - } - - /*! \brief \copybrief XsFile_read */ - inline XsSize read(void *destination, XsSize size, XsSize count) - { - return XsFile_read(this, destination, size, count); - } - - /*! \brief \copybrief XsFile_write */ - inline XsSize write(const void *source, XsSize size, XsSize count) - { - return XsFile_write(this, source, size, count); - } - - /*! \brief \copybrief XsFile_getc */ - inline int getc() - { - return XsFile_getc(this); - } - - /*! \brief \copybrief XsFile_putc */ - inline XsResultValue putc(int character) - { - return XsFile_putc(this, character); - } - - /*! \brief \copybrief XsFile_gets */ - inline char* gets(char *destination, int maxCount) - { - return XsFile_gets(this, destination, maxCount); - } - - /*! \brief \copybrief XsFile_puts */ - inline XsResultValue puts(const char *source) - { - return XsFile_puts(this, source); - } - - /*! \brief \copybrief XsFile_seek */ - inline XsResultValue seek(XsFilePos offset) - { - return XsFile_seek(this, offset); - } - - /*! \brief \copybrief XsFile_seek_r */ - inline XsResultValue seek_r(XsFilePos offset) - { - return XsFile_seek_r(this, offset); - } - - /*! \brief \copybrief XsFile_tell */ - inline XsFilePos tell() - { - return XsFile_tell(this); - } - - /*! \brief \copybrief XsFile_eof */ - inline bool eof() - { - return (0!=XsFile_eof(this)); - } - - /*! \brief \copybrief XsFile_error */ - inline XsResultValue error() - { - return XsFile_error(this); - } - - /*! \brief \copybrief XsFile_fullPath */ - static XsResultValue fullPath(const XsString &filename, XsString &fullPath) - { - return XsFile_fullPath(&filename, &fullPath); - } - - /*! \brief \copybrief XsFile_getline */ - inline XsResultValue getline(XsString& line) - { - return XsFile_getline(this, &line); - } - - /*! \brief \copybrief XsFile_getline */ - inline XsResultValue getline(std::string& line) - { - XsString tmp; - XsResultValue rv = XsFile_getline(this, &tmp); - if (rv == XRV_OK) - line = tmp.toStdString(); - return rv; - } - - /*! \brief \copybrief XsFile_handle */ - inline FILE* handle() - { - return XsFile_handle(this); - } -private: -#endif - FILE* m_handle; -}; - -typedef struct XsFile XsFile; - -#endif diff --git a/extern/include/xsens/xsfilepos.h b/extern/include/xsens/xsfilepos.h deleted file mode 100644 index d8eb8a7..0000000 --- a/extern/include/xsens/xsfilepos.h +++ /dev/null @@ -1,58 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSFILEPOS_H -#define XSFILEPOS_H - -/*! \addtogroup cinterface C Interface - @{ -*/ - -/*! \typedef XsFilePos - \brief The type that is used for positioning inside a file -*/ -/*! \typedef XsIoHandle - \brief The type that is used for low-level identification of an open I/O device -*/ -/*! \typedef XsFileHandle - \brief The type that is used for low-level identification of an open file -*/ - -/*! @} */ - -#include -#ifdef _WIN32 -#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; -#endif -typedef FILE XsFileHandle; - -#endif // file guard diff --git a/extern/include/xsens/xsgpspvtdata.h b/extern/include/xsens/xsgpspvtdata.h deleted file mode 100644 index 40ce1d3..0000000 --- a/extern/include/xsens/xsgpspvtdata.h +++ /dev/null @@ -1,88 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSGPSPVTDATA_H -#define XSGPSPVTDATA_H - -#include "xstypesconfig.h" -#include "pstdint.h" - -#ifdef __cplusplus -extern "C" { -#else -#define XSGPSPVTDATA_INITIALIZER { 0,255,0,0,0,0,0,0,0,0,0,0,255 } -#endif - -struct XsGpsPvtData; - -XSTYPES_DLL_API void XsGpsPvtData_destruct(struct XsGpsPvtData* thisPtr); -XSTYPES_DLL_API int XsGpsPvtData_empty(const struct XsGpsPvtData* thisPtr); - -#ifdef __cplusplus -} // extern "C" -#endif - - -/*! \brief Data from the GPS unit of a legacy MTi-G.*/ -struct XsGpsPvtData { - uint16_t m_pressure; //!< The pressure measurement in units of 2 Pascal, only valid if m_pressureAge is not 255 - uint8_t m_pressureAge; //!< The age of the pressure measurement in packets. When it decreases relative to the previous packet, it indicates that new data is available. - uint32_t m_itow; //!< Integer time of week in ms - int32_t m_latitude; //!< Latitude in 1e-7 degrees - int32_t m_longitude; //!< Longitude in 1e-7 degrees - int32_t m_height; //!< Height in mm - int32_t m_veln; //!< Velocity towards north in cm/s - int32_t m_vele; //!< Velocity towards east in cm/s - int32_t m_veld; //!< Velocity towards down in cm/s - uint32_t m_hacc; //!< Horizontal accuracy estimate, expected error standard deviation in mm - uint32_t m_vacc; //!< Vertical accuracy estimate, expected error standard deviation in mm - uint32_t m_sacc; //!< Speed accuracy estimate, expected error standard deviation in cm/s - uint8_t m_gpsAge; //!< The age of the GPS measurement in packets. When it decreases relative to the previous packet, it indicates that new data is available. - -#ifdef __cplusplus - /*! \brief \copybrief XsGpsPvtData_destruct */ - inline void clear() - { - XsGpsPvtData_destruct(this); - } - - /*! \brief \copybrief XsGpsPvtData_empty */ - inline bool empty() const - { - return 0 != XsGpsPvtData_empty(this); - } - - /*! \brief Returns true if all fields of this and \a other are exactly identical */ - inline bool operator == (const XsGpsPvtData& other) const - { - return m_pressure == other.m_pressure && - m_pressureAge == other.m_pressureAge && - m_itow == other.m_itow && - m_latitude == other.m_latitude && - m_longitude == other.m_longitude && - m_height == other.m_height && - m_veln == other.m_veln && - m_vele == other.m_vele && - m_veld == other.m_veld && - m_hacc == other.m_hacc && - m_vacc == other.m_vacc && - m_sacc == other.m_sacc && - m_gpsAge == other.m_gpsAge; - } -#endif -}; - -typedef struct XsGpsPvtData XsGpsPvtData; - -#endif diff --git a/extern/include/xsens/xsinforequest.h b/extern/include/xsens/xsinforequest.h deleted file mode 100644 index e085fc6..0000000 --- a/extern/include/xsens/xsinforequest.h +++ /dev/null @@ -1,33 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSINFOREQUEST_H -#define XSINFOREQUEST_H - -/*! \addtogroup enums Global enumerations - @{ -*/ -/*! \brief Information request identifiers - \details These values are used by the XsDevice::requestInfo function and - XsCallback::onInfoResponse functions. -*/ -enum XsInfoRequest { - XIR_BatteryLevel = 0, //!< Request battery level - XIR_GnssSvInfo, //!< Request Gnss satellite vehicle information - XIR_GpsSvInfo = XIR_GnssSvInfo //!< Request Gps satellite vehicle information \deprecated Replaced with XIR_GnssSvInfo -}; -/*! @} */ -typedef enum XsInfoRequest XsInfoRequest; - -#endif diff --git a/extern/include/xsens/xsint64array.h b/extern/include/xsens/xsint64array.h deleted file mode 100644 index f0a64c9..0000000 --- a/extern/include/xsens/xsint64array.h +++ /dev/null @@ -1,70 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSINT64ARRAY_H -#define XSINT64ARRAY_H - -#include "xsarray.h" -#include "pstdint.h" - -#ifdef __cplusplus -extern "C" { -#endif - -extern XsArrayDescriptor const XSTYPES_DLL_API g_xsInt64ArrayDescriptor; - -#ifndef __cplusplus -#define XSINT64ARRAY_INITIALIZER XSARRAY_INITIALIZER(&g_xsInt64ArrayDescriptor) -XSARRAY_STRUCT(XsInt64Array, int64_t); -typedef struct XsInt64Array XsInt64Array; - -XSTYPES_DLL_API void XsInt64Array_construct(XsInt64Array* thisPtr, XsSize count, int64_t const* src); - -#else -} // extern "C" -#endif - -#ifdef __cplusplus -struct XsInt64Array : public XsArrayImpl { - //! \brief Constructs an XsInt64Array - inline explicit XsInt64Array(XsSize sz = 0, int64_t const* src = 0) - : ArrayImpl(sz, src) - { - } - - //! \brief Constructs an XsInt64Array as a copy of \a other - inline XsInt64Array(XsInt64Array const& other) - : ArrayImpl(other) - { - } - - //! \brief Constructs an XsInt64Array that references the data supplied in \a ref - inline explicit XsInt64Array(int64_t* ref, XsSize sz, XsDataFlags flags = XSDF_None) - : ArrayImpl(ref, sz, flags) - { - } - -#ifndef XSENS_NOITERATOR - //! \brief Constructs an XsInt64Array with the array bound by the supplied iterators \a beginIt and \a endIt - template - inline XsInt64Array(Iterator beginIt, Iterator endIt) - : ArrayImpl(beginIt, endIt) - { - } -#endif -}; -#endif - - -#endif diff --git a/extern/include/xsens/xsintarray.h b/extern/include/xsens/xsintarray.h deleted file mode 100644 index 84597a4..0000000 --- a/extern/include/xsens/xsintarray.h +++ /dev/null @@ -1,68 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSINTARRAY_H -#define XSINTARRAY_H - -#include "xsarray.h" - -#ifdef __cplusplus -extern "C" { -#endif - -extern XsArrayDescriptor const XSTYPES_DLL_API g_xsIntArrayDescriptor; - -#ifndef __cplusplus -#define XsIntArray_INITIALIZER XSARRAY_INITIALIZER(&g_xsIntArrayDescriptor) - -XSARRAY_STRUCT(XsIntArray, int); -typedef struct XsIntArray XsIntArray; - -XSTYPES_DLL_API void XsIntArray_construct(XsIntArray* thisPtr, XsSize count, int const* src); -#else -} // extern "C" -#endif - -#ifdef __cplusplus -struct XsIntArray : public XsArrayImpl { - //! \brief Constructs an XsIntArray - inline explicit XsIntArray(XsSize sz = 0, int const* src = 0) - : ArrayImpl(sz, src) - { - } - - //! \brief Constructs an XsIntArray as a copy of \a other - inline XsIntArray(XsIntArray const& other) - : ArrayImpl(other) - { - } - - //! \brief Constructs an XsIntArray that references the data supplied in \a ref - inline explicit XsIntArray(int* ref, XsSize sz, XsDataFlags flags = XSDF_None) - : ArrayImpl(ref, sz, flags) - { - } - -#ifndef XSENS_NOITERATOR - //! \brief Constructs an XsIntArray with the array bound by the supplied iterators \a beginIt and \a endIt - template - inline XsIntArray(Iterator beginIt, Iterator endIt) - : ArrayImpl(beginIt, endIt) - { - } -#endif -}; -#endif - -#endif diff --git a/extern/include/xsens/xsintlist.h b/extern/include/xsens/xsintlist.h deleted file mode 100644 index d688cb8..0000000 --- a/extern/include/xsens/xsintlist.h +++ /dev/null @@ -1,37 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSINTLIST_H -#define XSINTLIST_H - -#include "xsintarray.h" - -#define XsIntList XsIntArray - -#ifndef __cplusplus -// obsolete: -#define XSINTLIST_INITIALIZER XsIntArray_INITIALIZER -#define XsIntList_construct(thisPtr, sz, src) XsIntArray_construct(thisPtr, sz, src) -#define XsIntList_assign(thisPtr, sz, src) XsArray_assign(thisPtr, sz, src) -#define XsIntList_destruct(thisPtr) XsArray_destruct(thisPtr) -#define XsIntList_copy(thisPtr, copy) XsArray_copy(copy, thisPtr) -#define XsIntList_append(thisPtr, other) XsArray_append(thisPtr, other) -#define XsIntList_popFront(thisPtr, count) XsArray_erase(thisPtr, 0, count) -#define XsIntList_popBack(thisPtr, count) XsArray_erase(thisPtr, (XsSize)-1, count) -#define XsIntList_swap(a, b) XsArray_swap(a, b) -#define XsIntList_erase(thisPtr, index, count) XsArray_erase(thisPtr, index, count) -#define XsIntList_find(thisPtr, needle) XsArray_find(thisPtr, needle) - -#endif -#endif diff --git a/extern/include/xsens/xslibraryloader.h b/extern/include/xsens/xslibraryloader.h deleted file mode 100644 index 298a1d0..0000000 --- a/extern/include/xsens/xslibraryloader.h +++ /dev/null @@ -1,113 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSLIBRARYLOADER_H -#define XSLIBRARYLOADER_H - -#include "xstypesconfig.h" -#include "xsstring.h" - -struct XsLibraryLoader; - -#ifdef __cplusplus -extern "C" { -#else -#define XSLIBRARYLOADER_INITIALIZER { NULL } -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 int XsLibraryLoader_unload(XsLibraryLoader* thisp); -XSTYPES_DLL_API int XsLibraryLoader_isLoaded(const XsLibraryLoader* thisp); -XSTYPES_DLL_API void XsLibraryLoader_getErrorString(XsString* error); - -#ifdef __cplusplus -} -#endif - -/*! \brief The Xsens dynamic library loader base class -*/ -struct XsLibraryLoader { -#ifdef __cplusplus -public: - /*! \brief Create a library loader */ - inline XsLibraryLoader() : - m_handle(NULL) - { - // avoid compiler warnings about - // an unused handle. It is used in the c implementations - (void)m_handle; - } - - /*! \brief Destroy a library loader */ - inline ~XsLibraryLoader() - { - unload(); - } - - /*! \brief Load the library - \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) - { - return XsLibraryLoader_load(this, &libraryName) != 0; - } - - /*! \brief Return true if a library has been loaded - - \return true if a library has been loaded, false otherwise - */ - inline bool isLoaded() const - { - return XsLibraryLoader_isLoaded(this) != 0; - } - - /*! \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 - */ - inline void* resolve(const char *functionName) const - { - return XsLibraryLoader_resolve(this, functionName); - } - - /*! \brief Unload the loaded library - */ - inline void unload() throw() - { - XsLibraryLoader_unload(this); - } - - /*! \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. - - \returns a string describing the error that occurred - */ - inline static XsString errorString() - { - XsString rv; - XsLibraryLoader_getErrorString(&rv); - return rv; - } -private: -#endif - void* m_handle; -}; - -#endif //XSLIBRARYLOADER_H diff --git a/extern/include/xsens/xsmalloc.h b/extern/include/xsens/xsmalloc.h deleted file mode 100644 index ecdacef..0000000 --- a/extern/include/xsens/xsmalloc.h +++ /dev/null @@ -1,49 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSMALLOC_H -#define XSMALLOC_H - -#include "xstypesconfig.h" -#include - -#ifdef __cplusplus -extern "C" { -#endif - -XSTYPES_DLL_API void* xsMalloc(size_t sz); -XSTYPES_DLL_API void* xsRealloc(void* ptr, size_t sz); -XSTYPES_DLL_API void xsFree(void* ptr); - -XSTYPES_DLL_API void* xsAlignedMalloc(size_t sz); -XSTYPES_DLL_API void* xsAlignedRealloc(void* ptr, size_t sz); -XSTYPES_DLL_API void xsAlignedFree(void* ptr); - -#ifndef xsMathMalloc -#define xsMathMalloc(n) xsAlignedMalloc(n) -#endif - -#ifndef xsMathRealloc -#define xsMathRealloc(p, n) xsAlignedRealloc(p,n) -#endif - -#ifndef xsMathFree -#define xsMathFree(p) xsAlignedFree(p) -#endif - -#ifdef __cplusplus -} // extern "C" -#endif - -#endif diff --git a/extern/include/xsens/xsmath.h b/extern/include/xsens/xsmath.h deleted file mode 100644 index fdc0bad..0000000 --- a/extern/include/xsens/xsmath.h +++ /dev/null @@ -1,170 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSMATH_H -#define XSMATH_H - -#include "xstypesconfig.h" -#include "xstypedefs.h" -#include "pstdint.h" - -#ifdef __cplusplus -extern "C" { -#endif - -XSTYPES_DLL_API extern const XsReal XsMath_e; -XSTYPES_DLL_API extern const XsReal XsMath_pi; -XSTYPES_DLL_API extern const XsReal XsMath_tinyValue; -XSTYPES_DLL_API extern const XsReal XsMath_hugeValue; -XSTYPES_DLL_API extern const XsReal XsMath_epsilon; -XSTYPES_DLL_API extern const XsReal XsMath_sqrtEpsilon; -XSTYPES_DLL_API extern const XsReal XsMath_denormalized; -XSTYPES_DLL_API extern const XsReal XsMath_sqrtDenormalized; -XSTYPES_DLL_API extern const XsReal XsMath_rad2degValue; -XSTYPES_DLL_API extern const XsReal XsMath_deg2radValue; -XSTYPES_DLL_API extern const XsReal XsMath_zero; -XSTYPES_DLL_API extern const XsReal XsMath_pt25; -XSTYPES_DLL_API extern const XsReal XsMath_pt5; -XSTYPES_DLL_API extern const XsReal XsMath_minusPt5; -XSTYPES_DLL_API extern const XsReal XsMath_one; -XSTYPES_DLL_API extern const XsReal XsMath_minusOne; -XSTYPES_DLL_API extern const XsReal XsMath_two; -XSTYPES_DLL_API extern const XsReal XsMath_four; -XSTYPES_DLL_API extern const XsReal XsMath_minusTwo; -XSTYPES_DLL_API extern const XsReal XsMath_minusHalfPi; -XSTYPES_DLL_API extern const XsReal XsMath_halfPi; -XSTYPES_DLL_API extern const XsReal XsMath_twoPi; -XSTYPES_DLL_API extern const XsReal XsMath_sqrt2; -XSTYPES_DLL_API extern const XsReal XsMath_sqrtHalf; -XSTYPES_DLL_API extern const XsReal XsMath_infinity; - -XSTYPES_DLL_API XsReal XsMath_rad2deg(XsReal radians); -XSTYPES_DLL_API XsReal XsMath_deg2rad(XsReal degrees); -XSTYPES_DLL_API XsReal XsMath_pow2(XsReal a); -XSTYPES_DLL_API XsReal XsMath_pow3(XsReal a); -XSTYPES_DLL_API XsReal XsMath_asinClamped(XsReal x); -XSTYPES_DLL_API int XsMath_isFinite(XsReal x); -XSTYPES_DLL_API int32_t XsMath_doubleToLong(double d); -#ifndef XSENS_NO_INT64 -XSTYPES_DLL_API int64_t XsMath_doubleToInt64(double d); -#endif - -#ifdef __cplusplus -} // extern "C" - -namespace XsMath { -#ifdef __GNUC__ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wunused-variable" -#endif - //! \brief The value e - static const XsReal& e = XsMath_e; - //! \brief The value pi - static const XsReal& pi = XsMath_pi; - //! \brief A really small value - static const XsReal& tinyValue = XsMath_tinyValue; - //! \brief A convincingly large number - static const XsReal& hugeValue = XsMath_hugeValue; - //! \brief A value related to the precision of floating point arithmetic (2.2204460492503131e-016) - static const XsReal& epsilon = XsMath_epsilon; - /*! \brief Square root of epsilon - \sa epsilon - */ - static const XsReal& sqrtEpsilon = XsMath_sqrtEpsilon; - //! \brief Value that represents the subnormal number in floating point wizardry - static const XsReal& denormalized = XsMath_denormalized; - /*! \brief Square root of denormalized - \sa denormalized - */ - static const XsReal& sqrtDenormalized = XsMath_sqrtDenormalized; - //! \brief Value to convert radians to degrees by multiplication - static const XsReal& rad2degValue = XsMath_rad2degValue; - //! \brief Value to convert degrees to radians by multiplication - static const XsReal& deg2radValue = XsMath_deg2radValue; - //! \brief 0 - static const XsReal& zero = XsMath_zero; - //! \brief 0.25 - static const XsReal& pt25 = XsMath_pt25; - //! \brief 0.5 - static const XsReal& pt5 = XsMath_pt5; - //! \brief -0.5 - static const XsReal& minusPt5 = XsMath_minusPt5; - //! \brief 1 - static const XsReal& one = XsMath_one; - //! \brief -1 - static const XsReal& minusOne = XsMath_minusOne; - //! \brief 2 - static const XsReal& two = XsMath_two; - //! \brief 4 - static const XsReal& four = XsMath_four; - //! \brief -2 - static const XsReal& minusTwo = XsMath_minusTwo; - //! \brief -pi/2 - static const XsReal& minusHalfPi = XsMath_minusHalfPi; - //! \brief pi/2 - static const XsReal& halfPi = XsMath_halfPi; - //! \brief 2*pi - static const XsReal& twoPi = XsMath_twoPi; - //! \brief sqrt(2) - static const XsReal& sqrt2 = XsMath_sqrt2; - //! \brief sqrt(0.5) - static const XsReal& sqrtHalf = XsMath_sqrtHalf; - //! \brief infinity - static const XsReal& infinity = XsMath_infinity; -#ifdef __GNUC__ -#pragma GCC diagnostic pop -#endif - - - //! \brief \copybrief XsMath_asinClamped - inline XsReal asinClamped(XsReal x) - { - return XsMath_asinClamped(x); - } - //! \brief \copybrief XsMath_rad2deg - inline XsReal rad2deg(XsReal radians) - { - return XsMath_rad2deg(radians); - } - //! \brief \copybrief XsMath_deg2rad - inline XsReal deg2rad(XsReal degrees) - { - return XsMath_deg2rad(degrees); - } - //! \brief \copybrief XsMath_pow2 - inline XsReal pow2(XsReal a) - { - return XsMath_pow2(a); - } - //! \brief \copybrief XsMath_pow3 - inline XsReal pow3(XsReal a) - { - return XsMath_pow3(a); - } - //! \brief \copybrief XsMath_doubleToLong - inline int32_t doubleToLong(double d) - { - return XsMath_doubleToLong(d); - } -#ifndef XSENS_NO_INT64 - //! \brief \copybrief XsMath_doubleToInt64 - inline int64_t doubleToInt64(double d) - { - return XsMath_doubleToInt64(d); - } -#endif -} // namespace -#endif - -#endif diff --git a/extern/include/xsens/xsmatrix.h b/extern/include/xsens/xsmatrix.h deleted file mode 100644 index 2aa3c9b..0000000 --- a/extern/include/xsens/xsmatrix.h +++ /dev/null @@ -1,291 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSMATRIX_H -#define XSMATRIX_H - -#include "xsmath.h" -#include -#include - -struct XsMatrix; -struct XsEuler; -struct XsQuaternion; - -#ifdef __cplusplus -extern "C" { -#else -#define XSMATRIX_INITIALIZER { NULL, 0, 0, 0, XSDF_Managed } -typedef struct XsMatrix XsMatrix; -#endif - -XSTYPES_DLL_API void XsMatrix_ref(XsMatrix* thisPtr, XsSize rows, XsSize cols, XsSize stride, XsReal* buffer, XsDataFlags flags); -XSTYPES_DLL_API void XsMatrix_construct(XsMatrix* thisPtr, XsSize rows, XsSize cols, XsSize stride, const XsReal* src, XsSize srcStride); -XSTYPES_DLL_API void XsMatrix_assign(XsMatrix* thisPtr, XsSize rows, XsSize cols, XsSize stride, const XsReal* src, XsSize srcStride); -XSTYPES_DLL_API void XsMatrix_destruct(XsMatrix* thisPtr); -XSTYPES_DLL_API void XsMatrix_copy(XsMatrix* copy, XsMatrix const* src); -XSTYPES_DLL_API void XsMatrix_setZero(XsMatrix* thisPtr); -XSTYPES_DLL_API int XsMatrix_empty(const XsMatrix* thisPtr); -XSTYPES_DLL_API void XsMatrix_multiplyScalar(const XsMatrix* thisPtr, XsReal scalar, XsMatrix* dest); -XSTYPES_DLL_API XsSize XsMatrix_offset(const XsMatrix* thisPtr, XsSize row, XsSize column); -XSTYPES_DLL_API XsReal XsMatrix_value(const XsMatrix* thisPtr, XsSize row, XsSize column); -XSTYPES_DLL_API void XsMatrix_setValue(XsMatrix* thisPtr, XsSize row, XsSize column, XsReal value); -XSTYPES_DLL_API int XsMatrix_dimensionsMatch(const XsMatrix* thisPtr, XsSize rows, XsSize columns); -XSTYPES_DLL_API void XsMatrix_fromQuaternion(XsMatrix* thisPtr, const struct XsQuaternion* quat); -XSTYPES_DLL_API void XsMatrix_swap(XsMatrix* a, XsMatrix* b); - -#define XsMatrix_offsetM(thisPtr, row, column) (thisPtr->m_stride*row + column) - -#ifdef __cplusplus -} // extern "C" -#endif -#ifndef XSENS_NO_PACK -#pragma pack(push, 1) -#endif -struct XsMatrix { - //lint --e{613} -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 - const XsSize m_stride; //!< Number of items per row in memory (usually equal to cols but not always) - const int m_flags; //!< Flags for data management - -#ifdef __cplusplus - //! \brief Return the data management flags of the matrix. - inline int flags() { 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) - : m_data(0) - , m_rows(0) - , m_cols(0) - , m_stride(0) - , m_flags(0) - { - if (rows && cols) - XsMatrix_construct(this, rows, cols, strde?strde:cols, dat, 0); - } - - /*! \brief Initialize an XsMatrix object from the \a other XsMatrix */ - inline XsMatrix(const XsMatrix& other) - : m_data(0) - , m_rows(0) - , m_cols(0) - , m_stride(0) - , m_flags(0) - { - XsMatrix_copy(this, &other); - } - - /*! \brief Initialize an XsMatrix object that references the data passed in \a ref. \a rows, \a cols and \a stride can be used to specify the layout of the data */ - inline explicit XsMatrix(XsReal* ref, XsSize rows, XsSize cols, XsSize stride, XsDataFlags flags = XSDF_None) - : m_data(ref) - , m_rows(rows) - , m_cols(cols) - , m_stride(stride) - , m_flags(flags) - { - } - - /*! \brief Initialize a copy of \a other in an XsMatrix object that references the data passed in \a ref. \a rows, \a cols and \a stride can be used to specify the layout of the data */ - inline explicit XsMatrix(const XsMatrix& other, XsReal* ref, XsSize rows, XsSize cols, XsSize stride, XsDataFlags flags = XSDF_None) - : m_data(ref) - , m_rows(rows) - , m_cols(cols) - , m_stride(stride) - , m_flags(flags) - { - XsMatrix_copy(this, &other); - } - - //! \brief \copybrief XsMatrix_fromQuaternion - inline explicit XsMatrix(const XsQuaternion& quat) - : m_data(0) - , m_rows(0) - , m_cols(0) - , m_stride(0) - , m_flags(0) - { - XsMatrix_fromQuaternion(this, &quat); - } - - //! \copydoc XsMatrix_destruct - inline ~XsMatrix() - { - XsMatrix_destruct(this); - } - - /*! \brief Resize the matrix to the specified number of \a rows and \a cols, destroying its current contents */ - inline void setSize(XsSize rows, XsSize cols, XsSize stride = 0) - { - XsMatrix_assign(this, rows, cols, stride, 0, 0); - } - - /*! \brief \copybrief XsMatrix_copy */ - inline XsMatrix& operator=(const XsMatrix& other) - { - //lint --e{1529} self-assignment checked by copy function - XsMatrix_copy(this, &other); - return *this; - } - - //! \brief \copybrief XsMatrix_empty - inline bool empty() const - { - return 0 != XsMatrix_empty(this); - } - - //! \brief \copybrief XsMatrix_setZero - inline void setZero() - { - XsMatrix_setZero(this); - } - - //! \copydoc XsMatrix_offset */ - inline XsSize offset(XsSize row, XsSize column) const - { - return XsMatrix_offset(this, row, column); - } - - /*! \brief Returns the value at \a row and \a column in the matrix */ - inline XsReal value(XsSize row, XsSize column) const - { - return m_data[XsMatrix_offset(this, row, column)]; - } - - /*! \brief Sets the \a value at \a row and \a column in the matrix */ - inline void setValue(XsSize row, XsSize column, XsReal value) - { - m_data[XsMatrix_offsetM(this, row, column)] = value; - } - - /*! \brief Returns a pointer to the data in \a row */ - inline const XsReal* operator[](XsSize row) const - { - return &m_data[XsMatrix_offsetM(this, row, 0)]; - } - - /*! \brief Returns a reference to the data in \a row */ - inline XsReal* operator[](XsSize row) - { - return &m_data[XsMatrix_offsetM(this, row, 0)]; - } - - /*! \brief \copybrief XsMatrix_multiplyScalar */ - inline XsMatrix operator*(XsReal scalar) const - { - XsMatrix tmp(m_rows, m_cols); - XsMatrix_multiplyScalar(this, scalar, &tmp); - return tmp; - } - - /*! \brief \copybrief XsMatrix_fromQuaternion */ - inline XsMatrix& fromQuaternion(const XsQuaternion& quat) - { - XsMatrix_fromQuaternion(this, &quat); - return *this; - } - - /*! \brief Fill the matrix with zeroes */ - inline void zero() - { - for (XsSize r = 0; r < m_rows; ++r) - for (XsSize c = 0; c < m_cols; ++c) - m_data[XsMatrix_offsetM(this, r, c)] = XsMath_zero; - } - - /*! \brief Return the number of rows in the matrix */ - inline XsSize rows() const - { - return m_rows; - } - - /*! \brief Return the number of columns in the matrix */ - inline XsSize cols() const - { - return m_cols; - } - - /*! \brief Return the stride of the matrix. - \details The stride of a matrix is for internal administration. It defines the number of items - in a row in the data buffer. This is always greater than or equal to the number of columns. - Especially for matrices that reference a part of another matrix this may differ from the - cols() value. - \returns The stride of the matrix. - */ - inline XsSize stride() const - { - return m_stride; - } - - //! \brief Return a const pointer to the internal data - inline const XsReal* data() const - { - return m_data; - } - - //! \brief Returns true if \a other is numerically identical to this - inline bool operator ==(const XsMatrix& other) const - { - if (this == &other) - return true; - if (m_rows != other.m_rows || m_cols != other.m_cols) - return false; - for (XsSize r = 0; r < m_rows; ++r) - for (XsSize c = 0; c < m_cols; ++c) - if (m_data[XsMatrix_offsetM(this, r, c)] != other.m_data[XsMatrix_offsetM((&other), r, c)]) - return false; - return true; - } - - //! \brief Returns true if the values of this and \a other are within \a tolerance of each other - inline bool compare(const XsMatrix& other, XsReal tolerance) const - { - if (this == &other) - return true; - if (m_rows != other.m_rows || m_cols != other.m_cols) - return false; - for (XsSize r = 0; r < m_rows; ++r) - for (XsSize c = 0; c < m_cols; ++c) - if (fabs(m_data[XsMatrix_offsetM(this, r, c)] - other.m_data[XsMatrix_offsetM((&other), r, c)]) > tolerance) - return false; - return true; - } - - /*! \brief Swap the contents of \a b with this - \details This function swaps the internal buffers so no actual data is moved around. For unmanaged - data an elementwise swap is done, but only if the matrices are the same size. - \param b Object whose contents will be swapped with this - */ - inline void swap(XsMatrix& b) - { - XsMatrix_swap(this, &b); - } - -#endif -}; -#ifndef XSENS_NO_PACK -#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) -{ - return (m*scalar); -} -#endif - -#endif diff --git a/extern/include/xsens/xsmatrix3x3.h b/extern/include/xsens/xsmatrix3x3.h deleted file mode 100644 index 36d51c9..0000000 --- a/extern/include/xsens/xsmatrix3x3.h +++ /dev/null @@ -1,86 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSMATRIX3X3_H -#define XSMATRIX3X3_H - -#include "xsmatrix.h" - -struct XsMatrix3x3; -#ifdef __cplusplus -extern "C" { -#else -typedef struct XsMatrix3x3 XsMatrix3x3; -#endif - - -XSTYPES_DLL_API void XsMatrix3x3_construct(XsMatrix3x3* thisPtr); -XSTYPES_DLL_API void XsMatrix3x3_assign(XsMatrix3x3* thisPtr, const XsReal* src, XsSize srcStride); -XSTYPES_DLL_API void XsMatrix3x3_destruct(XsMatrix3x3* thisPtr); -XSTYPES_DLL_API void XsMatrix3x3_copy(XsMatrix* copy, XsMatrix3x3 const* src); - -#ifdef __cplusplus -} // extern "C" -#endif - -#ifdef __cplusplus -/* 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 -#else -struct XsMatrix3x3 { - struct XsMatrix m_matrix; //!< The underlying XsMatrix -#endif - 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) - { - } - - //! \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 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 -}; - -#endif diff --git a/extern/include/xsens/xsmessage.h b/extern/include/xsens/xsmessage.h deleted file mode 100644 index 59b62b2..0000000 --- a/extern/include/xsens/xsmessage.h +++ /dev/null @@ -1,666 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSMESSAGE_H -#define XSMESSAGE_H - -#include "xstypesconfig.h" -#include "pstdint.h" -#include "xsbytearray.h" -#include "xsdataidentifier.h" -#include "xsxbusmessageid.h" -#include "xsstring.h" - -struct XsMessage; -struct XsMessageHeader; - -#ifdef __cplusplus -extern "C" { -#else -#define XSMESSAGE_INITIALIZER { XSBYTEARRAY_INITIALIZER, 1, 0 } -typedef struct XsMessage XsMessage; -typedef struct XsMessageHeader XsMessageHeader; -#endif - -XSTYPES_DLL_API void XsMessage_construct(XsMessage* thisPtr); -XSTYPES_DLL_API void XsMessage_constructSized(XsMessage* thisPtr, XsSize dataSize); -XSTYPES_DLL_API void XsMessage_copyConstruct(XsMessage* thisPtr, XsMessage const* src); -XSTYPES_DLL_API void XsMessage_assign(XsMessage* thisPtr, XsSize dataSize); -XSTYPES_DLL_API void XsMessage_load(XsMessage* thisPtr, XsSize msgSize, unsigned char const* src); -XSTYPES_DLL_API void XsMessage_destruct(XsMessage* thisPtr); -XSTYPES_DLL_API void XsMessage_copy(XsMessage* copy, XsMessage const* src); -XSTYPES_DLL_API void XsMessage_swap(XsMessage* a, XsMessage* b); -XSTYPES_DLL_API XsSize XsMessage_dataSize(XsMessage const* thisPtr); -XSTYPES_DLL_API const uint8_t* XsMessage_constData(XsMessage const* thisPtr, XsSize offset); -XSTYPES_DLL_API const uint8_t* XsMessage_getMessageStart(XsMessage const* thisPtr); -XSTYPES_DLL_API XsSize XsMessage_getTotalMessageSize(XsMessage const* thisPtr); -XSTYPES_DLL_API uint8_t XsMessage_getDataByte(XsMessage const* thisPtr, XsSize offset); -XSTYPES_DLL_API uint16_t XsMessage_getDataShort(XsMessage const* thisPtr, XsSize offset); -XSTYPES_DLL_API uint32_t XsMessage_getDataLong(XsMessage const* thisPtr, XsSize offset); -XSTYPES_DLL_API uint64_t XsMessage_getDataLongLong(XsMessage const* thisPtr, XsSize offset); -XSTYPES_DLL_API float XsMessage_getDataFloat(XsMessage const* thisPtr, XsSize offset); -XSTYPES_DLL_API double XsMessage_getDataDouble(XsMessage const* thisPtr, XsSize offset); -XSTYPES_DLL_API double XsMessage_getDataF1220(XsMessage const* thisPtr, XsSize offset); -XSTYPES_DLL_API double XsMessage_getDataFP1632(XsMessage const* thisPtr, XsSize offset); -XSTYPES_DLL_API const uint8_t* XsMessage_getDataBuffer(XsMessage const* thisPtr, XsSize offset); -XSTYPES_DLL_API void XsMessage_setDataByte(XsMessage* thisPtr, uint8_t value, XsSize offset); -XSTYPES_DLL_API void XsMessage_setDataShort(XsMessage* thisPtr, uint16_t value, XsSize offset); -XSTYPES_DLL_API void XsMessage_setDataLong(XsMessage* thisPtr, uint32_t value, XsSize offset); -XSTYPES_DLL_API void XsMessage_setDataLongLong(XsMessage* thisPtr, uint64_t value, XsSize offset); -XSTYPES_DLL_API void XsMessage_setDataFloat(XsMessage* thisPtr, float value, XsSize offset); -XSTYPES_DLL_API void XsMessage_setDataDouble(XsMessage* thisPtr, double value, XsSize offset); -XSTYPES_DLL_API void XsMessage_setDataF1220(XsMessage* thisPtr, double value, XsSize offset); -XSTYPES_DLL_API void XsMessage_setDataFP1632(XsMessage* thisPtr, double value, XsSize offset); -XSTYPES_DLL_API void XsMessage_setDataBuffer(XsMessage* thisPtr, const uint8_t* buffer, XsSize size, XsSize offset); -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 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); -XSTYPES_DLL_API void XsMessage_setBusId(XsMessage* thisPtr, uint8_t busId); -XSTYPES_DLL_API void XsMessage_setMessageId(XsMessage* thisPtr, XsXbusMessageId msgId); -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_getDataFPValues(XsMessage const* thisPtr, uint64_t outputSettings, double *dest, XsSize offset, XsSize numValues); -XSTYPES_DLL_API void XsMessage_setDataFPValues(XsMessage* thisPtr, uint64_t outputSettings, double 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); - -#ifdef __cplusplus -} // extern "C" -#endif - -#define XS_PREAMBLE 0xFA -#define XS_EXTLENCODE 0xFF - -#define XS_LEN_MSGHEADER 4 -#define XS_LEN_MSGEXTHEADER 6 -#define XS_LEN_MSGHEADERCS 5 -#define XS_LEN_MSGEXTHEADERCS 7 -#define XS_LEN_CHECKSUM 1 -#define XS_LEN_UNSIGSHORT 2 -#define XS_LEN_UNSIGINT 4 -#define XS_LEN_FLOAT 4 - -// Maximum message/data length -#define XS_MAXDATALEN (8192-XS_LEN_MSGEXTHEADERCS) -#define XS_MAXSHORTDATALEN 254 -#define XS_MAXMSGLEN (XS_MAXDATALEN+XS_LEN_MSGEXTHEADERCS) -#define XS_MAXSHORTMSGLEN (XS_MAXSHORTDATALEN+XS_LEN_MSGHEADERCS) -#define XS_MAXGARBAGE (XS_MAXMSGLEN+1) - -////////////////////////////////////////////////////////////////////////////////////////// -// 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 -/*! \brief A message header - \details This structure is used to interpret the header of a message. -*/ -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 - //lint --e{18} - //! \brief Contains optional extended length of message and first byte of data buffer - union LengthData { - //! \brief Contains extended length information and first byte of data buffer if normal length is 255 - struct ExtendedLength { - //! \brief The high and low byte of the extended length - 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 - uint8_t m_data[1]; //!< \brief The first byte of the data buffer, the data buffer is always at least 1 byte since it has to contain the checksum, but it can be bigger. - } m_extended; //!< \brief The extended length, only valid if normal length is 255 - uint8_t m_data[1]; //!< \brief The first byte of the data buffer if length < 255, the data buffer is always at least 1 byte since it has to contain the checksum, but it can be bigger. - } m_datlen; //!< \brief Data or length and data -#ifdef SWIG -}; -#else -} PACK_POST; //lint !e659 -#endif - -#ifdef _MSC_VER - #pragma pack(pop) -#endif - -////////////////////////////////////////////////////////////////////////////////////////// -//! \brief Structure for storing a single message. -struct XsMessage { -#ifdef __cplusplus - /*! \brief Create a XsMessage object with the given data length and message Id. - - The function allocates enough memory to hold an entire message with the given - data length. - \param msgId The message Id that will be assigned to the m_messageId field. - \param dataLength The length of the data in the message. - */ - explicit XsMessage(XsXbusMessageId msgId = XMID_InvalidMessage, XsSize dataLength = 0) - : m_autoUpdateChecksum(1) - , m_checksum(0) - { - XsMessage_constructSized(this, dataLength); - XsMessage_setMessageId(this, msgId); - } - - /*! \brief Create a message from the given source string - - This is done through a simple memory copy. The number of bytes copied is taken - from the data in the message (so the message is interpreted first). - Note that this does NOT recompute the checksum, nor is it checked. - - \param source The source string containing message data - \param size The size of the source string - */ - XsMessage(const uint8_t* source, XsSize size) - : m_autoUpdateChecksum(1) - , m_checksum(0) - { - XsMessage_load(this, size, source); - } - - //! \brief Copy constructor - XsMessage(const XsMessage& src) - : m_message(src.m_message) - , m_autoUpdateChecksum(src.m_autoUpdateChecksum) - , m_checksum(0) - { - updateChecksumPtr(); - } - - //! Destroy the message - ~XsMessage() - { - XsMessage_destruct(this); - } - - //! \brief Clear all data in the message - void clear(void) - { - XsMessage_destruct(this); - } - - /*! \brief Test if this message is empty - - \returns true if this message is empty, false otherwise - */ - bool empty(void) const - { - return 0 != XsMessage_empty(this); - } - - //! Return the busId header field. - uint8_t getBusId(void) const - { - const XsMessageHeader* hdr = XsMessage_getConstHeader(this); - if (!hdr) - return 0; - return hdr->m_busId; - } - - /*! \copydoc XsMessage_constData - */ - const uint8_t* getDataBuffer(XsSize offset = 0) const - { - return XsMessage_constData(this, offset); - } - - /*! \copydoc XsMessage_getDataByte - */ - uint8_t getDataByte(XsSize offset = 0) const - { - return XsMessage_getDataByte(this, offset); - } - - /*! \copydoc XsMessage_getDataDouble - */ - double getDataDouble(XsSize offset = 0) const - { - return XsMessage_getDataDouble(this, offset); - } - - /*! \copydoc XsMessage_getDataFloat - */ - float getDataFloat(XsSize offset = 0) const - { - return XsMessage_getDataFloat(this, offset); - } - - /*! \copydoc XsMessage_getDataF1220 - */ - double getDataF1220(XsSize offset = 0) const - { - return XsMessage_getDataF1220(this, offset); - } - - /*! \copydoc XsMessage_getDataFP1632 - */ - double getDataFP1632(XsSize offset = 0) const - { - return XsMessage_getDataFP1632(this, offset); - } - - /*! \copydoc XsMessage_getDataLong - */ - uint32_t getDataLong(XsSize offset = 0) const - { - return XsMessage_getDataLong(this, offset); - } - /*! \copydoc XsMessage_getDataLongLong - */ - uint64_t getDataLongLong(XsSize offset = 0) const - { - return XsMessage_getDataLongLong(this, offset); - } - - /*! \copydoc XsMessage_getDataShort - */ - uint16_t getDataShort(XsSize offset = 0) const - { - return XsMessage_getDataShort(this, offset); - } - - /*! \copydoc XsMessage_dataSize - */ - XsSize getDataSize(void) const - { - return XsMessage_dataSize(this); - } - - //! Return the current value of the m_messageId field. - XsXbusMessageId getMessageId(void) const - { - const XsMessageHeader* hdr = XsMessage_getConstHeader(this); - if (!hdr) - return XMID_InvalidMessage; - return (XsXbusMessageId) hdr->m_messageId; - } - - /*! \copydoc XsMessage_getMessageStart - */ - const uint8_t* getMessageStart(void) const - { - return XsMessage_getMessageStart(this); - } - - /*! \copydoc XsMessage_getTotalMessageSize - */ - XsSize getTotalMessageSize(void) const - { - return XsMessage_getTotalMessageSize(this); - } - - /*! \copydoc XsMessage_isChecksumOk - */ - bool isChecksumOk(void) const - { - return 0 != XsMessage_isChecksumOk(this); - } - - /*! \brief Initialize the %XsMessage with the data from \a src - - \param msgSize the size of the data pointed to by src - \param src the data to load the message from - - \returns true if the checksum of the loaded message is OK. - */ - bool loadFromString(const uint8_t* src, XsSize msgSize) - { - XsMessage_load(this, msgSize, src); - return isChecksumOk(); - } - - /*! \copydoc XsMessage_recomputeChecksum - */ - void recomputeChecksum(void) - { - XsMessage_recomputeChecksum(this); - } - - /*! \copydoc XsMessage_resizeData - */ - void resizeData(XsSize newSize) - { - XsMessage_resizeData(this, newSize); - } - - /*! \copydoc XsMessage_setBusId - */ - void setBusId(uint8_t busId) - { - XsMessage_setBusId(this, busId); - } - - /*! \copydoc XsMessage_setDataBuffer - */ - void setDataBuffer(const uint8_t* buffer, XsSize size, XsSize offset = 0) - { - XsMessage_setDataBuffer(this, buffer, size, offset); - } - - /*! \copydoc XsMessage_setDataByte - */ - void setDataByte(const uint8_t value, XsSize offset = 0) - { - XsMessage_setDataByte(this, value, offset); - } - - /*! \copydoc XsMessage_setDataDouble - */ - void setDataDouble(const double value, XsSize offset=0) - { - XsMessage_setDataDouble(this, value, offset); - } - - /*! \copydoc XsMessage_setDataFloat - */ - void setDataFloat(const float value, XsSize offset = 0) - { - XsMessage_setDataFloat(this, value, offset); - } - - /*! \copydoc XsMessage_setDataF1220 - */ - void setDataF1220(const double value, XsSize offset = 0) - { - XsMessage_setDataF1220(this, value, offset); - } - - /*! \copydoc XsMessage_setDataFP1632 - */ - void setDataFP1632(const double value, XsSize offset = 0) - { - XsMessage_setDataFP1632(this, value, offset); - } - - /*! \copydoc XsMessage_setDataLong - */ - void setDataLong(const uint32_t value, XsSize offset = 0) - { - XsMessage_setDataLong(this, value, offset); - } - /*! \copydoc XsMessage_setDataLongLong - */ - void setDataLongLong(const uint64_t value, XsSize offset = 0) - { - XsMessage_setDataLongLong(this, value, offset); - } - - - /*! \copydoc XsMessage_setDataShort - */ - void setDataShort(const uint16_t value, XsSize offset = 0) - { - XsMessage_setDataShort(this, value, offset); - } - - /*! \copydoc XsMessage_setMessageId - */ - void setMessageId(const XsXbusMessageId msgId) - { - XsMessage_setMessageId(this, msgId); - } - - //! Copy message src into this - XsMessage& operator = (const XsMessage& src) - { - if (this != &src) - XsMessage_copy(this, &src); - return *this; - } - - /*! \copydoc XsMessage_deleteData */ - void deleteData(XsSize count, XsSize offset = 0) - { - XsMessage_deleteData(this, count, offset); - } - - /*! \copydoc XsMessage_insertData */ - void insertData(XsSize count, XsSize offset = 0) - { - XsMessage_insertData(this, count, offset); - } - - /*! \copydoc XsMessage_getFPValueSize */ - inline static uint8_t getFPValueSize(XsDataIdentifier id) - { - return XsMessage_getFPValueSize(id); - } - - /*! \copydoc XsMessage_getDataFPValues */ - void getDataFPValue(uint64_t outputSettings, double *dest, XsSize offset = 0, XsSize numValues = 1) const - { - XsMessage_getDataFPValues(this, outputSettings, dest, offset, numValues); - } - - /*! \brief Return current data value as a double(64 bits), after converting it from float, double, FP1632 or FP1220 depending on outputSettings - - \param outputSettings the output settings containing the current floating/fixed point settings - \param offset the offset in the message payload at which to start reading - - \returns the double precision representation of the value at \a offset - */ - double getDataFPValue(uint64_t outputSettings, XsSize offset = 0) const - { - double tmp; - XsMessage_getDataFPValues(this, outputSettings, &tmp, offset, 1); - return tmp; - } - - /*! \copydoc XsMessage_setDataFPValues */ - void setDataFPValue(uint64_t outputSettings, const double *data, XsSize offset = 0, XsSize numValues = 1) - { - XsMessage_setDataFPValues(this, outputSettings, data, offset, numValues); - } - - /*! \brief Write a floating/fixed point value into to the data buffer, conversion depends on outputSettings - - \param outputSettings MT output settings - \param data The data array to be written to the buffer. - \param offset Offset in the data buffer from where to start writing. - - \sa setDataFPValue(uint64_t, const double *, XsSize, XsSize) - */ - void setDataFPValue(uint64_t outputSettings, double data, XsSize offset = 0) - { - XsMessage_setDataFPValues(this, outputSettings, &data, offset, 1); - } - - /*! \copydoc XsMessage_getDataFPValuesById */ - void getDataFPValue(XsDataIdentifier dataIdentifier, double *dest, XsSize offset = 0, XsSize numValues = 1) const - { - XsMessage_getDataFPValuesById(this, dataIdentifier, dest, offset, numValues); - } - - /*! \brief Return current data values as double, conversion depends on outputSetting - - \param dataIdentifier Data identifier containing data precision - \param offset offset in the data buffer from where to start reading. - - \returns the current data value as double - */ - double getDataFPValue(XsDataIdentifier dataIdentifier, XsSize offset = 0) const - { - double tmp; - XsMessage_getDataFPValuesById(this, dataIdentifier, &tmp, offset, 1); - return tmp; - } - - /*! \copydoc XsMessage_setDataFPValuesById */ - void setDataFPValue(XsDataIdentifier dataIdentifier, const double *data, XsSize offset = 0, XsSize numValues = 1) - { - XsMessage_setDataFPValuesById(this, dataIdentifier, data, offset, numValues); - } - - /*! \brief Write a floating/fixed point value into to the data buffer, conversion depends on outputSettings - - \param dataIdentifier Data Identifier - \param data The data array to be written to the buffer. - \param offset Offset in the data buffer from where to start writing. - */ - void setDataFPValue(XsDataIdentifier dataIdentifier, double data, XsSize offset = 0) - { - XsMessage_setDataFPValuesById(this, dataIdentifier, &data, offset, 1); - } - - /*! \brief Return true if \a other is identical to this */ - inline bool operator == (const XsMessage& other) const - { - if (this == &other) - return true; - return m_message == other.m_message; - } - - /*! \brief Return the message in its raw form */ - inline XsByteArray const& rawMessage() const - { - return m_message; - }; - - /*! \brief Return a string containing the first \a maxBytes bytes of the message in hex format - \param maxBytes the maximum number of bytes to include in the string, when set to 0, the full message will be used - \return A string containing the first \a maxBytes bytes of the message in hex format - */ - inline XsString toHexString(XsSize maxBytes = 0) const - { - XsString rv; - XsMessage_toHexString(this, maxBytes, &rv); - return rv; - } - - /*! \brief Get data of type T from the message - \details This function can be used to get data of (simple) type T from the message. - \param data A pointer to a buffer with space for at least \a numValues of T - \param id Ignored for this function, but added here for generalization purposes - \param offset The offset of the first byte of the data in the message - \param numValues The number of consecutive values to read - */ - template - void getData(T* data, XsDataIdentifier id, XsSize offset = 0, int numValues = 1) const - { - (void) id; - for (int i = 0; i < numValues; ++i) - XsMessage_getEndianCorrectData(this, &data[i], sizeof(T), offset+i*sizeof(T)); - } - - /*! \brief Write data of type T to the message - \details This function can be used to write data of (simple) type T to the message. - \param data A pointer to a buffer with space for at least \a numValues of T - \param id Ignored for this function, but added here for generalization purposes - \param offset The offset of the first byte of the data in the message - \param numValues The number of consecutive values to write - */ - template - void setData(T const* data, XsDataIdentifier id, XsSize offset = 0, int numValues = 1) - { - (void) id; - for (int i = 0; i < numValues; ++i) - XsMessage_setEndianCorrectData(this, &data[i], sizeof(T), offset+i*sizeof(T)); - } - - /*! \brief Return the number of bytes that \a numValues items of type T will require in a message - \param id Ignored for this function, but added here for generalization purposes - \param numValues The desired number of items, used as a simple multiplier - \return The number of bytes that \a numValues items of type T will require in a message - */ - template - static int sizeInMsg(XsDataIdentifier id, int numValues = 1) - { - (void) id; - return numValues * sizeof(T); - } - -private: - /*! \brief Update the checksum pointer after changing the size of the message */ - void updateChecksumPtr() - { - XsSize sz = XsMessage_getTotalMessageSize(this); - if (sz) - *((uint8_t**) &m_checksum) = &m_message[sz-1]; - else - *((uint8_t**) &m_checksum) = 0; - } - -#endif - - XsByteArray m_message; - int m_autoUpdateChecksum; - uint8_t* const m_checksum; //! Points to the checksum to speed up automatic checksum updates -}; - -#ifdef __cplusplus -/*! \brief Get 'real' data from the message - \details This function can be used to get 'real' data from the message, possibly converting from an - underlying fixed or floating point type if necessary. - \param data A pointer to a buffer with space for at least \a numValues doubles - \param id The dataidentifer that will be used to determine the underlying data type - \param offset The offset of the first byte of the data in the message - \param numValues The number of consecutive values to read -*/ -template <> -inline void XsMessage::getData(XsReal* data, XsDataIdentifier id, XsSize offset, int numValues) const -{ - getDataFPValue(id, data, offset, numValues); -} - -/*! \brief Write 'real' data from the message - \details This function can be used to write 'real' data to the message, possibly converting to an - underlying fixed or floating point type if necessary. - \param data A pointer to a buffer with space for at least \a numValues doubles - \param id The dataidentifer that will be used to determine the underlying data type - \param offset The offset of the first byte of the data in the message - \param numValues The number of consecutive values to write -*/ -template <> -inline void XsMessage::setData(XsReal const* data, XsDataIdentifier id, XsSize offset, int numValues) -{ - setDataFPValue(id, data, offset, numValues); -} - -/*! \brief Return the number of bytes that \a numValues 'real' items will require in a message - \param id Specifies the desired underlying type to use - \param numValues The desired number of items, used as a simple multiplier - \return The number of bytes that \a numValues 'real' items will require in a message -*/ -template <> -inline int XsMessage::sizeInMsg(XsDataIdentifier id, int numValues) -{ - return XsMessage_getFPValueSize(id) * numValues; -} -#endif - -// some macros to help when constructing/parsing messages -#define swapEndian16(src) (((src) >> 8) | ((src) << 8)) -#define swapEndian32(src) (((src) >> 24) | (((src) >> 8) & 0xFF00) | (((src) << 8) & 0xFF0000) | ((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))) - -#endif // file guard diff --git a/extern/include/xsens/xsmessagearray.h b/extern/include/xsens/xsmessagearray.h deleted file mode 100644 index 2de3bb0..0000000 --- a/extern/include/xsens/xsmessagearray.h +++ /dev/null @@ -1,71 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSMESSAGEARRAY_H -#define XSMESSAGEARRAY_H - -#include "xsarray.h" -#include "pstdint.h" - -#ifdef __cplusplus -#include "xsmessage.h" -extern "C" { -#endif - -extern XsArrayDescriptor const XSTYPES_DLL_API g_xsMessageArrayDescriptor; - -#ifndef __cplusplus -#define XSMESSAGEARRAY_INITIALIZER XSARRAY_INITIALIZER(&g_xsMessageArrayDescriptor) - -struct XsMessage; -XSARRAY_STRUCT(XsMessageArray, struct XsMessage); -typedef struct XsMessageArray XsMessageArray; - -XSTYPES_DLL_API void XsMessageArray_construct(XsMessageArray* thisPtr, XsSize count, struct XsMessage const* src); -#else -} // extern "C" -#endif - -#ifdef __cplusplus -struct XsMessageArray : public XsArrayImpl { - //! \brief Constructs an XsMessageArray - inline explicit XsMessageArray(XsSize sz = 0, XsMessage const* src = 0) - : ArrayImpl(sz, src) - { - } - - //! \brief Constructs an XsMessageArray as a copy of \a other - inline XsMessageArray(XsMessageArray const& other) - : ArrayImpl(other) - { - } - - //! \brief Constructs an XsMessageArray that references the data supplied in \a ref - inline explicit XsMessageArray(XsMessage* ref, XsSize sz, XsDataFlags flags = XSDF_None) - : ArrayImpl(ref, sz, flags) - { - } - -#ifndef XSENS_NOITERATOR - //! \brief Constructs an XsMessageArray with the array bound by the supplied iterators \a beginIt and \a endIt - template - inline XsMessageArray(Iterator beginIt, Iterator endIt) - : ArrayImpl(beginIt, endIt) - { - } -#endif -}; -#endif - -#endif diff --git a/extern/include/xsens/xsmessagelist.h b/extern/include/xsens/xsmessagelist.h deleted file mode 100644 index 9e9e7bd..0000000 --- a/extern/include/xsens/xsmessagelist.h +++ /dev/null @@ -1,34 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSMESSAGELIST_H -#define XSMESSAGELIST_H - -#include "xsmessagearray.h" -#define XsMessageList XsMessageArray - -#ifndef __cplusplus -// obsolete: -#define XSMESSAGELIST_INITIALIZER XSMESSAGEARRAY_INITIALIZER -#define XsMessageList_assign(thisPtr, size, src) XsArray_assign(thisPtr, size, src) -#define XsMessageList_destruct(thisPtr) XsArray_destruct(thisPtr) -#define XsMessageList_copy(thisPtr, copy) XsArray_copy(copy, thisPtr) -#define XsMessageList_append(thisPtr, other) XsArray_append(thisPtr, other) -#define XsMessageList_popFront(thisPtr, count) XsArray_erase(thisPtr, 0, count) -#define XsMessageList_popBack(thisPtr, count) XsArray_erase(thisPtr, (XsSize) -1, count) -#define XsMessageList_swap(a, b) XsArray_swap(a, b) -#define XsMessageList_erase(thisPtr, index, count) XsArray_erase(thisPtr, index, count) -#endif - -#endif diff --git a/extern/include/xsens/xsmutex.h b/extern/include/xsens/xsmutex.h deleted file mode 100644 index fe9c457..0000000 --- a/extern/include/xsens/xsmutex.h +++ /dev/null @@ -1,280 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSMUTEX_H -#define XSMUTEX_H - -#include "xstypesconfig.h" - -#ifdef XSENS_WINDOWS -#include -#else -#include -#endif - -#ifndef __cplusplus -typedef struct XsMutex XsMutex; -#endif - -struct XsMutex; - -/*! \addtogroup cinterface C Interface - @{ -*/ -/*! \brief Create a mutex */ -static inline void XsMutex_construct(struct XsMutex* thisPtr); -/*! \brief Destroy a mutex */ -static inline void XsMutex_destruct(struct XsMutex* thisPtr); -/*! \brief Lock the mutex - \return non-zero on success, zero otherwise -*/ -static inline int XsMutex_lock(struct XsMutex* thisPtr); -/*! \brief Try locking the mutex - \return non-zero on success, zero otherwise -*/ -static inline int XsMutex_tryLock(struct XsMutex* thisPtr); -/*! \brief Unlock a previously locked mutex - \return non-zero on success, zero otherwise -*/ -static inline int XsMutex_unlock(struct XsMutex* thisPtr); -/*! @} */ - -/*! \brief A platform independent mutex implementation - -\details A mutex is used to protect data from being accessed by multiple threads at the same time. - -This mutex is always recursive, which means that a single thread can -lock the same mutex multiple times. Of course it is important to unlock -the mutex just as many times before other threads can access the -protected data again. - -XsMutex relies heavily on code inlining. The net result will be that any -dependent code will call platform code directly. - -Typical usage in C++: -\code{.cpp} -class MyData -{ -public: - MyData() : m_value(0) {} - - void setValue(int newValue); - int value() const; - -private: - int m_value; - mutable XsMutex m_mutex; -}; - -void MyData::setValue(int newValue) -{ - m_mutex.lock(); - m_value = newValue; - m_mutex.unlock(); -} - -int MyData::value() const -{ - m_mutex.lock(); - int v = m_value; - m_mutex.unlock(); - return v; -} -\endcode - - -Typical usage in C: -\code{.c} -struct MyData -{ - int value; - struct XsMutex mutex; -}; - -struct MyData d; -d.value = 0; -XsMutex_construct(&d.mutex); - -XsMutex_lock(&d.mutex); -d.value = 3; -XsMutex_unlock(&d.mutex); - -XsMutex_destruct(&d.mutex); -\endcode -*/ -struct XsMutex -{ -#ifdef __cplusplus -public: - /*! \brief Create a mutex */ - inline XsMutex() - { - XsMutex_construct(this); - } - - /*! \brief Destroy a mutex */ - inline ~XsMutex() - { - XsMutex_destruct(this); - } - - /*! \brief Lock the mutex - \return true on success - */ - inline bool lock() - { - return XsMutex_lock(this) != 0; - } - - /*! \brief Try locking the mutex - \return true on success - */ - inline bool tryLock() - { - return XsMutex_tryLock(this) != 0; - } - - /*! \brief Unlock a previously locked mutex - \return true on success - */ - inline bool unlock() - { - return XsMutex_unlock(this) != 0; - } -private: - friend inline void XsMutex_construct(struct XsMutex*); - friend inline void XsMutex_destruct(struct XsMutex*); - friend inline int XsMutex_lock(struct XsMutex*); - friend inline int XsMutex_tryLock(struct XsMutex*); - friend inline int XsMutex_unlock(struct XsMutex*); -#endif -#ifdef XSENS_WINDOWS - CRITICAL_SECTION m_mutex; -#else - pthread_mutexattr_t m_attr; - pthread_mutex_t m_mutex; -#endif -}; - -#ifdef XSENS_WINDOWS -static inline void XsMutex_construct(struct XsMutex* thisPtr) -{ - InitializeCriticalSection(&thisPtr->m_mutex); -} - -static inline void XsMutex_destruct(struct XsMutex* thisPtr) -{ - DeleteCriticalSection(&thisPtr->m_mutex); -} - -static inline int XsMutex_lock(struct XsMutex* thisPtr) -{ - EnterCriticalSection(&thisPtr->m_mutex); - return TRUE; -} - -static inline int XsMutex_tryLock(struct XsMutex* thisPtr) -{ - return TryEnterCriticalSection(&thisPtr->m_mutex) == 1; -} - -static inline int XsMutex_unlock(struct XsMutex* thisPtr) -{ - LeaveCriticalSection(&thisPtr->m_mutex); - return TRUE; -} -#else -static inline void XsMutex_construct(struct XsMutex* thisPtr) -{ - pthread_mutexattr_init(&thisPtr->m_attr); - pthread_mutexattr_settype(&thisPtr->m_attr, PTHREAD_MUTEX_RECURSIVE); - pthread_mutex_init(&thisPtr->m_mutex, &thisPtr->m_attr); -} - -static inline void XsMutex_destruct(struct XsMutex* thisPtr) -{ - pthread_mutex_destroy(&thisPtr->m_mutex); - pthread_mutexattr_destroy(&thisPtr->m_attr); -} - -static inline int XsMutex_lock(struct XsMutex* thisPtr) -{ - return pthread_mutex_lock(&thisPtr->m_mutex) == 0; -} - -static inline int XsMutex_tryLock(struct XsMutex* thisPtr) -{ - return pthread_mutex_trylock(&thisPtr->m_mutex) == 0; -} - -static inline int XsMutex_unlock(struct XsMutex* thisPtr) -{ - return pthread_mutex_unlock(&thisPtr->m_mutex) == 0; -} -#endif - -#ifdef __cplusplus -/*! \brief A RAII object that ensures mutexes are unlocked when leaving scope - * - * When using this object, it is advised not to touch the mutex we're working - * on. The internal states may mismatch and result in undefined behavior. - */ -struct XsMutexLocker -{ -public: - /*! \brief Create a mutex locker and lock the mutex */ - explicit XsMutexLocker(XsMutex& m) : - m_mutex(m), - m_locked(false) - { - lock(); - } - - /*! \brief Destroy the mutex locker -- unlock the mutex */ - ~XsMutexLocker() - { - unlock(); - } - - /*! \brief Lock the mutex if it isn't already locked */ - inline bool lock() - { - if (!m_locked) - return (m_locked = m_mutex.lock()); - return true; - } - - /*! \brief Unlock the mutex if it isn't already unlocked */ - inline bool unlock() - { - if (m_locked) - return !(m_locked = !m_mutex.unlock()); - return true; - } - - /*! \brief Try to lock the mutex */ - inline bool tryLock() - { - if (!m_locked) - return (m_locked = m_mutex.tryLock()); - return true; - } - -private: - XsMutex& m_mutex; - bool m_locked; -}; -#endif - -#endif diff --git a/extern/include/xsens/xsoutputconfiguration.h b/extern/include/xsens/xsoutputconfiguration.h deleted file mode 100644 index 6b5e5d0..0000000 --- a/extern/include/xsens/xsoutputconfiguration.h +++ /dev/null @@ -1,68 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSOUTPUTCONFIGURATION_H -#define XSOUTPUTCONFIGURATION_H - -#include "xstypesconfig.h" -#include "pstdint.h" -#include "xsdataidentifier.h" - -#define XS_MAX_OUTPUTCONFIGURATIONS (32) - -#ifdef __cplusplus -extern "C" { -#else -#define XSOUTPUTCONFIGURATION_INITIALIZER { XDI_None, 0 } -#endif - -struct XsOutputConfiguration; - -XSTYPES_DLL_API void XsOutputConfiguration_swap(struct XsOutputConfiguration* a, struct XsOutputConfiguration* b); - -#ifdef __cplusplus -} // extern "C" -#endif - - -/*! \brief Single data type output configuration - \details This structure contains a single data type and the frequency at which it should be produced. - If m_frequency is 0xFFFF and the %XsOutputConfiguration is used for input, the device will configure - 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 { - XsDataIdentifier m_dataIdentifier; //!< The data identifier - uint16_t m_frequency; //!< The frequency - -#ifdef __cplusplus - //! Constructor, initializes to an empty object - XsOutputConfiguration() - : m_dataIdentifier(XDI_None), m_frequency(0) {} - - //! Constructor, initializes to specified values - XsOutputConfiguration(XsDataIdentifier di, uint16_t freq) - : m_dataIdentifier(di), m_frequency(freq) - {} - - //! Comparison operator - bool operator == (const XsOutputConfiguration& other) const - { - return m_dataIdentifier == other.m_dataIdentifier && m_frequency == other.m_frequency; - } -#endif -}; -typedef struct XsOutputConfiguration XsOutputConfiguration; - -#endif diff --git a/extern/include/xsens/xsoutputconfigurationarray.h b/extern/include/xsens/xsoutputconfigurationarray.h deleted file mode 100644 index 441e7a1..0000000 --- a/extern/include/xsens/xsoutputconfigurationarray.h +++ /dev/null @@ -1,70 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSOUTPUTCONFIGURATIONARRAY_H -#define XSOUTPUTCONFIGURATIONARRAY_H - -#include "xsarray.h" - -#ifdef __cplusplus -#include "xsoutputconfiguration.h" -extern "C" { -#endif - -extern XsArrayDescriptor const XSTYPES_DLL_API g_xsOutputConfigurationArrayDescriptor; - -#ifndef __cplusplus -#define XSOUTPUTCONFIGURATIONARRAY_INITIALIZER XSARRAY_INITIALIZER(&g_xsOutputConfigurationArrayDescriptor) -struct XsOutputConfiguration; - -XSARRAY_STRUCT(XsOutputConfigurationArray, struct XsOutputConfiguration); -typedef struct XsOutputConfigurationArray XsOutputConfigurationArray; - -XSTYPES_DLL_API void XsOutputConfigurationArray_construct(XsOutputConfigurationArray* thisPtr, XsSize count, struct XsOutputConfiguration const* src); -#else -} // extern "C" -#endif - -#ifdef __cplusplus -struct XsOutputConfigurationArray : public XsArrayImpl { - //! \brief Constructs an XsOutputConfigurationArray - inline explicit XsOutputConfigurationArray(XsSize sz = 0, XsOutputConfiguration const* src = 0) - : ArrayImpl(sz, src) - { - } - - //! \brief Constructs an XsOutputConfigurationArray as a copy of \a other - inline XsOutputConfigurationArray(XsOutputConfigurationArray const& other) - : ArrayImpl(other) - { - } - - //! \brief Constructs an XsOutputConfigurationArray that references the data supplied in \a ref - inline explicit XsOutputConfigurationArray(XsOutputConfiguration* ref, XsSize sz, XsDataFlags flags = XSDF_None) - : ArrayImpl(ref, sz, flags) - { - } - -#ifndef XSENS_NOITERATOR - //! \brief Constructs an XsOutputConfigurationArray with the array bound by the supplied iterators \a beginIt and \a endIt - template - inline XsOutputConfigurationArray(Iterator beginIt, Iterator endIt) - : ArrayImpl(beginIt, endIt) - { - } -#endif -}; -#endif - -#endif diff --git a/extern/include/xsens/xsoutputconfigurationlist.h b/extern/include/xsens/xsoutputconfigurationlist.h deleted file mode 100644 index 7faa1fa..0000000 --- a/extern/include/xsens/xsoutputconfigurationlist.h +++ /dev/null @@ -1,36 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSOUTPUTCONFIGURATIONLIST_H -#define XSOUTPUTCONFIGURATIONLIST_H - -#include "xsoutputconfigurationarray.h" -#define XsOutputConfigurationList XsOutputConfigurationArray - -#ifndef __cplusplus -// obsolete: -#define XSOUTPUTCONFIGURATIONLIST_INITIALIZER XSOUTPUTCONFIGURATIONARRAY_INITIALIZER -#define XsOutputConfigurationList_assign(thisPtr, size, src) XsArray_assign(thisPtr, size, src) -#define XsOutputConfigurationList_construct(thisPtr, size, src) XsOutputConfigurationArray_construct(thisPtr, size, src) -#define XsOutputConfigurationList_destruct(thisPtr) XsArray_destruct(thisPtr) -#define XsOutputConfigurationList_copy(thisPtr, copy) XsArray_copy(copy, thisPtr) -#define XsOutputConfigurationList_append(thisPtr, other) XsArray_append(thisPtr, other) -#define XsOutputConfigurationList_popFront(thisPtr, count) XsArray_erase(thisPtr, 0, count) -#define XsOutputConfigurationList_popBack(thisPtr, count) XsArray_erase(thisPtr, (XsSize)-1, count) -#define XsOutputConfigurationList_swap(a, b) XsArray_swap(a, b) -#define XsOutputConfigurationList_erase(thisPtr, index, count) XsArray_erase(thisPtr, index, count) -#define XsOutputConfigurationList_equal(a, b) XsArray_equal(a, b) -#endif - -#endif diff --git a/extern/include/xsens/xsoutputmode.h b/extern/include/xsens/xsoutputmode.h deleted file mode 100644 index bd3070b..0000000 --- a/extern/include/xsens/xsoutputmode.h +++ /dev/null @@ -1,66 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSOUTPUTMODE_H -#define XSOUTPUTMODE_H - -/*! \addtogroup enums Global enumerations - @{ -*/ -//! Bit values for legacy output mode -enum XsOutputMode { - XOM_None = 0x0000, //!< No data output \deprecated - XOM_Temperature = 0x0001, //!< Temperature data output \deprecated - XOM_Calibrated = 0x0002, //!< Calibrated data output \deprecated - XOM_Orientation = 0x0004, //!< Orientation data output \deprecated - XOM_Auxiliary = 0x0008, //!< Auxiliary data output \deprecated - XOM_Position = 0x0010, //!< Position data output \deprecated - XOM_Velocity = 0x0020, //!< Velocity data output \deprecated - XOM_Sdi = 0x0200, //!< SDI data output \deprecated - XOM_Status = 0x0800, //!< Status data output \deprecated - XOM_GpsPvt_Pressure = 0x1000, //!< GPS-PVT and pressure data output \deprecated - XOM_Reserved = 0x2000, //!< Reserved \deprecated - XOM_Raw = 0x4000, //!< Raw data output \deprecated - XOM_Mt9 = 0x8000 //!< Legacy MT9-C data output \deprecated -}; -/*! @} */ -typedef enum XsOutputMode XsOutputMode; - -// Extended (analog) Output Modes -#define XS_EXTOUTPUTMODE_DISABLED 0x0000 -#define XS_EXTOUTPUTMODE_EULER 0x0001 - -#define XS_DEFAULT_OUTPUT_MODE XOM_Orientation - -#ifdef __cplusplus -/*! \brief Allow logical or of XsOutputMode to be a valid XsOutputMode value */ -inline XsOutputMode operator | (XsOutputMode a, XsOutputMode b) -{ - return (XsOutputMode) ((unsigned short) a | (unsigned short) b); -} - -/*! \brief Allow logical and of XsOutputMode to be a valid XsOutputMode value */ -inline XsOutputMode operator & (XsOutputMode a, XsOutputMode b) -{ - return (XsOutputMode) ((unsigned short) a & (unsigned short) b); -} - -/*! \brief Allow logical inversion of XsOutputMode to be a valid XsOutputMode value */ -inline XsOutputMode operator ~ (XsOutputMode a) -{ - return (XsOutputMode) ~((unsigned short)a); -} -#endif - -#endif // XSOUTPUTMODE_H diff --git a/extern/include/xsens/xsoutputsettings.h b/extern/include/xsens/xsoutputsettings.h deleted file mode 100644 index 1dc3ea1..0000000 --- a/extern/include/xsens/xsoutputsettings.h +++ /dev/null @@ -1,110 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSOUTPUTSETTINGS_H -#define XSOUTPUTSETTINGS_H - -/*! \addtogroup enums Global enumerations - @{ -*/ -//! Bit values for legacy output settings -enum XsOutputSettings { - XOS_Timestamp_Mask = 0x00000003, //!< Timestamp mask \deprecated - XOS_Timestamp_None = 0x00000000, //!< No timestamp \deprecated - XOS_Timestamp_PacketCounter = 0x00000001, //!< Packetcounter timestamp\deprecated - XOS_Timestamp_SampleUtc = 0x00000002, //!< UTC timestamp \deprecated - XOS_OrientationMode_Mask = 0x0000000C, //!< Orientation mode mask \deprecated - XOS_OrientationMode_Quaternion = 0x00000000, //!< Quaternion orientation data \deprecated - XOS_OrientationMode_Euler = 0x00000004, //!< Euler angles orientation data \deprecated - XOS_OrientationMode_Matrix = 0x00000008, //!< Rotation matrix orientation data \deprecated - XOS_CalibratedMode_All = 0x00000000, //!< All calibrated data \deprecated - XOS_CalibratedMode_AccGyrMag_Mask = 0x00000070, //!< Mask for acceleration, gyroscope and magnetometer data \deprecated - XOS_CalibratedMode_Mask = XOS_CalibratedMode_AccGyrMag_Mask, //!< Mask for acceleration, gyroscope and magnetometer data \deprecated - XOS_CalibratedMode_Acc_Mask = 0x00000010, //!< Mask for accelerationdata \deprecated - XOS_CalibratedMode_AccOnly = 0x00000060, //!< Acceleration output only \deprecated - XOS_CalibratedMode_AccGyrOnly = 0x00000040, //!< Acceleration and gyroscope output only \deprecated - XOS_CalibratedMode_AccMagOnly = 0x00000020, //!< Acceleration and magnetometer output only \deprecated - XOS_CalibratedMode_Gyr_Mask = 0x00000020, //!< Mask for gyroscope data \deprecated - XOS_CalibratedMode_GyrOnly = 0x00000050, //!< Gyroscope output only \deprecated - XOS_CalibratedMode_GyrMagOnly = 0x00000010, //!< Gyroscope and magnetometer output only \deprecated - XOS_CalibratedMode_Mag_Mask = 0x00000040, //!< Mask for magnetometer data \deprecated - XOS_CalibratedMode_MagOnly = 0x00000030, //!< Magnetometer output only \deprecated - XOS_Status_Compact = 0x00000000, //!< Compact status output \deprecated - XOS_Status_Detailed = 0x00000080, //!< Detailed status output \deprecated - XOS_Dataformat_Mask = 0x00000300, //!< Mask for the data format output for all none-raw values \deprecated - XOS_Dataformat_Float = 0x00000000, //!< Floating point data format for all none-raw values \deprecated - XOS_Dataformat_F1220 = 0x00000100, //!< Fixed point 12.20 data format for all none-raw values \deprecated - XOS_Dataformat_Fp1632 = 0x00000200, //!< Fixed point 16.32 data format for all none-raw values \deprecated - XOS_Dataformat_Double = 0x00000300, //!< Double data format for all none-raw values \deprecated - - XOS_AuxiliaryMode_Mask = 0x00000C00, //!< Auxiliary mode mask \deprecated - XOS_AuxiliaryMode_Ain1_Mask = 0x00000400, //!< Auxiliary mode analog in 1 mask \deprecated - XOS_AuxiliaryMode_Ain2_Mask = 0x00000800, //!< Auxiliary mode analog in 2 mask \deprecated - XOS_AuxiliaryMode_Ain1 = 0x00000800, //!< Auxiliary mode analog in 1 \deprecated - XOS_AuxiliaryMode_Ain2 = 0x00000400, //!< Auxiliary mode analog in 2 \deprecated - XOS_PositionMode_Mask = 0x0001C000, //!< Position mode mask \deprecated - XOS_PositionMode_Lla_Wgs84 = 0x00000000, //!< Position in latitude, longitude format \deprecated - XOS_VelocityMode_Mask = 0x00060000, //!< Velocity mask \deprecated - XOS_VelocityMode_Ms_Xyz = 0x00000000, //!< Velocity in m/s, XYZ coordinate system \deprecated - XOS_GpsInGpsPvt = 0x00000000, //!< Gps in PVT data \deprecated - XOS_NoGpsInGpsPvt = 0x00080000, //!< No Gps in PVT data \deprecated - XOS_ExtendedTemperature_Mask = 0x01000000, //!< Extended temperature mask\deprecated - XOS_SampleTimeFine_Mask = 0x02000000, //!< Sample time fine mask \deprecated - XOS_Coordinates_Ned = 0x80000000 //!< North east down coordinates \deprecated - - //XOS_Uncertainty_Orient = 0x00100000 - //XOS_Uncertainty_Pos = 0x00200000 - //XOS_Uncertainty_Vel = 0x00400000 - //XOS_Uncertainty_Mask = 0x00F00000 - -}; -/*! @} */ -typedef enum XsOutputSettings XsOutputSettings; - -#define XS_DEFAULT_OUTPUT_SETTINGS (XsOutputSettings)(XOS_OrientationMode_Quaternion | XOS_Timestamp_PacketCounter) - -#ifdef __cplusplus -/*! \brief Allow logical or of XsOutputSettings to be a valid XsOutputSettings value */ -inline XsOutputSettings operator | (XsOutputSettings a, XsOutputSettings b) -{ - return (XsOutputSettings) ((unsigned long) a | (unsigned long) b); -} - -/*! \brief Allow logical and of XsOutputSettings to be a valid XsOutputSettings value */ -inline XsOutputSettings operator & (XsOutputSettings a, XsOutputSettings b) -{ - return (XsOutputSettings) ((unsigned long) a & (unsigned long) b); -} - -/*! \brief Allow logical inversion of XsOutputSettings to be a valid XsOutputSettings value */ -inline XsOutputSettings operator ~ (XsOutputSettings a) -{ - return (XsOutputSettings) ~((unsigned long)a); -} - -/*! \brief Allow &= operator on XsOutputSettings */ -inline XsOutputSettings& operator &= (XsOutputSettings& left, XsOutputSettings right) -{ - return left = left & right; -} - -/*! \brief Allow |= operator on XsOutputSettings */ -inline XsOutputSettings& operator |= (XsOutputSettings& left, XsOutputSettings right) -{ - return left = left | right; -} - -#endif - -#endif diff --git a/extern/include/xsens/xsplatform.h b/extern/include/xsens/xsplatform.h deleted file mode 100644 index 15606c7..0000000 --- a/extern/include/xsens/xsplatform.h +++ /dev/null @@ -1,34 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSPLATFORM_H -#define XSPLATFORM_H - -#include "xstypesconfig.h" - -#ifdef _WIN32 -/// microsoft / windows -#include - -#define XsIoHandle HANDLE - -#else -/// gcc / linux -#include - -typedef int32_t XsIoHandle; - -#endif - -#endif // file guard diff --git a/extern/include/xsens/xsportinfo.h b/extern/include/xsens/xsportinfo.h deleted file mode 100644 index ba47e96..0000000 --- a/extern/include/xsens/xsportinfo.h +++ /dev/null @@ -1,294 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSPORTINFO_H -#define XSPORTINFO_H - -#include "xstypesconfig.h" -#include "pstdint.h" -#include "xsdeviceid.h" -#include "xsbaud.h" -#include "xsstring.h" -#include - -#define XSENS_VENDOR_ID 0x2639 -#define FTDI_VENDOR_ID 0x0403 // needed for Xsens USB-serial converters -#define ATMEL_VENDOR_ID 0x03eb // needed for old MTw -#define ATMEL_BORROWED_PRODUCT_ID 0x2307 // needed for old MTw -#define ST_VENDOR_ID 0x0483 // needed for FIS2100 EVK (ST Nucleo) - -#ifdef __cplusplus -extern "C" { -#endif - -#ifndef __cplusplus - typedef struct XsPortInfo XsPortInfo; -#else - 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_isNetwork(XsPortInfo const* thisPtr); -XSTYPES_DLL_API const char* XsPortInfo_networkServiceName(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); - -#ifdef __cplusplus -} -#endif - -#if defined(_MSC_VER) -#pragma warning(push) -#pragma warning(disable : 4996) -#endif - -typedef enum XsPortLinesOptions -{ - XPLO_Invalid = 0, - - XPLO_RTS_Set = (1 << 0), - XPLO_RTS_Clear = (1 << 1), - XPLO_RTS_Ignore = (1 << 2), - - XPLO_DTR_Set = (1 << 3), - XPLO_DTR_Clear = (1 << 4), - XPLO_DTR_Ignore = (1 << 5), - - XPLO_All_Set = XPLO_RTS_Set | XPLO_DTR_Set, - XPLO_All_Clear = XPLO_RTS_Clear | XPLO_DTR_Clear, - XPLO_All_Ignore = XPLO_RTS_Ignore | XPLO_DTR_Ignore, - - XPLO_RtsCtsFlowControl = (1 << 6) -} XsPortLinesOptions; - -struct XsPortInfo { -#ifdef __cplusplus - /*! \brief Default constructor, creates an empty port info object */ - XsPortInfo() - : m_deviceId(0) - , m_baudrate(XBR_Invalid) - , m_linesOptions(XPLO_All_Ignore) - , m_vid(0) - , m_pid(0) - { - m_portName[0] = '\0'; - } - - /*! \brief Named constructor, initializes the object to the supplied \a portname and optional \a baudRate - \param portname The name of the port, maximum 255 characters - \param baudRate The baud rate to configure for the port, for scanning XBR_Invalid may be used to scan all known baud rates - \param linesOptions The options for the hardware flow control lines - */ - explicit XsPortInfo(const XsString& portname, XsBaudRate baudRate = XBR_Invalid, XsPortLinesOptions linesOptions = XPLO_All_Ignore) - : m_deviceId(0) - , m_baudrate(baudRate) - , m_linesOptions(linesOptions) - , m_vid(0) - , m_pid(0) - { - if (portname.size() < 255) - strcpy(m_portName, portname.c_str()); - else - m_portName[0] = '\0'; - } - -#ifndef XSENS_NO_PORT_NUMBERS - /*! \brief Port number constructor, initializes the port to have COM as its name and the optional \a baudRate for a baud rate - \param portNr The number of the COM port - \param baudRate The baud rate to configure for the port, for scanning XBR_Invalid may be used to scan all known baud rates - \param linesOptions The options for the hardware flow control lines - \note Numbered COM ports are only available on Windows platforms. - */ - explicit XsPortInfo(int portNr, XsBaudRate baudRate = XBR_Invalid, XsPortLinesOptions linesOptions = XPLO_All_Ignore) - : m_deviceId(0) - , m_baudrate(baudRate) - , m_linesOptions(linesOptions) - , m_vid(0) - , m_pid(0) - { - sprintf(m_portName, "COM%d", portNr); - } -#endif - - /*! \brief \copybrief XsPortInfo_clear */ - inline void clear() - { - XsPortInfo_clear(this); - } - - /*! \brief \copybrief XsPortInfo_empty */ - inline bool empty() const - { - return XsPortInfo_empty(this) != 0; - } - - /*! \brief greater than operator, used for sorting the list. */ - 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; } - - /*! \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; } - - /*! \brief equality operator, used for finding items in a list. */ - inline bool operator == (const char *port) const { return strcmp(m_portName, port) == 0; } - - /*! \copydoc XsPortInfo_portNumber */ - inline int portNumber() const - { - return XsPortInfo_portNumber(this); - } - - /*! \brief The port name - */ - inline XsString portName() const - { - return XsString(m_portName); - } - - /*! \brief Set the port name - */ - inline void setPortName(const XsString& portName_) - { - strncpy(m_portName, portName_.c_str(), 256); - } - - /*! \brief \copybrief XsPortInfo_isUsb */ - inline bool isUsb() const - { - return XsPortInfo_isUsb(this) != 0; - } - - /*! \brief \copybrief XsPortInfo_isNetwork */ - inline bool isNetwork() const - { - return XsPortInfo_isNetwork(this) != 0; - } - - /*! \brief \copybrief XsPortInfo_networkServiceName */ - inline XsString networkServiceName() const - { - return XsString(XsPortInfo_networkServiceName(this)); - } - - /*! \copydoc XsPortInfo_usbBus */ - inline int usbBus() const - { - return XsPortInfo_usbBus(this); - } - - /*! \copydoc XsPortInfo_usbAddress */ - inline int usbAddress() const - { - return XsPortInfo_usbAddress(this); - } - - /*! \brief The baudrate - */ - inline XsBaudRate baudrate() const - { - return m_baudrate; - } - - /*! \brief Set the baudrate - */ - inline void setBaudrate(XsBaudRate baudrate_) - { - m_baudrate = baudrate_; - } - - /*! \brief The device ID - */ - inline XsDeviceId deviceId() const - { - return m_deviceId; - } - - /*! \brief Set the device ID - */ - inline void setDeviceId(XsDeviceId deviceId_) - { - m_deviceId = deviceId_; - } - - /*! \brief The options for the hw flow control lines - */ - inline XsPortLinesOptions linesOptions() const - { - return m_linesOptions; - } - - /*! \brief Set the options for the hw flow control lines - */ - inline void setLinesOptions(XsPortLinesOptions linesOptions) - { - m_linesOptions = linesOptions; - } - - /*! \brief Get the vid and pid of this portinfo - */ - inline void getVidPid(uint16_t& vid, uint16_t& pid) const - { - vid = m_vid; - pid = m_pid; - } - - /*! \brief Set the vid and pid - */ - inline void setVidPid(uint16_t vid, uint16_t pid) - { - m_vid = vid; - m_pid = pid; - } - -private: -#endif - - XsDeviceId m_deviceId; //!< The device Id of main Xsens device detected on the port - char m_portName[256]; //!< The port name - XsBaudRate m_baudrate; //!< The baudrate at which an Xsens device was detected, may be XBR_Invalid for pure USB ports - XsPortLinesOptions m_linesOptions; //!< The hardware flow control lines options for the port - uint16_t m_vid, m_pid; //!< The USB Vendor Id and Hardware Id of this connection, when available -}; - -#if defined(_MSC_VER) -#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; -} -} - -#endif - - -#endif diff --git a/extern/include/xsens/xsportinfoarray.h b/extern/include/xsens/xsportinfoarray.h deleted file mode 100644 index c0358fd..0000000 --- a/extern/include/xsens/xsportinfoarray.h +++ /dev/null @@ -1,70 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSPORTINFOARRAY_H -#define XSPORTINFOARRAY_H - -#include "xsarray.h" -#include "xsportinfo.h" - -#ifdef __cplusplus -extern "C" { -#endif - -extern XsArrayDescriptor const XSTYPES_DLL_API g_xsPortInfoArrayDescriptor; - -#ifndef __cplusplus -#define XSPORTINFOARRAY_INITIALIZER XSARRAY_INITIALIZER(&g_xsPortInfoArrayDescriptor) -struct XsPortInfo; - -XSARRAY_STRUCT(XsPortInfoArray, struct XsPortInfo); -typedef struct XsPortInfoArray XsPortInfoArray; - -XSTYPES_DLL_API void XsPortInfoArray_construct(XsPortInfoArray* thisPtr, XsSize count, struct XsPortInfo const* src); -#else -} // extern "C" -#endif - -#ifdef __cplusplus -struct XsPortInfoArray : public XsArrayImpl { - //! \brief Constructs an XsPortInfoArray - inline explicit XsPortInfoArray(XsSize sz = 0, XsPortInfo const* src = 0) - : ArrayImpl(sz, src) - { - } - - //! \brief Constructs an XsPortInfoArray as a copy of \a other - inline XsPortInfoArray(XsPortInfoArray const& other) - : ArrayImpl(other) - { - } - - //! \brief Constructs an XsPortInfoArray that references the data supplied in \a ref - inline explicit XsPortInfoArray(XsPortInfo* ref, XsSize sz, XsDataFlags flags = XSDF_None) - : ArrayImpl(ref, sz, flags) - { - } - -#ifndef XSENS_NOITERATOR - //! \brief Constructs an XsPortInfoArray with the array bound by the supplied iterators \a beginIt and \a endIt - template - inline XsPortInfoArray(Iterator beginIt, Iterator endIt) - : ArrayImpl(beginIt, endIt) - { - } -#endif -}; -#endif - -#endif diff --git a/extern/include/xsens/xsportinfolist.h b/extern/include/xsens/xsportinfolist.h deleted file mode 100644 index 9302c12..0000000 --- a/extern/include/xsens/xsportinfolist.h +++ /dev/null @@ -1,34 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSPORTINFOLIST_H -#define XSPORTINFOLIST_H - -#include "xsportinfoarray.h" - -#define XsPortInfoList XsPortInfoArray - -#ifndef __cplusplus -// obsolete: -#define XSPORTINFOLIST_INITIALIZER XSPORTINFOARRAY_INITIALIZER -#define XsPortInfoList_assign(thisPtr, size, src) XsArray_assign(thisPtr, sz, src) -#define XsPortInfoList_construct(thisPtr, size, src) XsPortInfoArray_construct(thisPtr, size, src) -#define XsPortInfoList_destruct(thisPtr) XsArray_destruct(thisPtr) -#define XsPortInfoList_copy(thisPtr, copy) XsArray_copy(copy, thisPtr) -#define XsPortInfoList_append(thisPtr, other) XsArray_append(thisPtr, other) -#define XsPortInfoList_erase(thisPtr, index, count) XsArray_erase(thisPtr, index, count) -#define XsPortInfoList_swap(a, b) XsArray_swap(a, b) -#endif - -#endif diff --git a/extern/include/xsens/xspressure.h b/extern/include/xsens/xspressure.h deleted file mode 100644 index 1bb8cd9..0000000 --- a/extern/include/xsens/xspressure.h +++ /dev/null @@ -1,70 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSPRESSURE_H -#define XSPRESSURE_H - -#include "pstdint.h" - -/*! \brief Pressure data. - \details Contains the pressure data and the pressure age -*/ -struct XsPressure { -#ifdef __cplusplus - /*! \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) - { - } - - /*! \brief Create a new XsPressure as copy from \a other - \param other the pressure carrier to copy from - */ - inline XsPressure(XsPressure const& other) : - m_pressure(other.m_pressure), - m_pressureAge(other.m_pressureAge) - { - } - - /*! \brief Copy the data from \a other - \param other the pressure carrier to copy from - \return this - */ - inline XsPressure const & operator=(XsPressure const& other) - { - //lint --e{1529} trivial assignment - m_pressure = other.m_pressure; - m_pressureAge = other.m_pressureAge; - return *this; - } - - /*! \brief Return true if this is equal to \a other - \param other the pressure carrier to compare against - \return true if both XsPressures are equal - */ - 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 -}; -typedef struct XsPressure XsPressure; - -#endif diff --git a/extern/include/xsens/xsquaternion.h b/extern/include/xsens/xsquaternion.h deleted file mode 100644 index eccc5cc..0000000 --- a/extern/include/xsens/xsquaternion.h +++ /dev/null @@ -1,276 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSQUATERNION_H -#define XSQUATERNION_H - -#include "xsmath.h" - -struct XsEuler; -struct XsMatrix; -struct XsVector; -struct XsQuaternion; - -#ifdef __cplusplus -extern "C" { -#else -#define XSQUATERNION_INITIALIZER { { { XsMath_zero, XsMath_zero, XsMath_zero, XsMath_zero } } } -typedef struct XsQuaternion XsQuaternion; -#endif - -XSTYPES_DLL_API void XsQuaternion_destruct(XsQuaternion* thisPtr); -XSTYPES_DLL_API int XsQuaternion_empty(const XsQuaternion* thisPtr); -XSTYPES_DLL_API void XsQuaternion_inverse(const XsQuaternion* thisPtr, XsQuaternion* dest); -XSTYPES_DLL_API XsReal XsQuaternion_normalized(const XsQuaternion* thisPtr, XsQuaternion* dest); -XSTYPES_DLL_API XsReal XsQuaternion_normalize(XsQuaternion* thisPtr); -XSTYPES_DLL_API void XsQuaternion_fromEulerAngles(XsQuaternion* thisPtr, const struct XsEuler* src); -XSTYPES_DLL_API void XsQuaternion_fromRotationMatrix(XsQuaternion* thisPtr, const struct XsMatrix* ori); -XSTYPES_DLL_API const XsQuaternion* XsQuaternion_identity(void); -XSTYPES_DLL_API void XsQuaternion_multiply(const XsQuaternion* left, const XsQuaternion* right, XsQuaternion* dest); -XSTYPES_DLL_API void XsQuaternion_swap(XsQuaternion* a, XsQuaternion* b); -XSTYPES_DLL_API void XsQuaternion_copy(XsQuaternion* copy, XsQuaternion const* src); -XSTYPES_DLL_API int XsQuaternion_equal(XsQuaternion const* a, XsQuaternion const* b); -XSTYPES_DLL_API int XsQuaternion_compare(XsQuaternion const* thisPtr, XsQuaternion const* other, XsReal tolerance); -XSTYPES_DLL_API XsReal XsQuaternion_dotProduct(XsQuaternion const* thisPtr, XsQuaternion const* other); - -#ifdef __cplusplus -} // extern "C" -#endif - -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 - XsReal m_z; //!< Stores the z component of the quaternion - }; - XsReal m_data[4]; //!< Stores the quaternion in an array of four elements - }; -#ifdef __cplusplus -public: - //! \brief Construct a quaternion with the supplied values, or zero's by default - inline explicit XsQuaternion(XsReal w_ = XsMath_zero, XsReal x_ = XsMath_zero, XsReal y_ = XsMath_zero, XsReal z_ = XsMath_zero) - : m_w(w_) - , m_x(x_) - , m_y(y_) - , m_z(z_) - {} - - //! \brief Construct a quaternion with the supplied values from the \a other Quaternion - inline XsQuaternion(const XsQuaternion& other) - : m_w(other.m_w) - , m_x(other.m_x) - , m_y(other.m_y) - , m_z(other.m_z) - {} - - //! \brief Construct a quaternion by converting from an XsEuler object - inline explicit XsQuaternion(const XsEuler& euler) - { - XsQuaternion_fromEulerAngles(this, &euler); - } - - //! \brief Construct a quaternion by converting from an XsMatrix rotation matrix object - inline explicit XsQuaternion(const XsMatrix& ori) - { - XsQuaternion_fromRotationMatrix(this, &ori); - } - - //! \brief Destructor, intentionally empty - inline ~XsQuaternion() - { - } - - //! \brief Assigns the \a other quaternion to this quaternion - inline XsQuaternion& operator=(const XsQuaternion& other) - { - if (this != &other) - { - m_w = other.m_w; - m_x = other.m_x; - m_y = other.m_y; - m_z = other.m_z; - } - return *this; - } - - /*! \brief Set the Quaternion to these specific values - */ - inline void assign(XsReal w_, XsReal x_, XsReal y_, XsReal z_) - { - m_w = w_; - m_x = x_; - m_y = y_; - m_z = z_; - } - - /*! \brief Set the Quaternion to the specific values in the supplied array. - \param values An array that contains at least 4 items. The first four will be interpreted as w,x,y,z - */ - inline void assign(const XsReal* values) - { - for (int i = 0; i < 4; ++i) - m_data[i] = values[i]; - } - - //! \brief Returns a reference to the \a index'th component of the quaternion - inline XsReal& operator[](XsSize index) - { - assert(index < 4); - return m_data[index]; - } - - //! \brief Returns the \a index'th component of the quaternion - inline XsReal operator[](XsSize index) const - { - assert(index < 4); - return m_data[index]; - } - - //! \brief Return a const pointer to the internal data - inline const XsReal* data() const - { - return m_data; - } - - //! \brief \copybrief XsQuaternion_inverse \returns The inverse/conjugate of the quaternion - inline XsQuaternion inverse() const - { - XsQuaternion tmp; - XsQuaternion_inverse(this, &tmp); - return tmp; - } - - //! \brief \copybrief XsQuaternion_inverse \returns The inverse/conjugate of the quaternion - inline XsQuaternion conjugate() const - { - XsQuaternion tmp; - XsQuaternion_inverse(this, &tmp); - return tmp; - } - - //! \brief Return a normalized version of the quaternion \sa XsQuaternion_normalized \returns The normalized quaternion - XsQuaternion normalized() const - { - XsQuaternion tmp; - XsQuaternion_normalized(this, &tmp); - return tmp; - } - - //! \brief Normalize the quaternion \sa XsQuaternion_normalized \returns The cartesian length of the quaternion before normalization - inline XsReal normalize() - { - return XsQuaternion_normalized(this, this); - } - - //! \brief \copybrief XsQuaternion_empty - inline bool empty() const - { - return 0 != XsQuaternion_empty(this); - } - - //! \brief \copybrief XsQuaternion_fromEulerAngles - inline XsQuaternion& fromEulerAngles(const XsEuler& src) - { - XsQuaternion_fromEulerAngles(this, &src); - return *this; - } - - //! \brief \copybrief XsQuaternion_fromRotationMatrix - inline XsQuaternion& fromRotationMatrix(const XsMatrix& ori) - { - XsQuaternion_fromRotationMatrix(this, &ori); - return *this; - } - - //! \brief \copybrief XsQuaternion_identity - inline static const XsQuaternion& identity() - { - return *XsQuaternion_identity(); - } - - /*! \brief In-place multiplication of this quaternion with \a other quaternion - \param other The other quaternion to multiply with (right side of multiplication) - \sa XsQuaternion_multiply() - */ - inline void operator*=(const XsQuaternion& other) - { - XsQuaternion_multiply(this, &other, this); - } - - //! \brief Return the w component of the quaternion - inline XsReal w() const { return m_w; } - //! \brief Return the x component of the quaternion - inline XsReal x() const { return m_x; } - //! \brief Return the y component of the quaternion - inline XsReal y() const { return m_y; } - //! \brief Return the z component of the quaternion - inline XsReal z() const { return m_z; } - //! \brief Return a reference to the w component of the quaternion - inline XsReal& w() { return m_w; } - //! \brief Return a reference to the x component of the quaternion - inline XsReal& x() { return m_x; } - //! \brief Return a reference to the y component of the quaternion - inline XsReal& y() { return m_y; } - //! \brief Return a reference to the z component of the quaternion - inline XsReal& z() { return m_z; } - - //! \brief Swap the contents with \a other - inline void swap(XsQuaternion& other) - { - XsQuaternion_swap(this, &other); - } - - //! \brief Returns true if \a other is numerically identical to this - 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; - } - - //! \brief Returns true if the fields of this and \a other are within \a tolerance of each otehr - inline bool compare(const XsQuaternion& other, XsReal tolerance) const - { - return !!XsQuaternion_compare(this, &other, tolerance); - } - - //! \brief Returns the dot product of this with \a other - inline XsReal dotProduct(const XsQuaternion& other) const - { - return XsQuaternion_dotProduct(this, &other); - } -#endif -}; - -#ifdef __cplusplus -//! \brief Return the negated version of the Quaternion \a q (w,-x,-y,-z) -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) -{ - XsQuaternion tmp(lhs); - tmp *= rhs; - return tmp; -} -#endif - -#endif diff --git a/extern/include/xsens/xsrange.h b/extern/include/xsens/xsrange.h deleted file mode 100644 index 7e62629..0000000 --- a/extern/include/xsens/xsrange.h +++ /dev/null @@ -1,135 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSRANGE_H -#define XSRANGE_H - -#include "xstypesconfig.h" - -#ifdef __cplusplus -extern "C" { -#else -#define XSRANGE_INITIALIZER { 0, -1 } -#endif - -struct XsRange; - -XSTYPES_DLL_API int XsRange_count(const struct XsRange* thisPtr); -XSTYPES_DLL_API int XsRange_contains(const struct XsRange* thisPtr, int i); -XSTYPES_DLL_API int XsRange_interval(const struct XsRange* thisPtr); -XSTYPES_DLL_API void XsRange_setRange(struct XsRange* thisPtr, int f, int l); -XSTYPES_DLL_API int XsRange_empty(const struct XsRange* thisPtr); - -#ifdef __cplusplus -} // extern "C" -#endif - -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) - : m_first(f) - , m_last(l) - {} - - //! \brief Constructs a range based upon \a other - inline XsRange(const XsRange& other) - : m_first(other.m_first) - , m_last(other.m_last) - {} - - //! \brief Assigns the range \a other to this - inline XsRange& operator = (const XsRange& other) - { - //lint --e{1529} trivial assignment - m_first = other.m_first; - m_last = other.m_last; - return *this; - } - - //! \brief \copybrief XsRange_count - inline int count() const - { - return XsRange_count(this); - } - - //! \brief \copybrief XsRange_interval - inline int interval() const - { - return XsRange_interval(this); - } - - //! \brief \copybrief XsRange_contains - inline bool contains(int i) const - { - return 0 != XsRange_contains(this, i); - } - - //! \brief Set the range to \a f - \a l - inline void setRange(int f, int l) - { - m_first = f; - m_last = l; - } - - //! \brief \copybrief XsRange_empty - inline bool empty() const - { - return 0 != XsRange_empty(this); - } - - //! \brief Return the \e first value of the range - inline int first() const - { - return m_first; - } - - //! \brief Return the \e last value of the range - inline int last() const - { - return m_last; - } - - //! \brief Return true if this is equal to other - bool operator == (const XsRange& other) const - { - return m_first == other.m_first && m_last == other.m_last; - } - - //! \brief Return true if start of this is less than start of other - bool operator < (const XsRange& other) const - { - return m_first < other.m_first; - } - -private: -#endif - - int m_first; //!< Storage for the lower end of the range - int m_last; //!< Storage for the upper end of the range -}; - -typedef struct XsRange XsRange; - -#if defined(__cplusplus) && !defined(XSENS_NO_STL) -#include - -template -std::basic_ostream<_CharT, _Traits>& operator<<(std::basic_ostream<_CharT, _Traits>& o, XsRange const& xs) -{ - return (o << "(" << xs.first() << ", " << xs.last() << "]"); -} -#endif - -#endif diff --git a/extern/include/xsens/xsrawgnsspvtdata.h b/extern/include/xsens/xsrawgnsspvtdata.h deleted file mode 100644 index 82c25ed..0000000 --- a/extern/include/xsens/xsrawgnsspvtdata.h +++ /dev/null @@ -1,112 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSRAWGNSSPVTDATA_H -#define XSRAWGNSSPVTDATA_H - -#include "pstdint.h" - -#ifdef _MSC_VER -#pragma pack(push, 1) -#elif defined (__ICCARM__) -_Pragma("pack(push, 1)") -#endif - -#ifndef __cplusplus -#define XSRAWGNSSPVTDATA_INITIALIZER { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 } -#else -#include -#endif - -/*! \brief A container for GNSS Position Velocity and Time data -*/ -struct XsRawGnssPvtData -{ - uint32_t m_itow; //!< GPS time of week (ms) - uint16_t m_year; //!< Year (UTC) - uint8_t m_month; //!< Month (UTC) - uint8_t m_day; //!< Day of Month (UTC) - uint8_t m_hour; //!< Hour of day 0..23 (UTC) - uint8_t m_min; //!< Minute of hour 0..59 (UTC) - uint8_t m_sec; //!< Seconds of minute 0..60 (UTC) - uint8_t m_valid; /*!< Validity Flags - bit(0) = Set if UTC Date is valid - bit(1) = Set if UTC Time of Day if valid - bit(2) = Set if UTC Time of Day has been fully resolved (no seconds uncertainty) - */ - uint32_t m_tAcc; //!< Time accuracy estimate (ns) (UTC) - int32_t m_nano; //!< Fraction of second (ns) -1e9..1e9 (UTC) - uint8_t m_fixType; /*!< GNSSfix Type, range 0..5 - 0x00 = No Fix - 0x01 = Dead Reckoning only - 0x02 = 2D-Fix - 0x03 = 3D-Fix - 0x04 = GNSS + dead reckoning combined - 0x05 = Time only fix - 0x06..0xff: reserved - */ - 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(5) = Set if heading of vehicle is valid - */ - - uint8_t m_numSv; //!< Number of satellites used in Nav Solution - uint8_t m_res1; //!< Reserved for future use (1) - int32_t m_lon; //!< Longitude (deg) (scaling 1e-7) - int32_t m_lat; //!< Latitude (deg) (scaling 1e-7) - int32_t m_height; //!< Height above ellipsoid (mm) - int32_t m_hMsl; //!< Height above mean sea level (mm) - - uint32_t m_hAcc; //!< Horizontal accuracy estimate (mm) - uint32_t m_vAcc; //!< Vertical accuracy estimate (mm) - - int32_t m_velN; //!< NED north velocity (mm/s) - int32_t m_velE; //!< NED east velocity (mm/s) - int32_t m_velD; //!< NED down velocity (mm/s) - int32_t m_gSpeed; //!< 2D Ground Speed (mm/s) - int32_t m_headMot; //!< 2D Heading of motion (deg) (scaling 1e-5) - - uint32_t m_sAcc; //!< Speed accuracy estimate (mm/s) - uint32_t m_headAcc; //!< Heading accuracy estimate (both motion and vehicle) (deg) (scaling 1-e5) - int32_t m_headVeh; //!< 2D Heading of vehicle (deg) (scaling 1e-5) - - uint16_t m_gdop; //!< Geometric DOP (scaling 0.01) - uint16_t m_pdop; //!< Position DOP (scaling 0.01) - uint16_t m_tdop; //!< Time DOP (scaling 0.01) - uint16_t m_vdop; //!< Vertical DOP (scaling 0.01) - uint16_t m_hdop; //!< Horizontal DOP (scaling 0.01) - uint16_t m_ndop; //!< Northing DOP (scaling 0.01) - uint16_t m_edop; //!< Easting DOP (scaling 0.01) - -#ifdef __cplusplus - /*! \brief Returns true if all fields of this and \a other are exactly identical */ - inline bool operator == (const XsRawGnssPvtData& other) const - { - // direct memory comparison is allowed because all fields line up properly - return std::memcmp(this, &other, sizeof(XsRawGnssPvtData)) == 0; - } -#endif - -}; -typedef struct XsRawGnssPvtData XsRawGnssPvtData; - -#ifdef _MSC_VER -#pragma pack(pop) -#elif defined (__ICCARM__) -_Pragma("pack(pop)") -#endif - -#endif diff --git a/extern/include/xsens/xsrawgnsssatinfo.h b/extern/include/xsens/xsrawgnsssatinfo.h deleted file mode 100644 index 9297e2c..0000000 --- a/extern/include/xsens/xsrawgnsssatinfo.h +++ /dev/null @@ -1,126 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSRAWGNSSSATINFO_H -#define XSRAWGNSSSATINFO_H - -#include "pstdint.h" - -#ifdef _MSC_VER -#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 } -#endif - -/*! \addtogroup enums Global enumerations - @{ -*/ -/*! \brief Xsens Satellite Info Flags -*/ -enum XsSatInfoFlags -{ - XSIF_SignalQualityIndicator_Mask = 0x7, - XSIF_SignalQualityIndicator_NoSignal = 0x0, - XSIF_SignalQualityIndicator_Searching = 0x1, - XSIF_SignalQualityIndicator_Acquired = 0x2, - XSIF_SignalQualityIndicator_Unusable = 0x3, - XSIF_SignalQualityIndicator_CodeTimeOk = 0x4, - XSIF_SignalQualityIndicator_CodeCarrierTimeOk1 = 0x5, - XSIF_SignalQualityIndicator_CodeCarrierTimeOk2 = 0x6, - XSIF_SignalQualityIndicator_CodeCarrierTimeOk3 = 0x7, - XSIF_UsedForNavigation_Mask = 0x8, - XSIF_UsedForNavigation_Used = 0x8, - XSIF_HealthFlag_Mask = 0x30, - XSIF_HealthFlag_Unknown = 0x00, - XSIF_HealthFlag_Healthy = 0x10, - XSIF_HealthFlag_Unhealthy = 0x20, - XSIF_Differential_Mask = 0x40, - XSIF_Differential_Available = 0x40 -}; -/*! @} */ -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; /*!< - bits[0..2] : Signal quality indicator - 0 = No signal - 1 = Searching signal - 2 = Signal acquired - 3 = Signal detected but unusable - 4 = Code locked and time synchronized - 5, 6, 7 = Code and carrier locked and time synchronized - bits[3] : Is set to 1 when the SV is being used for navigation - bits[4..5] : SV health flag - 0 = unknown - 1 = healthy - 2 = unhealthy - bits[6] : Is set to 1 when differential correction data is available - bits[7] : reserved - */ -}; -typedef struct XsSatInfo XsSatInfo; - -/*! \brief A container for GNSS Satellite Information -*/ -struct XsRawGnssSatInfo -{ - uint32_t m_itow; //!< GPS time of week (ms) - uint8_t m_numSvs; //!< Number of satellites - uint8_t m_res1; //!< Reserved for future use (1) - uint8_t m_res2; //!< Reserved for future use (2) - uint8_t m_res3; //!< Reserved for future use (3) - - XsSatInfo m_satInfos[60]; //!< The information of all satellites, maximum 60 - -#ifdef __cplusplus - /*! \brief Returns true if all fields of this and \a other are exactly identical */ - inline bool operator == (const XsRawGnssSatInfo& b) const - { - if (m_itow != b.m_itow || - m_numSvs != b.m_numSvs || - m_res1 != b.m_res1 || - m_res2 != b.m_res2 || - m_res3 != b.m_res3) - return false; - for (uint8_t i = 0; i < m_numSvs; ++i) - if (m_satInfos[i].m_gnssId != b.m_satInfos[i].m_gnssId || - m_satInfos[i].m_svId != b.m_satInfos[i].m_svId || - m_satInfos[i].m_cno != b.m_satInfos[i].m_cno || - m_satInfos[i].m_flags != b.m_satInfos[i].m_flags) - return false; - return true; - } -#endif -}; -typedef struct XsRawGnssSatInfo XsRawGnssSatInfo; - -#ifdef _MSC_VER -#pragma pack(pop) -#endif - -#endif diff --git a/extern/include/xsens/xsrawgpsdop.h b/extern/include/xsens/xsrawgpsdop.h deleted file mode 100644 index 761ea6c..0000000 --- a/extern/include/xsens/xsrawgpsdop.h +++ /dev/null @@ -1,54 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSRAWGPSDOP_H -#define XSRAWGPSDOP_H - -#include "pstdint.h" - -#pragma pack(push, 1) - -#ifndef __cplusplus -#define XSRAWGPSDOP_INITIALIZER { 0, 0, 0, 0, 0, 0, 0, 0 } -#endif - -/*! \brief A container for NAV-DOP data - \details DOP values are dimensionless. - All dop values are scaled by a factor of 100. that is, if the unit transmits a value of e.g. 156, - it means that the DOP value is 1.56. - \deprecated -*/ -struct XsRawGpsDop -{ - uint32_t m_itow; //!< Gps time of week (ms) - uint16_t m_gdop; //!< Geometric DOP - uint16_t m_pdop; //!< Position DOP - uint16_t m_tdop; //!< Time DOP - uint16_t m_vdop; //!< Vertical DOP - uint16_t m_hdop; //!< Horizontal DOP - uint16_t m_ndop; //!< Northing DOP - uint16_t m_edop; //!< Easting DOP -}; -typedef struct XsRawGpsDop XsRawGpsDop; - -#pragma pack(pop) - -#ifdef __cplusplus -inline bool operator == (XsRawGpsDop const& a, XsRawGpsDop const& b) -{ - return memcmp(&a, &b, sizeof(XsRawGpsDop)) == 0; -} -#endif - -#endif diff --git a/extern/include/xsens/xsrawgpssol.h b/extern/include/xsens/xsrawgpssol.h deleted file mode 100644 index 711622a..0000000 --- a/extern/include/xsens/xsrawgpssol.h +++ /dev/null @@ -1,77 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSRAWGPSSOL_H -#define XSRAWGPSSOL_H - -#include "pstdint.h" - -#ifdef _MSC_VER -#pragma pack(push, 1) -#endif - -#ifndef __cplusplus -#define XSRAWGPSSOL_INITIALIZER { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 } -#endif - -/*! \brief A container for NAV-SOL data - \deprecated -*/ -struct XsRawGpsSol -{ - uint32_t m_itow; //!< Gps time of week (ms) - int32_t m_frac; //!< Nanoseconds remainder of rounded ms above, range -500000 .. 500000 - int16_t m_week; //!< GPS week (GPS time) - uint8_t m_gpsfix; /*!< GPSfix Type, range 0..4 - 0x00 = No Fix - 0x01 = Dead Reckoning only - 0x02 = 2D-Fix - 0x03 = 3D-Fix - 0x04 = GPS + dead reckoning combined - 0x05..0xff: reserved - */ - uint8_t m_flags; /*!< 0x01=GPSfixOK (i.e. within DOP & ACC Masks) - 0x02=DiffSoln (is DGPS used) - 0x04=WKNSET (is Week Number valid) - 0x08=TOWSET (is Time of Week valid) - 0x?0=reserved - */ - int32_t m_ecef_x; //!< ECEF X position (cm) - int32_t m_ecef_y; //!< ECEF Y position (cm) - int32_t m_ecef_z; //!< ECEF Z position (cm) - uint32_t m_pacc; //!< Position Accuracy Estimate (cm) - int32_t m_ecef_vx; //!< ECEF X velocity (cm/s) - int32_t m_ecef_vy; //!< ECEF Y velocity (cm/s) - int32_t m_ecef_vz; //!< ECEF Z velocity (cm/s) - uint32_t m_sacc; //!< Speed Accuracy Estimate (cm/s) - uint16_t m_pdop; //!< Position DOP - uint8_t m_res1; //!< Reserved - uint8_t m_numsv; //!< Number of SVs used in Nav Solution - uint32_t m_res2; //!< Reserved - -}; -typedef struct XsRawGpsSol XsRawGpsSol; - -#ifdef _MSC_VER -#pragma pack(pop) -#endif - -#ifdef __cplusplus -inline bool operator == (XsRawGpsSol const& a, XsRawGpsSol const& b) -{ - return memcmp(&a, &b, sizeof(XsRawGpsSol)) == 0; -} -#endif - -#endif diff --git a/extern/include/xsens/xsrawgpssvinfo.h b/extern/include/xsens/xsrawgpssvinfo.h deleted file mode 100644 index cff2a82..0000000 --- a/extern/include/xsens/xsrawgpssvinfo.h +++ /dev/null @@ -1,87 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSRAWGPSSVINFO_H -#define XSRAWGPSSVINFO_H - -#include "pstdint.h" - -#ifdef _MSC_VER -#pragma pack(push, 1) -#endif - -#ifndef __cplusplus -#define XSSVINFO_INITIALIZER { 0, 0, 0, 0, 0, 0, 0, 0 } -#define XSRAWGPSSVINFO_INITIALIZER { 0, XSSVINFO_INITIALIZER, XSSVINFO_INITIALIZER, XSSVINFO_INITIALIZER, XSSVINFO_INITIALIZER, XSSVINFO_INITIALIZER, XSSVINFO_INITIALIZER, XSSVINFO_INITIALIZER, XSSVINFO_INITIALIZER, XSSVINFO_INITIALIZER, XSSVINFO_INITIALIZER, XSSVINFO_INITIALIZER, XSSVINFO_INITIALIZER, XSSVINFO_INITIALIZER, XSSVINFO_INITIALIZER, XSSVINFO_INITIALIZER, XSSVINFO_INITIALIZER }; -#endif - -/*! \brief A container for information of one satellite - \deprecated -*/ -struct XsSvInfo -{ - uint8_t m_chn; //!< channel number, range 0..NCH-1 - uint8_t m_svid; //!< Satellite ID - uint8_t m_flags; /*!< Bitmask, made up of the following bit values - 0x01 = SV is used for navigation - 0x02 = Differential correction data is available for this SV - 0x04 = Orbit information is available for this SV (Ephemeris or Almanach) - 0x08 = Orbit information is Ephemeris - 0x10 = SV is unhealthy / shall not be used - 0x20 = reserved - 0x40 = reserved - 0x80 = reserved - */ - int8_t m_qi; /*!< Signal Quality indicator (range 0..7). The following list shows the meaning of the different QI values: - 0: This channel is idle - 1, 2: Channel is searching - 3: Signal detected but unusable - 4: Code Lock on Signal - 5, 6: Code and Carrier locked - 7: Code and Carrier locked, receiving 50bps data - */ - uint8_t m_cno; //!< Carrier to Noise Ratio (Signal Strength) (dbHz) - int8_t m_elev; //!< Elevation in integer (deg) - int16_t m_azim; //!< Azimuth in integer (deg) - int32_t m_prres; //!< Pseudo range residual (cm) -}; -typedef struct XsSvInfo XsSvInfo; - -/*! \brief A container for NAV-SVINFO data -*/ -struct XsRawGpsSvInfo -{ - uint32_t m_itow; //!< Gps time of week (ms) - uint8_t m_nch; //!< Number of channels range 0..16 - uint8_t m_res1; //!< Reserved - uint16_t m_res2; //!< Reserved - XsSvInfo m_svInfos[16]; //!< The information of all satellites, maxmimum 16 -}; -typedef struct XsRawGpsSvInfo XsRawGpsSvInfo; - -#ifdef _MSC_VER -#pragma pack(pop) -#endif - -#ifdef __cplusplus -inline bool operator == (XsRawGpsSvInfo const& a, XsRawGpsSvInfo const& b) -{ - if (a.m_nch != b.m_nch) - return false; - - return memcmp(&a, &b, a.m_nch * sizeof(XsSvInfo) + offsetof(XsRawGpsSvInfo, m_svInfos)) == 0; -} -#endif - -#endif diff --git a/extern/include/xsens/xsrawgpstimeutc.h b/extern/include/xsens/xsrawgpstimeutc.h deleted file mode 100644 index 6a7856f..0000000 --- a/extern/include/xsens/xsrawgpstimeutc.h +++ /dev/null @@ -1,61 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSRAWGPSTIMEUTC_H -#define XSRAWGPSTIMEUTC_H - -#include "pstdint.h" -#include - -#ifdef _MSC_VER -#pragma pack(push, 1) -#endif - -#ifndef __cplusplus -#define XSRAWGPSTIMEUTC_INITIALIZER { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 } -#endif - -/*! \brief A container for NAV-TIMEUTC data - \deprecated -*/ -struct XsRawGpsTimeUtc -{ - uint32_t m_itow; //!< Gps time of week (ms) - uint32_t m_tacc; //!< Time Accuracy Estimate (ns) - int32_t m_nano; //!< Nanoseconds of second, range -500000000 .. 500000000 (UTC) - uint16_t m_year; //!< Year, range 1999..2099 (UTC) - uint8_t m_month; //!< Month, range 1..12 (UTC) - uint8_t m_day; //!< Day of Month, range 1..31 (UTC) - uint8_t m_hour; //!< Hour of Day, range 0..23 (UTC) - uint8_t m_min; //!< Minute of Hour, range 0..59 (UTC) - uint8_t m_sec; //!< Minute of Hour, range 0..59 (UTC) - uint8_t m_valid; /*!< Valid 0x01 = Valid Time of Week - 0x02 = Valid Week Number - 0x04 = Valid UTC (Leap Seconds already known?) - */ -}; -typedef struct XsRawGpsTimeUtc XsRawGpsTimeUtc; - -#ifdef _MSC_VER -#pragma pack(pop) -#endif - -#ifdef __cplusplus -inline bool operator == (XsRawGpsTimeUtc const& a, XsRawGpsTimeUtc const& b) -{ - return memcmp(&a, &b, sizeof(XsRawGpsTimeUtc)) == 0; -} -#endif - -#endif diff --git a/extern/include/xsens/xsresultvalue.h b/extern/include/xsens/xsresultvalue.h deleted file mode 100644 index 47032d8..0000000 --- a/extern/include/xsens/xsresultvalue.h +++ /dev/null @@ -1,175 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSRESULTVALUE_H -#define XSRESULTVALUE_H - -#include "xstypesconfig.h" - -#ifdef __cplusplus -extern "C" { -#endif - -////////////////////////////////////////////////////////////////////////////////////////// -/*! \addtogroup enums Global enumerations - @{ -*/ -/*! \brief Xsens result values - \details These values are used to signal success or specific failures of functions - \sa XsResultValue_toString -*/ -enum XsResultValue { - // general OK - XRV_OK = 0 //!< 0: Operation was performed successfully - - // communication protocol - ,XRV_NOBUS = 1 //!< 1: No bus communication possible - ,XRV_BUSNOTREADY = 2 //!< 2: InitBus and/or SetBID are not issued - ,XRV_INVALIDPERIOD = 3 //!< 3: Period sent is invalid - ,XRV_INVALIDMSG = 4 //!< 4: The message is invalid or not implemented - ,XRV_INITBUSFAIL1 = 16 //!< 16: A slave did not respond to WaitForSetBID - ,XRV_INITBUSFAIL2 = 17 //!< 17: An incorrect answer received after WaitForSetBID - ,XRV_INITBUSFAIL3 = 18 //!< 18: After four bus-scans still undetected Motion Trackers - ,XRV_SETBIDFAIL1 = 20 //!< 20: No reply to SetBID message during SetBID procedure - ,XRV_SETBIDFAIL2 = 21 //!< 21: Other than SetBIDAck received - ,XRV_MEASUREMENTFAIL1 = 24 //!< 24: Timer overflow - period too short to collect all data from Motion Trackers - ,XRV_MEASUREMENTFAIL2 = 25 //!< 25: Motion Tracker responds with other than SlaveData message - ,XRV_MEASUREMENTFAIL3 = 26 //!< 26: Total bytes of data of Motion Trackers including sample counter exceeds 255 bytes - ,XRV_MEASUREMENTFAIL4 = 27 //!< 27: Timer overflows during measurement - ,XRV_MEASUREMENTFAIL5 = 28 //!< 28: Timer overflows during measurement - ,XRV_MEASUREMENTFAIL6 = 29 //!< 29: No correct response from Motion Tracker during measurement - ,XRV_TIMEROVERFLOW = 30 //!< 30: Timer overflows during measurement - ,XRV_BAUDRATEINVALID = 32 //!< 32: Baud rate does not comply with valid range - ,XRV_INVALIDPARAM = 33 //!< 33: An invalid parameter is supplied - ,XRV_MEASUREMENTFAIL7 = 35 //!< 35: TX PC Buffer is full - ,XRV_MEASUREMENTFAIL8 = 36 //!< 36: TX PC Buffer overflow, cannot fit full message - ,XRV_WIRELESSFAIL = 37 //!< 37: Wireless subsystem failed - ,XRV_DEVICEERROR = 40 //!< 40: The device generated an error, try updating the firmware - ,XRV_DATAOVERFLOW = 41 //!< 41: The device generates more data than the bus communication can handle (baud rate may be too low) - ,XRV_BUFFEROVERFLOW = 42 //!< 42: The sample buffer of the device was full during a communication outage - ,XRV_EXTTRIGGERERROR = 43 //!< 43: The external trigger is not behaving as configured - ,XRV_SAMPLESTREAMERROR = 44 //!< 44: The sample stream detected an error in the ordering of sample data - ,XRV_POWER_DIP = 45 //!< 45: A dip in the power supply was detected and recovered from - ,XRV_POWER_OVERCURRENT = 46 //!< 46: A current limiter has been activated, shutting down the device - ,XRV_OVERHEATING = 47 //!< 47: Device temperature is not within operational limits - ,XRV_BATTERYLOW = 48 //!< 48: Battery level reached lower limit - ,XRV_INVALIDFILTERPROFILE = 49 //!< 49: Specified filter profile ID is not available on the device or the user is trying to duplicate an existing filter profile type - ,XRV_INVALIDSTOREDSETTINGS = 50 //!< 50: The settings stored in the device's non volatile memory are invalid - ,XRV_ACCESSDENIED = 51 //!< 51: Request for control of the device was denied - - // CMT / XDA / XME / etc - ,XRV_ERROR = 256 //!< 256: A generic error occurred - ,XRV_NOTIMPLEMENTED = 257 //!< 257: Operation not implemented in this version (yet) - ,XRV_TIMEOUT = 258 //!< 258: A timeout occurred - ,XRV_TIMEOUTNODATA = 259 //!< 259: Operation aborted because of no data read - ,XRV_CHECKSUMFAULT = 260 //!< 260: Checksum fault occurred - ,XRV_OUTOFMEMORY = 261 //!< 261: No internal memory available - ,XRV_NOTFOUND = 262 //!< 262: The requested item was not found - ,XRV_UNEXPECTEDMSG = 263 //!< 263: Unexpected message received (e.g. no acknowledge message received) - ,XRV_INVALIDID = 264 //!< 264: Invalid id supplied - ,XRV_INVALIDOPERATION = 265 //!< 265: Operation is invalid at this point - ,XRV_INSUFFICIENTSPACE = 266 //!< 266: Insufficient buffer space available - ,XRV_INPUTCANNOTBEOPENED = 267 //!< 267: The specified i/o device can not be opened - ,XRV_OUTPUTCANNOTBEOPENED = 268 //!< 268: The specified i/o device can not be opened - ,XRV_ALREADYOPEN = 269 //!< 269: An I/O device is already opened with this object - ,XRV_ENDOFFILE = 270 //!< 270: End of file is reached - ,XRV_COULDNOTREADSETTINGS = 271 //!< 271: A required settings file could not be opened or is missing some data - ,XRV_NODATA = 272 //!< 272: No data is available - ,XRV_READONLY = 273 //!< 273: Tried to change a read-only value - ,XRV_NULLPTR = 274 //!< 274: Tried to supply a NULL value where it is not allowed - ,XRV_INSUFFICIENTDATA = 275 //!< 275: Insufficient data was supplied to a function - ,XRV_BUSY = 276 //!< 276: Busy processing, try again later - ,XRV_INVALIDINSTANCE = 277 //!< 277: Invalid instance called, because of an invalid or missing license - ,XRV_DATACORRUPT = 278 //!< 278: A trusted data stream proves to contain corrupted data - - ,XRV_READINITFAILED = 279 //!< 279: Failure during read of settings - ,XRV_NOXMFOUND = 280 //!< 280: Could not find any MVN-compatible hardware - ,XRV_ONLYONEXMFOUND = 281 //!< 281: Found only one responding Xbus Master - ,XRV_DEVICECOUNTZERO = 282 //!< 282: No xsens devices found - ,XRV_MTLOCATIONINVALID = 283 //!< 283: One or more sensors are not where they were expected - ,XRV_INSUFFICIENTMTS = 284 //!< 284: Not enough sensors were found - ,XRV_INITFUSIONFAILED = 285 //!< 285: Failure during initialization of Fusion Engine - ,XRV_OTHER = 286 //!< 286: Something else was received than was requested - - ,XRV_NOFILEOPEN = 287 //!< 287: No file opened for reading/writing - ,XRV_NOPORTOPEN = 288 //!< 288: No serial port opened for reading/writing - ,XRV_NOFILEORPORTOPEN = 289 //!< 289: No file or serial port opened for reading/writing - ,XRV_PORTNOTFOUND = 290 //!< 290: A required port could not be found - ,XRV_INITPORTFAILED = 291 //!< 291: The low-level port handler failed to initialize - ,XRV_CALIBRATIONFAILED = 292 //!< 292: A calibration routine failed - - ,XRV_CONFIGCHECKFAIL = 293 //!< 293: The in-config check of the device failed - ,XRV_ALREADYDONE = 294 //!< 294: The operation is once only and has already been performed - - ,XRV_SYNC_SINGLE_SLAVE = 295 //!< 295: The single connected device is configured as a slave - ,XRV_SYNC_SECOND_MASTER = 296 //!< 296: More than one master was detected - ,XRV_SYNC_NO_SYNC = 297 //!< 297: A device was detected that was neither master nor slave - ,XRV_SYNC_NO_MASTER = 298 //!< 298: No master detected - ,XRV_SYNC_DATA_MISSING = 299 //!< 299: A device is not sending enough data - - ,XRV_VERSION_TOO_LOW = 300 //!< 300: The version of the object is too low for the requested operation - ,XRV_VERSION_PROBLEM = 301 //!< 301: The object has an unrecognised version, so it's not safe to perform the operation - - ,XRV_ABORTED = 302 //!< 302: The process was aborted by an external event, usually a user action or process termination - ,XRV_UNSUPPORTED = 303 //!< 303: The requested functionality is not supported by the device - - ,XRV_PACKETCOUNTERMISSED = 304 //!< 304: A packet counter value was missed - - ,XRV_MEASUREMENTFAILED = 305 //!< 305: An error occurred while trying to put the device in measurement mode - ,XRV_STARTRECORDINGFAILED = 306 //!< 306: A device could not start recording - - ,XRV_RADIO_CHANNEL_IN_USE = 311 //!< 311: Radio channel is in use by another system - ,XRV_UNEXPECTED_DISCONNECT = 312 //!< 312: Motion tracker disconnected unexpectedly - ,XRV_TOO_MANY_CONNECTED_TRACKERS = 313 //!< 313: Too many motion trackers connected - ,XRV_GOTOCONFIGFAILED = 314 //!< 314: A device could not be put in config mode - ,XRV_OUTOFRANGE = 315 //!< 315: Device has gone out of range - ,XRV_BACKINRANGE = 316 //!< 316: Device is back in range, resuming normal operation - ,XRV_EXPECTED_DISCONNECT = 317 //!< 317: The device was disconnected - - // notifications - ,XRV_SHUTTINGDOWN = 400 //!< 400: The device is shutting down -}; -/*! @} */ -typedef enum XsResultValue XsResultValue; - -//! These enum values can be used to specify a device error (XRV_DEVICEERROR 0x28), i.e. a xbus message like [FA FF 42 05 28 XXXXXXXX CS] -enum XsDeviceErrorType -{ - XERR_Unknown = 0 - , XERR_ImcuTimeout = 1 - , XERR_ImcuSettingsInvalid = 2 - , XERR_ImcuSettingsFailure = 3 - , XERR_ImcuEmtsWriteFailure = 4 - , XERR_ImcuEmtsReadFailure = 5 - , XERR_DspCrashed = 6 - , XERR_DspBootingTimeout = 7 - , XERR_DspSettingsInvalid = 8 -}; -typedef enum XsDeviceErrorType XsDeviceErrorType; - -////////////////////////////////////////////////////////////////////////////////////////// - -/* \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 - "!!Undefined Result Value!!" string is returned. -*/ -XSTYPES_DLL_API const char* XsResultValue_toString(XsResultValue result); - -#ifdef __cplusplus -} // extern "C" -#endif - -#endif diff --git a/extern/include/xsens/xsrssi.h b/extern/include/xsens/xsrssi.h deleted file mode 100644 index f54d1e8..0000000 --- a/extern/include/xsens/xsrssi.h +++ /dev/null @@ -1,55 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSRSSI_H -#define XSRSSI_H - -#include "xstypesconfig.h" - -#define XS_RSSI_MAX (128) -#define XS_RSSI_UNKNOWN (-XS_RSSI_MAX) - -#ifdef __cplusplus -extern "C" { -#endif - -XSTYPES_DLL_API int XsRssi_max(void); -XSTYPES_DLL_API int XsRssi_unknown(void); -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; - - /*! \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; - - /*! \copydoc XsRssi_unbiased */ - inline int unbiased(int raw) - { - return XsRssi_unbiased(raw); - } -} -#endif - -#endif diff --git a/extern/include/xsens/xsscrdata.h b/extern/include/xsens/xsscrdata.h deleted file mode 100644 index 1a9ca07..0000000 --- a/extern/include/xsens/xsscrdata.h +++ /dev/null @@ -1,37 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSSCRDATA_H -#define XSSCRDATA_H - -#include "pstdint.h" -#include "xsushortvector.h" - -#ifndef __cplusplus -#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 { - XsUShortVector m_acc; //!< The raw accelerometer data - XsUShortVector m_gyr; //!< The raw gyroscope data - XsUShortVector m_mag; //!< The raw magnetometer data - uint16_t m_temp; //!< The temperature data -}; -typedef struct XsScrData XsScrData; - -#endif diff --git a/extern/include/xsens/xssdidata.h b/extern/include/xsens/xssdidata.h deleted file mode 100644 index 1d2914b..0000000 --- a/extern/include/xsens/xssdidata.h +++ /dev/null @@ -1,117 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSSDIDATA_H -#define XSSDIDATA_H - -#include "xstypesconfig.h" -#include "xsvector3.h" -#include "xsquaternion.h" - -struct XsSdiData; - -#ifdef __cplusplus -extern "C" { -#else -#define XSSDIDATA_INITIALIZER {XSQUATERNION_INITIALIZER, XSVECTOR3_INITIALIZER} -#endif - -XSTYPES_DLL_API void XsSdiData_construct(struct XsSdiData* thisPtr, const XsReal* orientationIncrement, const XsReal* velocityIncrement); -XSTYPES_DLL_API void XsSdiData_destruct(struct XsSdiData* thisPtr); - -#ifdef __cplusplus -} // extern "C" -#endif - -struct XsSdiData -{ -#ifdef __cplusplus - //! \brief Construct an empty object - inline XsSdiData() - { - } - - //! \brief Construct an initialized object - inline XsSdiData(const XsQuaternion& dq, const XsVector& dv) - : m_orientationIncrement(dq) - , m_velocityIncrement(dv) - { - } - - //! \brief Copy constructor - inline XsSdiData(const XsSdiData& other) - : m_orientationIncrement(other.m_orientationIncrement) - , m_velocityIncrement(other.m_velocityIncrement) - { - } - - //! \brief Assignment operator - inline const XsSdiData& operator=(const XsSdiData& other) - { - if (this != &other) - { - m_orientationIncrement = other.m_orientationIncrement; - m_velocityIncrement = other.m_velocityIncrement; - } - return *this; - } - - //! \brief Clear the object so it contains unity data - inline void zero() - { - m_orientationIncrement = XsQuaternion::identity(); - m_velocityIncrement.zero(); - } - - //! \brief Returns the contained orientation increment - inline const XsQuaternion& orientationIncrement() const - { - return m_orientationIncrement; - } - - //! \brief Update the contained orientation increment - inline void setOrientationIncrement(const XsQuaternion& dq) - { - m_orientationIncrement = dq; - } - - //! \brief Returns the contained velocity increment - inline const XsVector3& velocityIncrement() const - { - return m_velocityIncrement; - } - - //! \brief Update the contained velocity increment - inline void setVelocityIncrement(const XsVector& dv) - { - m_velocityIncrement = dv; - } - - /*! \brief Returns true if all fields of this and \a other are exactly identical */ - inline bool operator == (const XsSdiData& other) const - { - return m_orientationIncrement == other.m_orientationIncrement && - m_velocityIncrement == other.m_velocityIncrement; - } - -private: -#endif - - XsQuaternion m_orientationIncrement; //!< The orientation increment - XsVector3 m_velocityIncrement; //!< The velocity increment -}; - -typedef struct XsSdiData XsSdiData; - -#endif // file guard diff --git a/extern/include/xsens/xssimpleversion.h b/extern/include/xsens/xssimpleversion.h deleted file mode 100644 index b9e3a8e..0000000 --- a/extern/include/xsens/xssimpleversion.h +++ /dev/null @@ -1,137 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSSIMPLEVERSION_H -#define XSSIMPLEVERSION_H - -#include "xstypesconfig.h" - -typedef struct XsSimpleVersion XsSimpleVersion; - -#ifdef __cplusplus -extern "C" { -#else -#define XSSIMPLEVERSION_INITIALIZER { 0, 0, 0 } -#endif - -XSTYPES_DLL_API int XsSimpleVersion_empty(const XsSimpleVersion* thisPtr); -XSTYPES_DLL_API void XsSimpleVersion_swap(XsSimpleVersion* a, XsSimpleVersion* b); -XSTYPES_DLL_API int XsSimpleVersion_compare(XsSimpleVersion const* a, XsSimpleVersion const* b); - -#ifdef __cplusplus -} // extern "C" -#endif - -struct XsSimpleVersion { -#ifdef __cplusplus - //! \brief Constructs a simple-version object using the supplied parameters or an empty version object if no parameters are given. - explicit XsSimpleVersion(int vmaj = 0, int vmin = 0, int vrev = 0) - : m_major((uint8_t) vmaj) - , m_minor((uint8_t) vmin) - , m_revision((uint8_t) vrev) - {} - - //! \brief Constructs a simple-version object based upon the \a other object - XsSimpleVersion(const XsSimpleVersion& other) - : m_major(other.m_major) - , m_minor(other.m_minor) - , m_revision(other.m_revision) - {} - - //! \brief Assign the simple-version from the \a other object - XsSimpleVersion& operator = (const XsSimpleVersion& other) - { - m_major = other.m_major; - m_minor = other.m_minor; - m_revision = other.m_revision; - return *this; - } - - /*! \brief Test if the \a other simple-version is equal to this. */ - inline bool operator == (const XsSimpleVersion& other) const - { - return !XsSimpleVersion_compare(this, &other); - } - - /*! \brief Test if the \a other simple-version is different to this. */ - inline bool operator != (const XsSimpleVersion& other) const - { - if (m_major != other.m_major || m_minor != other.m_minor || m_revision != other.m_revision) - return true; - - return false; - } - - /*! \brief Test if the \a other version is lower than this. The comparison involves only the version numbers (major, minor and revision). */ - inline bool operator < (const XsSimpleVersion& other) const - { - if (m_major < other.m_major) - return true; - else if (m_major > other.m_major) - return false; - 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; - } - } - } - - /*! \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); - } - - /*! \brief Test if the \a other version is higher than this. */ - inline bool operator > (const XsSimpleVersion& other) const - { - return !(*this <= other); - } - - /*! \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); - } - - //! \brief \copybrief XsSimpleVersion_empty - inline bool empty() const - { - return 0 != XsSimpleVersion_empty(this); - } - - //! \brief Return the \e major part of the version - 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; } - //! \brief Return the \e revision part of the version - inline int revision() const { return (int) m_revision; } - -private: -#endif - uint8_t m_major; //!< The major part of the version number - uint8_t m_minor; //!< The minor part of the version number - uint8_t m_revision; //!< The revision number of the version -}; - -#endif diff --git a/extern/include/xsens/xssnapshot.h b/extern/include/xsens/xssnapshot.h deleted file mode 100644 index b07ed81..0000000 --- a/extern/include/xsens/xssnapshot.h +++ /dev/null @@ -1,115 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSSNAPSHOT_H -#define XSSNAPSHOT_H - -#include "xstypesconfig.h" -#include "xsdeviceid.h" - -struct XsSnapshot; - -#ifdef __cplusplus -extern "C" { -#else -#define XSSNAPSHOT_INITIALIZER { XSDEVICEID_INITIALIZER, 0, 0, 0,0,0,0, 0,0,0, 0,0,0, 0, 0, 0, 0 } -#endif - -XSTYPES_DLL_API void XsSnapshot_construct(struct XsSnapshot* thisPtr); -XSTYPES_DLL_API void XsSnapshot_destruct(struct XsSnapshot* thisPtr); - -#ifdef __cplusplus -} // extern "C" -#endif - -enum SnapshotType -{ - ST_Awinda = 0, // - ST_Full -}; -typedef enum SnapshotType SnapshotType; -/*! \brief A container for Snapshot data -*/ -struct XsSnapshot -{ - XsDeviceId m_deviceId; /*!< \brief The ID of the device that created the data */ - uint32_t m_frameNumber; /*!< \brief The frame */ - uint64_t m_timestamp; /*!< \brief The timestamp */ - int32_t m_iQ[4]; /*!< \brief The integrated orientation */ - int64_t m_iV[3]; /*!< \brief The integrated velocity */ - int32_t m_mag[3]; /*!< \brief The magnetic field */ - int32_t m_baro; /*!< \brief The barometric pressure */ - uint16_t m_status; /*!< \brief The clipping flags of the latest interval */ - uint8_t m_accClippingCounter; /*!< \brief The clipping event counter for the Acc */ - uint8_t m_gyrClippingCounter; /*!< \brief The clipping event counter for the Gyr */ - SnapshotType m_type; /*!< \brief The type of the snapshot (Awinda, Full) */ -#ifdef __cplusplus - /*! \brief Returns true if all fields of this and \a other are exactly identical */ - inline bool operator == (const XsSnapshot& other) const - { - if (m_frameNumber != other.m_frameNumber || - m_baro != other.m_baro || - m_status != other.m_status || - m_accClippingCounter != other.m_accClippingCounter || - m_gyrClippingCounter != other.m_gyrClippingCounter|| - m_type != other.m_type) - return false; - - for (int i = 0; i < 3; ++i) - { - if (m_iQ[i] != other.m_iQ[i] || - m_iV[i] != other.m_iV[i] || - m_mag[i] != other.m_mag[i]) - return false; - } - if (m_type == ST_Full) - { - if (m_iQ[3] != other.m_iQ[3] || m_timestamp != other.m_timestamp ) - return false; - } - return true; - } -#endif -}; -typedef struct XsSnapshot XsSnapshot; - -/*! \brief Status flag definitions for XsSnapshot status field */ -enum SnapshotStatusFlag -{ - FSFL_ClipAccX = 0x0001, - FSFL_ClipAccY = 0x0002, - FSFL_ClipAccZ = 0x0004, - FSFL_ClipAccMask = 0x0007, - FSFL_ClipGyrX = 0x0008, - FSFL_ClipGyrY = 0x0010, - FSFL_ClipGyrZ = 0x0020, - FSFL_ClipGyrMask = 0x0038, - FSFL_ClipMagX = 0x0040, - FSFL_ClipMagY = 0x0080, - FSFL_ClipMagZ = 0x0100, - FSFL_ClipMagMask = 0x01C0, - FSFL_MagIsNew = 0x0200, - FSFL_BaroIsNew = 0x0400, - FSFL_RotationMask = 0x1800 -}; -typedef enum SnapshotStatusFlag SnapshotStatusFlag; - - - -#define FSFL_ClipAccShift 0 -#define FSFL_ClipGyrShift 04 -#define FSFL_ClipMagshift 6 -#define FSFL_RotationShift 11 - -#endif diff --git a/extern/include/xsens/xssocket.h b/extern/include/xsens/xssocket.h deleted file mode 100644 index 6abbb2a..0000000 --- a/extern/include/xsens/xssocket.h +++ /dev/null @@ -1,441 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSSOCKET_H -#define XSSOCKET_H - -#include "xsstring.h" -#include "xsbytearray.h" -#include "xsresultvalue.h" -#include "xstypedefs.h" - -struct XsSocket; -struct XsSocketInterface; -typedef struct XsSocket XsSocket; - -/*! \brief the protocol on top of IP */ -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 { - NLP_IPV4, /*!< \brief IPv4 address family */ - NLP_IPV6, /*!< \brief IPv6 address family */ - NLP_IPVX /*!< \brief any IP address family */ -}; - -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 -#else -typedef int XSOCKET; -#endif - -#ifdef __cplusplus -extern "C" { -#else -#define XSSOCKET_INITIALIZER { NULL } -#endif - -#define XSSOCKET_INFINITE_TIMEOUT -1 - -XSTYPES_DLL_API void XsSocket_create(XsSocket* thisp, enum NetworkLayerProtocol ip, enum IpProtocol protocol); -XSTYPES_DLL_API void XsSocket_createFromFileDescriptor(XsSocket* thisp, int sockfd, XsDataFlags flags); -XSTYPES_DLL_API void XsSocket_destroy(XsSocket* thisp); -XSTYPES_DLL_API XsResultValue XsSocket_close(XsSocket* thisp); - -XSTYPES_DLL_API XSOCKET XsSocket_nativeDescriptor(XsSocket const* thisp); - -XSTYPES_DLL_API int XsSocket_select(XsSocket* thisp, int mstimeout, int *canRead, int *canWrite); -XSTYPES_DLL_API int XsSocket_read(XsSocket* thisp, void* dest, XsSize size, int timeout); -XSTYPES_DLL_API int XsSocket_readFrom(XsSocket* thisp, void* dest, XsSize size, XsString* hostname, uint16_t* port, int timeout); - -XSTYPES_DLL_API int XsSocket_write(XsSocket* thisp, const void* buffer, XsSize size); -XSTYPES_DLL_API int XsSocket_writeTo(XsSocket* thisp, const void* buffer, XsSize size, const XsString* hostname, uint16_t port); - -XSTYPES_DLL_API void XsSocket_flush(XsSocket* thisp); - -XSTYPES_DLL_API XsSocket* XsSocket_accept(XsSocket* thisp, int mstimeout); - -XSTYPES_DLL_API XsSocket* XsSocket_allocate(); -XSTYPES_DLL_API void XsSocket_free(XsSocket* thisp); - -XSTYPES_DLL_API XsResultValue XsSocket_setSocketOption(XsSocket* thisp, enum XsSocketOption option, void* valuePtr, int valueSize); -XSTYPES_DLL_API XsResultValue XsSocket_bind(XsSocket* thisp, const XsString* hostname, uint16_t port); -XSTYPES_DLL_API XsResultValue XsSocket_listen(XsSocket* thisp, int maxPending); -XSTYPES_DLL_API XsResultValue XsSocket_connect(XsSocket* thisp, const XsString* host, uint16_t port); - -XSTYPES_DLL_API int XsSocket_isUsable(const XsSocket* thisp); -XSTYPES_DLL_API void XsSocket_getRemoteAddress(const XsSocket* thisp, XsString *address); -XSTYPES_DLL_API int XsSocket_getLastSystemError(const XsSocket* thisp); - -#ifdef __cplusplus -} -#endif - -/*! \brief A platform independent socket implementation - - The internals are hidden to remove any inconvenience that - may occur because of the order of inclusion of system headers. -*/ -struct XsSocket -{ -#ifdef __cplusplus - /*! \brief \copybrief XsSocket_create - - \param[in] protocol the protocol on top of IP to use - \param[in] ipVersion the ip address family to use - */ - inline explicit XsSocket(IpProtocol protocol, NetworkLayerProtocol ipVersion = NLP_IPVX) : - d(NULL) - { - (void)d; - XsSocket_create(this, ipVersion, protocol); - } - - /*! - * \brief \copybrief XsSocket_createFromFileDescriptor - * - * \param[in] sockfd the file descriptor of the socket to wrap - * \param[in] flags if set to XSDF_Managed the wrapped socket will be closed when object is destroyed - */ - inline explicit XsSocket(int sockfd, XsDataFlags flags = XSDF_None) : - d(NULL) - { - (void)d; - XsSocket_createFromFileDescriptor(this, sockfd, flags); - } - - /*! \brief \copybrief XsSocket_destroy - */ - inline ~XsSocket() - { - XsSocket_destroy(this); - } - - /*! \brief \copybrief XsSocket_close - */ - inline XsResultValue close() - { - return XsSocket_close(this); - } - - /*! \brief Return the native file descriptor */ - inline XSOCKET nativeDescriptor() const - { - return XsSocket_nativeDescriptor(this); - } - - /*! \brief read data from the socket into \a buffer - - \param[in,out] buffer the buffer to fill, contents are automatically adjusted to the data size available - \param[in] timeout the maximum time to wait before data is in in milliseconds - - \returns the number of bytes read from the socket - \see read(void*,XsSize,int) - \see XsSocket_read - */ - inline int read(XsByteArray& buffer, int timeout = 0) - { - int requiredSize = read(NULL, 0, timeout); - if (requiredSize <= 0) - return requiredSize; - - buffer.resize(requiredSize); - return read(buffer.data(), buffer.size(), 0); - } - - /*! \brief \copybrief XsSocket_read - - \param[in,out] dest the destination buffer for the incoming data - \param[in] size the size of the destination buffer - \param[in] timeout the maximum time to wait before data is in in milliseconds - - \returns the number of bytes read from the socket - \see read(XsByteArray&,int) - \see XsSocket_read - */ - inline int read(void* dest, XsSize size, int timeout = 0) - { - return XsSocket_read(this, dest, size, timeout); - } - - /*! \brief read data from the socket into \a buffer - - \param[in,out] buffer the buffer to write into - \param[in,out] hostname a pointer to a string that contains the hostname of the sender after returning from this function. May be NULL - \param[in,out] port a pointer that will contain the port of the sender after returning from this function. May be NULL - \param[in] timeout the read timeout in milliseconds - - \returns the size of the data read from the socket, -1 on error - \see readFrom(void*,XsSize,XsString*,uint16_t*,int) - \see XsSocket_readFrom - */ - inline int readFrom(XsByteArray& buffer, XsString* hostname = NULL, uint16_t* port = NULL, int timeout = 0) - { - int requiredSize = readFrom(NULL, 0, NULL, NULL, timeout); - buffer.resize(requiredSize); - return readFrom(buffer.data(), buffer.size(), hostname, port, 0); - } - - /*! \brief \copybrief XsSocket_readFrom - - \param[in,out] dest the buffer to write into - \param[in] size the size of the destination buffer - \param[in,out] hostname a pointer to a string that contains the hostname of the sender after returning from this function. May be NULL - \param[in,out] port a pointer that will contain the port of the sender after returning from this function. May be NULL - \param[in] timeout the read timeout in milliseconds - - \returns the size of the data read from the socket, -1 on error - \see readFrom(XsByteArray,XsString*,uint16_t,int) - \see XsSocket_readFrom - */ - inline int readFrom(void* dest, XsSize size, XsString* hostname = NULL, uint16_t* port = NULL, int timeout = 0) - { - return XsSocket_readFrom(this, dest, size, hostname, port, timeout); - } - - /*! \brief Write the data in \a buffer to the socket - - \param[in] buffer the data to write - - \returns the number of bytes written - \see write(const void*,XsSize) - \see XsSocket_write - */ - int write(const XsByteArray& buffer) - { - return write(buffer.data(), buffer.size()); - } - - /*! \brief \copybrief XsSocket_write - - \param[in] data the data to write - \param[in] size the size of the data to write - - \returns the number of bytes written to the socket - \see write(const XsByteArray&) - \see XsSocket_write - */ - int write(const void* data, XsSize size) - { - return XsSocket_write(this, data, size); - } - - /*! \brief Write the data in \a buffer to the socket - - \param[in] buffer the data to write - \param[in] hostname the host to write data to - \param[in] port the port to write data to - - \returns the number of bytes written - \see writeTo(const void*,XsSize,XsString,uint16_t) - \see XsSocket_writeTo - */ - int writeTo(const XsByteArray& buffer, const XsString& hostname, uint16_t port) - { - return writeTo(buffer.data(), buffer.size(), hostname, port); - } - - /*! \brief \copybrief XsSocket_writeTo - - \param[in] data the data to write - \param[in] size the size of the data to write - \param[in] hostname the host to write data to - \param[in] port the port to write data to - - \returns the number of bytes written to the socket - \see writeTo(XsByteArray,XsString,uint16_t) - \see XsSocket_writeTo - */ - int writeTo(const void* data, XsSize size, const XsString& hostname, uint16_t port) - { - return XsSocket_writeTo(this, data, size, &hostname, port); - } - - /*! \brief \copybrief XsSocket_flush - */ - void flush() - { - XsSocket_flush(this); - } - - /*! - * \brief \copybrief XsSocket_select - * \param[in] mstimeout timeout to wait for socket to be readable/writeable - * \param[in,out] canRead if not null then will be set to non-zero to indicate that the socket has data to read. - * \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) - { - return XsSocket_select(this, mstimeout, canRead, canWrite); - } - - /*! \brief \copybrief XsSocket_accept - - \param[in] mstimeout the timeout in milliseconds. Negative timeouts count as infinite; XSSOCKET_INFINITE_TIMEOUT (-1) by default - \returns a pointer to a newly created XsSocket, NULL on error. The pointer can be deleted. - - \see listen - \see XsSocket_accept - */ - XsSocket* accept(int mstimeout = XSSOCKET_INFINITE_TIMEOUT) - { - return XsSocket_accept(this, mstimeout); - } - - /*! \brief Allocate memory for a socket */ - inline void* operator new(size_t) -#ifdef _MSC_VER - throw(...) -#else - throw(std::bad_alloc) -#endif - { - XsSocket *s = XsSocket_allocate(); - if (!s) - throw std::bad_alloc(); - return s; - } - - /*! \brief De-allocate the socket */ - inline void operator delete(void* p) throw() - { - XsSocket_free(reinterpret_cast(p)); - } - - /*! \brief \copybrief XsSocket_setSocketOption - * \param[in] option The socket option to change - * \param[in] valuePtr Points to the new value of the socket option - * \param[in] valueSize The size of the value \a valuePtr points to - * \returns XRV_OK on success, another XsResultValue otherwise - * \see XsSocket_setSocketOption - */ - XsResultValue setSocketOption(XsSocketOption option, void* valuePtr, int valueSize) - { - return XsSocket_setSocketOption(this, option, valuePtr, valueSize); - } - - /*! \brief Sets an socket option of int type - * \param[in] option The socket option to change - * \param[in] value The new value of the socket option - * \returns XRV_OK on success, another XsResultValue otherwise - * \see XsSocket_setSocketOption - */ - XsResultValue setSocketOption(XsSocketOption option, int value) - { - return XsSocket_setSocketOption(this, option, (void*)&value, sizeof(int)); - } - - /*! \brief Bind to any interface on \a port - - \param[in] port the port to bind to - - \returns XRV_OK on success, another XsResultValue otherwise - \see bind(XsString, port) - \see XsSocket_bind - */ - XsResultValue bind(uint16_t port) - { - return XsSocket_bind(this, NULL, port); - } - - /*! \brief \copybrief XsSocket_bind - - \param[in] hostname the hostname to bind to - \param[in] port the port to bind to - - \returns XRV_OK on success, another XsResultValue otherwise - \see bind(uint16_t) - \see XsSocket_bind - */ - XsResultValue bind(const XsString& hostname, uint16_t port) - { - return XsSocket_bind(this, &hostname, port); - } - - /*! \brief \copybrief XsSocket_listen - - \param[in] maxPending the maximum pending connections - - \returns XRV_OK on success, another XsResultValue when an error occurs - \see XsSocket_listen - \see XsSocket_accept - */ - XsResultValue listen(int maxPending = 20) - { - return XsSocket_listen(this, maxPending); - } - - /*! \brief \copybrief XsSocket_connect - - \param[in] host the host to connect to - \param[in] port the port to connect to - - \returns XRV_OK on success, another XsResultValue when an error occurs - \see XsSocket_connect - */ - XsResultValue connect(const XsString& host, uint16_t port) - { - return XsSocket_connect(this, &host, port); - } - - - /*! \brief Return true if the socket is usable - - \returns true if the socket can be used for communication, false otherwise - \see XsSocket_isUsable - */ - bool isUsable() const - { - return XsSocket_isUsable(this) != 0; - } - - /*! - * \brief Get the IP address of the remote host this socket is connected to. - * \returns the remote host IP address - * - * Only works for connected TCP sockets - */ - XsString getRemoteAddress() const - { - XsString address; - XsSocket_getRemoteAddress(this, &address); - return address; - } - - /*! \brief Return the system error reported by the last operation - */ - int getLastSystemError() const - { - return XsSocket_getLastSystemError(this); - } - -private: -#endif - struct XsSocketPrivate *d; -}; - -#endif diff --git a/extern/include/xsens/xsstring.h b/extern/include/xsens/xsstring.h deleted file mode 100644 index 1abc66b..0000000 --- a/extern/include/xsens/xsstring.h +++ /dev/null @@ -1,480 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSSTRING_H -#define XSSTRING_H - -#include "xstypesconfig.h" -#include "xsarray.h" -#ifndef XSENS_NO_WCHAR -#include -#endif // XSENS_NO_WCHAR -#include - -#ifdef __cplusplus -#include -#include -#include -extern "C" { -#endif - -extern XsArrayDescriptor const XSTYPES_DLL_API g_xsStringDescriptor; - -#ifndef __cplusplus -#define XsString_INITIALIZER XSARRAY_INITIALIZER(&g_xsStringDescriptor) -XSARRAY_STRUCT(XsString, char); -typedef struct XsString XsString; -// obsolete: -#define XSSTRING_INITIALIZER XsString_INITIALIZER -#else -struct XsString; -#endif - -XSTYPES_DLL_API void XsString_construct(XsString* thisPtr); -XSTYPES_DLL_API void XsString_destruct(XsString* thisPtr); -XSTYPES_DLL_API void XsString_assign(XsString* thisPtr, XsSize count, const char* src); -XSTYPES_DLL_API void XsString_assignCharArray(XsString* thisPtr, const char* src); -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 void XsString_sort(XsString* thisPtr); -XSTYPES_DLL_API void XsString_reverse(XsString* thisPtr); - -#ifndef XSENS_NO_WCHAR -XSTYPES_DLL_API XsSize XsString_copyToWCharArray(const XsString* thisPtr, wchar_t* dest, XsSize size); -XSTYPES_DLL_API wchar_t XsString_utf8At(const XsString* thisPtr, XsSize index); -XSTYPES_DLL_API void XsString_assignWCharArray(XsString* thisPtr, const wchar_t* src); -XSTYPES_DLL_API void XsString_push_backWChar(XsString* thisPtr, wchar_t c); -#endif // XSENS_NO_WCHAR - -#ifndef __cplusplus -// obsolete: -#define XsString_copy(thisPtr, copy) XsArray_copy(copy, thisPtr) -#define XsString_swap(a, b) XsArray_swap(a, b) -#else -} // extern "C" -#endif - -#ifdef __cplusplus -/* 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; - -/*! \brief Returns the number of items currently in the array, excluding the terminating 0 - \details This function returns the number of bytes in the underlying char array. This doesn't mean it returns the number of characters as XsString assumes the char* is a utf-8 encoded string. - \returns The number of items currently in the array - \sa reserved \sa setSize \sa resize \sa utf8Len -*/ -template<> inline XsSize XsStringType::size() const -{ - return m_size?m_size-1:0; -} - -//! \copydoc XsArray_reserve -template<> inline void XsStringType::reserve(XsSize count) -{ - if (count) - XsArray_reserve(this, count+1); - else - XsArray_reserve(this, 0); -} - -//! \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, index); -} - -/*! \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. */ -template<> inline void XsStringType::erase(XsSize index, XsSize count) -{ - XsString_erase((XsString*) this, index, count); -} - -/*! \brief Insert \a count \a items at \a index in the string - \param items The items to insert, may not be 0 unless count is 0 - \param index The index to use for inserting. Anything beyond the end of the string (ie. -1) will - append to the actual end of the string. - \param count The number of items to insert -*/ -template<> inline void XsArrayImpl::insert(char const* items, XsSize index, XsSize count) -{ - if (size()) - { - if (index >= size()) - index = size(); - XsArray_insert(this, index, count, items); - } - else - XsString_assign((XsString*) this, count, items); -} - -/*! \brief Assign the characters in \a src to the string - \details This function is resizes the string to fit \a count characters and copies those from \a src. - \param count The number of characters in src. If this value is 0 and \a scr is not 0, the value is - determined automatically by looking through src until a terminating 0 is found. - \param src The source string to copy from. If this is 0 and \a count is not 0, the string will be - filled with spaces instead. - \sa XsString_assign -*/ -template<> inline void XsArrayImpl::assign(XsSize count, char const* src) -{ - XsString_assign((XsString*) this, count, src); -} - -/*! \brief This function resizes the contained string to the desired size, while retaining its contents - \param count The desired size of the string. This excludes the terminating 0 character. - \sa XsString_resize -*/ -template<> inline void XsStringType::resize(XsSize count) -{ - XsString_resize((XsString*) this, count); -} - -/*! \brief Set the size of the array to \a count. - \details The contents of the array after this operation are undefined. - \param count The desired new size fo the array. - \sa XsArray_assign \sa reserve \sa resize -*/ -template<> inline void XsStringType::setSize(XsSize count) -{ - if (count != size()) - XsString_assign((XsString*) this, count, 0); -} - -/*! \copydoc XsArray_append \sa XsArray_append */ -template<> inline void XsArrayImpl::append(const XsStringType& other) -{ - XsString_append((XsString*) this, (XsString const*) &other); -} - -struct XsString : public XsStringType { - //! \brief Constructs an XsString - inline explicit XsString(XsSize sz = 0, char const* src = 0) - : XsStringType() - { - if (sz || src) - XsString_assign(this, sz, src); - } - - //! \brief Constructs an XsString as a copy of \a other - inline XsString(const XsStringType& other) - : XsStringType(other) - { - } - - //! \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) - { - } -#ifndef XSENS_NOITERATOR - //! \brief Constructs an XsString with the list bound by the supplied iterators \a beginIt and \a endIt - template - inline explicit XsString(Iterator const& beginIt, Iterator const& endIt) - : XsStringType(beginIt, endIt) - { - } -#endif - - //! \brief Construct an XsString from a 0-terminated character array - inline XsString(char const* src) - : XsStringType() - { - if (src && src[0]) - XsString_assignCharArray(this, src); - } - -#ifndef XSENS_NO_WCHAR - //! \brief Construct an XsString from a 0-terminated character array - inline XsString(wchar_t const* src) - : XsStringType() - { - if (src && src[0]) - XsString_assignWCharArray(this, src); - } -#endif - -#ifndef XSENS_NO_STL - //! \brief Construct an XsString from a std::string - inline XsString(std::string const& src) - : XsStringType() - { - if (!src.empty()) - XsString_assign(this, src.size()+1, src.c_str()); - } - - //! \brief Construct an XsString from a std::wstring - inline XsString(std::wstring const& src) - : XsStringType() - { - if (!src.empty()) - XsString_assignWCharArray(this, src.c_str()); - } -#endif // XSENS_NO_STL - - //! \brief Return the internal 0-terminated C-style string - inline char* c_str() - { - static const char nullChar = 0; - if (empty()) - return (char*) &nullChar; - return begin().operator ->(); - } - - //! \brief Return the internal 0-terminated C-style string - inline char const* c_str() const - { - static const char nullChar = 0; - if (empty()) - return &nullChar; - return begin().operator ->(); - } - -#ifndef XSENS_NO_STL - //! \brief Return the string as a std::string - std::string toStdString() const - { - if (empty()) - return std::string(); - return std::string((char const*)begin().operator ->()); - } -#endif // XSENS_NO_STL - - //! \brief Return \a other appended to this, without modifying this - XsString operator + (XsString const& other) const - { - XsString tmp; - tmp.reserve(size()+other.size()); - tmp.append(*this); - tmp.append(other); - return tmp; - } - -#ifndef XSENS_NO_STL - //! \brief Return the string as a std::wstring - std::wstring toStdWString() const - { - if (empty()) - return std::wstring(); - size_t s = XsString_copyToWCharArray(this, NULL, 0); - std::wstring w; - w.resize(s-1); - (void) XsString_copyToWCharArray(this, &w[0], s); - return w; - } -#endif // XSENS_NO_STL - - /*! \cond NODOXYGEN */ - using XsStringType::operator ==; - using XsStringType::operator !=; -#ifndef XSENS_NOITERATOR - using XsStringType::operator <<; -#endif - /*! \endcond */ - - //! \brief Return true if the contents of \a str are identical to this string - bool operator == (char const* str) const - { - if (!str) return empty(); - return !strcmp(c_str(), 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 Append a character array to the string in a stream-like way - inline XsString& operator << (char const* str) - { - if (str && str[0]) - { - XsString const ref(const_cast(str), strlen(str), XSDF_None); - append(ref); - } - return *this; - } - - //! \brief Append an integer to the string in a stream-like way - inline XsString& operator << (int i) - { - char buffer[32]; - append(XsString(buffer, std::sprintf(buffer, "%d", i), XSDF_None)); - return *this; - } - - //! \brief Append another string to the string in a stream-like way - inline XsString& operator << (XsString const& s) - { - append(s); - return *this; - } - - //! \brief Return true if the contents if \a str are greater then this string - inline bool operator < (const XsString &str) const - { -#ifdef XSENS_NO_STL - return (strcmp(c_str(), str.c_str()) < 0); -#else - return (this->toStdString() < str.toStdString()); -#endif - } - - //! \brief Return true if the contents if \a str are less then this string - inline bool operator > (const XsString &str) const - { -#ifdef XSENS_NO_STL - return (strcmp(c_str(), str.c_str()) > 0); -#else - return (this->toStdString() > str.toStdString()); -#endif - } - - //! \brief Append a character to the string - inline XsString& push_back(char c) - { - XsString_push_back(this, c); - return *this; - } - - //! \brief Return true when the string is empty - inline bool empty() const - { - return !!XsString_empty(this); - } - - using XsStringType::append; - - //! \brief Append a char array to the string - inline void append(char const* other) - { - if (other) - { - XsString tmp(other); - XsString_append(this, &tmp); - } - } - -#ifndef XSENS_NO_WCHAR - //! \brief Append a wchar array to the string - inline void append(wchar_t const* other) - { - if (other) - { - XsString tmp(other); - XsString_append(this, &tmp); - } - } -#endif - - //! \returns the number of characters in a utf-8 encoded string - inline XsSize utf8Len() const - { - return XsString_utf8Len(this); - } - - /*! \brief Returns whether this string ends with \a other (case-insensitive!) - \param other The string to match with the end of this string - \param caseSensitive Whether to compare case sensitive or not (case insensitive is the default) - \return true when the string ends with the given string - */ - inline bool endsWith(XsString const& other, bool caseSensitive = false) const - { - return 0 != XsString_endsWith(this, &other, caseSensitive); - } - - /*! \brief Returns whether this string starts with \a other (case-insensitive!) - \param other The string to match with the start of this string - \param caseSensitive Whether to compare case sensitive or not (case insensitive is the default) - \return true when the string starts with the given string - */ - inline bool startsWith(XsString const& other, bool caseSensitive = false) const - { - return 0 != XsString_startsWith(this, &other, caseSensitive); - } - - /*! \brief Returns whether this string contains \a other (case-insensitive!) - \param other The string to match with this string - \param caseSensitive Whether to compare case sensitive or not (case insensitive is the default) - \param offset When not null, this will be filled with the first index where \a other was found - \return true when the string contains the given string - */ - inline bool contains(XsString const& other, bool caseSensitive = false, XsSize* offset = 0) const - { - return 0 != XsString_contains(this, &other, caseSensitive, offset); - } - -#ifndef XSENS_NO_WCHAR - /*! \brief The decoded UTF-8 character at index \a index in the UTF-8 encoded string - \details http://en.wikipedia.org/wiki/Utf-8#Description - \param index The index of the character to return. - \returns the decoded UTF-8 character at index \a index in the UTF-8 encoded string - */ - inline wchar_t utf8At(XsSize index) const - { - return XsString_utf8At(this, index); - } - - //! \brief Append a unicode character to the string - inline XsString& push_back(wchar_t c) - { - XsString_push_backWChar(this, c); - return *this; - } -#endif - - //! \brief Sort the characters in the string - inline void sort() - { - XsString_sort(this); - } - - /*! \brief Reverse the characters in the string - \note This does not take into account utf-8 encoded characters - */ - inline void reverse() - { - XsString_reverse(this); - } -}; - -#ifndef XSENS_NO_STL -namespace std { -template -basic_ostream<_CharT, _Traits>& operator<<(basic_ostream<_CharT, _Traits>& o, XsString const& xs) -{ - return (o << xs.toStdString()); -} -} -#endif - -#endif - -#endif diff --git a/extern/include/xsens/xsstringarray.h b/extern/include/xsens/xsstringarray.h deleted file mode 100644 index 9f5cd9b..0000000 --- a/extern/include/xsens/xsstringarray.h +++ /dev/null @@ -1,131 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSSTRINGARRAY_H -#define XSSTRINGARRAY_H - -#include "xsarray.h" - -#ifdef __cplusplus -#include "xsstring.h" -extern "C" { -#endif - -extern XsArrayDescriptor const XSTYPES_DLL_API g_xsStringArrayDescriptor; - -#ifndef __cplusplus -#define XSSTRINGARRAY_INITIALIZER XSARRAY_INITIALIZER(&g_xsStringArrayDescriptor) -struct XsString; - -XSARRAY_STRUCT(XsStringArray, struct XsString); -typedef struct XsStringArray XsStringArray; - -XSTYPES_DLL_API void XsStringArray_construct(XsStringArray* thisPtr, XsSize count, struct XsString const* src); -#define XsStringArray_destruct(thisPtr) XsArray_destruct(thisPtr) -#endif -XSTYPES_DLL_API void XsStringArray_fromSplicedString(struct XsStringArray* thisPtr, struct XsString const* src, struct XsString const* separators); -XSTYPES_DLL_API void XsStringArray_join(struct XsStringArray const* thisPtr, struct XsString* result, struct XsString const* separator); - -#ifdef __cplusplus -} // extern "C" -#endif - -#ifdef __cplusplus -struct XsStringArray : public XsArrayImpl { - //! \brief Constructs an XsStringArray - inline explicit XsStringArray(XsSize sz = 0, XsString const* src = 0) - : ArrayImpl(sz, src) - { - } - - //! \brief Constructs an XsStringArray as a copy of \a other - inline XsStringArray(XsStringArray const& other) - : ArrayImpl(other) - { - } - - //! \brief Constructs an XsStringArray that references the data supplied in \a ref - inline explicit XsStringArray(XsString* ref, XsSize sz, XsDataFlags flags = XSDF_None) - : ArrayImpl(ref, sz, flags) - { - } - -#ifndef XSENS_NOITERATOR - //! \brief Constructs an XsStringArray with the array bound by the supplied iterators \a beginIt and \a endIt - template - inline XsStringArray(Iterator beginIt, Iterator endIt) - : ArrayImpl(beginIt, endIt) - { - } -#endif - - /*! \brief Join the non-empty strings contained in the XsStringArray into one XsString, separating each item with \a separator - \param separator An optional separator string to put between substrings - \return The joined string - */ - XsString join(XsString const& separator) const - { - XsString tmp; - XsStringArray_join(this, &tmp, &separator); - return tmp; - } - - /*! \copydoc XsStringArray_fromSplicedString */ - void fromSplicedString(XsString const& src, XsString const& separators) - { - XsStringArray_fromSplicedString(this, &src, &separators); - } - - /*! \brief find same string in array which can be case insensitive or not - \param needle String to be found - \param isCaseSensitive Optional, default is true - \return place of needle in the array or -1 if the string was not found - */ - int find(XsString const& needle, bool isCaseSensitive = true) const - { - if (isCaseSensitive) - return XsArrayImpl::find(needle); - - if (size() == 0) - return -1; - - XsString needleLower(needle); - for (XsString::iterator c = needleLower.begin(); c != needleLower.end(); ++c) - *c = tolower(*c); - - for (XsSize i = 0; i < size(); ++i) // loop over all elements of the lists - { - XsString const& a = at(i); - if (a.size() != needleLower.size()) - continue; - - bool found = true; - for (XsSize j = 0; j < a.size(); ++j) - { - if (tolower(a[j]) != needleLower[j]) - { - found = false; - break; - } - } - - if (found) - return (int)i; - } - return -1; - } -}; -#endif - -#endif diff --git a/extern/include/xsens/xsstringstreaming.h b/extern/include/xsens/xsstringstreaming.h deleted file mode 100644 index 9554c50..0000000 --- a/extern/include/xsens/xsstringstreaming.h +++ /dev/null @@ -1,119 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSSTRINGSTREAMING_H -#define XSSTRINGSTREAMING_H - -#ifdef __cplusplus -#include "xsstring.h" -#include "xsvector.h" -#include "xsmatrix.h" -#include "xsquaternion.h" - -#ifndef XSENS_NO_STL -#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, XsMatrix const& xm) -{ - 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; -} - -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 - -inline XsString& operator<<(XsString& o, XsSize const& v) -{ - char buffer[32]; // 2e64 = 1.8e19 so this should be enough - sprintf(buffer, "%" PRINTF_SIZET_MODIFIER "u", v); - o << XsString(buffer); - return o; -} - -inline XsString& operator<<(XsString& o, XsReal const& v) -{ - char buffer[64]; - sprintf(buffer, "%g", v); - o << XsString(buffer); - return o; -} - -inline XsString& operator<<(XsString& 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] << ")"); -} - -inline XsString& operator<<(XsString& o, XsMatrix const& xm) -{ - o << "M<" << xm.rows() << "," << xm.cols() << ">("; - for (XsSize r = 0; r < xm.rows(); ++r) - { - if (xm.cols()) - { - if (r > 0) - o << "\t"; - 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; -} - -inline XsString& operator<<(XsString& o, XsQuaternion const& xq) -{ - o << "Q("; - for (int i = 0; i < 3; i++) - o << xq[i] << ", "; - return (o << xq[3] << ")"); -} - -#endif - -#endif diff --git a/extern/include/xsens/xssyncfunction.h b/extern/include/xsens/xssyncfunction.h deleted file mode 100644 index 7d55061..0000000 --- a/extern/include/xsens/xssyncfunction.h +++ /dev/null @@ -1,52 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSSYNCFUNCTION_H -#define XSSYNCFUNCTION_H - -/*! \addtogroup enums Global enumerations - @{ -*/ -/*! \brief Actions to be taken on input triggers */ -enum XsSyncFunction -{ - XSF_StartRecordingIn, /*!< \brief Start recording on trigger or emit trigger when first recording frame is started (In). \remark Applies to Awinda Station. */ - XSF_StopRecordingIn, /*!< \brief Stop recording on trigger or emit trigger when recording is stopped (In). \remark Applies to Awinda Station. */ - XSF_ResetTimer, /*!< \brief On input trigger, the outgoing timer of the station will be set to 0. \remark Applies to Awinda Station. */ - XSF_TriggerIndication, /*!< \brief An indication is sent to the driver when trigger is detected. \remark Applies to Awinda Station. */ - - XSF_IntervalTransitionMeasurement, /*!< \brief Emit trigger on an interval transition during measurement and recording. */ - XSF_IntervalTransitionRecording, /*!< \brief Emit trigger on an interval transition during recording. */ - XSF_GotoOperational, /*!< \brief Emit trigger when going to Operational mode \remark Applies to Awinda Station. */ - - XSF_SampleAndSend, /*!< \brief Sample a sample and send the MT Data message. \remark Applies to Mt. */ - XSF_SendLatest, /*!< \brief Send the latest sample. \remark Applies to Mt. */ - XSF_ClockBiasEstimation, /*!< \brief Do a clock bias estimation on trigger. \remark Applies to Mti-G. */ - - XSF_PulseWithModulation, /*!< \brief Do interval transition measurement with pulse width modulation. \remark Applies only to Xbusmaster. */ - - XSF_StartSampling, /*!< \brief Start sampling. Data will only be transmitted after this trigger has been received. \remark Applies only to Mk4. */ - - XSF_StartRecordingOut, /*!< \brief Start recording on trigger or emit trigger when first recording frame is started (Out). \remark Applies to Awinda Station. */ - XSF_StopRecordingOut, /*!< \brief Stop recording on trigger or emit trigger when recording is stopped (Out). \remark Applies to Awinda Station. */ - - XSF_Gnss1Pps, /*!< \brief Emit trigger on the start of each second, generated by the GNSS receiver. */ - - XSF_Invalid, /*!< \brief Invalid action \details This indicates the trigger action is not usable. */ - XSF_Count = XSF_Invalid /*!< \brief Amount of trigger actions */ -}; -/*! @} */ -typedef enum XsSyncFunction XsSyncFunction; - -#endif diff --git a/extern/include/xsens/xssyncline.h b/extern/include/xsens/xssyncline.h deleted file mode 100644 index 66bbb68..0000000 --- a/extern/include/xsens/xssyncline.h +++ /dev/null @@ -1,46 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSSYNCLINE_H -#define XSSYNCLINE_H - -/*! \addtogroup enums Global enumerations - @{ -*/ -/*! \brief Synchronization line identifiers */ -enum XsSyncLine -{ - XSL_Inputs, /*!< \brief Value for checking if a line is an input, any item equal to or greater than XSL_Inputs and less than XSL_Outputs is an input */ - XSL_In1 = XSL_Inputs, /*!< \brief Sync In 1 \remark Applies to Awinda Station and Mt */ - XSL_In2, /*!< \brief Sync In 2 \remark Applies to Awinda Station */ - XSL_Bi1In, /*!< \brief Bidirectional Sync 1 In \remark Applies to Xbus Master */ - XSL_ClockIn, /*!< \brief Clock synchronisation input \remark Applies to Mk4 */ - XSL_CtsIn, /*!< \brief RS232 CTS sync in \remark Applies to Xbus Master */ - XSL_GnssClockIn, /*!< \brief Clock synchronisation input line attached to internal GPS unit \remark Applies to Mk4 */ - XSL_GpsClockIn = XSL_GnssClockIn, /*!< \brief Clock synchronisation input line attached to internal GPS unit \remark Applies to Mk4 \deprecated */ - XSL_ExtTimepulseIn, /*!< \brief External time pulse input (e.g. for external GNSS unit) \remark Applies to Mk4 with external device */ - XSL_ReqData, /*!< \brief Serial data sync option, use \a XMID_ReqData message id for this \remark Applies to Mk4*/ - - XSL_Outputs, /*!< \brief Value for checking if a line is output. Values equal to or greater than this are outputs */ - XSL_Out1 = XSL_Outputs, /*!< \brief Sync Out 1 \remark Applies to Awinda Station and Mt */ - XSL_Out2, /*!< \brief Sync Out 2 \remark Applies to Awinda Station */ - XSL_Bi1Out, /*!< \brief Bidirectional Sync 1 Out \remark Applies to Xbus Master */ - XSL_RtsOut, /*!< \brief RS232 RTS sync out \remark Applies to Xbus Master */ - - XSL_Invalid /*!< \brief Invalid sync setting. Used if no sync line is set */ -}; -/*! @} */ -typedef enum XsSyncLine XsSyncLine; - -#endif diff --git a/extern/include/xsens/xssyncpolarity.h b/extern/include/xsens/xssyncpolarity.h deleted file mode 100644 index a9f38f6..0000000 --- a/extern/include/xsens/xssyncpolarity.h +++ /dev/null @@ -1,34 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSSYNCPOLARITY_H -#define XSSYNCPOLARITY_H - -/*! \addtogroup enums Global enumerations - @{ -*/ -/*! \brief Signal polarity */ -enum XsSyncPolarity -{ - XSP_None = 0, /*!< \brief Don't generate or react to trigger level changes */ - XSP_RisingEdge = 1, /*!< \brief React to a rising edge on input */ - XSP_PositivePulse = XSP_RisingEdge, /*!< \brief Generate a positive pulse on output */ - XSP_FallingEdge = 2, /*!< \brief React to a falling edge on input */ - XSP_NegativePulse = XSP_FallingEdge, /*!< \brief Generate a falling edge on output */ - XSP_Both /*!< \brief Toggle output or react on all toggles on input */ -}; -/*! @} */ -typedef enum XsSyncPolarity XsSyncPolarity; - -#endif diff --git a/extern/include/xsens/xssyncrole.h b/extern/include/xsens/xssyncrole.h deleted file mode 100644 index d80491f..0000000 --- a/extern/include/xsens/xssyncrole.h +++ /dev/null @@ -1,32 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSSYNCROLE_H -#define XSSYNCROLE_H - -/*! \addtogroup enums Global enumerations - @{ -*/ -/*! \brief Synchronization roles -*/ -enum XsSyncRole { - XSR_Slave, - XSR_None, - XSR_MasterSlave, - XSR_Master -}; -/*! @} */ -typedef enum XsSyncRole XsSyncRole; - -#endif diff --git a/extern/include/xsens/xssyncsetting.h b/extern/include/xsens/xssyncsetting.h deleted file mode 100644 index 20aeb8a..0000000 --- a/extern/include/xsens/xssyncsetting.h +++ /dev/null @@ -1,137 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSSYNCSETTINGS_H -#define XSSYNCSETTINGS_H - -#include - -#include "pstdint.h" -#include "xstypesconfig.h" -#include "xssyncline.h" -#include "xssyncfunction.h" -#include "xssyncpolarity.h" - -struct XsSyncSetting; - -#ifdef __cplusplus -extern "C" { -#else -#define XSSYNCSETTINGS_INITIALIZER { XSL_Invalid, XSF_Invalid, XSP_None, 1, 0, 0, 0, 0, 0, 0 } -#endif - -XSTYPES_DLL_API int XsSyncSetting_isInput(const struct XsSyncSetting* thisPtr); -XSTYPES_DLL_API int XsSyncSetting_isOutput(const struct XsSyncSetting* thisPtr); -XSTYPES_DLL_API void XsSyncSetting_swap(struct XsSyncSetting* a, struct XsSyncSetting* b); -XSTYPES_DLL_API int XsSyncSetting_compare(const struct XsSyncSetting* a, const struct XsSyncSetting* b); - -#ifdef __cplusplus -} // extern "C" -#endif - -/*! \brief A structure for storing all xsens sync settings */ -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. */ - uint32_t m_pulseWidth; /*!< The time to keep the line polarity before toggling back, in microseconds. */ - int32_t m_offset; /*!< The time between reception of a line change and the execution of the sync action, in microseconds. */ - uint16_t m_skipFirst; /*!< The number of frames to skip before executing the action. */ - uint16_t m_skipFactor; /*!< The number of frames to skip between 2 actions. */ - uint16_t m_clockPeriod; /*!< The frequency of the external clock in milliseconds, only valid when function is XSF_ClockBiasEstimation. */ - uint8_t m_triggerOnce; /*!< Whether the action is repeated for each frame. */ - uint8_t m_padding; /*!< Padding to get at a 4 byte boundary so memcpy/memcmp works with sizeof(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) - : m_line(line) - , m_function(function) - , m_polarity(polarity) - , m_pulseWidth(pulseWidth) - , m_offset(offset) - , m_skipFirst(skipFirst) - , m_skipFactor(skipFactor) - , m_clockPeriod(clockPeriod) - , m_triggerOnce(triggerOnce) - , m_padding(0) - { - } - - //! \brief Construct a XsSyncSetting as a copy of \a other. - XsSyncSetting(const XsSyncSetting& other) - { - memcpy(this, &other, sizeof(XsSyncSetting)); - } - - //! \brief Copy values of \a other into this. - const XsSyncSetting& operator =(const XsSyncSetting& other) - { - if (this != &other) - memcpy(this, &other, sizeof(XsSyncSetting)); - return *this; - } - - //! \brief \copybrief XsSyncSetting_isInput - inline bool isInput() const - { - return 0 != XsSyncSetting_isInput(this); - } - - //! \brief \copybrief XsSyncSetting_isOutput - inline bool isOutput() const - { - return 0 != XsSyncSetting_isOutput(this); - } - - /*! \brief Swap the contents with \a other - */ - inline void swap(XsSyncSetting& other) - { - XsSyncSetting_swap(this, &other); - } - - /*! \brief Return true if \a other is identical to this - */ - inline bool operator == (const XsSyncSetting& other) const - { - return (this == &other) || XsSyncSetting_compare(this, &other) == 0; - } - - /*! \brief Return true if \a other is not identical to this - */ - inline bool operator != (const XsSyncSetting& other) const - { - return (this != &other) && XsSyncSetting_compare(this, &other) != 0; - } - - /*! \brief Return true if \a other is less than this - */ - inline bool operator < (const XsSyncSetting& other) const - { - return (this != &other) && XsSyncSetting_compare(this, &other) < 0; - } -#endif -}; - -typedef struct XsSyncSetting XsSyncSetting; - -#endif diff --git a/extern/include/xsens/xssyncsettingarray.h b/extern/include/xsens/xssyncsettingarray.h deleted file mode 100644 index a1f3e86..0000000 --- a/extern/include/xsens/xssyncsettingarray.h +++ /dev/null @@ -1,69 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSSYNCSETTINGARRAY_H -#define XSSYNCSETTINGARRAY_H - -#include "xsarray.h" - -#ifdef __cplusplus -#include "xssyncsetting.h" -extern "C" { -#endif - -extern XsArrayDescriptor const XSTYPES_DLL_API g_xsSyncSettingArrayDescriptor; - -#ifndef __cplusplus -#define XSSYNCSETTINGSARRAY_INITIALIZER XSARRAY_INITIALIZER(&g_xsSyncSettingArrayDescriptor) - -struct XsSyncSetting; -XSARRAY_STRUCT(XsSyncSettingArray, struct XsSyncSetting); -typedef struct XsSyncSettingArray XsSyncSettingArray; - -XSTYPES_DLL_API void XsSyncSettingArray_construct(XsSyncSettingArray* thisPtr, XsSize count, struct XsSyncSetting const* src); -#else -} // extern "C" -#endif - -#ifdef __cplusplus -struct XsSyncSettingArray : public XsArrayImpl { - //! \brief Constructs an XsSyncSettingArray - inline explicit XsSyncSettingArray(XsSize sz = 0, XsSyncSetting const* src = 0) - : ArrayImpl(sz, src) - { - } - - //! \brief Constructs an XsSyncSettingArray as a copy of \a other - inline XsSyncSettingArray(XsSyncSettingArray const& other) - : ArrayImpl(other) - { - } - - //! \brief Constructs an XsSyncSettingArray that references the data supplied in \a ref - inline explicit XsSyncSettingArray(XsSyncSetting* ref, XsSize sz, XsDataFlags flags = XSDF_None) - : ArrayImpl(ref, sz, flags) - { - } - -#ifndef XSENS_NOITERATOR - //! \brief Constructs an XsSyncSettingArray with the array bound by the supplied iterators \a beginIt and \a endIt - template - inline XsSyncSettingArray(Iterator beginIt, Iterator endIt) - : ArrayImpl(beginIt, endIt) - { - } -#endif -}; -#endif -#endif diff --git a/extern/include/xsens/xssyncsettinglist.h b/extern/include/xsens/xssyncsettinglist.h deleted file mode 100644 index 795e906..0000000 --- a/extern/include/xsens/xssyncsettinglist.h +++ /dev/null @@ -1,36 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSSYNCSETTINGLIST_H -#define XSSYNCSETTINGLIST_H - -#include "xssyncsettingarray.h" -#define XsSyncSettingList XsSyncSettingArray - -#ifndef __cplusplus - -#define XSSYNCSETTINGLIST_INITIALIZER XSSYNCSETTINGSARRAY_INITIALIZER - -#define XsSyncSettingList_assign(thisPtr, size, src) XsArray_assign(thisPtr, size, src) -#define XsSyncSettingList_construct(thisPtr, size, src) XsSyncSettingArray_construct(thisPtr, size, src) -#define XsSyncSettingList_destruct(thisPtr) XsArray_destruct(thisPtr) -#define XsSyncSettingList_copy(thisPtr, copy) XsArray_copy(copy, thisPtr) -#define XsSyncSettingList_append(thisPtr, other) XsArray_append(thisPtr, other) -#define XsSyncSettingList_popFront(thisPtr, count) XsArray_erase(thisPtr, 0, count) -#define XsSyncSettingList_popBack(thisPtr, count) XsArray_erase(thisPtr, (XsSize)-1, count) -#define XsSyncSettingList_swap(a, b) XsArray_swap(a, b) -#define XsSyncSettingList_erase(thisPtr, index, count) XsArray_erase(thisPtr, index, count) - -#endif -#endif diff --git a/extern/include/xsens/xsthread.h b/extern/include/xsens/xsthread.h deleted file mode 100644 index 47237fd..0000000 --- a/extern/include/xsens/xsthread.h +++ /dev/null @@ -1,158 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSTHREAD_H -#define XSTHREAD_H - -#include "xstime.h" -#if defined(XSENS_DEBUG) && defined(_MSC_VER) -#pragma warning (disable: 4985) -#include -#endif - -#ifndef __GNUC__ -#pragma warning(disable: 4127) -#endif - -#ifdef __cplusplus -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; -#ifdef __cplusplus - typedef ::DWORD XsThreadId; -#else - 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) - - XSTYPES_DLL_API void xsNameThisThread(const char* threadName); - - #define xsGetCurrentThreadId() GetCurrentThreadId() - #define xsGetCurrentThreadHandle() GetCurrentThread() - #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 -//#define XSENS_USE_POSIX_LOCKING 1 - - /*! \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() - - 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 - -#ifndef __GNUC__ -#pragma warning(default: 4127) -#endif - -#ifdef __cplusplus -} // extern "C" -#endif - -typedef enum XsThreadPriority XsThreadPriority; - -#endif // file guard diff --git a/extern/include/xsens/xstime.h b/extern/include/xsens/xstime.h deleted file mode 100644 index 6f2e51d..0000000 --- a/extern/include/xsens/xstime.h +++ /dev/null @@ -1,141 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSTIME_H -#define XSTIME_H - -#include "xstypesconfig.h" -#ifdef _WIN32 -#include -#endif - -#include -#include "pstdint.h" -#include "xstimestamp.h" - -#ifdef __cplusplus -#include "xsstring.h" -extern "C" { -#endif - -XSTYPES_DLL_API extern const XsTimeStamp XsTime_secPerDay; -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 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); -XSTYPES_DLL_API void XsTime_getTimeAsWString(wchar_t* dest, struct tm const* time); -XSTYPES_DLL_API void XsTime_msleep(uint32_t ms); -XSTYPES_DLL_API void XsTime_udelay(uint64_t us); -XSTYPES_DLL_API int64_t XsTime_timeStampNow(XsTimeStamp* now); -XSTYPES_DLL_API void XsTime_initializeTime(void); - -#ifdef __cplusplus -} // extern "C" - -namespace XsTime { -#ifdef __GNUC__ -#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; -#ifdef __GNUC__ -#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) - { - XsTime_msleep(ms); - } - - //! \copydoc XsTime_udelay - inline void udelay(uint64_t us) - { - 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 - -#endif // file guard diff --git a/extern/include/xsens/xstimestamp.h b/extern/include/xsens/xstimestamp.h deleted file mode 100644 index d2407bb..0000000 --- a/extern/include/xsens/xstimestamp.h +++ /dev/null @@ -1,239 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSTIMESTAMP_H -#define XSTIMESTAMP_H - -#include "xstypesconfig.h" -#include "pstdint.h" - -#ifdef __cplusplus -extern "C" { -#else -#define XSTIMESTAMP_INITIALIZER { 0 } -#endif - -struct XsTimeStamp; -struct XsUtcTime; - -XSTYPES_DLL_API void XsTimeStamp_setMilliSecondTime(struct XsTimeStamp* thisPtr, int64_t t); -XSTYPES_DLL_API double XsTimeStamp_timeOfDay(const struct XsTimeStamp* thisPtr); -XSTYPES_DLL_API int64_t XsTimeStamp_secondTime(const struct XsTimeStamp* thisPtr); -XSTYPES_DLL_API int32_t XsTimeStamp_milliSecondPart(const struct XsTimeStamp* thisPtr); -XSTYPES_DLL_API int32_t XsTimeStamp_secondPart(const struct XsTimeStamp* thisPtr); -XSTYPES_DLL_API int32_t XsTimeStamp_minutePart(const struct XsTimeStamp* thisPtr); -XSTYPES_DLL_API int32_t XsTimeStamp_hourPart(const struct XsTimeStamp* thisPtr); -XSTYPES_DLL_API int64_t XsTimeStamp_now(struct XsTimeStamp* thisPtr); -XSTYPES_DLL_API int64_t XsTimeStamp_maxValue(void); -XSTYPES_DLL_API int64_t XsTimeStamp_fromUtcTime(struct XsTimeStamp* thisPtr, const struct XsUtcTime* utc); -XSTYPES_DLL_API void XsTimeStamp_toUtcTime(struct XsTimeStamp* thisPtr, struct XsUtcTime* utc); - -#ifdef __cplusplus -} // extern "C" -#endif - -/*! \struct XsTimeStamp - \brief Class for managing timestamps in a unified way. -*/ -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) {} - - /*! \brief Construct a timestamp with \a t as the time in milliseconds. */ - 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)) {} - - /*! \brief Construct a copy of \a other. */ - inline XsTimeStamp(const XsTimeStamp& other) : m_msTime(other.m_msTime) {} - - /*! \brief Construct from \a utc */ - inline XsTimeStamp(const XsUtcTime& utc) - { - XsTimeStamp_fromUtcTime(this, &utc); - } - - /*! \brief Convert this timestamp to UTC time \a utc */ - inline void toUtcTime(XsUtcTime& utc) - { - XsTimeStamp_toUtcTime(this, &utc); - } - - /*! \brief Assign the contents of the \a other timestamp to this timestamp. */ - inline XsTimeStamp& operator = (const XsTimeStamp& other) - { - //lint --e{1529} trivial assignment - m_msTime = other.m_msTime; - return *this; - } - - /*! \brief Get the stored time as milliseconds. */ - inline int64_t msTime(void) const - { return m_msTime; } - - /*! \brief Set the stored time to \a t milliseconds. */ - inline void setMsTime(int64_t 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); } - - /*! \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); } - - /*! \brief Return the time as seconds */ - inline double secTime() const - { return ((double)m_msTime)*0.001; } - - /*! \brief Set the time as seconds */ - inline void setSecTime(double t) - { 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); } - - /*! \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); } - - /*! \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; } - - /*! \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; } - - /*! \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; } - - /*! \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; } - - /*! \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; } - - /*! \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; } - - /*! \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; } - - /*! \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; } - - /*! \brief Test if the given \a other is smaller than the current timestamp. */ - 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; } - - /*! \brief Test if the given \a other is equal to the current timestamp. */ - inline bool operator == (int other) const - { 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; } - - /*! \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; } - - /*! \brief Test if the given \a other is not equal to the current timestamp. */ - inline bool operator != (int other) const - { 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; } - - /*! \brief Returns the millisecond part of the time (in the range 0-999) */ - inline int32_t milliSecondPart() const - { 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); } - - /*! \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); } - - /*! \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); } - - /*! \brief Returns the current time in ms since the epoch (Jan 1st 1970) */ - inline static XsTimeStamp now() - { - XsTimeStamp tmp; - XsTimeStamp_now(&tmp); - return tmp; - } - - /*! \brief Returns the current time in ms since the epoch (Jan 1st 1970) */ - inline static int64_t nowMs() - { - XsTimeStamp tmp; - XsTimeStamp_now(&tmp); - return tmp.msTime(); - } - - /*! \brief Returns the maximum value of an %XsTimeStamp */ - inline static XsTimeStamp maxValue() - { - return XsTimeStamp(int64_t(9223372036854775807LL)); //INT64_MAX - } - - /*! \brief Increment the timestamp by one ms, prefix */ - XsTimeStamp operator++() - { return XsTimeStamp(++m_msTime); } - - /*! \brief Increment the timestamp by one ms, postfix */ - XsTimeStamp operator++(int) - { return XsTimeStamp(m_msTime++); } - - /*! \brief Decrement the timestamp by one ms, prefix */ - XsTimeStamp operator--() - { return XsTimeStamp(--m_msTime); } - - /*! \brief Decrement the timestamp by one ms, postfix */ - XsTimeStamp operator--(int) - { return XsTimeStamp(m_msTime--); } - -private: -#endif - - int64_t m_msTime; //!< The timestamp value -}; - -typedef struct XsTimeStamp XsTimeStamp; - -#endif diff --git a/extern/include/xsens/xstriggerindicationdata.h b/extern/include/xsens/xstriggerindicationdata.h deleted file mode 100644 index 9a3c95c..0000000 --- a/extern/include/xsens/xstriggerindicationdata.h +++ /dev/null @@ -1,80 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSTRIGGERINDICATIONDATA_H -#define XSTRIGGERINDICATIONDATA_H - -#include "xstypesconfig.h" -#include "pstdint.h" - -#ifdef __cplusplus -extern "C" { -#else -#define XSTRIGGERINDICATIONDATA_INITIALIZER { 0, 0, 0, 0 } -#endif - -struct XsTriggerIndicationData; - -XSTYPES_DLL_API void XsTriggerIndicationData_destruct(struct XsTriggerIndicationData* thisPtr); -XSTYPES_DLL_API int XsTriggerIndicationData_valid(const struct XsTriggerIndicationData* thisPtr); - -#ifdef __cplusplus -} // extern "C" -#endif - - -/*! \brief Data for a trigger indication message */ -struct XsTriggerIndicationData { - uint8_t m_line; //!< The line number - uint8_t m_polarity; //!< The polarity - uint32_t m_timestamp; //!< The timestamp - uint16_t m_frameNumber; //!< The frame number - -#ifdef __cplusplus - /*! Constructor - \param[in] line Line - \param[in] polarity Polarity - \param[in] timestamp Timestamp - \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) - {} - - /*! \brief \copybrief XsTriggerIndicationData_destruct */ - inline void clear() - { - XsTriggerIndicationData_destruct(this); - } - - /*! \brief \copybrief XsTriggerIndicationData_valid */ - inline bool valid() const - { - return 0 != XsTriggerIndicationData_valid(this); - } - - /*! \brief Returns true if all fields of this and \a other are exactly identical */ - 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; - } -#endif -}; - -typedef struct XsTriggerIndicationData XsTriggerIndicationData; - -#endif diff --git a/extern/include/xsens/xstypedefs.h b/extern/include/xsens/xstypedefs.h deleted file mode 100644 index 22c0d23..0000000 --- a/extern/include/xsens/xstypedefs.h +++ /dev/null @@ -1,87 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSTYPEDEFS_H -#define XSTYPEDEFS_H - -#include "xstypesconfig.h" -#include "pstdint.h" - -#ifndef XSENS_SINGLE_PRECISION -#include -typedef double XsReal; //!< Defines the floating point type used by the Xsens libraries -typedef size_t XsSize; //!< XsSize must be unsigned number! -# ifndef PRINTF_SIZET_MODIFIER -# if defined(XSENS_64BIT) -# if defined(__APPLE__) -# define PRINTF_SIZET_MODIFIER "l" -# else -# define PRINTF_SIZET_MODIFIER PRINTF_INT64_MODIFIER -# endif -# else -# define PRINTF_SIZET_MODIFIER PRINTF_INT32_MODIFIER -# endif -# endif // PRINTF_SIZET_MODIFIER -#else -typedef float XsReal; //!< Defines the floating point type used by the Xsens libraries -typedef unsigned int XsSize; //!< XsSize must be unsigned number! -#endif // XSENS_SINGLE_PRECISION - - -/*! \addtogroup enums Global enumerations - @{ -*/ -/*! \brief These flags define the behaviour of data contained by Xsens data structures - \details Normally, the user should never need to use these directly. -*/ -enum XsDataFlags { - XSDF_None = 0 //!< No flag set - ,XSDF_Managed = 1 //!< The contained data should be managed (freed) by the object, when false, the object assumes the memory is freed by some other process after its destruction - ,XSDF_FixedSize = 2 //!< The contained data points to a fixed-size buffer, this allows creation of dynamic objects on the stack without malloc/free overhead. - ,XSDF_Empty = 4 //!< The object contains undefined data / should be considered empty. Usually only relevant when XSDF_FixedSize is also set, as otherwise the data pointer will be NULL and empty-ness is implicit. -}; -/*! @} */ -typedef enum XsDataFlags XsDataFlags; - -#ifdef __cplusplus -extern "C" { -#endif - -XSTYPES_DLL_API const char *XsDataFlags_toString(XsDataFlags f); - -#ifdef __cplusplus -} // extern "C" -/*! \brief \copybrief XsDataFlags_toString \sa XsDataFlags_toString */ -inline const char *toString(XsDataFlags s) -{ - return XsDataFlags_toString(s); -} -#else -// define BOOL, TRUE and FALSE -#ifndef BOOL -typedef int BOOL; -#endif - -#ifndef TRUE -#define TRUE (1) -#endif - -#ifndef FALSE -#define FALSE (0) -#endif -#endif // __cplusplus - -#define XS_ENUM_TO_STR_CASE(value) case value: return #value; - -#endif diff --git a/extern/include/xsens/xstypesconfig.h b/extern/include/xsens/xstypesconfig.h deleted file mode 100644 index 505be62..0000000 --- a/extern/include/xsens/xstypesconfig.h +++ /dev/null @@ -1,243 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#define XSTYPES_DLL_API -#define XSNOEXPORT -#define XSENS_NO_AUTOLIB -// include this file in Visual Studio using C/C++->Advanced->Force Includes (the /FI option) -#ifndef XSTYPES_CONFIG_H -#define XSTYPES_CONFIG_H - -////////////////////////////////////////////////// -// generic preprocessor defines - -//http://support.microsoft.com/kb/155196 -#define __STR2__(x) #x -#define __STR1__(x) __STR2__(x) -#define __LOC__ __FILE__ "(" __STR1__(__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 -#endif - -#ifdef WIN32 -# ifndef _WIN32 -# define _WIN32 -# define XSENS_WINDOWS -# endif -#endif -#if !defined(_WIN32) && defined(_MSC_VER) && !defined(_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 -#endif -#ifndef _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 -#else -#define XSCCONST -#define XSCPPPROTECTED protected: -#endif - -#ifdef __GNUC__ -#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 WIN64 -# define WIN64 -# endif -#else -# ifndef XSENS_32BIT -# define XSENS_32BIT -# endif -#endif - -// all xsens libraries should use unicode -#ifndef 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) - //// Debug or RelWithDeb build - #if defined(XSENS_RELEASE) - //// RelWithDeb build - #else - //// Debug build - #endif -#else - //// Release build -#endif -*/ - -#if defined(XSENS_DEBUG) - //// Debug or RelWithDeb build - - #if !defined(XSENS_RELEASE) - //// Debug build - #if !defined(QT_DEBUG) && !defined(QT_NO_DEBUG) - #define QT_DEBUG 1 - #endif - #define XSENS_CONFIG 0 - #else - //// RelWithDeb build - #if !defined(QT_DEBUG) && !defined(QT_NO_DEBUG) - #define QT_NO_DEBUG 1 - #endif - #define XSENS_CONFIG 1 - #endif -#else - //// Release build - #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 - #endif - #endif -#endif - -////////////////////////////////////////////////// -// more generic preprocessor defines -// required for gnu c++ compiler versions due to difference in attribute declarations -#if defined(__AVR32__) -# define __cdecl -# define __stdcall -#elif defined(_ADI_COMPILER) -# 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 -#endif - - -////////////////////////////////////////////////// -// generic preprocessor defines - -// use XSENS_32BIT and XSENS_64BIT to check for 32/64 bit builds in your application -// 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 -#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 -#include "xstypedefs.h" - -#ifndef XSNOCOMEXPORT -#define XSNOCOMEXPORT -#endif - -#define XSTRINGIFY2(s) #s -#define XSTRINGIFY(s) XSTRINGIFY2(s) - -#ifdef XSENS_DEBUG -#define XSDEBUGLINE(a) a -#else -#define XSDEBUGLINE(...) -#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 -#endif - -#endif // file guard diff --git a/extern/include/xsens/xstypesdynlib.h b/extern/include/xsens/xstypesdynlib.h deleted file mode 100644 index 4e10598..0000000 --- a/extern/include/xsens/xstypesdynlib.h +++ /dev/null @@ -1,32 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSTYPESDYNLIB_H -#define XSTYPESDYNLIB_H - -#include "xstypesconfig.h" - -struct XsString; - -#ifdef __cplusplus -extern "C" { -#endif - -XSTYPES_DLL_API void xstypesPath(struct XsString* path); - -#ifdef __cplusplus -} -#endif - -#endif diff --git a/extern/include/xsens/xsushortvector.h b/extern/include/xsens/xsushortvector.h deleted file mode 100644 index 124e74d..0000000 --- a/extern/include/xsens/xsushortvector.h +++ /dev/null @@ -1,118 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSUSHORTVECTOR_H -#define XSUSHORTVECTOR_H - -#ifndef __cplusplus -#define XSUSHORTVECTOR_INITIALIZER {{0,0,0}} -#else -#include "xstypedefs.h" -#endif - -/*! \brief A vector containing 3 short values. */ -struct XsUShortVector { -#ifdef __cplusplus - //! \brief Constructor that creates the vector with all components set to 0 - inline XsUShortVector() - { - m_data[0] = 0; - m_data[1] = 0; - m_data[2] = 0; - } - - //! \brief Constructor that creates the vector with all components set to given values \a v1 \a v2 and \a v3 - inline XsUShortVector(unsigned short v1, unsigned short v2, unsigned short v3) - { - m_data[0] = v1; - m_data[1] = v2; - m_data[2] = v3; - } - - //! \brief Constructor that creates the vector with all components set to values in array \a a - inline explicit XsUShortVector(const unsigned short* a) - { - m_data[0] = a[0]; - m_data[1] = a[1]; - m_data[2] = a[2]; - } - - //! \brief Constructor that creates the vector and initializes it with data from the \a other vector - inline XsUShortVector(const XsUShortVector& other) - { - m_data[0] = other.m_data[0]; - m_data[1] = other.m_data[1]; - m_data[2] = other.m_data[2]; - } - - //! \brief Assignment operator copies the data from the \a other vector to this vector - inline const XsUShortVector& operator = (const XsUShortVector& other) - { - if (this != &other) - { - m_data[0] = other.m_data[0]; - m_data[1] = other.m_data[1]; - m_data[2] = other.m_data[2]; - } - return *this; - } - - //! \brief Comparison operator, returns true if the contents of the \a other vector match those of this vector - 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]; - } - - //! \brief Return the size of the vector (always 3) - inline XsSize size() const - { - return 3; - } - - //! \brief Return a value from the vector (needed to allow generated C# access to these elements) - inline unsigned short at(int index) - { - return m_data[index]; - } - - //! \brief Returns the \a index'th item in the vector - inline unsigned short operator[](XsSize index) const - { - assert(index < 3); - return m_data[index]; - } - - //! \brief Returns a reference the \a index'th item in the vector - inline unsigned short& operator[](XsSize index) - { - assert(index < 3); - return m_data[index]; //lint !e1536 - } - - //! \brief Returns the start of the internal data buffer - inline unsigned short const* data() const - { - return m_data; - } -private: -#endif - - unsigned short m_data[3]; //!< vector component storage -}; - -typedef struct XsUShortVector XsUShortVector; - -#endif diff --git a/extern/include/xsens/xsutctime.h b/extern/include/xsens/xsutctime.h deleted file mode 100644 index af69122..0000000 --- a/extern/include/xsens/xsutctime.h +++ /dev/null @@ -1,73 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSUTCTIME_H -#define XSUTCTIME_H - -#include "xstypesconfig.h" -#include "pstdint.h" - -#ifdef __cplusplus -extern "C" -{ -#else -#define XSUTCTIME_INITIALIZER { 0, 0 ,0, 0, 0, 0, 0, 0} -#endif -struct XsUtcTime; - -XSTYPES_DLL_API void XsUtcTime_currentTime(struct XsUtcTime * now); - -#ifdef __cplusplus -} // extern "C" -#endif - -/*! \brief A structure for storing UTC Time values. */ -struct XsUtcTime { - uint32_t m_nano; //!< \brief Nanosecond part of the time - uint16_t m_year; //!< \brief The year (if date is valid) - uint8_t m_month; //!< \brief The month (if date is valid) - uint8_t m_day; //!< \brief The day of the month (if date is valid) - uint8_t m_hour; //!< \brief The hour (if time is valid) - uint8_t m_minute; //!< \brief The minute (if time is valid) - uint8_t m_second; //!< \brief The second (if time is valid) - uint8_t m_valid; //!< \brief Validity indicator \details When received: bit (0) - UTC Date is valid; bit (1) - UTC Time of Day is valid; bit (2) - UTC Time of Day has been fully resolved (i.e. no seconds uncertainty). - -#ifdef __cplusplus - /*! \copydoc XsUtcTime_currentTime - \return The current UTC Time - */ - inline static XsUtcTime currentTime() - { - XsUtcTime tmp; - XsUtcTime_currentTime(&tmp); - return tmp; - } - - /*! \brief Returns true if all fields of this and \a other are exactly identical */ - inline bool operator == (const XsUtcTime& 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; - } -#endif -}; -typedef struct XsUtcTime XsUtcTime; - -#endif diff --git a/extern/include/xsens/xsvector.h b/extern/include/xsens/xsvector.h deleted file mode 100644 index 792e40c..0000000 --- a/extern/include/xsens/xsvector.h +++ /dev/null @@ -1,366 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSVECTOR_H -#define XSVECTOR_H - -#include "xsmath.h" -#include -#include // memcpy - -struct XsVector; -struct XsQuaternion; - -#ifdef __cplusplus -#include -#include -extern "C" { -#else -#define XSVECTOR_INITIALIZER { NULL, 0, 0 } -typedef struct XsVector XsVector; -#endif - -XSTYPES_DLL_API void XsVector_ref(XsVector* thisPtr, XsSize sz, XsReal* buffer, XsDataFlags flags); -XSTYPES_DLL_API void XsVector_construct(XsVector* thisPtr, XsSize sz, const XsReal* src); -XSTYPES_DLL_API void XsVector_assign(XsVector* thisPtr, XsSize sz, const XsReal* src); -XSTYPES_DLL_API void XsVector_destruct(XsVector* thisPtr); -XSTYPES_DLL_API void XsVector_copy(XsVector* copy, XsVector const* src); -XSTYPES_DLL_API XsReal XsVector_dotProduct(const XsVector* a, const XsVector* b); -XSTYPES_DLL_API XsReal XsVector_cartesianLength(const XsVector* thisPtr); -XSTYPES_DLL_API void XsVector_normalize(XsVector* thisPtr); -XSTYPES_DLL_API void XsVector_setZero(XsVector* thisPtr); -XSTYPES_DLL_API int XsVector_empty(const XsVector* thisPtr); -XSTYPES_DLL_API void XsVector_multiplyScalar(const XsVector* thisPtr, XsReal scalar, XsVector* dest); -XSTYPES_DLL_API void XsVector_angularVelocityFromQuaternion(XsVector* thisPtr, XsReal deltaT, const struct XsQuaternion* quat); -XSTYPES_DLL_API void XsVector_swap(XsVector* a, XsVector* b); -XSTYPES_DLL_API void XsVector_fill(XsVector* thisPtr, XsReal value); -XSTYPES_DLL_API int XsVector_equal(const XsVector* thisPtr, const XsVector* thatPtr); -XSTYPES_DLL_API int XsVector_compare(const XsVector* thisPtr, const XsVector* thatPtr, XsReal epsilon); - -#ifdef __cplusplus -} // extern "C" -#endif -#ifndef XSENS_NO_PACK -#pragma pack(push,1) -#endif -struct XsVector { -XSCPPPROTECTED - //lint --e{613} - XsReal* const m_data; //!< \protected Points to contained data buffer - const XsSize m_size; //!< \protected Size of contained data buffer in elements - const int m_flags; //!< \protected Flags for data management - -#ifdef __cplusplus - //! \brief Return the data management flags of the vector. - inline int flags() { 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) - : m_data(0) - , m_size(0) - , m_flags(0) - { - if (sz) - XsVector_construct(this, sz, src); - } - - //! \brief Initialize a vector using the supplied \a other vector - inline XsVector(const XsVector& other) - : m_data(0) - , m_size(0) - , m_flags(0) - { - *this = other; - } - - //! \brief Initialize a vector that references the supplied data - inline explicit XsVector(XsReal* ref, XsSize sz, XsDataFlags flags_ = XSDF_None) - : m_data(ref) - , m_size(sz) - , m_flags(flags_) - { - } - - //! \brief Initialize a vector that references the supplied data - inline explicit XsVector(const XsVector& other, XsReal* ref, XsSize sz, XsDataFlags flags_ = XSDF_None) - : m_data(ref) - , m_size(sz) - , m_flags(flags_) - { - XsVector_copy(this, &other); - } - - //! \copydoc XsVector_angularVelocityFromQuaternion - inline explicit XsVector(const XsQuaternion& quat, XsReal deltaT) - : m_data(0) - , m_size(0) - , m_flags(0) - { - XsVector_angularVelocityFromQuaternion(this, deltaT, &quat); - } - - //! \brief Assignment operator. Copies from \a other into this - inline XsVector& operator=(const XsVector& other) - { - //lint --e{1529} check done in copy function - XsVector_copy(this, &other); - return *this; - } - - //! \copydoc XsVector_destruct - inline ~XsVector() - { - XsVector_destruct(this); - } - - //! \copydoc XsVector_assign - inline void assign(XsSize sz, const XsReal* src) - { - XsVector_assign(this, sz, src); - } - - /*! \brief Sets the size of the XsVector to \a sz items - \param sz The desired size of the vector - \sa XsVector_assign - */ - inline void setSize(XsSize sz) - { - XsVector_assign(this, sz, 0); - } - - //! \brief Returns the number of elements in the vector - inline XsSize size() const - { - return m_size; - } - - //! \brief Return a const pointer to the data - inline const XsReal* data() const - { - return m_data; - } - - //! \brief Multiply the vector by \a scalar and return the result - inline XsVector operator * (XsReal scalar) const - { - XsVector v(m_size); - for (XsSize i = 0; i < m_size; ++i) - v.m_data[i] = m_data[i] * scalar; - return v; - } - - //! \brief Multiply the vector by \a scalar and store the result in this vector - inline void operator *=(XsReal scalar) - { - for (XsSize i = 0; i < m_size; ++i) - m_data[i] *= scalar; - } - - //! \brief Returns a reference to the \a index'th item in the vector - inline XsReal& at(XsSize index) - { - assert(index < m_size); - return m_data[index]; - } - - //! \brief Returns a const reference to the \a index'th item in the vector - inline const XsReal& at(XsSize index) const - { - assert(index < m_size); - return m_data[index]; - } - - //! \brief Returns the \a index'th item in the vector - inline XsReal value(XsSize index) const - { - assert(index < m_size); - return m_data[index]; - } - - //! \brief Sets the \a index'th item in the vector - inline void setValue(XsSize index, XsReal val) - { - assert(index < m_size); - m_data[index] = val; - } - - //! \brief Returns the \a index'th item in the vector - inline XsReal operator[](XsSize index) const - { - assert(index < m_size); - return m_data[index]; - } - - //! \brief Returns a reference the \a index'th item in the vector - inline XsReal& operator[](XsSize index) - { - assert(index < m_size); - return m_data[index]; - } - - //! \brief \copybrief XsVector_dotProduct - inline XsReal dotProduct(const XsVector &v) const - { - return XsVector_dotProduct(this, &v); - } - - //! \copydoc XsVector_cartesianLength - inline XsReal cartesianLength() const - { - return XsVector_cartesianLength(this); - } - - //! \copydoc XsVector_normalize - inline void normalize() - { - XsVector_normalize(this); - } - - //! \brief \copybrief XsVector_setZero - inline void setZero() - { - return XsVector_setZero(this); - } - - //! \brief \copybrief XsVector_empty - inline bool empty() const - { - return 0 != XsVector_empty(this); - } - - //! \copydoc XsVector_angularVelocityFromQuaternion - inline XsVector& angularVelocityFromQuaternion(const XsQuaternion& quat, XsReal deltaT) - { - XsVector_angularVelocityFromQuaternion(this, deltaT, &quat); - return *this; - } - - //! \brief Return \e this - \a sub - XsVector operator-(const XsVector& sub) const - { - assert(m_size == sub.m_size); - XsVector tmp(m_size); - for (XsSize i = 0; i < m_size; ++i) - tmp[i] = m_data[i] - sub.m_data[i]; - return tmp; - } - - //! \brief Return \e this + \a sub - XsVector operator+(const XsVector& sub) const - { - assert(m_size == sub.m_size); - XsVector tmp(m_size); - for (XsSize i = 0; i < m_size; ++i) - tmp[i] = m_data[i] + sub.m_data[i]; - return tmp; - } - - //! \brief Return true when the values in this vector are exactly (to the last bit) equal to \a other - bool operator==(const XsVector& other) const - { - return 0 != XsVector_equal(this, &other); - } - - /*! \brief Return true when the values in this vector are equal within \a epsilon - \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 compare(const XsVector &other, XsReal epsilon) const - { - return 0 != XsVector_compare(this, &other, epsilon); - } - -#ifndef XSENS_NO_STL - //! \brief Returns the XsVector as a std::vector of XsReal - inline std::vector toVector() const - { - std::vector tmp(m_size); - if (m_size) - memcpy(&tmp[0], m_data, m_size * sizeof(XsReal)); - return tmp; - } -#endif - - /*! \brief Fill the vector with zeroes */ - inline void zero() - { - for (XsSize i = 0; i < m_size; ++i) - m_data[i] = XsMath_zero; - } - - /*! \brief Fill the vector with \a val */ - inline void fill(XsReal val) - { - for (XsSize i = 0; i < m_size; ++i) - m_data[i] = val; - } - - /*! \brief Swap the contents of \a b with this - \details This function swaps the internal buffers so no actual data is moved around. For unmanaged - data an elementwise swap is done, but only if the vectors are the same size. - \param b Object whose contents will be swapped with this - */ - inline void swap(XsVector& b) - { - XsVector_swap(this, &b); - } - - /*! \brief Append \a other to this - \details Append the vector in \a other to the end of this vector - \param other The vector to append - */ - inline void append(XsVector const& other) - { - XsVector tmp(size() + other.size()); - for (XsSize i = 0; i < size(); ++i) - tmp[i] = (*this)[i]; - for (XsSize i = 0; i < other.size(); ++i) - tmp[i+size()] = other[i]; - swap(tmp); - } - - /*! \brief Clear the vector, making it size 0 */ - inline void clear() - { - setSize(0); - } - -#ifndef XSENS_NO_STL - /*! \brief Reverse the values in the vector */ - inline void reverse() - { - XsSize sz = size(); - XsSize half = sz >> 1; - --sz; - for (XsSize i = 0; i < half; ++i) - std::swap(operator[](i), operator[](sz-i)); - } -#endif - -#endif -}; -#ifndef XSENS_NO_PACK -#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) -{ - return v*scalar; -} -#endif - -#endif diff --git a/extern/include/xsens/xsvector3.h b/extern/include/xsens/xsvector3.h deleted file mode 100644 index 6eb8537..0000000 --- a/extern/include/xsens/xsvector3.h +++ /dev/null @@ -1,94 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSVECTOR3_H -#define XSVECTOR3_H - -#include "xsvector.h" - -struct XsVector3; -#ifdef __cplusplus -extern "C" { -#else -typedef struct XsVector3 XsVector3; -#endif - -XSTYPES_DLL_API void XsVector3_construct(XsVector3* thisPtr, const XsReal* src); -XSTYPES_DLL_API void XsVector3_assign(XsVector3* thisPtr, const XsReal* src); -XSTYPES_DLL_API void XsVector3_destruct(XsVector3* thisPtr); -XSTYPES_DLL_API void XsVector3_copy(XsVector* copy, XsVector3 const* src); - -#ifdef __cplusplus -} // extern "C" -#endif - -#ifdef __cplusplus -/* 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 -#else -struct XsVector3 { - XsVector m_vector; //!< The underlying vector -#endif - XsReal XSCCONST m_fixedData[3]; //!< Fixed size storage for the components in the vector - -#ifdef __cplusplus -public: - //! \brief Constructs an empty vector3 - XsVector3() : XsVector(m_fixedData, 3, XSDF_FixedSize) - { - //XsVector3_construct(this, 0); - } - - //! \brief Constructs a vector3 from an \a other XsVector - XsVector3(XsVector3 const& other) : XsVector(other, m_fixedData, 3, XSDF_FixedSize) - { - } - - //! \brief Constructs a vector3 from an \a other XsVector - XsVector3(XsVector const& other) : XsVector(other, m_fixedData, 3, XSDF_FixedSize) - { - } - - //! \brief Constructs a vector3 using the values \a x, \a y, \a z - XsVector3(XsReal x, XsReal y, XsReal z) : XsVector(m_fixedData, 3, XSDF_FixedSize) - { - m_data[0] = x; - m_data[1] = y; - m_data[2] = z; - } - - //! \brief Return a 3-element zero vector - static XsVector3 const& zero3() - { - static const XsVector3 rv(XsMath_zero, XsMath_zero, XsMath_zero); - return rv; - } - - using XsVector::operator=; - //! \brief Assignment operator. Copies from \a other into this - inline XsVector3& operator=(const XsVector3& other) - { - //lint --e{1529} check done in copy function - XsVector_copy(this, &other); - return *this; - } - - // using XsVector::operator[]; -#endif -}; - -#endif diff --git a/extern/include/xsens/xsversion.h b/extern/include/xsens/xsversion.h deleted file mode 100644 index 2ee700a..0000000 --- a/extern/include/xsens/xsversion.h +++ /dev/null @@ -1,226 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSVERSION_H -#define XSVERSION_H - -#include "xstypesconfig.h" -#include "xssimpleversion.h" -#include "xsstring.h" - -typedef struct XsVersion XsVersion; - -#ifdef __cplusplus -extern "C" { -#else -#define XSVERSION_INITIALIZER { 0, 0, 0, 0, 0, XsString_INITIALIZER } -#endif - -XSTYPES_DLL_API int XsVersion_empty(const XsVersion* thisPtr); -XSTYPES_DLL_API void XsVersion_toString(const XsVersion* thisPtr, XsString* version); -XSTYPES_DLL_API void XsVersion_fromString(XsVersion* thisPtr, const XsString* version); -XSTYPES_DLL_API void XsVersion_fromSimpleVersion(XsVersion* thisPtr, const XsSimpleVersion* simpleVersion); -XSTYPES_DLL_API void XsVersion_toSimpleVersion(const XsVersion* thisPtr, XsSimpleVersion* simpleVersion); -XSTYPES_DLL_API void XsVersion_toSimpleString(const XsVersion* thisPtr, XsString* version); - -#ifdef __cplusplus -} // extern "C" -#endif - -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) - : m_major(maj) - , m_minor(min) - , m_revision(rev) - , m_build(build) - , m_reposVersion(0) - , m_extra(extra) - {} - - //! \brief Constructs a version object using the supplied parameters or an empty version object if no parameters are given. - explicit XsVersion(int maj = 0, int min = 0, int rev = 0, int build = 0, int reposVersion = 0, const XsString& extra = XsString()) - : m_major(maj) - , m_minor(min) - , m_revision(rev) - , m_build(build) - , m_reposVersion(reposVersion) - , m_extra(extra) - {} - - //! \brief Constructs a version object based upon the version contained by \a vString. - explicit XsVersion(const XsString& vString) - { - XsVersion_fromString(this, &vString); - } - - //! \brief Constructs a version object based upon the version contained by \a simpleVersion. - explicit XsVersion(const XsSimpleVersion& simpleVersion) - { - XsVersion_fromSimpleVersion(this, &simpleVersion); - } - - //! \brief Constructs a version object based upon the \a other object - XsVersion(const XsVersion& other) - : m_major(other.m_major) - , m_minor(other.m_minor) - , m_revision(other.m_revision) - , m_build(other.m_build) - , m_reposVersion(other.m_reposVersion) - , m_extra(other.m_extra) - {} - - //! \brief Assign the version from the \a other object - XsVersion& operator = (const XsVersion& other) - { - m_major = other.m_major; - m_minor = other.m_minor; - m_revision = other.m_revision; - m_build = other.m_build; - m_reposVersion = other.m_reposVersion; - m_extra = other.m_extra; - return *this; - } - - /*! \brief Test if the \a other version is equal to this. The comparison involves the entire object.*/ - 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); - } - - /*! \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; - } - - /*! \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); - } - - /*! \brief Test if the \a other version is lower than 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) - return true; - else if (m_major > other.m_major) - return false; - 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; - } - } - } - - /*! \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); - } - - /*! \brief Test if the \a other version is higher than this. The comparison involves only the version numbers (major, minor and revision). */ - inline bool operator > (const XsVersion& other) const - { - return !(*this <= other); - } - - /*! \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); - } - - //! \brief \copybrief XsVersion_empty - inline bool empty() const - { - return 0 != XsVersion_empty(this); - } - - //! \brief \copybrief XsVersion_toString - inline XsString toString() const - { - XsString tmp; - XsVersion_toString(this, &tmp); - return tmp; - } - - //! \brief \copybrief XsVersion_toSimpleString - inline XsString toSimpleString() const - { - XsString tmp; - XsVersion_toSimpleString(this, &tmp); - return tmp; - } - - //! \brief \copybrief XsVersion_toSimpleVersion - inline XsSimpleVersion toSimpleVersion() const - { - XsSimpleVersion result; - XsVersion_toSimpleVersion(this, &result); - return result; - } - - //! \brief Return the \e major part of the version - inline int major() const { return m_major; } - //! \brief Return the \e minor part of the version - inline int minor() const { return m_minor; } - //! \brief Return the \e revision part of the version - inline int revision() const { return m_revision; } - //! \brief Return the \e build number used for this 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;} - //! \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; } - - //! \brief Set the \e major part of the version - 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; } - //! \brief Set the \e revision part of the version - 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; } - //! \brief Set the \e reposVersion part of the version - 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; } - -private: -#endif - - int m_major; //!< The major part of the version number - int m_minor; //!< The minor part of the version number - int m_revision; //!< The revision number of the version - int m_build; //!< The build number for this build - int m_reposVersion; //!< The source revision used for this build - XsString m_extra; //!< Storage for some extra information about the version -}; - -#endif diff --git a/extern/include/xsens/xsxbusmessageid.h b/extern/include/xsens/xsxbusmessageid.h deleted file mode 100644 index 45bf464..0000000 --- a/extern/include/xsens/xsxbusmessageid.h +++ /dev/null @@ -1,453 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef XSXBUSMESSAGEID_H -#define XSXBUSMESSAGEID_H - -/*! \addtogroup enums Global enumerations - @{ -*/ -/*! \brief Xsens Xbus Message Identifiers -*/ -enum XsXbusMessageId { - XMID_InvalidMessage = 0x00, - - // Config state messages - XMID_ReqDid = 0x00, - XMID_DeviceId = 0x01, - XMID_Initbus = 0x02, - XMID_InitBusResults = 0x03, - XMID_ReqPeriod = 0x04, - XMID_ReqPeriodAck = 0x05, - XMID_SetPeriod = 0x04, - XMID_SetPeriodAck = 0x05, - // XbusMaster - XMID_SetBid = 0x06, - XMID_SetBidAck = 0x07, - XMID_AutoStart = 0x06, - XMID_AutoStartAck = 0x07, - XMID_BusPower = 0x08, - XMID_BusPowerAck = 0x09, - // End XbusMaster= - XMID_ReqDataLength = 0x0A, - XMID_DataLength = 0x0B, - XMID_ReqConfiguration = 0x0C, - XMID_Configuration = 0x0D, - XMID_RestoreFactoryDef = 0x0E, - XMID_RestoreFactoryDefAck = 0x0F, - - XMID_GotoMeasurement = 0x10, - XMID_GotoMeasurementAck = 0x11, - XMID_ReqFirmwareRevision = 0x12, - XMID_FirmwareRevision = 0x13, - // XbusMaster - XMID_ReqBluetoothDisable = 0x14, - XMID_ReqBluetoothDisableAck = 0x15, - XMID_DisableBluetooth = 0x14, - XMID_DisableBluetoothAck = 0x15, - XMID_ReqXmOutputMode = 0x16, - XMID_ReqXmOutputModeAck = 0x17, - XMID_SetXmOutputMode = 0x16, - XMID_SetXmOutputModeAck = 0x17, - // End XbusMaster - XMID_ReqBaudrate = 0x18, - XMID_ReqBaudrateAck = 0x19, - XMID_SetBaudrate = 0x18, - XMID_SetBaudrateAck = 0x19, - // XbusMaster - XMID_ReqSyncMode = 0x1A, - XMID_ReqSyncModeAck = 0x1B, - XMID_SetSyncMode = 0x1A, - XMID_SetSyncModeAck = 0x1B, - // End XbusMaster - XMID_ReqProductCode = 0x1C, - XMID_ProductCode = 0x1D, - - XMID_ReqProcessingFlags = 0x20, - XMID_ReqProcessingFlagsAck = 0x21, - XMID_SetProcessingFlags = 0x20, - XMID_SetProcessingFlagsAck = 0x21, - - XMID_SetNoRotation = 0x22, - XMID_SetNoRotationAck = 0x23, - - XMID_RunSelfTest = 0x24, - XMID_SelfTestResults = 0x25, - - XMID_GotoConfig = 0x30, - XMID_GotoConfigAck = 0x31, - XMID_BusData = 0x32, - XMID_MtData = 0x32, - - XMID_ReqInputTrigger = 0x26, - XMID_ReqInputTriggerAck = 0x27, - XMID_SetInputTrigger = 0x26, - XMID_SetInputTriggerAck = 0x27, - - XMID_ReqOutputTrigger = 0x28, - XMID_ReqOutputTriggerAck = 0x29, - XMID_SetOutputTrigger = 0x28, - XMID_SetOutputTriggerAck = 0x29, - - XMID_SetSyncStationMode = 0x2A, - XMID_SetSyncStationModeAck = 0x2B, - XMID_ReqSyncStationMode = 0x2A, - XMID_ReqSyncStationModeAck = 0x2B, - // deprecated names - XMID_SetSyncBoxMode = 0x2A, - XMID_SetSyncBoxModeAck = 0x2B, - XMID_ReqSyncBoxMode = 0x2A, - XMID_ReqSyncBoxModeAck = 0x2B, - // end of deprecated names - - XMID_SetSyncConfiguration = 0x2C, - XMID_SetSyncConfigurationAck = 0x2D, - XMID_ReqSyncConfiguration = 0x2C, - XMID_SyncConfiguration = 0x2D, - - XMID_DriverDisconnect = 0x2E, - XMID_DriverDisconnectAck = 0x2F, - - // Manual - XMID_PrepareData = 0x32, - XMID_ReqData = 0x34, - XMID_ReqDataAck = 0x35, - - XMID_MtData2 = 0x36, - XMID_MtData2Ack = 0x37, - - XMID_RequestControl = 0x38, - XMID_RequestControlAck = 0x39, - - XMID_SetDataPort = 0x3A, - XMID_SetDataPortAck = 0x3B, - - XMID_ReqRetransmission = 0x3C, - XMID_ReqRetransmissionAck = 0x3D, - - // Wakeup state messages - XMID_Wakeup = 0x3E, - XMID_WakeupAck = 0x3F, - - // Valid in all states - XMID_Reset = 0x40, - XMID_ResetAck = 0x41, - XMID_Error = 0x42, - // end Valid in all states - - // XbusMaster - XMID_XmPowerOff = 0x44, - // End XbusMaster - - // Wireless - XMID_MasterIndication = 0x46, - - XMID_ReqOptionFlags = 0x48, - XMID_ReqOptionFlagsAck = 0x49, - XMID_SetOptionFlags = 0x48, - XMID_SetOptionFlagsAck = 0x49, - XMID_ReqStealthMode = 0x4A, - XMID_StealthMode = 0x4B, - XMID_SetStealthMode = 0x4A, - XMID_SetStealthModeAck = 0x4B, - - XMID_UserInterface = 0x4C, - XMID_UserInterfaceAck = 0x4D, - - XMID_EndOfRecording = 0x4E, - XMID_EndOfRecordingAck = 0x4F, - - XMID_GotoTransparentMode = 0x50, - XMID_GotoTransparentModeAck = 0x51, - - XMID_RunFactoryTest = 0x56, - XMID_FactoryTestResults = 0x57, - XMID_FactoryTestConnect = 0x58, - XMID_FactoryTestConnectAck = 0x59, - - XMID_SetUtcTime = 0x60, - XMID_ReqUtcTime = 0x60, - XMID_SetUtcTimeAck = 0x61, - XMID_UtcTime = 0x61, - XMID_FactoryTestSensorTiming = 0x60, - XMID_FactoryTestSensorTimingResults = 0x61, - - XMID_ReqAvailableFilterProfiles = 0x62, - XMID_AvailableFilterProfiles = 0x63, - - XMID_ReqFilterProfile = 0x64, - XMID_ReqFilterProfileAck = 0x65, - XMID_SetFilterProfile = 0x64, - XMID_SetFilterProfileAck = 0x65, - - XMID_ReqAvailableScenarios = XMID_ReqAvailableFilterProfiles, //!< \deprecated The name 'Scenario' has been deprecated for this use in favor of the name 'FilterProfile' - XMID_AvailableScenarios = XMID_AvailableFilterProfiles, //!< \deprecated The name 'Scenario' has been deprecated for this use in favor of the name 'FilterProfile' - XMID_ReqScenario = XMID_ReqFilterProfile, //!< \deprecated The name 'Scenario' has been deprecated for this use in favor of the name 'FilterProfile' - XMID_ReqScenarioAck = XMID_ReqFilterProfileAck, //!< \deprecated The name 'Scenario' has been deprecated for this use in favor of the name 'FilterProfile' - XMID_SetScenario = XMID_SetFilterProfile, //!< \deprecated The name 'Scenario' has been deprecated for this use in favor of the name 'FilterProfile' - XMID_SetScenarioAck = XMID_SetFilterProfileAck, //!< \deprecated The name 'Scenario' has been deprecated for this use in favor of the name 'FilterProfile' - - XMID_ReqGravityMagnitude = 0x66, - XMID_ReqGravityMagnitudeAck = 0x67, - XMID_SetGravityMagnitude = 0x66, - XMID_SetGravityMagnitudeAck = 0x67, - - XMID_ReqGnssLeverArm = 0x68, - XMID_ReqGnssLeverArmAck = 0x69, - XMID_SetGnssLeverArm = 0x68, - XMID_SetGnssLeverArmAck = 0x69, - - XMID_ReqGpsLeverArm = XMID_ReqGnssLeverArm, //!< \deprecated - XMID_ReqGpsLeverArmAck = XMID_ReqGnssLeverArmAck, //!< \deprecated - XMID_SetGpsLeverArm = XMID_SetGnssLeverArm, //!< \deprecated - XMID_SetGpsLeverArmAck = XMID_SetGnssLeverArmAck, //!< \deprecated - - XMID_ReqReplayMode = 0x6C, - XMID_ReqReplayModeAck = 0x6D, - XMID_SetReplayMode = 0x6C, - XMID_SetReplayModeAck = 0x6D, - - XMID_ReqLatLonAlt = 0x6E, - XMID_ReqLatLonAltAck = 0x6F, - XMID_SetLatLonAlt = 0x6E, - XMID_SetLatLonAltAck = 0x6F, - - // Xbus Master - XMID_ReqXmErrorMode = 0x82, - XMID_ReqXmErrorModeAck = 0x83, - XMID_SetXmErrorMode = 0x82, - XMID_SetXmErrorModeAck = 0x83, - - XMID_ReqBufferSize = 0x84, - XMID_ReqBufferSizeAck = 0x85, - XMID_SetBufferSize = 0x84, - XMID_SetBufferSizeAck = 0x85, - // End Xbus Master - - XMID_ReqHeading = 0x82, - XMID_ReqHeadingAck = 0x83, - XMID_SetHeading = 0x82, - XMID_SetHeadingAck = 0x83, - - XMID_ReqMagneticField = 0x6A, - XMID_ReqMagneticFieldAck = 0x6B, - XMID_SetMagneticField = 0x6A, - XMID_SetMagneticFieldAck = 0x6B, - - XMID_KeepAlive = 0x70, - XMID_KeepAliveAck = 0x71, - XMID_ReqConnectionSettings = 0x78, - XMID_ReqConnectionSettingsAck = 0x79, - XMID_SetConnectionSettings = 0x78, - XMID_SetConnectionSettingsAck = 0x79, - - XMID_CloseConnection = 0x72, - XMID_CloseConnectionAck = 0x73, - - XMID_IccCommand = 0x74, - XMID_IccCommandAck = 0x75, - - XMID_ReqGnssPlatform = 0x76, - XMID_ReqGnssPlatformAck = 0x77, - XMID_SetGnssPlatform = 0x76, - XMID_SetGnssPlatformAck = 0x77, - - XMID_BodyPackBundle = 0x7A, - XMID_BodyPackBundleAck = 0x7B, - - XMID_ReqStationOptions = 0x7C, - XMID_ReqStationOptionsAck = 0x7D, - - XMID_ReqLocationId = 0x84, - XMID_ReqLocationIdAck = 0x85, - XMID_SetLocationId = 0x84, - XMID_SetLocationIdAck = 0x85, - - XMID_ReqExtOutputMode = 0x86, - XMID_ReqExtOutputModeAck = 0x87, - XMID_SetExtOutputMode = 0x86, - XMID_SetExtOutputModeAck = 0x87, - - XMID_ReqStringOutputType = 0x8E, - XMID_ReqStringOutputTypeAck = 0x8F, - XMID_SetStringOutputType = 0x8E, - XMID_SetStringOutputTypeAck = 0x8F, - - // XbusMaster - XMID_ReqBatteryLevel = 0x88, - XMID_Batterylevel = 0x89, - // End XbusMaster - - XMID_ReqInitTrackMode = 0x88, - XMID_ReqInitTrackModeAck = 0x89, - XMID_SetInitTrackMode = 0x88, - XMID_SetInitTrackModeAck = 0x89, - - XMID_ReqMasterSettings = 0x8A, - XMID_MasterSettings = 0x8B, - - XMID_StoreFilterState = 0x8A, - XMID_StoreFilterStateAck = 0x8B, - - XMID_ReqEmts = 0x90, - XMID_EmtsData = 0x91, - XMID_UpdateFilterProfile = 0x92, - XMID_UpdateFilterProfileAck = 0x93, - - XMID_RestoreEmts = 0x94, - XMID_RestoreEmtsAck = 0x95, - XMID_StoreEmts = 0x96, - XMID_StoreEmtsAck = 0x97, - - XMID_AdjustUtcTime = 0xA8, - XMID_AdjustUtcTimeAck = 0xA9, - - XMID_ReqActiveClockCorrection = 0x9C, - XMID_ActiveClockCorrection = 0x9D, - XMID_StoreActiveClockCorrection = 0x9E, - XMID_StoreActiveClockCorrectionAck = 0x9F, - - XMID_ReqFilterSettings = 0xA0, - XMID_ReqFilterSettingsAck = 0xA1, - XMID_SetFilterSettings = 0xA0, - XMID_SetFilterSettingsAck = 0xA1, - XMID_ReqAmd = 0xA2, - XMID_ReqAmdAck = 0xA3, - XMID_SetAmd = 0xA2, - XMID_SetAmdAck = 0xA3, - XMID_ResetOrientation = 0xA4, - XMID_ResetOrientationAck = 0xA5, - - XMID_ReqGnssStatus = 0xA6, - XMID_GnssStatus = 0xA7, - XMID_ReqGpsStatus = XMID_ReqGnssStatus,//!< \deprecated - XMID_GpsStatus = XMID_GnssStatus, //!< \deprecated - - XMID_ReqComponentsInformation = 0xAA, - XMID_ComponentsInformation = 0xAB, - - XMID_ReqAccessControlList = 0xAE, - XMID_AccessControlList = 0xAF, - XMID_SetAccessControlList = 0xAE, - XMID_SetAccessControlListAck = 0xAF, - - // Wireless - XMID_ScanChannels = 0xB0, - XMID_ScanChannelsAck = 0xB1, - XMID_EnableMaster = 0xB2, - XMID_EnableMasterAck = 0xB3, - XMID_DisableMaster = 0xB4, - XMID_DisableMasterAck = 0xB5, - XMID_ReqRadioChannel = 0xB6, - XMID_ReqRadioChannelAck = 0xB7, - XMID_SetClientPriority = 0xB8, - XMID_SetClientPriorityAck = 0xB9, - XMID_ReqClientPriority = 0xB8, - XMID_ReqClientPriorityAck = 0xB9, - XMID_SetWirelessConfig = 0xBA, - XMID_SetWirelessConfigAck = 0xBB, - XMID_ReqWirelessConfig = 0xBA, - XMID_ReqWirelessConfigAck = 0xBB, - XMID_UpdateBias = 0xBC, - XMID_UpdateBiasAck = 0xBD, - XMID_ToggleIoPins = 0xBE, - XMID_ToggleIoPinsAck = 0xBF, - - XMID_GotoOperational = 0xC0, - XMID_GotoOperationalAck = 0xC1, - - XMID_SetTransportMode = 0xC2, - XMID_SetTransportModeAck = 0xC3, - XMID_ReqTransportMode = 0xC2, - XMID_ReqTransportModeAck = 0xC3, - - XMID_AcceptMtw = 0xC4, - XMID_AcceptMtwAck = 0xC5, - XMID_RejectMtw = 0xC6, - XMID_RejectMtwAck = 0xC7, - XMID_InfoRequest = 0xC8, - XMID_InfoRequestAck = 0xC9, - - XMID_ReqFrameRates = 0xCA, - XMID_ReqFrameRatesAck = 0xCB, - - XMID_StartRecording = 0xCC, - XMID_StartRecordingAck = 0xCD, - XMID_StopRecording = 0xCE, - XMID_StopRecordingAck = 0xCF, - // End Wireless - - XMID_ReqOutputConfiguration = 0xC0, - XMID_ReqOutputConfigurationAck = 0xC1, - XMID_SetOutputConfiguration = 0xC0, - XMID_SetOutputConfigurationAck = 0xC1, - - XMID_ReqOutputMode = 0xD0, - XMID_ReqOutputModeAck = 0xD1, - XMID_SetOutputMode = 0xD0, - XMID_SetOutputModeAck = 0xD1, - - XMID_ReqOutputSettings = 0xD2, - XMID_ReqOutputSettingsAck = 0xD3, - XMID_SetOutputSettings = 0xD2, - XMID_SetOutputSettingsAck = 0xD3, - - XMID_ReqOutputSkipFactor = 0xD4, - XMID_ReqOutputSkipFactorAck = 0xD5, - XMID_SetOutputSkipFactor = 0xD4, - XMID_SetOutputSkipFactorAck = 0xD5, - - XMID_ReqSyncInSettings = 0xD6, - XMID_ReqSyncInSettingsAck = 0xD7, - XMID_SetSyncInSettings = 0xD6, - XMID_SetSyncInSettingsAck = 0xD7, - - XMID_ReqSyncOutSettings = 0xD8, - XMID_ReqSyncOutSettingsAck = 0xD9, - XMID_SetSyncOutSettings = 0xD8, - XMID_SetSyncOutSettingsAck = 0xD9, - - XMID_ReqErrorMode = 0xDA, - XMID_ReqErrorModeAck = 0xDB, - XMID_SetErrorMode = 0xDA, - XMID_SetErrorModeAck = 0xDB, - - XMID_ReqTransmitDelay = 0xDC, - XMID_ReqTransmitDelayAck = 0xDD, - XMID_SetTransmitDelay = 0xDC, - XMID_SetTransmitDelayAck = 0xDD, - - XMID_SetMfmResults = 0xDE, - XMID_SetMfmResultsAck = 0xDF, - - XMID_ReqObjectAlignment = 0xE0, - XMID_ReqObjectAlignmentAck = 0xE1, - XMID_SetObjectAlignment = 0xE0, - XMID_SetObjectAlignmentAck = 0xE1, - - XMID_ReqAlignmentRotation = 0xEC, - XMID_ReqAlignmentRotationAck = 0xED, - XMID_SetAlignmentRotation = 0xEC, - XMID_SetAlignmentRotationAck = 0xED, - - XMID_ExtensionReserved1 = 0xEE, - XMID_ExtensionReserved2 = 0xEF, - - XMID_SetDeviceIdContext = 0xFE, - XMID_SetDeviceIdContextAck = 0xFF -}; -/*! @} */ -typedef enum XsXbusMessageId XsXbusMessageId; - -#endif diff --git a/extern/xcommunication/enumerateusbdevices.cpp b/extern/xcommunication/enumerateusbdevices.cpp deleted file mode 100644 index 90a8b4f..0000000 --- a/extern/xcommunication/enumerateusbdevices.cpp +++ /dev/null @@ -1,282 +0,0 @@ -/* Copyright (c) 2003-2017 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. -*/ - -#include "xcommunicationconfig.h" -#include "enumerateusbdevices.h" - -#ifdef _WIN32 -# include -# include -# include -# include -# include -# include -# include -#else -# 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) -{ - char deviceInstanceID[MAX_DEVICE_ID_LEN]; - SetupDiGetDeviceInstanceIdA(hDevInfo, DeviceInfoData,deviceInstanceID, MAX_DEVICE_ID_LEN, NULL); - return std::string(deviceInstanceID); -} - - -/* Get the first match for _regex_ within _devpath_ */ -static inline std::string searchOne(std::string const& devpath, std::string const& regex) -{ - std::smatch sm; - std::regex_search(devpath, sm, std::regex(regex)); - if (sm.size() < 2) - return std::string(); - return sm[1]; -} - -/* Fetch the vendor ID from the given device path */ -static inline uint16_t vidFromDevPath(std::string const& devpath) -{ - try - { - auto t1 = searchOne(devpath, "VID_([0-9a-fA-F]{4}).*PID_.*"); - if (t1.empty()) // prevent unnecessary exceptions - return 0; - return static_cast(std::stoi(t1, nullptr, 16)); - } - catch (std::invalid_argument &) - { - return 0; - } -} - -/* Fetch the product ID from the given device path */ -static inline uint16_t pidFromDevPath(std::string const& devpath) -{ - try - { - auto t1 = searchOne(devpath, "VID_.*PID_([0-9a-fA-F]{4})"); - if (t1.empty()) // prevent unnecessary exceptions - return 0; - return static_cast(std::stoi(t1, nullptr, 16)); - } - catch (std::invalid_argument &) - { - return 0; - } -} -#endif - -/*! \brief Enumerate Xsens USB devices - - If the OS already has drivers running for a device, the device should already have been - found by xsEnumerateSerialPorts(). - - \param[in,out] ports The list of serial ports to append to -*/ -bool xsEnumerateUsbDevices(XsPortInfoList& ports) -{ - XsPortInfo current; -#ifdef USE_WINUSB - BOOL bResult = FALSE; - ULONG length; - ULONG requiredLength=0; - - // {FD51225C-700A-47e5-9999-B2D9031B88ED} - GUID guid = { 0xfd51225c, 0x700a, 0x47e5, { 0x99, 0x99, 0xb2, 0xd9, 0x3, 0x1b, 0x88, 0xed } }; - - HDEVINFO deviceInfo; - SP_DEVICE_INTERFACE_DATA interfaceData; - PSP_DEVICE_INTERFACE_DETAIL_DATA_A detailData = NULL; - - deviceInfo = SetupDiGetClassDevs(&guid, NULL, NULL, DIGCF_PRESENT | DIGCF_DEVICEINTERFACE); - - // Initialize variables. - interfaceData.cbSize = sizeof(SP_INTERFACE_DEVICE_DATA); - int port = 0; - //lint --e{441} irregular for loop - for (DWORD dwIndex = 0; port == 0; ++dwIndex) - { - BOOL bRet = SetupDiEnumDeviceInterfaces( deviceInfo, NULL, &guid, dwIndex, &interfaceData); - if (!bRet) - { - if (GetLastError() == ERROR_NO_MORE_ITEMS) - break; - } - else - { - if (!SetupDiGetDeviceInterfaceDetail(deviceInfo, &interfaceData, NULL, 0, &requiredLength, NULL)) - { - if (GetLastError() != ERROR_INSUFFICIENT_BUFFER) - { - SetupDiDestroyDeviceInfoList(deviceInfo); - return false; - } - } - detailData = (PSP_DEVICE_INTERFACE_DETAIL_DATA_A)LocalAlloc(LMEM_FIXED, requiredLength); - if (NULL == detailData) - { - SetupDiDestroyDeviceInfoList(deviceInfo); - return false; - } - - detailData->cbSize = sizeof(SP_DEVICE_INTERFACE_DETAIL_DATA_A); - length = requiredLength; - SP_DEVINFO_DATA DevInfoData; - DevInfoData.cbSize = sizeof(SP_DEVINFO_DATA); - bResult = SetupDiGetDeviceInterfaceDetailA(deviceInfo, &interfaceData, detailData, length, &requiredLength, &DevInfoData); - - if (!bResult) - { - LocalFree(detailData); - SetupDiDestroyDeviceInfoList(deviceInfo); - return false; - } - - unsigned char serialNumber[256]; - char* ptrEnd, *ptrStart = strchr(detailData->DevicePath, '#'); - if (!ptrStart) - continue; - ptrStart = strchr(ptrStart+1, '#'); - if (!ptrStart) - continue; - ptrEnd = strchr(ptrStart+1, '#'); - if (!ptrEnd) - continue; - - strncpy((char*)serialNumber, ptrStart+1, ptrEnd-ptrStart-1); - serialNumber[ptrEnd-ptrStart-1] = '\0'; - - current.setPortName(detailData->DevicePath); - - int id = 0; - sscanf((const char *)serialNumber, "%X", &id); - current.setDeviceId((uint32_t) id); - - std::string devpath = getDevicePath(deviceInfo, &DevInfoData); - uint16_t vid = vidFromDevPath(devpath); - uint16_t pid = pidFromDevPath(devpath); - current.setVidPid(vid, pid); - - ports.push_back(current); - } - } - - SetupDiDestroyDeviceInfoList(deviceInfo); - return true; -#elif defined(HAVE_LIBUSB) - XsLibUsb libUsb; - libusb_context *context; - int result = libUsb.init(&context); - if (result != LIBUSB_SUCCESS) - return true; - - 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_descriptor desc; - result = libUsb.get_device_descriptor(device, &desc); - if (result != LIBUSB_SUCCESS) - continue; - - if (desc.idVendor != XSENS_VENDOR_ID && desc.idVendor != ATMEL_VENDOR_ID) - continue; - - libusb_device_handle *handle; - result = libUsb.open(device, &handle); - if (result != LIBUSB_SUCCESS) - continue; - - unsigned char serialNumber[256]; - result = libUsb.get_string_descriptor_ascii(handle, desc.iSerialNumber, serialNumber, 256); - - if (desc.idVendor == ATMEL_VENDOR_ID && desc.idProduct == ATMEL_BORROWED_PRODUCT_ID) - { - unsigned char productName[256]; - result = libUsb.get_string_descriptor_ascii(handle, desc.iProduct, productName, 256); - - if (strcmp("Xsens COM port", (const char *)productName) != 0) - { - libUsb.close(handle); - continue; - } - } - - libusb_config_descriptor *configDesc; - result = libUsb.get_active_config_descriptor(device, &configDesc); - if (result != LIBUSB_SUCCESS) - { - libUsb.close(handle); - continue; - } - - bool kernelActive = false; - for (uint8_t ifCount = 0; ifCount < configDesc->bNumInterfaces; ++ifCount) { - int res = libUsb.kernel_driver_active(handle, ifCount); - kernelActive |= (res == 1); - } - - libUsb.free_config_descriptor(configDesc); - - if (!kernelActive) - { - char name[256]; - sprintf(name, "USB%03u:%03u", libUsb.get_bus_number(device), - libUsb.get_device_address(device)); - current.setPortName(name); - - int id = 0; - sscanf((const char *)serialNumber, "%X", &id); - current.setDeviceId((uint32_t) id); - current.setVidPid(desc.idVendor, desc.idProduct); - ports.push_back(current); - } - else - { - JLDEBUG(gJournal, "Kernel driver active on USB" << - libUsb.get_bus_number(device) << ":" << libUsb.get_device_address(device) << - " device " << serialNumber); - - } - libUsb.close(handle); - } - libUsb.free_device_list(deviceList, 1); - libUsb.exit(context); - return true; -#else - (void)ports; - return false; -#endif -} diff --git a/extern/xcommunication/int_xsdatapacket.cpp b/extern/xcommunication/int_xsdatapacket.cpp deleted file mode 100644 index a7a7ff4..0000000 --- a/extern/xcommunication/int_xsdatapacket.cpp +++ /dev/null @@ -1,203 +0,0 @@ -/* Copyright (c) 2003-2017 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. -*/ - -#include -#include "int_xsdatapacket.h" -#include "legacydatapacket.h" -#include "packetfixeddata.h" -#include "mtwsdidata.h" -#include -#include -#include -#include - -/*! \class XsDataPacket - \brief Contains data received from a device or read from a file - - This class is used by XDA for easy access to data contained in a message. It analyzes its internal - XsMessage upon contruction to give access to the individual contained pieces of data. - New data can also be added to the %XsDataPacket or updated if it already existed. -*/ -extern "C" { - -void validatePacket(XsDataPacket* thisPtr); - -void XsDataPacket_assignFromLegacyDataPacket(struct XsDataPacket* thisPtr, struct LegacyDataPacket const* legacy, int index) -{ - assert(legacy); - - if (!legacy->isXbusSystem()) - index = 0; - - thisPtr->clear(); - - thisPtr->setDeviceId(legacy->deviceId(index)); - thisPtr->setTimeOfArrival(legacy->timeOfArrival()); - thisPtr->setPacketId(legacy->largePacketCounter()); - XsDataFormat format = legacy->dataFormat(index); - if (legacy->containsRawData(index)) - thisPtr->setRawData(legacy->rawData(index)); - if (legacy->rawTemperatureChannelCount(index) == 4) - { - XsUShortVector rawGyroTemperatures; - rawGyroTemperatures[0] = legacy->rawTemperature(index, 1); - rawGyroTemperatures[1] = legacy->rawTemperature(index, 2); - rawGyroTemperatures[2] = legacy->rawTemperature(index, 3); - thisPtr->setRawGyroscopeTemperatureData(rawGyroTemperatures); - } - if (legacy->containsCalibratedAcceleration(index)) - thisPtr->setCalibratedAcceleration(legacy->calibratedAcceleration(index)); - if (legacy->containsCalibratedGyroscopeData(index)) - { - //\todo Fix legacy watermarking -// XsDataIdentifier di; -// switch (format.m_outputSettings & XOS_Dataformat_Mask) { -// case XOS_Dataformat_Float: -// di = XDI_SubFormatFloat; -// break; -// case XOS_Dataformat_Double: -// di = XDI_SubFormatDouble; -// break; -// case XOS_Dataformat_Fp1632: -// di = XDI_SubFormatFp1632; -// break; -// case XOS_Dataformat_F1220: -// di = XDI_SubFormatFp1220; -// break; -// default: -// di = XDI_None; -// break; -// } - // Special copy to preserve watermarking - //thisPtr->message().setDataShort(XDI_RateOfTurn | di, thisPtr->message().getDataSize()); - //thisPtr->message().setDataByte(3*thisPtr->getFPValueSize(di), thisPtr->message().getDataSize()); - //thisPtr->itemCount()++; - //thisPtr->message().setDataBuffer(thisPtr->legacyMsg().getDataBuffer(info.m_calGyr), 3*thisPtr->getFPValueSize(di), XsDataPacket_itemOffsetExact(thisPtr, XDI_RateOfTurn | di)); - thisPtr->setCalibratedGyroscopeData(legacy->calibratedGyroscopeData(index)); - } - if (legacy->containsCalibratedMagneticField(index)) - thisPtr->setCalibratedMagneticField(legacy->calibratedMagneticField(index)); - if (legacy->containsOrientationQuaternion(index)) - thisPtr->setOrientationQuaternion(legacy->orientationQuaternion(index), (format.m_outputSettings & XOS_Coordinates_Ned) ? XDI_CoordSysNed : XDI_CoordSysNwu); - if (legacy->containsOrientationEuler(index)) - thisPtr->setOrientationEuler(legacy->orientationEuler(index), (format.m_outputSettings & XOS_Coordinates_Ned) ? XDI_CoordSysNed : XDI_CoordSysNwu); - if (legacy->containsOrientationMatrix(index)) - thisPtr->setOrientationMatrix(legacy->orientationMatrix(index), (format.m_outputSettings & XOS_Coordinates_Ned) ? XDI_CoordSysNed : XDI_CoordSysNwu); - if (legacy->containsPositionLLA(index)) - thisPtr->setPositionLLA(legacy->positionLLA(index)); - if (legacy->containsVelocity(index)) - thisPtr->setVelocity(legacy->velocity(index), (format.m_outputSettings & XOS_Coordinates_Ned) ? XDI_CoordSysNed : XDI_CoordSysNwu); - if (legacy->containsStatus(index)) - { - bool isDetailed = true; - uint32_t status = legacy->status(index, &isDetailed); - - // For MTw's only, the status needs to be swapEndian32-ed - if (thisPtr->deviceId().isMtw1()) - { - status = swapEndian32(status); - } - - if (isDetailed) - { - thisPtr->setStatus(status); - } - else - { - thisPtr->setStatusByte((uint8_t) status); - } - } - if (legacy->containsGpsPvtData()) - { - XsPressure tmp; - tmp.m_pressure = legacy->gpsPvtData(index).m_pressure * 2.0; // Convert to Pascal - tmp.m_pressureAge = legacy->gpsPvtData(index).m_pressureAge; - thisPtr->setPressure(tmp); - XsGpsPvtData pvt; - pvt = legacy->gpsPvtData(index); - thisPtr->setGpsPvtData(pvt); - } - if (legacy->containsUtcTime(index)) - { - XsUtcTime time = legacy->utcTime(index); - thisPtr->setUtcTime(time); - } - if (legacy->containsSampleTimeFine(index)) - { - uint32_t sampleTimeFine = legacy->sampleTimeFine(index); - thisPtr->setSampleTimeFine(sampleTimeFine); - } - - // packet counter MUST go before anything that sets a frame range! - if (legacy->containsPacketCounter(index)) - thisPtr->setPacketCounter(legacy->packetCounter(index)); - - if (legacy->containsMtwSdiData(index)) - { - // not yet converted: - //uint8_t m_timeSync; - //XsVector3 m_currentBias; - - MtwSdiData mtwsdi = legacy->mtwSdiData(index); - thisPtr->setDeviceId(mtwsdi.m_deviceId); - { - XsSdiData tmp(mtwsdi.orientationIncrement(), mtwsdi.velocityIncrement()); - thisPtr->setSdiData(tmp); - - //// Special copy to preserve watermarking - //thisPtr->message().setDataShort(XDI_DeltaQ | di, thisPtr->message().getDataSize()); - //thisPtr->message().setDataByte(4*thisPtr->getFPValueSize(di), thisPtr->message().getDataSize()); - //thisPtr->itemCount()++; - //thisPtr->message().setDataBuffer(thisPtr->legacyMsg().getDataBuffer(info.m_wOrientationIncrement), 4*thisPtr->getFPValueSize(di), XsDataPacket_itemOffsetExact(thisPtr, XDI_DeltaQ | di)); - - //thisPtr->message().setDataShort(XDI_DeltaV | di, thisPtr->message().getDataSize()); - //thisPtr->message().setDataByte(3*thisPtr->getFPValueSize(di), thisPtr->message().getDataSize()); - //thisPtr->itemCount()++; - //thisPtr->message().setDataBuffer(thisPtr->legacyMsg().getDataBuffer(info.m_wVelocityIncrement), 3*thisPtr->getFPValueSize(di), XsDataPacket_itemOffsetExact(thisPtr, XDI_DeltaV | di)); - } - - //if (mtwsdi.m_aidingData) - thisPtr->setCalibratedMagneticField(mtwsdi.m_magnetoMeter); - if (mtwsdi.m_barometer) - { - XsPressure tmp; - tmp.m_pressure = mtwsdi.m_barometer * 100.0; // convert from millibar to pascal - tmp.m_pressureAge = mtwsdi.m_barometer?0:255; - thisPtr->setPressure(tmp); - } - thisPtr->setFrameRange(XsRange((int) mtwsdi.m_firstFrameNumber, (int) mtwsdi.m_lastFrameNumber)); - thisPtr->setRssi(mtwsdi.m_rssi); - } - if (legacy->containsTemperature(index)) - thisPtr->setTemperature(legacy->temperature(index)); - - // enable this when you experience weird crashes in XsByteArray_destruct or XsDataPacket_destruct - //validatePacket(thisPtr); -} - -} // extern "C" diff --git a/extern/xcommunication/iointerface.cpp b/extern/xcommunication/iointerface.cpp deleted file mode 100644 index 42b476d..0000000 --- a/extern/xcommunication/iointerface.cpp +++ /dev/null @@ -1,167 +0,0 @@ -/* Copyright (c) 2003-2017 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. -*/ - -#include -#include "iointerface.h" -#include -#ifndef _WIN32 -# include // close -# include // ioctl -# include // open, O_RDWR -# include // strcpy -# include -# include -#else -# include -# include -#endif - -#ifndef _CRT_SECURE_NO_DEPRECATE -# define _CRT_SECURE_NO_DEPRECATE -# ifdef _WIN32 -# pragma warning(disable:4996) -# endif -#endif - -/*! \class IoInterface - \brief An abstract IO interface - \details An %IoInterface provides a generic interface for dealing with an interface device such - as a file, a com port or a USB port. - \note The class is not thread safe. -*/ - -// IOInterface functions -/*! \copydoc SerialInterface::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) -{ - (void) ms; - return XRV_INVALIDOPERATION; -} -/*! \copydoc SerialInterface::waitForData(XsFilePos, XsByteArray&) */ -XsResultValue IoInterface::waitForData (XsFilePos maxLength, XsByteArray& data) -{ - (void) maxLength; - (void) data; - return XRV_INVALIDOPERATION; -} -/*! \copydoc SerialInterface::cancelIo(void) const */ -void IoInterface::cancelIo(void) const -{ - -} - -// IOInterfaceFile functions -/*! \copydoc IoInterfaceFile::appendData() */ -XsResultValue IoInterface::appendData(const XsByteArray& bdata) -{ - (void) bdata; - return XRV_INVALIDOPERATION; -} -/*! \copydoc IoInterfaceFile::closeAndDelete() */ -XsResultValue IoInterface::closeAndDelete(void) -{ - return XRV_INVALIDOPERATION; -} -/*! \copydoc IoInterfaceFile::create(const XsString&) */ -XsResultValue IoInterface::create(const XsString& filename) -{ - (void) filename; - return XRV_INVALIDOPERATION; -} -/*! \copydoc IoInterfaceFile::deleteData(XsFilePos, XsFilePos) */ -XsResultValue IoInterface::deleteData(XsFilePos start, XsFilePos length) -{ - (void) start; - (void) length; - return XRV_INVALIDOPERATION; -} -/*! \copydoc IoInterfaceFile::find(const XsByteArray&, XsFilePos&) */ -XsResultValue IoInterface::find(const XsByteArray& needleV, XsFilePos& pos) -{ - (void) needleV; - (void) pos; - return XRV_INVALIDOPERATION; -} -/*! \copydoc IoInterfaceFile::getFileSize() const */ -XsFilePos IoInterface::getFileSize(void) const -{ - return 0; -} -/*! \copydoc IoInterfaceFile::getName(XsString&) const */ -XsResultValue IoInterface::getName(XsString& filename) const -{ - (void) filename; - return XRV_INVALIDOPERATION; -} -/*! \copydoc IoInterfaceFile::getReadPosition() const */ -XsFilePos IoInterface::getReadPosition(void) const -{ - return 0; -} -/*! \copydoc IoInterfaceFile::getWritePosition() const */ -XsFilePos IoInterface::getWritePosition(void) const -{ - return 0; -} -/*! \copydoc IoInterfaceFile::insertData(XsFilePos, const XsByteArray&) */ -XsResultValue IoInterface::insertData(XsFilePos start, const XsByteArray& data) -{ - (void) start; - (void) data; - return XRV_INVALIDOPERATION; -} -/*! \copydoc IoInterfaceFile::isReadOnly() const */ -bool IoInterface::isReadOnly(void) const -{ - return true; -} -/*! \copydoc IoInterfaceFile::open(const XsString&, bool, bool) */ -XsResultValue IoInterface::open(const XsString& filename, bool createNew, bool readOnly) -{ - (void) filename; - (void) createNew; - (void) readOnly; - return XRV_INVALIDOPERATION; -} -/*! \copydoc IoInterfaceFile::setReadPosition(XsFilePos) */ -XsResultValue IoInterface::setReadPosition(XsFilePos pos) -{ - (void) pos; - return XRV_INVALIDOPERATION; -} -/*! \copydoc IoInterfaceFile::setWritePosition(XsFilePos) */ -XsResultValue IoInterface::setWritePosition(XsFilePos pos) -{ - (void) pos; - return XRV_INVALIDOPERATION; -} diff --git a/extern/xcommunication/iointerfacefile.cpp b/extern/xcommunication/iointerfacefile.cpp deleted file mode 100644 index 78678f9..0000000 --- a/extern/xcommunication/iointerfacefile.cpp +++ /dev/null @@ -1,824 +0,0 @@ -/* Copyright (c) 2003-2017 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. -*/ - -#include "iointerfacefile.h" - -#include -#ifndef _WIN32 -# include // close -# include // ioctl -# include // open, O_RDWR -# include // strcpy -# include -# include -# include -#else -# include -# include -# include -#endif - -#include - -#ifndef _CRT_SECURE_NO_DEPRECATE -# define _CRT_SECURE_NO_DEPRECATE -# ifdef _WIN32 -# pragma warning(disable:4996) -# endif -#endif - -static const XsFilePos fileBlockSize = 4096; - -/*! Default constructor, initializes all members to their default values. -*/ -IoInterfaceFile::IoInterfaceFile() - : m_handle(NULL) - , m_fileSize(0) - , m_readPos(0) - , m_writePos(0) - , m_lastResult(XRV_OK) - , m_reading(true) - , m_readOnly(false) -{ -} - -/*! Destructor -*/ -IoInterfaceFile::~IoInterfaceFile() -{ - try { - closeFile(); //lint !e534 - } catch(...) - {} -} - -/*! \brief Write data to the end of the file. - \details The function writes the given data to the file at the end. The current write - position is also moved to the end of the file. - \param bdata The byte data to append to the file - \returns XRV_OK if the write was successful -*/ -XsResultValue IoInterfaceFile::appendData(const XsByteArray& bdata) -{ - if (!m_handle) - return m_lastResult = XRV_NOFILEOPEN; - if (m_readOnly) - return m_lastResult = XRV_READONLY; - if (!bdata.size()) - return m_lastResult = XRV_OK; - - if (m_reading || m_writePos != m_fileSize) - { - m_reading = false; - m_handle->seek_r(0); //lint !e534 - } - XsFilePos bytesWritten = m_handle->write(bdata.data(), 1, bdata.size()); - m_writePos = m_handle->tell(); - m_fileSize = m_writePos; - - return (m_lastResult = XRV_OK); -} - -////////////////////////////////////////////////////////////////////////////////////////// -/*! \brief Close the file, overrides IoInterface::close(). - \returns XRV_OK if the file was closed successfully -*/ -XsResultValue IoInterfaceFile::close() -{ - return closeFile(); -} - -/*! \brief Close the file. - \returns XRV_OK if the file was closed successfully -*/ -XsResultValue IoInterfaceFile::closeFile() -{ - if (m_handle) - { - m_handle->flush(); - m_handle->close(); - - delete m_handle; - m_handle = NULL; - } - - m_readPos = 0; - m_writePos = 0; - m_reading = true; - m_fileSize = 0; - m_readOnly = false; - - return m_lastResult = XRV_OK; -} - -/*! \brief Close the file and delete it. - \returns XRV_OK if the file was closed and deleted successfully -*/ -XsResultValue IoInterfaceFile::closeAndDelete() -{ - if (m_handle) - { - m_handle->flush(); - m_handle->close(); - - if (m_readOnly) - m_lastResult = XRV_READONLY; - else - m_lastResult = XsFile::erase(m_filename); - - delete m_handle; - m_handle = NULL; - } - else - m_lastResult = XRV_NOFILEOPEN; - - m_readPos = 0; - m_writePos = 0; - m_reading = true; - m_fileSize = 0; - m_readOnly = false; - - return m_lastResult; -} - -/*! \brief Create an empty file - \param filename The desired (path+)name of the file - \returns XRV_OK if the file was created successfully -*/ -XsResultValue IoInterfaceFile::create(const XsString& filename) -{ - if (m_handle) - return m_lastResult = XRV_ALREADYOPEN; - - m_handle = new XsFile(); - m_lastResult = m_handle->create(filename, false); - if (m_lastResult != XRV_OK) - { - delete m_handle; - m_handle = NULL; - return m_lastResult; - } - - m_lastResult = XsFile::fullPath(filename, m_filename); - - if (m_lastResult != XRV_OK) - { - m_handle->close(); - XsFile::erase(m_filename); - delete m_handle; - m_handle = NULL; - - return m_lastResult = XRV_INVALIDPARAM; - } - - m_readPos = 0; - m_writePos = 0; - m_fileSize = 0; - m_reading = true; - m_readOnly = false; - return m_lastResult = XRV_OK; -} - -/*! \brief Delete the given data from the file. - \details The function erases the given data from the file at the given write position. This - operation may take a while to complete, but is faster than insertData. - - The write position is not changed and the read position is checked for validity upon function exit. - \param start The offset of the first byte to delete - \param length The total number of bytes to delete - \returns XRV_OK if the data was deleted successfully -*/ -XsResultValue IoInterfaceFile::deleteData(XsFilePos start, XsFilePos length) -{ - if (!m_handle) - return m_lastResult = XRV_NOFILEOPEN; - if (m_readOnly) - return m_lastResult = XRV_READONLY; - - gotoWrite(); - - XsFilePos wPos = start; - XsFilePos rPos = wPos + length; - - XsFilePos read1; - XsFilePos endPos = (start + (XsFilePos) length); - if (endPos < m_fileSize) - { - XsFilePos remaining = m_fileSize - endPos; - char buffer[m_fileBlockSize]; - - // copy data - m_handle->seek(rPos); - - while (remaining > 0) - { - if (remaining >= m_fileBlockSize) - read1 = m_handle->read(buffer, 1, m_fileBlockSize); - else - read1 = m_handle->read(buffer, 1, (size_t) remaining); - - remaining -= read1; - rPos += read1; - - // write block to the correct position - m_handle->seek(wPos); - wPos += m_handle->write(buffer, 1, read1); - m_handle->seek(rPos); - } - m_fileSize -= length; - } - else - { - m_fileSize = start; - } - - XsResultValue truncateResult = m_handle->truncate(m_fileSize); - - m_writePos = start; - m_handle->seek(wPos); - - return m_lastResult = truncateResult; -} - -/*! \brief Find a string of bytes in the file - \details The function searches from the current read position until the given \c needle is - found. If the needle is not found, XsResultValue::NOT_FOUND is returned. The function - will update the seek position to the first character of the found needle. - \param needleV The byte string to find. - \param pos The position where \a needleV was found. This will point to the first character - of the found \a needleV. - \returns XRV_OK if the data was found, XRV_ENDOFFILE if it wasn't found -*/ -XsResultValue IoInterfaceFile::find(const XsByteArray& needleV, XsFilePos& pos) -{ - if (!m_handle) - return m_lastResult = XRV_NOFILEOPEN; - - XsFilePos needleLength = needleV.size(); - - pos = 0; - if (needleLength == 0) - return m_lastResult = XRV_OK; - - const char* needle = (const char*) needleV.data(); - - gotoRead(); - - char buffer[m_fileBlockSize]; - 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)))); - else - readBytes = m_handle->read(buffer, 1, m_fileBlockSize); // read a block of data - - while (readBytes > 0) - { - m_readPos += readBytes; - bufferPos = 0; - - while (bufferPos < readBytes && needlePos < needleLength) - { - if (buffer[bufferPos] == needle[needlePos]) - { - // found a byte - ++needlePos; - } - else - { - if (needlePos > 0) - needlePos = 0; - else - if (buffer[bufferPos] == needle[0]) - { - // found a byte - needlePos = 1; - } - } - ++bufferPos; - } - if (needlePos < needleLength) - readBytes = m_handle->read(buffer, 1, m_fileBlockSize); // read next block - else - { - m_readPos = m_readPos + bufferPos - readBytes - needleLength; // or without needleLength - pos = m_readPos; // - needleLength; - m_handle->seek(m_readPos); - return m_lastResult = XRV_OK; - } - } - return m_lastResult = XRV_ENDOFFILE; -} - -/*! \brief Return the size of the file. - \returns The size of the file. -*/ -XsFilePos IoInterfaceFile::getFileSize() const -{ - return m_fileSize; -} - -/*! \brief Return the creation date of the file - \returns The creation date of the file -*/ -XsTimeStamp IoInterfaceFile::getFileDate() const -{ -#ifdef _WIN32 - struct _stat stats; - if (_wstat(m_filename.toStdWString().c_str(), &stats) == 0) -#else - struct stat stats; - if (stat(m_filename.c_str(), &stats) == 0) -#endif - { - XsTimeStamp t = XsTimeStamp( (int64_t)stats.st_mtime * 1000); - return t; - } - return XsTimeStamp(); -} - -/*! \copydoc IoInterface::flushData */ -XsResultValue IoInterfaceFile::flushData () -{ - m_handle->flush(); - - return m_lastResult = XRV_OK; -} - -/*! \brief Retrieve the filename that was last successfully opened. - - \param filename The XsString which will contain the filename. - \returns XRV_OK -*/ -XsResultValue IoInterfaceFile::getName(XsString& filename) const -{ - filename = m_filename; - return m_lastResult = XRV_OK; -} - -/*! \brief Return the filename that was last successfully opened. -*/ -XsString IoInterfaceFile::getFileName() const -{ - return m_filename; -} - -//! \brief Change from writing to reading mode -void IoInterfaceFile::gotoRead() -{ - if (m_reading) - return; - - m_handle->flush(); - m_handle->seek(m_readPos); - m_reading = true; -} - -//! \brief Change from reading to writing mode -void IoInterfaceFile::gotoWrite() -{ - if (!m_reading) - return; - - m_handle->flush(); - m_handle->seek(m_writePos); - m_reading = false; -} - -/*! \brief Insert the given data into the file. - \details The function writes the given data to the file at the current write position. This - operation may take a while to complete. - - The write position is placed at the end of the inserted data. - \param start The offset in the file to write the first byte - \param data The data to insert in the file - \returns XRV_OK if the data was inserted successfully -*/ -XsResultValue IoInterfaceFile::insertData(XsFilePos start, const XsByteArray& data) -{ - if (!m_handle) - return m_lastResult = XRV_NOFILEOPEN; - if (m_readOnly) - return m_lastResult = XRV_READONLY; - - gotoWrite(); - - XsFilePos length = data.size(); - XsFilePos rPos = start; - XsFilePos wPos = rPos + length; - - XsFilePos read1, read2; - XsFilePos remaining = m_fileSize - start; - XsFilePos bsize = (length > m_fileBlockSize) ? length : m_fileBlockSize; - char* bufferRoot = (char*) malloc((XsSize) (bsize*2)); - if (!bufferRoot) - return XRV_OUTOFMEMORY; - char* buffer1 = bufferRoot; - char* buffer2 = bufferRoot+bsize; - char* btemp; - - // copy data - m_handle->seek(rPos); - - if (data.size() == 0) - return m_lastResult = XRV_OK; - - if (remaining >= bsize) - read1 = m_handle->read(buffer1, 1, bsize); - else - read1 = m_handle->read(buffer1, 1, remaining); - - remaining -= read1; - rPos += read1; - - while(remaining > 0) - { - // move data to correct buffer - read2 = read1; - btemp = buffer1; buffer1 = buffer2; buffer2 = btemp; - - // read next block - if (remaining >= bsize) - read1 = m_handle->read(buffer1, 1, bsize); - else - read1 = m_handle->read(buffer1, 1, remaining); - - remaining -= read1; - rPos += read1; - - // write block to the correct position - m_handle->seek(wPos); - wPos += m_handle->write(buffer2, 1, read2); - m_handle->seek(rPos); - } - - m_handle->seek(wPos); - wPos += m_handle->write(buffer1, 1, read1); - - m_handle->seek(start); - m_writePos = start + m_handle->write(data.data(), 1, length); - m_fileSize += length; - - free(bufferRoot); - return m_lastResult = XRV_OK; -} - -/*! \brief Open a file. - \param filename The name of the file to open - \param createNew When true, the file will be created if it doesn't exist yet - \param readOnly When true, the file will be marked as read only for %IoInterfaceFile, - preventing accidental writes to the file. - \returns XRV_OK if the file was opened successfully - \sa createFile -*/ -XsResultValue IoInterfaceFile::open(const XsString& filename, bool createNew, bool readOnly) -{ - if (m_handle) - return m_lastResult = XRV_ALREADYOPEN; - - m_handle = new XsFile(); - - //! \test does this work for non-existing files? Or do we need a check and create? - m_readOnly = readOnly; - XsResultValue localResult = m_handle->open(filename, readOnly); - - if (localResult != XRV_OK) - { - if (createNew) - { - localResult = m_handle->create(filename, false); - } - else - { - // final attempt: open it forced readonly - localResult = m_handle->open(filename, false); - m_readOnly = true; - } - } - if (localResult != XRV_OK) - { - delete m_handle; - m_handle = NULL; - return m_lastResult = XRV_INPUTCANNOTBEOPENED; // overrides a possible OUTPUTCANNOTBEOPENED - } - - bool fail = false; -#ifdef _WIN32 - wchar_t fullpath[XS_MAX_FILENAME_LENGTH]; - 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]; - if (realpath(filename.c_str(), fullpath) == NULL) - fail = true; -#endif - m_filename = XsString(fullpath); - - if (fail) - { - m_handle->close(); - delete m_handle; - m_handle = NULL; - return m_lastResult = XRV_INVALIDPARAM; - } - - m_readPos = 0; - m_writePos = 0; - m_reading = true; - m_handle->seek_r(0); - m_fileSize = m_handle->tell(); - m_handle->seek(0); - return (m_lastResult = XRV_OK); -} - -/*! \copydoc IoInterface::readData - \note This function reads exactly the number of bytes as requested from the file unless the end - of file boundary is encountered. -*/ -XsResultValue IoInterfaceFile::readData(XsFilePos maxLength, XsByteArray& data) -{ - if (!m_handle) - return m_lastResult = XRV_NOFILEOPEN; - - if (maxLength == 0) - { - data.clear(); - return m_lastResult = XRV_OK; - } - - XsFilePos length; - - gotoRead(); - data.setSize((XsSize) maxLength); - - length = m_handle->read(data.data(), 1, maxLength); - if (m_handle->eof() && length <= 0) - { - data.clear(); - return (m_lastResult = XRV_ENDOFFILE); - } - - m_readPos += length; - if (length < maxLength) - data.pop_back((XsSize) (maxLength - length)); - return m_lastResult = XRV_OK; -} - -/*! \brief This function will read blocks of data aligned to \a blockSize - \details The Function will read as much data as is necessary to align to the block size + \a blockCount blocks. - So the given blockCount is an indication for the minimum amount of data read, unless the end of file is encountered. - \param blockCount The number of blocks to read. - \param data A buffer for the data that was read from the file - \returns XRV_OK if the data was read successfully -*/ -XsResultValue IoInterfaceFile::readDataBlocks(XsFilePos blockCount, XsByteArray& data) -{ - XsFilePos realign = (m_readPos & (m_fileBlockSize-1)); - if (realign) - blockCount = m_fileBlockSize * blockCount + m_fileBlockSize - realign; - else - blockCount *= m_fileBlockSize; - if (blockCount == 0) - return XRV_OK; - return readData(blockCount, data); -} - -/*! \brief Read data from the file and put it into the data buffer. - - This function reads up to the number of bytes as requested from the file. - The function will also stop if the given terminator character is encountered. - The terminator is included in the output buffer. - \param maxLength The amount of data that will be read. - \param terminator A character that will end the read operation if encountered. - \param bdata A buffer that will store the read data. - \returns XRV_OK if the data was read successfully -*/ -XsResultValue IoInterfaceFile::readTerminatedData(XsFilePos maxLength, unsigned char terminator, XsByteArray& bdata) -{ - if (!m_handle) - return m_lastResult = XRV_NOFILEOPEN; - - if (maxLength == 0) - { - bdata.clear(); - return m_lastResult = XRV_OK; - } - - bdata.setSize((XsSize) maxLength); - char *data = (char *) bdata.data(); - - XsFilePos length; - int readChar; - - gotoRead(); - - length = 0; - readChar = m_handle->getc(); - - while (!m_handle->eof() && !m_handle->error()) - { - data[length] = (char) readChar; - ++length; - ++m_readPos; - - if (length >= maxLength) - return m_lastResult = XRV_OK; - if ((unsigned char) readChar == terminator) - { - bdata.pop_back((XsSize) (maxLength - length)); - return m_lastResult = XRV_OK; - } - } - bdata.pop_back((XsSize) (maxLength - length)); - return m_lastResult = XRV_ENDOFFILE; -} - -/*! \brief Set the new absolute read position - \details The read position is checked against the file size first. - \param pos The new read position - \returns XRV_OK if the read position was updated successfully -*/ -XsResultValue IoInterfaceFile::setReadPosition(XsFilePos pos) -{ - if (!m_handle) - return m_lastResult = XRV_NOFILEOPEN; - - if (m_readPos != pos) - { - m_readPos = pos; - if (m_reading) - m_handle->seek(m_readPos); - } - - return m_lastResult = XRV_OK; -} - -/*! \brief Set the new absolute write position - \details The write position is checked against the file size first. - \param pos The new write position - \returns XRV_OK if the write position was updated successfully -*/ -XsResultValue IoInterfaceFile::setWritePosition(XsFilePos pos) -{ - if (!m_handle) - return m_lastResult = XRV_NOFILEOPEN; - if (m_readOnly) - return m_lastResult = XRV_READONLY; - - if (pos == -1) - { - if (m_reading) - m_reading = false; - m_handle->seek_r(0); - m_writePos = m_handle->tell(); - } - else - { - if (m_writePos != pos) - { - m_writePos = pos; - if (!m_reading) - m_handle->seek(m_writePos); - } - } - - return m_lastResult = XRV_OK; -} - -/*! \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) -{ - if (!m_handle) - return m_lastResult = XRV_NOFILEOPEN; - if (m_readOnly) - return m_lastResult = XRV_READONLY; - - XsFilePos length = (XsFilePos) data.size(); - if (length == 0) - return m_lastResult = XRV_OK; - - gotoWrite(); - XsFilePos writeRes = m_handle->write(data.data(), 1, length); - if (writeRes == (XsFilePos)EOF || writeRes < length) - { - 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; - } - } - m_writePos += writeRes; - if (written) - *written = writeRes; - - if (m_writePos > m_fileSize) - m_fileSize = m_writePos; - - return m_lastResult = XRV_OK; -} - -/*! \brief Return the current read position. - \returns The current read position. -*/ -XsFilePos IoInterfaceFile::getReadPosition() const -{ - return m_readPos; -} - -/*! \brief Return the current write position. - \returns The current write position. -*/ -XsFilePos IoInterfaceFile::getWritePosition() const -{ - return m_writePos; -} - -/*! \brief Return the result code of the last operation. - \returns The result code of the last operation. -*/ -XsResultValue IoInterfaceFile::getLastResult() const -{ - return m_lastResult; -} - -/*! \brief Return whether the file is open or not. - \returns true if the file is open -*/ -bool IoInterfaceFile::isOpen() const -{ - return m_handle != NULL; -} - -/*! \brief Return whether the file is read-only or not. - \returns true if the file is read-only -*/ -bool IoInterfaceFile::isReadOnly(void) const -{ - return !isOpen() || m_readOnly; -} - -/*! \brief Make sure the file is at least \a minSize bytes big. - \param minSize The minimum file size required - \returns XRV_OK if the reserve succeeded - \details The file will not be truncated, any added bytes will be filled with '\0' characters -*/ -XsResultValue IoInterfaceFile::reserve(XsFilePos minSize) -{ - if (isReadOnly()) - return XRV_READONLY; - - if (minSize <= m_fileSize) - return XRV_OK; - - auto rv = m_handle->resize(minSize); - if (rv != XRV_OK) - return rv; - - m_fileSize = minSize; - return XRV_OK; -} - -/*! \brief Flushes the buffers of a specified file and causes all buffered data to be written to a file. - \details This will ensure that the metadata is written to the file. - \note Only for Windows! - \returns XRV_OK if the buffers were flushed successfully -*/ -XsResultValue IoInterfaceFile::flushFileBuffers() -{ -#ifdef XSENS_WINDOWS - return FlushFileBuffers((HANDLE)_get_osfhandle(_fileno(m_handle->handle()))) ? XRV_ERROR : XRV_OK; -#else - return XRV_OK; -#endif -} diff --git a/extern/xcommunication/legacydatapacket.cpp b/extern/xcommunication/legacydatapacket.cpp deleted file mode 100644 index d620673..0000000 --- a/extern/xcommunication/legacydatapacket.cpp +++ /dev/null @@ -1,2216 +0,0 @@ -/* Copyright (c) 2003-2017 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. -*/ - -#include -#include "legacydatapacket.h" -#include "packetfixeddata.h" -#include "mtwsdidata.h" -#include -#include -#include -#include -#include -#include -#include -#include - -#ifdef LOG_PACKET -# include "xslog.h" -# define PACKETLOG XSENSLOG -#else -# define PACKETLOG(...) (void)0 -#endif - -////////////////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////// Packet class ////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////////////////// -#define FORMAT_DOUBLE ((m_fixedData->m_formatList[index].m_outputSettings & XOS_Dataformat_Mask)|XOS_Dataformat_Double) -#define ISDOUBLE(a) (m_fixedData->m_infoList[index].a >= m_fixedData->m_infoList[index].m_doubleBoundary) -#define CHECKIFDOUBLE(a) ISDOUBLE(a)?FORMAT_DOUBLE:m_fixedData->m_formatList[index].m_outputSettings - -/*! \struct LegacyDataPacket - \brief Contains an MTData XsMessage and supports functions for extracting contained data. -*/ - -/*! \brief Construct a new %LegacyDataPacket without data. The %LegacyDataPacket will be invalid. */ -LegacyDataPacket::LegacyDataPacket() - : m_fixedData(new PacketFixedData) - , m_lastFoundId(0) - , m_lastFoundIndex(0) - , m_rtc(0) - , m_toa(0) - , m_packetId(0) - , m_triggerIn1(0) - , m_triggerIn2(0) -{ - PACKETLOG("%s creating default %p\n", __FUNCTION__, this); -} - -/*! \brief Construct a new %LegacyDataPacket with room for data from \a count devices. - \param count The number of devices whose data is in the object - \param xbus Whether the message was sent by an Xbus Master or by a standalone MT. This matters - because the sample counter is in a different place. -*/ -LegacyDataPacket::LegacyDataPacket(XsSize count, bool xbus) - : m_fixedData(new PacketFixedData(count)) - , m_lastFoundId(0) - , m_lastFoundIndex(0) - , m_rtc(0) - , m_toa(0) - , m_packetId(0) - , m_triggerIn1(0) - , m_triggerIn2(0) - -{ - PACKETLOG("%s creating %p with %d items and xbus option %d\n", __FUNCTION__, this, count, xbus); - m_fixedData->m_xm = xbus; -} - -/*! \brief Construct a LegacyDataPacket as a copy of \a other. */ -LegacyDataPacket::LegacyDataPacket(const LegacyDataPacket& other) - : m_fixedData(0) - , m_lastFoundId(0) - , m_lastFoundIndex(0) - , m_rtc(0) - , m_toa(0) - , m_packetId(0) - , m_triggerIn1(0) - , m_triggerIn2(0) - -{ - PACKETLOG("%s creating %p from %p\n", __FUNCTION__, this, &other); - *this = other; - PACKETLOG("%s creating %p from %p done\n", __FUNCTION__, this, &other); -} - -/*! \brief Assignment operator, copies the contents of \a pack into this -*/ -const LegacyDataPacket& LegacyDataPacket::operator = (const LegacyDataPacket& pack) -{ - PACKETLOG("%s copying from %p to %p\n", __FUNCTION__, &pack, this); - if (this == &pack) - return *this; - - delete m_fixedData; - m_fixedData = NULL; - - if (pack.m_fixedData) // Can be empty - { - m_fixedData = new PacketFixedData(*pack.m_fixedData); - m_lastFoundId = pack.m_lastFoundId; - m_lastFoundIndex = pack.m_lastFoundIndex; - } - - m_toa = pack.m_toa; - m_rtc = pack.m_rtc; - m_msg = pack.m_msg; - m_packetId = pack.m_packetId; - m_triggerIn1 = pack.m_triggerIn1; - m_triggerIn2 = pack.m_triggerIn2; - - PACKETLOG("%s copying from %p to %p done\n", __FUNCTION__, &pack, this); - return *this; -} - -/*! \brief Destructor */ -LegacyDataPacket::~LegacyDataPacket() -{ - try { - PACKETLOG("%s destroying %p\n", __FUNCTION__, this); - delete m_fixedData; - m_fixedData = NULL; - PACKETLOG("%s destroyed %p\n", __FUNCTION__, this); - } - catch (...) - {} -} - -//lint -esym(613, LegacyDataPacket::m_fixedData) assert and dataSize take care of this -/*! \brief Returns the number of devices whose data is contained in the object -*/ -XsSize LegacyDataPacket::itemCount(void) const -{ - assert(m_fixedData); - return m_fixedData->m_itemCount; -} - -/*! \brief Set the number of devices whose data is contained in this object to \a count -*/ -void LegacyDataPacket::setItemCount(XsSize count) -{ - assert(m_fixedData); - m_fixedData->m_itemCount = count; -} - -/*! \brief Returns the Time Of Arrival value as stored in the object -*/ -XsTimeStamp LegacyDataPacket::timeOfArrival(void) const -{ - return m_toa; -} - -/*! \brief Set the Time Of Arrival value to \a timeofarrival -*/ -void LegacyDataPacket::setTimeOfArrival(XsTimeStamp timeofarrival) -{ - m_toa = timeofarrival; -} - -/*! \brief Returns the Real Time Clock value as stored in the object -*/ -XsTimeStamp LegacyDataPacket::rtc(void) const -{ - return m_rtc; -} - -/*! \brief Set the Real Time Clock value to \a realtimeclock -*/ -void LegacyDataPacket::setRtc(const XsTimeStamp realtimeclock) -{ - m_rtc = realtimeclock; -} - -/*! \brief Return the 64-bit sample counter associated with this packet - \returns The 64-bit sample counter associated with this packet - \note This sample counter may differ a lot from the normal 16-bit sample counter -*/ -int64_t LegacyDataPacket::largePacketCounter(void) const -{ - return m_packetId.msTime(); -} - -/*! \brief Set the 64-bit sample counter associated with this packet -*/ -void LegacyDataPacket::setLargePacketCounter(const int64_t sc) -{ - m_packetId = sc; -} - -/*! \brief Returns a copy of the %XsMessage contained by the object, including computed and added data. */ -XsMessage LegacyDataPacket::message(void) const -{ - return m_msg; -} - -/*! \brief Returns the original message as it was received, without computed and added data (except for SDI interval reconstruction) -*/ -XsMessage LegacyDataPacket::originalMessage(void) const -{ - assert(m_fixedData); - uint16_t originalSize = 0; - for (uint16_t i = 0;i < m_fixedData->m_itemCount;++i) - if (m_fixedData->m_infoList[i].m_doubleBoundary > originalSize) - originalSize = m_fixedData->m_infoList[i].m_doubleBoundary; - - if (originalSize == m_msg.getDataSize()) - return m_msg; - - XsMessage m = m_msg; - m.resizeData(originalSize); - m.setDataBuffer(m_msg.getDataBuffer(), originalSize); - m.recomputeChecksum(); - return m; -} - -/*! \brief Set the source message to \a msg - \param msg The message to use - \note Call updateInfoList() to actually start using the new message -*/ -void LegacyDataPacket::setMessage(const XsMessage& msg) -{ - m_msg = msg; -} - -/*! \brief Returns the packet info for the \a index'th device in the packet. - \details This describes what data is contained by the object. - \param index The index of the device whose packet info should be returned - \returns The packet info for device \a index -*/ -PacketInfo LegacyDataPacket::packetInfo(int32_t index) const -{ - assert(m_fixedData); - if (index < 0 || index >= (int32_t) m_fixedData->m_itemCount) - return PacketInfo(); - - return m_fixedData->m_infoList[index]; -} - -/*! \brief Return the frame counter (previously: sample counter) of the packet - \details For SDI data, this function will return the Last Frame Number in the SDI interval. - For other data, this function will return the plain Sample Counter. - \returns The frame counter of the packet or 0 if it isn't present in the packet (note that 0 is - a valid sample counter, so don't use it for an error check). -*/ -uint16_t LegacyDataPacket::frameCounter() const -{ - assert(m_fixedData); - if (containsPacketCounter(0)) - return m_msg.getDataShort(m_fixedData->m_infoList[0].m_sc); - else if (containsMtwSdiData(0)) - return m_msg.getDataShort(m_fixedData->m_infoList[0].m_wLastFrameNumber); - return 0; -} - -////////////////////////////////////////////////////////////////////////////////////////// -/*! \brief Returns the index of the fixed data with id \a dev - \details If the result is not -1, it can be used as the \a index parameter in other functions - that require it (see the list below). - \param dev The device ID to find - \returns The index of the fixed data - \sa deviceId \sa setDeviceId \sa dataFormat \sa setDataFormat(const XsDataFormat&, int32_t) - \sa setDataFormat(XsOutputMode, XsOutputSettings, int32_t) -*/ -int32_t LegacyDataPacket::findDeviceId(XsDeviceId dev) const -{ - assert(m_fixedData); - if (!dev.isValid()) - return 0; - if (dev == m_lastFoundId) - return (int32_t) m_lastFoundIndex; - for (XsSize i=0;im_itemCount;++i) - { - if (m_fixedData->m_idList[i] == dev) - { - m_lastFoundId = dev; - m_lastFoundIndex = i; - return (int32_t) i; - } - } - return -1; -} - -/*! \brief Returns the device ID of the device with the given \a index - \param index The index of the device whose device ID should be returned - \returns The device ID of the device - \sa findDeviceId -*/ -XsDeviceId LegacyDataPacket::deviceId(int32_t index) const -{ - assert(m_fixedData); - return m_fixedData->m_idList[index]; -} - -/*! \brief Sets the device ID of the device with the given \a index to \a deviceid - \param deviceid The device ID to set - \param index The index of the device whose device ID should be updated - \returns true if the device ID was successfully updated - \sa findDeviceId -*/ -void LegacyDataPacket::setDeviceId(XsDeviceId deviceid, int32_t index) -{ - assert(m_fixedData); - m_fixedData->m_idList[index] = deviceid; -} - -////////////////////////////////////////////////////////////////////////////////////////// -/*! \brief Returns the data format of the device with the given \a index - \param index The index of the device whose data format should be returned - \returns The data format of the device - \sa findDeviceId -*/ -XsDataFormat LegacyDataPacket::dataFormat(int32_t index) const -{ - if (!m_fixedData || index >= (int32_t) m_fixedData->m_itemCount || m_fixedData->m_formatList == NULL) - return XsDataFormat(); - - return m_fixedData->m_formatList[index]; -} - -////////////////////////////////////////////////////////////////////////////////////////// -/*! \brief Sets the data format of the device with the given \a index to \a format - \param format The data format of the device - \param index The index of the device whose data format should be updated - \returns true if the data format was successfully updated - \sa findDeviceId -*/ -bool LegacyDataPacket::setDataFormat(const XsDataFormat& format, int32_t index) -{ - assert(m_fixedData); - - if (index < (int32_t) m_fixedData->m_itemCount) - { - m_fixedData->m_formatList[index] = format; - updateInfoList(); - return true; - } - return false; -} - -////////////////////////////////////////////////////////////////////////////////////////// -/*! \brief Sets the data format of the device with the given \a index to \a outputMode and \a outputSettings - \param outputMode The output mode of the device (see low level communication documentation) - \param outputSettings The output settings of the device (see low level communication documentation) - \param index The index of the device whose data format should be updated - \returns true if the data format was successfully updated - \sa findDeviceId -*/ -bool LegacyDataPacket::setDataFormat(XsOutputMode outputMode, XsOutputSettings outputSettings, int32_t index) -{ - assert(m_fixedData); - - if (index < (int32_t) m_fixedData->m_itemCount) - { - m_fixedData->m_formatList[index].m_outputMode = outputMode; - m_fixedData->m_formatList[index].m_outputSettings = outputSettings; - updateInfoList(); - return true; - } - return false; -} - -////////////////////////////////////////////////////////////////////////////////////////// -/*! \brief Returns whether the xbus flag is set or not - \returns trur if the xbus flag is set or not - \sa setXbusSystem -*/ -bool LegacyDataPacket::isXbusSystem(void) const -{ - assert(m_fixedData); - return m_fixedData->m_xm; -} - -////////////////////////////////////////////////////////////////////////////////////////// -/*! \brief Sets the xbus flag - \details The xbus flag determines where the packet counter is stored in the message. Setting - this value incorrectly can lead to corrupted data reads. - \param xbus The desired setting -*/ -void LegacyDataPacket::setXbusSystem(bool xbus) -{ - assert(m_fixedData); - - if (xbus != m_fixedData->m_xm) - { - // update the state cache - m_fixedData->m_xm = xbus; - updateInfoList(); - } -} - -/*! \brief Returns the floating/fixed point value size in bytes - \param index The index of the item whose fp size should be returned. - \returns The floating/fixed point value size in bytes -*/ -uint16_t LegacyDataPacket::getFPValueSize(int32_t index) const -{ - assert(m_fixedData); - uint16_t ds = 4; - switch (m_fixedData->m_formatList[index].m_outputSettings & XOS_Dataformat_Mask) - { - case XOS_Dataformat_Float: - ds = 4; - break; - - case XOS_Dataformat_Double: - ds = 8; - break; - - case XOS_Dataformat_Fp1632: - ds = 6; - break; - - case XOS_Dataformat_F1220: - ds = 4; - break; - default: - break; - } - return ds; -} - -/*! \brief Returns the size of the data - \param index The index of the item of which the size should be returned. - \returns The size of the data -*/ -XsSize LegacyDataPacket::dataSize(int32_t index) const -{ - if (m_fixedData && index < (int32_t) m_fixedData->m_itemCount) - return m_fixedData->m_infoList[index].m_size; - return 0; -} - -#define XS_EXTRA_TEMPERATURE_CHANNELS 3 - -/*! \brief Update the internal info list by analyzing the known XsDataFormat for each device. -*/ -void LegacyDataPacket::updateInfoList() -{ - assert(m_fixedData); - if (m_fixedData->m_infoList != NULL) - { - delete[] m_fixedData->m_infoList; - m_fixedData->m_infoList = NULL; - } - - assert(m_fixedData->m_itemCount); - // allocate list - m_fixedData->m_infoList = new PacketInfo[m_fixedData->m_itemCount]; - uint16_t totalOffset; - if (m_fixedData->m_xm) - totalOffset = 2; - else - totalOffset = 0; - - // fill lists - for (uint16_t i = 0;i < m_fixedData->m_itemCount;++i) - { - m_fixedData->m_infoList[i].m_offset = totalOffset; - m_fixedData->m_infoList[i].m_size = 0; - - uint16_t ds = getFPValueSize(i); - - if (m_fixedData->m_formatList[i].m_outputMode & XOM_Raw) - { - m_fixedData->m_infoList[i].m_rawData = totalOffset; - m_fixedData->m_infoList[i].m_rawAcc = totalOffset; - m_fixedData->m_infoList[i].m_rawGyr = totalOffset+6; - m_fixedData->m_infoList[i].m_rawMag = totalOffset+12; - m_fixedData->m_infoList[i].m_rawTemp = totalOffset+18; - - m_fixedData->m_infoList[i].m_size += 20; - totalOffset += 20; - - if(m_fixedData->m_formatList[i].m_outputSettings & XOS_ExtendedTemperature_Mask) - { - for(int j = 0; j < XS_EXTRA_TEMPERATURE_CHANNELS; j++) - { - m_fixedData->m_infoList[i].m_rawGyroTemp[j] = totalOffset; - m_fixedData->m_infoList[i].m_size += 2; - totalOffset += 2; - } - } else - { - for(int j = 0; j < XS_EXTRA_TEMPERATURE_CHANNELS; j++) - { - m_fixedData->m_infoList[i].m_rawGyroTemp[j] = XS_DATA_ITEM_NOT_AVAILABLE; - } - } - } - else - { - m_fixedData->m_infoList[i].m_rawData = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_rawAcc = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_rawGyr = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_rawMag = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_rawTemp = XS_DATA_ITEM_NOT_AVAILABLE; - for(int j = 0; j < XS_EXTRA_TEMPERATURE_CHANNELS; j++) - m_fixedData->m_infoList[i].m_rawGyroTemp[j] = XS_DATA_ITEM_NOT_AVAILABLE; - } - - if (m_fixedData->m_formatList[i].m_outputMode & XOM_Sdi) - { - m_fixedData->m_infoList[i].m_mtwSdiData = totalOffset; - m_fixedData->m_infoList[i].m_wClientId = totalOffset; - m_fixedData->m_infoList[i].m_wTimeSync = totalOffset + 4; - m_fixedData->m_infoList[i].m_wFirstFrameNumber = totalOffset + 5; - m_fixedData->m_infoList[i].m_wLastFrameNumber = totalOffset + 7; - m_fixedData->m_infoList[i].m_wCurrentBias = totalOffset + 9; - m_fixedData->m_infoList[i].m_wOrientationIncrement = totalOffset + 9+3*ds; - m_fixedData->m_infoList[i].m_wVelocityIncrement = totalOffset + 9+7*ds; - m_fixedData->m_infoList[i].m_wAidingData = totalOffset + 9+10*ds; - m_fixedData->m_infoList[i].m_wBaroMeter = totalOffset + 10+10*ds; - m_fixedData->m_infoList[i].m_wMagnetoMeter = totalOffset + 12+10*ds; - m_fixedData->m_infoList[i].m_wRssi = totalOffset + 12+13*ds; - m_fixedData->m_infoList[i].m_size += 13+13*ds; - totalOffset += 13+13*ds; - } - else - { - m_fixedData->m_infoList[i].m_mtwSdiData = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_wClientId = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_wTimeSync = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_wFirstFrameNumber = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_wLastFrameNumber = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_wCurrentBias = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_wOrientationIncrement = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_wVelocityIncrement = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_wAidingData = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_wBaroMeter = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_wMagnetoMeter = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_wRssi = XS_DATA_ITEM_NOT_AVAILABLE; - } - - - if (m_fixedData->m_formatList[i].m_outputMode & XOM_GpsPvt_Pressure) - { - m_fixedData->m_infoList[i].m_gpsPvtData = totalOffset; - m_fixedData->m_infoList[i].m_gpsPvtPressure = totalOffset; - m_fixedData->m_infoList[i].m_gpsPvtPressureAge = totalOffset + 2; - m_fixedData->m_infoList[i].m_size += 3; - totalOffset += 3; - - if ((m_fixedData->m_formatList[i].m_outputSettings & XOS_NoGpsInGpsPvt) == 0) - { - m_fixedData->m_infoList[i].m_gpsPvtGpsData = totalOffset; - m_fixedData->m_infoList[i].m_gpsPvtItow = totalOffset; - m_fixedData->m_infoList[i].m_gpsPvtLatitude = totalOffset + 4; - m_fixedData->m_infoList[i].m_gpsPvtLongitude = totalOffset + 8; - m_fixedData->m_infoList[i].m_gpsPvtHeight = totalOffset + 12; - m_fixedData->m_infoList[i].m_gpsPvtVeln = totalOffset + 16; - m_fixedData->m_infoList[i].m_gpsPvtVele = totalOffset + 20; - m_fixedData->m_infoList[i].m_gpsPvtVeld = totalOffset + 24; - m_fixedData->m_infoList[i].m_gpsPvtHacc = totalOffset + 28; - m_fixedData->m_infoList[i].m_gpsPvtVacc = totalOffset + 32; - m_fixedData->m_infoList[i].m_gpsPvtSacc = totalOffset + 36; - m_fixedData->m_infoList[i].m_gpsPvtGpsAge = totalOffset + 40; - m_fixedData->m_infoList[i].m_size += 41; - totalOffset += 41; - } - else - { - m_fixedData->m_infoList[i].m_gpsPvtGpsData = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_gpsPvtItow = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_gpsPvtLatitude = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_gpsPvtLongitude = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_gpsPvtHeight = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_gpsPvtVeln = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_gpsPvtVele = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_gpsPvtVeld = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_gpsPvtHacc = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_gpsPvtVacc = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_gpsPvtSacc = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_gpsPvtGpsAge = XS_DATA_ITEM_NOT_AVAILABLE; - } - } - else - { - m_fixedData->m_infoList[i].m_gpsPvtData = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_gpsPvtPressure = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_gpsPvtPressureAge = XS_DATA_ITEM_NOT_AVAILABLE; - - m_fixedData->m_infoList[i].m_gpsPvtGpsData = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_gpsPvtItow = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_gpsPvtLatitude = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_gpsPvtLongitude = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_gpsPvtHeight = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_gpsPvtVeln = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_gpsPvtVele = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_gpsPvtVeld = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_gpsPvtHacc = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_gpsPvtVacc = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_gpsPvtSacc = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_gpsPvtGpsAge = XS_DATA_ITEM_NOT_AVAILABLE; - } - - m_fixedData->m_infoList[i].m_temp = XS_DATA_ITEM_NOT_AVAILABLE; - for(int j = 0; j < XS_EXTRA_TEMPERATURE_CHANNELS; j++) - m_fixedData->m_infoList[i].m_gyroTemp[j] = XS_DATA_ITEM_NOT_AVAILABLE; - - if (m_fixedData->m_formatList[i].m_outputMode & XOM_Temperature) - { - m_fixedData->m_infoList[i].m_temp = totalOffset; - m_fixedData->m_infoList[i].m_size += ds; - totalOffset += ds; - - if (m_fixedData->m_formatList[i].m_outputSettings & XOS_ExtendedTemperature_Mask) - { - for (int j = 0; j < XS_EXTRA_TEMPERATURE_CHANNELS; j++) - { - m_fixedData->m_infoList[i].m_gyroTemp[j] = totalOffset; - m_fixedData->m_infoList[i].m_size += ds; - totalOffset += ds; - } - } - } - - if (m_fixedData->m_formatList[i].m_outputMode & XOM_Calibrated) - { - m_fixedData->m_infoList[i].m_calData = totalOffset; - if ((m_fixedData->m_formatList[i].m_outputSettings & XOS_CalibratedMode_Acc_Mask) == 0) - { - m_fixedData->m_infoList[i].m_calAcc = totalOffset; - m_fixedData->m_infoList[i].m_size += 3*ds; - totalOffset += 3*ds; - } - else - m_fixedData->m_infoList[i].m_calAcc = XS_DATA_ITEM_NOT_AVAILABLE; - - if ((m_fixedData->m_formatList[i].m_outputSettings & XOS_CalibratedMode_Gyr_Mask) == 0) - { - m_fixedData->m_infoList[i].m_calGyr = totalOffset; - m_fixedData->m_infoList[i].m_size += 3*ds; - totalOffset += 3*ds; - } - else - m_fixedData->m_infoList[i].m_calGyr = XS_DATA_ITEM_NOT_AVAILABLE; - - if ((m_fixedData->m_formatList[i].m_outputSettings & XOS_CalibratedMode_Mag_Mask) == 0) - { - m_fixedData->m_infoList[i].m_calMag = totalOffset; - m_fixedData->m_infoList[i].m_size += 3*ds; - totalOffset += 3*ds; - } - else - m_fixedData->m_infoList[i].m_calMag = XS_DATA_ITEM_NOT_AVAILABLE; - - if ((m_fixedData->m_formatList[i].m_outputSettings & XOS_CalibratedMode_AccGyrMag_Mask) == XOS_CalibratedMode_AccGyrMag_Mask) - m_fixedData->m_infoList[i].m_calData = XS_DATA_ITEM_NOT_AVAILABLE; - } - else - { - m_fixedData->m_infoList[i].m_calData = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_calAcc = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_calGyr = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_calMag = XS_DATA_ITEM_NOT_AVAILABLE; - } - - m_fixedData->m_infoList[i].m_oriEul = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_oriQuat = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_oriMat = XS_DATA_ITEM_NOT_AVAILABLE; - - if (m_fixedData->m_formatList[i].m_outputMode & XOM_Orientation) - { - switch (m_fixedData->m_formatList[i].m_outputSettings & XOS_OrientationMode_Mask) - { - case XOS_OrientationMode_Euler: - m_fixedData->m_infoList[i].m_oriEul = totalOffset; - m_fixedData->m_infoList[i].m_size += 3*ds; - totalOffset += 3*ds; - break; - case XOS_OrientationMode_Quaternion: - m_fixedData->m_infoList[i].m_oriQuat = totalOffset; - m_fixedData->m_infoList[i].m_size += 4*ds; - totalOffset += 4*ds; - break; - case XOS_OrientationMode_Matrix: - m_fixedData->m_infoList[i].m_oriMat = totalOffset; - m_fixedData->m_infoList[i].m_size += 9*ds; - totalOffset += 9*ds; - break; - default: - break; - } - } - - m_fixedData->m_infoList[i].m_analogIn1 = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_analogIn2 = XS_DATA_ITEM_NOT_AVAILABLE; - if (m_fixedData->m_formatList[i].m_outputMode & XOM_Auxiliary) - { - if ((m_fixedData->m_formatList[i].m_outputSettings & XOS_AuxiliaryMode_Ain1_Mask) == 0) - { - m_fixedData->m_infoList[i].m_analogIn1 = totalOffset; - m_fixedData->m_infoList[i].m_size += 2; - totalOffset += 2; - } - else - m_fixedData->m_infoList[i].m_analogIn1 = XS_DATA_ITEM_NOT_AVAILABLE; - - if ((m_fixedData->m_formatList[i].m_outputSettings & XOS_AuxiliaryMode_Ain2_Mask) == 0) - { - m_fixedData->m_infoList[i].m_analogIn2 = totalOffset; - m_fixedData->m_infoList[i].m_size += 2; - totalOffset += 2; - } - else - m_fixedData->m_infoList[i].m_analogIn2 = XS_DATA_ITEM_NOT_AVAILABLE; - } - - m_fixedData->m_infoList[i].m_posLLA = XS_DATA_ITEM_NOT_AVAILABLE; - if (m_fixedData->m_formatList[i].m_outputMode & XOM_Position) - { - if ((m_fixedData->m_formatList[i].m_outputSettings & XOS_PositionMode_Mask) == XOS_PositionMode_Lla_Wgs84) - { - m_fixedData->m_infoList[i].m_posLLA = totalOffset; - m_fixedData->m_infoList[i].m_size += 3*ds; - totalOffset += 3*ds; - } - } - - m_fixedData->m_infoList[i].m_velNEDorNWU = XS_DATA_ITEM_NOT_AVAILABLE; - if (m_fixedData->m_formatList[i].m_outputMode & XOM_Velocity) - { - if ((m_fixedData->m_formatList[i].m_outputSettings & XOS_VelocityMode_Mask) == XOS_VelocityMode_Ms_Xyz) - { - m_fixedData->m_infoList[i].m_velNEDorNWU = totalOffset; - m_fixedData->m_infoList[i].m_size += 3*ds; - totalOffset += 3*ds; - } - } - - if (m_fixedData->m_formatList[i].m_outputMode & XOM_Status) - { - m_fixedData->m_infoList[i].m_status = totalOffset; - - if (m_fixedData->m_formatList[i].m_outputSettings & XOS_Status_Detailed) - { - m_fixedData->m_infoList[i].m_detailedStatus = totalOffset; - m_fixedData->m_infoList[i].m_size += 4; - totalOffset += 4; - } - else - { - m_fixedData->m_infoList[i].m_detailedStatus = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_size += 1; - totalOffset += 1; - } - } - else - { - m_fixedData->m_infoList[i].m_status = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_detailedStatus = XS_DATA_ITEM_NOT_AVAILABLE; - } - - m_fixedData->m_infoList[i].m_sc = XS_DATA_ITEM_NOT_AVAILABLE; - if (m_fixedData->m_xm) - m_fixedData->m_infoList[i].m_sc = 0; - if (m_fixedData->m_formatList[i].m_outputSettings & XOS_Timestamp_PacketCounter) - { - if (!m_fixedData->m_xm) - m_fixedData->m_infoList[i].m_sc = totalOffset; - m_fixedData->m_infoList[i].m_size += 2; - totalOffset += 2; - } - - if (m_fixedData->m_formatList[i].m_outputSettings & XOS_Timestamp_SampleUtc) - { - m_fixedData->m_infoList[i].m_utcTime = totalOffset; - m_fixedData->m_infoList[i].m_utcNano = totalOffset; - m_fixedData->m_infoList[i].m_utcYear = totalOffset + 4; - m_fixedData->m_infoList[i].m_utcMonth = totalOffset + 6; - m_fixedData->m_infoList[i].m_utcDay = totalOffset + 7; - m_fixedData->m_infoList[i].m_utcHour = totalOffset + 8; - m_fixedData->m_infoList[i].m_utcMinute = totalOffset + 9; - m_fixedData->m_infoList[i].m_utcSecond = totalOffset + 10; - m_fixedData->m_infoList[i].m_utcValid = totalOffset + 11; - m_fixedData->m_infoList[i].m_size += 12; - totalOffset += 12; - } - else { - m_fixedData->m_infoList[i].m_utcTime = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_utcNano = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_utcYear = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_utcMonth = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_utcDay = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_utcHour = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_utcMinute = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_utcSecond = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_utcValid = XS_DATA_ITEM_NOT_AVAILABLE; - } - - if (m_fixedData->m_formatList[i].m_outputSettings & XOS_SampleTimeFine_Mask) - { - m_fixedData->m_infoList[i].m_sampleTimeFine = totalOffset; - m_fixedData->m_infoList[i].m_size += 4; - totalOffset += 4; - } else - { - m_fixedData->m_infoList[i].m_sampleTimeFine = XS_DATA_ITEM_NOT_AVAILABLE; - } - - // post-processing data is never available at this point - m_fixedData->m_infoList[i].m_acc_g = XS_DATA_ITEM_NOT_AVAILABLE; - m_fixedData->m_infoList[i].m_doubleBoundary = totalOffset; - - if (m_fixedData->m_infoList[i].m_sc == XS_DATA_ITEM_NOT_AVAILABLE) - m_fixedData->m_infoList[i].m_sc = m_fixedData->m_infoList[i].m_wLastFrameNumber; - } -} - -#if 0 -/*! \brief Take packet fixed data out of this packet - \details The packet fixed data can usually be shared across several other packets. When - implementing caching, the fixed packet data can cause a huge overhead. By removing it from the - packet, the amount of memory usage is significantly reduced. -*/ -PacketFixedData LegacyDataPacket::takePacketFixedData() -{ - if (m_fixedData == NULL) - return PacketFixedData(); - - PacketFixedData data(*m_fixedData); - CHKDELNUL(m_fixedData); - return data; -} - - -/*! \brief Put packet fixed data back into this packet - \see takePacketFixedData -*/ -void LegacyDataPacket::putPacketFixedData(const PacketFixedData& data) -{ - CHKDELNUL(m_fixedData); - m_fixedData = new PacketFixedData(data); -} -#endif - -/*! \brief The Raw Accelerometer component of a data item. - - \param index The index of the item of which the data should be returned. - - \returns A XsUShortVector containing the x, y and z axis values in that order -*/ -XsUShortVector LegacyDataPacket::rawAcceleration(int32_t index) const -{ - XsUShortVector buffer; - if (containsRawAcceleration(index)) - for (uint16_t i=0;i<3;++i) - buffer[i] = m_msg.getDataShort(m_fixedData->m_infoList[index].m_rawAcc + (2*i)); - - return buffer; -} - -/*! \brief Check if data item contains Raw Accelerometer data - - \param index The index of the item of which the data should be returned - - \returns true if this packet contains raw acceleration data -*/ -bool LegacyDataPacket::containsRawAcceleration(int32_t index) const -{ - if (dataSize(index) == 0) - return false; - if (m_fixedData->m_infoList[index].m_rawAcc == XS_DATA_ITEM_NOT_AVAILABLE) - return false; - return true; -} - -/*! \brief Add/update Raw Accelerometer data for the item */ -bool LegacyDataPacket::setRawAcceleration(const XsUShortVector& vec, int32_t index) -{ - if (dataSize(index) == 0) - return false; - if (m_fixedData->m_infoList[index].m_rawAcc == XS_DATA_ITEM_NOT_AVAILABLE) - { - // add - m_fixedData->m_infoList[index].m_rawAcc = (uint16_t) m_msg.getDataSize(); - m_msg.resizeData(m_msg.getDataSize() + 3*2); - m_fixedData->m_infoList[index].m_size += 3*2; - } - // update - for (uint16_t i=0;i<3;++i) - m_msg.setDataShort(vec[i], m_fixedData->m_infoList[index].m_rawAcc + (2*i)); - return true; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Return the Raw Gyroscope component of a data item. -XsUShortVector LegacyDataPacket::rawGyroscopeData(int32_t index) const -{ - XsUShortVector buffer; - if (containsRawGyroscopeData(index)) - for (uint16_t i=0;i<3;++i) - buffer[i] = m_msg.getDataShort(m_fixedData->m_infoList[index].m_rawGyr + (2*i)); - - return buffer; -} -bool LegacyDataPacket::containsRawGyroscopeData(int32_t index) const -{ - if (dataSize(index) == 0) - return false; - if (m_fixedData->m_infoList[index].m_rawGyr == XS_DATA_ITEM_NOT_AVAILABLE) - return false; - return true; -} -bool LegacyDataPacket::setRawGyroscopeData(const XsUShortVector& vec, int32_t index) -{ - if (dataSize(index) == 0) - return false; - if (m_fixedData->m_infoList[index].m_rawGyr == XS_DATA_ITEM_NOT_AVAILABLE) - { - // add - m_fixedData->m_infoList[index].m_rawGyr = (uint16_t) m_msg.getDataSize(); - m_msg.resizeData(m_msg.getDataSize() + 3*2); - m_fixedData->m_infoList[index].m_size += 3*2; - } - // update - for (uint16_t i=0;i<3;++i) - m_msg.setDataShort(vec[i], m_fixedData->m_infoList[index].m_rawGyr + (2*i)); - return true; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Return the Raw Magnetometer component of a data item. -XsUShortVector LegacyDataPacket::rawMagneticField(int32_t index) const -{ - XsUShortVector buffer; - if (containsRawMagneticField(index)) - for (uint16_t i=0;i<3;++i) - buffer[i] = m_msg.getDataShort(m_fixedData->m_infoList[index].m_rawMag + (2*i)); - - return buffer; -} -bool LegacyDataPacket::containsRawMagneticField(int32_t index) const -{ - if (dataSize(index) == 0) - return false; - if (m_fixedData->m_infoList[index].m_rawMag == XS_DATA_ITEM_NOT_AVAILABLE) - return false; - return true; -} -bool LegacyDataPacket::setRawMagneticField(const XsUShortVector& vec, int32_t index) -{ - if (dataSize(index) == 0) - return false; - if (m_fixedData->m_infoList[index].m_rawMag == XS_DATA_ITEM_NOT_AVAILABLE) - { - // add - m_fixedData->m_infoList[index].m_rawMag = (uint16_t) m_msg.getDataSize(); - m_msg.resizeData(m_msg.getDataSize() + 3*2); - m_fixedData->m_infoList[index].m_size += 3*2; - } - // update - for (uint16_t i=0;i<3;++i) - m_msg.setDataShort(vec[i], m_fixedData->m_infoList[index].m_rawMag + (2*i)); - return true; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Return the Raw Temperature component of a data item. -uint16_t LegacyDataPacket::rawTemperature(int32_t index, int channel) const -{ - if (!containsRawTemperature(index, channel)) - return 0; - - if (channel) - return m_msg.getDataShort(m_fixedData->m_infoList[index].m_rawGyroTemp[channel-1]); - else - return m_msg.getDataShort(m_fixedData->m_infoList[index].m_rawTemp); -} -bool LegacyDataPacket::containsRawTemperature(int32_t index, int channel) const -{ - if (dataSize(index) == 0) - return false; - if (channel >= 1 + XS_EXTRA_TEMPERATURE_CHANNELS) - return false; - if (channel == 0) - return m_fixedData->m_infoList[index].m_rawTemp != XS_DATA_ITEM_NOT_AVAILABLE; - else - return m_fixedData->m_infoList[index].m_rawGyroTemp[channel-1] != XS_DATA_ITEM_NOT_AVAILABLE; -} -bool LegacyDataPacket::setRawTemperature(const uint16_t temp, int32_t index, int channel) -{ - if (dataSize(index) == 0) - return false; - if (channel >= 1 + XS_EXTRA_TEMPERATURE_CHANNELS) - return false; - - if (m_fixedData->m_infoList[index].m_rawTemp == XS_DATA_ITEM_NOT_AVAILABLE) - { - m_fixedData->m_infoList[index].m_rawTemp = (uint16_t) m_msg.getDataSize(); - m_msg.resizeData(m_msg.getDataSize() + 2); - m_fixedData->m_infoList[index].m_size += 2; - m_msg.setDataShort(temp, m_fixedData->m_infoList[index].m_rawTemp); - } - else if (channel == 0) - { - m_msg.setDataShort(temp, m_fixedData->m_infoList[index].m_rawTemp); - } - - if (channel > 0) - { - if (m_fixedData->m_infoList[index].m_rawGyroTemp[0] == XS_DATA_ITEM_NOT_AVAILABLE) - { - for(int i = 0; i <= XS_EXTRA_TEMPERATURE_CHANNELS; ++i) - { - m_fixedData->m_infoList[index].m_rawGyroTemp[i] = (uint16_t) m_msg.getDataSize(); - m_msg.resizeData(m_msg.getDataSize() + 2); - m_fixedData->m_infoList[index].m_size += 2; - m_msg.setDataShort(temp, m_fixedData->m_infoList[index].m_rawGyroTemp[i]); // set value in all items as initial value - } - } - else - { - m_msg.setDataShort(temp, m_fixedData->m_infoList[index].m_rawGyroTemp[channel-1]); - } - } - return true; -} -int LegacyDataPacket::rawTemperatureChannelCount(int32_t index) const -{ - int count = ((m_fixedData->m_infoList[index].m_rawTemp != XS_DATA_ITEM_NOT_AVAILABLE) ? 1 : 0); - count += ((m_fixedData->m_infoList[index].m_rawGyroTemp[0] != XS_DATA_ITEM_NOT_AVAILABLE) ? XS_EXTRA_TEMPERATURE_CHANNELS : 0); - return count; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Return the Raw Data component of a data item. -XsScrData LegacyDataPacket::rawData(int32_t index) const -{ - XsScrData buffer; - if (containsRawData(index)) - { - const uint8_t* tmp = m_msg.getDataBuffer(m_fixedData->m_infoList[index].m_rawData); - const uint16_t* sh = (const uint16_t*) tmp; - uint16_t* bare = (uint16_t*) &buffer; - - for (uint16_t i=0;i<(9+rawTemperatureChannelCount(index));++i, ++sh, ++bare) - *bare = swapEndian16(*sh);// m_msg.getDataShort(m_fixedData->m_infoList[index].m_rawData + (2*i)); - } - return buffer; -} -bool LegacyDataPacket::containsRawData(int32_t index) const -{ - if (dataSize(index) == 0) - return false; - return (m_fixedData->m_infoList[index].m_rawData != XS_DATA_ITEM_NOT_AVAILABLE); -} -bool LegacyDataPacket::setRawData(const XsScrData& data, int32_t index) -{ - if (dataSize(index) == 0) - return false; - if (m_fixedData->m_infoList[index].m_rawData == XS_DATA_ITEM_NOT_AVAILABLE) - { - // add - m_fixedData->m_infoList[index].m_rawData = (uint16_t) m_msg.getDataSize(); - m_msg.resizeData(m_msg.getDataSize() + 3* 3*2 + 2); - m_fixedData->m_infoList[index].m_rawAcc = m_fixedData->m_infoList[index].m_rawData; - m_fixedData->m_infoList[index].m_rawGyr = m_fixedData->m_infoList[index].m_rawData + 3*2; - m_fixedData->m_infoList[index].m_rawMag = m_fixedData->m_infoList[index].m_rawData + 6*2; - m_fixedData->m_infoList[index].m_rawTemp = m_fixedData->m_infoList[index].m_rawData + 9*2; - for(int i = 0; i < XS_EXTRA_TEMPERATURE_CHANNELS; i++) - { - m_fixedData->m_infoList[index].m_rawGyroTemp[i] = m_fixedData->m_infoList[index].m_rawData + 10*2 + (i * 2); - } - m_fixedData->m_infoList[index].m_size += 3* 3*2 + 2 + (XS_EXTRA_TEMPERATURE_CHANNELS * 2); - } - // update - int16_t* bare = (int16_t*) &data; - for (uint16_t i=0;i<10;++i) - m_msg.setDataShort(bare[i], m_fixedData->m_infoList[index].m_rawData + (2*i)); - for (uint16_t i=0;im_infoList[index].m_rawGyroTemp[i]); - return true; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Return the Gps PVT Data component of a data item. -XsGpsPvtData LegacyDataPacket::gpsPvtData(int32_t index) const -{ - XsGpsPvtData buffer; - if (containsGpsPvtData(index)) - { - //const uint8_t* tmp = m_msg.getDataBuffer(m_fixedData->m_infoList[index].m_gpsPvtData); - //const uint16_t* sh = (const uint16_t*) tmp; - //uint16_t* bare = (uint16_t*) &buffer; - - // pressure data - buffer.m_pressure = m_msg.getDataShort(m_fixedData->m_infoList[index].m_gpsPvtPressure); - // pressAge - buffer.m_pressureAge = m_msg.getDataByte(m_fixedData->m_infoList[index].m_gpsPvtPressureAge); - - // lon,lat,height,hacc,vacc,veln,vele,veld - //tmp = m_msg.getDataBuffer(m_fixedData->m_infoList[index].m_gpsPvtGpsData); - //const uint32_t* ln = (const uint32_t*) tmp; - uint32_t *bareln = (uint32_t*) &buffer.m_itow; - for (uint16_t i=0; i < 10; ++i) - { - //lint --e{662, 661} - if (m_fixedData->m_infoList[index].m_gpsPvtGpsData != XS_DATA_ITEM_NOT_AVAILABLE) - { - bareln[i] = m_msg.getDataLong(m_fixedData->m_infoList[index].m_gpsPvtGpsData + (4*i)); //lint !e661 !e662 - } - else - { - bareln[i] = 0; - } - } - - // gpsAge - if (m_fixedData->m_infoList[index].m_gpsPvtGpsAge != XS_DATA_ITEM_NOT_AVAILABLE) - { - buffer.m_gpsAge = m_msg.getDataByte(m_fixedData->m_infoList[index].m_gpsPvtGpsAge); - } - else - { - buffer.m_gpsAge = 0; - } - } - else - buffer.clear(); - return buffer; -} -bool LegacyDataPacket::containsGpsPvtData(int32_t index) const -{ - if (dataSize(index) == 0) - return false; - if (m_fixedData->m_infoList[index].m_gpsPvtData == XS_DATA_ITEM_NOT_AVAILABLE) - return false; - return true; -} -bool LegacyDataPacket::setGpsPvtData(const XsGpsPvtData& data, int32_t index) -{ - if (dataSize(index) == 0) - return false; - if (m_fixedData->m_infoList[index].m_gpsPvtData == XS_DATA_ITEM_NOT_AVAILABLE) - { - // add - m_fixedData->m_infoList[index].m_gpsPvtData = (uint16_t) m_msg.getDataSize(); - m_msg.resizeData(m_msg.getDataSize() + (2+1) + (40 + 1)); - m_fixedData->m_infoList[index].m_gpsPvtPressure = m_fixedData->m_infoList[index].m_gpsPvtData; - m_fixedData->m_infoList[index].m_gpsPvtPressureAge = m_fixedData->m_infoList[index].m_gpsPvtData + 2; - - m_fixedData->m_infoList[index].m_gpsPvtGpsData = m_fixedData->m_infoList[index].m_gpsPvtData + 3; - m_fixedData->m_infoList[index].m_gpsPvtItow = m_fixedData->m_infoList[index].m_gpsPvtData + 3; - m_fixedData->m_infoList[index].m_gpsPvtLatitude = m_fixedData->m_infoList[index].m_gpsPvtData + 3 + 4; - m_fixedData->m_infoList[index].m_gpsPvtLongitude = m_fixedData->m_infoList[index].m_gpsPvtData + 3 + 8; - m_fixedData->m_infoList[index].m_gpsPvtHeight = m_fixedData->m_infoList[index].m_gpsPvtData + 3 + 12; - m_fixedData->m_infoList[index].m_gpsPvtVeln = m_fixedData->m_infoList[index].m_gpsPvtData + 3 + 16; - m_fixedData->m_infoList[index].m_gpsPvtVele = m_fixedData->m_infoList[index].m_gpsPvtData + 3 + 20; - m_fixedData->m_infoList[index].m_gpsPvtVeld = m_fixedData->m_infoList[index].m_gpsPvtData + 3 + 24; - m_fixedData->m_infoList[index].m_gpsPvtHacc = m_fixedData->m_infoList[index].m_gpsPvtData + 3 + 28; - m_fixedData->m_infoList[index].m_gpsPvtVacc = m_fixedData->m_infoList[index].m_gpsPvtData + 3 + 32; - m_fixedData->m_infoList[index].m_gpsPvtSacc = m_fixedData->m_infoList[index].m_gpsPvtData + 3 + 36; - m_fixedData->m_infoList[index].m_gpsPvtGpsAge = m_fixedData->m_infoList[index].m_gpsPvtData + 3 + 40; - - m_fixedData->m_infoList[index].m_size += (2+1) + (40 + 1); - } - // update - m_msg.setDataShort(data.m_pressure, m_fixedData->m_infoList[index].m_gpsPvtPressure); - m_msg.setDataByte(data.m_pressureAge, m_fixedData->m_infoList[index].m_gpsPvtPressureAge); - - // lon,lat,height,hacc,vacc,veln,vele,veld - int32_t* bareln = (int32_t*)&data.m_itow; - for (uint16_t i=0; i<10;++i) - m_msg.setDataLong(bareln[i],m_fixedData->m_infoList[index].m_gpsPvtGpsData + (4*i)); //lint !e661 !e662 - - // gpsAge - m_msg.setDataByte(data.m_gpsAge, m_fixedData->m_infoList[index].m_gpsPvtGpsAge); - return true; -} - -/*! \brief Return the pressure data component of a data item. - \param index The index of the item of which the data should be returned. - \returns The pressure data component of a data item. -*/ -XsPressure LegacyDataPacket::pressure(int32_t index) const -{ - XsPressure buffer; - if (containsPressure(index)) - { - // pressure data - // MH; \todo need a conversion factor here to go from short to double? - buffer.m_pressure = (double)m_msg.getDataShort(m_fixedData->m_infoList[index].m_gpsPvtPressure); - // pressAge - buffer.m_pressureAge = m_msg.getDataByte(m_fixedData->m_infoList[index].m_gpsPvtPressureAge); - } - else if (containsMtwSdiData(index)) - { - // pressure data - buffer.m_pressure = (double)m_msg.getDataShort(m_fixedData->m_infoList[index].m_wBaroMeter); - // pressAge - buffer.m_pressureAge = (m_msg.getDataByte(m_fixedData->m_infoList[index].m_wAidingData)?0:255); - } - else - buffer = XsPressure(); - return buffer; -} - -/*! \brief Return true if the packet contains pressure data -*/ -bool LegacyDataPacket::containsPressure(int32_t index) const -{ - if (dataSize(index) == 0) - return false; - if (m_fixedData->m_infoList[index].m_gpsPvtPressure == XS_DATA_ITEM_NOT_AVAILABLE && - m_fixedData->m_infoList[index].m_wBaroMeter == XS_DATA_ITEM_NOT_AVAILABLE) - return false; - return true; -} - -/*! \brief Add/update pressure data for the item -*/ -bool LegacyDataPacket::setPressure(const XsPressure& data, int32_t index) -{ - if (dataSize(index) == 0) - return false; - if (m_fixedData->m_infoList[index].m_gpsPvtPressure == XS_DATA_ITEM_NOT_AVAILABLE) - { - // add - m_fixedData->m_infoList[index].m_gpsPvtData = (uint16_t) m_msg.getDataSize(); - m_msg.resizeData(m_msg.getDataSize() + (2+1)); - m_fixedData->m_infoList[index].m_gpsPvtPressure = m_fixedData->m_infoList[index].m_gpsPvtData; - m_fixedData->m_infoList[index].m_gpsPvtPressureAge = m_fixedData->m_infoList[index].m_gpsPvtData + 2; - - m_fixedData->m_infoList[index].m_size += (2+1); - } - // update - m_msg.setDataShort((uint16_t) data.m_pressure, m_fixedData->m_infoList[index].m_gpsPvtPressure); - m_msg.setDataByte(data.m_pressureAge, m_fixedData->m_infoList[index].m_gpsPvtPressureAge); - - return true; -} - -/*! \brief Return the strapdown integration (SDI) data component of a data item. - \param index The index of the item of which the data should be returned. - \returns The SDI data component of a data item. -*/ -MtwSdiData LegacyDataPacket::mtwSdiData(int32_t index) const -{ - MtwSdiData buffer; - if (containsMtwSdiData(index)) - { - buffer.m_deviceId = m_msg.getDataLong(m_fixedData->m_infoList[index].m_wClientId); - buffer.m_timeSync = m_msg.getDataByte(m_fixedData->m_infoList[index].m_wTimeSync); - buffer.m_firstFrameNumber = m_msg.getDataShort(m_fixedData->m_infoList[index].m_wFirstFrameNumber); - buffer.m_lastFrameNumber = m_msg.getDataShort(m_fixedData->m_infoList[index].m_wLastFrameNumber); - m_msg.getDataFPValue(CHECKIFDOUBLE(m_wCurrentBias), &buffer.m_currentBias[0], m_fixedData->m_infoList[index].m_wCurrentBias, 3); - buffer.m_aidingData = (m_msg.getDataByte(m_fixedData->m_infoList[index].m_wAidingData)==1)?true:false; - buffer.m_barometer = m_msg.getDataShort(m_fixedData->m_infoList[index].m_wBaroMeter) / 50.0; - buffer.m_rssi = (int8_t) m_msg.getDataByte(m_fixedData->m_infoList[index].m_wRssi); - - m_msg.getDataFPValue(CHECKIFDOUBLE(m_wOrientationIncrement), &buffer.m_orientationIncrement[0], m_fixedData->m_infoList[index].m_wOrientationIncrement, 4); - m_msg.getDataFPValue(CHECKIFDOUBLE(m_wVelocityIncrement), &buffer.m_velocityIncrement[0], m_fixedData->m_infoList[index].m_wVelocityIncrement, 3); - m_msg.getDataFPValue(CHECKIFDOUBLE(m_wMagnetoMeter), &buffer.m_magnetoMeter[0], m_fixedData->m_infoList[index].m_wMagnetoMeter, 3); - } - else - { - buffer.m_deviceId = 0; - buffer.m_timeSync = 0; - buffer.m_firstFrameNumber = 0; - buffer.m_lastFrameNumber = 0; - buffer.m_currentBias.zero(); - buffer.m_aidingData = 0; - buffer.m_barometer = 0; - buffer.m_rssi = 0; - - buffer.m_orientationIncrement = XsQuaternion::identity(); - buffer.m_velocityIncrement.zero(); - buffer.m_magnetoMeter.zero(); - } - return buffer; -} - -/*! \brief Check if data item contains strapdown integration data - \param index The index of the item of which the data should be returned. - \returns true if the packet contains MTw SDI data -*/ -bool LegacyDataPacket::containsMtwSdiData(int32_t index) const -{ - if (dataSize(index) == 0) - return false; - if (m_fixedData->m_infoList[index].m_mtwSdiData == XS_DATA_ITEM_NOT_AVAILABLE) - return false; - return true; -} - -/*! \brief Add/update strapdown integration data for the item - \param data The updated data - \param index The index of the item of which the data should be returned. - \returns true if the data was successfully updated -*/ -bool LegacyDataPacket::setMtwSdiData(const MtwSdiData& data, int32_t index) -{ - if (m_fixedData->m_infoList[index].m_mtwSdiData == XS_DATA_ITEM_NOT_AVAILABLE) - { - // add - m_fixedData->m_infoList[index].m_mtwSdiData = (uint16_t) m_msg.getDataSize(); - XsSize ds = 8; // add as doubles - m_msg.resizeData(m_msg.getDataSize() + 13 + 13*ds); - - m_fixedData->m_infoList[index].m_wClientId = m_fixedData->m_infoList[index].m_mtwSdiData; - m_fixedData->m_infoList[index].m_wTimeSync = m_fixedData->m_infoList[index].m_mtwSdiData + 4; - m_fixedData->m_infoList[index].m_wFirstFrameNumber = m_fixedData->m_infoList[index].m_mtwSdiData + 5; - m_fixedData->m_infoList[index].m_wLastFrameNumber = m_fixedData->m_infoList[index].m_mtwSdiData + 7; - m_fixedData->m_infoList[index].m_wCurrentBias = m_fixedData->m_infoList[index].m_mtwSdiData + 9; - m_fixedData->m_infoList[index].m_wOrientationIncrement = (uint16_t) (m_fixedData->m_infoList[index].m_mtwSdiData + 9+3*ds); - m_fixedData->m_infoList[index].m_wVelocityIncrement = (uint16_t) (m_fixedData->m_infoList[index].m_mtwSdiData + 9+7*ds); - m_fixedData->m_infoList[index].m_wAidingData = (uint16_t) (m_fixedData->m_infoList[index].m_mtwSdiData + 9+10*ds); - m_fixedData->m_infoList[index].m_wBaroMeter = (uint16_t) (m_fixedData->m_infoList[index].m_mtwSdiData + 10+10*ds); - m_fixedData->m_infoList[index].m_wMagnetoMeter = (uint16_t) (m_fixedData->m_infoList[index].m_mtwSdiData + 12+10*ds); - m_fixedData->m_infoList[index].m_wRssi = (uint16_t) (m_fixedData->m_infoList[index].m_mtwSdiData + 12+13*ds); - m_fixedData->m_infoList[index].m_size += (uint16_t) (13 + 13*ds); - } - // update - m_msg.setDataLong(data.m_deviceId.toInt(), m_fixedData->m_infoList[index].m_wClientId); - m_msg.setDataByte(data.m_timeSync, m_fixedData->m_infoList[index].m_wTimeSync); - m_msg.setDataShort(data.m_firstFrameNumber, m_fixedData->m_infoList[index].m_wFirstFrameNumber); - m_msg.setDataShort(data.m_lastFrameNumber, m_fixedData->m_infoList[index].m_wLastFrameNumber); - m_msg.setDataFPValue(CHECKIFDOUBLE(m_wCurrentBias), data.m_currentBias.data(), m_fixedData->m_infoList[index].m_wCurrentBias, 3); - m_msg.setDataFPValue(CHECKIFDOUBLE(m_wOrientationIncrement), data.m_orientationIncrement.data(), m_fixedData->m_infoList[index].m_wOrientationIncrement, 4); - m_msg.setDataFPValue(CHECKIFDOUBLE(m_wVelocityIncrement), data.m_velocityIncrement.data(), m_fixedData->m_infoList[index].m_wVelocityIncrement, 3); - m_msg.setDataByte((data.m_aidingData==true)?1:0, m_fixedData->m_infoList[index].m_wAidingData); - m_msg.setDataShort((uint16_t)(XsMath_doubleToLong(data.m_barometer * 50)), m_fixedData->m_infoList[index].m_wBaroMeter); - m_msg.setDataFPValue(CHECKIFDOUBLE(m_wMagnetoMeter), data.m_magnetoMeter.data(), m_fixedData->m_infoList[index].m_wMagnetoMeter, 3); - m_msg.setDataByte(data.m_rssi, m_fixedData->m_infoList[index].m_wRssi); - - return true; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Return the Temperature component of a data item. -double LegacyDataPacket::temperature(int32_t index, int channel) const -{ - if (containsTemperature(index, channel)) - { - if (!channel) - return m_msg.getDataFPValue(CHECKIFDOUBLE(m_temp), m_fixedData->m_infoList[index].m_temp); - else - return m_msg.getDataFPValue(CHECKIFDOUBLE(m_gyroTemp[channel-1]), m_fixedData->m_infoList[index].m_gyroTemp[channel-1]); - } - - return 0.0; -} -bool LegacyDataPacket::containsTemperature(int32_t index, int channel) const -{ - if (dataSize(index) == 0) - return false; - if (channel >= 1 + XS_EXTRA_TEMPERATURE_CHANNELS) - return false; - if (!channel) - return m_fixedData->m_infoList[index].m_temp != XS_DATA_ITEM_NOT_AVAILABLE; - else - return m_fixedData->m_infoList[index].m_gyroTemp[channel-1] != XS_DATA_ITEM_NOT_AVAILABLE; -} -bool LegacyDataPacket::setTemperature(double temp, int32_t index, int channel) -{ - if (dataSize(index) == 0) - return false; - if (channel >= 1 + XS_EXTRA_TEMPERATURE_CHANNELS) - return false; - - if (m_fixedData->m_infoList[index].m_temp == XS_DATA_ITEM_NOT_AVAILABLE) - { - uint16_t ds = 8; - - m_fixedData->m_infoList[index].m_temp = (uint16_t) m_msg.getDataSize(); - m_msg.resizeData(m_msg.getDataSize() + ds); - m_fixedData->m_infoList[index].m_size += ds; - m_msg.setDataFPValue(CHECKIFDOUBLE(m_temp), temp, m_fixedData->m_infoList[index].m_temp); - } - else if (channel == 0) - { - m_msg.setDataFPValue(CHECKIFDOUBLE(m_temp), temp, m_fixedData->m_infoList[index].m_temp); - } - - if (channel > 0) - { - if (m_fixedData->m_infoList[index].m_gyroTemp[0] == XS_DATA_ITEM_NOT_AVAILABLE) - { - uint16_t ds = 8; - for(int i = 0; i <= XS_EXTRA_TEMPERATURE_CHANNELS; ++i) - { - m_fixedData->m_infoList[index].m_gyroTemp[i] = (uint16_t) m_msg.getDataSize(); - m_msg.resizeData(m_msg.getDataSize() + ds); - m_fixedData->m_infoList[index].m_size += ds; - m_msg.setDataFPValue(CHECKIFDOUBLE(m_temp), temp, m_fixedData->m_infoList[index].m_gyroTemp[i]); // set value in all items as initial value - } - } - else - { - m_msg.setDataFPValue(CHECKIFDOUBLE(m_temp), temp, m_fixedData->m_infoList[index].m_gyroTemp[channel-1]); - } - } - return true; -} -int LegacyDataPacket::temperatureChannelCount(int32_t index) const -{ - int count = ((m_fixedData->m_infoList[index].m_temp != XS_DATA_ITEM_NOT_AVAILABLE) ? 1 : 0); - count += ((m_fixedData->m_infoList[index].m_gyroTemp[0] != XS_DATA_ITEM_NOT_AVAILABLE) ? XS_EXTRA_TEMPERATURE_CHANNELS : 0); - return count; -} -////////////////////////////////////////////////////////////////////////////////////////// -// Return the Calibrated Accelerometer component of a data item. -XsVector LegacyDataPacket::calibratedAcceleration(int32_t index) const -{ - XsVector3 buffer; - if (containsCalibratedAcceleration(index)) - m_msg.getDataFPValue(CHECKIFDOUBLE(m_calAcc), &buffer[0], m_fixedData->m_infoList[index].m_calAcc, 3); - else - buffer.zero(); - return buffer; -} -bool LegacyDataPacket::containsCalibratedAcceleration(int32_t index) const -{ - if (dataSize(index) == 0) - return false; - if (m_fixedData->m_infoList[index].m_calAcc == XS_DATA_ITEM_NOT_AVAILABLE) - return false; - return true; -} -bool LegacyDataPacket::setCalibratedAcceleration(const XsVector& vec, int32_t index) -{ - const uint16_t numValues = 3; - if (dataSize(index) == 0) - return false; - - uint16_t ds = getFPValueSize(index); - - if (m_fixedData->m_infoList[index].m_calAcc == XS_DATA_ITEM_NOT_AVAILABLE || !ISDOUBLE(m_calAcc)) - { - // add - ds = 8; - //m_msg.m_autoUpdateChecksum = false; - - m_fixedData->m_infoList[index].m_calAcc = (uint16_t) m_msg.getDataSize(); - m_msg.resizeData(m_msg.getDataSize() + numValues*ds); - m_fixedData->m_infoList[index].m_size += numValues*ds; - } - // update - m_msg.setDataFPValue(CHECKIFDOUBLE(m_calAcc), vec.data(), m_fixedData->m_infoList[index].m_calAcc, numValues); - return true; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Return the Calibrated Gyroscope component of a data item. -XsVector LegacyDataPacket::calibratedGyroscopeData(int32_t index) const -{ - XsVector3 buffer; - if (containsCalibratedGyroscopeData(index)) - m_msg.getDataFPValue(CHECKIFDOUBLE(m_calGyr), &buffer[0], m_fixedData->m_infoList[index].m_calGyr, 3); - else - buffer.zero(); - return buffer; -} -bool LegacyDataPacket::containsCalibratedGyroscopeData(int32_t index) const -{ - if (dataSize(index) == 0) - return false; - if (m_fixedData->m_infoList[index].m_calGyr == XS_DATA_ITEM_NOT_AVAILABLE) - return false; - return true; -} -bool LegacyDataPacket::setCalibratedGyroscopeData(const XsVector& vec, int32_t index) -{ - const uint16_t numValues = 3; - if (dataSize(index) == 0) - return false; - - uint16_t ds = getFPValueSize(index); - - if (m_fixedData->m_infoList[index].m_calGyr == XS_DATA_ITEM_NOT_AVAILABLE || !ISDOUBLE(m_calGyr)) - { - // add - ds = 8; - //m_msg.m_autoUpdateChecksum = false; - - m_fixedData->m_infoList[index].m_calGyr = (uint16_t) m_msg.getDataSize(); - m_msg.resizeData(m_msg.getDataSize() + numValues*ds); - m_fixedData->m_infoList[index].m_size += numValues*ds; - } - // update - m_msg.setDataFPValue(CHECKIFDOUBLE(m_calGyr), vec.data(), m_fixedData->m_infoList[index].m_calGyr, numValues); - return true; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Return the Calibrated Magnetometer component of a data item. -XsVector LegacyDataPacket::calibratedMagneticField(int32_t index) const -{ - XsVector3 buffer; - if (containsCalibratedMagneticField(index)) - m_msg.getDataFPValue(CHECKIFDOUBLE(m_calMag), &buffer[0], m_fixedData->m_infoList[index].m_calMag, 3); - else - buffer.zero(); - return buffer; -} -bool LegacyDataPacket::containsCalibratedMagneticField(int32_t index) const -{ - if (dataSize(index) == 0) - return false; - if (m_fixedData->m_infoList[index].m_calMag == XS_DATA_ITEM_NOT_AVAILABLE) - return false; - return true; -} -bool LegacyDataPacket::setCalibratedMagneticField(const XsVector& vec, int32_t index) -{ - const uint16_t numValues = 3; - if (dataSize(index) == 0) - return false; - - uint16_t ds = getFPValueSize(index); - - if (m_fixedData->m_infoList[index].m_calMag == XS_DATA_ITEM_NOT_AVAILABLE || !ISDOUBLE(m_calMag)) - { - // add - ds = 8; - //m_msg.m_autoUpdateChecksum = false; - - m_fixedData->m_infoList[index].m_calMag = (uint16_t) m_msg.getDataSize(); - m_msg.resizeData(m_msg.getDataSize() + numValues*ds); - m_fixedData->m_infoList[index].m_size += numValues*ds; - } - // update - m_msg.setDataFPValue(CHECKIFDOUBLE(m_calMag), vec.data(), m_fixedData->m_infoList[index].m_calMag, numValues); - return true; -} - -/*! \brief Return the Calibrated Data component of a data item. - \param index The index of the item of which the data should be returned. - \returns The Calibrated Data component of a data item. -*/ -XsCalibratedData LegacyDataPacket::calibratedData(int32_t index) const -{ - XsCalibratedData buffer; - if (containsCalibratedData(index)) - { - //lint --e{419} - double* bare = &buffer.m_acc[0]; - if (m_fixedData->m_infoList[index].m_calAcc == XS_DATA_ITEM_NOT_AVAILABLE) - memset(bare, 0, 3*sizeof(double)); - else - m_msg.getDataFPValue(CHECKIFDOUBLE(m_calAcc), bare, m_fixedData->m_infoList[index].m_calAcc, 3); - - bare = &buffer.m_gyr[0]; - if (m_fixedData->m_infoList[index].m_calGyr == XS_DATA_ITEM_NOT_AVAILABLE) - memset(bare, 0, 3*sizeof(double)); - else - m_msg.getDataFPValue(CHECKIFDOUBLE(m_calGyr), bare, m_fixedData->m_infoList[index].m_calGyr, 3); - - bare = &buffer.m_mag[0]; - if (m_fixedData->m_infoList[index].m_calMag == XS_DATA_ITEM_NOT_AVAILABLE) - memset(bare, 0, 3*sizeof(double)); - else - m_msg.getDataFPValue(CHECKIFDOUBLE(m_calMag), bare, m_fixedData->m_infoList[index].m_calMag, 3); - } - else - { - buffer.m_acc.zero(); - buffer.m_gyr.zero(); - buffer.m_mag.zero(); - } - return buffer; -} - -/*! \brief Check if data item contains Calibrated Data - \param index The index of the item of which the data should be returned. - \returns true if the packet contains Calibrated Data -*/ -bool LegacyDataPacket::containsCalibratedData(int32_t index) const -{ - if (dataSize(index) == 0) - return false; - return (m_fixedData->m_infoList[index].m_calData != XS_DATA_ITEM_NOT_AVAILABLE); -} - -/*! \brief Add/update Calibrated Data for the item */ -bool LegacyDataPacket::setCalibratedData(const XsCalibratedData& data, int32_t index) -{ - const uint16_t numValues = 9; - if (dataSize(index) == 0) - return false; - - uint16_t ds = getFPValueSize(index); - - if (m_fixedData->m_infoList[index].m_calData == XS_DATA_ITEM_NOT_AVAILABLE || !ISDOUBLE(m_calData)) - { - // add - ds = 8; // added values are always in double precision - //m_msg.m_autoUpdateChecksum = false; - - m_fixedData->m_infoList[index].m_calData = (uint16_t) m_msg.getDataSize(); - m_msg.resizeData(m_msg.getDataSize() + numValues*ds); - m_fixedData->m_infoList[index].m_calAcc = m_fixedData->m_infoList[index].m_calData; - m_fixedData->m_infoList[index].m_calGyr = m_fixedData->m_infoList[index].m_calData + 3*ds; - m_fixedData->m_infoList[index].m_calMag = m_fixedData->m_infoList[index].m_calData + 6*ds; - m_fixedData->m_infoList[index].m_size += numValues*ds; - } - // update - double* bare = (double*) &data; - if (m_fixedData->m_infoList[index].m_calAcc != XS_DATA_ITEM_NOT_AVAILABLE) - m_msg.setDataFPValue(CHECKIFDOUBLE(m_calAcc), bare, m_fixedData->m_infoList[index].m_calAcc, 3); - bare += 3; - if (m_fixedData->m_infoList[index].m_calGyr != XS_DATA_ITEM_NOT_AVAILABLE) - m_msg.setDataFPValue(CHECKIFDOUBLE(m_calGyr), bare, m_fixedData->m_infoList[index].m_calGyr, 3); - bare += 3; - if (m_fixedData->m_infoList[index].m_calMag != XS_DATA_ITEM_NOT_AVAILABLE) - m_msg.setDataFPValue(CHECKIFDOUBLE(m_calMag), bare, m_fixedData->m_infoList[index].m_calMag, 3); - - return true; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Return the Orientation component of a data item as a Quaternion. -XsQuaternion LegacyDataPacket::orientationQuaternion(int32_t index) const -{ - XsQuaternion buffer; - if (containsOrientationQuaternion(index)) - m_msg.getDataFPValue(CHECKIFDOUBLE(m_oriQuat), &buffer[0], m_fixedData->m_infoList[index].m_oriQuat, 4); - else - memset(&buffer, 0, sizeof(buffer)); - return buffer; -} - -bool LegacyDataPacket::containsOrientationQuaternion(int32_t index) const -{ - if (dataSize(index) == 0) - return false; - if (m_fixedData->m_infoList[index].m_oriQuat == XS_DATA_ITEM_NOT_AVAILABLE) - return false; - return true; -} -bool LegacyDataPacket::setOrientationQuaternion(const XsQuaternion& data, int32_t index) -{ - const uint16_t numValues = 4; - if (dataSize(index) == 0) - return false; - - uint16_t ds = getFPValueSize(index); - - if (m_fixedData->m_infoList[index].m_oriQuat == XS_DATA_ITEM_NOT_AVAILABLE || !ISDOUBLE(m_oriQuat)) - { - // add - ds = 8; // added values are always in double precision - //m_msg.m_autoUpdateChecksum = false; - - m_fixedData->m_infoList[index].m_oriQuat = (uint16_t) m_msg.getDataSize(); - m_msg.resizeData(m_msg.getDataSize() + numValues*ds); - m_fixedData->m_infoList[index].m_size += numValues*ds; - - double* bare = (double*) &data; - m_msg.setDataFPValue((m_fixedData->m_formatList[index].m_outputSettings & XOS_Dataformat_Mask)|XOS_Dataformat_Double, - bare, m_fixedData->m_infoList[index].m_oriQuat, numValues); - return true; - } - // update - m_msg.setDataFPValue(CHECKIFDOUBLE(m_oriQuat), data.data(), m_fixedData->m_infoList[index].m_oriQuat, numValues); - return true; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Return the Orientation component of a data item as XsEuler angles. -XsEuler LegacyDataPacket::orientationEuler(int32_t index) const -{ - XsEuler buffer; - if (containsOrientationEuler(index)) - { - - m_msg.getDataFPValue(CHECKIFDOUBLE(m_oriEul), &buffer[0], m_fixedData->m_infoList[index].m_oriEul, 3); - } - else - memset(&buffer, 0, sizeof(buffer)); - return buffer; -} -bool LegacyDataPacket::containsOrientationEuler(int32_t index) const -{ - if (dataSize(index) == 0) - return false; - if (m_fixedData->m_infoList[index].m_oriEul == XS_DATA_ITEM_NOT_AVAILABLE) - return false; - return true; -} -bool LegacyDataPacket::setOrientationEuler(const XsEuler& data, int32_t index) -{ - const uint16_t numValues = 3; - if (dataSize(index) == 0) - return false; - - uint16_t ds = getFPValueSize(index); - - if (m_fixedData->m_infoList[index].m_oriEul == XS_DATA_ITEM_NOT_AVAILABLE || !ISDOUBLE(m_oriEul)) - { - // add - ds = 8; // added values are always in double precision - //m_msg.m_autoUpdateChecksum = false; - - m_fixedData->m_infoList[index].m_oriEul = (uint16_t) m_msg.getDataSize(); - m_msg.resizeData(m_msg.getDataSize() + numValues*ds); - m_fixedData->m_infoList[index].m_size += numValues*ds; - } - // update - m_msg.setDataFPValue(CHECKIFDOUBLE(m_oriEul), data.data(), m_fixedData->m_infoList[index].m_oriEul, numValues); - return true; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Return the Orientation component of a data item as an Orientation Matrix. -XsMatrix LegacyDataPacket::orientationMatrix(int32_t index) const -{ - XsMatrix3x3 buffer; - uint16_t k = 0; - if (containsOrientationMatrix(index)) - { - // remember to use column major order in the xbus message! - uint16_t ds = getFPValueSize(index); - for (int32_t i=0;i<3;++i) - for (int32_t j=0;j<3;++j, k+=ds) - buffer.setValue(j, i, m_msg.getDataFPValue(CHECKIFDOUBLE(m_oriMat), m_fixedData->m_infoList[index].m_oriMat+k)); - } - else - buffer.zero(); - - return buffer; -} -bool LegacyDataPacket::containsOrientationMatrix(int32_t index) const -{ - if (dataSize(index) == 0) - return false; - if (m_fixedData->m_infoList[index].m_oriMat == XS_DATA_ITEM_NOT_AVAILABLE) - return false; - return true; -} -bool LegacyDataPacket::setOrientationMatrix(const XsMatrix& data, int32_t index) -{ - const uint16_t numValues = 9; - if (dataSize(index) == 0) - return false; - - uint16_t ds = getFPValueSize(index); - - // remember to use column major order in the xbus message! - if (m_fixedData->m_infoList[index].m_oriMat == XS_DATA_ITEM_NOT_AVAILABLE || !ISDOUBLE(m_oriMat)) - { - // add - ds = 8; // added values are always in double precision - //m_msg.m_autoUpdateChecksum = false; - - m_fixedData->m_infoList[index].m_oriMat = (uint16_t) m_msg.getDataSize(); - m_msg.resizeData(m_msg.getDataSize() + numValues*ds); - m_fixedData->m_infoList[index].m_size += numValues*ds; - } - // update - uint16_t k = 0; - for (int32_t i=0;i<3;++i) - for (int32_t j=0;j<3;++j, k+=ds) - m_msg.setDataFPValue(CHECKIFDOUBLE(m_oriMat), data.value(j, i), m_fixedData->m_infoList[index].m_oriMat+k); - return true; -} - -bool LegacyDataPacket::containsOrientation(int32_t index) const -{ - if (dataSize(index) == 0) - return false; - if (m_fixedData->m_infoList[index].m_oriEul == XS_DATA_ITEM_NOT_AVAILABLE && - m_fixedData->m_infoList[index].m_oriMat == XS_DATA_ITEM_NOT_AVAILABLE && - m_fixedData->m_infoList[index].m_oriQuat == XS_DATA_ITEM_NOT_AVAILABLE) - return false; - return true; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Return the AnalogIn 1 component of a data item. -XsAnalogInData LegacyDataPacket::analogIn1Data(int32_t index) const -{ - XsAnalogInData buffer; - if (containsAnalogIn1Data(index)) - buffer.m_data = m_msg.getDataShort(m_fixedData->m_infoList[index].m_analogIn1); - - return buffer; -} -bool LegacyDataPacket::containsAnalogIn1Data(int32_t index) const -{ - if (dataSize(index) == 0) - return false; - if (m_fixedData->m_infoList[index].m_analogIn1 == XS_DATA_ITEM_NOT_AVAILABLE) - return false; - return true; -} -bool LegacyDataPacket::setAnalogIn1Data(const XsAnalogInData& data, int32_t index) -{ - if (dataSize(index) == 0) - return false; - if (m_fixedData->m_infoList[index].m_analogIn1 == XS_DATA_ITEM_NOT_AVAILABLE) - { - // add - m_fixedData->m_infoList[index].m_analogIn1 = (uint16_t) m_msg.getDataSize(); - m_msg.resizeData(m_msg.getDataSize() + 2); - m_fixedData->m_infoList[index].m_size += 2; - } - // update - m_msg.setDataShort(data.m_data, m_fixedData->m_infoList[index].m_analogIn1); - return true; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Return the AnalogIn 2 component of a data item. -XsAnalogInData LegacyDataPacket::analogIn2Data(int32_t index) const -{ - XsAnalogInData buffer; - if (containsAnalogIn2Data(index)) - buffer.m_data = m_msg.getDataShort(m_fixedData->m_infoList[index].m_analogIn2); - - return buffer; -} -bool LegacyDataPacket::containsAnalogIn2Data(int32_t index) const -{ - if (dataSize(index) == 0) - return false; - if (m_fixedData->m_infoList[index].m_analogIn2 == XS_DATA_ITEM_NOT_AVAILABLE) - return false; - return true; -} -bool LegacyDataPacket::setAnalogIn2Data(const XsAnalogInData& data, int32_t index) -{ - if (dataSize(index) == 0) - return false; - if (m_fixedData->m_infoList[index].m_analogIn2 == XS_DATA_ITEM_NOT_AVAILABLE) - { - // add - m_fixedData->m_infoList[index].m_analogIn2 = (uint16_t) m_msg.getDataSize(); - m_msg.resizeData(m_msg.getDataSize() + 2); - m_fixedData->m_infoList[index].m_size += 2; - } - // update - m_msg.setDataShort(data.m_data, m_fixedData->m_infoList[index].m_analogIn2); - return true; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Return the Position LLA component of a data item. -XsVector LegacyDataPacket::positionLLA(int32_t index) const -{ - XsVector3 buffer; - if (containsPositionLLA(index)) - m_msg.getDataFPValue(CHECKIFDOUBLE(m_posLLA), &buffer[0], m_fixedData->m_infoList[index].m_posLLA, 3); - else - buffer.zero(); - return buffer; -} -bool LegacyDataPacket::containsPositionLLA(int32_t index) const -{ - if (dataSize(index) == 0) - return false; - if (m_fixedData->m_infoList[index].m_posLLA == XS_DATA_ITEM_NOT_AVAILABLE) - return false; - return true; -} -bool LegacyDataPacket::setPositionLLA(const XsVector& data, int32_t index) -{ - const uint16_t numValues = 3; - if (dataSize(index) == 0) - return false; - - uint16_t ds = getFPValueSize(index); - - if (m_fixedData->m_infoList[index].m_posLLA == XS_DATA_ITEM_NOT_AVAILABLE || !ISDOUBLE(m_posLLA)) - { - // add - ds = 8; // added values are always in double precision - //m_msg.m_autoUpdateChecksum = false; - - m_fixedData->m_infoList[index].m_posLLA = (uint16_t) m_msg.getDataSize(); - m_msg.resizeData(m_msg.getDataSize() + numValues*ds); - m_fixedData->m_infoList[index].m_size += numValues*ds; - } - // update - m_msg.setDataFPValue(CHECKIFDOUBLE(m_posLLA), data.data(), m_fixedData->m_infoList[index].m_posLLA, numValues); - return true; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Return the Velocity NED component of a data item. -XsVector LegacyDataPacket::velocity(int32_t index) const -{ - XsVector3 buffer; - if (containsVelocity(index)) - m_msg.getDataFPValue(CHECKIFDOUBLE(m_velNEDorNWU), &buffer[0], m_fixedData->m_infoList[index].m_velNEDorNWU, 3); - else - buffer.zero(); - return buffer; -} -bool LegacyDataPacket::containsVelocity(int32_t index) const -{ - if (dataSize(index) == 0) - return false; - if (m_fixedData->m_infoList[index].m_velNEDorNWU == XS_DATA_ITEM_NOT_AVAILABLE) - return false; - return true; -} -bool LegacyDataPacket::setVelocity(const XsVector& data, int32_t index) -{ - const uint16_t numValues = 3; - if (dataSize(index) == 0) - return false; - - uint16_t ds = getFPValueSize(index); - - if (m_fixedData->m_infoList[index].m_velNEDorNWU == XS_DATA_ITEM_NOT_AVAILABLE || !ISDOUBLE(m_posLLA)) - { - // add - ds = 8; // added values are always in double precision - //m_msg.m_autoUpdateChecksum = false; - - m_fixedData->m_infoList[index].m_velNEDorNWU = (uint16_t) m_msg.getDataSize(); - m_msg.resizeData(m_msg.getDataSize() + numValues*ds); - m_fixedData->m_infoList[index].m_size += numValues*ds; - } - // update - m_msg.setDataFPValue(CHECKIFDOUBLE(m_velNEDorNWU), data.data(), m_fixedData->m_infoList[index].m_velNEDorNWU, numValues); - return true; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Return the Status component of a data item. -uint32_t LegacyDataPacket::status(int32_t index, bool* outIsDetailed) const -{ - assert(outIsDetailed != 0); - if (containsStatus(index)) - { - if (m_fixedData->m_infoList[index].m_detailedStatus == XS_DATA_ITEM_NOT_AVAILABLE) - { - *outIsDetailed = false; - return m_msg.getDataByte(m_fixedData->m_infoList[index].m_status); - } - else - { - *outIsDetailed = true; - uint32_t tmp = m_msg.getDataLong(m_fixedData->m_infoList[index].m_detailedStatus); - return tmp; - } - } - else - { - return 0; - } -} -bool LegacyDataPacket::containsStatus(int32_t index) const -{ - if (dataSize(index) == 0) - return false; - if (m_fixedData->m_infoList[index].m_status == XS_DATA_ITEM_NOT_AVAILABLE) - return false; - return true; -} -bool LegacyDataPacket::containsDetailedStatus(int32_t index) const -{ - if (dataSize(index) == 0) - return false; - if (m_fixedData->m_infoList[index].m_detailedStatus == XS_DATA_ITEM_NOT_AVAILABLE) - return false; - return true; -} -bool LegacyDataPacket::setStatus(const uint32_t data, int32_t index) -{ - if (dataSize(index) == 0) - return false; - - if (m_fixedData->m_infoList[index].m_status == XS_DATA_ITEM_NOT_AVAILABLE) - { - // add - m_fixedData->m_infoList[index].m_status = (uint16_t) m_msg.getDataSize(); - m_fixedData->m_infoList[index].m_detailedStatus = (uint16_t) m_msg.getDataSize(); - m_msg.resizeData(m_msg.getDataSize() + 4); - m_fixedData->m_infoList[index].m_size += 4; - } - // update - if (m_fixedData->m_infoList[index].m_detailedStatus == XS_DATA_ITEM_NOT_AVAILABLE) - m_msg.setDataByte(data&0xFF, m_fixedData->m_infoList[index].m_status); - else - m_msg.setDataLong(data, m_fixedData->m_infoList[index].m_detailedStatus); - - return true; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Return the Sample Counter component of the packet. -uint16_t LegacyDataPacket::packetCounter(int32_t index) const -{ - if (!containsPacketCounter(index)) - return 0; - return m_msg.getDataShort(m_fixedData->m_infoList[index].m_sc); -} -bool LegacyDataPacket::containsPacketCounter(int32_t index) const -{ - if (dataSize(index) == 0) - return false; - if (m_fixedData->m_infoList[index].m_sc == XS_DATA_ITEM_NOT_AVAILABLE) - return false; - return true; -} -bool LegacyDataPacket::setPacketCounter(const uint16_t counter, int32_t index) -{ - if (dataSize(index) == 0) - return false; - if (m_fixedData->m_infoList[index].m_sc == XS_DATA_ITEM_NOT_AVAILABLE) - { - // add - m_fixedData->m_infoList[index].m_sc = (uint16_t) m_msg.getDataSize(); - m_msg.resizeData(m_msg.getDataSize() + 2); - m_fixedData->m_infoList[index].m_size += 2; - } - // update - m_msg.setDataShort(counter, m_fixedData->m_infoList[index].m_sc); - return true; -} - - -////////////////////////////////////////////////////////////////////////////////////////// -// Return the SampleTimeFine component of the packet. -uint32_t LegacyDataPacket::sampleTimeFine(int32_t index) const -{ - if (!containsSampleTimeFine(index)) - return 0; - return m_msg.getDataLong(m_fixedData->m_infoList[index].m_sampleTimeFine); -} -bool LegacyDataPacket::containsSampleTimeFine(int32_t index) const -{ - if (dataSize(index) == 0) - return false; - if (m_fixedData->m_infoList[index].m_sampleTimeFine == XS_DATA_ITEM_NOT_AVAILABLE) - return false; - return true; -} -bool LegacyDataPacket::setSampleTimeFine(uint32_t counter, int32_t index) -{ - if (dataSize(index) == 0) - return false; - if (m_fixedData->m_infoList[index].m_sampleTimeFine == XS_DATA_ITEM_NOT_AVAILABLE) - { - // add - m_fixedData->m_infoList[index].m_sampleTimeFine = (uint16_t) m_msg.getDataSize(); - m_msg.resizeData(m_msg.getDataSize() + 4); - m_fixedData->m_infoList[index].m_size += 4; - } - // update - m_msg.setDataLong(counter, m_fixedData->m_infoList[index].m_sampleTimeFine); - return true; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Return the UTC Time component of the packet. -XsUtcTime LegacyDataPacket::utcTime(int32_t index) const -{ - XsUtcTime buffer = {}; - if (containsUtcTime(index)) - { - buffer.m_nano = m_msg.getDataLong(m_fixedData->m_infoList[index].m_utcNano); - buffer.m_year = m_msg.getDataShort(m_fixedData->m_infoList[index].m_utcYear); - - // month, day, hour, minute, second and valid - uint8_t *bareByte = (uint8_t*) &buffer.m_month; - for (uint16_t i=0; i < 6; ++i) - bareByte[i] = m_msg.getDataByte(m_fixedData->m_infoList[index].m_utcMonth + i); //lint !e661 !e662 - } - return buffer; -} -bool LegacyDataPacket::containsUtcTime(int32_t index) const -{ - if (dataSize(index) == 0) - return false; - if (m_fixedData->m_infoList[index].m_utcTime == XS_DATA_ITEM_NOT_AVAILABLE) - return false; - return true; -} -bool LegacyDataPacket::setUtcTime(const XsUtcTime& data, int32_t index) -{ - if (dataSize(index) == 0) - return false; - if (m_fixedData->m_infoList[index].m_utcTime == XS_DATA_ITEM_NOT_AVAILABLE) - { - // add - m_fixedData->m_infoList[index].m_utcTime = (uint16_t) m_msg.getDataSize(); - m_msg.resizeData(m_msg.getDataSize() + 12); - m_fixedData->m_infoList[index].m_utcNano = m_fixedData->m_infoList[index].m_utcTime; - m_fixedData->m_infoList[index].m_utcYear = m_fixedData->m_infoList[index].m_utcTime + 4; - m_fixedData->m_infoList[index].m_utcMonth = m_fixedData->m_infoList[index].m_utcTime + 6; - m_fixedData->m_infoList[index].m_utcDay = m_fixedData->m_infoList[index].m_utcTime + 7; - m_fixedData->m_infoList[index].m_utcHour = m_fixedData->m_infoList[index].m_utcTime + 8; - m_fixedData->m_infoList[index].m_utcMinute = m_fixedData->m_infoList[index].m_utcTime + 9; - m_fixedData->m_infoList[index].m_utcSecond = m_fixedData->m_infoList[index].m_utcTime + 10; - m_fixedData->m_infoList[index].m_utcValid = m_fixedData->m_infoList[index].m_utcTime + 11; - - m_fixedData->m_infoList[index].m_size += 12; - } - // update - m_msg.setDataLong(data.m_nano, m_fixedData->m_infoList[index].m_utcNano); - m_msg.setDataShort(data.m_year, m_fixedData->m_infoList[index].m_utcYear); - - // month, day, hour, minute, second and valid - int8_t* bareByte = (int8_t*)&data.m_month; - for (uint16_t i=0; i<6;++i) - m_msg.setDataByte(bareByte[i],m_fixedData->m_infoList[index].m_utcMonth + i); //lint !e661 !e662 - - return true; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Return the XKF-3 Acc G component of the packet. -XsVector LegacyDataPacket::freeAcceleration(int32_t index) const -{ - XsVector3 buffer; - if (containsFreeAcceleration(index)) - m_msg.getDataFPValue(CHECKIFDOUBLE(m_acc_g), &buffer[0], m_fixedData->m_infoList[index].m_acc_g, 3); - else - buffer.zero(); - return buffer; -} -bool LegacyDataPacket::containsFreeAcceleration(int32_t index) const -{ - if (dataSize(index) == 0) - return false; - if (m_fixedData->m_infoList[index].m_acc_g == XS_DATA_ITEM_NOT_AVAILABLE) - return false; - return true; -} -bool LegacyDataPacket::setFreeAcceleration(const XsVector& g, int32_t index) -{ - const uint16_t numValues = 3; - if (dataSize(index) == 0) - return false; - - uint16_t ds = getFPValueSize(index); - - if (m_fixedData->m_infoList[index].m_acc_g == XS_DATA_ITEM_NOT_AVAILABLE || !ISDOUBLE(m_acc_g)) - { - // add - ds = 8; // added values are always in double precision - //m_msg.m_autoUpdateChecksum = false; - - m_fixedData->m_infoList[index].m_acc_g = (uint16_t) m_msg.getDataSize(); - m_msg.resizeData(m_msg.getDataSize() + numValues*ds); - m_fixedData->m_infoList[index].m_size += numValues*ds; - } - // update - m_msg.setDataFPValue(CHECKIFDOUBLE(m_acc_g), g.data(), m_fixedData->m_infoList[index].m_acc_g, numValues); - return true; -} - -XsTimeStamp LegacyDataPacket::triggerIndication( int channelID ) const -{ - switch(channelID) - { - case 1: - return m_triggerIn1; - case 2: - return m_triggerIn2; - - default: - return XsTimeStamp(); - } -} - -bool LegacyDataPacket::containsTriggerIndication( int channelID /*= 0*/ ) const -{ - switch(channelID) - { - case 0: - return (m_triggerIn1.msTime() == 0 && m_triggerIn2.msTime() == 0) ? false : true; - case 1: - return (m_triggerIn1.msTime() == 0) ? false : true; - case 2: - return (m_triggerIn2.msTime() == 0) ? false : true; - - default: - return false; - } -} - -bool LegacyDataPacket::setTriggerIndication( int channelID, const XsTimeStamp &t ) -{ - switch(channelID) - { - case 1: - m_triggerIn1 = t; - return true; - case 2: - m_triggerIn2 = t; - return true; - } - - return false; -} -//lint +esym(613, LegacyDataPacket::m_fixedData) diff --git a/extern/xcommunication/messageextractor.cpp b/extern/xcommunication/messageextractor.cpp deleted file mode 100644 index ffd8b8a..0000000 --- a/extern/xcommunication/messageextractor.cpp +++ /dev/null @@ -1,162 +0,0 @@ -/* Copyright (c) 2003-2017 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. -*/ - -#include "messageextractor.h" -#include "xcommunicationconfig.h" -#include - -/*! \class MessageExtractor - - Helper class that extracts XsMessages from a stream of data. The user must call the \a processNewData function every time a new block of data is available. - It is advised not to process too small blocks of data (e.g. per byte); Every single message must not span more than \a m_maxIncompleteRetryCount blocks - to guarantee correct operation. - - A MessageExtractor object maintains a buffer representing a sliding window over the data stream that is just big enough to contain any incompletely received - XsMessage. The user can explicitly clear this buffer using the \a clearBuffer function -*/ - -const int MessageExtractor::m_maxIncompleteRetryCount = 5; //!< The maximum number of process attempts before advancing over an incompletely received message - - -/*! \brief Constructor - \param protocolManager: the protocol manager to use for finding messages in the buffered data -*/ -MessageExtractor::MessageExtractor(std::shared_ptr protocolManager) - : m_protocolManager { protocolManager } - , m_retryTimeout {0} - , m_buffer {} -{ -} - -/*! \brief Processes new incoming data for message extraction - - \param newData: Buffer that contains the newly arrived data - \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(XsByteArray const& newData, std::deque &messages) -{ - if (!m_protocolManager) - return XRV_ERROR; - -#ifdef XSENS_DEBUG - XsSize prevSize = m_buffer.size(); -#endif - if (newData.size()) - m_buffer.append(newData); -#ifdef XSENS_DEBUG - assert(m_buffer.size() == newData.size() + prevSize); -#endif - - int popped = 0; - messages.clear(); - - auto retval = [&]() { - if (popped > 0) - m_buffer.pop_front(popped); - if (messages.empty()) - return XRV_TIMEOUTNODATA; - - return XRV_OK; - }; - - for (;;) - { - XsByteArray raw(m_buffer.data() + popped, m_buffer.size() - popped); - XsMessage message; - - MessageLocation location = m_protocolManager->findMessage(message, raw); - - JLTRACEG("location valid: " << std::boolalpha << location.isValid() << ", looks sane: " << m_protocolManager->validateMessage(message)); - - if (location.isValid() && m_protocolManager->validateMessage(message)) - { - assert(location.m_startPos == -1 || location.m_incompletePos == -1 || location.m_incompletePos < location.m_startPos); - - if (location.m_startPos > 0) - { - // We are going to skip something - if (location.m_incompletePos != -1) - { - // We are going to skip an incomplete but potentially valid message - // First wait a couple of times to see if we can complete that message before skipping - if (m_retryTimeout++ < m_maxIncompleteRetryCount) - { - // wait a bit until we have more data - // but already pop the data that we know contains nothing useful - if (location.m_incompletePos > 0) - { - JLALERTG("Skipping " << location.m_incompletePos << " bytes from the input buffer"); - popped += location.m_incompletePos; - } - return retval(); - } - else - { - // We've waited a bit for the incomplete message to complete but it never completed - // So: We are going to skip an incomplete but potentially valid message - JLALERTG("Skipping " << location.m_startPos << " bytes from the input buffer that may contain an incomplete message at " << location.m_incompletePos - << " found: " << (int)message.getTotalMessageSize() - << std::hex << std::setfill('0') - << " First bytes " << std::setw(2) << (int)message.getMessageStart()[0] - << " " << std::setw(2) << (int)message.getMessageStart()[1] - << " " << std::setw(2) << (int)message.getMessageStart()[2] - << " " << std::setw(2) << (int)message.getMessageStart()[3] - << " " << std::setw(2) << (int)message.getMessageStart()[4] - << std::dec << std::setfill(' ')); - } - } - else - { - // We are going to skip something but we are not going to skip an incomplete but potentially valid message - JLALERTG("Skipping " << location.m_startPos << " bytes from the input buffer"); - } - } - if (m_retryTimeout) - { - JLTRACEG("Resetting retry count from " << m_retryTimeout); - m_retryTimeout = 0; - } - - // message is valid, remove data from cache - popped += location.m_size + location.m_startPos; - messages.push_back(message); - } - else - { - return retval(); - } - } -} - -/*! \brief Clears the processing buffer -*/ -void MessageExtractor::clearBuffer() -{ - m_buffer.clear(); -} diff --git a/extern/xcommunication/mtwsdidata.cpp b/extern/xcommunication/mtwsdidata.cpp deleted file mode 100644 index 1aff4c5..0000000 --- a/extern/xcommunication/mtwsdidata.cpp +++ /dev/null @@ -1,161 +0,0 @@ -/* Copyright (c) 2003-2017 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. -*/ - -#include -#include -#include "mtwsdidata.h" - -/*! - \class MtwSdiData - \brief Class to store strapdown integration data. - \note Intended for internal use. -*/ - -/*! \brief Construct an empty strapdown integration data object */ -MtwSdiData::MtwSdiData() -: m_deviceId(0) -, m_timeSync(0) -, m_firstFrameNumber(0) -, m_lastFrameNumber(0) -, m_aidingData(0) -, m_barometer(0) -, m_rssi(0) -{ -} - -/*! \brief Construct a copy of \a other */ -MtwSdiData::MtwSdiData(const MtwSdiData &other) -: m_deviceId(other.m_deviceId) -, m_timeSync(other.m_timeSync) -, m_firstFrameNumber(other.m_firstFrameNumber) -, m_lastFrameNumber(other.m_lastFrameNumber) -, m_currentBias(other.m_currentBias) -, m_orientationIncrement(other.m_orientationIncrement) -, m_velocityIncrement(other.m_velocityIncrement) -, m_aidingData(other.m_aidingData) -, m_barometer(other.m_barometer) -, m_magnetoMeter(other.m_magnetoMeter) -, m_rssi(other.m_rssi) -{ -} - -/*! \brief Destroy the strapdown integration data structure. */ -MtwSdiData::~MtwSdiData() -{ -} - -/*! \brief Assign \a other to this. */ -const MtwSdiData& MtwSdiData::operator=(const MtwSdiData& other) -{ - if (this == &other) - return *this; - m_deviceId = other.m_deviceId; - m_timeSync = other.m_timeSync; - m_firstFrameNumber = other.m_firstFrameNumber; - m_lastFrameNumber = other.m_lastFrameNumber; - m_currentBias = other.m_currentBias; - m_orientationIncrement = other.m_orientationIncrement; - m_velocityIncrement = other.m_velocityIncrement; - m_aidingData = other.m_aidingData; - m_barometer = other.m_barometer; - m_magnetoMeter = other.m_magnetoMeter; - m_rssi = other.m_rssi; - return *this; -} - - -/*! \brief Test if this is a null Awinda object. */ -bool MtwSdiData::empty() const -{ - return !m_deviceId.toInt(); -} - -/*! \brief Test if strapdown integration data is available. */ -bool MtwSdiData::containsAidingData() const -{ - if (empty()) - return false; - return m_aidingData; -} - -/*! \brief Get the orientation increment value. */ -XsQuaternion MtwSdiData::orientationIncrement() const -{ - if (empty()) - return XsQuaternion(); - return m_orientationIncrement; -} - -/*! \brief Get the velocity increment value. */ -XsVector MtwSdiData::velocityIncrement() const -{ - if (empty()) - return XsVector(); - return m_velocityIncrement; -} - -/*! \brief Get the pressure as measured by the barometer in hPa. */ -double MtwSdiData::pressure() const -{ - if (empty()) - return 0; - return m_barometer; -} - -/*! \brief Get the magnetic field value. */ -XsVector MtwSdiData::magneticField() const -{ - if (empty()) - return XsVector(); - return m_magnetoMeter; -} - -/*! \brief Get the current gyroscope bias value. */ -XsVector MtwSdiData::currentBias() const -{ - if (empty()) - return XsVector(); - return m_currentBias; -} - -/*! \brief Get the frame range of the current strapdown integration data. */ -XsRange MtwSdiData::frameRange() const -{ - if (empty()) - return XsRange(); - return XsRange(m_firstFrameNumber, m_lastFrameNumber); -} - -/*! \brief Get the rssi of the received strapdown integration data */ -double MtwSdiData::rssi() const -{ - if(empty()) - return XS_RSSI_UNKNOWN; - return (double)m_rssi; -} - diff --git a/extern/xcommunication/packetfixeddata.cpp b/extern/xcommunication/packetfixeddata.cpp deleted file mode 100644 index 9e84c82..0000000 --- a/extern/xcommunication/packetfixeddata.cpp +++ /dev/null @@ -1,127 +0,0 @@ -/* Copyright (c) 2003-2017 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. -*/ - -#include -#include "packetfixeddata.h" - -#ifdef LOG_PACKET -# include "xslog.h" -# define PACKETLOG XSENSLOG -#else -# define PACKETLOG(...) -#endif - -/*! \brief Default constructor, creates an empty (invalid) object -*/ -PacketFixedData::PacketFixedData() - : m_infoList(NULL) - , m_formatList(NULL) - , m_idList(NULL) - , m_xm(false) - , m_itemCount(0) -{ - PACKETLOG("%s creating default %p\n", __FUNCTION__, this); -} - -/*! \brief Sized constructor, creates an object with room for \a count device's worth of data - \details The constructor sets the xbus flag to false - \param count The number of devices whose metadata is stored in the object -*/ -PacketFixedData::PacketFixedData(XsSize count) - : m_infoList(NULL) - , m_formatList(NULL) - , m_idList(NULL) - , m_xm(false) - , m_itemCount(count) -{ - PACKETLOG("%s creating %p with %d items\n", __FUNCTION__, this, count); - m_formatList = new XsDataFormat[m_itemCount]; - m_infoList = new PacketInfo[m_itemCount]; - m_idList = new XsDeviceId[m_itemCount]; -} - -/*! \brief Copy constructor - \param p The object to copy the contents from -*/ -PacketFixedData::PacketFixedData(const PacketFixedData& p) - : m_infoList(NULL) - , m_formatList(NULL) - , m_idList(NULL) - , m_xm(false) - , m_itemCount(0) -{ - PACKETLOG("%s creating %p from %p\n", __FUNCTION__, this, &p); - *this = p; - PACKETLOG("%s done creating %p\n", __FUNCTION__, this); -} - -/*! \brief Destructor -*/ -PacketFixedData::~PacketFixedData() -{ - PACKETLOG("%s %p\n", __FUNCTION__, this); - m_itemCount = 0; - delete[] m_formatList; - delete[] m_infoList; - delete[] m_idList; - PACKETLOG("%s %p exit\n", __FUNCTION__, this); -} - -/*! \brief Assignment operator, copies contents from \a data - \param data The object to copy from -*/ -void PacketFixedData::operator = (const PacketFixedData& data) -{ - if (this == &data) - return; - - PACKETLOG("%s copy from %p to %p\n", __FUNCTION__, &data, this); - - delete[] m_formatList; - delete[] m_idList; - delete[] m_infoList; - m_formatList = NULL; - m_idList = NULL; - m_infoList = NULL; - - m_itemCount = data.m_itemCount; - m_formatList = new XsDataFormat[data.m_itemCount]; - m_idList = new XsDeviceId[data.m_itemCount]; - m_infoList = new PacketInfo[data.m_itemCount]; - - for (uint16_t i = 0; i < data.m_itemCount; ++i) - { - m_formatList[i] = data.m_formatList[i]; - m_infoList[i] = data.m_infoList[i]; - m_idList[i] = data.m_idList[i]; - } - m_xm = data.m_xm; - - PACKETLOG("%s exit\n", __FUNCTION__); -} - diff --git a/extern/xcommunication/packetstamper.cpp b/extern/xcommunication/packetstamper.cpp deleted file mode 100644 index 114d569..0000000 --- a/extern/xcommunication/packetstamper.cpp +++ /dev/null @@ -1,254 +0,0 @@ -/* Copyright (c) 2003-2017 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. -*/ - -#include -#include "packetstamper.h" - -/*! \class PacketStamper - \brief Supplies functionality for timestamping data packets. - \details This class can analyze a data packet and create a proper packet id for it. -*/ - -//! \brief 32 bit MT Sample Counter boundary -const int64_t PacketStamper::AWINDABOUNDARY = 0x100000000LL; - -//! \brief 16 bit MT Sample Counter boundary -const int64_t PacketStamper::MTSCBOUNDARY = 0x00010000LL; - -//! \brief 8 bit Sample Counter boundary -const int64_t PacketStamper::SC8BOUNDARY = 0x00000100LL; - -//! \brief Default constructor -PacketStamper::PacketStamper() -{ - resetTosEstimation(); -} - -//! \brief Reset the Time Of Sampling estimation parameters -void PacketStamper::resetTosEstimation() -{ - m_t0Est = 0; - m_pid0Est = 0; - m_tPerPidEst = 0.0; - m_t0New = 0; - m_pid0New = 0; - m_tPerPidNew = 0.0; -} - -/*! \brief Calculate the new large packet counter value based on \a frameCounter and the \a lastCounter - \details Wraparound is at the given \a boundary - \param[in] frameCounter The frame counter - \param[in] lastCounter The last counter - \param[in] boundary the boundary at which to assume a wrap-around - \returns The computed packet counter value - \note If lastCounter < 0, returns frameCounter -*/ -int64_t PacketStamper::calculateLargePacketCounter(int64_t frameCounter, int64_t lastCounter, int64_t boundary) -{ - if (lastCounter < 0) - return frameCounter; - - const int64_t lowMask = boundary - 1; - const int64_t boundaryHalf = boundary / 2; - - int64_t low = lastCounter & lowMask; - int64_t dt = frameCounter - low; - if (dt < -boundaryHalf) - return lastCounter + dt + boundary; // positive wraparound - if (dt < boundaryHalf) - return lastCounter + dt; // normal increment - - return lastCounter + dt - boundary; // negative wraparound -} - -/*! \brief Create 64 bit counter for a packet. - \details Wrap when new XsDataPacket is too far away from the previous XsDataPacket in time. - Use half cache size as reasonable time difference - When infinite cache, simply wrap when new is lower than old - \param pack The XsDataPacket that needs its 64-bit sample counter updated - \param highestPacket The highest packet available for the current device, it will be updated if - the new counter is higher than the stored value. - \returns The computed counter for the packet. -*/ -int64_t PacketStamper::stampPacket(XsDataPacket& pack, XsDataPacket& highestPacket) -{ - //! \todo This could be a (couple of) milliseconds too late, this should be set as soon as the source message arrives: mantis 7157 - pack.setTimeOfArrival(XsTimeStamp::now()); - int64_t newCounter, lastCounter = -1; - - if (!highestPacket.empty()) - lastCounter = highestPacket.packetId(); - - if (pack.packetId() > 0) - newCounter = pack.packetId(); - else if (pack.containsPacketCounter()) - newCounter = calculateLargePacketCounter(pack.packetCounter(), lastCounter, MTSCBOUNDARY); - else if (pack.containsSampleTimeFine()) - { - newCounter = lastCounter + 1; - //if (pack.containsSampleTimeCoarse()) - // newCounter = (int64_t) pack.sampleTime64(); - //else - // newCounter = calculateLargeSampleTime((int32_t) pack.sampleTimeFine(), lastCounter); - } - else if (pack.containsPacketCounter8()) - newCounter = calculateLargePacketCounter(pack.packetCounter8(), lastCounter, SC8BOUNDARY); - else if (pack.containsAwindaSnapshot()) - newCounter = calculateLargePacketCounter(pack.awindaSnapshot().m_frameNumber, lastCounter, AWINDABOUNDARY); - else - newCounter = lastCounter + 1; - -// JLDEBUG(gJournal, "XsensDeviceAPI", "%s [%08x] old = %I64d new = %I64d diff = %I64d\n", __FUNCTION__, did, lastCounter, newCounter, (newCounter-lastCounter)); - - pack.setPacketId(newCounter); - estimateTos(pack); - if (newCounter > lastCounter) - highestPacket = pack; - - return newCounter; -} - -/*! \brief Calculate the new large sample time value based on \a frameTime and the \a lastTime - \details Wraparound is at 864000000 (1 day @ 10kHz) - \param[in] frameTime The frame time - \param[in] lastTime The last time - \returns The computed packet counter value - \note If lastTime < 0, returns frameTime -*/ -int64_t PacketStamper::calculateLargeSampleTime(int64_t frameTime, int64_t lastTime) -{ - if (lastTime < 0) - return frameTime; - - int64_t low = lastTime % 864000000; - int64_t dt = frameTime - low; - if (dt < (-864000000/2)) - return lastTime + dt + 864000000; // positive wraparound - if (dt < (864000000/2)) - return lastTime + dt; // normal increment - - return lastTime + dt - 864000000; // negative wraparound -} - -void PacketStamper::estimateTos(XsDataPacket& pack) -{ - int64_t toa = pack.timeOfArrival().msTime(); - int64_t tos = toa; - int64_t pid = pack.packetId(); - - // update - bool commit = false; - if (m_tPerPidNew > 0.0) - { - int64_t dpid = pid - m_pid0New; - if (dpid >= 100) - { - int64_t dt = (int64_t) (dpid * m_tPerPidNew); - int64_t tosEst = m_t0New + dt; - - // enough samples to assume the estimate is decent - int64_t dtos = tos - tosEst; - if (dtos <= 0 && dt < 2000) // we only allow t0 corrections for the first 2 seconds - { - m_t0New += dtos; - commit = true; - } - else if (dtos <= m_tPerPidNew) // else we have some data arrival timing glitch - { - m_tPerPidNew = (0.01 * (double) (toa - m_t0New) / (double) (pid - m_pid0New)) + (m_tPerPidNew * 0.99); - commit = true; - } - - // after 20 seconds we commit the new estimation values - if (dt >= 20000) - { - if (commit) - { - m_t0Est = m_t0New; - m_pid0Est = m_pid0New; - m_tPerPidEst = m_tPerPidNew; - - // and we restart estimation - dt = 900000; - } - // after 15 minutes without valid data points, we reset the stamping - if (dt >= 900000) - { - // restart - m_t0New = toa; - m_pid0New = pid; - m_tPerPidNew = 0.0; - } - } - else - commit = false; - } - } - else - { - // initial estimation - if (m_t0New == 0) - { - m_t0New = toa; - m_pid0New = pid; - } - else if (toa < m_t0New || pid < m_pid0New) - m_t0New = 0; - else if (pid - m_pid0New >= 10) - { - m_tPerPidNew = (double) (toa - m_t0New) / (double) (pid - m_pid0New); - if (m_t0Est == 0) - { - m_t0Est = m_t0New; - m_pid0Est = m_pid0New; - m_tPerPidEst = m_tPerPidNew; - } - } - } - - // estimate - if (m_tPerPidEst > 0.0) - { - int64_t dpid = pid - m_pid0Est; - int64_t tosEst = m_t0Est + (int64_t) (dpid * m_tPerPidEst); - - int64_t dtos = tos - tosEst; - if (dtos <= 0) - m_t0Est += dtos; - else - tos = tosEst; - } - - if (commit) - { - m_pid0New = pid; - m_t0New = tos; - } - - pack.setEstimatedTimeOfSampling(tos); -} diff --git a/extern/xcommunication/protocolhandler.cpp b/extern/xcommunication/protocolhandler.cpp deleted file mode 100644 index bb9a834..0000000 --- a/extern/xcommunication/protocolhandler.cpp +++ /dev/null @@ -1,274 +0,0 @@ -/* Copyright (c) 2003-2017 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. -*/ - -#include "protocolhandler.h" -#include -#include -#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 -#endif - -/*! \class ProtocolHandler - \brief Message protocol handling class - - This class' purpose is to get valid messages according to its protocol from the raw - data that is supplied to it. The default implementation (ProtocolHandler) implements - the Xsens message protocol. To use a different protocol, overload the findMessage function. - - The class is intended to be state-less with respect to the data it handles. -*/ - -//! Default constructor -ProtocolHandler::ProtocolHandler() - : m_ignoreMaxMsgSize(false) -{} - -//! Destructor -ProtocolHandler::~ProtocolHandler() -{} - -/*! \brief Compute the expected message size given a possibly incomplete message - \details When analyzing a message with incomplete size information, the function - will return the minimum size of the message given the information that is available -*/ -int expectedMessageSize(const unsigned char* buffer, int sz) -{ - const XsMessageHeader* hdr = (const XsMessageHeader*) buffer; - if (sz < 4) - return XS_LEN_MSGHEADERCS; // no size information available at all, return a minimum message - - // we have size information - if (hdr->m_length == XS_EXTLENCODE) - { - if (sz < 6) - return XS_EXTLENCODE + XS_LEN_MSGEXTHEADERCS; // typical minimum size at which point extended size is needed - - return XS_LEN_MSGEXTHEADERCS + ((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 + (uint32_t) (hdr->m_length); -} - -/*! \brief Write the contents of a uint8 buffer to string as hex characters */ -inline std::string dumpBuffer(const uint8_t* buff, XsSize sz) -{ -#ifdef DUMP_BUFFER_ON_ERROR - std::ostringstream ostr; - ostr << std::hex << std::setfill('0'); -#if DUMP_BUFFER_ON_ERROR > 0 - sz = std::min(sz, DUMP_BUFFER_ON_ERROR); -#endif - for (XsSize i = 0; i < sz; ++i) - ostr << " " << std::setw(2) << (int) buff[i]; - return ostr.str(); -#else - return std::string(); -#endif -} - -/*! \copydoc IProtocolHandler::findMessage - \todo Since the assumption is that we receive a stream of valid messages without garbage, the scan - is implemented in a rather naive and simple way. If we can expect lots of garbage in the data - stream, this should probably be looked into. -*/ -MessageLocation ProtocolHandler::findMessage(XsMessage& rcv, const XsByteArray& raw) const -{ - JLTRACE(gJournal, "Entry"); - MessageLocation rv(-1,0,-1,0); - rcv.clear(); - - int bufferSize = (int) raw.size(); - if (bufferSize == 0) - return rv; - - const unsigned char* buffer = raw.data(); - - // loop through the buffer to find a preamble - for (int pre = 0; pre < bufferSize; ++pre) - { - if (buffer[pre] == XS_PREAMBLE) - { - // incompletePos is the position of the first incomplete but potentially valid message that is skipped - if (rv.m_incompletePos == -1) - rv.m_incompletePos = pre; - - JLTRACE(gJournal, "Preamble found at " << pre); - // we found a preamble, see if we can read a message from here - if (rv.m_startPos == -1) - rv.m_startPos = (int32_t) pre; - int remaining = bufferSize-pre; // remaining bytes in buffer INCLUDING preamble - - if (remaining < XS_LEN_MSGHEADERCS) - { - JLTRACE(gJournal, "Not enough header data read"); - if (rv.m_startPos != pre) - continue; - rv.m_size = -expectedMessageSize(&buffer[pre], remaining); - rv.m_incompleteSize = expectedMessageSize(&buffer[pre], remaining); - return rv; - } - - // read header - const uint8_t* msgStart = &(buffer[pre]); - const XsMessageHeader* hdr = (const XsMessageHeader*) msgStart; - if (hdr->m_length == XS_EXTLENCODE) - { - if (remaining < XS_LEN_MSGEXTHEADERCS) - { - JLTRACE(gJournal, "Not enough extended header data read"); - if (rv.m_startPos != pre) - continue; - rv.m_size = -expectedMessageSize(&buffer[pre], remaining); - rv.m_incompleteSize = expectedMessageSize(&buffer[pre], remaining); - return rv; - } - } - else if (hdr->m_busId == 0 && hdr->m_messageId == 0) - { - // found 'valid' message that isn't actually valid... happens inside GPS raw data - // skip to next preamble - // and completely ignore this message, since it cannot be valid - if (rv.m_incompletePos == pre) - { - rv.m_incompletePos = -1; - rv.m_incompleteSize = 0; - } - if (rv.m_startPos == pre) - { - rv.m_startPos = -1; - rv.m_size = 0; - } - continue; - } - - // check the reported size - int target = expectedMessageSize(&buffer[pre], remaining); - - JLTRACE(gJournal, "Bytes in buffer=" << remaining << ", full target = " << target); - if (!m_ignoreMaxMsgSize && target > (XS_LEN_MSGEXTHEADERCS + XS_MAXDATALEN)) - { - // 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 - */ - JLALERT(gJournal, "Invalid message length: " << target); - rv.m_startPos = -1; - } - continue; - } - - if (remaining < target) - { - // not enough data read, skip current preamble - JLTRACE(gJournal, "Not enough data read: " << remaining << " / " << target); - if (rv.m_size == 0) - rv.m_size = -target; - if (rv.m_incompleteSize == 0) - rv.m_incompleteSize = target; - continue; - } - - // we have read enough data to fulfill our target so we'll try to parse the message - // and check the checksum - //if (rcv->loadFromString(msgStart, (uint16_t) target) == XRV_OK) - if (rcv.loadFromString(msgStart, (uint16_t)target)) - { - JLTRACE(gJournal, - "OK, size = " << (int) rcv.getTotalMessageSize() << " buffer: " - << dumpBuffer(msgStart, target)); - rv.m_size = (int) rcv.getTotalMessageSize(); - rv.m_startPos = pre; // we do this again here because this may not be the first preamble encountered (the check for -1 at the start of the loop is necessary) - - if (rv.m_incompletePos == pre) - { - // We've actually completed the incomplete message - rv.m_incompletePos = -1; - rv.m_incompleteSize = 0; - } - - return rv; - } - - // we could not read the message, clear message and try next preamble - rcv.clear(); - if (rv.m_startPos == pre) - { - rv.m_startPos = -1; - if (rv.m_incompletePos == pre) - rv.m_incompletePos = -1; - JLALERT(gJournal, - "Invalid checksum for msg at offset " << pre << " bufferSize = " << bufferSize - << " buffer at offset: " << dumpBuffer(raw.data()+pre, raw.size()-pre)); - } - } - } - JLTRACE(gJournal, "Exit"); - return rv; -} - -/*! \brief Returns the minimum size of a valid message of this protocol including preambles and checksums */ -int ProtocolHandler::minimumMessageSize() const -{ - return XS_LEN_MSGHEADERCS; // minimum size of xsens xbus protocol message -} - -/*! \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 -} - -/*! \brief Compose a message for transmission - \param raw The raw byte array to be constructed from the message - \param msg The message to translate into a raw byte array - \returns The size of the generated byte array - \todo Generalize this method -> IProtocolHandler -*/ -int ProtocolHandler::composeMessage(XsByteArray& raw, const XsMessage& msg) -{ - if (msg.getTotalMessageSize() < 5) // minimum size of an xsens message including envelope is 5 bytes - return -1; - - raw.assign(msg.getTotalMessageSize(), msg.getMessageStart()); - return (int) raw.size(); -} - -int ProtocolHandler::type() const -{ - return 0; // XPT_Xbus; -} - -void ProtocolHandler::ignoreMaximumMessageSize(bool ignore) -{ - m_ignoreMaxMsgSize = ignore; -} diff --git a/extern/xcommunication/serialinterface.cpp b/extern/xcommunication/serialinterface.cpp deleted file mode 100644 index 7d21bea..0000000 --- a/extern/xcommunication/serialinterface.cpp +++ /dev/null @@ -1,964 +0,0 @@ -/* Copyright (c) 2003-2017 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. -*/ - -#include "serialinterface.h" -#include -#include -#include "rx_tx_log.h" - -#include -#ifndef _WIN32 -# include // close -# include // ioctl -# include // open, O_RDWR -# include // strcpy -# include -# include -# include -#if !defined(__APPLE__) && !defined(ANDROID) -# include -#endif -#else -# include -# include -#endif - -#ifndef _CRT_SECURE_NO_DEPRECATE -# define _CRT_SECURE_NO_DEPRECATE -# ifdef _WIN32 -# pragma warning(disable:4996) -# endif -#endif - -//lint -emacro(534, FSEEK, FSEEK_R) -//lint -emacro({534}, FSEEK, FSEEK_R) -#ifdef _WIN32 -# define FSEEK(x) _fseeki64(m_handle, x, SEEK_SET) -# define FSEEK_R(x) _fseeki64(m_handle, x, SEEK_END) -# define FTELL() _ftelli64(m_handle) -#else -# define FSEEK(x) fseeko(m_handle, x, SEEK_SET) -# define FSEEK_R(x) fseeko(m_handle, x, SEEK_END) -# define FTELL() ftello(m_handle) -#endif - -// maybe log to nothing at this level -#ifdef LOG_CMT1 -# include "xslog.h" -# define XDA1LOG_OBSOLETE XSENSLOG -#else -# define XDA1LOG_OBSOLETE(...) (void)0 -#endif - -////////////////////////////////////////////////////////////////////////////////////////// -///////////////////////////////////////// SerialInterface ///////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////////////////// - -//! \brief Default constructor, initializes all members to their default values. -SerialInterface::SerialInterface() -{ - m_port = 0; - m_lastResult = XRV_OK; - m_timeout = 0; - m_endTime = 0; - m_baudrate = XBR_Invalid; - m_portname[0] = 0; -#ifdef _WIN32 - m_handle = INVALID_HANDLE_VALUE; -#else - m_handle = -1; - memset(&m_commState, 0, sizeof(m_commState)); -#endif -} - -//! Destructor, de-initializes, frees memory allocated for buffers, etc. -SerialInterface::~SerialInterface() -{ - try { - closeLive(); //lint !e534 - } catch(...) - {} -} - -//! \brief Close the serial communication port. -XsResultValue SerialInterface::close(void) -{ - return closeLive(); -} - -//! \brief Close the serial communication port. -XsResultValue SerialInterface::closeLive(void) -{ -#ifdef LOG_RX_TX - rx_log.close(); - tx_log.close(); -#endif - if (!isOpen()) - return m_lastResult = XRV_NOPORTOPEN; - - m_lastResult = XRV_OK; -#ifdef _WIN32 - if (::FlushFileBuffers(m_handle)) - { - // 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)) - { - cto.ReadIntervalTimeout = MAXDWORD; - cto.ReadTotalTimeoutConstant = 0; - cto.ReadTotalTimeoutMultiplier = 0; - if (::SetCommTimeouts(m_handle,&cto)) - { - char buffer[1024]; - DWORD length; - do { - if (!::ReadFile(m_handle, buffer, 1024, &length, NULL)) - break; - } while (length > 0); - } - else - m_lastResult = XRV_ERROR; - } - else - m_lastResult = XRV_ERROR; - } - if (!::CloseHandle(m_handle)) - m_lastResult = XRV_ERROR; - m_handle = INVALID_HANDLE_VALUE; -#else - flushData(); - ::close(m_handle); - m_handle = -1; -#endif - m_endTime = 0; - - return m_lastResult; -} - -/*! \brief Manipulate the Serial control lines - - The function manipulates the serial control lines that are indicated by the - mask parameter. Note that only the DTR and RTS lines can be set by win32. - \param mask Indicates which lines are to be manipulated and which should be - left alone. - \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) -{ - //lint --e{655} bitwise operations are intended on these enums - if (!isOpen()) - return (m_lastResult = XRV_NOPORTOPEN); -#ifdef _WIN32 - BOOL rv = 0; - if (mask & XCL_DTR) - { - if (state & XCL_DTR) - rv = EscapeCommFunction(m_handle,SETDTR); - else - rv = EscapeCommFunction(m_handle,CLRDTR); - } - - if (mask & XCL_RTS) - { - if (state & XCL_RTS) - rv = EscapeCommFunction(m_handle,SETRTS); - else - rv = EscapeCommFunction(m_handle,CLRRTS); - } - if (rv) - return m_lastResult = XRV_OK; - else - return m_lastResult = XRV_ERROR; -#else - bool rv = true; - int32_t status; - if (mask & XCL_DTR) - { - if (ioctl(m_handle, TIOCMGET, &status) == -1) - { - if (state & XCL_DTR) status |= TIOCM_DTR; - else status &= ~TIOCM_DTR; - rv = (ioctl(m_handle, TIOCMSET, &status) == -1); - } - else - rv = false; - } - if (rv && (mask & XCL_RTS)) - { - if (ioctl(m_handle, TIOCMGET, &status) == -1) - { - if (state & XCL_RTS) status |= TIOCM_RTS; - else status &= ~TIOCM_RTS; - rv = (ioctl(m_handle, TIOCMSET, &status) == -1); - } - else - rv = false; - } - if (rv) - return m_lastResult = XRV_OK; - else - return m_lastResult = XRV_ERROR; -#endif -} - -/*! \copydoc IoInterface::flushData - \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) -{ - m_lastResult = XRV_OK; -#ifdef _WIN32 - // Remove any 'old' data in buffer - if (!PurgeComm(m_handle, PURGE_TXCLEAR | PURGE_RXCLEAR)) - m_lastResult = XRV_ERROR; -#else - tcflush(m_handle, TCIOFLUSH); -#endif - m_endTime = 0; - return m_lastResult; -} - -//! Return the baudrate that is currently being used by the port -XsBaudRate SerialInterface::getBaudrate(void) const -{ - if (isOpen()) - return m_baudrate; - return XBR_Invalid; -} -//! Return the handle of the port -XsIoHandle SerialInterface::getHandle(void) const { return m_handle; } -//! Retrieve the port number that was last successfully opened. -uint16_t SerialInterface::getPortNumber (void) const { return m_port; } -//! Retrieve the port name that was last successfully opened. -void SerialInterface::getPortName(XsString& portname) const { portname = m_portname; } -//! Return the error code of the last operation. -XsResultValue SerialInterface::getLastResult(void) const { return m_lastResult; } -//! Return the current timeout value -uint32_t SerialInterface::getTimeout (void) const { return m_timeout; } -//! Return whether the communication port is open or not. -bool SerialInterface::isOpen (void) const -{ -#ifdef _WIN32 - return m_handle != INVALID_HANDLE_VALUE; -#else - return m_handle >= 0; -#endif -} - -#ifndef _WIN32 -template -static T setBitsEnabled(T field, T bits, bool cond) -{ - if (cond) - field |= bits; - else - field &= (~bits); - return field; -} -#endif - -/*! \brief Open a communication channel to the given port info. - \details If the baudrate in \a portInfo is set to XBR_Invalid, the baud rate is automatically - detected if possible. - \param portInfo The details of the port that should be opened. Depending on the type of interface, - parts of this parameter may be ignored. - \param readBufSize The size of the read buffer in bytes (if appliccable to the device) - \param writeBufSize The size of the write buffer in bytes (if appliccable to the device) - \param options The options to enable (flow control, stop bits) - \returns XRV_OK if the device was opened successfully -*/ -XsResultValue SerialInterface::open(const XsPortInfo& portInfo, - XsFilePos readBufSize, - XsFilePos writeBufSize, - PortOptions options) -{ - m_endTime = 0; - - JLDEBUG(gJournal, portInfo); - - if (isOpen()) - { - JLALERT(gJournal, "Port " << portInfo.portName() << " is already open"); - return (m_lastResult = XRV_ALREADYOPEN); - } - m_baudrate = portInfo.baudrate(); - - if (options & PO_RtsCtsFlowControl || (portInfo.linesOptions() & XPLO_RtsCtsFlowControl)) - JLTRACE(gJournal, "Requested RTS/CTS flow control"); - if (options & PO_DtrDsrFlowControl) - JLTRACE(gJournal, "Requested DTR/DSR flow control"); - if (options & PO_XonXoffFlowControl) - JLTRACE(gJournal, "Requested Xon/Xoff flow control"); - -#ifdef _WIN32 - XsResultValue fail = XRV_OK; - char winPortName[256]; - - // Open port - sprintf(winPortName, "\\\\.\\%s", portInfo.portName().c_str()); - m_handle = CreateFileA(winPortName, GENERIC_READ | GENERIC_WRITE, 0, NULL, - OPEN_EXISTING, 0, NULL); - if (m_handle == INVALID_HANDLE_VALUE) - { - JLDEBUG(gJournal, "Port " << portInfo.portName() << " cannot be opened"); - return (m_lastResult = XRV_INPUTCANNOTBEOPENED); - } - - DCB commState; //!< Stored settings about the serial port - - commState.DCBlength = sizeof(DCB); - - //Get the current state & then change it - if (!GetCommState(m_handle, &commState)) // Get current state - fail = XRV_ERROR; - - commState.BaudRate = (int) 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; - - // Setup flow control - 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.fInX = commState.fOutX; - - if (!SetCommState(m_handle, (LPDCB)&commState)) // Set new state - { - // Bluetooth ports cannot always be opened with 2 stopbits - // Now try to open port with 1 stopbit. - commState.StopBits = ONESTOPBIT; - if (!SetCommState(m_handle, (LPDCB)&commState)) - fail = XRV_INPUTCANNOTBEOPENED; - } - std::string tmp = portInfo.portName().toStdString(); - m_port = atoi(&tmp.c_str()[3]); - sprintf(m_portname, "%s", tmp.c_str()); - - if (setTimeout(20)) - fail = m_lastResult; - - // Other initialization functions - int tmpFail = fail; - applyHwControlLinesOptions(options, portInfo.linesOptions(), tmpFail); - fail = static_cast(tmpFail); - - if (!SetupComm(m_handle, (DWORD) readBufSize, (DWORD) writeBufSize)) // Set queue size - fail = XRV_ERROR; - - // Remove any 'old' data in buffer - //PurgeComm(m_handle, PURGE_TXCLEAR | PURGE_RXCLEAR); - if (!PurgeComm(m_handle, PURGE_TXABORT | PURGE_RXABORT | PURGE_TXCLEAR | PURGE_RXCLEAR)) - fail = XRV_ERROR; - - if (fail != XRV_OK) - { - CloseHandle(m_handle); //lint !e534 - m_handle = INVALID_HANDLE_VALUE; - return (m_lastResult = fail); - } - -#else // !_WIN32 - (void)readBufSize; - (void)writeBufSize; - // Open port - std::string pn = portInfo.portName().toStdString(); - m_handle = ::open(pn.c_str(), O_RDWR | O_NOCTTY); - - // O_RDWR: Read+Write - // O_NOCTTY: Raw input, no "controlling terminal" - // O_NDELAY: Don't care about DCD signal - - if (m_handle < 0) { - // Port not open - return m_lastResult = XRV_INPUTCANNOTBEOPENED; - } - - // Check if the file is already opened by someome else (other thread/process) - if (flock(m_handle, LOCK_EX | LOCK_NB)) - { - closeLive(); - return m_lastResult = XRV_INPUTCANNOTBEOPENED; - } - - /* Start configuring of port for non-canonical transfer mode */ - // Get current options for the port - if (tcgetattr(m_handle, &m_commState) != 0) - return XRV_ERROR; - - // Set baudrate. - if (cfsetispeed(&m_commState, portInfo.baudrate()) != 0) - return XRV_ERROR; - - if (cfsetospeed(&m_commState, portInfo.baudrate()) != 0) - return XRV_ERROR; - - // 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 |= CS8; // Select 8 data bits - - 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); -#ifdef CDTRDSR - 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); - // 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); - // 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 - - // Set the new options for the port - if (tcsetattr(m_handle,TCSANOW, &m_commState) != 0) - return XRV_INPUTCANNOTBEOPENED; - -#if defined(JLLOGLEVEL) && JLLOGLEVEL <= JLL_ALERT - termios checkCommState; - if (tcgetattr(m_handle, &checkCommState) != 0) - return XRV_ERROR; - - if (cfgetispeed(&checkCommState) != portInfo.baudrate()) - JLALERT(gJournal, "Set baudrate doesn't match requested baudrate"); - - if (cfgetospeed(&checkCommState) != portInfo.baudrate()) - JLALERT(gJournal, "Set baudrate doesn't match requested baudrate"); - - if (options&PO_RtsCtsFlowControl && !(checkCommState.c_cflag&CRTSCTS)) - JLALERT(gJournal, "Requested RTS/CTS flow control, but could not be set."); - - if (options&PO_DtrDsrFlowControl && -#ifdef CDTRDSR - !(checkCommState.c_cflag&CDTRDSR) -#else - false -#endif - ) - JLALERT(gJournal, "Requested DTR/DSR flow control, but could not be set."); - - if (options&PO_XonXoffFlowControl && !((checkCommState.c_iflag&(IXON|IXOFF)) == (IXON|IXOFF))) - JLALERT(gJournal, "Requested Xon/Xoff flow control, but could not be set."); -#endif // JLLOGLEVEL < JLL_ALERT - -#if defined(JLLOGLEVEL) && JLLOGLEVEL <= JLL_DEBUG -#define CHECK_COMMSTATE(req, res, field)\ - if (req.field != res.field) \ - {\ - JLDEBUG(gJournal, "field " << #field << " does not match");\ - JLDEBUG(gJournal, "actual : " << std::oct << (uint64_t)res.field);\ - JLDEBUG(gJournal, "expected: " << std::oct << (uint64_t)req.field);\ - } -#else -#define CHECK_COMMSTATE(req, res, field) -#endif - CHECK_COMMSTATE(m_commState, checkCommState, c_cflag); - CHECK_COMMSTATE(m_commState, checkCommState, c_iflag); - CHECK_COMMSTATE(m_commState, checkCommState, c_oflag); - CHECK_COMMSTATE(m_commState, checkCommState, c_cc[VMIN]); - CHECK_COMMSTATE(m_commState, checkCommState, c_cc[VTIME]); - - m_port = 1; - sprintf(m_portname, "%s", pn.c_str()); - - tcflush(m_handle, TCIOFLUSH); - - // setting RTS and DTR; RTS for Xbus Master, DTR for calibration sensors - int cmbits; - if (ioctl(m_handle, TIOCMGET, &cmbits) < 0) - { - JLDEBUG(gJournal, "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) - { - JLDEBUG(gJournal, "TIOCMSET failed, which is OK for USB connected MkIV devices"); - } -#endif // !_WIN32 - - JLDEBUG(gJournal, "Port " << portInfo.portName().toStdString() << " opened"); - return (m_lastResult = XRV_OK); -} - -/*! \brief Read data from the serial port and put it into the data buffer. - \details This function reads up to \a maxLength bytes from the port (non-blocking) and - puts it into the \a data buffer. - \param maxLength The maximum amount of data read. - \param data The buffer that will store the received data. - \returns XRV_OK if no error occurred. It can be that no data is available and XRV_OK will be - returned. Check data.size() for the number of bytes that were read. -*/ -XsResultValue SerialInterface::readData(XsFilePos maxLength, XsByteArray& data) -{ - if (!isOpen()) - return (m_lastResult = XRV_NOPORTOPEN); - -#ifdef _WIN32 - DWORD length; - data.setSize((XsSize) maxLength); - BOOL rres = ::ReadFile(m_handle, data.data(), (DWORD) maxLength, &length, NULL); - data.pop_back((XsSize) (maxLength-length)); - JLTRACE(gJournal, "ReadFile result " << rres << ", length " << length); - - if (!rres) - { - DWORD wErr = ::GetLastError(); - JLALERT(gJournal, "ReadFile returned windows error " << wErr); - if (wErr == ERROR_ACCESS_DENIED) - return (m_lastResult = XRV_UNEXPECTED_DISCONNECT); - if (wErr >= ERROR_INVALID_FUNCTION && wErr <= ERROR_INVALID_HANDLE) - return (m_lastResult = XRV_NOFILEORPORTOPEN); - return (m_lastResult = XRV_ERROR); - } - - if (length == 0) - return (m_lastResult = XRV_TIMEOUT); -#else - fd_set fd; - fd_set err; - timeval timeout; - FD_ZERO(&fd); - FD_ZERO(&err); - FD_SET(m_handle, &fd); - FD_SET(m_handle, &err); - - 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); - if (res < 0 || FD_ISSET(m_handle, &err)) - { - data.clear(); - return (m_lastResult = XRV_ERROR); - } else if (res == 0) { - data.clear(); - return (m_lastResult = XRV_TIMEOUT); - } - - data.setSize(maxLength); - int length = read(m_handle, (void*)data.data(), maxLength); - if (length > 0) - { - data.pop_back(maxLength - length); - } - else - { - int err = errno; - - data.clear(); - switch (err) - { - case EAGAIN: -#if defined(EWOULDBLOCK) && EWOULDBLOCK != EAGAIN - case EWOULDBLOCK: -#endif - return XRV_TIMEOUT; - - case EIO: - return XRV_UNEXPECTED_DISCONNECT; - - default: - break; - } - } -#if defined(JLLOGLEVEL) && JLLOGLEVEL <= JLL_TRACE && !defined(ANDROID) - serial_icounter_struct ic; - res = ioctl(m_handle, TIOCGICOUNT, &ic); - if (res == 0) - { - JLTRACE(gJournal, "rx: " << ic.rx); - JLTRACE(gJournal, "tx: " << ic.tx); - JLTRACE(gJournal, "frame " << ic.frame); - JLTRACE(gJournal, "overrun " << ic.overrun); - JLTRACE(gJournal, "buf_overrun " << ic.buf_overrun); - } -#endif -#endif - -#ifdef LOG_RX_TX - if (length > 0) - { - CHECK_STATE_RX(length, data, rx_log); - - if (!rx_log.isOpen()) - { - char fname[XS_MAX_FILENAME_LENGTH]; -#ifdef _WIN32 - sprintf(fname, "rx_%03d_%d.log", (int32_t) m_port, m_baudrate); -#else - char *devname = strrchr(m_portname, '/'); - sprintf(fname, "rx_%s_%d.log", devname + 1, XsBaud::rateToNumeric(m_baudrate)); -#endif - makeFilenameUnique(fname, state); - - rx_log.create(XsString(fname), true); - } - rx_log.write(data.data(), 1, length); -#ifdef LOG_RX_TX_FLUSH - rx_log.flush(); -#endif - } -#endif - - JLTRACE(gJournal, "returned success, read " << length << " of " << maxLength << " bytes, first: " << JLHEXLOG(data[0])); - return (m_lastResult = XRV_OK); -} - -/*! \brief Set the default timeout value to use in blocking operations. - \details This function sets the value of m_timeout. There is no infinity value. The value 0 - means that all blocking operations now become polling (non-blocking) operations. - If the value is set to or from 0, the low-level serial port settings may be - changed in addition to the m_timeout value. - \param ms The new timeout in milliseconds - \returns XRV_OK if the function succeeded -*/ -XsResultValue SerialInterface::setTimeout (const uint32_t ms) -{ - JLDEBUG(gJournal, "Setting timeout to " << ms << " ms"); - - m_timeout = ms; -#ifdef _WIN32 - // Set COM timeouts - COMMTIMEOUTS commTimeouts; - - if (!GetCommTimeouts(m_handle,&commTimeouts)) // Fill CommTimeouts structure - return m_lastResult = XRV_ERROR; - - // immediate return if data is available, wait 1ms otherwise - if (m_timeout > 0) - { - commTimeouts.ReadIntervalTimeout = 0; - commTimeouts.ReadTotalTimeoutConstant = m_timeout; // ms time - commTimeouts.ReadTotalTimeoutMultiplier = 0; - commTimeouts.WriteTotalTimeoutConstant = m_timeout; - commTimeouts.WriteTotalTimeoutMultiplier = 0; - } - else - { - // immediate return whether data is available or not - commTimeouts.ReadIntervalTimeout = MAXDWORD; - commTimeouts.ReadTotalTimeoutConstant = 0; - commTimeouts.ReadTotalTimeoutMultiplier = 0; - commTimeouts.WriteTotalTimeoutConstant = 0; - commTimeouts.WriteTotalTimeoutMultiplier = 0; - } - - if (!SetCommTimeouts(m_handle, &commTimeouts)) // Set CommTimeouts structure - return m_lastResult = XRV_ERROR; -#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 - - // Set the new options for the port if it is open - if (isOpen()) - tcsetattr(m_handle,TCSANOW, &m_commState); -#endif - return (m_lastResult = XRV_OK); -} - -/*! \brief Wait for data to arrive or a timeout to occur. - \details The function waits until \c maxLength data is available or until a timeout occurs. - The function returns success if data is available or XsResultValue::TIMEOUT if a - timeout occurred. A timeout value of 0 indicates that the default timeout stored - in the class should be used. - \param maxLength The maximum number of bytes to read before returning - \param data The buffer to put the read data in. - \returns XRV_OK if \a maxLength bytes were read, XRV_TIMEOUT if less was read, XRV_TIMEOUTNODATA if nothing was read -*/ -XsResultValue SerialInterface::waitForData(XsFilePos maxLength, XsByteArray& data) -{ - data.clear(); - data.reserve((XsSize) maxLength); - - //char *data = (char *)&_data[0]; - JLTRACE(gJournal, "timeout=" << m_timeout << ", maxLength=" << maxLength); - uint32_t timeout = m_timeout; - - uint32_t eTime = XsTime_getTimeOfDay(NULL, NULL) + timeout; -// uint32_t newLength = 0; - - while (((XsFilePos) data.size() < maxLength) && (XsTime_getTimeOfDay(NULL, NULL) <= eTime)) - { - XsByteArray raw; - - if (readData(maxLength - data.size(), raw) != XRV_OK) - return m_lastResult; - data.append(raw); - } - JLTRACE(gJournal, "Read " << data.size() << " of " << maxLength << " bytes"); - - if ((XsFilePos) data.size() < maxLength) - return (m_lastResult = XRV_TIMEOUT); - else - return (m_lastResult = XRV_OK); -} - -/*! \copydoc IoInterface::writeData - \note The default timeout is respected in this operation. -*/ -XsResultValue SerialInterface::writeData(const XsByteArray& data, XsFilePos* written) -{ - XsFilePos bytes; - if (written == NULL) - written = &bytes; - - if (!isOpen()) - return (m_lastResult = XRV_NOPORTOPEN); - - *written = 0; - -#ifdef _WIN32 - DWORD lwritten = 0; - if (WriteFile(m_handle, data.data(), (DWORD) data.size(), &lwritten, NULL) == 0) - { - DWORD wErr = ::GetLastError(); - JLALERT(gJournal, "WriteFile returned windows error " << wErr); - if (wErr == ERROR_ACCESS_DENIED) - return (m_lastResult = XRV_UNEXPECTED_DISCONNECT); - return (m_lastResult = XRV_ERROR); - } - - *written = lwritten; -#else - ssize_t result = write(m_handle, (const void*)data.data(), data.size()); - if (result <= 0) - { - int err = errno; - *written = 0; - switch (err) - { - case EAGAIN: -#if defined(EWOULDBLOCK) && EWOULDBLOCK != EAGAIN - case EWOULDBLOCK: -#endif - return XRV_TIMEOUT; - - case EIO: - return XRV_UNEXPECTED_DISCONNECT; - - /* we don't expect any other errors to actually occur */ - default: - break; - } - } - - if (result < 0) - *written = 0; - else - *written = result; -#endif - -#ifdef LOG_RX_TX - if (written[0] > 0) - { - CHECK_STATE_TX(written[0], data, tx_log); - if (!tx_log.isOpen()) - { - char fname[XS_MAX_FILENAME_LENGTH]; -#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)); -#endif - makeFilenameUnique(fname, state); - - tx_log.create(XsString(fname), true); - } - tx_log.write(data.data(), 1, *written); -#ifdef LOG_RX_TX_FLUSH - tx_log.flush(); -#endif - } -#endif - - return (m_lastResult = XRV_OK); -} - -/*! \brief Cancel any pending io requests */ -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. - */ - //CancelIoEx(m_handle, NULL); -#endif -} - -/*! \brief Logical \a or operator for flow controls */ -SerialInterface::PortOptions operator|(SerialInterface::PortOptions lhs, SerialInterface::PortOptions rhs) -{ - return static_cast(static_cast(lhs) | static_cast(rhs)); -} - -/*! \brief Logical \a and operator for flow controls */ -SerialInterface::PortOptions operator&(SerialInterface::PortOptions lhs, SerialInterface::PortOptions rhs) -{ - return static_cast(static_cast(lhs) & static_cast(rhs)); -} - -/*! \brief Logical inversion operator for flow controls */ -SerialInterface::PortOptions operator~(SerialInterface::PortOptions lhs) -{ - return static_cast(~static_cast(lhs)); -} - -/*! \brief Apply the specified options for the hardware control lines. - \param options The options to enable (flow control, stop bits) - \param portLinesOptions The options for the hardware control lines (RTS/DTR levels) -*/ -void SerialInterface::applyHwControlLinesOptions(PortOptions options, int portLinesOptions, int& p) -{ - const XsPortLinesOptions pLinesOpts = static_cast(portLinesOptions); - -#ifdef _WIN32 - - // DTR Line Options - if (!(options&PO_DtrDsrFlowControl)) - { - // Flow Control is disabled - if ((pLinesOpts == XPLO_Invalid) || (pLinesOpts & XPLO_DTR_Ignore)) - { - // Default behaviour - if (!EscapeCommFunction(m_handle, SETDTR)) // Set DTR (Calibration sensors need DTR to startup, won't hurt otherwise) - p = XRV_ERROR; - } - else - { - // Handle flow control line options - if (((pLinesOpts & (XPLO_DTR_Set | XPLO_DTR_Clear)) == (XPLO_DTR_Set | XPLO_DTR_Clear)) - || ((pLinesOpts & (XPLO_DTR_Set | XPLO_DTR_Clear)) == 0)) - { - // Line options are not valid, both Set and Clear are high or down - if (!EscapeCommFunction(m_handle, SETDTR)) // Default behaviour - p = XRV_ERROR; - } - else if (pLinesOpts & XPLO_DTR_Set) - { - if (!EscapeCommFunction(m_handle, SETDTR)) // Set DTR - p = XRV_ERROR; - } - else - { - // XPLO_DTR_Clear is Set - assert(pLinesOpts & XPLO_DTR_Clear); - - if (!EscapeCommFunction(m_handle, CLRDTR)) // Clear DTR - p = XRV_ERROR; - } - } - } - - // RTS Line Options - if (!(options&PO_RtsCtsFlowControl)) - { - // Flow Control is disabled - if ((pLinesOpts != XPLO_Invalid) && !(pLinesOpts & XPLO_RTS_Ignore)) - { - if ((((pLinesOpts & XPLO_RTS_Set) && (pLinesOpts & XPLO_RTS_Clear)) == 0) - && (((pLinesOpts & XPLO_RTS_Set) || (pLinesOpts & XPLO_RTS_Clear)) != 0)) - { - // Only one between Set and Clear is high - if (pLinesOpts & XPLO_RTS_Set) - { - if (!EscapeCommFunction(m_handle, SETRTS)) // Set RTS - p = XRV_ERROR; - } - else - { - // XPLO_RTS_Clear is Set - assert(pLinesOpts & XPLO_RTS_Clear); - - if (!EscapeCommFunction(m_handle, CLRRTS)) // Clear RTS - p = XRV_ERROR; - } - } - } - } - -#else - - // DTR Line - if (!(options&PO_DtrDsrFlowControl)) - { - // Flow control is disabled - if ((pLinesOpts != XPLO_Invalid) && !(pLinesOpts & XPLO_DTR_Ignore)) - { - if (((pLinesOpts & (XPLO_DTR_Set | XPLO_DTR_Clear)) == (XPLO_DTR_Set | XPLO_DTR_Clear)) - || ((pLinesOpts & (XPLO_DTR_Set | XPLO_DTR_Clear)) == 0)) - { - // Line options are not valid, both Set and Clear are high or down - p = setBitsEnabled(p, TIOCM_DTR, true); // Default behaviour - } - else - p = setBitsEnabled(p, TIOCM_DTR, (pLinesOpts & XPLO_DTR_Set)); - } - else - p = setBitsEnabled(p, TIOCM_DTR, true); // Default behaviour - } - - // RTS Line - if (!(options&PO_RtsCtsFlowControl)) - { - if ((pLinesOpts != XPLO_Invalid) && !(pLinesOpts & XPLO_RTS_Ignore)) - { - if (((pLinesOpts & (XPLO_RTS_Set | XPLO_RTS_Clear)) == (XPLO_RTS_Set | XPLO_RTS_Clear)) - || ((pLinesOpts & (XPLO_RTS_Set | XPLO_RTS_Clear)) == 0)) - { - // Line options are not valid, both Set and Clear are high or down - p = setBitsEnabled(p, TIOCM_RTS, true); // Default behaviour - } - else - p = setBitsEnabled(p, TIOCM_RTS, (pLinesOpts & XPLO_RTS_Set)); - } - else - p = setBitsEnabled(p, TIOCM_RTS, true); // Default behaviour - } - -#endif - -} diff --git a/extern/xcommunication/synclinegmt.c b/extern/xcommunication/synclinegmt.c deleted file mode 100644 index 1b7d9a6..0000000 --- a/extern/xcommunication/synclinegmt.c +++ /dev/null @@ -1,67 +0,0 @@ -/* Copyright (c) 2003-2017 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. -*/ - -#include "synclinegmt.h" - -/*! \addtogroup cinterface C Interface - @{ -*/ - -//! \brief Translate an SyncLineGmt into a generic XsSyncLine -XsSyncLine xslgmtToXsl(SyncLineGmt mk4Line) -{ - switch (mk4Line) - { - case XSLGMT_ClockIn: return XSL_ClockIn; - case XSLGMT_GpsClockIn: return XSL_GpsClockIn; - case XSLGMT_ReqData: return XSL_ReqData; - case XSLGMT_In: return XSL_In1; - case XSLGMT_Out: return XSL_Out1; - case XSLGMT_BiIn: return XSL_Bi1In; - case XSLGMT_BiOut: return XSL_Bi1Out; - default: return XSL_Invalid; - } -} - -//! \brief Translate an XsSyncLine into a SyncLineGmt -SyncLineGmt xslToXslgmt(XsSyncLine line) -{ - switch (line) - { - case XSL_ClockIn: return XSLGMT_ClockIn; - case XSL_GpsClockIn: return XSLGMT_GpsClockIn; - case XSL_In1: return XSLGMT_In; - case XSL_Out1: return XSLGMT_Out; - case XSL_ReqData: return XSLGMT_ReqData; - case XSL_Bi1In: return XSLGMT_BiIn; - case XSL_Bi1Out: return XSLGMT_BiOut; - default: return XSLGMT_Invalid; - } -} - -/*! @} */ diff --git a/extern/xcommunication/synclinemk4.c b/extern/xcommunication/synclinemk4.c deleted file mode 100644 index 870faf6..0000000 --- a/extern/xcommunication/synclinemk4.c +++ /dev/null @@ -1,67 +0,0 @@ -/* Copyright (c) 2003-2017 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. -*/ - -#include "synclinemk4.h" - -/*! \addtogroup cinterface C Interface - @{ -*/ - -//! \brief Translate an SyncLineMk4 into a generic XsSyncLine -XsSyncLine xsl4ToXsl(SyncLineMk4 mk4Line) -{ - switch (mk4Line) - { - case XSL4_ClockIn: return XSL_ClockIn; - case XSL4_GpsClockIn: return XSL_GpsClockIn; - 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; - default: return XSL_Invalid; - } -} - -//! \brief Translate an XsSyncLine into a Mk4-specififc SyncLineMk4 -SyncLineMk4 xslToXsl4(XsSyncLine line) -{ - switch (line) - { - case XSL_ClockIn: return XSL4_ClockIn; - case XSL_GpsClockIn: return XSL4_GpsClockIn; - 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; - default: return XSL4_Invalid; - } -} - -/*! @} */ diff --git a/extern/xcommunication/usbinterface.cpp b/extern/xcommunication/usbinterface.cpp deleted file mode 100644 index 5ea17b1..0000000 --- a/extern/xcommunication/usbinterface.cpp +++ /dev/null @@ -1,1129 +0,0 @@ -/* Copyright (c) 2003-2017 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. -*/ - -#include -#include -#include "usbinterface.h" -#include - -#include "xswinusb.h" -#include "xslibusb.h" -#include "rx_tx_log.h" - -#ifndef _WIN32 -# include // strcpy -#else -# include -# include -#endif -#include - -#ifndef _CRT_SECURE_NO_DEPRECATE -# 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 - - //! The time at which an operation will end in ms, used by several functions. - uint32_t m_endTime; - //! 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. - */ - uint32_t m_timeout; - - int m_interfaceCount; - int m_interface; - int m_dataInEndPoint; - int m_dataOutEndPoint; - char m_portname[256]; - -#ifdef USE_WINUSB - XsWinUsb m_winUsb; - WINUSB_INTERFACE_HANDLE m_usbHandle[2]; - uint8_t m_bulkInPipe, m_bulkOutPipe, m_interruptPipe, m_deviceSpeed; - uint16_t m_bulkInPipePacketSize; - XsIoHandle m_deviceHandle; - - XsThreadId m_threadId; - XsThread m_threadHandle; - static const int m_oCount = MAXIMUM_WAIT_OBJECTS-1; - OVERLAPPED m_overlapped[m_oCount]; - XsByteArray m_varBuffer; - static const int m_fixedBufferSize = 8192; - static const int m_fastPolicyThreshold = 3; - uint8_t m_fixedBuffer[m_oCount][m_fixedBufferSize]; - //int m_offset; - CRITICAL_SECTION m_mutex; - HANDLE m_quitEvent; - std::atomic_bool m_running; - HANDLE m_waitEvents[m_oCount]; - int m_readIdx; - XsResultValue m_threadedResult; - - void threadFunc(); - -#elif defined(HAVE_LIBUSB) - 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. - */ - class UsbContext { - public: - /*! \brief Create the USB context - */ - UsbContext() - { - if (!m_claimedContexts) - m_libUsb.init(&m_usbContext); - m_claimedContexts++; - //libusb_set_debug(m_usbContext, 3); - } - - /*! \brief Destroy the USB context */ - ~UsbContext() - { - m_claimedContexts--; - if (!m_claimedContexts) - m_libUsb.exit(m_usbContext); - } - static libusb_context *m_usbContext; // needed for proper use of libusb - XsLibUsb m_libUsb; - private: - static int m_claimedContexts; - }; - UsbContext m_contextManager; - - /*! \brief Map a libusb_error to XsResultValue - - \a param libusbError [in] the result code to convert - \a param hint give a hint for the code to return when in doubt - */ - XsResultValue libusbErrorToXrv(int libusbError, XsResultValue hint = XRV_ERROR) - { - switch (libusbError) { - case LIBUSB_SUCCESS: - return XRV_OK; - - case LIBUSB_ERROR_IO: - return hint; - - case LIBUSB_ERROR_INVALID_PARAM: - return XRV_INVALIDPARAM; - - case LIBUSB_ERROR_ACCESS: - return hint; - - case LIBUSB_ERROR_NO_DEVICE: - return XRV_UNEXPECTED_DISCONNECT; - - case LIBUSB_ERROR_NOT_FOUND: - return XRV_NOTFOUND; - - case LIBUSB_ERROR_BUSY: - return XRV_INVALIDOPERATION; - - case LIBUSB_ERROR_TIMEOUT: - return XRV_TIMEOUT; - - case LIBUSB_ERROR_OVERFLOW: - return hint; - - case LIBUSB_ERROR_PIPE: - return hint; - - case LIBUSB_ERROR_INTERRUPTED: - return XRV_UNEXPECTEDMSG; - - case LIBUSB_ERROR_NO_MEM: - return XRV_OUTOFMEMORY; - - case LIBUSB_ERROR_NOT_SUPPORTED: - return XRV_NOTIMPLEMENTED; - - case LIBUSB_ERROR_OTHER: - return hint; - } - return hint; - } - - /*! \brief Convert a libusb error to a human-readable string */ - const char *libusbErrorToString(int libusbError) - { - switch (libusbError) { - case LIBUSB_SUCCESS: - return "LIBUSB_SUCCESS"; - - case LIBUSB_ERROR_IO: - return "LIBUSB_ERROR_IO"; - - case LIBUSB_ERROR_INVALID_PARAM: - return "LIBUSB_ERROR_INVALID_PARAM"; - - case LIBUSB_ERROR_ACCESS: - return "LIBUSB_ERROR_ACCESS"; - - case LIBUSB_ERROR_NO_DEVICE: - return "LIBUSB_ERROR_NO_DEVICE"; - - case LIBUSB_ERROR_NOT_FOUND: - return "LIBUSB_ERROR_NOT_FOUND"; - - case LIBUSB_ERROR_BUSY: - return "LIBUSB_ERROR_BUSY"; - - case LIBUSB_ERROR_TIMEOUT: - return "LIBUSB_ERROR_TIMEOUT"; - - case LIBUSB_ERROR_OVERFLOW: - return "LIBUSB_ERROR_OVERFLOW"; - - case LIBUSB_ERROR_PIPE: - return "LIBUSB_ERROR_PIPE"; - - case LIBUSB_ERROR_INTERRUPTED: - return "LIBUSB_ERROR_INTERRUPTED"; - - case LIBUSB_ERROR_NO_MEM: - return "LIBUSB_ERROR_NO_MEM"; - - case LIBUSB_ERROR_NOT_SUPPORTED: - return "LIBUSB_ERROR_NOT_SUPPORTED"; - - case LIBUSB_ERROR_OTHER: - return "LIBUSB_ERROR_OTHER"; - } - return "Unknown"; - } -#else - void *m_deviceHandle; -#endif -}; - -#ifdef USE_WINUSB -DWORD usbReadThreadFunc(void* obj) -{ - UsbInterfacePrivate* d = (UsbInterfacePrivate*) obj; - d->m_running = true; - xsSetThreadPriority(xsGetCurrentThreadHandle(), XS_THREAD_PRIORITY_HIGHEST); //lint !e534 - xsNameThisThread("USB reader"); - try { - d->threadFunc(); - d->m_running = false; - } catch(...) - { - xsNameThisThread("Crashed USB reader"); - d->m_running = false; - XsTime::msleep(10000); - } - return 0; -} - -void UsbInterfacePrivate::threadFunc() -{ - HANDLE handles[1+m_oCount]; - handles[0] = m_quitEvent; - 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) - { - handles[m_readIdx+1] = m_waitEvents[m_readIdx]; - //m_readIdx = 0; - m_overlapped[m_readIdx] = OVERLAPPED(); - ::ResetEvent(m_waitEvents[m_readIdx]); //lint !e534 - 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]); //lint !e534 - } - int fastCount = 0; - //m_readIdx = 1; - bool policyFast = false; - bool run = true; - while (run) - { - // start follow-up read operation - m_overlapped[m_readIdx] = OVERLAPPED(); - ::ResetEvent(m_waitEvents[m_readIdx]); //lint !e534 - 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]); //lint !e534 - m_readIdx = (m_readIdx + 1) % m_oCount; - int64_t tBegin = XsTime_timeStampNow(0); - DWORD waitResult = ::WaitForMultipleObjects(1+m_oCount, handles, FALSE, INFINITE); -#if 0 // not sure if this causes problems, but it should help in catching up - int64_t tEnd = XsTime_timeStampNow(0); - switch (tEnd - tBegin) - { - case 0: - if (++fastCount > m_fastPolicyThreshold && !policyFast) - { - policyFast = true; - // set fast policy - UCHAR enable = TRUE; - m_winUsb.SetPipePolicy(m_usbHandle[1], m_bulkInPipe, IGNORE_SHORT_PACKETS, sizeof(UCHAR), &enable); //lint !e534 - } - break; - - case 1: - if (fastCount) - --fastCount; - if (policyFast && fastCount <= m_fastPolicyThreshold) - { - // reset policy - policyFast = false; - UCHAR enable = FALSE; - m_winUsb.SetPipePolicy(m_usbHandle[1], m_bulkInPipe, IGNORE_SHORT_PACKETS, sizeof(UCHAR), &enable); //lint !e534 - } - break; - - default: - fastCount = 0; - if (policyFast) - { - // reset policy - policyFast = false; - UCHAR enable = FALSE; - m_winUsb.SetPipePolicy(m_usbHandle[1], m_bulkInPipe, IGNORE_SHORT_PACKETS, sizeof(UCHAR), &enable); //lint !e534 - } - break; - } -#endif - - // handle data - switch (waitResult) - { - case WAIT_TIMEOUT: - case WAIT_FAILED: - case WAIT_OBJECT_0: - run = false; - break; - - default: - if (waitResult >= WAIT_ABANDONED_0) - { - JLDEBUG(gJournal, "WFMO abandoned: " << (waitResult - WAIT_OBJECT_0)); - break; - } - -#ifndef XSENS_RELEASE - JLDEBUG(gJournal, "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) - { - case ERROR_SEM_TIMEOUT: - case ERROR_IO_INCOMPLETE: - //JLDEBUG(gJournal, "m_winUsb.GetOverlappedResult resulted in acceptable windows error " << err); - break; - - case ERROR_GEN_FAILURE: - JLDEBUG(gJournal, "We seem to have lost contact with the usb device"); - m_threadedResult = XRV_UNEXPECTED_DISCONNECT; - run = false; - break; - - default: - JLALERT(gJournal, "m_winUsb.GetOverlappedResult resulted in windows error " << err); - run = false; - break; - } - //assert (err == ERROR_IO_INCOMPLETE); - } - else - { - // append unread data to var buffer - JLTRACE(gJournal, "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; - } - } -} - -#elif defined(HAVE_LIBUSB) -int UsbInterfacePrivate::UsbContext::m_claimedContexts = 0; -libusb_context *UsbInterfacePrivate::UsbContext::m_usbContext = NULL; -#endif - -/*! \class UsbInterface - \brief An IoInterface for dealing specifically with Xsens USB devices -*/ - -/*! \brief Default constructor, initializes all members to their default values. -*/ -UsbInterface::UsbInterface() : - d(new UsbInterfacePrivate) -{ - d->m_lastResult = XRV_OK; - d->m_timeout = 20; - d->m_endTime = 0; - d->m_dataInEndPoint = -1; - d->m_dataOutEndPoint = -1; - d->m_deviceHandle = 0; - d->m_portname[0] = 0; - - #ifdef USE_WINUSB - d->m_threadHandle = INVALID_HANDLE_VALUE; - ::InitializeCriticalSection(&d->m_mutex); - d->m_usbHandle[0] = 0; - d->m_usbHandle[1] = 0; - for (int i = 0; i < d->m_oCount; ++i) - { - d->m_waitEvents[i] = ::CreateEvent(NULL, TRUE, FALSE, NULL); - ::ResetEvent(d->m_waitEvents[i]); - } - d->m_quitEvent = ::CreateEvent(NULL, TRUE, FALSE, NULL); - ::ResetEvent(d->m_quitEvent); - d->m_readIdx = 0; -// d->m_offset = 0; - d->m_threadedResult = XRV_OK; - #endif -} - -//! Destructor, de-initializes, frees memory allocated for buffers, etc. -UsbInterface::~UsbInterface() -{ - try { - closeUsb(); //lint !e534 -#ifdef USE_WINUSB - ::CloseHandle(d->m_quitEvent); //lint !e534 - for (int i = 0; i < d->m_oCount; ++i) - ::CloseHandle(d->m_waitEvents[i]); //lint !e534 - ::DeleteCriticalSection(&d->m_mutex); -#endif - delete d; - } catch(...) - {} -} - -/*! \brief Close the USB communication port. -*/ -XsResultValue UsbInterface::close(void) -{ - return closeUsb(); -} - -/*! \brief Close the USB communication port. - \returns XRV_OK if the port was closed successfully - \note Linux:\n - If a kernel driver was detached when communication with the device started, - attach it again. No guarantee is given that udev will pick up on it though. -*/ -XsResultValue UsbInterface::closeUsb(void) -{ - //lint --e{534} -#ifdef LOG_RX_TX - d->rx_log.close(); - d->tx_log.close(); - -#endif - if (!isOpen()) - return d->m_lastResult = XRV_NOPORTOPEN; - - d->m_lastResult = XRV_OK; -#ifdef USE_WINUSB - if (d->m_threadHandle != INVALID_HANDLE_VALUE) - { - while (d->m_running) - { - ::SetEvent(d->m_quitEvent); - XsTime::msleep(10); - } - ::CloseHandle(d->m_threadHandle); - } - - flushData(); - if(d->m_usbHandle[0]) { - d->m_winUsb.Free(d->m_usbHandle[0]); - d->m_usbHandle[0] = NULL; - } - if(d->m_usbHandle[1]) { - d->m_winUsb.Free(d->m_usbHandle[1]); - d->m_usbHandle[1] = NULL; - } - if (d->m_deviceHandle) { - CloseHandle(d->m_deviceHandle); - d->m_deviceHandle = NULL; - } -#elif defined(HAVE_LIBUSB) - flushData(); - 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; - while (result != LIBUSB_SUCCESS) { - result = d->m_contextManager.m_libUsb.release_interface(d->m_deviceHandle, i); - if (result == LIBUSB_SUCCESS) { - d->m_contextManager.m_libUsb.attach_kernel_driver(d->m_deviceHandle, i); - } - } - } - - d->m_contextManager.m_libUsb.close(d->m_deviceHandle); - d->m_deviceHandle = NULL; - - d->m_contextManager.m_libUsb.unref_device(dev); - d->m_interface = -1; - d->m_dataInEndPoint = -1; - d->m_dataOutEndPoint = -1; -#else - d->m_lastResult = XRV_NOTIMPLEMENTED; -#endif - d->m_endTime = 0; - - return d->m_lastResult; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Flush all data to be transmitted / received. -XsResultValue UsbInterface::flushData(void) -{ - //lint --e{534} - d->m_lastResult = XRV_OK; -#ifdef USE_WINUSB - if(d->m_usbHandle[0]) { - d->m_winUsb.AbortPipe(d->m_usbHandle[0], d->m_bulkInPipe); - d->m_winUsb.FlushPipe(d->m_usbHandle[0], d->m_bulkInPipe); - d->m_winUsb.AbortPipe(d->m_usbHandle[0], d->m_bulkOutPipe); - d->m_winUsb.FlushPipe(d->m_usbHandle[0], d->m_bulkOutPipe); - } - if(d->m_usbHandle[1]) { - d->m_winUsb.AbortPipe(d->m_usbHandle[1], d->m_bulkInPipe); - d->m_winUsb.FlushPipe(d->m_usbHandle[1], d->m_bulkInPipe); - d->m_winUsb.AbortPipe(d->m_usbHandle[1], d->m_bulkOutPipe); - d->m_winUsb.FlushPipe(d->m_usbHandle[1], d->m_bulkOutPipe); - } -#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) - break; - if (actual == 0) - break; - } -#else - d->m_lastResult = XRV_NOTIMPLEMENTED; -#endif - d->m_endTime = 0; - return d->m_lastResult; -} - -//! Return the error code of the last operation. -XsResultValue UsbInterface::getLastResult(void) const -{ - return d->m_lastResult; -} - -//! Return the current timeout value -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 -{ - 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) -{ - d->m_endTime = 0; - -#ifdef USE_WINUSB - JLDEBUG(gJournal, "Open usb port " << portInfo.portName().toStdString()); -#else - JLDEBUG(gJournal, "Open usb port " << portInfo.usbBus() << ":" << portInfo.usbAddress()); -#endif - - if (isOpen()) - { - JLALERT(gJournal, "Port " << portInfo.portName().toStdString() << " already open"); - return (d->m_lastResult = XRV_ALREADYOPEN); - } - -#ifdef USE_WINUSB - d->m_deviceHandle = CreateFileA(portInfo.portName().c_str(), - GENERIC_WRITE | GENERIC_READ, - FILE_SHARE_WRITE | FILE_SHARE_READ, - NULL, - OPEN_EXISTING, - FILE_ATTRIBUTE_NORMAL | FILE_FLAG_OVERLAPPED, - NULL); - - if (d->m_deviceHandle == INVALID_HANDLE_VALUE) - { - d->m_deviceHandle = NULL; - return (d->m_lastResult = XRV_PORTNOTFOUND); - } - - BOOL result = FALSE; - UCHAR speed = 0; - ULONG length = 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]); - } - else - { -#ifdef XSENS_DEBUG - DWORD err = GetLastError(); - assert(result); -#endif - return (d->m_lastResult = XRV_ERROR); - } - - for (int k = 0; k<2;k++) - { - if(result) - { - assert(d->m_usbHandle[k] != 0); - length = sizeof(UCHAR); - result = d->m_winUsb.QueryDeviceInformation(d->m_usbHandle[k], - DEVICE_SPEED, - &length, - &speed); - } - - if(result) - { - d->m_deviceSpeed = speed; - result = d->m_winUsb.QueryInterfaceSettings(d->m_usbHandle[k], - 0, - &interfaceDescriptor); - } - if(result) - { - for(int i=0;im_winUsb.QueryPipe(d->m_usbHandle[k], - 0, - (UCHAR) i, - &pipeInfo); - - if(pipeInfo.PipeType == UsbdPipeTypeBulk && - USB_ENDPOINT_DIRECTION_IN(pipeInfo.PipeId)) - { - d->m_bulkInPipe = pipeInfo.PipeId; - d->m_bulkInPipePacketSize = - 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; - break; - } - } - } - } - - setTimeout(0); //lint !e534 - flushData(); //lint !e534 - - sprintf(d->m_portname, "%s", portInfo.portName().c_str()); - -// d->m_offset = 0; - ::ResetEvent(&d->m_quitEvent); //lint !e534 - d->m_threadHandle = xsStartThread(usbReadThreadFunc, d, &d->m_threadId); - if (d->m_threadHandle == XSENS_INVALID_THREAD) - { -#ifdef XSENS_DEBUG - assert(0); -#endif - return (d->m_lastResult = XRV_ERROR); - } - -#elif defined(HAVE_LIBUSB) - 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); - - // "USBxxx:yyy" - uint8_t bus = XsPortInfo_usbBus(&portInfo); - uint8_t address = XsPortInfo_usbAddress(&portInfo); - - XsResultValue xrv = XRV_OK; - int result; - libusb_device *device = NULL; - for (int i = 0; i < listLength && device == NULL; ++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; - - libusb_device_descriptor desc; - result = d->m_contextManager.m_libUsb.get_device_descriptor(dev, &desc); - if (result != LIBUSB_SUCCESS) - break; - - libusb_config_descriptor *configDesc; - result = d->m_contextManager.m_libUsb.get_active_config_descriptor(dev, &configDesc); - if (result != LIBUSB_SUCCESS) - break; - - d->m_interface = -1; - d->m_interfaceCount = configDesc->bNumInterfaces; - // find the bulk transfer endpoints - for (uint8_t ifCount = 0; ifCount < configDesc->bNumInterfaces && d->m_interface == -1; ++ifCount) { - for (uint8_t altsettingCount = 0; altsettingCount < configDesc->interface[ifCount].num_altsetting; altsettingCount++) { - 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) - continue; - - switch (endpoints[i].bEndpointAddress&LIBUSB_ENDPOINT_DIR_MASK) { - 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; - } - - } - - if (outEndpoint == -1 || inEndpoint == -1) - continue; - - d->m_interface = ifCount; - d->m_dataOutEndPoint = outEndpoint; - d->m_dataInEndPoint = inEndpoint; - } - } - if (d->m_interface == -1) { - xrv = XRV_INPUTCANNOTBEOPENED; - break; - } - - d->m_contextManager.m_libUsb.free_config_descriptor(configDesc); - d->m_contextManager.m_libUsb.ref_device(dev); - device = dev; - result = LIBUSB_SUCCESS; - } - - d->m_contextManager.m_libUsb.free_device_list(deviceList, 1); - if (result != LIBUSB_SUCCESS) { - d->m_contextManager.m_libUsb.unref_device(device); - return d->m_lastResult = d->libusbErrorToXrv(result); - } - - if (xrv != XRV_OK) { - d->m_contextManager.m_libUsb.unref_device(device); - return d->m_lastResult = xrv; - } - - libusb_device_handle *handle; - result = d->m_contextManager.m_libUsb.open(device, &handle); - if (result != LIBUSB_SUCCESS) { - d->m_contextManager.m_libUsb.unref_device(device); - return d->m_lastResult = d->libusbErrorToXrv(result); - } - - // be rude and claim all interfaces - for (int i = 0; i < d->m_interfaceCount; i++) { - result = d->m_contextManager.m_libUsb.kernel_driver_active(handle, i); - if (result > 0) - result = d->m_contextManager.m_libUsb.detach_kernel_driver(handle, i); - if (result == LIBUSB_SUCCESS) - result = d->m_contextManager.m_libUsb.claim_interface(handle, i); - if (result != LIBUSB_SUCCESS) { - for (int j = 0; j < i; j++) { - while (result != LIBUSB_SUCCESS) { - result = d->m_contextManager.m_libUsb.release_interface(handle, j); - d->m_contextManager.m_libUsb.attach_kernel_driver(handle, j); - } - } - - d->m_contextManager.m_libUsb.close(handle); - d->m_contextManager.m_libUsb.unref_device(device); - return d->m_lastResult = d->libusbErrorToXrv(result); - } - } - - d->m_deviceHandle = handle; - sprintf(d->m_portname, "%s", portInfo.portName().c_str()); - - flushData(); -#else // HAVE_LIBUSB - d->m_lastResult = XRV_NOTIMPLEMENTED; -#endif - JLDEBUG(gJournal, "USB Port opened"); - return (d->m_lastResult = XRV_OK); -} - -/*! \brief Read data from the USB port and put it into the data buffer. - \details This function reads up to \a maxLength bytes from the port (non-blocking) and - puts it into the \a data buffer. - \param maxLength The maximum amount of data read. - \param data The buffer that will store the received data. - \returns XRV_OK if no error occurred. It can be that no data is available and XRV_OK will be - returned. Check data.size() for the number of bytes that were read. -*/ -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)); - return res; -} - -/*! \brief Read data from the serial port and put it into the data buffer. - \details This function reads up to \a maxLength bytes from the USB port (non-blocking) and - puts it into the \a data buffer. - \param maxLength The maximum number of bytes to read. - \param data Pointer to a buffer that will store the received data. - \param length The number of bytes placed into \c 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) -{ - JLTRACE(gJournal, "maxLength=" << maxLength << ", data=" << JLHEXLOG(data) << ", length=" << JLHEXLOG(length)); - XsFilePos ln; - if (length == NULL) - length = &ln; - - if (!isOpen()) - return (d->m_lastResult = XRV_NOPORTOPEN); - -#ifdef USE_WINUSB - XsFilePos remaining = 0; - ::EnterCriticalSection(&d->m_mutex); - remaining = *length = d->m_varBuffer.size(); - if (*length > maxLength) - *length = maxLength; - if (*length) - { - memcpy(data, d->m_varBuffer.data(), (XsSize) *length); - d->m_varBuffer.erase(0, (XsSize) *length); - remaining = d->m_varBuffer.size(); - } - - if (d->m_threadedResult != XRV_OK) - return (d->m_lastResult = d->m_threadedResult); - - ::LeaveCriticalSection(&d->m_mutex); - JLTRACE(gJournal, "returned success, read " << *length << " of " << maxLength << " bytes, first: " << JLHEXLOG(((char*)data)[0]) << ", " << remaining << " remaining in buffer"); - -#elif defined(HAVE_LIBUSB) - int actual = 0; - JLTRACE(gJournal, "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); - JLTRACE(gJournal, "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); - - *length = actual; -#endif - -#ifdef LOG_RX_TX - if (*length > 0) - { - CHECK_STATE_RX(*length, data, d->rx_log); - - if (!d->rx_log.isOpen()) - { - char fname[XS_MAX_FILENAME_LENGTH]; - sprintf(fname,"rx_USB%03u_%03u.log", usbBus(), usbAddress()); - d->rx_log.create(XsString(fname), true); - } - d->rx_log.write(data,1,*length); -#ifdef LOG_RX_TX_FLUSH - d->rx_log.flush(); -#endif - } -#endif - - return (d->m_lastResult = XRV_OK); -} - -/*! \brief Set the default timeout value to use in blocking operations. - \details This function sets the value of m_timeout. There is no infinity value. The value 0 - means that the default value is used. - \param ms The desired timeout in milliseconds - \returns XRV_OK if the timeout value was successfully updated -*/ -XsResultValue UsbInterface::setTimeout(uint32_t ms) -{ -#ifdef USE_WINUSB - JLDEBUG(gJournal, "Request to set timeout to " << ms << " ms overridden, setting to 0 ms instead"); - ms = 0; // no timeout ever - UCHAR enable = FALSE; - - d->m_winUsb.SetPipePolicy(d->m_usbHandle[1], d->m_bulkInPipe, IGNORE_SHORT_PACKETS, sizeof(UCHAR), &enable); //lint !e534 - d->m_winUsb.SetPipePolicy(d->m_usbHandle[1], d->m_bulkInPipe, PIPE_TRANSFER_TIMEOUT, sizeof(ULONG), &ms); //lint !e534 - - d->m_timeout = ms; -#else - JLDEBUG(gJournal, "Setting timeout to " << ms); - if (ms == 0) - d->m_timeout = 1; - else - d->m_timeout = ms; -#endif - return (d->m_lastResult = XRV_OK); -} - -/*! \brief Sets the RAWIO mode of the USB interface - \note Only applies to WinUSB implementations - \param enable : If true will enable RAW IO mode -*/ -void UsbInterface::setRawIo(bool enable) -{ - JLDEBUG(gJournal, "Setting RAWIO mode to " << enable); - -#ifdef USE_WINUSB - enable = false; // never use raw IO - UCHAR rawIo = (UCHAR)enable; - d->m_winUsb.SetPipePolicy(d->m_usbHandle[1], d->m_bulkInPipe, RAW_IO, sizeof(UCHAR), &rawIo); //lint !e534 -#else - (void)enable; -#endif - d->m_lastResult = XRV_OK; -} - -/*! \brief Retrieves the state of the RAWIO mode of the USB interface - \returns true if raw IO mode is enabled - \note Only applies to WinUSB implementations -*/ -bool UsbInterface::getRawIo(void) -{ -#ifdef USE_WINUSB - UCHAR rawIo = 0; - ULONG someSize = sizeof(UCHAR); - d->m_winUsb.GetPipePolicy(d->m_usbHandle[1], d->m_bulkInPipe, RAW_IO, &someSize, &rawIo); //lint !e534 - return (rawIo != 0); -#else - return false; -#endif -} - -/*! \brief Wait for data to arrive or a timeout to occur. - \details The function waits until \c maxLength data is available or until a timeout occurs. - The function returns success if data is available or XsResultValue::TIMEOUT if a - timeout occurred. A timeout value of 0 indicates that the default timeout stored - in the class should be used. - \param maxLength The maximum number of bytes to wait for - \param data A buffer that will be filled with the read data. It must be able to contain at - least \a maxLength bytes. - \param length An optional pointer to storage for the actual number of bytes read. - \returns XRV_OK if the requested data was read -*/ -XsResultValue UsbInterface::waitForData(XsFilePos maxLength, void* data, XsFilePos* length) -{ - JLTRACE(gJournal, "timeout=" << d->m_timeout << ", data=" << data << ", length=" << length); - uint32_t timeout = d->m_timeout; - - char *bdata = (char *)data; - - XsFilePos ln; - if (length == NULL) - length = &ln; - uint32_t eTime = XsTime::getTimeOfDay() + timeout; - XsFilePos newLength = 0; - - *length = 0; - while ((*length < maxLength) && (XsTime::getTimeOfDay() <= eTime)) - { - if (readData(maxLength - *length, bdata + *length, &newLength)) - return d->m_lastResult; - *length += newLength; - } - JLTRACE(gJournal, "read " << length[0] << " of " << maxLength << " bytes"); - - if (length[0] < maxLength) - return (d->m_lastResult = XRV_TIMEOUT); - else - return (d->m_lastResult = XRV_OK); -} - -/*! \brief Write the data to the USB port. - \details The function writes the given data to the connected USB port. - The default timeout is respected in this operation. - \param data The data to be written - \param written An optional pointer to storage for the actual number of bytes that were written - \returns XRV_OK if the data was successfully written - \sa writeData(XsFilePos, const void *, XsFilePos*) -*/ -XsResultValue UsbInterface::writeData(const XsByteArray& data, XsFilePos* written) -{ - return writeData(data.size(), data.data(), written); -} - -/*! \brief Write the data to the USB port. - \details The function writes the given data to the connected USB port. - The default timeout is respected in this operation. - \param length The number of bytes to write. - \param data A pointer to a memory buffer that contains the bytes to send - \param written An optional pointer to storage for the actual number of bytes that were written - \returns XRV_OK if the data was successfully written - \sa writeData(const XsByteArray&, XsFilePos*) -*/ -XsResultValue UsbInterface::writeData(XsFilePos length, const void *data, XsFilePos* written) -{ - XsFilePos bytes; - if (written == NULL) - written = &bytes; - - if (!isOpen()) - return (d->m_lastResult = XRV_NOPORTOPEN); - - *written = 0; - -#ifdef USE_WINUSB - ULONG dataWritten; - d->m_winUsb.WritePipe(d->m_usbHandle[1], - d->m_bulkOutPipe, - (uint8_t*)data, - (ULONG)length, - &dataWritten, - NULL); //lint !e534 - - *written = dataWritten; -#elif defined(HAVE_LIBUSB) - *written = 0; - 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); - *written += actual; - if (result != LIBUSB_SUCCESS) - { - JLALERT(gJournal, "bulk write failed: " << d->libusbErrorToString(result) << ". " << actual << " bytes sent"); - return (d->m_lastResult = d->libusbErrorToXrv(result)); - } - } -#else - (void)length; - (void)data; -#endif - -#ifdef LOG_RX_TX - if (*written > 0) - { - CHECK_STATE_TX(*written, data, d->tx_log); - - if (!d->tx_log.isOpen()) - { - char fname[XS_MAX_FILENAME_LENGTH]; - sprintf(fname,"tx_USB%03u_%03u.log", usbBus(), usbAddress()); - d->tx_log.create(XsString(fname), true); - } - d->tx_log.write(data,1,*written); -#ifdef LOG_RX_TX_FLUSH - d->tx_log.flush(); -#endif - } -#endif - - return (d->m_lastResult = XRV_OK); -} - -/*! \brief The USB bus number this device is on (libusb/linux only) */ -uint8_t UsbInterface::usbBus() const -{ -#if defined(HAVE_LIBUSB) - if (!d->m_deviceHandle) - return 0; - - 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; -#endif -} - -/*! \brief The address of the device (libusb/linux only) */ -uint8_t UsbInterface::usbAddress() const -{ -#if defined(HAVE_LIBUSB) - if (!d->m_deviceHandle) - return 0; - - 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; -#endif -} - -//! Retrieve the port name that was last successfully opened. -void UsbInterface::getPortName(XsString& portname) const -{ - portname = d->m_portname; -} diff --git a/extern/xcommunication/xslibusb.cpp b/extern/xcommunication/xslibusb.cpp deleted file mode 100644 index db2d2e8..0000000 --- a/extern/xcommunication/xslibusb.cpp +++ /dev/null @@ -1,511 +0,0 @@ -/* Copyright (c) 2003-2017 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. -*/ - -#include "xslibusb.h" -#include - -#ifdef HAVE_LIBUSB - -/*! \class XsLibUsb - \brief Class for dynamic loading of winusb -*/ -XsLibUsb::XsLibUsb(void) -{ - m_libraryLoader = new XsLibraryLoader(); - initLibrary(); -} - -XsLibUsb::~XsLibUsb(void) -{ - delete m_libraryLoader; -} - -void XsLibUsb::tryLoadLibrary() -{ - if (m_libraryLoader->isLoaded()) - return; - if (m_libraryLoader->load("libusb-1.0.so")) - return; - if (m_libraryLoader->load("libusb1.0.so")) - return; -} - -void XsLibUsb::initLibrary() -{ - tryLoadLibrary(); - - memset(&m_libUsb, 0, sizeof(m_libUsb)); - - if (m_libraryLoader->isLoaded()) - { - m_libUsb.init = (libUSB_init*)m_libraryLoader->resolve("libusb_init"); - m_libUsb.exit = (libUSB_exit*)m_libraryLoader->resolve("libusb_exit"); - m_libUsb.open = (libUSB_open*)m_libraryLoader->resolve("libusb_open"); - m_libUsb.close = (libUSB_close*)m_libraryLoader->resolve("libusb_close"); - m_libUsb.kernel_driver_active = (libUSB_kernel_driver_active*)m_libraryLoader->resolve("libusb_kernel_driver_active"); - m_libUsb.attach_kernel_driver = (libUSB_attach_kernel_driver*)m_libraryLoader->resolve("libusb_attach_kernel_driver"); - m_libUsb.detach_kernel_driver = (libUSB_detach_kernel_driver*)m_libraryLoader->resolve("libusb_detach_kernel_driver"); - m_libUsb.ref_device = (libUSB_ref_device*)m_libraryLoader->resolve("libusb_ref_device"); - m_libUsb.unref_device = (libUSB_unref_device*)m_libraryLoader->resolve("libusb_unref_device"); - m_libUsb.claim_interface = (libUSB_claim_interface*)m_libraryLoader->resolve("libusb_claim_interface"); - m_libUsb.release_interface = (libUSB_release_interface*)m_libraryLoader->resolve("libusb_release_interface"); - m_libUsb.get_active_config_descriptor = (libUSB_get_active_config_descriptor*)m_libraryLoader->resolve("libusb_get_active_config_descriptor"); - m_libUsb.free_config_descriptor = (libUSB_free_config_descriptor*)m_libraryLoader->resolve("libusb_free_config_descriptor"); - m_libUsb.get_bus_number = (libUSB_get_bus_number*)m_libraryLoader->resolve("libusb_get_bus_number"); - m_libUsb.get_device = (libUSB_get_device*)m_libraryLoader->resolve("libusb_get_device"); - m_libUsb.get_device_address = (libUSB_get_device_address*)m_libraryLoader->resolve("libusb_get_device_address"); - m_libUsb.get_device_descriptor = (libUSB_get_device_descriptor*)m_libraryLoader->resolve("libusb_get_device_descriptor"); - m_libUsb.get_device_list = (libUSB_get_device_list*)m_libraryLoader->resolve("libusb_get_device_list"); - m_libUsb.free_device_list = (libUSB_free_device_list*)m_libraryLoader->resolve("libusb_free_device_list"); - m_libUsb.get_string_descriptor_ascii = (libUSB_get_string_descriptor_ascii*)m_libraryLoader->resolve("libusb_get_string_descriptor_ascii"); - m_libUsb.bulk_transfer = (libUSB_bulk_transfer*)m_libraryLoader->resolve("libusb_bulk_transfer"); - m_libUsb.set_debug = (libUSB_set_debug*)m_libraryLoader->resolve("libusb_set_debug"); - } -} - -/*! \brief Initialize libusb. This function must be called before calling any other libusb function. - - If you do not provide an output location for a context pointer, a default - context will be created. If there was already a default context, it will - be reused (and nothing will be initialized/reinitialized). - - \param context Optional output location for context pointer. - Only valid on return code 0. - \returns 0 on success, or a LIBUSB_ERROR code on failure -*/ -int XsLibUsb::init(libusb_context **ctx) -{ - if (m_libUsb.init) - return m_libUsb.init(ctx); - else - return LIBUSB_ERROR_NOT_SUPPORTED; -} - -/*! \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) -{ - if (m_libUsb.exit) - m_libUsb.exit(ctx); -} - -/*! \brief Open a device and obtain a device handle. A handle allows you to perform I/O on the device in question. - - Internally, this function adds a reference to the device and makes it available to you through libusb_get_device(). - This reference is removed during libusb_close(). - - This is a non-blocking function; no requests are sent over the bus. - - \param dev the device to open - \param handle output location for the returned device handle pointer. Only populated when the return code is 0. - - \returns 0 on success - \returns LIBUSB_ERROR_NO_MEM on memory allocation failure - \returns LIBUSB_ERROR_ACCESS if the user has insufficient permissions - \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) -{ - if (m_libUsb.open) - return m_libUsb.open(dev, handle); - else - return LIBUSB_ERROR_NOT_SUPPORTED; -} - -/*! \brief Close a device handle. Should be called on all open handles before your application exits. - Internally, this function destroys the reference that was added by libusb_open() on the given device. - - 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) -{ - if (m_libUsb.close) - m_libUsb.close(dev_handle); -} - -/*! \brief Determine if a kernel driver is active on an interface. - If a kernel driver is active, you cannot claim the interface, and libusb will be unable to perform I/O. - - This functionality is not available on Windows. - - \param dev a device handle - \param interface_number the interface to check - - \returns 0 if no kernel driver is active - \returns 1 if a kernel driver is active - \returns LIBUSB_ERROR_NO_DEVICE if the device has been disconnected - \returns LIBUSB_ERROR_NOT_SUPPORTED on platforms where the functionality is not available - \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) -{ - if (m_libUsb.kernel_driver_active) - return m_libUsb.kernel_driver_active(dev, interface_number); - else - return LIBUSB_ERROR_NOT_SUPPORTED; -} - -/** \brief Re-attach an interface's kernel driver, which was previously detached using libusb_detach_kernel_driver(). - This call is only effective on Linux and returns LIBUSB_ERROR_NOT_SUPPORTED on all other platforms. - - This functionality is not available on Darwin or Windows. - - \param dev a device handle - \param interface_number the interface to attach the driver from - - \returns 0 on success - \returns LIBUSB_ERROR_NOT_FOUND if no kernel driver was active - \returns LIBUSB_ERROR_INVALID_PARAM if the interface does not exist - \returns LIBUSB_ERROR_NO_DEVICE if the device has been disconnected - \returns LIBUSB_ERROR_NOT_SUPPORTED on platforms where the functionality is not available - \returns LIBUSB_ERROR_BUSY if the driver cannot be attached because the interface is claimed by a program or driver - \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) -{ - if (m_libUsb.attach_kernel_driver) - return m_libUsb.attach_kernel_driver(dev, interface_number); - else - return LIBUSB_ERROR_NOT_SUPPORTED; -} - -/*! \brief Detach a kernel driver from an interface. If successful, you will then be able to claim the interface and perform I/O. - - This functionality is not available on Darwin or Windows. - - \param dev a device handle - \param interface_number the interface to detach the driver from - - \returns 0 on success - \returns LIBUSB_ERROR_NOT_FOUND if no kernel driver was active - \returns LIBUSB_ERROR_INVALID_PARAM if the interface does not exist - \returns LIBUSB_ERROR_NO_DEVICE if the device has been disconnected - \returns LIBUSB_ERROR_NOT_SUPPORTED on platforms where the functionality is not available - \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) -{ - if (m_libUsb.detach_kernel_driver) - return m_libUsb.detach_kernel_driver(dev, interface_number); - else - return LIBUSB_ERROR_NOT_SUPPORTED; -} - -/*! \brief Increment the reference count of a device. - \param dev the device to reference - \returns the same device -*/ -libusb_device * XsLibUsb::ref_device(libusb_device *dev) -{ - if (m_libUsb.ref_device) - return m_libUsb.ref_device(dev); - else - return NULL; -} - -/*! \brief Decrement the reference count of a device. - 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) -{ - if (m_libUsb.unref_device) - m_libUsb.unref_device(dev); -} - -/*! \brief Claim an interface on a given device handle. - You must claim the interface you wish to use before you can perform I/O on any of its endpoints. - - It is legal to attempt to claim an already-claimed interface, in which - case libusb just returns 0 without doing anything. - - Claiming of interfaces is a purely logical operation; it does not cause - any requests to be sent over the bus. Interface claiming is used to - instruct the underlying operating system that your application wishes - to take ownership of the interface. - - This is a non-blocking function. - - \param dev a device handle - \param interface_number the \a bInterfaceNumber of the interface you wish to claim - \returns 0 on success - \returns LIBUSB_ERROR_NOT_FOUND if the requested interface does not exist - \returns LIBUSB_ERROR_BUSY if another program or driver has claimed the interface - \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) -{ - if (m_libUsb.claim_interface) - return m_libUsb.claim_interface(dev, interface_number); - else - return LIBUSB_ERROR_NOT_SUPPORTED; -} - -/*! \brief Release an interface previously claimed with libusb_claim_interface(). - You should release all claimed interfaces before closing a device handle. - - This is a blocking function. A SET_INTERFACE control request will be sent - to the device, resetting interface state to the first alternate setting. - - \param dev a device handle - \param interface_number the \a bInterfaceNumber of the previously-claimed interface - \returns 0 on success - \returns LIBUSB_ERROR_NOT_FOUND if the interface was not claimed - \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) -{ - if (m_libUsb.release_interface) - return m_libUsb.release_interface(dev, interface_number); - else - return LIBUSB_ERROR_NOT_SUPPORTED; -} - -/*! \brief Get the USB configuration descriptor for the currently active configuration. - - This is a non-blocking function which does not involve any requests being - sent to the device. - - \param dev a device - \param config output location for the USB configuration descriptor. - Only valid if 0 was returned. Must be freed with libusb_free_config_descriptor() after use. - \returns 0 on success - \returns LIBUSB_ERROR_NOT_FOUND if the device is in unconfigured state - \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) -{ - if (m_libUsb.get_active_config_descriptor) - return m_libUsb.get_active_config_descriptor(dev, config); - else - return LIBUSB_ERROR_NOT_SUPPORTED; -} - -/*! \brief Free a configuration descriptor obtained from libusb_get_active_config_descriptor() or libusb_get_config_descriptor(). - - It is safe to call this function with a NULL config parameter, in which case the function simply returns. - - \param config the configuration descriptor to free -*/ -void XsLibUsb::free_config_descriptor(struct libusb_config_descriptor *config) -{ - if (m_libUsb.free_config_descriptor) - m_libUsb.free_config_descriptor(config); -} - -/*! \brief Get the number of the bus that a device is connected to. - \param dev a device - \returns the bus number -*/ -uint8_t XsLibUsb::get_bus_number(libusb_device *dev) -{ - if (m_libUsb.get_bus_number) - return m_libUsb.get_bus_number(dev); - else - return 0; -} - -/*! \brief Get the underlying device for a handle. - - This function does not modify the reference count of the returned device, - so do not feel compelled to unreference it when you are done. - \param dev_handle a device handle - \returns the underlying device -*/ -libusb_device * XsLibUsb::get_device(libusb_device_handle *dev_handle) -{ - if (m_libUsb.get_device) - return m_libUsb.get_device(dev_handle); - else - return NULL; -} - -/*! \brief Get the address of the device on the bus it is connected to. - \param dev a device - \returns the device address -*/ -uint8_t XsLibUsb::get_device_address(libusb_device *dev) -{ - if (m_libUsb.get_device_address) - return m_libUsb.get_device_address(dev); - else - return 0; -} - -/*! \brief Get the USB device descriptor for a given device. - - This is a non-blocking function; the device descriptor is cached in memory. - - \param dev the device - \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) -{ - if (m_libUsb.get_device_descriptor) - return m_libUsb.get_device_descriptor(dev, desc); - else - return LIBUSB_ERROR_NOT_SUPPORTED; -} - -/*! \brief Returns a list of USB devices currently attached to the system. - This is your entry point into finding a USB device to operate. - - You are expected to unreference all the devices when you are done with - them, and then free the list with libusb_free_device_list(). Note that - libusb_free_device_list() can unref all the devices for you. Be careful - not to unreference a device you are about to open until after you have - opened it. - - This return value of this function indicates the number of devices in - the resultant list. The list is actually one element larger, as it is - NULL-terminated. - - \param ctx the context to operate on, or NULL for the default context - \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) -{ - if (m_libUsb.get_device_list) - return m_libUsb.get_device_list(ctx, list); - else - return LIBUSB_ERROR_NOT_SUPPORTED; -} - -/*! \brief Frees a list of devices previously discovered using libusb_get_device_list(). - If the unref_devices parameter is set, the reference count of each device in the list is decremented by 1. - \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) -{ - if (m_libUsb.free_device_list) - m_libUsb.free_device_list(list, unref_devices); -} - -/*! \brief Retrieve a string descriptor in C style ASCII. - - Wrapper around libusb_get_string_descriptor(). Uses the first language supported by the device. - - \param dev a device handle - \param desc_index the index of the descriptor to retrieve - \param data output buffer for ASCII string descriptor - \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) -{ - if (m_libUsb.get_string_descriptor_ascii) - return m_libUsb.get_string_descriptor_ascii(dev, desc_index, data, length); - else - return LIBUSB_ERROR_NOT_SUPPORTED; -} - -/*! \brief Perform a USB bulk transfer. The direction of the transfer is inferred from the direction bits of the endpoint address. - - For bulk reads, the \a length field indicates the maximum length of - data you are expecting to receive. If less data arrives than expected, - this function will return that data, so be sure to check the - \a transferred output parameter. - - You should also check the \a transferred parameter for bulk writes. - Not all of the data may have been written. - - Also check \a transferred when dealing with a timeout error code. - libusb may have to split your transfer into a number of chunks to satisfy - underlying O/S requirements, meaning that the timeout may expire after - the first few chunks have completed. libusb is careful not to lose any data - that may have been transferred; do not assume that timeout conditions - indicate a complete lack of I/O. - - \param dev_handle a handle for the device to communicate with - \param endpoint the address of a valid endpoint to communicate with - \param data a suitably-sized data buffer for either input or output (depending on endpoint) - \param length for bulk writes, the number of bytes from data to be sent. for bulk reads, the maximum number of bytes to receive into the data buffer. - \param transferred output location for the number of bytes actually transferred. - \param timeout timeout (in millseconds) that this function should wait before giving up due to no response being received. For an unlimited timeout, use value 0. - - \returns 0 on success (and populates \a transferred) - \returns LIBUSB_ERROR_TIMEOUT if the transfer timed out (and populates \a transferred) - \returns LIBUSB_ERROR_PIPE if the endpoint halted - \returns LIBUSB_ERROR_OVERFLOW if the device offered more data - \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) -{ - if (m_libUsb.bulk_transfer) - return m_libUsb.bulk_transfer(dev_handle, endpoint, data, length, actual_length, timeout); - else - return LIBUSB_ERROR_NOT_SUPPORTED; -} - -/*! \brief Set message verbosity. - - Level 0: no messages ever printed by the library (default) - - Level 1: error messages are printed to stderr - - Level 2: warning and error messages are printed to stderr - - Level 3: informational messages are printed to stdout, warning and error messages are printed to stderr - - The default level is 0, which means no messages are ever printed. If you - choose to increase the message verbosity level, ensure that your - application does not close the stdout/stderr file descriptors. - - You are advised to set level 3. libusb is conservative with its message - logging and most of the time, will only log messages that explain error - conditions and other oddities. This will help you debug your software. - - If the LIBUSB_DEBUG environment variable was set when libusb was - initialized, this function does nothing: the message verbosity is fixed - to the value in the environment variable. - - If libusb was compiled without any message logging, this function does - nothing: you'll never get any messages. - - If libusb was compiled with verbose debug message logging, this function - does nothing: you'll always get messages from all levels. - - \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) -{ - if (m_libUsb.set_debug) - m_libUsb.set_debug(ctx, level); -} - -#endif // HAVE_LIBUSB diff --git a/extern/xcommunication/xsudev.cpp b/extern/xcommunication/xsudev.cpp deleted file mode 100644 index da90fb3..0000000 --- a/extern/xcommunication/xsudev.cpp +++ /dev/null @@ -1,294 +0,0 @@ -/* Copyright (c) 2003-2017 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. -*/ - -#include "xsudev.h" -#include - -/*! \class XsUdev - \brief Class for dynamic loading of winusb -*/ -XsUdev::XsUdev(void) -{ - m_libraryLoader = new XsLibraryLoader(); - initLibrary(); -} - -XsUdev::~XsUdev(void) -{ - delete m_libraryLoader; -} - -void XsUdev::initLibrary() -{ - if (!m_libraryLoader->isLoaded()) - m_libraryLoader->load("libudev.so"); - - memset(&m_uDev, 0, sizeof(m_uDev)); - - if (m_libraryLoader->isLoaded()) - { - m_uDev.unew = (uDEV_new*)m_libraryLoader->resolve("udev_new"); - m_uDev.unref = (uDEV_unref*)m_libraryLoader->resolve("udev_unref"); - m_uDev.device_unref = (uDEV_device_unref*)m_libraryLoader->resolve("udev_device_unref"); - m_uDev.enumerate_new = (uDEV_enumerate_new*)m_libraryLoader->resolve("udev_enumerate_new"); - m_uDev.enumerate_add_match_subsystem = (uDEV_enumerate_add_match_subsystem*)m_libraryLoader->resolve("udev_enumerate_add_match_subsystem"); - m_uDev.enumerate_scan_devices = (uDEV_enumerate_scan_devices*)m_libraryLoader->resolve("udev_enumerate_scan_devices"); - m_uDev.enumerate_get_list_entry = (uDEV_enumerate_get_list_entry*)m_libraryLoader->resolve("udev_enumerate_get_list_entry"); - m_uDev.enumerate_unref = (uDEV_enumerate_unref*)m_libraryLoader->resolve("udev_enumerate_unref"); - m_uDev.list_entry_get_next = (uDEV_list_entry_get_next*)m_libraryLoader->resolve("udev_list_entry_get_next"); - m_uDev.list_entry_get_name = (uDEV_list_entry_get_name*)m_libraryLoader->resolve("udev_list_entry_get_name"); - m_uDev.device_new_from_syspath = (uDEV_device_new_from_syspath*)m_libraryLoader->resolve("udev_device_new_from_syspath"); - m_uDev.device_get_parent = (uDEV_device_get_parent*)m_libraryLoader->resolve("udev_device_get_parent"); - m_uDev.device_get_devnode = (uDEV_device_get_devnode*)m_libraryLoader->resolve("udev_device_get_devnode"); - m_uDev.device_get_parent_with_subsystem_devtype = (uDEV_device_get_parent_with_subsystem_devtype*)m_libraryLoader->resolve("udev_device_get_parent_with_subsystem_devtype"); - m_uDev.device_get_sysattr_value = (uDEV_device_get_sysattr_value*)m_libraryLoader->resolve("udev_device_get_sysattr_value"); - } -} - -/*! \brief Create udev library context. - - This reads the udev configuration file, and fills in the default values. - - The initial refcount is 1, and needs to be decremented to release the resources of the udev library context. - - \returns a new udev library context -*/ -udev *XsUdev::unew(void) -{ - if (m_uDev.unew) - return m_uDev.unew(); - else - return NULL; -} - -/*! \brief Drop a reference of the udev library context. - - \param udev udev library context - - If the refcount reaches zero, the resources of the context will be released. -*/ -udev *XsUdev::unref(struct udev *udev) -{ - if (m_uDev.unref) - return m_uDev.unref(udev); - else - return NULL; -} - -/*! \brief Drop a reference of a udev device. - - If the refcount reaches zero, the resources of the device will be released. - - \param udev_device udev device - \return NULL -*/ -udev_device *XsUdev::device_unref(struct udev_device *udev_device) -{ - if (m_uDev.device_unref) - return m_uDev.device_unref(udev_device); - else - return NULL; -} - -/*! \brief Create an enumeration context to scan. - \param udev udev library context - - \return an enumeration context. -*/ -udev_enumerate *XsUdev::enumerate_new(struct udev *udev) -{ - if (m_uDev.enumerate_new) - return m_uDev.enumerate_new(udev); - else - return NULL; -} - -/*! \brief Match only devices belonging to a certain kernel subsystem. - \param udev_enumerate context - \param subsystem filter for a subsystem of the device to include in the list - \return: 0 on success, otherwise a negative error value. -*/ -int XsUdev::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); - else - return -1; -} - -/*! \brief Scan /sys for all devices which match the given filters. No matches will return all currently available devices. - \param udev_enumerate udev enumeration context - \return 0 on success, otherwise a negative error value. -*/ -int XsUdev::enumerate_scan_devices(struct udev_enumerate *udev_enumerate) -{ - if (m_uDev.enumerate_scan_devices) - return m_uDev.enumerate_scan_devices(udev_enumerate); - else - return -1; -} - -/*! \brief Get the next entry from the list. - - \param list_entry current entry - \return udev_list_entry, NULL if no more entries are available. -*/ -udev_list_entry *XsUdev::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); - else - return NULL; -} - -/*! \brief Get the first entry of the sorted list of device paths. - \param udev_enumerate context - \return a udev_list_entry. -*/ -udev_list_entry *XsUdev::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); - else - return NULL; -} - -/*! \brief Drop a reference of an enumeration context. - - If the refcount reaches zero, all resources of the enumeration context will be released. - - \param udev_enumerate context - - \return: NULL -*/ -udev_enumerate *XsUdev::enumerate_unref(struct udev_enumerate *udev_enumerate) -{ - if (m_uDev.enumerate_unref) - return m_uDev.enumerate_unref(udev_enumerate); - else - return NULL; -} - -/*! \brief Get the name of a list entry. - \param list_entry: current entry - \return the name string of this entry. -*/ -const char *XsUdev::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); - else - return ""; -} - -/*! \brief Create new udev device, and fill in information from the sys device and the udev database entry. - - The syspath is the absolute path to the device, including the sys mount point. - - \param udev udev library context - \param syspath sys device path including sys directory - \return a new udev device, or NULL, if it does not exist -*/ -udev_device *XsUdev::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); - else - return NULL; -} - -/*! \brief Find the next parent device, and fill in information from the sys device and the udev database entry. - - Returned device is not referenced. It is attached to the child device, and will be cleaned up when the child device is cleaned up. - It is not necessarily just the upper level directory, empty or not recognized sys directories are ignored. - - It can be called as many times as needed, without caring about references. - - \param udev_device: the device to start searching from - \return a new udev device, or NULL, if it no parent exist. -*/ -udev_device *XsUdev::device_get_parent(struct udev_device *udev_device) -{ - if (m_uDev.device_get_parent) - return m_uDev.device_get_parent(udev_device); - else - return NULL; -} - -/*! \brief Retrieve the device node file name belonging to the udev device. - - The path is an absolute path, and starts with the device directory. - - \param udev_device udev device - \return the device node file name of the udev device, or NULL if no device node exists -*/ -const char *XsUdev::device_get_devnode(struct udev_device *udev_device) -{ - if (m_uDev.device_get_devnode) - return m_uDev.device_get_devnode(udev_device); - else - return ""; -} - -/*! \brief Find the next parent device, with a matching subsystem and devtypevalue, and fill in information from the sys device and the udev database entry. - - If devtype is NULL, only subsystem is checked, and any devtype will match. - - Returned device is not referenced. It is attached to the child device, and will be cleaned up when the child device is cleaned up. - - It can be called as many times as needed, without caring about references. - - \param udev_device udev device to start searching from - \param subsystem the subsystem of the device - \param devtype the type (DEVTYPE) of the device - \return a new udev device, or NULL if no matching parent exists. -*/ -udev_device *XsUdev::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); - else - return NULL; -} - -/*! \brief Get a sys attribute value - - The retrieved value is cached in the device. Repeated calls will return the same value and not open the attribute again. - - \param udev_device udev device - \param sysattr attribute name - - \return the content of a sys attribute file, or NULL if there is no sys attribute value. -*/ -const char *XsUdev::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); - else - return ""; -} diff --git a/extern/xcommunication/xswinusb.cpp b/extern/xcommunication/xswinusb.cpp deleted file mode 100644 index 4562b85..0000000 --- a/extern/xcommunication/xswinusb.cpp +++ /dev/null @@ -1,534 +0,0 @@ -/* Copyright (c) 2003-2017 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. -*/ - -#include "xswinusb.h" - -#ifdef USE_WINUSB -#include - -/*! \class XsWinUsb - \brief Class for dynamic loading of winusb -*/ -XsWinUsb::XsWinUsb(void) -{ - m_libraryLoader = new XsLibraryLoader(); - initLibrary(); -} - -XsWinUsb::~XsWinUsb(void) -{ - delete m_libraryLoader; -} - -void XsWinUsb::initLibrary() -{ - if (!m_libraryLoader->isLoaded()) - m_libraryLoader->load("winusb.dll"); - - m_winUsb.AbortPipe = NULL; - m_winUsb.Initialize = NULL; - m_winUsb.Free = NULL; - m_winUsb.GetAssociatedInterface = NULL; - m_winUsb.GetDescriptor = NULL; - m_winUsb.QueryInterfaceSettings = NULL; - m_winUsb.QueryDeviceInformation = NULL; - m_winUsb.SetCurrentAlternateSetting = NULL; - m_winUsb.GetCurrentAlternateSetting = NULL; - m_winUsb.QueryPipe = NULL; - m_winUsb.SetPipePolicy = NULL; - m_winUsb.GetPipePolicy = NULL; - m_winUsb.ReadPipe = NULL; - m_winUsb.WritePipe = NULL; - m_winUsb.ControlTransfer = NULL; - m_winUsb.ResetPipe = NULL; - m_winUsb.AbortPipe = NULL; - m_winUsb.FlushPipe = NULL; - m_winUsb.SetPowerPolicy = NULL; - m_winUsb.GetPowerPolicy = NULL; - m_winUsb.GetOverlappedResult = NULL; - - if (m_libraryLoader->isLoaded()) - { - m_winUsb.AbortPipe = (WinUSB_AbortPipe*)m_libraryLoader->resolve("WinUsb_AbortPipe"); - m_winUsb.Initialize = (WinUSB_Initialize*)m_libraryLoader->resolve("WinUsb_Initialize"); - m_winUsb.Free = (WinUSB_Free*)m_libraryLoader->resolve("WinUsb_Free"); - m_winUsb.GetAssociatedInterface = (WinUSB_GetAssociatedInterface*)m_libraryLoader->resolve("WinUsb_GetAssociatedInterface"); - m_winUsb.GetDescriptor = (WinUSB_GetDescriptor*)m_libraryLoader->resolve("WinUsb_GetDescriptor"); - m_winUsb.QueryInterfaceSettings = (WinUSB_QueryInterfaceSettings*)m_libraryLoader->resolve("WinUsb_QueryInterfaceSettings"); - m_winUsb.QueryDeviceInformation = (WinUSB_QueryDeviceInformation*)m_libraryLoader->resolve("WinUsb_QueryDeviceInformation"); - m_winUsb.SetCurrentAlternateSetting = (WinUSB_SetCurrentAlternateSetting*)m_libraryLoader->resolve("WinUsb_SetCurrentAlternateSetting"); - m_winUsb.GetCurrentAlternateSetting = (WinUSB_GetCurrentAlternateSetting*)m_libraryLoader->resolve("WinUsb_GetCurrentAlternateSetting"); - m_winUsb.QueryPipe = (WinUSB_QueryPipe*)m_libraryLoader->resolve("WinUsb_QueryPipe"); - m_winUsb.SetPipePolicy = (WinUSB_SetPipePolicy*)m_libraryLoader->resolve("WinUsb_SetPipePolicy"); - m_winUsb.GetPipePolicy = (WinUSB_GetPipePolicy*)m_libraryLoader->resolve("WinUsb_GetPipePolicy"); - m_winUsb.ReadPipe = (WinUSB_ReadPipe*)m_libraryLoader->resolve("WinUsb_ReadPipe"); - m_winUsb.WritePipe = (WinUSB_WritePipe*)m_libraryLoader->resolve("WinUsb_WritePipe"); - m_winUsb.ControlTransfer = (WinUSB_ControlTransfer*)m_libraryLoader->resolve("WinUsb_ControlTransfer"); - m_winUsb.ResetPipe = (WinUSB_ResetPipe*)m_libraryLoader->resolve("WinUsb_ResetPipe"); - m_winUsb.AbortPipe = (WinUSB_AbortPipe*)m_libraryLoader->resolve("WinUsb_AbortPipe"); - m_winUsb.FlushPipe = (WinUSB_FlushPipe*)m_libraryLoader->resolve("WinUsb_FlushPipe"); - m_winUsb.SetPowerPolicy = (WinUSB_SetPowerPolicy*)m_libraryLoader->resolve("WinUsb_SetPowerPolicy"); - m_winUsb.GetPowerPolicy = (WinUSB_GetPowerPolicy*)m_libraryLoader->resolve("WinUsb_GetPowerPolicy"); - m_winUsb.GetOverlappedResult = (WinUSB_GetOverlappedResult*)m_libraryLoader->resolve("WinUsb_GetOverlappedResult"); - } -} - -/*! \brief Creates/opens a WinUsb interface handle from the device list. - - \param[out] InterfaceHandle Receives a handle configured to the first (default) interface on the device. This handle is required by other WinUsb routines that perform operations on the default interface. The handle is opaque. To release this handle, call the \a Free function. - \param[in] DevInfo The device list element to open. - - \returns On success, TRUE. Otherwise FALSE. Use GetLastError() to get extended error information. -*/ -BOOL XsWinUsb::Initialize(HANDLE DeviceHandle, PWINUSB_INTERFACE_HANDLE InterfaceHandle) -{ - if (m_winUsb.Initialize) - return m_winUsb.Initialize(DeviceHandle, InterfaceHandle); - else - return FALSE; -} - -/*! \brief Frees a WinUsb interface handle. - \param[in] InterfaceHandle Handle to an interface on the device. This handle must be created by a previous call to see \a Initialize or \a GetAssociatedInterface. - - \returns TRUE - - \sa GetAssociatedInterface. -*/ -BOOL XsWinUsb::Free(WINUSB_INTERFACE_HANDLE InterfaceHandle) -{ - if (m_winUsb.Free) - return m_winUsb.Free(InterfaceHandle); - else - return FALSE; -} - -/*! \brief Retrieves a handle for an associated interface. - \param[in] InterfaceHandle An initialized usb handle, see \a Initialize. - \param[in] AssociatedInterfaceIndex An index that specifies the associated interface to retrieve. A value of 0 indicates the first associated interface, a value of 1 indicates the second associated interface, and so on. - \param[out] AssociatedInterfaceHandle A handle for the associated interface. Callers must pass this interface handle to WinUsb Functions exposed by WinUsb.dll. To close this handle, call Free. - - \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) -{ - if (m_winUsb.GetAssociatedInterface) - return m_winUsb.GetAssociatedInterface(InterfaceHandle, AssociatedInterfaceIndex, AssociatedInterfaceHandle); - else - return FALSE; -} - -/*! \brief Gets the requested descriptor. This is a synchronous operation. - \param[in] InterfaceHandle An initialized usb handle, see \a Initialize. - \param[in] DescriptorType A value that specifies the type of descriptor to return. This parameter corresponds to the bDescriptorType field of a standard device descriptor, whose values are described in the Universal Serial Bus specification. - \param[in] Index The descriptor index. For an explanation of the descriptor index, see the Universal Serial Bus specification (www.usb.org). - \param[in] LanguageID A value that specifies the language identifier, if the requested descriptor is a string descriptor. - \param[out] Buffer A caller-allocated buffer that receives the requested descriptor. - \param[in] BufferLength The length, in bytes, of Buffer. - \param[out] LengthTransferred The number of bytes that were copied into Buffer. - - \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) -{ - if (m_winUsb.GetDescriptor) - return m_winUsb.GetDescriptor(InterfaceHandle, DescriptorType, Index, LanguageID, Buffer, BufferLength, LengthTransferred); - else - return FALSE; -} - -/*! \brief Retrieves the interface descriptor for the specified alternate interface settings for a particular interface handle. - - The \a QueryInterfaceSettings call searches the current/default interface array for the alternate interface specified by the caller in the \a AltSettingIndex. - If the specified alternate interface is found, the function populates the caller-allocated USB_INTERFACE_DESCRIPTOR structure. - If the specified alternate interface is not found, then the call fails with the ERROR_NO_MORE_ITEMS code. - - \param[in] InterfaceHandle An initialized usb handle, see \a Initialize. - \param[in] AltSettingIndex A value that indicates which alternate setting index to return. A value of 0 indicates the first alternate setting, a value of 1 indicates the second alternate setting, and so on. - \param[out] UsbAltInterfaceDescriptor A pointer to a caller-allocated USB_INTERFACE_DESCRIPTOR structure that contains information about the interface that AltSettingNumber specified. - - \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) -{ - if (m_winUsb.QueryInterfaceSettings) - return m_winUsb.QueryInterfaceSettings(InterfaceHandle, AlternateInterfaceNumber, UsbAltInterfaceDescriptor); - else - return FALSE; -} - -/*! \brief Retrieves information about the physical device that is associated with a WinUSB handle. - \param[in] InterfaceHandle An initialized usb handle, see \a Initialize. - \param[in] InformationType A value that specifies which interface information value to retrieve. On input, InformationType must have the following value: DEVICE_SPEED (0x01). - \param[in,out] BufferLength The maximum number of bytes to read. This number must be less than or equal to the size, in bytes, of Buffer. On output, BufferLength is set to the actual number of bytes that were copied into Buffer. - \param[in,out] Buffer A caller-allocated buffer that receives the requested value. On output, Buffer indicates the device speed: - (0x01) low/full speed device. - (0x03) high speed device. - - \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) -{ - if (m_winUsb.QueryDeviceInformation) - return m_winUsb.QueryDeviceInformation(InterfaceHandle, InformationType, BufferLength, Buffer); - else - return FALSE; -} - -/*! \brief Sets the alternate setting of an interface. - Sets the active \a bAlternateSetting for the current/default interface. - - \param[in] InterfaceHandle An initialized usb handle, see \a Initialize. - \param[in] AltSettingNumber The value that is contained in the \a bAlternateSetting member of the USB_INTERFACE_DESCRIPTOR structure. This structure can be populated by the \a QueryInterfaceSettings routine. - - \returns On success, TRUE. Otherwise FALSE. Use GetLastError() to get extended error information. - - \sa QueryInterfaceSettings -*/ -BOOL XsWinUsb::SetCurrentAlternateSetting(WINUSB_INTERFACE_HANDLE InterfaceHandle,UCHAR SettingNumber) -{ - if (m_winUsb.SetCurrentAlternateSetting) - return m_winUsb.SetCurrentAlternateSetting(InterfaceHandle, SettingNumber); - else - return FALSE; -} - -/*! \brief Gets the current alternate interface setting for an interface. - Gets the active bAlternateSetting for the current/default interface. - - \param[in] InterfaceHandle An initialized usb handle, see \a Initialize. - \param[out] AltSettingNumber A pointer to an unsigned character that receives an integer that indicates the current alternate setting. - - \returns On success, TRUE. Otherwise FALSE. Use GetLastError() to get extended error information. -*/ -BOOL XsWinUsb::GetCurrentAlternateSetting(WINUSB_INTERFACE_HANDLE InterfaceHandle,PUCHAR SettingNumber) -{ - if (m_winUsb.GetCurrentAlternateSetting) - return m_winUsb.GetCurrentAlternateSetting(InterfaceHandle, SettingNumber); - else - return FALSE; -} - -/*! \brief Retrieves information about a pipe that is associated with an interface. - The \a QueryPipe function does not retrieve information about the control pipe. - - Each interface on the USB device can have multiple endpoints. To communicate with each of these endpoints, the bus driver creates pipes for each endpoint on the interface. - The pipe indices are zero-based. Therefore for n number of endpoints, the pipes' indices are set from n-1. - \a QueryPipe parses the configuration descriptor to get the interface specified by the caller. - It searches the interface descriptor for the endpoint descriptor associated with the caller-specified pipe. - If the endpoint is found, the function populates the caller-allocated WINUSB_PIPE_INFORMATION structure with information from the endpoint descriptor. - - \param[in] InterfaceHandle An initialized usb handle, see \a Initialize. - \param[in] AltSettingNumber A value that specifies the alternate interface to return the information for. - \param[in] PipeIndex A value that specifies the pipe to return information about. This value is not the same as the bEndpointAddress field in the endpoint descriptor. - A PipeIndex value of 0 signifies the first endpoint that is associated with the interface, a value of 1 signifies the second endpoint, and so on. - PipeIndex must be less than the value in the bNumEndpoints field of the interface descriptor. - \param[out] PipeInformation A pointer, on output, to a caller-allocated WINUSB_PIPE_INFORMATION structure that contains pipe information. - - \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) -{ - if (m_winUsb.QueryPipe) - return m_winUsb.QueryPipe(InterfaceHandle, AlternateInterfaceNumber, PipeIndex, PipeInformation); - else - return FALSE; -} - -/*! \brief Sets the policy for a specific pipe associated with an endpoint on the device. This is a synchronous operation. - \param[in] InterfaceHandle An initialized usb handle, see \a Initialize. - \param[in] PipeID An 8-bit value that consists of a 7-bit address and a direction bit. This parameter corresponds to the bEndpointAddress field in the endpoint descriptor. - \param[in] PolicyType A UINT variable that specifies the policy parameter to change. The Value parameter contains the new value for the policy parameter. - See the remarks section for information about each of the pipe policies and the resulting behavior. - \param[in] ValueLength The size, in bytes, of the buffer at Value. - \param[in] Value The new value for the policy parameter that PolicyType specifies. The size of this input parameter depends on the policy to change. For information about the size of this parameter, see the description of the PolicyType parameter. - - \remarks - The following list describes symbolic constants for \a PolicyType - - - SHORT_PACKET_TERMINATE (0x01) - - The default value is FALSE. - - To enable SHORT_PACKET_TERMINATE, in Value pass the address of a caller-allocated UCHAR variable set to TRUE (nonzero). - - Enabling SHORT_PACKET_TERMINATE causes the driver to send a zero-length packet at the end of every write request to the host controller. - - - AUTO_CLEAR_STALL (0x02) - - The default value is FALSE. To enable AUTO_CLEAR_STALL, in Value pass the address of a caller-allocated UCHAR variable set to TRUE (nonzero). - - Enabling AUTO_CLEAR_STALL causes winUSB to reset the pipe in order to automatically clear the stall condition. Data continues to flow on the bulk and interrupt IN endpoints again as soon as a new or a queued transfer arrives on the endpoint. This policy parameter does not affect control pipes. - - Disabling AUTO_CLEAR_STALL causes all transfers (that arrive to the endpoint after the stalled transfer) to fail until the caller manually resets the endpoint's pipe by calling ResetPipe. - - - PIPE_TRANSFER_TIMEOUT (0x03) - - The default value is zero. To set a time-out value, in Value pass the address of a caller-allocated UINT variable that contains the time-out interval. - - The PIPE_TRANSFER_TIMEOUT value specifies the time-out interval, in milliseconds. The host controller cancels transfers that do not complete within the specified time-out interval. - - A value of zero (default) indicates that transfers do not time out because the host controller never cancels the transfer. - - - IGNORE_SHORT_PACKETS (0x04) - - The default value is FALSE. To enable IGNORE_SHORT_PACKETS, in Value pass the address of a caller-allocated UCHAR variable set to TRUE (nonzero). - - Enabling IGNORE_SHORT_PACKETS causes the host controller to not complete a read operation after it receives a short packet. Instead, the host controller completes the operation only after the host has read the specified number of bytes. - - Disabling IGNORE_SHORT_PACKETS causes the host controller to complete a read operation when either the host has read the specified number of bytes or the host has received a short packet. - - - ALLOW_PARTIAL_READS (0x05) - - The default value is TRUE (nonzero). To disable ALLOW_PARTIAL_READS, in Value pass the address of a caller-allocated UCHAR variable set to FALSE (zero). - - Disabling ALLOW_PARTIAL_READS causes the read requests to fail whenever the device returns more data (on bulk and interrupt IN endpoints) than the caller requested. - - Enabling ALLOW_PARTIAL_READS causes winUSB to save or discard the extra data when the device returns more data (on bulk and interrupt IN endpoints) than the caller requested. This behavior is defined by setting the AUTO_FLUSH value. - - - AUTO_FLUSH (0x06) - - The default value is FALSE (zero). To enable AUTO_FLUSH, in Value pass the address of a caller-allocated UCHAR variable set to TRUE (nonzero). - - AUTO_FLUSH must be used with ALLOW_PARTIAL_READS enabled. If ALLOW_PARTIAL_READS is TRUE, the value of AUTO_FLUSH determines the action taken by winUSB when the device returns more data than the caller requested. - - Disabling ALLOW_PARTIAL_READS causes winUSB to ignore the AUTO_FLUSH value. - - Disabling AUTO_FLUSH with ALLOW_PARTIAL_READS enabled causes winUSB to save the extra data, add the data to the beginning of the caller's next read request, and send it to the caller in the next read operation. - - Enabling AUTO_FLUSH with ALLOW_PARTIAL_READS enabled causes winUSB to discard the extra data remaining from the read request. - - - RAW_IO (0x07) - - The default value is FALSE (zero). To enable RAW_IO, in Value pass the address of a caller-allocated UCHAR variable set to TRUE (nonzero). - - Enabling RAW_IO causes winUSB to send data directly to the USB driver stack, bypassing winUSB's queuing and error handling mechanism. - - The buffers that are passed to \a ReadPipe must be configured by the caller as follows: - - The buffer length must be a multiple of the maximum endpoint packet size. - - The length must be less than or equal to the value of MAXIMUM_TRANSFER_SIZE retrieved by GetPipePolicy. - - Disabling RAW_IO (FALSE) does not impose any restriction on the buffers that are passed to \a ReadPipe. - - - RESET_PIPE_ON_RESUME (0x09) - - The default value is FALSE (zero). To enable RESET_PIPE_ON_RESUME, in Value pass the address of a caller-allocated UCHAR variable set to TRUE (nonzero). - - TRUE (or a nonzero value) indicates that on resume from suspend, winUSB resets the endpoint before it allows the caller to send new requests to the endpoint. - - - \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) -{ - if (m_winUsb.SetPipePolicy) - return m_winUsb.SetPipePolicy(InterfaceHandle, PipeID, PolicyType, ValueLength, Value); - else - return FALSE; -} - -/*! \brief Gets the policy for a specific pipe (endpoint). - \param[in] InterfaceHandle An initialized usb handle, see \a Initialize. - \param[in] PipeID An 8-bit value that consists of a 7-bit address and a direction bit. This parameter corresponds to the bEndpointAddress field in the endpoint descriptor. - \param[in] PolicyType A UINT variable that specifies the policy parameter to retrieve. The current value for the policy parameter is retrieved the Value parameter. - \param[in,out] ValueLength A pointer to the size, in bytes, of the buffer that Value points to. On output, ValueLength receives the size, in bytes, of the data that was copied into the Value buffer. - \param[out] Value A pointer to a buffer that receives the specified pipe policy value. - - \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) -{ - if (m_winUsb.GetPipePolicy) - return m_winUsb.GetPipePolicy(InterfaceHandle, PipeID, PolicyType, ValueLength, Value); - else - return FALSE; -} - -/*! \brief Reads data from the specified pipe. - \param[in] InterfaceHandle An initialized usb handle, see \a Initialize. - \param[in] PipeID An 8-bit value that consists of a 7-bit address and a direction bit. This parameter corresponds to the bEndpointAddress field in the endpoint descriptor. - \param[out] Buffer A caller-allocated buffer that receives the data that is read. - \param[in] BufferLength The maximum number of bytes to read. This number must be less than or equal to the size, in bytes, of Buffer. - \param[out] LengthTransferred A pointer to a UINT variable that receives the actual number of bytes that were copied into Buffer. For more information, see Remarks. - \param[in] Overlapped An optional pointer to an overlapped structure for asynchronous operations. This can be a KOVL_HANDLE or a pointer to a standard windows OVERLAPPED structure. - If this parameter is specified, \a ReadPipe returns immediately rather than waiting synchronously for the operation to complete before returning. An event is signaled when the operation is complete. - - \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) -{ - if (m_winUsb.ReadPipe) - return m_winUsb.ReadPipe(InterfaceHandle, PipeID, Buffer, BufferLength, LengthTransferred, Overlapped); - else - return FALSE; -} - -/*! \brief Writes data to a pipe. - \param[in] InterfaceHandle An initialized usb handle, see \a Initialize. - \param[in] PipeID An 8-bit value that consists of a 7-bit address and a direction bit. This parameter corresponds to the bEndpointAddress field in the endpoint descriptor. - \param[in] Buffer A caller-allocated buffer the data is written from. - \param[in] BufferLength The maximum number of bytes to write. This number must be less than or equal to the size, in bytes, of Buffer. - \param[out] LengthTransferred A pointer to a UINT variable that receives the actual number of bytes that were transferred from Buffer. - \param[in] Overlapped An optional pointer to an overlapped structure for asynchronous operations. This can be a KOVL_HANDLE or a pointer to a standard windows OVERLAPPED structure. - If this parameter is specified, \a WritePipe returns immediately rather than waiting synchronously for the operation to complete before returning. An event is signaled when the operation is complete. - - \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) -{ - if (m_winUsb.WritePipe) - return m_winUsb.WritePipe(InterfaceHandle, PipeID, Buffer, BufferLength, LengthTransferred, Overlapped); - else - return FALSE; -} - -/*! \brief Transmits control data over a default control endpoint. - A ControlTransfer is never cached. These requests always go directly to the usb device. - - \param[in] InterfaceHandle A valid winUSB interface handle returned by: - - Initialize - - GetAssociatedInterface - \param[in] SetupPacket The 8-byte setup packet of type WINUSB_SETUP_PACKET. - \param[in,out] Buffer A caller-allocated buffer that contains the data to transfer. - \param[in] BufferLength The number of bytes to transfer, not including the setup packet. This number must be less than or equal to the size, in bytes, of Buffer. - \param[out] LengthTransferred A pointer to a UINT variable that receives the actual number of transferred bytes. If the application does not expect any data to be transferred during the data phase (BufferLength is zero), LengthTransferred can be NULL. - \param[in] Overlapped An optional pointer to an OVERLAPPED structure, which is used for asynchronous operations. If this parameter is specified, ControlTransfer immediately returns, and the event is signaled when the operation is complete. If Overlapped is not supplied, the ControlTransfer function transfers data synchronously. - - \returns On success, TRUE. Otherwise FALSE. Use GetLastError() to get extended error information. If an Overlapped member is supplied and the operation succeeds this function returns FALSE and sets last error to ERROR_IO_PENDING. -*/ -BOOL XsWinUsb::ControlTransfer(WINUSB_INTERFACE_HANDLE InterfaceHandle,WINUSB_SETUP_PACKET SetupPacket,PUCHAR Buffer,ULONG BufferLength,PULONG LengthTransferred,LPOVERLAPPED Overlapped) -{ - if (m_winUsb.ControlTransfer) - return m_winUsb.ControlTransfer(InterfaceHandle, SetupPacket, Buffer, BufferLength, LengthTransferred, Overlapped); - else - return FALSE; -} - -/*! \brief Resets the data toggle and clears the stall condition on a pipe. - \param[in] InterfaceHandle An initialized usb handle, see \a Initialize. - \param[in] PipeID An 8-bit value that consists of a 7-bit address and a direction bit. This parameter corresponds to the \a bEndpointAddress field in the endpoint descriptor. - - \returns On success, TRUE. Otherwise FALSE. Use GetLastError() to get extended error information. -*/ -BOOL XsWinUsb::ResetPipe(WINUSB_INTERFACE_HANDLE InterfaceHandle,UCHAR PipeID) -{ - if (m_winUsb.ResetPipe) - return m_winUsb.ResetPipe(InterfaceHandle, PipeID); - else - return FALSE; -} - -/*! \brief Aborts all of the pending transfers for a pipe. - \param[in] InterfaceHandle An initialized usb handle, see \a Initialize. - \param[in] PipeID An 8-bit value that consists of a 7-bit address and a direction bit. This parameter corresponds to the \a bEndpointAddress field in the endpoint descriptor. - - \returns On success, TRUE. Otherwise FALSE. Use GetLastError() to get extended error information. -*/ -BOOL XsWinUsb::AbortPipe(WINUSB_INTERFACE_HANDLE InterfaceHandle,UCHAR PipeID) -{ - if (m_winUsb.AbortPipe) - return m_winUsb.AbortPipe(InterfaceHandle, PipeID); - else - return FALSE; -} - -/*! \brief Discards any data that is cached in a pipe. - \param[in] InterfaceHandle An initialized usb handle, see \a Initialize. - \param[in] PipeID An 8-bit value that consists of a 7-bit address and a direction bit. This parameter corresponds to the \a bEndpointAddress field in the endpoint descriptor. - - \returns On success, TRUE. Otherwise FALSE. Use GetLastError() to get extended error information. -*/ -BOOL XsWinUsb::FlushPipe(WINUSB_INTERFACE_HANDLE InterfaceHandle,UCHAR PipeID) -{ - if (m_winUsb.FlushPipe) - return m_winUsb.FlushPipe(InterfaceHandle, PipeID); - else - return FALSE; -} - -/*! \brief Sets the power policy for a device. - The following list summarizes the effects of changes to power management states: - - - All pipe handles, interface handles, locks, and alternate settings are preserved across power management events. - - Any transfers that are in progress are suspended when a device transfers to a low power state, and they are resumed when the device is restored to a working state. - - The device and system must be in a working state before the client can restore a device-specific configuration. Clients can determine whether the device and system are in a working state from the WM_POWERBROADCAST message. - - The client can indicate that an interface is idle by calling \a SetPowerPolicy. - - \param[in] InterfaceHandle An initialized usb handle, see \a Initialize. - \param[in] PolicyType A value that specifies the power policy to set. The following table describes symbolic constants. - - AUTO_SUSPEND (0x81) - - Specifies the auto-suspend policy type; the power policy parameter must be specified by the caller in the Value parameter. - - For auto-suspend, the Value parameter must point to a UCHAR variable. - - If Value is TRUE (nonzero), the USB stack suspends the device if the device is idle. A device is idle if there are no transfers pending, or if the only pending transfers are IN transfers to interrupt or bulk endpoints. - - The default value is determined by the value set in the DefaultIdleState registry setting. By default, this value is TRUE. - - - SUSPEND_DELAY (0x83) - - Specifies the suspend-delay policy type; the power policy parameter must be specified by the caller in the Value parameter. - - For suspend-delay, Value must point to a UINT variable. - - Value specifies the minimum amount of time, in milliseconds, that the driver must wait post transfer before it can suspend the device. - - The default value is determined by the value set in the DefaultIdleTimeout registry setting. By default, this value is five seconds. - - \param[in] ValueLength The size, in bytes, of the buffer at Value. - \param[in] Value The new value for the power policy parameter. Data type and value for Value depends on the type of power policy passed in PolicyType. For more information, see PolicyType. - - \returns On success, TRUE. Otherwise FALSE. Use GetLastError() to get extended error information. -*/ -BOOL XsWinUsb::SetPowerPolicy(WINUSB_INTERFACE_HANDLE InterfaceHandle,ULONG PolicyType,ULONG ValueLength,PVOID Value) -{ - if (m_winUsb.SetPowerPolicy) - return m_winUsb.SetPowerPolicy(InterfaceHandle, PolicyType, ValueLength, Value); - else - return FALSE; -} - -/*! \brief Gets the power policy for a device. - \param[in] InterfaceHandle An initialized usb handle, see \a Initialize - \param[in] PolicyType A value that specifies the power policy parameter to retrieve in Value. The following table describes symbolic constants that are valid. - - AUTO_SUSPEND (0x81) - - If the caller specifies a power policy of AUTO_SUSPEND, \a GetPowerPolicy returns the value of the auto suspend policy parameter in the Value parameter. - - If Value is TRUE (that is, nonzero), the USB stack suspends the device when no transfers are pending or the only transfers pending are IN transfers on an interrupt or bulk endpoint. - - The value of the DefaultIdleState registry value determines the default value of the auto suspend policy parameter. - - The Value parameter must point to a UCHAR variable. - - - SUSPEND_DELAY (0x83) - - If the caller specifies a power policy of SUSPEND_DELAY, \a GetPowerPolicy returns the value of the suspend delay policy parameter in Value. - - The suspend delay policy parameter specifies the minimum amount of time, in milliseconds, that the driver must wait after any transfer before it can suspend the device. - - Value must point to a UINT variable. - - \param[in,out] ValueLength A pointer to the size of the buffer that Value. On output, ValueLength receives the size of the data that was copied into the Value buffer. - \param[out] Value A buffer that receives the specified power policy parameter. For more information, see PolicyType. - - \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) -{ - if (m_winUsb.GetPowerPolicy) - return m_winUsb.GetPowerPolicy(InterfaceHandle, PolicyType, ValueLength, Value); - else - return FALSE; -} - -/*! \brief Retrieves the results of an overlapped operation on the specified winUSB handle. - - This function is like the Win32 API routine, GetOverlappedResult, with one difference; instead of passing a file handle that is returned from CreateFile, the caller passes an interface handle that is returned from \a Initialize, or \a GetAssociatedInterface. The caller can use either API routine, if the appropriate handle is passed. The \a GetOverlappedResult function extracts the file handle from the interface handle and then calls GetOverlappedResult. - The results that are reported by the GetOverlappedResult function are those from the specified handle's last overlapped operation to which the specified standard windows OVERLAPPED structure was provided, and for which the operation's results were pending. A pending operation is indicated when the function that started the operation returns FALSE, and the GetLastError routine returns ERROR_IO_PENDING. When an I/O operation is pending, the function that started the operation resets the hEvent member of the standard windows OVERLAPPED structure to the nonsignaled state. Then when the pending operation has been completed, the system sets the event object to the signaled state. - The caller can specify that an event object is manually reset in the standard windows OVERLAPPED structure. If an automatic reset event object is used, the event handle must not be specified in any other wait operation in the interval between starting the overlapped operation and the call to \a GetOverlappedResult. For example, the event object is sometimes specified in one of the wait routines to wait for the operation to be completed. When the wait routine returns, the system sets an auto-reset event's state to nonsignaled, and a successive call to \a GetOverlappedResult with the bWait parameter set to TRUE causes the function to be blocked indefinitely. - If the bWait parameter is TRUE, GetOverlappedResult determines whether the pending operation has been completed by waiting for the event object to be in the signaled state. - - If the hEvent member of the standard windows OVERLAPPED structure is NULL, the system uses the state of the file handle to signal when the operation has been completed. Do not use file handles for this purpose. It is better to use an event object because of the confusion that can occur when multiple concurrent overlapped operations are performed on the same file. In this situation, you cannot know which operation caused the state of the object to be signaled. - - \param[in] InterfaceHandle An initialized usb handle, see \a Initialize - \param[in] Overlapped A pointer to a standard windows OVERLAPPED structure that was specified when the overlapped operation was started. - \param[out] lpNumberOfBytesTransferred A pointer to a variable that receives the number of bytes that were actually transferred by a read or write operation. - \param[in] bWait If this parameter is TRUE, the function does not return until the operation has been completed. If this parameter is FALSE and the operation is still pending, the function returns FALSE and the GetLastError function returns ERROR_IO_INCOMPLETE. - - \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) -{ - if (m_winUsb.GetOverlappedResult) - return m_winUsb.GetOverlappedResult(InterfaceHandle, lpOverlapped, lpNumberOfBytesTransferred, bWait); - else - return FALSE; -} - -#endif // USE_WINUSB diff --git a/extern/xspublic/xscommon/abstractadditionallogger.h b/extern/xspublic/xscommon/abstractadditionallogger.h new file mode 100644 index 0000000..52d9913 --- /dev/null +++ b/extern/xspublic/xscommon/abstractadditionallogger.h @@ -0,0 +1,90 @@ + +// Copyright (c) 2003-2022 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 ABSTRACTADDITIONALLOGGER_H +#define ABSTRACTADDITIONALLOGGER_H + +#include "journalloglevel.h" +#include + +/*! \class AbstractAdditionalLogger + Interface for all additional loggers that can be attached to the Journaller +*/ +class AbstractAdditionalLogger +{ +public: + /*! \brief Destructor */ + virtual ~AbstractAdditionalLogger() {} + + /*! \returns true if loglevel \a level should be logged, false otherwise + \param[in] level The log level to check + */ + virtual bool logLevel(JournalLogLevel level) const = 0; + + /*! \returns the current log level + */ + virtual JournalLogLevel logLevel() const = 0; + + /*! \returns the current debug log level + */ + virtual JournalLogLevel debugLevel() const = 0; + + /*! \brief Set the current log level + \param[in] level The new log level + */ + virtual void setLogLevel(JournalLogLevel level) = 0; + + /*! \brief Set the current debug log level + \param[in] level The new debug log level + */ + virtual void setDebugLevel(JournalLogLevel level) = 0; + + /*! \brief Write line to the log + \param[in] level The log level + \param[in] file The name of the file from which the logging originates + \param[in] line The line number from which the logging originates + \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; + + /*! \brief Write line to the log without decoration (timestamp, thread) + \param[in] level The log level + \param[in] file The name of the file from which the logging originates + \param[in] line The line number from which the logging originates + \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; +}; + +#endif diff --git a/extern/xspublic/xscommon/additionalloggerbase.cpp b/extern/xspublic/xscommon/additionalloggerbase.cpp new file mode 100644 index 0000000..90206b1 --- /dev/null +++ b/extern/xspublic/xscommon/additionalloggerbase.cpp @@ -0,0 +1,112 @@ + +// Copyright (c) 2003-2022 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 "additionalloggerbase.h" + +/*! \brief Constructs an AdditionalLoggerBase + \param[in] initialLogLevel The initial log level to use as log level and debug log level +*/ +AdditionalLoggerBase::AdditionalLoggerBase(JournalLogLevel initialLogLevel) + : m_level(initialLogLevel), m_debugLevel(initialLogLevel) +{ +} + +/*! \copydoc AbstractAdditionalLogger::logLevel() const +*/ +JournalLogLevel AdditionalLoggerBase::logLevel() const +{ + return m_level; +} + +/*! \copydoc AbstractAdditionalLogger::debugLevel +*/ +JournalLogLevel AdditionalLoggerBase::debugLevel() const +{ + return m_debugLevel; +} + +/*! \copydoc AbstractAdditionalLogger::setLogLevel +*/ +void AdditionalLoggerBase::setLogLevel(JournalLogLevel level) +{ + if (level != m_level) + { + JournalLogLevel oldLevel = m_level; + m_level = level; + onLogLevelChanged(m_level, oldLevel); + } +} + +/*! \copydoc AbstractAdditionalLogger::setDebugLevel +*/ +void AdditionalLoggerBase::setDebugLevel(JournalLogLevel level) +{ + if (level != m_debugLevel) + { + JournalLogLevel oldLevel = m_debugLevel; + m_debugLevel = level; + onDebugLevelChanged(m_debugLevel, oldLevel); + } +} + +/*! \brief Called when the log level changes + This can be overridden in derived classes +*/ +void AdditionalLoggerBase::onLogLevelChanged(JournalLogLevel newLevel, JournalLogLevel oldLevel) +{ + (void)newLevel; + (void)oldLevel; +} + +/*! \brief Called when the debug log level changes + This can be overridden in derived classes +*/ +void AdditionalLoggerBase::onDebugLevelChanged(JournalLogLevel newLevel, JournalLogLevel oldLevel) +{ + (void)newLevel; + (void)oldLevel; +} + +/*! \copydoc AbstractAdditionalLogger::logLevel +*/ +bool AdditionalLoggerBase::logLevel(JournalLogLevel level) const +{ + return level >= m_level || level >= m_debugLevel; +} + +/*! \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) +{ + log(level, file, line, function, msg); +} diff --git a/extern/xspublic/xscommon/additionalloggerbase.h b/extern/xspublic/xscommon/additionalloggerbase.h new file mode 100644 index 0000000..dbab86a --- /dev/null +++ b/extern/xspublic/xscommon/additionalloggerbase.h @@ -0,0 +1,66 @@ + +// Copyright (c) 2003-2022 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 ADDITIONALLOGGERBASE_H +#define ADDITIONALLOGGERBASE_H + +#include "abstractadditionallogger.h" + +/*! \class AdditionalLoggerBase + Convenient base class for custom additional loggers. + Already implements functionality such as log level that should be common for all additional loggers + Custom additional loggers can be derived from this +*/ +class AdditionalLoggerBase : public virtual AbstractAdditionalLogger +{ +public: + AdditionalLoggerBase(JournalLogLevel initialLogLevel = JLL_Alert); + + JournalLogLevel logLevel() const override; + JournalLogLevel debugLevel() const override; + void setLogLevel(JournalLogLevel level) override; + void setDebugLevel(JournalLogLevel level) override; + + bool logLevel(JournalLogLevel level) const 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); + virtual void onDebugLevelChanged(JournalLogLevel newLevel, JournalLogLevel oldLevel); + +private: + JournalLogLevel m_level; + JournalLogLevel m_debugLevel; +}; + +#endif diff --git a/extern/xspublic/xscommon/calltracer.h b/extern/xspublic/xscommon/calltracer.h new file mode 100644 index 0000000..04e1b5a --- /dev/null +++ b/extern/xspublic/xscommon/calltracer.h @@ -0,0 +1,91 @@ + +// Copyright (c) 2003-2022 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. +// + +#ifdef CALLTRACER_H + XSENS_COMPILER_WARNING("X0008", "calltracer.h should only be included ONCE and NEVER in a header file") +#else + #define CALLTRACER_H +#endif + +//#define TRACECALLS // define before including this header file to enable semi-automatic calltracing +#ifdef TRACECALLS +#if JLDEF_BUILD <= JLL_TRACE +#include +#include "journaller.h" + +/*! \brief The CallTracer class is used to log function entry and exit. + \details Usually the class is instantiated through one of the TRACETHIS, TRACEGLOBAL or + TRACETHIS2 macros, but it can be created manually if desired. The class writes a log-line + during creation and another one when it gets destroyed, if a minimum time has passed since + construction. +*/ +class CallTracer +{ +public: + Journaller* m_journal; + std::string m_msg; + + /*! \brief Constructor, writes an entry logline and starts timing the function + \param functionName The name of the function that contains the CallTracer object (or some other identifying string) + \param object A pointer to an object, usually the this pointer of the containing function. Used to separate nested and concurrent calls to the same function. + \param minTime The minimum time in ms to pass for an exit logline to be created. Set to 0 to always write an exit line. + */ + CallTracer(Journaller* journal, const std::string& functionName, const void* object) + : m_journal(journal) + , m_msg(functionName) + { + if (object) + { + char ptr[32]; + sprintf(ptr, " [%p]", object); + m_msg += ptr; + } + JLTRACE_NODEC(m_journal, m_msg << " entry"); + } + + /*! \brief Destructor, writes an exit logline if at least m_minimumTime has passed + */ + ~CallTracer() + { + JLTRACE_NODEC(m_journal, m_msg << " exit"); + } +}; +#define TRACEFUNC(journal) CallTracer trAceR(journal, std::string(__FILE__) + ":" + __FUNCTION__, NULL) +#define TRACETHIS(journal) CallTracer trAceR(journal, std::string(__FILE__) + ":" + __FUNCTION__, this) +#else +#define TRACEFUNC(j) ((void) 0) +#define TRACETHIS(j) ((void) 0) +#endif +#else +#define TRACEFUNC(j) ((void) 0) +#define TRACETHIS(j) ((void) 0) +#endif diff --git a/extern/xspublic/xscommon/common_qdebug.h b/extern/xspublic/xscommon/common_qdebug.h new file mode 100644 index 0000000..22458dc --- /dev/null +++ b/extern/xspublic/xscommon/common_qdebug.h @@ -0,0 +1,62 @@ + +// Copyright (c) 2003-2022 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 COMMON_QDEBUG_H +#define COMMON_QDEBUG_H + +#ifdef __cplusplus + #include + + #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 + #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 + + #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 diff --git a/extern/xspublic/xscommon/consolelogger.cpp b/extern/xspublic/xscommon/consolelogger.cpp new file mode 100644 index 0000000..e6353fa --- /dev/null +++ b/extern/xspublic/xscommon/consolelogger.cpp @@ -0,0 +1,74 @@ + +// Copyright (c) 2003-2022 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 "consolelogger.h" +#include + +/*! \class ConsoleLogger + \brief A class that is used to log messages to the console +*/ + +/*! \brief Constructs a ConsoleLogger + \param[in] logLevel The initial log level to use as log level for console logging (std out) + \param[in] errLevel The initial log level to use as log level for error logging (std err) +*/ +ConsoleLogger::ConsoleLogger(JournalLogLevel logLevel, JournalLogLevel errLevel) + : AdditionalLoggerBase(logLevel) +{ + setDebugLevel(errLevel); +} + +/*! \brief Write a log line to the console + \param[in] level The log level + \param[in] file The name of the file from which the logging originates (not used) + \param[in] line The line number from which the logging originates (not used) + \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)file; + (void)line; + // get just the function name + size_t p = strlen(function); + while (p > 0) + { + if (function[p - 1] == ':') + break; + --p; + } + + if (level >= logLevel()) + std::cout << function + p << ": " << msg << std::endl; + if (level >= debugLevel()) + std::cerr << function + p << ": " << msg << std::endl; +} diff --git a/extern/xspublic/xscommon/consolelogger.h b/extern/xspublic/xscommon/consolelogger.h new file mode 100644 index 0000000..4531f66 --- /dev/null +++ b/extern/xspublic/xscommon/consolelogger.h @@ -0,0 +1,46 @@ + +// Copyright (c) 2003-2022 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 CONSOLELOGGER_H +#define CONSOLELOGGER_H + +#include "additionalloggerbase.h" + +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; +}; + +#endif diff --git a/extern/xspublic/xscommon/enumexpandersbase.cpp b/extern/xspublic/xscommon/enumexpandersbase.cpp new file mode 100644 index 0000000..94e1527 --- /dev/null +++ b/extern/xspublic/xscommon/enumexpandersbase.cpp @@ -0,0 +1,538 @@ + +// Copyright (c) 2003-2022 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 "enumexpandersbase.h" + +extern Journaller* gJournal; //lint !e752 + +JLENUMEXPANDER(XsResultValue, + JLENUMCASE(XRV_OK) //!< 0: Operation was performed successfully + + // communication protocol + JLENUMCASE(XRV_NOBUS) //!< 1: No bus communication possible + JLENUMCASE(XRV_BUSNOTREADY) //!< 2: InitBus and/or SetBID are not issued + JLENUMCASE(XRV_INVALIDPERIOD) ///!< 3: Period sent is invalid + JLENUMCASE(XRV_INVALIDMSG) //!< 4: The message is invalid or not implemented + JLENUMCASE(XRV_INITBUSFAIL1) //!< 16: A slave did not respond to WaitForSetBID + JLENUMCASE(XRV_INITBUSFAIL2) //!< 17: An incorrect answer received after WaitForSetBID + JLENUMCASE(XRV_INITBUSFAIL3) //!< 18: After four bus-scans still undetected Motion Trackers + JLENUMCASE(XRV_SETBIDFAIL1) //!< 20: No reply to SetBID message during SetBID procedure + JLENUMCASE(XRV_SETBIDFAIL2) //!< 21: Other than SetBIDAck received + JLENUMCASE(XRV_MEASUREMENTFAIL1) //!< 24: Timer overflow - period too short to collect all data from Motion Trackers + JLENUMCASE(XRV_MEASUREMENTFAIL2) //!< 25: Motion Tracker responds with other than SlaveData message + JLENUMCASE(XRV_MEASUREMENTFAIL3) //!< 26: Total bytes of data of Motion Trackers including sample counter exceeds 255 bytes + JLENUMCASE(XRV_MEASUREMENTFAIL4) //!< 27: Timer overflows during measurement + JLENUMCASE(XRV_MEASUREMENTFAIL5) //!< 28: Timer overflows during measurement + JLENUMCASE(XRV_MEASUREMENTFAIL6) //!< 29: No correct response from Motion Tracker during measurement + JLENUMCASE(XRV_TIMEROVERFLOW) //!< 30: Timer overflows during measurement + JLENUMCASE(XRV_BAUDRATEINVALID) //!< 32: Baud rate does not comply with valid range + JLENUMCASE(XRV_INVALIDPARAM) //!< 33: An invalid parameter is supplied + JLENUMCASE(XRV_MEASUREMENTFAIL7) //!< 35: TX PC Buffer is full + JLENUMCASE(XRV_MEASUREMENTFAIL8) //!< 36: TX PC Buffer overflow, cannot fit full message + JLENUMCASE(XRV_WIRELESSFAIL) //!< 37: Wireless subsystem failed + JLENUMCASE(XRV_DEVICEERROR) //!< 40: The device generated an error, try updating the firmware + JLENUMCASE(XRV_DATAOVERFLOW) //!< 41: The device generates more data than the bus communication can handle (baud rate may be too low) + JLENUMCASE(XRV_BUFFEROVERFLOW) //!< 42: The sample buffer of the device was full during a communication outage + JLENUMCASE(XRV_EXTTRIGGERERROR) //!< 43: The external trigger is not behaving as configured + JLENUMCASE(XRV_SAMPLESTREAMERROR) //!< 44: The sample stream detected an error in the ordering of sample data + JLENUMCASE(XRV_POWER_DIP) //!< 45: A dip in the power supply was detected and recovered from + JLENUMCASE(XRV_POWER_OVERCURRENT) //!< 46: A current limiter has been activated, shutting down the device + JLENUMCASE(XRV_OVERHEATING) //!< 47: Device temperature is not within operational limits + JLENUMCASE(XRV_BATTERYLOW) //!< 48: Battery level reached lower limit + JLENUMCASE(XRV_INVALIDFILTERPROFILE) //!< 49: Specified filter profile ID is not available on the device or the user is trying to duplicate an existing filter profile type + JLENUMCASE(XRV_INVALIDSTOREDSETTINGS) //!< 50: The settings stored in the device's non volatile memory are invalid + JLENUMCASE(XRV_ACCESSDENIED) //!< 51: Request for control of the device was denied + JLENUMCASE(XRV_FILEERROR) //!< 52: Failure reading, writing, opening or closing a file + JLENUMCASE(XRV_OUTPUTCONFIGERROR) //!< 53: Erroneous output configuration, device can not go to measurement + + // CMT / XDA / XME / etc + JLENUMCASE(XRV_ERROR) //!< 256: A generic error occurred + JLENUMCASE(XRV_NOTIMPLEMENTED) //!< 257: Operation not implemented in this version (yet) + JLENUMCASE(XRV_TIMEOUT) //!< 258: A timeout occurred + JLENUMCASE(XRV_TIMEOUTNODATA) //!< 259: Operation aborted because of no data read + JLENUMCASE(XRV_CHECKSUMFAULT) //!< 260: Checksum fault occurred + JLENUMCASE(XRV_OUTOFMEMORY) //!< 261: No internal memory available + JLENUMCASE(XRV_NOTFOUND) //!< 262: The requested item was not found + JLENUMCASE(XRV_UNEXPECTEDMSG) //!< 263: Unexpected message received (e.g. no acknowledge message received) + JLENUMCASE(XRV_INVALIDID) //!< 264: Invalid id supplied + JLENUMCASE(XRV_INVALIDOPERATION) //!< 265: Operation is invalid at this point + JLENUMCASE(XRV_INSUFFICIENTSPACE) //!< 266: Insufficient buffer space available + JLENUMCASE(XRV_INPUTCANNOTBEOPENED) //!< 267: The specified i/o device can not be opened + JLENUMCASE(XRV_OUTPUTCANNOTBEOPENED) //!< 268: The specified i/o device can not be opened + JLENUMCASE(XRV_ALREADYOPEN) //!< 269: An I/O device is already opened with this object + JLENUMCASE(XRV_ENDOFFILE) //!< 270: End of file is reached + JLENUMCASE(XRV_COULDNOTREADSETTINGS) //!< 271: A required settings file could not be opened or is missing some data + JLENUMCASE(XRV_NODATA) //!< 272: No data is available + JLENUMCASE(XRV_READONLY) //!< 273: Tried to change a read-only value + JLENUMCASE(XRV_NULLPTR) //!< 274: Tried to supply a NULL value where it is not allowed + JLENUMCASE(XRV_INSUFFICIENTDATA) //!< 275: Insufficient data was supplied to a function + JLENUMCASE(XRV_BUSY) //!< 276: Busy processing, try again later + JLENUMCASE(XRV_INVALIDINSTANCE) //!< 277: Invalid instance called, because of an invalid or missing license + JLENUMCASE(XRV_DATACORRUPT) //!< 278: A trusted data stream proves to contain corrupted data + + JLENUMCASE(XRV_READINITFAILED) //!< 279: Failure during read of settings + JLENUMCASE(XRV_NOXMFOUND) //!< 280: Could not find any MVN-compatible hardware + JLENUMCASE(XRV_DEVICECOUNTZERO) //!< 282: No xsens devices found + JLENUMCASE(XRV_MTLOCATIONINVALID) //!< 283: One or more sensors are not where they were expected + JLENUMCASE(XRV_INSUFFICIENTMTS) //!< 284: Not enough sensors were found + JLENUMCASE(XRV_INITFUSIONFAILED) //!< 285: Failure during initialization of Fusion Engine + JLENUMCASE(XRV_OTHER) //!< 286: Something else was received than was requested + + JLENUMCASE(XRV_NOFILEOPEN) //!< 287: No file opened for reading/writing + JLENUMCASE(XRV_NOPORTOPEN) //!< 288: No serial port opened for reading/writing + JLENUMCASE(XRV_NOFILEORPORTOPEN) //!< 289: No file or serial port opened for reading/writing + JLENUMCASE(XRV_PORTNOTFOUND) //!< 290: A required port could not be found + JLENUMCASE(XRV_INITPORTFAILED) //!< 291: The low-level port handler failed to initialize + JLENUMCASE(XRV_CALIBRATIONFAILED) //!< 292: A calibration routine failed + + JLENUMCASE(XRV_CONFIGCHECKFAIL) //!< 293: The in-config check of the device failed + JLENUMCASE(XRV_ALREADYDONE) //!< 294: The operation is once only and has already been performed + + JLENUMCASE(XRV_SYNC_SINGLE_SLAVE) //!< 295: The single connected device is configured as a slave + JLENUMCASE(XRV_SYNC_SECOND_MASTER) //!< 296: More than one master was detected + JLENUMCASE(XRV_SYNC_NO_SYNC) //!< 297: A device was detected that was neither master nor slave + JLENUMCASE(XRV_SYNC_NO_MASTER) //!< 298: No master detected + JLENUMCASE(XRV_SYNC_DATA_MISSING) //!< 299: A device is not sending enough data + + JLENUMCASE(XRV_VERSION_TOO_LOW) //!< 300: The version of the object is too low for the requested operation + JLENUMCASE(XRV_VERSION_PROBLEM) //!< 301: The object has an unrecognised version, so it's not safe to perform the operation + + JLENUMCASE(XRV_ABORTED) //!< 302: The process was aborted by an external event, usually a user action or process termination + JLENUMCASE(XRV_UNSUPPORTED) //!< 303: The requested functionality is not supported by the device + + JLENUMCASE(XRV_PACKETCOUNTERMISSED) //!< 304: A packet counter value was missed + + JLENUMCASE(XRV_MEASUREMENTFAILED) //!< 305: An error occurred while trying to put the device in measurement mode + JLENUMCASE(XRV_STARTRECORDINGFAILED) //!< 306: A device could not start recording + JLENUMCASE(XRV_STOPRECORDINGFAILED) //!< 307: A device could not stop recording + + JLENUMCASE(XRV_RADIO_CHANNEL_IN_USE) //!< 311: Radio channel is in use by another system + JLENUMCASE(XRV_UNEXPECTED_DISCONNECT) //!< 312: Motion tracker disconnected unexpectedly + JLENUMCASE(XRV_TOO_MANY_CONNECTED_TRACKERS) //!< 313: Too many motion trackers connected + JLENUMCASE(XRV_GOTOCONFIGFAILED) //!< 314: A device could not be put in config mode + JLENUMCASE(XRV_OUTOFRANGE) //!< 315: Device has gone out of range + JLENUMCASE(XRV_BACKINRANGE) //!< 316: Device is back in range, resuming normal operation + JLENUMCASE(XRV_EXPECTED_DISCONNECT) //!< 317: The device was disconnected + JLENUMCASE(XRV_RESTORE_COMMUNICATION_FAILED) //!< 318: Restore communication failed + JLENUMCASE(XRV_RESTORE_COMMUNICATION_STOPPED) //!< 319: Restore communication was stopped + JLENUMCASE(XRV_EXPECTED_CONNECT) //!< 320: The device was connected + JLENUMCASE(XRV_IN_USE) //!< 321: The requested device/port/address is already in use. Most likely returned by XsSocket::bind + + JLENUMCASE(XRV_PERFORMANCE_WARNING) //!< 322: The system running the application can't fully keep up with the incoming data. This may lead to degraded performance or lag. + JLENUMCASE(XRV_PERFORMANCE_OK) //!< 323: The system running the application has recovered from a previously reported XRV_PERFORMANCE_WARNING. + + // notifications + JLENUMCASE(XRV_SHUTTINGDOWN) //!< 400: The device is shutting down + 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_GNSSCONNECTIONLOST) //!< 405: Connection lost with the GNSS module + JLENUMCASE(XRV_GNSSLOWINPUTRATE) //!< 406: GNSS input rate is too low + JLENUMCASE(XRV_GNSSINCOMPLETEDATASET) //!< 407: Incomplete dataset for the GNSS module + + JLENUMCASE(XRV_DEVICE_NOT_CALIBRATED) //!< 404: The EMTS of the device does not contain calibration data + JLENUMCASE(XRV_FILE_SYSTEM_CORRUPT) //!< 54: The internal file system of the device has become corrupt +) + +JLENUMEXPANDERHEX(XsXbusMessageId, + //JLENUMCASE(XMID_InvalidMessage) // 0x00, + JLENUMCASE(XMID_ReqDid) // 0x00, + JLENUMCASE(XMID_DeviceId) // 0x01, + JLENUMCASE(XMID_Initbus) // 0x02, + JLENUMCASE(XMID_InitBusResults) // 0x03, + JLENUMCASE(XMID_ReqPeriod) // 0x04, + JLENUMCASE(XMID_ReqPeriodAck) // 0x05, + //JLENUMCASE(XMID_SetPeriod) // 0x04, + //JLENUMCASE(XMID_SetPeriodAck) // 0x05, + JLENUMCASE2(XMID_SetBid, "XMID_SetBid/XMID_AutoStart") // 0x06, + JLENUMCASE2(XMID_SetBidAck, "XMID_SetBidAck/XMID_AutoStartAck") // 0x07, + //JLENUMCASE(XMID_AutoStart) // 0x06, + //JLENUMCASE(XMID_AutoStartAck) // 0x07, + JLENUMCASE(XMID_BusPower) // 0x08, + JLENUMCASE(XMID_BusPowerAck) // 0x09, + JLENUMCASE(XMID_ReqDataLength) // 0x0A, + JLENUMCASE(XMID_DataLength) // 0x0B, + JLENUMCASE(XMID_ReqConfiguration) // 0x0C, + JLENUMCASE(XMID_Configuration) // 0x0D, + JLENUMCASE(XMID_RestoreFactoryDef) // 0x0E, + JLENUMCASE(XMID_RestoreFactoryDefAck) // 0x0F, + JLENUMCASE(XMID_GotoMeasurement) // 0x10, + JLENUMCASE(XMID_GotoMeasurementAck) // 0x11, + JLENUMCASE(XMID_ReqFirmwareRevision) // 0x12, + JLENUMCASE(XMID_FirmwareRevision) // 0x13, + JLENUMCASE(XMID_ReqUniqueId) // 0x14, + JLENUMCASE(XMID_UniqueId) // 0x15, + JLENUMCASE(XMID_ReqBodypackMode) // 0x16, + JLENUMCASE(XMID_ReqBodypackAck) // 0x17, + //JLENUMCASE(XMID_SetXmOutputMode) // 0x16, + //JLENUMCASE(XMID_SetXmOutputModeAck) // 0x17, + JLENUMCASE(XMID_ReqBaudrate) // 0x18, + JLENUMCASE(XMID_ReqBaudrateAck) // 0x19, + //JLENUMCASE(XMID_SetBaudrate) // 0x18, + //JLENUMCASE(XMID_SetBaudrateAck) // 0x19, + //JLENUMCASE(XMID_ReqSyncMode) // 0x1A, + //JLENUMCASE(XMID_ReqSyncModeAck) // 0x1B, + //JLENUMCASE(XMID_SetSyncMode) // 0x1A, + //JLENUMCASE(XMID_SetSyncModeAck) // 0x1B, + JLENUMCASE(XMID_ReqProductCode) // 0x1C, + JLENUMCASE(XMID_ProductCode) // 0x1D, + JLENUMCASE(XMID_ReqProcessingFlags) // 0x20, + JLENUMCASE(XMID_ReqProcessingFlagsAck) // 0x21, + //JLENUMCASE(XMID_SetProcessingFlags) // 0x20, + //JLENUMCASE(XMID_SetProcessingFlagsAck) // 0x21, + JLENUMCASE(XMID_SetNoRotation) // 0x22, + JLENUMCASE(XMID_SetNoRotationAck) // 0x23, + JLENUMCASE(XMID_RunSelfTest) // 0x24, + JLENUMCASE(XMID_SelfTestResults) // 0x25, + JLENUMCASE(XMID_GotoConfig) // 0x30, + JLENUMCASE(XMID_GotoConfigAck) // 0x31, + JLENUMCASE2(XMID_BusData, "XMID_BusData/XMID_MtData/XMID_PrepareData") // 0x32, + //JLENUMCASE(XMID_MtData) // 0x32, + JLENUMCASE(XMID_ReqInputTrigger) // 0x26, + JLENUMCASE(XMID_ReqInputTriggerAck) // 0x27, + //JLENUMCASE(XMID_SetInputTrigger) // 0x26, + //JLENUMCASE(XMID_SetInputTriggerAck) // 0x27, + JLENUMCASE(XMID_ReqOutputTrigger) // 0x28, + JLENUMCASE(XMID_ReqOutputTriggerAck) // 0x29, + //JLENUMCASE(XMID_SetOutputTrigger) // 0x28, + //JLENUMCASE(XMID_SetSyncStationMode) // 0x2A, + //JLENUMCASE(XMID_SetSyncStationModeAck) // 0x2B, + JLENUMCASE(XMID_ReqSyncStationMode) // 0x2A, + JLENUMCASE(XMID_ReqSyncStationModeAck) // 0x2B, + //JLENUMCASE(XMID_SetSyncBoxMode) // 0x2A, + //JLENUMCASE(XMID_SetSyncBoxModeAck) // 0x2B, + //JLENUMCASE(XMID_ReqSyncBoxMode) // 0x2A, + //JLENUMCASE(XMID_ReqSyncBoxModeAck) // 0x2B, + //JLENUMCASE(XMID_SetSyncConfiguration) // 0x2C, + JLENUMCASE(XMID_SetSyncConfigurationAck) // 0x2D, + JLENUMCASE(XMID_ReqSyncConfiguration) // 0x2C, + //JLENUMCASE(XMID_SyncConfiguration) // 0x2D, + JLENUMCASE(XMID_DriverDisconnect) // 0x2E, + JLENUMCASE(XMID_DriverDisconnectAck) // 0x2F, + //JLENUMCASE(XMID_PrepareData) // 0x32, + JLENUMCASE(XMID_ReqData) // 0x34, + JLENUMCASE(XMID_ReqDataAck) // 0x35, + JLENUMCASE(XMID_MtData2) // 0x36, + JLENUMCASE(XMID_MtData2Ack) // 0x37, + JLENUMCASE(XMID_RequestControl) // 0x38, + JLENUMCASE(XMID_RequestControlAck) // 0x39, + JLENUMCASE(XMID_SetDataPort) // 0x3A, + JLENUMCASE(XMID_SetDataPortAck) // 0x3B, + JLENUMCASE(XMID_ReqRetransmission) // 0x3C, + JLENUMCASE(XMID_ReqRetransmissionAck) // 0x3D, + JLENUMCASE(XMID_Wakeup) // 0x3E, + JLENUMCASE(XMID_WakeupAck) // 0x3F, + JLENUMCASE(XMID_Reset) // 0x40, + JLENUMCASE(XMID_ResetAck) // 0x41, + JLENUMCASE(XMID_Error) // 0x42, + JLENUMCASE(XMID_XmPowerOff) // 0x44, + JLENUMCASE(XMID_MasterIndication) // 0x46, + JLENUMCASE(XMID_ReqOptionFlags) // 0x48, + JLENUMCASE(XMID_ReqOptionFlagsAck) // 0x49, + //JLENUMCASE(XMID_SetOptionFlags) // 0x48, + //JLENUMCASE(XMID_SetOptionFlagsAck) // 0x49, + JLENUMCASE(XMID_ReqStealthMode) // 0x4A, + JLENUMCASE(XMID_StealthMode) // 0x4B, + //JLENUMCASE(XMID_SetStealthMode) // 0x4A, + //JLENUMCASE(XMID_SetStealthModeAck) // 0x4B, + JLENUMCASE(XMID_UserInterface) // 0x4C, + JLENUMCASE(XMID_UserInterfaceAck) // 0x4D, + JLENUMCASE(XMID_EndOfRecording) // 0x4E, + JLENUMCASE(XMID_EndOfRecordingAck) // 0x4F, + JLENUMCASE(XMID_GotoTransparentMode) // 0x50, + JLENUMCASE(XMID_GotoTransparentModeAck) // 0x51, + JLENUMCASE(XMID_RunFactoryTest) // 0x56, + JLENUMCASE(XMID_FactoryTestResults) // 0x57, + JLENUMCASE(XMID_FactoryTestConnect) // 0x58, + JLENUMCASE(XMID_FactoryTestConnectAck) // 0x59, + JLENUMCASE(XMID_SetDataOutputDelay) // 0x5A, + JLENUMCASE(XMID_SetDataOutputDelayAck) // 0x5B, + JLENUMCASE(XMID_SetBodypackConfigFile) // 0x5C, + JLENUMCASE(XMID_SetBodypackConfigFileAck) // 0x5D, + JLENUMCASE(XMID_ReqObrStatus) // 0x5E, + JLENUMCASE(XMID_ObrStatus) // 0x5F, + + //JLENUMCASE(XMID_SetUtcTime) // 0x60, + JLENUMCASE2(XMID_ReqUtcTime, "XMID_ReqUtcTime/XMID_FactoryTestSensorTiming") // 0x60, + JLENUMCASE2(XMID_SetUtcTimeAck, "XMID_ReqUtcTimeAck/XMID_FactoryTestSensorTimingResults") // 0x61, + //JLENUMCASE(XMID_UtcTime) // 0x61, + //JLENUMCASE(XMID_FactoryTestSensorTiming) // 0x60, + //JLENUMCASE(XMID_FactoryTestSensorTimingResults) // 0x61, + JLENUMCASE(XMID_ReqAvailableFilterProfiles) // 0x62, + JLENUMCASE(XMID_AvailableFilterProfiles) // 0x63, + JLENUMCASE(XMID_ReqFilterProfile) // 0x64, + JLENUMCASE(XMID_ReqFilterProfileAck) // 0x65, + //JLENUMCASE(XMID_SetFilterProfile) // 0x64, + //JLENUMCASE(XMID_SetFilterProfileAck) // 0x65, + JLENUMCASE(XMID_ReqGravityMagnitude) // 0x66, + JLENUMCASE(XMID_ReqGravityMagnitudeAck) // 0x67, + //JLENUMCASE(XMID_SetGravityMagnitude) // 0x66, + //JLENUMCASE(XMID_SetGravityMagnitudeAck) // 0x67, + JLENUMCASE(XMID_ReqGnssLeverArm) // 0x68, + JLENUMCASE(XMID_ReqGnssLeverArmAck) // 0x69, + //JLENUMCASE(XMID_SetGnssLeverArm) // 0x68, + //JLENUMCASE(XMID_SetGnssLeverArmAck) // 0x69, + JLENUMCASE(XMID_ReqReplayMode) // 0x6C, + JLENUMCASE(XMID_ReqReplayModeAck) // 0x6D, + //JLENUMCASE(XMID_SetReplayMode) // 0x6C, + //JLENUMCASE(XMID_SetReplayModeAck) // 0x6D, + JLENUMCASE(XMID_ReqLatLonAlt) // 0x6E, + JLENUMCASE(XMID_ReqLatLonAltAck) // 0x6F, + //JLENUMCASE(XMID_SetLatLonAlt) // 0x6E, + //JLENUMCASE(XMID_SetLatLonAltAck) // 0x6F, + JLENUMCASE2(XMID_ReqXmErrorMode, "XMID_ReqXmErrorMode/XMID_ReqHeading") // 0x82, + JLENUMCASE2(XMID_ReqXmErrorModeAck, "XMID_ReqXmErrorModeAck/XMID_ReqHeadingAck") // 0x83, + //JLENUMCASE(XMID_SetXmErrorMode) // 0x82, + //JLENUMCASE(XMID_SetXmErrorModeAck) // 0x83, + JLENUMCASE2(XMID_ReqBufferSize, "XMID_ReqBufferSize/XMID_ReqLocationId") // 0x84, + JLENUMCASE2(XMID_ReqBufferSizeAck, "XMID_ReqBufferSizeAck/XMID_ReqLocationIdAck") // 0x85, + //JLENUMCASE(XMID_SetBufferSize) // 0x84, + //JLENUMCASE(XMID_SetBufferSizeAck) // 0x85, + //JLENUMCASE(XMID_ReqHeading) // 0x82, + //JLENUMCASE(XMID_ReqHeadingAck) // 0x83, + //JLENUMCASE(XMID_SetHeading) // 0x82, + //JLENUMCASE(XMID_SetHeadingAck) // 0x83, + JLENUMCASE(XMID_ReqMagneticField) // 0x6A, + JLENUMCASE(XMID_ReqMagneticFieldAck) // 0x6B, + //JLENUMCASE(XMID_SetMagneticField) // 0x6A, + //JLENUMCASE(XMID_SetMagneticFieldAck) // 0x6B, + JLENUMCASE(XMID_KeepAlive) // 0x70, + JLENUMCASE(XMID_KeepAliveAck) // 0x71, + JLENUMCASE(XMID_ReqConnectionSettings) // 0x78, + JLENUMCASE(XMID_ReqConnectionSettingsAck) // 0x79, + //JLENUMCASE(XMID_SetConnectionSettings) // 0x78, + //JLENUMCASE(XMID_SetConnectionSettingsAck) // 0x79, + JLENUMCASE(XMID_CloseConnection) // 0x72, + JLENUMCASE(XMID_CloseConnectionAck) // 0x73, + JLENUMCASE(XMID_IccCommand) // 0x74, + JLENUMCASE(XMID_IccCommandAck) // 0x75, + JLENUMCASE(XMID_ReqGnssPlatform) // 0x76, + JLENUMCASE(XMID_ReqGnssPlatformAck) // 0x77, + //JLENUMCASE(XMID_SetGnssPlatform) // 0x76, + //JLENUMCASE(XMID_SetGnssPlatformAck) // 0x77, + JLENUMCASE(XMID_BodyPackBundle) // 0x7A, + JLENUMCASE(XMID_BodyPackBundleAck) // 0x7B, + JLENUMCASE(XMID_ReqStationOptions) // 0x7C, + JLENUMCASE(XMID_ReqStationOptionsAck) // 0x7D, + //JLENUMCASE(XMID_ReqLocationId) // 0x84, + //JLENUMCASE(XMID_ReqLocationIdAck) // 0x85, + //JLENUMCASE(XMID_SetLocationId) // 0x84, + //JLENUMCASE(XMID_SetLocationIdAck) // 0x85, + JLENUMCASE(XMID_ReqExtOutputMode) // 0x86, + JLENUMCASE(XMID_ReqExtOutputModeAck) // 0x87, + //JLENUMCASE(XMID_SetExtOutputMode) // 0x86, + //JLENUMCASE(XMID_SetExtOutputModeAck) // 0x87, + JLENUMCASE(XMID_ReqStringOutputType) // 0x8E, + JLENUMCASE(XMID_ReqStringOutputTypeAck) // 0x8F, + //JLENUMCASE(XMID_SetStringOutputType) // 0x8E, + //JLENUMCASE(XMID_SetStringOutputTypeAck) // 0x8F, + JLENUMCASE2(XMID_ReqBatteryLevel, "XMID_ReqBatteryLevel/XMID_ReqInitTrackMode") // 0x88, + JLENUMCASE2(XMID_Batterylevel, "XMID_Batterylevel/XMID_ReqInitTrackModeAck") // 0x89, + //JLENUMCASE(XMID_ReqInitTrackMode) // 0x88, + //JLENUMCASE(XMID_ReqInitTrackModeAck) // 0x89, + //JLENUMCASE(XMID_SetInitTrackMode) // 0x88, + //JLENUMCASE(XMID_SetInitTrackModeAck) // 0x89, + JLENUMCASE2(XMID_ReqMasterSettings, "XMID_ReqMasterSettings/XMID_StoreFilterState") // 0x8A, + JLENUMCASE2(XMID_MasterSettings, "XMID_MasterSettings/XMID_StoreFilterStateAck") // 0x8B, + //JLENUMCASE(XMID_StoreFilterState) // 0x8A, + //JLENUMCASE(XMID_StoreFilterStateAck) // 0x8B, + JLENUMCASE(XMID_ReqPortConfig) // 0x8C, + JLENUMCASE(XMID_PortConfig) // 0x8D, + JLENUMCASE(XMID_ReqEmts) // 0x90, + JLENUMCASE(XMID_EmtsData) // 0x91, + JLENUMCASE(XMID_UpdateFilterProfile) // 0x92, + JLENUMCASE(XMID_UpdateFilterProfileAck) // 0x93, + JLENUMCASE(XMID_RestoreEmts) // 0x94, + JLENUMCASE(XMID_RestoreEmtsAck) // 0x95, + JLENUMCASE(XMID_StoreEmts) // 0x96, + JLENUMCASE(XMID_StoreEmtsAck) // 0x97, + JLENUMCASE(XMID_ClockSyncCommand) // 0x9A, + JLENUMCASE(XMID_ClockSyncCommandAck) // 0x9B, + JLENUMCASE(XMID_ReqActiveClockCorrection) // 0x9C, + JLENUMCASE(XMID_ActiveClockCorrection) // 0x9D, + JLENUMCASE(XMID_StoreActiveClockCorrection) // 0x9E, + JLENUMCASE(XMID_StoreActiveClockCorrectionAck) // 0x9F, + JLENUMCASE(XMID_ReqFilterSettings) // 0xA0, + JLENUMCASE(XMID_ReqFilterSettingsAck) // 0xA1, + //JLENUMCASE(XMID_SetFilterSettings) // 0xA0, + //JLENUMCASE(XMID_SetFilterSettingsAck) // 0xA1, + JLENUMCASE(XMID_ReqAmd) // 0xA2, + JLENUMCASE(XMID_ReqAmdAck) // 0xA3, + //JLENUMCASE(XMID_SetAmd) // 0xA2, + //JLENUMCASE(XMID_SetAmdAck) // 0xA3, + JLENUMCASE(XMID_ResetOrientation) // 0xA4, + JLENUMCASE(XMID_ResetOrientationAck) // 0xA5, + JLENUMCASE(XMID_ReqGnssStatus) // 0xA6, + JLENUMCASE(XMID_GnssStatus) // 0xA7, + JLENUMCASE(XMID_AdjustUtcTime) // 0xA8, + JLENUMCASE(XMID_AdjustUtcTimeAck) // 0xA9, + JLENUMCASE(XMID_ReqAssemblyId) // 0xAA, + JLENUMCASE(XMID_AssemblyId) // 0xAB, + JLENUMCASE(XMID_ReqAccessControlList) // 0xAE, + JLENUMCASE(XMID_AccessControlList) // 0xAF, + //JLENUMCASE(XMID_SetAccessControlList) // 0xAE, + //JLENUMCASE(XMID_SetAccessControlListAck) // 0xAF, + JLENUMCASE(XMID_ScanChannels) // 0xB0, + JLENUMCASE(XMID_ScanChannelsAck) // 0xB1, + JLENUMCASE(XMID_EnableMaster) // 0xB2, + JLENUMCASE(XMID_EnableMasterAck) // 0xB3, + JLENUMCASE(XMID_DisableMaster) // 0xB4, + JLENUMCASE(XMID_DisableMasterAck) // 0xB5, + JLENUMCASE(XMID_ReqRadioChannel) // 0xB6, + JLENUMCASE(XMID_ReqRadioChannelAck) // 0xB7, + //JLENUMCASE(XMID_SetClientPriority) // 0xB8, + //JLENUMCASE(XMID_SetClientPriorityAck) // 0xB9, + JLENUMCASE(XMID_ReqClientPriority) // 0xB8, + JLENUMCASE(XMID_ReqClientPriorityAck) // 0xB9, + //JLENUMCASE(XMID_SetWirelessConfig) // 0xBA, + //JLENUMCASE(XMID_SetWirelessConfigAck) // 0xBB, + JLENUMCASE(XMID_ReqWirelessConfig) // 0xBA, + JLENUMCASE(XMID_ReqWirelessConfigAck) // 0xBB, + JLENUMCASE(XMID_UpdateBias) // 0xBC, + JLENUMCASE(XMID_UpdateBiasAck) // 0xBD, + JLENUMCASE(XMID_ToggleIoPins) // 0xBE, + JLENUMCASE(XMID_ToggleIoPinsAck) // 0xBF, + JLENUMCASE2(XMID_GotoOperational, "XMID_GotoOperational/XMID_ReqOutputConfiguration") // 0xC0, + JLENUMCASE2(XMID_GotoOperationalAck, "XMID_GotoOperationalAck/XMID_ReqOutputConfigurationAck") // 0xC1, + //JLENUMCASE(XMID_SetTransportMode) // 0xC2, + //JLENUMCASE(XMID_SetTransportModeAck) // 0xC3, + JLENUMCASE(XMID_ReqTransportMode) // 0xC2, + JLENUMCASE(XMID_ReqTransportModeAck) // 0xC3, + JLENUMCASE(XMID_AcceptMtw) // 0xC4, + JLENUMCASE(XMID_AcceptMtwAck) // 0xC5, + JLENUMCASE(XMID_RejectMtw) // 0xC6, + JLENUMCASE(XMID_RejectMtwAck) // 0xC7, + JLENUMCASE(XMID_InfoRequest) // 0xC8, + JLENUMCASE(XMID_InfoRequestAck) // 0xC9, + JLENUMCASE(XMID_ReqFrameRates) // 0xCA, + JLENUMCASE(XMID_ReqFrameRatesAck) // 0xCB, + JLENUMCASE(XMID_StartRecording) // 0xCC, + JLENUMCASE(XMID_StartRecordingAck) // 0xCD, + JLENUMCASE(XMID_StopRecording) // 0xCE, + JLENUMCASE(XMID_StopRecordingAck) // 0xCF, + //JLENUMCASE(XMID_ReqOutputConfiguration) // 0xC0, + //JLENUMCASE(XMID_ReqOutputConfigurationAck) // 0xC1, + //JLENUMCASE(XMID_SetOutputConfiguration) // 0xC0, + //JLENUMCASE(XMID_SetOutputConfigurationAck) // 0xC1, + JLENUMCASE(XMID_ReqOutputMode) // 0xD0, + JLENUMCASE(XMID_ReqOutputModeAck) // 0xD1, + //JLENUMCASE(XMID_SetOutputMode) // 0xD0, + //JLENUMCASE(XMID_SetOutputModeAck) // 0xD1, + JLENUMCASE(XMID_ReqOutputSettings) // 0xD2, + JLENUMCASE(XMID_ReqOutputSettingsAck) // 0xD3, + //JLENUMCASE(XMID_SetOutputSettings) // 0xD2, + //JLENUMCASE(XMID_SetOutputSettingsAck) // 0xD3, + JLENUMCASE(XMID_ReqOutputSkipFactor) // 0xD4, + JLENUMCASE(XMID_ReqOutputSkipFactorAck) // 0xD5, + //JLENUMCASE(XMID_SetOutputSkipFactor) // 0xD4, + //JLENUMCASE(XMID_SetOutputSkipFactorAck) // 0xD5, + JLENUMCASE(XMID_SetInitialHeading) // 0xD6, + JLENUMCASE(XMID_SetInitialHeadingAck) // 0xD7, + //JLENUMCASE(XMID_SetSyncInSettings) // 0xD6, + //JLENUMCASE(XMID_SetSyncInSettingsAck) // 0xD7, + //JLENUMCASE(XMID_ReqSyncOutSettings) // 0xD8, + //JLENUMCASE(XMID_ReqSyncOutSettingsAck) // 0xD9, + //JLENUMCASE(XMID_SetSyncOutSettings) // 0xD8, + //JLENUMCASE(XMID_SetSyncOutSettingsAck) // 0xD9, + JLENUMCASE(XMID_ReqErrorMode) // 0xDA, + JLENUMCASE(XMID_ReqErrorModeAck) // 0xDB, + //JLENUMCASE(XMID_SetErrorMode) // 0xDA, + //JLENUMCASE(XMID_SetErrorModeAck) // 0xDB, + JLENUMCASE(XMID_ReqTransmitDelay) // 0xDC, + JLENUMCASE(XMID_ReqTransmitDelayAck) // 0xDD, + //JLENUMCASE(XMID_SetTransmitDelay) // 0xDC, + //JLENUMCASE(XMID_SetTransmitDelayAck) // 0xDD, + JLENUMCASE(XMID_SetMfmResults) // 0xDE, + JLENUMCASE(XMID_SetMfmResultsAck) // 0xDF, + JLENUMCASE(XMID_ReqObjectAlignment) // 0xE0, + JLENUMCASE(XMID_ReqObjectAlignmentAck) // 0xE1, + //JLENUMCASE(XMID_SetObjectAlignment) // 0xE0, + //JLENUMCASE(XMID_SetObjectAlignmentAck) // 0xE1, + JLENUMCASE(XMID_ReqCanConfig) // 0xE6 + JLENUMCASE(XMID_CanConfig) // 0xE7 + //JLENUMCASE(XMID_SetCanConfig) // 0xE6 + //JLENUMCASE(XMID_SetCanConfigAck) // 0xE7 + JLENUMCASE(XMID_ReqCanOutputConfig) // 0xE8 + JLENUMCASE(XMID_CanOutputConfig) // 0xE9 + JLENUMCASE(XMID_ReqAlignmentRotation) // 0xEC, + JLENUMCASE(XMID_ReqAlignmentRotationAck) // 0xED, + //JLENUMCASE(XMID_SetAlignmentRotation) // 0xEC, + //JLENUMCASE(XMID_SetAlignmentRotationAck) // 0xED, + JLENUMCASE(XMID_ExtensionReserved1) // 0xEE, + JLENUMCASE(XMID_ExtensionReserved2) // 0xEF, + JLENUMCASE(XMID_SetDeviceIdContext) // 0xFE, + JLENUMCASE(XMID_SetDeviceIdContextAck) // 0xFF + + JLENUMCASE(XMID_ReqHardwareVersion) + JLENUMCASE(XMID_HardwareVersion) + JLENUMCASE(XMID_Warning) + JLENUMCASE(XMID_ReqDeviceCapabilities) + JLENUMCASE(XMID_DeviceCapabilities) + JLENUMCASE(XMID_DiscardRetransmissions) + JLENUMCASE(XMID_DiscardRetransmissionsAck) + JLENUMCASE(XMID_ReqErrorReport) + JLENUMCASE(XMID_ErrorReport) + JLENUMCASE(XMID_ReqGnssReceiverSettings) + JLENUMCASE(XMID_ReqGnssReceiverSettingsAck) + JLENUMCASE(XMID_ForwardGnssData) + 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/extern/xspublic/xscommon/enumexpandersbase.h b/extern/xspublic/xscommon/enumexpandersbase.h new file mode 100644 index 0000000..29c8543 --- /dev/null +++ b/extern/xspublic/xscommon/enumexpandersbase.h @@ -0,0 +1,45 @@ + +// Copyright (c) 2003-2022 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 ENUMEXPANDERS_BASE +#define ENUMEXPANDERS_BASE + +#include "journaller.h" +#include +#include +#include + +JLENUMEXPPROTO(XsResultValue); +JLENUMEXPPROTO(XsXbusMessageId); +JLENUMEXPPROTO(XsOption); + +#endif diff --git a/extern/xspublic/xscommon/extendedenum.h b/extern/xspublic/xscommon/extendedenum.h new file mode 100644 index 0000000..8cddeab --- /dev/null +++ b/extern/xspublic/xscommon/extendedenum.h @@ -0,0 +1,143 @@ + +// Copyright (c) 2003-2022 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/extern/xspublic/xscommon/fwupdate.c b/extern/xspublic/xscommon/fwupdate.c new file mode 100644 index 0000000..bcee322 --- /dev/null +++ b/extern/xspublic/xscommon/fwupdate.c @@ -0,0 +1,499 @@ + +// Copyright (c) 2003-2022 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/extern/xspublic/xscommon/fwupdate.h b/extern/xspublic/xscommon/fwupdate.h new file mode 100644 index 0000000..1ab8ecd --- /dev/null +++ b/extern/xspublic/xscommon/fwupdate.h @@ -0,0 +1,140 @@ + +// Copyright (c) 2003-2022 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/extern/xspublic/xscommon/journalexception.cpp b/extern/xspublic/xscommon/journalexception.cpp new file mode 100644 index 0000000..a954943 --- /dev/null +++ b/extern/xspublic/xscommon/journalexception.cpp @@ -0,0 +1,108 @@ + +// Copyright (c) 2003-2022 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 "journalstackwalker.h" +#include "journalexception.h" + +/*! \brief A helper class that combines the StackWalker output into a std::string */ +class JournalExceptionStackWalker : public StackWalker +{ +public: + JournalExceptionStackWalker(JournalException* exc) + : StackWalker() + , m_exception(exc) + {} + + JournalException* m_exception; + + virtual void OnOutput(LPCSTR szText) + { + m_exception->m_stack += szText; + m_exception->m_stack += "\n"; + } +}; + +/*! \class JournalException + \brief An exception class that automatically includes stack dump information +*/ + +/*! \brief Constructor, copies the \a message and creates a stack dump */ +JournalException::JournalException(std::string const& message) + : XsException(XRV_ERROR, message) + , m_stack("************ Dump Begin ************\n") +{ + JournalExceptionStackWalker sw(this); + sw.ShowCallstack(); + m_stack += "************* Dump End *************"; +} + +/*! \brief The message as supplied to the constructor */ +const char* JournalException::msg() const +{ + return what(); +} + +/*! \brief The stack dump as it was at the time of object construction */ +const std::string& JournalException::stack() const +{ + return m_stack; +} + +#ifdef XSENS_DEBUG + #define GOTODEBDEF true +#else + #define GOTODEBDEF false +#endif + +/*! When set to true, any caught exception at the C level will be passed on upward (ie to the debugger) + When set to false, the exception handling stops at this level. This only applies to C level exceptions. +*/ +bool gOnExceptionGotoDebugger = GOTODEBDEF; + +#ifdef XSENS_WINDOWS +/*! \brief Exception filter for crash handler. + + Using this filter in __try __except() will generate a crash log with a call stack. + \param pExPtrs pointers to exception struct, use GetExceptionInformation() + \param journal The Journaller object to use for logging +*/ +LONG journallerExceptionFilter(EXCEPTION_POINTERS* pExPtrs, Journaller* journal) +{ + journal->log(JLL_Fatal, "******* C level exception (CRASH) *******"); + journal->log(JLL_Fatal, "************ Dump Begin ************"); + JournalStackWalker sw(journal); + sw.ShowCallstack(GetCurrentThread(), pExPtrs->ContextRecord); + journal->log(JLL_Fatal, "************* Dump End *************"); + + return gOnExceptionGotoDebugger ? EXCEPTION_CONTINUE_SEARCH : EXCEPTION_EXECUTE_HANDLER; +} +#endif diff --git a/extern/xspublic/xscommon/journalexception.h b/extern/xspublic/xscommon/journalexception.h new file mode 100644 index 0000000..59c7b6b --- /dev/null +++ b/extern/xspublic/xscommon/journalexception.h @@ -0,0 +1,131 @@ + +// Copyright (c) 2003-2022 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 JOURNALEXCEPTION_H +#define JOURNALEXCEPTION_H + +#include +#include + +#include "journaller.h" + +#ifndef XSENS_WINDOWS + #include + #include +#endif + +class JournalException : public XsException +{ +public: + JournalException(std::string const& message); + ~JournalException() throw() {} + + const char* msg() const; + const std::string& stack() const; + +protected: + std::string m_stack; //!< A string that contains a stack dump + friend class JournalExceptionStackWalker; +}; + +//! Use this macro to throw an exception that includes callstack information +#define JLTHROW(msg) do { std::ostringstream os; os << msg; throw JournalException(os.str()); } while(0) + +extern bool gOnExceptionGotoDebugger; + +#ifdef XSENS_WINDOWS + #include + LONG journallerExceptionFilter(EXCEPTION_POINTERS* pExPtrs, Journaller*); +#endif + +#if JLDEF_BUILD > JLL_FATAL +#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 +#endif + +#if JLDEF_BUILD > JLL_ERROR +#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; } +#endif + +#endif diff --git a/extern/xspublic/xscommon/journalfile.cpp b/extern/xspublic/xscommon/journalfile.cpp new file mode 100644 index 0000000..1f7f4f6 --- /dev/null +++ b/extern/xspublic/xscommon/journalfile.cpp @@ -0,0 +1,126 @@ + +// Copyright (c) 2003-2022 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 "journalfile.h" +#ifdef _MSC_VER + #include +#endif + +/*! \class JournalFile + \brief A class containing a journal file and some meta-data + \details These objects are managed by Journaller through gJournalFileMap. +*/ + +/*! \var volatile std::atomic_int JournalFile::m_refCount + \brief A reference counter that tracks how many Journaller objects use this file +*/ + +/*! \brief Constructor, requires a filename + \details + \param name The (path and) filename of the log file to be used + \param purge When set to true (default) the file will be cleared when opened +*/ +JournalFile::JournalFile(const XsString& name, bool purge) + : m_refCount(1) + , m_filename(name) +{ +#ifdef _MSC_VER + if (m_filename.find(':') < 0 && m_filename.find('/') < 0 && m_filename.find('\\') < 0) + { + char cwdBuf[1024]; + GetCurrentDirectoryA(1024, cwdBuf); + m_filename = XsString(cwdBuf) + XsString("\\") + m_filename; + } +#endif + + if (purge || (m_file.openText(m_filename, false) != XRV_OK)) + m_file.createText(m_filename, false); + if (m_file.isOpen()) + m_file.seek_r(0); +} + +/*! \brief Destructor, flushes remaining data and closes the file */ +JournalFile::~JournalFile() +{ + try + { + flush(); + m_file.close(); + } + catch (...) + { + } +} + +/*! \brief Flush remaining data to disk */ +void JournalFile::flush() +{ + m_file.flush(); +} + +/*! \brief Increase reference count of JournalFile by 1 + \return The new ref count value +*/ +int JournalFile::addRef() +{ + return ++m_refCount; +} + +/*! \brief Returns the current ref count value + \return The ref count value +*/ +int JournalFile::refCount() volatile const +{ + return m_refCount.load(); +} + +/*! \brief Decrease reference count of JournalFile by 1 + \return The new ref count value +*/ +int JournalFile::removeRef() +{ + return --m_refCount; +} + +/*! \brief Returns the (path +) filename of the open file */ +XsString JournalFile::filename() const +{ + return m_filename; +} + +/*! \brief Appends \a msg to the end of the current data stream */ +JournalFile& JournalFile::operator<<(std::string const& msg) +{ + if (m_file.isOpen()) + m_file.write(msg.c_str(), (XsFilePos) sizeof(char), (XsFilePos) msg.length()); + return *this; +} diff --git a/extern/xspublic/xscommon/journalfile.h b/extern/xspublic/xscommon/journalfile.h new file mode 100644 index 0000000..1e3904d --- /dev/null +++ b/extern/xspublic/xscommon/journalfile.h @@ -0,0 +1,67 @@ + +// Copyright (c) 2003-2022 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 JOURNALFILE_H +#define JOURNALFILE_H + +#include +#include +#include +#include + +class JournalFile +{ +public: + JournalFile(const XsString& name, bool purge); + ~JournalFile(); + + void flush(); + + int addRef(); + int refCount() volatile const; + int removeRef(); + + 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; + } + +private: + volatile std::atomic_int m_refCount; + XsString m_filename; + XsFile m_file; +}; + +#endif diff --git a/extern/xspublic/xscommon/journaller.cpp b/extern/xspublic/xscommon/journaller.cpp new file mode 100644 index 0000000..c45fb0c --- /dev/null +++ b/extern/xspublic/xscommon/journaller.cpp @@ -0,0 +1,484 @@ + +// Copyright (c) 2003-2022 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 "journalstackwalker.h" +#include "journaller.h" +#include "journalthreader.h" +#include "journalfile.h" + +#include +#include +#include +#include // for PRINTF_INT64_MODIFIER +#include // for xsGetCurrentThreadId +#include +#include +#include + +#ifndef XSENS_NO_AUTOLIB + #pragma comment(lib, "psapi.lib") +#endif + +//! \brief The optional additional logger +AbstractAdditionalLogger* Journaller::m_additionalLogger = nullptr; + +inline int threadId() +{ +#if JOURNALLER_WITH_THREAD_SUPPORT && !defined(__APPLE__) + return (int) xsGetCurrentThreadId(); +#else + return 0; +#endif +} + +/*! \class Journaller + \brief A journalling class for debugging applications + + \details Create a Journaller object in your application or dll main and share it among the source files + of your application (ie by adding "extern Journaller* journal;" in the xscommon_config.h) + Use the supplied logging macros if you want to be able to remove the logging lines at compile-time. + + Multiple Journaller objects can use the same file +*/ + +#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" +#endif + +/*! \brief Constructor + \details The constructor configures the object to use the specified file. If it is already open by + another Journaller object, the existing file is reused and it will not be purged. + \param pathfile The (path+) filename of the logfile to use + \param purge Whether to clear the logfile or append when opening the file (default true) + \param initialLogLevel The initial log level to use (default JLL_Alert) +*/ +Journaller::Journaller(const XsString& pathfile, bool purge, JournalLogLevel initialLogLevel) + : m_file(nullptr) + , m_level(initialLogLevel) + , m_debugLevel(initialLogLevel) + , m_flushLevel(JLL_Alert) + , m_threader(new JournalThreader) + , m_useDateTime(false) +{ + init(pathfile, purge); +} + +/*! \brief Destructor, detaches from the logfile and closes it if this was the last reference */ +Journaller::~Journaller() +{ +} + +/*! \brief Initialize the Journaller by (re-)creating the internal journal file */ +void Journaller::init(XsString const& pathfile, bool purge) +{ +#ifdef ANDROID + setTag(tagFromFilename(pathfile.toStdString())); +#endif + m_file.reset(new JournalFile(pathfile, purge)); +} + +/*! \returns The tag string made from file name + \details This function is primarily useful on Android, where we always + need a tag set. + \param fn The file name string +*/ +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("."); + return fn.substr(b, l - b); +} + +/*! \brief When setting the Date Time to \a yes, the timestamps are translated from unix timestamp into a redable date/time + \param yes The boolean value to set + \note the translation is not free, the recommended behaviour is to stick with the default unix timestamps +*/ +void Journaller::setUseDateTime(bool yes) +{ + m_useDateTime = yes; +} + +/*! \brief Sets the additional logger + \param[in] additionallogger A pointer (may be null) to the additional logger. + If additionalLogger is a null pointer, the current additionalLogger is removed. + \note Journaller does NOT take ownership of the logger. +*/ +void Journaller::setAdditionalLogger(AbstractAdditionalLogger* additionallogger) +{ + m_additionalLogger = 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] " +}; + +/*! \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; + 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 + \param level The log level to use + \param msg The message to log + \details This function decorates \a msg with the time and log level and appends a newline +*/ +void Journaller::log(JournalLogLevel level, const std::string& msg) +{ +#ifdef ANDROID + __android_log_write(level, tag().c_str(), msg.c_str()); +#else + if (level < m_level && level < m_debugLevel) + return; + + m_threader->setLineLevel(threadId(), level); + writeTime(); +#if JOURNALLER_WITH_THREAD_SUPPORT + writeThread(); +#endif + writeTag(); + writeLevel(level); + writeMessage(msg); + writeMessage(std::string("\n")); + if (level >= m_flushLevel) + flush(); +#endif +} + +/*! \brief Write the current time to the file */ +void Journaller::writeTime() +{ + XsTimeStamp ts = XsTimeStamp::now(); + if (!m_useDateTime) + { + // when using timestamp format we use UTC time! + char timebuf[32]; + sprintf(timebuf, "%10" PRINTF_INT64_MODIFIER "d.%03d ", ts.secondTime(), (int) ts.milliSecondPart()); + writeMessage(timebuf); + } + else + { + // when using date time format we use LOCAL time! + writeMessage(ts.utcToLocalTime().toString().toStdString()); + } +} + +/*! \brief Write the current time to the file */ +void Journaller::writeThread() +{ + char buf[32]; +#ifdef __GNUC__ + sprintf(buf, "<%08X> ", (unsigned int) threadId()); +#else + sprintf(buf, "<%04X> ", (unsigned int) threadId()); +#endif + writeMessage(buf); +} + +/*! \brief Write the tag to the file */ +void Journaller::writeTag() +{ + std::string tmp = tag(); + if (!tmp.empty()) + writeMessage(tmp); +} + +/*! \brief Write the supplied log \a level to the file + \param level The log level to set + \note This does not actually CHANGE the log level of the line to the supplied value +*/ +void Journaller::writeLevel(JournalLogLevel level) +{ + writeMessage(std::string(gLogLevelString[level])); +} + +/*! \brief Write \a msg to the file without decoration or added newline + \param msg The message to write + \details This function is also used to do the actual writing. It will only write the data to the + outputs when the end of \a msg is a newline character, otherwise it will queue \a msg to be + written later. If \a msg is empty, the currently queued data is written immediately +*/ +void Journaller::writeMessage(const std::string& msg) +{ + if (msg.empty()) + { + flushLine(); + return; + } + + m_threader->line(threadId()).append(msg); + char eol = *msg.rbegin(); + if (eol == '\n' || eol == '\r') + flushLine(); +} + +/*! \brief Flush any data currently queued for logging to the file buffer +*/ +void Journaller::flushLine() +{ + int thread = threadId(); + std::string& line = m_threader->line(thread); + JournalLogLevel lineLevel = m_threader->lineLevel(thread); + if (!line.empty()) + { + if (lineLevel >= m_level) + m_threader->writeLine(thread, m_file.get()); + if (lineLevel > m_debugLevel) + m_threader->writeLine(thread, nullptr); + line.clear(); + } +} + +/*! \brief Flush any data to disk */ +void Journaller::flush() +{ + if (m_file) + { + flushLine(); + m_file->flush(); + } +} + +/*! \brief Set level threshold for automatically flushing lines to disk. */ +void Journaller::setFlushLevel(JournalLogLevel level, bool writeLogLine) +{ + m_flushLevel = level; + if (writeLogLine) + JLGENERIC(this, JLL_Write, "Flush level switched to " << gLogLevelString[m_flushLevel]); +} + +/*! \brief Write the current callstack to the file if \a level is at least equal to the current log level +*/ +void Journaller::writeCallstack(JournalLogLevel level) +{ + if (level < m_level) + return; + + JLGENERIC(this, level, "************ Dump Begin ************"); + JournalStackWalker sw(this); + sw.ShowCallstack(); + JLGENERIC(this, level, "************* Dump End *************"); +} + +/*! \brief Set the log level for logging to file + \param level The log level to use + \param writeLogLine If set to true then it writes a log line + \details Any log requests of a lower level will not be done +*/ +void Journaller::setLogLevel(JournalLogLevel level, bool writeLogLine) +{ + m_level = level; + if (writeLogLine) + JLGENERIC(this, JLL_Write, "Log level set to " << gLogLevelString[level]); +} + +/*! \brief Set the log level for logging to debug output + \param level The log level to use + \param writeLogLine If set to true then it writes a log line + \details Any log requests of a lower level will not be done +*/ +void Journaller::setDebugLevel(JournalLogLevel level, bool writeLogLine) +{ + m_debugLevel = level; + if (writeLogLine) + JLGENERIC(this, JLL_Write, "Debugger output log level set to " << gLogLevelString[level]); +} + +/*! \brief Returns the filename of the used journal file */ +const XsString Journaller::filename() const +{ + return m_file->filename(); +} + +/*! \brief Set a tag to be added before the log level tag in each log file +*/ +void Journaller::setTag(const std::string& tag) +{ + m_tag = tag; +} + +/*! \returns a tag that will be added before the log level tag in each log file. + \details On most platforms the default tag is an empty string, on + Android it defaults to the filename without extension to be able to + make full use of the logcat backend. + + Override the tag using setTag, or overload this function. +*/ +std::string Journaller::tag() const +{ + return m_tag; +} + +template <> +std::ostream& operator << (std::ostream& os, JlHexLogger const& hex) +{ + return os << JlHexLogger((int) hex.m_value); +} + +/*! \brief Move the log file to the supplied path, keeping current contents intact + \param pathfile The path of a file to change to + \param purge If set to true (default) then it clears the log file that we changed to + \param eraseOld If set to true (default) then the old log file will be removed +*/ +void Journaller::moveLogFile(const XsString& pathfile, bool purge, bool eraseOld) +{ + if (m_file && m_file->filename().replacedAll("\\", "/") == pathfile.replacedAll("\\", "/")) + return; + + 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; + if (m_file && m_file->xsFile().isOpen()) + { + oldFileName = m_file->filename(); + JLWRITE(this, "Switching to file " << pathfile); + + XsFile& file = m_file->xsFile(); + sz = file.tell(); + if (sz) + { + file.seek(0); + buffer.reset(new uint8_t[(int)sz]); + sz = file.read(buffer.get(), 1, sz); + } + } + + m_file.reset(newFile); + if (buffer && sz) + m_file->xsFile().write(buffer.get(), 1, sz); + else + writeFileHeader(m_appName); + + if (!oldFileName.empty()) + { + if (eraseOld) + _unlink(oldFileName.c_str()); + JLDEBUG(this, "Switched from " << oldFileName << " to " << pathfile); + } + else + JLDEBUG(this, "Switched to " << pathfile); +} + +/*! \brief Copy the contents of the current log file to \a target + \details After copying the contents, the current log file is closed. + \param target The destination Journaller to write to + \param eraseOld When true (default), the old log file is deleted +*/ +void Journaller::moveLogs(Journaller* target, bool eraseOld) +{ + assert(target); + if (target->filename() == filename()) + return; + + auto newFile = target->m_file; + XsString oldFileName; + std::unique_ptr buffer; + XsFilePos sz = 0; + if (m_file && m_file->xsFile().isOpen()) + { + oldFileName = m_file->filename(); + JLDEBUG(target, "************ Moving logs from " << oldFileName << " to " << target->filename()); + + XsFile& file = m_file->xsFile(); + sz = file.tell(); + if (sz) + { + file.seek(0); + buffer.reset(new uint8_t[(int)sz]); + sz = file.read(buffer.get(), 1, sz); + } + } + + if (buffer && sz && newFile) + newFile->xsFile().write(buffer.get(), 1, sz); + m_file = newFile; + + if (!oldFileName.empty()) + { + if (eraseOld) + _unlink(oldFileName.c_str()); + JLDEBUG(target, "************ Moved logs from " << oldFileName); + } +} + +/*! \brief Removes stored information on the current thread after flushing all data + \note It is recommended to call this in the DllMain DLL_THREAD_DETACH section +*/ +void Journaller::cleanupThread() +{ +#if JOURNALLER_WITH_THREAD_SUPPORT + flush(); + if (m_threader) + m_threader->cleanup(threadId()); +#endif +} + +/*! \brief Cleans up any remaining stuff + \details The function assumes that no new log lines will be created anymore + \param gj Optional pointer to Journaller pointer to be cleaned up, usually &gJournal should be provided. The pointer that gj points to will be set to nullptr. + \note JLINSTALLQTDEBUGHANDLER(0) is not done as it would create a dependency on Qt so you need to call that yourself before calling jlTerminate +*/ +void jlTerminate(Journaller** gj) +{ + if (gj && *gj) + { + delete *gj; + *gj = nullptr; + } +} + +#if __GNUC__ >= 6 + #pragma GCC diagnostic pop +#endif diff --git a/extern/xspublic/xscommon/journaller.h b/extern/xspublic/xscommon/journaller.h new file mode 100644 index 0000000..cadec94 --- /dev/null +++ b/extern/xspublic/xscommon/journaller.h @@ -0,0 +1,420 @@ + +// Copyright (c) 2003-2022 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 JOURNALLERBASE_H +#define JOURNALLERBASE_H + +#include "journalloglevel.h" +#include "abstractadditionallogger.h" +#include +#ifndef JL_NOTEMPLATE + #include +#endif +#include +#include + +class JournalFile; +class JournalThreader; +class Journaller +{ +public: + Journaller(const XsString& pathfile, bool purge = true, JournalLogLevel initialLogLevel = JLL_Alert); + ~Journaller(); + + void log(JournalLogLevel level, const std::string& msg); + void writeCallstack(JournalLogLevel level); + + void setLogLevel(JournalLogLevel level, bool writeLogLine = true); + + //! \returns The log level for logging to file + 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; + } + + void setDebugLevel(JournalLogLevel level, bool writeLogLine = true); + + //! \returns The log level for logging to debug output + 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; + } + + void writeFileHeader(const std::string& appName); + void setUseDateTime(bool yes); + + void writeTime(); + void writeThread(); + void writeTag(); + void writeLevel(JournalLogLevel level); + void writeMessage(const std::string& msg); + void flush(); + void cleanupThread(); + + const XsString filename() const; + + 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; + } + + //! \returns The additional logger + inline static AbstractAdditionalLogger* additionalLogger() + { + return m_additionalLogger; + } + + 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); + +private: + void init(XsString const& pathfile, bool purge); + void flushLine(); + + std::shared_ptr m_file; + std::string m_tag; + std::string m_appName; + JournalLogLevel m_level; + JournalLogLevel m_debugLevel; + JournalLogLevel m_flushLevel; + std::shared_ptr m_threader; + + bool m_useDateTime; + + static AbstractAdditionalLogger* m_additionalLogger; + + // no copying allowed + Journaller& operator = (Journaller const&) = delete; + Journaller(Journaller const&) = delete; +}; + +#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); +} +inline static constexpr char const* jlStrippedPathFile(char const* a) +{ + return jlStrippedPath(a, a); +} +#define STRIPPEDFILE jlStrippedPathFile(__FILE__) +#else +#define STRIPPEDFILE __FILE__ +#endif + +#if !defined(JLNOLINEINFO) + #define JLGENERIC_LINEINFO STRIPPEDFILE << "(" << __LINE__ << ") " +#else + #define JLGENERIC_LINEINFO "" +#endif + +#define JLGENERIC(journal, level, msg)\ + do { /*lint --e{506}*/ \ + if (journal && journal->logLevel(level)) \ + { \ + std::ostringstream os; \ + os << JLGENERIC_LINEINFO << __FUNCTION__ << " " << msg; \ + journal->log(level, os.str()); \ + } \ + if (Journaller::hasAdditionalLogger() && Journaller::additionalLogger()->logLevel(level)) \ + { \ + std::ostringstream os; \ + os << msg; \ + Journaller::additionalLogger()->log(level, STRIPPEDFILE, __LINE__, __FUNCTION__, os.str()); \ + } \ + } while(0) + +#define JLGENERIC_NODEC(journal, level, msg)\ + do { \ + if (journal && journal->logLevel(level)) \ + { \ + std::ostringstream os; \ + os << msg << '\n'; \ + journal->writeMessage(os.str()); \ + } \ + if (Journaller::hasAdditionalLogger() && Journaller::additionalLogger()->logLevel(level)) \ + { \ + std::ostringstream os; \ + os << msg << '\n'; \ + Journaller::additionalLogger()->logNoDecoration(level, STRIPPEDFILE, __LINE__, __FUNCTION__, os.str()); \ + } \ + } while(0) + +#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_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) + +#endif + +#ifndef _lint //pclint defines this + #define JLIF(journal, level, todo) do { if (journal && journal->logLevel(level)) { todo; } } while(0) +#else + #define JLIF(...) ((void)0) +#endif + +#ifndef JL_NOTEMPLATE + +/*! \class JlHexLogger + \brief A support class of journaller that is used for the logging of hex values +*/ +template +class JlHexLogger +{ +public: + T m_value; //!< A hex value + + /*! \brief Constructor + */ + explicit JlHexLogger(T i) + : m_value(i) + { + } +}; + +template +std::ostream& operator << (std::ostream& os, JlHexLogger const& hex) +{ + std::ios_base::fmtflags f = os.flags(); + os << std::hex << std::uppercase << hex.m_value << std::setiosflags(f); + return os; +} +template <> std::ostream& operator << (std::ostream& os, JlHexLogger const& hex); + +#define JLHEXLOG_BARE(d) JlHexLogger(d) // C++0x11 only! +#else +#define JLHEXLOG_BARE(d) std::hex << std::uppercase << (int) d << std::nouppercase << std::dec +#endif +#define JLHEXLOG(d) "0x" << JLHEXLOG_BARE(d) +#define JLVARLOG(a) #a ": " << a << " " + +#define JLPRECISE2(msg, prec)\ + [&](){\ + std::ostringstream os; \ + os.precision(prec); \ + os << msg; \ + return os.str();\ + }() +#define JLPRECISE(msg) JLPRECISE2(msg, 16) + +#define JLCASE2(s, a, b) case a: s << b << "(" << static_cast(a) << ")"; break; +#define JLCASE(s, a) case a: s << #a << "(" << static_cast(a) << ")"; break; +#define JLDEFAULTCASE(s) default: s << "Unknown case: " << static_cast(e); break; +#define JLENUMEXPPROTO(E) std::ostream& operator << (std::ostream& dbg, E e) +#define JLENUMEXPHDR(E, ...) /*! \brief Translate \a e into a text representation */ JLENUMEXPPROTO(E) { __VA_ARGS__ switch(e) { +#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, 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, 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) { /*lint --e{438, 650, 587} */ 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) << ")"; e = static_cast(static_cast(e) & ~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);\ + } + +#define JLINSTALLQTDEBUGHANDLER(journal) \ + qtJournal = journal;\ + if (journal) {\ + if (gOldQtMessageHandler == NULL) gOldQtMessageHandler = qInstallMessageHandler(jlQtMessageHandler);\ + else qInstallMessageHandler(jlQtMessageHandler);\ + } else if (gOldQtMessageHandler) {\ + qInstallMessageHandler(gOldQtMessageHandler);\ + gOldQtMessageHandler = NULL;\ + } + +////////////////////////////////////////////////////////////////////////////////////////// +/*! \class JournalValueJanitor + \brief Class for journalling a referenced value when the janitor leaves scope + \details The class stores a reference to the supplied object and uses streaming to output + it to the journal. Like all journalling this means that for the supplied type T + std::stringstream& std::stringstream::operator << (std::stringstream&, const T& value) must + be defined. +*/ +template +class JournalValueJanitor +{ +private: + const JournalValueJanitor& operator = (const JournalValueJanitor&); + + Journaller* m_journal; + const T& m_value; + std::string m_msg; + JournalLogLevel m_level; +public: + bool m_enabled; //!< Boolean value if set to true then the janitor is enabled + + /*! \brief Constructor + */ + JournalValueJanitor(Journaller* journal, const T& value, const std::string& msg = std::string(), JournalLogLevel level = JLL_Debug, bool enabled = true) + : m_journal(journal) + , m_value(value) + , m_msg(msg) + , m_level(level) + , m_enabled(enabled && journal) + {} + + /*! \brief Destructor + */ + ~JournalValueJanitor() + { + if (m_enabled) + { + std::stringstream os; + os << m_msg << m_value; + m_journal->log(m_level, os.str()); + } + } +}; + +void jlTerminate(Journaller** gj); + +#endif diff --git a/extern/xspublic/xscommon/journalloglevel.h b/extern/xspublic/xscommon/journalloglevel.h new file mode 100644 index 0000000..e62e508 --- /dev/null +++ b/extern/xspublic/xscommon/journalloglevel.h @@ -0,0 +1,114 @@ + +// Copyright (c) 2003-2022 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 JOURNALLOGLEVEL_H +#define JOURNALLOGLEVEL_H + +#ifdef ANDROID + #include + + #define JLL_TRACE ANDROID_LOG_VERBOSE + #define JLL_DEBUG ANDROID_LOG_DEBUG + #define JLL_ALERT ANDROID_LOG_WARN + #define JLL_ERROR ANDROID_LOG_ERROR + #define JLL_FATAL ANDROID_LOG_FATAL + #define JLL_WRITE ANDROID_LOG_SILENT + #define JLL_DISABLE (JLL_WRITE+1) +#else + #define JLL_TRACE 0 + #define JLL_DEBUG 1 + #define JLL_ALERT 2 + #define JLL_ERROR 3 + #define JLL_FATAL 4 + #define JLL_WRITE 5 + #define JLL_DISABLE 6 +#endif + +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_Disable = JLL_DISABLE //!< don't log any messages +}; + +#ifdef NO_JOURNALLER + #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 + #ifndef XSENS_RELEASE + // full debug + #ifndef JLDEF_BUILD + #define JLDEF_BUILD JLL_DEBUG // 'trace' needs to be enabled explicitly since it potentially has a huge impact on performance + #endif + #ifndef JLDEF_FILE + #define JLDEF_FILE JLL_Debug + #endif + #ifndef JLDEF_DEBUGGER + #define JLDEF_DEBUGGER JLL_Alert + #endif + #else + // Release With Debug Info (non-optimized Release build) + #ifndef JLDEF_BUILD + #define JLDEF_BUILD JLL_DEBUG + #endif + #ifndef JLDEF_FILE + #define JLDEF_FILE JLL_Debug + #endif + #ifndef JLDEF_DEBUGGER + #define JLDEF_DEBUGGER JLL_Error + #endif + #endif +#else + // Full optimized Release build, logging should be reduced to a minimum + #ifndef JLDEF_BUILD + #define JLDEF_BUILD JLL_ALERT + #endif + #ifndef JLDEF_FILE + #define JLDEF_FILE JLL_Alert + #endif + #ifndef JLDEF_DEBUGGER + #define JLDEF_DEBUGGER JLL_Fatal + #endif + #define JLNOLINEINFO +#endif + +#endif diff --git a/extern/xspublic/xscommon/journalstackwalker.h b/extern/xspublic/xscommon/journalstackwalker.h new file mode 100644 index 0000000..e843eff --- /dev/null +++ b/extern/xspublic/xscommon/journalstackwalker.h @@ -0,0 +1,70 @@ + +// Copyright (c) 2003-2022 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 JOURNALSTACKWALKER_H +#define JOURNALSTACKWALKER_H + +#ifdef _MSC_VER + #include "stackwalker.h" +#else + #include "stackwalker_linux.h" +#endif +#include "journaller.h" + +/*! \class JournalStackWalker + \brief A journaller class that is used for walking the stack +*/ +class JournalStackWalker : public StackWalker +{ +public: + /*! \brief Constructor + */ + JournalStackWalker(Journaller* journal) + : StackWalker() + , m_journaller(journal) + { + } + + /*! \brief Handles the output from stack + \param szText The string that contains stack output + */ + void OnOutput(LPCSTR szText) override + { + m_journaller->writeMessage(szText); + m_journaller->writeMessage("\n"); + } + +protected: + Journaller* m_journaller; //!< A pointer to journaller object +}; + +#endif diff --git a/extern/xspublic/xscommon/journalthreader.cpp b/extern/xspublic/xscommon/journalthreader.cpp new file mode 100644 index 0000000..e618f85 --- /dev/null +++ b/extern/xspublic/xscommon/journalthreader.cpp @@ -0,0 +1,132 @@ + +// Copyright (c) 2003-2022 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 "journalthreader.h" +#include "journalfile.h" + +#ifndef XSENS_WINDOWS +static void OutputDebugStringA(const char* msg) +{ + (void)fprintf(stderr, "%s", msg); +} +#endif + +/*! \class JournalThreader + \brief Manages threaded writes for the Journaller objects +*/ + +/*! \brief Constructor, sets up the necessary structures for threaded logging */ +JournalThreader::JournalThreader() +{ +} + +/*! \brief Destructor, flushes all remaining data +*/ +JournalThreader::~JournalThreader() +{ +} + +/*! \brief Flush all available non-empty lines to the supplied file, clearing lines as they are flushed + \param file The file to write to. Supply nullptr to write to the debug output +*/ +void JournalThreader::flushAll(JournalFile* file) +{ + while (true) + { + ThreadLine& line = nextLine(); + if (line.m_line.empty()) + return; + + if (file) + *file << line.m_line; + else + OutputDebugStringA(line.m_line.c_str()); + line.m_line.clear(); + } +} + +/*! \brief Write the line for thread \a thread to file \a file + \param thread The thread id associated with the line + \param file The file to write to. Supply nullptr to write to the debug output +*/ +void JournalThreader::writeLine(int thread, JournalFile* file) +{ + ThreadLine& line = threadLine(thread); + if (line.m_line.empty()) + return; + + if (file) + *file << line.m_line; + else + OutputDebugStringA(line.m_line.c_str()); +} + +/*! \brief Set the log level of the queued line + \param thread The thread id associated with the line + \param level The log level to set + \returns The old log level +*/ +JournalLogLevel JournalThreader::setLineLevel(int thread, JournalLogLevel level) +{ + ThreadLine& line = threadLine(thread); + auto old = line.m_level; + line.m_level = level; + return old; +} + +/*! \returns The log level of the queued line + \param thread The thread id associated with the line +*/ +JournalLogLevel JournalThreader::lineLevel(int thread) +{ + ThreadLine& line = threadLine(thread); + return line.m_level; +} + +/*! \returns The reference to the line associated with the given file and thread + \param thread The thread id associated with the line, ignored when JOURNALLER_WITH_THREAD_SUPPORT == 0 +*/ +std::string& JournalThreader::line(int thread) +{ + ThreadLine& line = threadLine(thread); + return line.m_line; +} + +/*! \brief Remove the thread-local storage for thread \a id + \param id The thread to remove +*/ +void JournalThreader::cleanup(int id) +{ + xsens::Lock lock(&m_mutex); + m_map.erase(id); + lock.unlock(); +} diff --git a/extern/xspublic/xscommon/journalthreader.h b/extern/xspublic/xscommon/journalthreader.h new file mode 100644 index 0000000..9c6bbc0 --- /dev/null +++ b/extern/xspublic/xscommon/journalthreader.h @@ -0,0 +1,111 @@ + +// Copyright (c) 2003-2022 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 JOURNALTHREADER_H +#define JOURNALTHREADER_H + +#include "xscommon_config.h" +#include +#include "journalloglevel.h" +#if JOURNALLER_WITH_THREAD_SUPPORT + #include + #include "xsens_mutex.h" +#endif + +class FileInfo; +class JournalFile; + +class JournalThreader +{ +public: + JournalThreader(); + ~JournalThreader(); + + void flushAll(JournalFile* file); + void writeLine(int thread, JournalFile* file); + JournalLogLevel setLineLevel(int thread, JournalLogLevel level); + JournalLogLevel lineLevel(int thread); + std::string& line(int thread); + void cleanup(int id); + +private: + /*! \brief Storage for logging queue of a specific thread + */ + class ThreadLine + { + public: + std::string m_line; //!< The contained text for this log line + JournalLogLevel m_level; //!< The level of this log line + + /*! \brief Constructor, creates an empty line at WRITE log level */ + ThreadLine() : m_level(JLL_Write) {} + }; + +#if JOURNALLER_WITH_THREAD_SUPPORT + std::map m_map; //!< The contained lines, one for each thread + mutable xsens::Mutex m_mutex; //!< A mutex guarding access to the map (not to the lines) + + /*! \brief Get the line object for the supplied thread, typically for the current thread */ + inline ThreadLine& threadLine(int thread) + { + xsens::Lock lock(&m_mutex); + return m_map[thread]; + } + + /*! \brief Get the next non-empty line object from the map of all threads, returns an empty line object if no non-empty objects were available */ + inline ThreadLine& nextLine() + { + xsens::Lock lock(&m_mutex); + for (auto it = m_map.begin(); it != m_map.end(); ++it) + if (!it->second.m_line.empty()) + return it->second; + return threadLine(0); + } + +#else + + ThreadLine m_line; //!< The contained line, in single-threaded mode only a single line is needed per journaller + /*! \brief Get the line object */ + inline ThreadLine& threadLine(int) + { + return m_line; + } + + /*! \brief Get the line object */ + inline ThreadLine& nextLine() + { + return m_line; + } +#endif +}; + +#endif diff --git a/extern/xspublic/xscommon/stackdumper.cpp b/extern/xspublic/xscommon/stackdumper.cpp new file mode 100644 index 0000000..57908f8 --- /dev/null +++ b/extern/xspublic/xscommon/stackdumper.cpp @@ -0,0 +1,83 @@ + +// Copyright (c) 2003-2022 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 "stackdumper.h" + +#ifdef _MSC_VER + #include "stackwalker.h" +#else + #include "stackwalker_linux.h" +#endif + +/*! \class DumpStackWalker + \brief Helper class for the \ref getStackDump() function +*/ +class DumpStackWalker : public StackWalker +{ +public: + //! Constructor + DumpStackWalker(bool includeModules) + : StackWalker() + , m_outputStarted(includeModules) + { + } + + //! Storage for the stack dump while it's being created + std::string m_string; + bool m_outputStarted; + + //! Overloaded function that deals with stqack dump information as it comes in + void OnOutput(LPCSTR szText) override + { +#ifdef _MSC_VER + if (!m_outputStarted) + { + if (strstr(szText, "************* Callstack *************")) + m_outputStarted = true; + } + if (!m_outputStarted) + return; +#endif + m_string.append("\n\t"); + m_string.append(szText); + } +}; + +/*! \brief Returns the callstack and other debug information as a string. + \details The stackdump includes some of the stack dumping functions +*/ +std::string getStackDump(bool includeModules) +{ + DumpStackWalker sw(includeModules); + sw.ShowCallstack(); + return sw.m_string; +} diff --git a/extern/xspublic/xscommon/stackdumper.h b/extern/xspublic/xscommon/stackdumper.h new file mode 100644 index 0000000..e85642f --- /dev/null +++ b/extern/xspublic/xscommon/stackdumper.h @@ -0,0 +1,39 @@ + +// Copyright (c) 2003-2022 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 STACKDUMPER_H +#define STACKDUMPER_H + +#include +std::string getStackDump(bool includeModules); + +#endif diff --git a/extern/xspublic/xscommon/stackwalker.cpp b/extern/xspublic/xscommon/stackwalker.cpp new file mode 100644 index 0000000..eea8cf2 --- /dev/null +++ b/extern/xspublic/xscommon/stackwalker.cpp @@ -0,0 +1,1272 @@ + +// Copyright (c) 2003-2022 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. +// + +/********************************************************************** + + 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__ + +#include +#include +#include +#pragma comment(lib, "version.lib") // for "VerQueryValue" + +#include "stackwalker.h" + + +// If VC7 and later, then use the shipped 'dbghelp.h'-file +#if _MSC_VER >= 1300 +#pragma warning(push) +#pragma warning(disable: 4091) +#include +#pragma warning(pop) +#else +// inline the important dbghelp.h-declarations... +typedef enum +{ + SymNone = 0, + SymCoff, + SymCv, + SymPdb, + SymExport, + SymDeferred, + SymSym, + SymDia, + SymVirtual, + NumSymTypes +} SYM_TYPE; + +typedef struct _IMAGEHLP_LINE64 +{ + DWORD SizeOfStruct; // set to sizeof(IMAGEHLP_LINE64) + PVOID Key; // internal + DWORD LineNumber; // line number in file + PCHAR FileName; // full filename + DWORD64 Address; // first instruction of line +} IMAGEHLP_LINE64, *PIMAGEHLP_LINE64; + +typedef struct _IMAGEHLP_MODULE64 +{ + 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 +} IMAGEHLP_MODULE64, *PIMAGEHLP_MODULE64; + +typedef struct _IMAGEHLP_SYMBOL64 +{ + DWORD SizeOfStruct; // set to sizeof(IMAGEHLP_SYMBOL64) + DWORD64 Address; // virtual address including dll base address + DWORD Size; // estimated size of symbol, can be zero + DWORD Flags; // info about the symbols, see the SYMF defines + DWORD MaxNameLength; // maximum size of symbol name in 'Name' + CHAR Name[1]; // symbol name (null terminated string) +} IMAGEHLP_SYMBOL64, *PIMAGEHLP_SYMBOL64; + +typedef enum +{ + AddrMode1616, + AddrMode1632, + AddrModeReal, + AddrModeFlat +} ADDRESS_MODE; + +typedef struct _tagADDRESS64 +{ + DWORD64 Offset; + WORD Segment; + ADDRESS_MODE Mode; +} ADDRESS64, *LPADDRESS64; + +typedef struct _KDHELP64 +{ + DWORD64 Thread; + DWORD ThCallbackStack; + DWORD ThCallbackBStore; + DWORD NextCallback; + DWORD FramePointer; + DWORD64 KiCallUserMode; + DWORD64 KeUserCallbackDispatcher; + DWORD64 SystemRangeStart; + DWORD64 Reserved[8]; +} KDHELP64, *PKDHELP64; + +typedef struct _tagSTACKFRAME64 +{ + ADDRESS64 AddrPC; // program counter + ADDRESS64 AddrReturn; // return address + ADDRESS64 AddrFrame; // frame pointer + ADDRESS64 AddrStack; // stack pointer + ADDRESS64 AddrBStore; // backing store pointer + PVOID FuncTableEntry; // pointer to pdata/fpo or NULL + DWORD64 Params[4]; // possible arguments to the function + BOOL Far; // WOW far call + BOOL Virtual; // is this a virtual frame? + DWORD64 Reserved[3]; + KDHELP64 KdHelp; +} STACKFRAME64, *LPSTACKFRAME64; + +typedef BOOL(__stdcall* PREAD_PROCESS_MEMORY_ROUTINE64)( + HANDLE hProcess, + DWORD64 qwBaseAddress, + PVOID lpBuffer, + DWORD nSize, + LPDWORD lpNumberOfBytesRead +); + +typedef PVOID(__stdcall* PFUNCTION_TABLE_ACCESS_ROUTINE64)( + HANDLE hProcess, + DWORD64 AddrBase +); + +typedef DWORD64(__stdcall* PGET_MODULE_BASE_ROUTINE64)( + HANDLE hProcess, + DWORD64 Address +); + +typedef DWORD64(__stdcall* PTRANSLATE_ADDRESS_ROUTINE64)( + HANDLE hProcess, + HANDLE hThread, + LPADDRESS64 lpaddr +); + +#define SYMOPT_CASE_INSENSITIVE 0x00000001 +#define SYMOPT_UNDNAME 0x00000002 +#define SYMOPT_DEFERRED_LOADS 0x00000004 +#define SYMOPT_NO_CPP 0x00000008 +#define SYMOPT_LOAD_LINES 0x00000010 +#define SYMOPT_OMAP_FIND_NEAREST 0x00000020 +#define SYMOPT_LOAD_ANYTHING 0x00000040 +#define SYMOPT_IGNORE_CVREC 0x00000080 +#define SYMOPT_NO_UNQUALIFIED_LOADS 0x00000100 +#define SYMOPT_FAIL_CRITICAL_ERRORS 0x00000200 +#define SYMOPT_EXACT_SYMBOLS 0x00000400 +#define SYMOPT_ALLOW_ABSOLUTE_SYMBOLS 0x00000800 +#define SYMOPT_IGNORE_NT_SYMPATH 0x00001000 +#define SYMOPT_INCLUDE_32BIT_MODULES 0x00002000 +#define SYMOPT_PUBLICS_ONLY 0x00004000 +#define SYMOPT_NO_PUBLICS 0x00008000 +#define SYMOPT_AUTO_PUBLICS 0x00010000 +#define SYMOPT_NO_IMAGE_SEARCH 0x00020000 +#define SYMOPT_SECURE 0x00040000 +#define SYMOPT_DEBUG 0x80000000 +#define UNDNAME_COMPLETE (0x0000) // Enable full undecoration +#define UNDNAME_NAME_ONLY (0x1000) // Crack only the name for primary declaration; +#endif // _MSC_VER < 1300 + +// Some missing defines (for VC5/6): +#ifndef INVALID_FILE_ATTRIBUTES + #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 +#endif + +// Normally it should be enough to use 'CONTEXT_FULL' (better would be 'CONTEXT_ALL') +#define USED_CONTEXT_FLAGS CONTEXT_FULL + +class StackWalkerInternal +{ +public: + StackWalkerInternal(StackWalker* parent, HANDLE hProcess) + { + m_parent = parent; + m_hDbhHelp = NULL; + pSC = NULL; + m_hProcess = hProcess; + m_szSymPath = NULL; + pSFTA = NULL; + pSGLFA = NULL; + pSGMB = NULL; + pSGMI = NULL; + pSGO = NULL; + pSGSFA = NULL; + pSI = NULL; + pSLM = NULL; + pSSO = NULL; + pSW = NULL; + pUDSN = NULL; + pSGSP = NULL; + } + + ~StackWalkerInternal() + { + if (pSC != NULL) + pSC(m_hProcess); // SymCleanup + if (m_hDbhHelp != NULL) + FreeLibrary(m_hDbhHelp); + m_hDbhHelp = NULL; + m_parent = NULL; + if (m_szSymPath != NULL) + free(m_szSymPath); + m_szSymPath = NULL; + } + + BOOL Init(LPCSTR szSymPath) + { + if (m_parent == NULL) + return FALSE; + // Dynamically load the Entry-Points for dbghelp.dll: + // First try to load the newsest one from + TCHAR szTemp[4096]; + // But before wqe do this, we first check if the ".local" file exists + if (GetModuleFileName(NULL, szTemp, 4096) > 0) + { + _tcscat_s(szTemp, _T(".local")); + if (GetFileAttributes(szTemp) == INVALID_FILE_ATTRIBUTES) + { + // ".local" file does not exist, so we can try to load the dbghelp.dll from the "Debugging Tools for Windows" + if (GetEnvironmentVariable(_T("ProgramFiles"), szTemp, 4096) > 0) + { + _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)) + { + _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")); + if (m_hDbhHelp == NULL) + return FALSE; + 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"); + + 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"); + + if (pSC == NULL || pSFTA == NULL || pSGMB == NULL || pSGMI == NULL || + pSGO == NULL || pSGSFA == NULL || pSI == NULL || pSSO == NULL || + pSW == NULL || pUDSN == NULL || pSLM == NULL) + { + FreeLibrary(m_hDbhHelp); + m_hDbhHelp = NULL; + pSC = NULL; + return FALSE; + } + + // SymInitialize + if (szSymPath != NULL) + m_szSymPath = _strdup(szSymPath); + if (pSI(m_hProcess, m_szSymPath, FALSE) == FALSE) + m_parent->OnDbgHelpErr("SymInitialize", GetLastError(), 0); + + DWORD symOptions = pSGO(); // SymGetOptions + symOptions |= SYMOPT_LOAD_LINES; + symOptions |= SYMOPT_FAIL_CRITICAL_ERRORS; + //symOptions |= SYMOPT_NO_PROMPTS; + // SymSetOptions + symOptions = pSSO(symOptions); + + char buf[StackWalker::STACKWALK_MAX_NAMELEN] = {0}; + if (pSGSP != NULL) + { + if (!pSGSP(m_hProcess, buf, StackWalker::STACKWALK_MAX_NAMELEN)) + m_parent->OnDbgHelpErr("SymGetSearchPath", GetLastError(), 0); + } + char szUserName[1024] = {0}; + DWORD dwSize = 1024; + GetUserNameA(szUserName, &dwSize); + m_parent->OnSymInit(buf, symOptions, szUserName); + + return TRUE; + } + + 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 + }; + + + // SymCleanup() + typedef BOOL (__stdcall* tSC)(IN HANDLE hProcess); + tSC pSC; + + // SymFunctionTableAccess64() + 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); + tSGLFA pSGLFA; + + // SymGetModuleBase64() + 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); + tSGMI pSGMI; + + // // 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); + tSGO pSGO; + + // SymGetSymFromAddr64() + 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); + 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); + tSLM pSLM; + + // SymSetOptions() + 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); + tSW pSW; + + // UnDecorateSymbolName() + typedef DWORD (__stdcall* tUDSN)(PCSTR DecoratedName, PSTR UnDecoratedName, + DWORD UndecoratedLength, DWORD Flags); + tUDSN pUDSN; + + 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 ) + typedef struct tagMODULEENTRY32 + { + DWORD dwSize; + DWORD th32ModuleID; // This module + 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 + 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 ) + + BOOL GetModuleListTH32(HANDLE hProcess, DWORD pid) + { + + // CreateToolhelp32Snapshot() + typedef HANDLE(__stdcall * tCT32S)(DWORD dwFlags, DWORD th32ProcessID); + // Module32First() + typedef BOOL (__stdcall * tM32F)(HANDLE hSnapshot, LPMODULEENTRY32 lpme); + // Module32Next() + typedef BOOL (__stdcall * tM32N)(HANDLE hSnapshot, LPMODULEENTRY32 lpme); + + // try both dlls... + const TCHAR* dllname[] = { _T("kernel32.dll"), _T("tlhelp32.dll") }; + HINSTANCE hToolhelp = NULL; + tCT32S pCT32S = NULL; + tM32F pM32F = NULL; + tM32N pM32N = NULL; + + HANDLE hSnap; + MODULEENTRY32 me; + me.dwSize = sizeof(me); + bool keepGoing; + size_t i; + + for (i = 0; i < (sizeof(dllname) / sizeof(dllname[0])); 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)) + break; // found the functions! + FreeLibrary(hToolhelp); + hToolhelp = NULL; + } + + if (hToolhelp == NULL) + return FALSE; + + hSnap = pCT32S(TH32CS_SNAPMODULE, pid); + if (hSnap == (HANDLE) - 1) + return FALSE; + + 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); + } + CloseHandle(hSnap); + FreeLibrary(hToolhelp); + if (cnt <= 0) + return FALSE; + return TRUE; + } // GetModuleListTH32 + + // **************************************** PSAPI ************************ + typedef struct _MODULEINFO + { + LPVOID lpBaseOfDll; + DWORD SizeOfImage; + LPVOID EntryPoint; + } MODULEINFO, *LPMODULEINFO; + + BOOL GetModuleListPSAPI(HANDLE hProcess) + { + // EnumProcessModules() + 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); + // GetModuleBaseName() + 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); + + HINSTANCE hPsapi; + tEPM pEPM; + tGMFNE pGMFNE; + tGMBN pGMBN; + tGMI pGMI; + + DWORD i; + //ModuleEntry e; + DWORD cbNeeded; + MODULEINFO mi; + HMODULE* hMods; + char* tt; + char* tt2; + const SIZE_T TTBUFLEN = 8096; + int cnt = 0; + + 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)) + { + // we couldnt find all functions + FreeLibrary(hPsapi); + return FALSE; + } + + 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)) + goto cleanup; + + if (! pEPM(hProcess, hMods, TTBUFLEN, &cbNeeded)) + { + //_ftprintf(fLogFile, _T("%lu: EPM failed, GetLastError = %lu\n"), g_dwShowCount, gle ); + goto cleanup; + } + + 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++) + { + // base address, size + pGMI(hProcess, hMods[i], &mi, sizeof mi); + // image file name + tt[0] = 0; + pGMFNE(hProcess, hMods[i], tt, TTBUFLEN); + // module name + tt2[0] = 0; + pGMBN(hProcess, hMods[i], tt2, TTBUFLEN); + + DWORD dwRes = this->LoadModule(hProcess, tt, tt2, (DWORD64) mi.lpBaseOfDll, mi.SizeOfImage); + if (dwRes != ERROR_SUCCESS) + m_parent->OnDbgHelpErr("LoadModule", dwRes, 0); + cnt++; + } + +cleanup: + FreeLibrary(hPsapi); + 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); + DWORD result = ERROR_SUCCESS; + 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)) + { + // try to retrive the file-version: + if ((m_parent->m_options & (int) StackWalker::RetrieveFileVersion) != 0) + { + VS_FIXEDFILEINFO* fInfo = NULL; + DWORD dwHandle; + DWORD dwSize = GetFileVersionInfoSizeA(szImg, &dwHandle); + if (dwSize > 0) + { + LPVOID vData = malloc(dwSize); + if (vData != NULL) + { + if (GetFileVersionInfoA(szImg, dwHandle, dwSize, vData) != 0) + { + UINT len; + TCHAR szSubBlock[] = _T("\\"); + if (VerQueryValue(vData, szSubBlock, (LPVOID*) &fInfo, &len) != 0) + fileVersion = ((ULONGLONG)fInfo->dwFileVersionLS) + ((ULONGLONG)fInfo->dwFileVersionMS << 32); + } + free(vData); + } + } + } + + // Retrieve some additional-infos about the module + IMAGEHLP_MODULE64_V2 Module; + const char* szSymType = "-unknown-"; + if (this->GetModuleInfo(hProcess, baseAddr, &Module) != FALSE) + { + 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; + } + } + m_parent->OnLoadModule(img, mod, baseAddr, size, result, szSymType, Module.LoadedImageName, fileVersion); + } + if (szImg != NULL) + free(szImg); + if (szMod != NULL) + free(szMod); + return result; + } +public: + BOOL LoadModules(HANDLE hProcess, DWORD dwProcessId) + { + // first try toolhelp32 + if (GetModuleListTH32(hProcess, dwProcessId)) + return TRUE; + // then try psapi + return GetModuleListPSAPI(hProcess); + } + + + BOOL GetModuleInfo(HANDLE hProcess, DWORD64 baseAddr, IMAGEHLP_MODULE64_V2* pModuleInfo) + { + if (pSGMI == NULL) + { + SetLastError(ERROR_DLL_INIT_FAILED); + return FALSE; + } + // First try to use the larger ModuleInfo-Structure + // memset(pModuleInfo, 0, sizeof(IMAGEHLP_MODULE64_V3)); + // pModuleInfo->SizeOfStruct = sizeof(IMAGEHLP_MODULE64_V3); + // if (this->pSGMI_V3 != NULL) + // { + // if (this->pSGMI_V3(hProcess, baseAddr, pModuleInfo) != FALSE) + // return TRUE; + // // check if the parameter was wrong (size is bad...) + // if (GetLastError() != ERROR_INVALID_PARAMETER) + // return FALSE; + // } + // 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... + if (pData == NULL) + { + SetLastError(ERROR_NOT_ENOUGH_MEMORY); + return FALSE; + } + memcpy(pData, pModuleInfo, sizeof(IMAGEHLP_MODULE64_V2)); + if (pSGMI(hProcess, baseAddr, (IMAGEHLP_MODULE64_V2*) pData) != FALSE) + { + // only copy as much memory as is reserved... + memcpy(pModuleInfo, pData, sizeof(IMAGEHLP_MODULE64_V2)); + pModuleInfo->SizeOfStruct = sizeof(IMAGEHLP_MODULE64_V2); + free(pData); + return TRUE; + } + free(pData); + SetLastError(ERROR_DLL_INIT_FAILED); + return FALSE; + } +}; + +// ############################################################# +StackWalker::StackWalker(DWORD dwProcessId, HANDLE hProcess) +{ + m_options = (int) OptionsAll; + m_modulesLoaded = FALSE; + m_hProcess = hProcess; + m_sw = new StackWalkerInternal(this, m_hProcess); + m_dwProcessId = dwProcessId; + m_szSymPath = NULL; +} +StackWalker::StackWalker(int options, LPCSTR szSymPath, DWORD dwProcessId, HANDLE hProcess) +{ + m_options = options; + m_modulesLoaded = FALSE; + m_hProcess = hProcess; + m_sw = new StackWalkerInternal(this, m_hProcess); + m_dwProcessId = dwProcessId; + if (szSymPath != NULL) + { + m_szSymPath = _strdup(szSymPath); + m_options |= (int) SymBuildPath; + } + else + m_szSymPath = NULL; +} + +StackWalker::~StackWalker() +{ + if (m_szSymPath != NULL) + free(m_szSymPath); + m_szSymPath = NULL; + if (m_sw != NULL) + delete m_sw; + m_sw = NULL; +} + +BOOL StackWalker::LoadModules() +{ + if (m_sw == NULL) + { + SetLastError(ERROR_DLL_INIT_FAILED); + return FALSE; + } + if (m_modulesLoaded != FALSE) + return TRUE; + + // Build the sym-path: + char* szSymPath = NULL; + if ((m_options & (int) SymBuildPath) != 0) + { + const size_t nSymPathLen = 4096; + szSymPath = (char*) malloc(nSymPathLen); + if (szSymPath == NULL) + { + SetLastError(ERROR_NOT_ENOUGH_MEMORY); + return FALSE; + } + szSymPath[0] = 0; + // Now first add the (optional) provided sympath: + if (m_szSymPath != NULL) + { + strcat_s(szSymPath, nSymPathLen, m_szSymPath); + strcat_s(szSymPath, nSymPathLen, ";"); + } + + strcat_s(szSymPath, nSymPathLen, ".;"); + + const size_t nTempLen = 1024; + char szTemp[nTempLen]; + // Now add the current directory: + if (GetCurrentDirectoryA(nTempLen, szTemp) > 0) + { + szTemp[nTempLen - 1] = 0; + strcat_s(szSymPath, nSymPathLen, szTemp); + strcat_s(szSymPath, nSymPathLen, ";"); + } + + // 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) + { + // locate the rightmost path separator + if ((*p == '\\') || (*p == '/') || (*p == ':')) + { + *p = 0; + break; + } + } // for (search for path separator...) + if (strlen(szTemp) > 0) + { + strcat_s(szSymPath, nSymPathLen, szTemp); + strcat_s(szSymPath, nSymPathLen, ";"); + } + } + if (GetEnvironmentVariableA("_NT_SYMBOL_PATH", szTemp, nTempLen) > 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; + strcat_s(szSymPath, nSymPathLen, szTemp); + strcat_s(szSymPath, nSymPathLen, ";"); + } + if (GetEnvironmentVariableA("SYSTEMROOT", szTemp, nTempLen) > 0) + { + szTemp[nTempLen - 1] = 0; + strcat_s(szSymPath, nSymPathLen, szTemp); + strcat_s(szSymPath, nSymPathLen, ";"); + // also add the "system32"-directory: + strcat_s(szTemp, nTempLen, "\\system32"); + strcat_s(szSymPath, nSymPathLen, szTemp); + strcat_s(szSymPath, nSymPathLen, ";"); + } + + if ((m_options & (int) SymBuildPath) != 0) + { + if (GetEnvironmentVariableA("SYSTEMDRIVE", szTemp, nTempLen) > 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;"); + } + } + + // First Init the whole stuff... + BOOL bRet = m_sw->Init(szSymPath); + if (szSymPath != NULL) + { + free(szSymPath); + // szSymPath = NULL; + } + if (bRet == FALSE) + { + this->OnDbgHelpErr("Error while initializing dbghelp.dll", 0, 0); + SetLastError(ERROR_DLL_INIT_FAILED); + return FALSE; + } + + bRet = m_sw->LoadModules(m_hProcess, m_dwProcessId); + if (bRet != FALSE) + m_modulesLoaded = TRUE; + return bRet; +} + + +// The following is used to pass the "userData"-Pointer to the user-provided readMemoryFunction +// This has to be done due to a problem with the "hProcess"-parameter in x64... +// Because this class is in no case multi-threading-enabled (because of the limitations +// of dbghelp.dll) it is "safe" to use a static-variable +static StackWalker::PReadProcessMemoryRoutine s_readMemoryFunction = NULL; +static LPVOID s_readMemoryFunction_UserData = NULL; + +#ifdef _MSC_VER + #pragma warning(disable : 4748) +#endif + +BOOL StackWalker::ShowCallstack(HANDLE hThread, const CONTEXT* context, PReadProcessMemoryRoutine readMemoryFunction, LPVOID pUserData) +{ + CONTEXT c;; + CallstackEntry csEntry; + IMAGEHLP_SYMBOL64* pSym = NULL; + StackWalkerInternal::IMAGEHLP_MODULE64_V2 Module; + IMAGEHLP_LINE64 Line; + int frameNum; + + if (m_modulesLoaded == FALSE) + { + OnOutput("************* Loaded Modules *************"); + this->LoadModules(); // ignore the result... + OnOutput("************* Callstack *************"); + } + + if (m_sw->m_hDbhHelp == NULL) + { + SetLastError(ERROR_DLL_INIT_FAILED); + return FALSE; + } + + s_readMemoryFunction = readMemoryFunction; + s_readMemoryFunction_UserData = pUserData; + + if (context == NULL) + { + // If no context is provided, capture the context + if (hThread == GetCurrentThread()) + GET_CURRENT_CONTEXT(c, USED_CONTEXT_FLAGS); + else + { + SuspendThread(hThread); + memset(&c, 0, sizeof(CONTEXT)); + c.ContextFlags = USED_CONTEXT_FLAGS; + if (GetThreadContext(hThread, &c) == FALSE) + { + ResumeThread(hThread); + return FALSE; + } + } + } + else + c = *context; + + // init STACKFRAME for first call + STACKFRAME64 s; // in/out stackframe + memset(&s, 0, sizeof(s)); + DWORD imageType; +#if defined(_M_IX86) + // normally, call ImageNtHeader() and use machine info from PE header + imageType = IMAGE_FILE_MACHINE_I386; + s.AddrPC.Offset = c.Eip; + s.AddrPC.Mode = AddrModeFlat; + s.AddrFrame.Offset = c.Ebp; + s.AddrFrame.Mode = AddrModeFlat; + s.AddrStack.Offset = c.Esp; + s.AddrStack.Mode = AddrModeFlat; +#elif defined(_M_X64) + imageType = IMAGE_FILE_MACHINE_AMD64; + s.AddrPC.Offset = c.Rip; + s.AddrPC.Mode = AddrModeFlat; + s.AddrFrame.Offset = c.Rsp; + s.AddrFrame.Mode = AddrModeFlat; + s.AddrStack.Offset = c.Rsp; + s.AddrStack.Mode = AddrModeFlat; +#elif defined(_M_IA64) + imageType = IMAGE_FILE_MACHINE_IA64; + s.AddrPC.Offset = c.StIIP; + s.AddrPC.Mode = AddrModeFlat; + s.AddrFrame.Offset = c.IntSp; + s.AddrFrame.Mode = AddrModeFlat; + s.AddrBStore.Offset = c.RsBSP; + s.AddrBStore.Mode = AddrModeFlat; + s.AddrStack.Offset = c.IntSp; + s.AddrStack.Mode = AddrModeFlat; +#else +#error "Platform not supported!" +#endif + + 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; + + memset(&Line, 0, sizeof(Line)); + Line.SizeOfStruct = sizeof(Line); + + memset(&Module, 0, sizeof(Module)); + Module.SizeOfStruct = sizeof(Module); + + 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)) + { + this->OnDbgHelpErr("StackWalk64", GetLastError(), s.AddrPC.Offset); + break; + } + + csEntry.offset = s.AddrPC.Offset; + csEntry.name[0] = 0; + csEntry.undName[0] = 0; + csEntry.undFullName[0] = 0; + csEntry.offsetFromSmybol = 0; + csEntry.offsetFromLine = 0; + csEntry.lineFileName[0] = 0; + csEntry.lineNumber = 0; + csEntry.loadedImageName[0] = 0; + csEntry.moduleName[0] = 0; + if (s.AddrPC.Offset == s.AddrReturn.Offset) + { + this->OnDbgHelpErr("StackWalk64-Endless-Callstack!", 0, s.AddrPC.Offset); + break; + } + if (s.AddrPC.Offset != 0) + { + // we seem to have a valid PC + // show procedure info (SymGetSymFromAddr64()) + if (m_sw->pSGSFA(m_hProcess, s.AddrPC.Offset, &(csEntry.offsetFromSmybol), pSym) != FALSE) + { + 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); + } + 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(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) + { + 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; +#endif + 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); + csEntry.baseOfImage = Module.BaseOfImage; + 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; + if (frameNum == 0) + et = firstEntry; + this->OnCallstackEntry(et, csEntry); + + if (s.AddrReturn.Offset == 0) + { + this->OnCallstackEntry(lastEntry, csEntry); + SetLastError(ERROR_SUCCESS); + break; + } + } // for ( frameNum ) + +cleanup: + if (pSym) + free(pSym); + + if (context == NULL) + ResumeThread(hThread); + + return TRUE; +} + +BOOL __stdcall StackWalker::myReadProcMem( + HANDLE hProcess, + DWORD64 qwBaseAddress, + PVOID lpBuffer, + DWORD nSize, + LPDWORD lpNumberOfBytesRead +) +{ + if (s_readMemoryFunction == NULL) + { + SIZE_T st; + BOOL bRet = ReadProcessMemory(hProcess, (LPVOID) qwBaseAddress, lpBuffer, nSize, &st); + *lpNumberOfBytesRead = (DWORD) st; + //printf("ReadMemory: hProcess: %p, baseAddr: %p, buffer: %p, size: %d, read: %d, result: %d\n", hProcess, (LPVOID) qwBaseAddress, lpBuffer, nSize, (DWORD) st, (DWORD) bRet); + 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) +{ + CHAR buffer[STACKWALK_MAX_NAMELEN]; + if (fileVersion == 0) + _snprintf_s(buffer, STACKWALK_MAX_NAMELEN, "%s:%s (%p), size: %d (result: %d), SymType: '%s', PDB: '%s'", img, mod, (LPVOID) baseAddr, size, result, symType, pdbName); + else + { + DWORD v4 = (DWORD) fileVersion & 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) +{ + CHAR buffer[STACKWALK_MAX_NAMELEN]; + if ((eType != lastEntry) && (entry.offset != 0)) + { + if (entry.name[0] == 0) + strcpy_s(entry.name, "(function-name not available)"); + if (entry.undName[0] != 0) + strcpy_s(entry.name, entry.undName); + if (entry.undFullName[0] != 0) + strcpy_s(entry.name, entry.undFullName); + if (entry.lineFileName[0] == 0) + { + strcpy_s(entry.lineFileName, "(filename not available)"); + if (entry.moduleName[0] == 0) + strcpy_s(entry.moduleName, "(module-name not available)"); + _snprintf_s(buffer, STACKWALK_MAX_NAMELEN, "%p (%s): %s: %s", (LPVOID) entry.offset, entry.moduleName, entry.lineFileName, entry.name); + } + else + _snprintf_s(buffer, STACKWALK_MAX_NAMELEN, "%s (%d): %s", entry.lineFileName, entry.lineNumber, entry.name); + OnOutput(buffer); + } +} + +void StackWalker::OnDbgHelpErr(LPCSTR /*szFuncName*/, DWORD /*gle*/, DWORD64 /*addr*/) +{ + //CHAR buffer[STACKWALK_MAX_NAMELEN]; + //_snprintf_s(buffer, STACKWALK_MAX_NAMELEN, "ERROR: %s, GetLastError: %d (Address: %p)", szFuncName, gle, (LPVOID) addr); + //OnOutput(buffer); +} + +void StackWalker::OnSymInit(LPCSTR szSearchPath, DWORD symOptions, LPCSTR szUserName) +{ + CHAR buffer[STACKWALK_MAX_NAMELEN]; + _snprintf_s(buffer, STACKWALK_MAX_NAMELEN, "SymInit: Symbol-SearchPath: '%s', symOptions: %d, UserName: '%s'", szSearchPath, symOptions, szUserName); + OnOutput(buffer); + // Also display the OS-version +#if _MSC_VER <= 1200 + OSVERSIONINFOA ver; + ZeroMemory(&ver, sizeof(OSVERSIONINFOA)); + ver.dwOSVersionInfoSize = sizeof(ver); + if (GetVersionExA(&ver) != FALSE) + { + _snprintf_s(buffer, STACKWALK_MAX_NAMELEN, "OS-Version: %d.%d.%d (%s)", + ver.dwMajorVersion, ver.dwMinorVersion, ver.dwBuildNumber, + ver.szCSDVersion); + OnOutput(buffer); + } +#else + OSVERSIONINFOEXA ver; + ZeroMemory(&ver, sizeof(OSVERSIONINFOEXA)); + ver.dwOSVersionInfoSize = sizeof(ver); +#pragma warning(suppress: 4996) + 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, + ver.szCSDVersion, ver.wSuiteMask, ver.wProductType); + OnOutput(buffer); + } +#endif +} + +void StackWalker::OnOutput(LPCSTR /*buffer*/) +{ + // OutputDebugStringA(buffer); +} +#endif diff --git a/extern/xspublic/xscommon/stackwalker.h b/extern/xspublic/xscommon/stackwalker.h new file mode 100644 index 0000000..002f439 --- /dev/null +++ b/extern/xspublic/xscommon/stackwalker.h @@ -0,0 +1,224 @@ + +// Copyright (c) 2003-2022 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. +// + +/********************************************************************** + + 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 + +#ifndef __GNUC__ + +// #pragma once is supported starting with _MCS_VER 1000, +// so we need not to check the version (because we only support _MSC_VER >= 1100)! +//#pragma once + +#include + +// 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 +#endif // _MSC_VER < 1300 + +class StackWalkerInternal; +/*! \brief A class that can help with walking the stack for debugging purposes (Windows version) */ +class StackWalker +{ +public: + enum StackWalkOptions + { + // No addition info will be retrived + // (only the address is available) + RetrieveNone = 0, + + // Try to get the symbol-name + RetrieveSymbol = 1, + + // Try to get the line for this symbol + RetrieveLine = 2, + + // Try to retrieve the module-infos + RetrieveModuleInfo = 4, + + // Also retrieve the version for the DLL/EXE + RetrieveFileVersion = 8, + + // Contains all the above + RetrieveVerbose = 0xF, + + // Generate a "good" symbol-search-path + SymBuildPath = 0x10, + + // Also use the public Microsoft-Symbol-Server + SymUseSymSrv = 0x20, + + // Contains all the abouve "Sym"-options + SymAll = 0x30, + + // Contains all options (default) + OptionsAll = 0x3F + }; + + StackWalker(int options = (int) OptionsAll, // 'int' is by design, to combine the enum-flags + 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" + ); + + 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 + ); + +protected: + enum { STACKWALK_MAX_NAMELEN = 1024 }; // max name length for found symbols + + /*! \brief Container for each Callstack-Entry */ + struct CallstackEntry + { + DWORD64 offset; // if 0, we have no valid entry + CHAR name[STACKWALK_MAX_NAMELEN]; + CHAR undName[STACKWALK_MAX_NAMELEN]; + CHAR undFullName[STACKWALK_MAX_NAMELEN]; + DWORD64 offsetFromSmybol; + DWORD offsetFromLine; + DWORD lineNumber; + CHAR lineFileName[STACKWALK_MAX_NAMELEN]; + DWORD symType; + LPCSTR symTypeString; + CHAR moduleName[STACKWALK_MAX_NAMELEN]; + DWORD64 baseOfImage; + CHAR loadedImageName[STACKWALK_MAX_NAMELEN]; + }; + + enum CallstackEntryType {firstEntry, nextEntry, lastEntry}; + + 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 OnDbgHelpErr(LPCSTR szFuncName, DWORD gle, DWORD64 addr); + virtual void OnOutput(LPCSTR szText); + + StackWalkerInternal* m_sw; + HANDLE m_hProcess; + DWORD m_dwProcessId; + BOOL m_modulesLoaded; + LPSTR m_szSymPath; + + int m_options; + + static BOOL __stdcall myReadProcMem(HANDLE hProcess, DWORD64 qwBaseAddress, PVOID lpBuffer, DWORD nSize, LPDWORD lpNumberOfBytesRead); + + friend StackWalkerInternal; + StackWalker(StackWalker const&) = delete; + StackWalker& operator = (StackWalker const&) = delete; +}; + +// The "ugly" assembler-implementation is needed for systems before XP +// If you have a new PSDK and you only compile for XP and later, then you can use +// the "RtlCaptureContext" +// Currently there is no define which determines the PSDK-Version... +// So we just use the compiler-version (and assumes that the PSDK is +// the one which was installed by the VS-IDE) + +// INFO: If you want, you can use the RtlCaptureContext if you only target XP and later... +// But I currently use it in x64/IA64 environments... +//#if defined(_M_IX86) && (_WIN32_WINNT <= 0x0500) && (_MSC_VER < 1400) + +#if defined(_M_IX86) +#ifdef CURRENT_THREAD_VIA_EXCEPTION +// 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) +#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) +#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) +#endif + +#endif + +#endif diff --git a/extern/xspublic/xscommon/stackwalker_linux.cpp b/extern/xspublic/xscommon/stackwalker_linux.cpp new file mode 100644 index 0000000..a78716e --- /dev/null +++ b/extern/xspublic/xscommon/stackwalker_linux.cpp @@ -0,0 +1,117 @@ + +// Copyright (c) 2003-2022 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 "stackwalker_linux.h" + +#include + +#ifdef ANDROID + #include +#elif defined(USING_UNWIND_LIB) + #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)" +#else + #include + #if UINTPTR_MAX == 0xffffffff + /* 32-bit */ + #define UNW_PRINTF_POINTER "0x%x: " + #define UNW_PRINTF_NAME_AND_POINTER " (%s+0x%x)" + #elif UINTPTR_MAX == 0xffffffffffffffff + /* 64-bit */ + #define UNW_PRINTF_POINTER "0x%lx: " + #define UNW_PRINTF_NAME_AND_POINTER " (%s+0x%lx)" + #else + /* wtf */ + #endif +#endif + +StackWalker::StackWalker() +{ +} + +StackWalker::~StackWalker() +{ +} + +void StackWalker::ShowCallstack() +{ +#ifdef ANDROID + __android_log_print(ANDROID_LOG_WARN, "stackwalker", "stack trace is not available"); +#elif defined(USING_UNWIND_LIB) + unw_context_t context; + unw_getcontext(&context); + + unw_cursor_t cursor; + unw_init_local(&cursor, &context); + + while (unw_step(&cursor) > 0) + { + unw_word_t instructionPointer; + unw_get_reg(&cursor, UNW_REG_IP, &instructionPointer); + + if (instructionPointer == 0) + break; + + static const int logLineSize = 256; + char logLine[logLineSize]; + int offset = std::snprintf(logLine, logLineSize, UNW_PRINTF_POINTER, instructionPointer); + + char symbol[logLineSize - 20]; + unw_word_t symbolOffset; + if (unw_get_proc_name(&cursor, symbol, sizeof(symbol), &symbolOffset) == 0) + { + int status; + char* demangled = abi::__cxa_demangle(symbol, nullptr, nullptr, &status); + + char* symbolName = symbol; + if (status == 0) + symbolName = demangled; + + std::snprintf(&logLine[offset], logLineSize - offset, UNW_PRINTF_NAME_AND_POINTER, symbolName, symbolOffset); + std::free(demangled); + } + else + std::snprintf(&logLine[offset], logLineSize - offset, " (unable to retrieve symbol name)"); + + OnOutput(logLine); + } +#else + OnOutput("Stack trace is not available"); +#endif +} diff --git a/extern/xspublic/xscommon/stackwalker_linux.h b/extern/xspublic/xscommon/stackwalker_linux.h new file mode 100644 index 0000000..a0e4570 --- /dev/null +++ b/extern/xspublic/xscommon/stackwalker_linux.h @@ -0,0 +1,51 @@ + +// Copyright (c) 2003-2022 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 STACKWALKER_LINUX_H +#define STACKWALKER_LINUX_H + +#include + +typedef const std::string& LPCSTR; + +/*! \brief A class that can help with walking the stack for debugging purposes (Linux version) */ +class StackWalker +{ +public: + StackWalker(); + virtual ~StackWalker(); + + void ShowCallstack(); + virtual void OnOutput(LPCSTR) = 0; +}; + +#endif diff --git a/extern/xspublic/xscommon/threading.cpp b/extern/xspublic/xscommon/threading.cpp new file mode 100644 index 0000000..bba7284 --- /dev/null +++ b/extern/xspublic/xscommon/threading.cpp @@ -0,0 +1,1155 @@ + +// Copyright (c) 2003-2022 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 "threading.h" +#include +#include +#include + +#ifdef __GNUC__ + #include + #include + #include + #include + #define _strdup strdup +#endif + +namespace xsens +{ +#ifdef __GNUC__ +pthread_t xsStartThread(void* (func)(void*), void* param, void* pid) +{ + return ::xsStartThread(func, param, pid); +} +#endif + +/*! Constructor, creates the necessary signals. Does NOT start the thread. +*/ +StandardThread::StandardThread() + : m_thread(XSENS_INVALID_THREAD) + , m_priority(XS_THREAD_PRIORITY_NORMAL) + , m_stop(false) + , m_yieldOnZeroSleep(true) +#ifdef _WIN32 + , m_stopHandle(::CreateEvent(NULL, TRUE, FALSE, NULL)) + , m_running(::CreateEvent(NULL, TRUE, FALSE, NULL)) + , m_threadId(0) +#else + , m_running(false) +#endif + , m_name(NULL) +{ +#ifndef _WIN32 + pthread_attr_init(&m_attr); +#endif +} + +/*! Destructor, stops the thread if it was running and cleans up afterwards. +*/ +StandardThread::~StandardThread() +{ + stopThread(); + + if (m_name) + { + free(m_name); + m_name = NULL; + } + +#ifdef _WIN32 + ::CloseHandle(m_running); + ::CloseHandle(m_stopHandle); +#else + pthread_attr_destroy(&m_attr); +#endif +} + +#ifdef _WIN32 +/*! \brief Kill the thread now */ +void StandardThread::terminateThread() +{ + if (m_thread != XSENS_INVALID_THREAD) + ::TerminateThread(m_thread, DWORD(-1)); + m_thread = XSENS_INVALID_THREAD; +} +#endif + +/*! \returns True if the thread is alive +*/ +bool StandardThread::isAlive(void) volatile const noexcept +{ + if (m_thread == XSENS_INVALID_THREAD) + return false; + +#ifdef _WIN32 + DWORD exitCode; + if (::GetExitCodeThread(m_thread, &exitCode)) + { + if (exitCode == STILL_ACTIVE) + return true; + } + return false; +#else + return (pthread_kill(m_thread, 0) == 0); +#endif +} + +/*! \brief Returns whether the thread is currently running. +*/ +bool StandardThread::isRunning(void) volatile const noexcept +{ + if (!isAlive()) + return false; +#ifdef _WIN32 + switch (::WaitForSingleObject(m_running, 0)) + { + case WAIT_ABANDONED: + case WAIT_TIMEOUT: + return false; + case WAIT_OBJECT_0: + return true; + default: + return false; + } +#else + return m_running; +#endif +} + +/*! \brief Returns whether the thread should (have) terminate(d) +*/ +bool StandardThread::isTerminating(void) volatile const noexcept +{ + return m_stop; +} + +/*! \brief Sets the priority of the thread + \param pri The thread priority to set + \return True if successful +*/ +bool StandardThread::setPriority(XsThreadPriority pri) +{ + m_priority = pri; + + if (!isAlive()) + return false; + +#ifdef _WIN32 + ::SetThreadPriority(m_thread, (int) pri); +#else +#ifdef _POSIX_PRIORITY_SCHEDULING + int32_t rv; + int32_t policy; + struct sched_param param; + + if (!isAlive()) + return false; + + 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; + } + + 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); + // 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); + + 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); + + param.sched_priority = (int32_t)(min_prio + (pri * priostep)); + break; + } + + rv = pthread_setschedparam(m_thread, policy, ¶m); + switch (rv) + { + 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. + */ + return false; + default: + break; + } + + return true; +#else + return true; +#endif +#endif + return true; +} + +/*! \brief Starts the thread + \param name The name of the thread as shown in the debugger, may be NULL in which case the system determines the name. + \returns True if successful +*/ +bool StandardThread::startThread(const char* name) +{ + if (isAlive()) + return false; + + if (m_name) + free(m_name); + if (name) + m_name = _strdup(name); + else + m_name = NULL; + +#ifdef _WIN32 + m_stop = false; + ::ResetEvent(m_stopHandle); + ::SetEvent(m_running); + m_thread = xsStartThread(&threadInit, this, &m_threadId); + if (m_thread == XSENS_INVALID_THREAD) + { + ::ResetEvent(m_running); + return false; + } +#else + m_stop = false; + m_running = true; + if (pthread_create(&m_thread, &m_attr, threadInit, this)) + { + /* something went wrong */ + m_thread = XSENS_INVALID_THREAD; + return false; + } +#endif + xsSetThreadPriority(m_thread, m_priority); + + return true; +} + +/*! \brief Tells the thread to stop but does not wait for it to end. +*/ +void StandardThread::signalStopThread(void) +{ +#ifdef _WIN32 + m_stop = true; + ::SetEvent(m_stopHandle); + ::SetThreadPriority(m_thread, THREAD_PRIORITY_ABOVE_NORMAL); +#else + setPriority(XS_THREAD_PRIORITY_HIGHEST); + m_stop = true; +#endif +} + +/*! \brief Tells the thread to stop and waits for it to end. +*/ +void StandardThread::stopThread(void) noexcept +{ +#ifdef _WIN32 + // prevent multiple threads from doing this at the same time on windows, pthreads deals with this in its own special way + xsens::Lock locky(&m_mux, false); + if (m_threadId == xsGetCurrentThreadId()) + locky.tryLock(); + else + locky.lock(); +#endif + + if (!isAlive()) + return; + + signalStopThread(); +#ifdef _WIN32 + // don't wait for my thread to end myself + if (m_threadId == xsGetCurrentThreadId()) + return; + + // in debug mode we ALWAYS want to know if there's some kind of deadlock occurring + //#if XSENS_THREAD_KILL_TIMEOUT_MS > 0 && !defined(XSENS_DEBUG) + // XsTimeStamp endOfWait = XsTimeStamp::now() + XSENS_THREAD_KILL_TIMEOUT_MS; + // while(isAlive() && (XsTimeStamp::now() < endOfWait)) + // xsYield(); + //#else + + while (isAlive()) + xsYield(); + //#endif + if (m_thread != XSENS_INVALID_THREAD && ::CloseHandle(m_thread) == 0) + { + DWORD lastErr = GetLastError(); + (void)lastErr; + return; + } +#else + // don't wait for my thread to end myself + if (pthread_equal(m_thread, xsGetCurrentThreadId())) + return; + + // in debug mode we ALWAYS want to know if there's some kind of deadlock occurring + //#if XSENS_THREAD_KILL_TIMEOUT_MS > 0 && !defined(XSENS_DEBUG) + // XsTimeStamp endOfWait = XsTimeStamp::now() + XSENS_THREAD_KILL_TIMEOUT_MS; + // while(isAlive() && (XsTimeStamp::now() < endOfWait)) + // xsYield(); + + // if (isAlive()) { + // if (pthread_kill(m_thread, 9)) + // return; + // } + //#else + int rv = 0; + while (true) + { + rv = pthread_tryjoin_np(m_thread, NULL); + if (rv == 0) + { + break; + } + if (errno == EBUSY) + { + xsYield(); + } + else + { + // Some other error--maybe the thread is invalid? + break; + } + } + //#endif + if (rv != 0) + { + switch (errno) + { + case EINVAL: + /* no joinable thread found */ + case ESRCH: + /* no thread fits thread id */ + case EDEADLK: + /* deadlock or trying to join self */ + default: + break; + } + } + + m_running = false; +#endif + m_thread = XSENS_INVALID_THREAD; +} + +XSENS_THREAD_RETURN StandardThread::threadInit(void* obj) +{ + StandardThread* thread = reinterpret_cast(obj); + if (thread->m_name) + xsNameThisThread(thread->m_name); + + thread->threadMain(); + return 0; +} + +#ifndef _WIN32 +/*! \brief Cleanup the thread by calling the exit function */ +void StandardThread::threadCleanup(void* obj) +{ + ((StandardThread*)obj)->exitFunction(); +} +#endif + +/*! \brief The inner loop of the thread, calls innerFunction repeatedly and sleeps when necessary +*/ +void StandardThread::threadMain(void) +{ + initFunction(); + do + { + int32_t rv = innerFunction(); + if (rv > 0) + { + int64_t sleepStart = XsTimeStamp::nowMs(); + while (!m_stop) + { + // sleep max 100ms at a time so we can terminate the thread quickly if necessary + int64_t timePassed = XsTimeStamp::nowMs() - sleepStart; + int32_t remaining = rv - (int32_t) timePassed; + if (remaining > 100) + XsTime::msleep(100); + else if (remaining <= 0) + break; + else + XsTime::msleep((uint32_t) remaining); + } + } + else if (m_yieldOnZeroSleep && !m_stop) + xsYield(); + } while (!m_stop); + exitFunction(); +} + +////////////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////// + +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)) +#else + , m_running(false) + , m_reset(false) + , m_stop(false) +#endif + , m_timeout(10000) + , m_func(func) + , m_param(param) + , m_name(NULL) + , m_threadId(0) +{ +#ifndef _WIN32 + pthread_attr_init(&m_attr); +#endif +} + +WatchDogThread::~WatchDogThread() +{ + stopTimer(); + + if (m_name) + { + free(m_name); + m_name = NULL; + } + +#ifdef _WIN32 + ::CloseHandle(m_reset); + ::CloseHandle(m_running); + ::CloseHandle(m_stop); +#else + pthread_attr_destroy(&m_attr); +#endif +} + +bool WatchDogThread::isAlive(void) volatile const noexcept +{ +#ifdef _WIN32 + DWORD exitCode; + if (::GetExitCodeThread(m_thread, &exitCode)) + return (exitCode == STILL_ACTIVE); +#else + if (m_thread == XSENS_INVALID_THREAD) + return false; + + return (pthread_kill(m_thread, 0) == 0); +#endif + return false; +} + +bool WatchDogThread::isRunning(void) volatile const noexcept +{ + if (!isAlive()) + return false; +#ifdef _WIN32 + 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; + default: + return false; + } + default: + return false; + } +#else + return m_running; +#endif +} + +/*! \brief Starts a timer using some parameters + \param timeout The timeout value to use + \param name The name of a thread to set + \returns True if successful +*/ +bool WatchDogThread::startTimer(uint32_t timeout, const char* name) +{ + if (isAlive()) + return false; + + if (timeout != 0) + m_timeout = timeout; + + if (m_name) + free(m_name); + if (name) + m_name = _strdup(name); + else + m_name = NULL; + +#ifdef _WIN32 + ::ResetEvent(m_stop); + ::SetEvent(m_running); + ::ResetEvent(m_reset); + m_thread = xsStartThread(&threadInit, this, &m_threadId); + if (m_thread == XSENS_INVALID_THREAD) + { + ::ResetEvent(m_running); + return false; + } + +#else + m_running = true; + m_reset = false; + m_stop = false; + + if (pthread_create(&m_thread, &m_attr, threadInit, this) != 0) + return false; +#endif + return true; +} + +/*! \brief Stops the timer + \returns True if successful +*/ +bool WatchDogThread::stopTimer(void) noexcept +{ + if (!isAlive()) + return true; + +#ifdef _WIN32 + ::SetEvent(m_stop); + ::SetThreadPriority(m_thread, THREAD_PRIORITY_ABOVE_NORMAL); + + while (isAlive()) + xsYield(); + if (::CloseHandle(m_thread) == 0) + { + DWORD lastErr = GetLastError(); + (void)lastErr; + return false; + } +#else + int rv; + int32_t policy; + struct sched_param param; + + m_stop = true; + rv = pthread_getschedparam(m_thread, &policy, ¶m); + if (rv) + { + switch (errno) + { + case ESRCH: + return false; + default: + break; + } + } + param.sched_priority = sched_get_priority_max(policy); + + rv = pthread_setschedparam(m_thread, policy, ¶m); + if (rv) + { + switch (errno) + { + case ESRCH: + case EINVAL: + case ENOTSUP: + case EPERM: + return false; + default: + break; + } + } + + rv = pthread_join(m_thread, NULL); + if (rv) + { + switch (errno) + { + case EINVAL: + case ESRCH: + case EDEADLK: + default: + break; + } + } + m_running = false; + m_thread = XSENS_INVALID_THREAD; +#endif + m_thread = XSENS_INVALID_THREAD; + return true; +} + +XSENS_THREAD_RETURN WatchDogThread::threadInit(void* obj) +{ + WatchDogThread* thread = reinterpret_cast(obj); + if (thread->m_name) + xsNameThisThread(thread->m_name); + + thread->threadMain(); + return 0; +} + +void WatchDogThread::threadMain(void) +{ + XsTimeStamp toTime((int64_t)(XsTimeStamp::now().msTime() + m_timeout)); + +#ifdef _WIN32 + HANDLE hlist[2]; +#endif + bool go = true; + while (go) + { +#ifdef _WIN32 + 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)) + { + 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; + } +#else + Lock lock(&m_mutex, true); + lock.unlock(); + + uint32_t timeout; + + while (go) + { + timeout = static_cast((toTime - XsTimeStamp::now()).msTime()); + if (timeout > m_timeout) + break; + + lock.lock(); + if (m_reset) + { + toTime = XsTimeStamp((XsTimeStamp::now().msTime() + m_timeout)); + m_reset = false; + } + lock.unlock(); + + if (m_stop) + go = false; + } +#endif + break; + } + if (go) + m_func(m_param); +} + +/*! \brief Resets the timer and sets a timeout + \param timeout The timeout value to set + \returns True if successful +*/ +bool WatchDogThread::resetTimer(uint32_t timeout) +{ + if (!isRunning()) + return false; + +#ifdef _WIN32 + if (timeout != 0) + m_timeout = timeout; + ::SetEvent(m_reset); +#else + Lock lock(&m_mutex); + + m_timeout = timeout; + m_reset = true; +#endif + return true; +} + +#ifdef _WIN32 +Semaphore::Semaphore(int32_t initVal, uint32_t nofOtherHandles, HANDLE* otherHandles) +{ + m_nofHandles = nofOtherHandles + 1; + m_handleList = new HANDLE[m_nofHandles]; + for (uint32_t i = 0; i < m_nofHandles - 1; i++) + m_handleList[i] = otherHandles[i]; + m_handleList[m_nofHandles - 1] = CreateSemaphore(NULL, initVal, 0x7fffffff, NULL); +} + +Semaphore::~Semaphore(void) +{ + post(); + CloseHandle(m_handleList[m_nofHandles - 1]); + delete[] m_handleList; +} + +bool Semaphore::wait1() +{ + return wait1(static_cast(-1)); +} + +bool Semaphore::wait1(uint32_t timeout) +{ + for (;;) // loop + timeout for debugging only + { + DWORD r = WaitForMultipleObjects(m_nofHandles, m_handleList, FALSE, timeout); + if (r == WAIT_TIMEOUT) + return false; + if (r < WAIT_OBJECT_0 + m_nofHandles) + return r - WAIT_OBJECT_0 == m_nofHandles - 1; + if (r != WAIT_TIMEOUT) + return false; + xsYield(); // prevent race condition + } +} + +int32_t Semaphore::post(int32_t increment) noexcept +{ + LONG prev; + ReleaseSemaphore(m_handleList[m_nofHandles - 1], increment, &prev); + return (int32_t) prev; +} + +#else // _WIN32 + +Semaphore::Semaphore(int32_t initVal, uint32_t, sem_t*) : + m_semname(nullptr), + m_handle(SEM_FAILED) +{ +#if defined(SEM_VALUE_MAX) && INT32_MAX > SEM_VALUE_MAX + if (initVal > SEM_VALUE_MAX) + initVal = SEM_VALUE_MAX; +#endif + + uint64_t id = (uint64_t)this; + char semname[20]; + + while (true) + { + sprintf(semname, "%" PRINTF_INT64_MODIFIER "x", id); + m_semname = strdup(semname); + m_handle = sem_open(semname, O_EXCL | O_CREAT, S_IRWXU, initVal); + if (m_handle != SEM_FAILED) + break; + + if (errno != EEXIST) + { + perror("opening semaphore"); + exit(-1); + } + + id++; + free(m_semname); + } +} + +Semaphore::~Semaphore(void) +{ + post(); + sem_unlink(m_semname); + free(m_semname); +} + +bool Semaphore::wait1() +{ + return wait1(static_cast(-1)); +} + +bool Semaphore::wait1(uint32_t ms) +{ + if (ms == UINT32_MAX) + return sem_wait(m_handle); + +#if _POSIX_C_SOURCE >= 200112L || _XOPEN_SOURCE >= 600 + timespec ts; + + clock_gettime(CLOCK_REALTIME, &ts); + + int64_t s = ms / 1000; + ts.tv_sec += s; + + ms -= s * 1000; + int64_t ns = ts.tv_nsec + (ms * 1000000); + + ts.tv_sec += ns / 1000000000L; + ts.tv_nsec = ns % 1000000000L; + + return sem_timedwait(m_handle, &ts) == 0; +#else + XsTimeStamp end = XsTimeStamp::now() + XsTimeStamp(int64_t(ms)); + + while (XsTimeStamp::now() < end) + { + if (sem_trywait(m_handle) == 0) + return true; + else + xsYield(); + } + + return false; +#endif +} + +#ifndef __APPLE__ +int32_t Semaphore::post(int32_t increment) throw() +{ + int prev; + sem_getvalue(m_handle, &prev); + for (int i = 0; i < increment; i++) + sem_post(m_handle); + return (int32_t)prev; +} +#else +int32_t Semaphore::post(int32_t increment) throw() +{ + int prev = 0; + for (int i = 0; i < increment; i++) + sem_post(m_handle); + return (int32_t)prev; +} +#endif +#endif // _WIN32 + +#ifdef __APPLE__ +/*! \brief An implementation of linux' clock_gettime() + + clock_gettime() is not available on Apple/Darwin platforms. This function helps + maintaining compatibility with Linux code. + + This comes straight from xstime. We should probably move thread primitives into xstypes. +*/ +#ifndef CLOCK_REALTIME + #define CLOCK_REALTIME 0 +#endif +static int clock_gettime(int clk_id, struct timespec* tp) +{ + (void)clk_id; + struct timeval now; + + int rv = gettimeofday(&now, NULL); + if (rv != 0) + return rv; + + tp->tv_sec = now.tv_sec; + tp->tv_nsec = now.tv_usec * 1000; + + return 0; +} +#endif + +/*! \brief Create a wait condition */ +WaitCondition::WaitCondition(Mutex& m) : m_mutex(m) +{ +#ifdef _WIN32 + InitializeConditionVariable(&m_cond); +#else + pthread_condattr_init(&m_condattr); +#if !defined(__APPLE__) && (defined(_POSIX_CLOCK_SELECTION) || defined(_SC_CLOCK_SELECTION)) && !defined(ANDROID) + m_clockId = CLOCK_MONOTONIC; + if (pthread_condattr_setclock(&m_condattr, m_clockId) != 0) + pthread_condattr_getclock(&m_condattr, &m_clockId); +#else + /* we'll fall back to the wallclock, but we may be influenced by + time keeping software (ntp, date), which doesn't happen with + the monotonic clock + */ + m_clockId = CLOCK_REALTIME; +#endif + pthread_cond_init(&m_cond, &m_condattr); +#endif +} + +/*! \brief Destroy the wait condition */ +WaitCondition::~WaitCondition() +{ + try + { + broadcast(); +#ifndef _WIN32 + pthread_cond_destroy(&m_cond); + pthread_condattr_destroy(&m_condattr); +#endif + } + catch (...) + { + } +} + +/*! \brief Unblock a single waiting thread */ +void WaitCondition::signal() +{ +#ifdef _WIN32 + WakeConditionVariable(&m_cond); +#else + pthread_cond_signal(&m_cond); +#endif +} + +/*! \brief Unblock all waiting threads */ +void WaitCondition::broadcast() +{ +#ifdef _WIN32 + WakeAllConditionVariable(&m_cond); +#else + pthread_cond_broadcast(&m_cond); +#endif +} + +/*! \brief Wait until we're signalled to continue + + \details Before calling this function, it is required that the mutex + provided during construction is _locked_. This function unlocks the mutex + internally, and returns with the mutex locked again. + + \return true if we were signalled, false otherwise +*/ +bool WaitCondition::wait() +{ +#ifdef _WIN32 + return SleepConditionVariableCS(&m_cond, &m_mutex.m_mutex, INFINITE) != 0; +#else + return pthread_cond_wait(&m_cond, &m_mutex.m_mutex) == 0; +#endif +} + +/*! \brief Wait until we're signalled to continue, or until timeout [ms] has passed + + \details Before calling this function, it is required that the mutex + provided during construction is _locked_. This function unlocks the mutex + internally, and returns with the mutex locked again. + + \param timeout The timeout value in ms + + \return true if we were signalled, false otherwise +*/ +bool WaitCondition::wait(uint32_t timeout) +{ +#ifdef _WIN32 + return SleepConditionVariableCS(&m_cond, &m_mutex.m_mutex, timeout) != 0; +#else + static const int64_t NANOS_PER_MILLI = 1000000; + static const int64_t NANOS_PER_ONE = NANOS_PER_MILLI * 1000; + + struct timespec time; + int64_t nsec; + + clock_gettime(m_clockId, &time); + + nsec = time.tv_nsec + timeout * NANOS_PER_MILLI; + time.tv_nsec = nsec % NANOS_PER_ONE; + time.tv_sec += nsec / NANOS_PER_ONE; + + return pthread_cond_timedwait(&m_cond, &m_mutex.m_mutex, &time) == 0; +#endif +} + +#if defined(_WIN32) +/*! \brief Constructor, initializes the event in the reset state */ +WaitEvent::WaitEvent() + : m_waiterCount(0) + , m_terminating(false) +{ + m_event = ::CreateEventA(NULL, TRUE, FALSE, NULL); +} + +/*! \brief Destructor, terminates the event and waits for waiting threads to be notified before finishing */ +WaitEvent::~WaitEvent() +{ + try + { + terminate(); + } + catch (...) + {} + assert(m_waiterCount == 0); + ::CloseHandle(m_event); + m_event = INVALID_HANDLE_VALUE; +} + +/*! \brief Wait for the event to be set or object termination + \return true if the event is set and the object is not being terminated +*/ +bool WaitEvent::wait() +{ + if (m_terminating) + return false; + + ++m_waiterCount; + + switch (::WaitForSingleObject(m_event, INFINITE)) + { + case WAIT_ABANDONED: + case WAIT_TIMEOUT: + --m_waiterCount; + return false; + case WAIT_OBJECT_0: + --m_waiterCount; + return !m_terminating; + default: + --m_waiterCount; + return false; + } +} + +/*! \brief Set the event. + \details Waiting items will (all) be notified and allowed to run +*/ +void WaitEvent::set() +{ + ::SetEvent(m_event); +} + +/*! \brief Reset the event. + \details Any wait() call will block until set() or terminate() is called. + \note After terminate() has been called, reset() will not reset the event anymore +*/ +void WaitEvent::reset() +{ + if (!m_terminating) + ::ResetEvent(m_event); +} +#else +WaitEvent::WaitEvent() + : m_waiterCount(0) + , m_terminating(false) +{ + pthread_mutex_init(&m_mutex, 0); + pthread_cond_init(&m_cond, 0); + m_triggered = false; +} + +WaitEvent::~WaitEvent() +{ + terminate(); + pthread_cond_destroy(&m_cond); + pthread_mutex_destroy(&m_mutex); +} + +/*! \brief Wait for the event to be set or object termination + \return true if the event is set and the object is not being terminated +*/ +bool WaitEvent::wait() +{ + if (m_terminating) + return false; + + ++m_waiterCount; + pthread_mutex_lock(&m_mutex); + while (!m_triggered && !m_terminating) + pthread_cond_wait(&m_cond, &m_mutex); + pthread_mutex_unlock(&m_mutex); + --m_waiterCount; + return !m_terminating; +} + +/*! \brief Set the event. + \details Waiting items will (all) be notified and allowed to run +*/ +void WaitEvent::set() +{ + pthread_mutex_lock(&m_mutex); + m_triggered = true; + pthread_cond_signal(&m_cond); + pthread_mutex_unlock(&m_mutex); +} + +/*! \brief Reset the event. + \details Any wait() call will block until set() or terminate() is called. + \note After terminate() has been called, reset() will not reset the event anymore +*/ +void WaitEvent::reset() +{ + if (!m_terminating) + { + pthread_mutex_lock(&m_mutex); + m_triggered = false; + pthread_mutex_unlock(&m_mutex); + } +} + +#endif + +/*! \brief Terminates the thread +*/ +void WaitEvent::terminate() +{ + m_terminating = true; + set(); + + while (m_waiterCount > 0) + XsTime_msleep(2); +} + +} // namespace xsens diff --git a/extern/xspublic/xscommon/threading.h b/extern/xspublic/xscommon/threading.h new file mode 100644 index 0000000..50b3b79 --- /dev/null +++ b/extern/xspublic/xscommon/threading.h @@ -0,0 +1,299 @@ + +// Copyright (c) 2003-2022 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 THREADING_H +#define THREADING_H + +#include "xsens_mutex.h" +#include + +#ifndef __GNUC__ + #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; +#ifdef _WIN32 + 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. +#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; +#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: +#endif + char* m_name; + static XSENS_THREAD_RETURN threadInit(void* obj); +#ifndef _WIN32 + static void threadCleanup(void* obj); +#endif + void threadMain(void); +protected: + //! Virtual initialization function + virtual void initFunction(void) { } + + //! Virtual exit function + virtual void exitFunction(void) { } + + //! 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(); + + 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 + { +#ifdef _WIN32 + return m_threadId; +#else + return m_thread; +#endif + } +#ifdef _WIN32 + void terminateThread(); +#endif +}; +#define XSENS_THREAD_CHECK() if (isTerminating()) return 0 + +#ifdef ANDROID + #define CDECL_XS +#else + #define CDECL_XS __cdecl +#endif + +#ifndef SWIG +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; +#ifdef _WIN32 + 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; +#endif + 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: + + /*! \brief Constructor + */ + WatchDogThread(WatchDogFunction func, void* param = NULL); + + /*! \brief Destructor + */ + ~WatchDogThread(); + + 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; + } +}; + +/*! \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: + + /*! \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() + { + 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); + } + + //! \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(); + } +}; +#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) +#endif + +#endif diff --git a/extern/xspublic/xscommon/xbus.c b/extern/xspublic/xscommon/xbus.c new file mode 100644 index 0000000..298d29d --- /dev/null +++ b/extern/xspublic/xscommon/xbus.c @@ -0,0 +1,166 @@ + +// Copyright (c) 2003-2022 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/extern/xspublic/xscommon/xbus.h b/extern/xspublic/xscommon/xbus.h new file mode 100644 index 0000000..fad6f7c --- /dev/null +++ b/extern/xspublic/xscommon/xbus.h @@ -0,0 +1,90 @@ + +// Copyright (c) 2003-2022 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/extern/xspublic/xscommon/xbusparser.c b/extern/xspublic/xscommon/xbusparser.c new file mode 100644 index 0000000..d047452 --- /dev/null +++ b/extern/xspublic/xscommon/xbusparser.c @@ -0,0 +1,160 @@ + +// Copyright (c) 2003-2022 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/extern/xspublic/xscommon/xbusparser.h b/extern/xspublic/xscommon/xbusparser.h new file mode 100644 index 0000000..e66e456 --- /dev/null +++ b/extern/xspublic/xscommon/xbusparser.h @@ -0,0 +1,87 @@ + +// Copyright (c) 2003-2022 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/extern/xspublic/xscommon/xprintf.cpp b/extern/xspublic/xscommon/xprintf.cpp new file mode 100644 index 0000000..58a2412 --- /dev/null +++ b/extern/xspublic/xscommon/xprintf.cpp @@ -0,0 +1,93 @@ + +// Copyright (c) 2003-2022 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 "xprintf.h" +#include +#include + +#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 ap The variadic arguments, properly initialized with va_start. The function will call va_end on this argument before exiting. +*/ +inline std::string _xprintf_ap(char const* fmt_str, va_list ap) +{ + std::string rv; + + // 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; +} + +/*! \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); +} + +/*! \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/extern/xspublic/xscommon/xprintf.h b/extern/xspublic/xscommon/xprintf.h new file mode 100644 index 0000000..b1fa197 --- /dev/null +++ b/extern/xspublic/xscommon/xprintf.h @@ -0,0 +1,46 @@ + +// Copyright (c) 2003-2022 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 XPRINTF_H +#define XPRINTF_H + +#ifdef __cplusplus + #include + + 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/extern/xspublic/xscommon/xscommon_config.h b/extern/xspublic/xscommon/xscommon_config.h new file mode 100644 index 0000000..cd5a8f2 --- /dev/null +++ b/extern/xspublic/xscommon/xscommon_config.h @@ -0,0 +1,54 @@ + +// Copyright (c) 2003-2022 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. +// + +#define XSNOEXPORT +#define XSENS_NO_AUTOLIB +// include this file in Visual Studio using C/C++->Advanced->Force Includes (the /FI option) +#ifndef CONFIG_H +#define CONFIG_H + +#ifndef UNICODE + #define UNICODE +#endif +#ifndef _UNICODE + #define _UNICODE +#endif + +#include + +////////////////////////////////////////////////// +// generic preprocessor defines + +//! Set to 0 to disable threading support (ie for in an embedded environment), 1 is enabled +#define JOURNALLER_WITH_THREAD_SUPPORT 1 + +#endif diff --git a/extern/xspublic/xscommon/xsens_debugtools.h b/extern/xspublic/xscommon/xsens_debugtools.h new file mode 100644 index 0000000..6c86896 --- /dev/null +++ b/extern/xspublic/xscommon/xsens_debugtools.h @@ -0,0 +1,55 @@ + +// Copyright (c) 2003-2022 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 XSENS_DEBUGTOOLS_H +#define XSENS_DEBUGTOOLS_H + +#ifndef XSTRINGIFY + #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)); + #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) +#endif +#define XSENS_COMPILER_WARNING0(w) XSENS_COMPILER_WARNING("X0000", w) + +#endif diff --git a/extern/xspublic/xscommon/xsens_generic_matrix.h b/extern/xspublic/xscommon/xsens_generic_matrix.h new file mode 100644 index 0000000..d321143 --- /dev/null +++ b/extern/xspublic/xscommon/xsens_generic_matrix.h @@ -0,0 +1,360 @@ + +// Copyright (c) 2003-2022 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 XSENS_GENERIC_MATRIX_H +#define XSENS_GENERIC_MATRIX_H + +#include "xsens_math_throw.h" + +#ifndef XSENS_MATH_FIRMWARE + #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 +#else + #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 +#endif + +#ifndef _PSTDINT_H_INCLUDED + #include +#endif + +#include +#include + +#ifdef __cplusplus +namespace xsens +{ +namespace +{ +/*! \brief Swap the values of two objects of a basic type T through assignment */ +template +inline void swapb(T& a, T& b) +{ + T temp = a; + a = b; + b = temp; +} +} + +/*! \brief Generic matrix class + + This class can store items of the given type. + Items in the list can be accessed through the [] operator or the get() function. + + Do NOT use any item type that requires a constructor to work correctly. Pointers to these + objects can work though. +*/ +template +class GenericMatrix +{ +protected: + T* m_data; //!< The array containing the items. + uint32_t m_rows; //!< The number of rows currently in the matrix. + uint32_t m_cols; //!< The number of columns currently in the matrix. + uint32_t m_allocSize; //!< The number of elemens currently allocated for the matrix. + int m_flags; //!< Flags for data management. + +public: + //! Standard constructor, creates a 1x1 matrix. + GenericMatrix(); + //! 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 + GenericMatrix(const GenericMatrix& src); + //! 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. + 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. + 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 + 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. + 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 + GenericMatrix& operator = (const GenericMatrix& src); + //! 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 + void zeroValues(); +}; + +template GenericMatrix::GenericMatrix() + : m_rows(1) + , m_cols(1) + , m_flags(XSDF_Managed) +{ + m_data = (T*)malloc(m_rows * m_cols * sizeof(T)); + + if (!m_data) + XSENS_THROW_BAD_ALLOC; + + m_allocSize = m_rows * m_cols; +} + +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); + + if (!m_data) + XSENS_THROW_BAD_ALLOC; + + m_allocSize = m_rows * m_cols; + + if (zeroVals) + zeroValues(); +} + +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); + + if (!m_data) + XSENS_THROW_BAD_ALLOC; + + m_allocSize = 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) + : m_rows(newRows) + , m_cols(newCols) + , m_flags(XSDF_Managed) +{ + 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; + + 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) + : m_rows(newRows) + , m_cols(newCols) + , m_flags(flags) +{ + if (m_flags & XSDF_FixedSize) + { + m_data = buffer; + + if (size() == 0) + m_flags |= XSDF_Empty; + } + + if (m_flags & XSDF_Managed) + 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; + + if (m_flags & XSDF_Managed) + memcpy(m_data, buffer, sizeof(T) * (size_t)m_rows * m_cols); +} + +template GenericMatrix::~GenericMatrix() +{ + if (m_flags & XSDF_Managed) + free(m_data); + + if (!(m_flags & XSDF_FixedSize)) + m_data = NULL; + else + m_flags |= XSDF_Empty; +} + +template +void GenericMatrix::setSize(uint32_t newRows, uint32_t newCols, bool zeroVals) +{ + if (newRows == m_rows && newCols == m_cols) + { + if (zeroVals) + zeroValues(); + + return; + } + +#ifdef XSENS_MATH_RESTRICT_RESIZE + if (m_data && !(m_flags & XSDF_Managed)) + XM_THROW("resizing of this object is not allowed"); +#endif + + const uint32_t newSize = newRows * newCols; + + if (m_flags & XSDF_FixedSize) + { + if (newSize == 0) + { + m_flags |= XSDF_Empty; + m_rows = newRows; + m_cols = newCols; + return; + } + + assert(newSize <= size()); + m_flags &= ~XSDF_Empty; + } + + if (newSize > m_allocSize || newSize == 0) + { + if (m_flags & XSDF_Managed) + { + free(m_data); + m_data = 0; + m_allocSize = 0; + } + + if (newSize > 0) + { + m_data = (T*)malloc(sizeof(T) * (size_t)newRows * newCols); + m_flags = XSDF_Managed; + m_allocSize = newRows * newCols; + } + } + + m_rows = newRows; + m_cols = newCols; + + if (!m_data) + XSENS_THROW_BAD_ALLOC; + + if (zeroVals) + zeroValues(); +} + +template +T& GenericMatrix::get(uint32_t row, uint32_t col) const XSENS_GENERIC_MATRIX_THROW +{ +#ifdef XSENS_GENERIC_MATRIX_RANGE_CHECKS + if (row >= m_rows || col >= m_cols) + XM_THROW("index out of bounds"); +#endif + return m_data[row * m_cols + col]; +} + +template +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]; +} + +template +void GenericMatrix::zeroValues() +{ + memset(static_cast(m_data), 0, sizeof(T) * (size_t)m_rows * m_cols); +} + +template +void GenericMatrix::swap(GenericMatrix& other) +{ + swapb(m_rows, other.m_rows); + swapb(m_cols, other.m_cols); + swapb(m_allocSize, other.m_allocSize); + swapb(m_flags, other.m_flags); + swapb(m_data, other.m_data); +} + +template +GenericMatrix& GenericMatrix::operator = (const GenericMatrix& src) +{ + if (this != &src) + { + GenericMatrix temp(src); + swap(temp); + } + return *this; +} + +} + +#endif +#endif diff --git a/extern/xspublic/xscommon/xsens_janitors.h b/extern/xspublic/xscommon/xsens_janitors.h new file mode 100644 index 0000000..55bc2b0 --- /dev/null +++ b/extern/xspublic/xscommon/xsens_janitors.h @@ -0,0 +1,1149 @@ + +// Copyright (c) 2003-2022 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 XSENS_JANITORS_H +#define XSENS_JANITORS_H + +#ifndef __AVR32__ + #include +#endif + +// required for gnu c++ compiler versions due to difference in attribute declarations +#if defined(__AVR32__) + #define __cdecl + #define __stdcall +#elif defined(_ADI_COMPILER) + #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 +#endif + +namespace xsens +{ + +#ifndef NOT_FOR_PUBLIC_RELEASE +////////////////////////////////////////////////////////////////////////////////////////// +/*! \brief Value restoring janitor class + \details This class can be used to make sure that the value that is in the variable at the time + the janitor is created will be in it again when the janitor leaves scope. +*/ +template +class JanitorRestore +{ +private: + T& m_control; + T m_value; + bool m_enabled; +public: + + /*! \brief Constructor + */ + JanitorRestore(T& control, bool enabl = true) : + m_control(control), m_value(control), m_enabled(enabl) {} + + /*! \brief Destructor + */ + ~JanitorRestore() + { + if (m_enabled) + m_control = m_value; + } + + /*! \brief Disables the value restoring + */ + void disable(void) + { + m_enabled = false; + } + + /*! \brief Enables the value restoring + */ + void enable(void) + { + m_enabled = true; + } +}; + +////////////////////////////////////////////////////////////////////////////////////////// +/*! \brief Memory releasing janitor class + \details This class can be used to make sure that the associated pointer is freed when the + janitor leaves scope. +*/ +template +class JanitorFree +{ +private: + T* m_control; + bool m_enabled; +public: + + /*! \brief Constructor + */ + JanitorFree(T* control, bool enabl = true) : + m_control(control), m_enabled(enabl) {} + + /*! \brief Destructor + */ + ~JanitorFree() + { + if (m_enabled) + free(m_control); + } + + /*! \brief Disables the memory releasing + */ + void disable(void) + { + m_enabled = false; + } + + /*! \brief Enables the memory releasing + */ + void enable(void) + { + m_enabled = true; + } +}; + +////////////////////////////////////////////////////////////////////////////////////////// +/*! \brief Memory releasing janitor class + \details This class can be used to make sure that the associated object is deleted when the + janitor leaves scope. +*/ +template +class JanitorDelete +{ +private: + T* m_control; + bool m_enabled; +public: + + /*! \brief Default constructor + */ + JanitorDelete() : + m_control(NULL), + m_enabled(true) + { + + } + + /*! \brief Constructor + */ + JanitorDelete(T* control, bool enabl = true) : + m_control(control), + m_enabled(enabl) + { + + } + + /*! \brief Destructor + */ + ~JanitorDelete() + { + if (m_enabled) + delete m_control; + } + + /*! \brief Sets the control object + \param control The control object + */ + void setControl(T* control) + { + m_control = control; + } + + /*! \brief Disables the memory releasing + */ + void disable(void) + { + m_enabled = false; + } + + /*! \brief Enables the memory releasing + */ + void enable(void) + { + m_enabled = true; + } + + /*! \returns A const pointer to the data. This does not detach/copy the data. + */ + 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; + } +}; + +////////////////////////////////////////////////////////////////////////////////////////// +/*! \brief Memory releasing and nulling janitor class + \details This class can be used to make sure that the associated object is deleted when the + janitor leaves scope and the referenced pointer is set to NULL. +*/ +template +class JanitorDeleteNull +{ +private: + T*& m_control; + bool m_enabled; +public: + + /*! \brief Constructor + */ + JanitorDeleteNull(T*& control, bool enabl = true) : + m_control(control), + m_enabled(enabl) + { + + } + + /*! \brief Destructor + */ + ~JanitorDeleteNull() + { + if (m_enabled) + { + delete m_control; + m_control = 0; + } + } + + /*! \brief Disables the memory releasing and nulling + */ + void disable(void) + { + m_enabled = false; + } + + /*! \brief Enables the memory releasing and nulling + */ + void enable(void) + { + m_enabled = true; + } + + /*! \returns A const pointer to the data. This does not detach/copy the data. + */ + 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; + } +}; + +////////////////////////////////////////////////////////////////////////////////////////// +/*! \brief Memory releasing janitor class + \details This class can be used to make sure that the associated object is deleted when the + janitor leaves scope. +*/ +template +class JanitorDeleteArray +{ +private: + T* m_control; + bool m_enabled; +public: + + /*! \brief Constructor + */ + JanitorDeleteArray(T* control, bool enabl = true) : + m_control(control), m_enabled(enabl) {} + + /*! \brief Destructor + */ + ~JanitorDeleteArray() + { + if (m_enabled) + delete[] m_control; + } + + /*! \brief Disables the memory releasing + */ + void disable(void) + { + m_enabled = false; + } + + /*! \brief Enables the memory releasing + */ + void enable(void) + { + m_enabled = true; + } +}; +#endif + +////////////////////////////////////////////////////////////////////////////////////////// +/*! \brief Class function calling janitor class + \details This class can be used to make sure that the given class function is called when the + janitor leaves scope. +*/ +template +class JanitorClassFunc +{ +public: + typedef R(T::*t_func_JanitorClassFunc)(void); //!< A function prototype for a janitor class +private: + const JanitorClassFunc& operator = (const JanitorClassFunc&); + + T& m_control; + t_func_JanitorClassFunc m_funcJCF; + bool m_enabled; +public: + + /*! \brief Constructor + */ + JanitorClassFunc(T& control, t_func_JanitorClassFunc func, bool enabl = true) : + m_control(control), m_funcJCF(func), m_enabled(enabl) + { + } + + /*! \brief Destructor + */ + ~JanitorClassFunc() + { + if (m_enabled) + (m_control.*m_funcJCF)(); + } + + /*! \brief Disables the function calling + */ + void disable(void) + { + m_enabled = false; + } + + /*! \brief Enables the function calling + */ + void enable(void) + { + m_enabled = true; + } +}; + +////////////////////////////////////////////////////////////////////////////////////////// +/*! \brief Class function calling janitor class with a parameter + \details This class can be used to make sure that the given class function is called when the + janitor leaves scope. +*/ +template +class JanitorClassFuncP1 +{ +public: + typedef R(T::*t_func_JanitorClassFunc)(P1); //!< A function prototype for a janitor class +private: + const JanitorClassFuncP1& operator = (const JanitorClassFuncP1&); + + T& m_control; + P1 m_param1; + t_func_JanitorClassFunc m_funcJCF; + bool m_enabled; +public: + + /*! \brief Constructor + */ + JanitorClassFuncP1(T& control, P1 p1, t_func_JanitorClassFunc func, bool enabl = true) : + m_control(control), m_param1(p1), m_funcJCF(func), m_enabled(enabl) + { + } + + /*! \brief Destructor + */ + ~JanitorClassFuncP1() + { + if (m_enabled) + (m_control.*m_funcJCF)(m_param1); + } + + /*! \brief Disables the function calling + */ + void disable(void) + { + m_enabled = false; + } + + /*! \brief Enable the function calling + */ + void enable(void) + { + m_enabled = true; + } +}; + +#ifndef NOT_FOR_PUBLIC_RELEASE +////////////////////////////////////////////////////////////////////////////////////////// +/*! \brief Function calling janitor class for function with 0 parameters + \details This class can be used to make sure that the given function is called on the given + object when the janitor leaves scope. Take care that the object is not of a type that + is destroyed before the function unrolling begins. +*/ +template +class JanitorFunc0 +{ +public: + 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; + bool m_enabled; +public: + + /*! \brief Constructor + */ + JanitorFunc0(t_func_JanitorFunc func, bool enabl = true) : + m_funcJF(func), m_enabled(enabl) {} + + /*! \brief Destructor + */ + ~JanitorFunc0() + { + if (m_enabled) + (*m_funcJF)(); + } + + /*! \brief Disables the function calling + */ + void disable(void) + { + m_enabled = false; + } + + /*! \brief Enables the function calling + */ + void enable(void) + { + m_enabled = true; + } +}; + +#ifndef __AVR32__ +////////////////////////////////////////////////////////////////////////////////////////// +/*! \brief Function calling janitor class for std::function with 0 parameters + \details This class can be used to make sure that the given function is called on the given + object when the janitor leaves scope. Take care that the object is not of a type that + is destroyed before the function unrolling begins. +*/ +template +class JanitorStdFunc0 +{ +public: + typedef std::function t_func_JanitorFunc; //!< A function prototype for a janitor function +private: + const JanitorStdFunc0& operator = (const JanitorStdFunc0&); + t_func_JanitorFunc m_funcJF; + bool m_enabled; +public: + + /*! \brief Constructor + */ + JanitorStdFunc0(t_func_JanitorFunc func, bool enabl = true) : + m_funcJF(func), m_enabled(enabl) {} + + /*! \brief Destructor + */ + ~JanitorStdFunc0() + { + if (m_enabled) + m_funcJF(); + } + + /*! \brief Disables the function calling + */ + void disable(void) + { + m_enabled = false; + } + + /*! \brief Enables the function calling + */ + void enable(void) + { + m_enabled = true; + } +}; +#endif + +////////////////////////////////////////////////////////////////////////////////////////// +/*! \brief Function calling janitor class for function with 1 parameter + \details This class can be used to make sure that the given function is called on the given + object when the janitor leaves scope. Take care that the object is not of a type that + is destroyed before the function unrolling begins. +*/ +template +class JanitorFunc1 +{ +public: + typedef ResultType(__cdecl* t_func_JanitorFunc)(ParamType); //!< A function prototype for a janitor function +private: + const JanitorFunc1& operator = (const JanitorFunc1&); + ParamType m_control; + t_func_JanitorFunc m_funcJF; + bool m_enabled; +public: + + /*! \brief Constructor + */ + JanitorFunc1(t_func_JanitorFunc func, ParamType control, bool enabl = true) : + m_control(control), m_funcJF(func), m_enabled(enabl) {} + + /*! \brief Destructor + */ + ~JanitorFunc1() + { + if (m_enabled) + (*m_funcJF)(m_control); + } + + /*! \brief Disables the function calling + */ + void disable(void) + { + m_enabled = false; + } + + /*! \brief Enables the function calling + */ + void enable(void) + { + m_enabled = true; + } +}; + +////////////////////////////////////////////////////////////////////////////////////////// +/*! \brief Function calling janitor class for function with 1 reference parameter + \details This class can be used to make sure that the given function is called on the given + object when the janitor leaves scope. Take care that the object is not of a type that + is destroyed before the function unrolling begins. +*/ +template +class JanitorFunc1R +{ +public: + typedef ResultType(__cdecl* t_func_JanitorFunc)(ParamType); //!< A function prototype for a janitor function +private: + const JanitorFunc1R& operator = (const JanitorFunc1R&); + ParamType& m_control; + t_func_JanitorFunc m_funcJF; + bool m_enabled; +public: + + /*! \brief Constructor + */ + JanitorFunc1R(t_func_JanitorFunc func, ParamType& control, bool enabl = true) : + m_control(control), m_funcJF(func), m_enabled(enabl) {} + + /*! \brief Destructor + */ + ~JanitorFunc1R() + { + if (m_enabled) + (*m_funcJF)(m_control); + } + + /*! \brief Disables the function calling + */ + void disable(void) + { + m_enabled = false; + } + + /*! \brief Enables the function calling + */ + void enable(void) + { + m_enabled = true; + } +}; + +////////////////////////////////////////////////////////////////////////////////////////// +/*! \brief Function calling janitor class for function with 2 parameters + \details This class can be used to make sure that the given function is called on the given + object when the janitor leaves scope. Take care that the object is not of a type that + is destroyed before the function unrolling begins. +*/ +template +class JanitorFunc2 +{ +public: + typedef ResultType(__cdecl* t_func_JanitorFunc)(Param1Type, Param2Type); //!< A function prototype for a janitor function +private: + const JanitorFunc2& operator = (const JanitorFunc2&); + Param1Type m_control1; + Param2Type m_control2; + t_func_JanitorFunc m_funcJF; + bool m_enabled; +public: + + /*! \brief Constructor + */ + 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 + */ + ~JanitorFunc2() + { + if (m_enabled) + (*m_funcJF)(m_control1, m_control2); + } + + /*! \brief Disables the function calling + */ + void disable(void) + { + m_enabled = false; + } + + /*! \brief Enables the function calling + */ + void enable(void) + { + m_enabled = true; + } +}; + +////////////////////////////////////////////////////////////////////////////////////////// +/*! \brief Function calling janitor class for function with 2 reference parameters + \details This class can be used to make sure that the given function is called on the given + object when the janitor leaves scope. Take care that the object is not of a type that + is destroyed before the function unrolling begins. +*/ +template +class JanitorFunc2R +{ +public: + typedef ResultType(__cdecl* t_func_JanitorFunc)(Param1Type, Param2Type); //!< A function prototype for a janitor function +private: + const JanitorFunc2R& operator = (const JanitorFunc2R&); + Param1Type& m_control1; + Param2Type& m_control2; + t_func_JanitorFunc m_funcJF; + bool m_enabled; +public: + + /*! \brief Constructor + */ + 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 + */ + ~JanitorFunc2R() + { + if (m_enabled) + (*m_funcJF)(m_control1, m_control2); + } + + /*! \brief Disables the function calling + */ + void disable(void) + { + m_enabled = false; + } + + /*! \brief Enables the function calling + */ + void enable(void) + { + m_enabled = true; + } +}; + +////////////////////////////////////////////////////////////////////////////////////////// +/*! \brief Log / printf-like function calling janitor class + \details This class can be used to make sure that the given printf-like function is called with the + supplied parameter when the janitor leaves scope. Take care that the object is not of a type that + is destroyed before the function unrolling begins. +*/ +template +class JanitorLogFunc +{ +public: + 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; + const char* m_filter; + const char* m_str; + const char* m_functionName; + T& m_control; + bool m_enabled; +public: + + /*! \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) : + 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) : + m_funcJF(func), m_filter(filter), m_str(str), m_functionName(0), m_control(control), m_enabled(enable) {} + + /*! \brief Destructor + */ + ~JanitorLogFunc() + { + if (m_enabled) + { + if (m_functionName) + (*m_funcJF)(m_filter, m_str, m_functionName, (C) m_control); + else + (*m_funcJF)(m_filter, m_str, (C) m_control); + } + } + + /*! \brief Disables the log function calling + */ + void disable(void) + { + m_enabled = false; + } + + /*! \brief Enables the log function calling + */ + void enable(void) + { + m_enabled = true; + } +}; + +////////////////////////////////////////////////////////////////////////////////////////// +/*! \brief Log / printf-like function calling janitor class + \details This class can be used to make sure that the given printf-like function is called with the + supplied string when the janitor leaves scope. +*/ +template +class JanitorSimpleLogFunc +{ +public: + 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; + const char* m_filter; + const char* m_str; + const char* m_functionName; + bool m_enabled; +public: + + /*! \brief Constructor with additional 'function name' parameter + */ + 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) : + m_funcJF(func), m_filter(filter), m_str(str), m_functionName(0), m_enabled(enable) {} + + /*! \brief Destructor + */ + ~JanitorSimpleLogFunc() + { + if (m_enabled) + { + if (m_functionName) + (*m_funcJF)(m_filter, m_str, m_functionName); + else + (*m_funcJF)(m_filter, m_str); + } + } + + /*! \brief Disables the log function calling + */ + void disable(void) + { + m_enabled = false; + } + + /*! \brief Enables the log function calling + */ + void enable(void) + { + m_enabled = true; + } +}; + +////////////////////////////////////////////////////////////////////////////////////////// +/*! \brief Function calling janitor class + \details This class can be used to make sure that the given function is called on the given + object when the janitor leaves scope. Take care that the object is not of a type that + is destroyed before the function unrolling begins. +*/ +template +class JanitorFuncStdCall +{ +public: +#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; + t_func_JanitorFuncStdCall m_funcJFSC; + bool m_enabled; +public: + + /*! \brief Constructor + */ + JanitorFuncStdCall(t_func_JanitorFuncStdCall func, ParamType& control, bool enabl = true) : + m_funcJFSC(func), m_control(control), m_enabled(enabl) {} + + /*! \brief Destructor + */ + ~JanitorFuncStdCall() + { + if (m_enabled) + (*m_funcJFSC)(m_control); + } + + /*! \brief Disables the function calling + */ + void disable(void) + { + m_enabled = false; + } + + /*! \brief Enables the function calling + */ + void enable(void) + { + m_enabled = true; + } +}; + +////////////////////////////////////////////////////////////////////////////////////////// +/*! \brief Value restoring janitor class + \details This class can be used to make sure that the value that is in the variable at the time + the janitor is created will be in it again when the janitor leaves scope. +*/ +template +class JanitorSet +{ +private: + const JanitorSet& operator = (const JanitorSet&); + T& m_control; + T m_value; + bool m_enabled; +public: + + /*! \brief Constructor + */ + JanitorSet(T& control, const T& val, bool enabl = true) : + m_control(control), m_value(val), m_enabled(enabl) {} + + /*! \brief Destructor + */ + ~JanitorSet() + { + if (m_enabled) + m_control = m_value; + } + + /*! \brief Disables the value restoring + */ + void disable(void) + { + m_enabled = false; + } + + /*! \brief Enables the value restoring + */ + void enable(void) + { + m_enabled = true; + } +}; + + +////////////////////////////////////////////////////////////////////////////////////////// +/*! \brief Reference counting janitor class + \details This class can be used to make sure that the associated pointer is deleted when the + last janitor for it leaves scope. +*/ +template +class JanitorRCDeallocationPolicy_Delete +{ +public: + /*! \brief Deallocates (deletes) a pointer to an object + \param p The pointer to an object to delete + */ + static void Deallocate(T* p) + { + delete p; + } +protected: + /*! \brief Destructor + \note Always use non-virtual protected destructors in policies + */ + ~JanitorRCDeallocationPolicy_Delete() {} +}; + +/*! \brief Reference counting janitor class + \details This class can be used to make sure that the associated pointer is freed when the + last janitor for it leaves scope. +*/ +template +class JanitorRCDeallocationPolicy_Free +{ +public: + /*! \brief Deallocates (frees) a pointer to an object + \param p The pointer to an object to free + */ + static void Deallocate(T* p) + { + free(p); + } +protected: + /*! \brief Destructor + \note Always use non-virtual protected destructors in policies + */ + ~JanitorRCDeallocationPolicy_Free() {} +}; + +/*! \brief Reference counting janitor class + \details This class can be used to make sure that the associated pointer is deleted/freed when the + last janitor for it leaves scope. +*/ +template class DeallocationPolicy> +class JanitorRCDeallocation +{ +public: + typedef JanitorRCDeallocation ThisType; //!< A type definition for this type + typedef T ElementType; //!< A type definition for an element type + typedef void (*PostDeallocateFunc)(); //!< A function prototype for a post deallocation function + + /*! \brief Constructor with an element type + */ + JanitorRCDeallocation(ElementType* target = 0, bool enabled = true) + : m_target(target) + , m_refCounter(0) + , m_enabled(enabled) + { + addRef(); + } + + /*! \brief Constructor with this type + */ + JanitorRCDeallocation(const ThisType& j, bool enabled = true) + : m_target(j.m_target) + , m_refCounter(j.m_refCounter) + , m_enabled(enabled) + { + addRef(); + } + + /*! \brief Destructor + */ + virtual ~JanitorRCDeallocation() + { + removeRef(); + } + + /*! \brief Copy the data from \a rhs + \param rhs The janitor deallication class + \returns this + */ + JanitorRCDeallocation& operator=(const JanitorRCDeallocation& rhs) + { + if (m_target != rhs.m_target) + { + removeRef(); + m_target = rhs.m_target; + m_refCounter = rhs.m_refCounter; + addRef(); + } + return *this; + } + /*! \returns A const pointer to the data. This does not detach/copy the data. + */ + ElementType* operator->() const + { + return m_target; + } + + /*! \returns A reference to the data. This does not detach/copy the data. + */ + ElementType& operator*() const + { + return *m_target; + } + + /*! \brief Disables the reference counting + */ + void disable(void) + { + m_enabled = false; + } + + /*! \brief Enables the reference counting + */ + void enable(void) + { + m_enabled = true; + } + + /*! \returns A pointer to an element type + */ + ElementType* get() const throw() + { + return m_target; + } + + /*! \brief Swaps the current object's variables with an other object's variables + \param other The reference to an other object + */ + void swap(JanitorRCDeallocation& other) throw() + { + using std::swap; + swap(m_target, other.m_target); + swap(m_refCounter, other.m_refCounter); + swap(m_enabled, other.m_enabled); + } +#ifndef SWIG + /*! \brief Swaps the two object's variables + \param first The reference to the first object + \param second The reference to the second object + */ + inline friend void swap(JanitorRCDeallocation& first, JanitorRCDeallocation& second) + { + first.swap(second); + } +#endif + + /*! \brief Swaps an element type with this type + \details If no argument is passed the object will be reset + \param p The pointer to an element type + */ + void reset(ElementType* p = 0) throw() + { + if (m_target != p) + ThisType(p).swap(*this); + } + +private: + + /*! \brief Adds a reference to a reference counter + \note If there was no reference counter, then it will create a new one + */ + void addRef(void) + { + if (m_target) + { + if (!m_refCounter) + m_refCounter = new int(0); + (*m_refCounter)++; + } + } + + /*! \brief Removes a reference from a reference counter + \note If the reference counter has less than one reference, it will delete itself and deallocate an element type + */ + void removeRef(void) + { + if (m_refCounter) + { + if (*m_refCounter > 1) + (*m_refCounter)--; + else + { + delete m_refCounter; + DeallocationPolicy::Deallocate(m_target); + } + } + } + + ElementType* m_target; + int* m_refCounter; + bool m_enabled; +}; + +/*! \brief A reference counter memory releasing janitor class +*/ +template class DeallocationPolicy = JanitorRCDeallocationPolicy_Free> +class JanitorRCFree : public JanitorRCDeallocation +{ +public: + + /*! \brief Constructor + */ + JanitorRCFree(T* target = 0, bool enabled = true): JanitorRCDeallocation(target, enabled) {} +}; + +/*! \brief A reference counter memory releasing janitor class +*/ +template class DeallocationPolicy = JanitorRCDeallocationPolicy_Delete> +class JanitorRCDelete : public JanitorRCDeallocation +{ +public: + + /*! \brief Constructor + */ + JanitorRCDelete(T* target = 0, bool enabled = true): JanitorRCDeallocation(target, enabled) {} +}; + +#endif + +} + +#endif diff --git a/extern/xspublic/xscommon/xsens_math_throw.cpp b/extern/xspublic/xscommon/xsens_math_throw.cpp new file mode 100644 index 0000000..238fde0 --- /dev/null +++ b/extern/xspublic/xscommon/xsens_math_throw.cpp @@ -0,0 +1,105 @@ + +// Copyright (c) 2003-2022 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 "xsens_math_throw.h" +#include "xsens_mutex.h" + +#ifndef __STDC_VERSION__ + #define __STDC_VERSION__ 0 +#endif + +#if __STDC_VERSION__ >= 199901L + #include +#endif + +namespace xsens +{ +namespace DebugTools +{ + +// please note that this will never be called in full release builds! +void mathThrowBreakFunc() +{ + // volatile to prevent the function being optimized away + volatile int putMathBreakPointHere = 0; + (void)putMathBreakPointHere; +} + +/*! \brief Enable floating point exceptions + + On windows, this requires /fp:except for proper use. + It also needs implementation and a bit of testing. +*/ +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); +#elif __STDC_VERSION__ >= 199901L + feenableexcept(FE_ALL_EXCEPT); +#endif +} + +/*! \brief Disable floating point exceptions + + \see enableFloatingPointExceptions +*/ +void disableFloatingPointExceptions() +{ +#ifdef _MSC_VER + // _clearfp(); + // _controlfp(_CW_DEFAULT, ~0); +#elif __STDC_VERSION__ >= 199901L + feclearexcept(FE_ALL_EXCEPT); +#endif +} + +#if TIMEOUTCHECKER_DEFINED +TimeoutChecker::TimeoutChecker(char category, void* lock) + : m_category(category) + , m_timeout(category == 'a' ? 50 : category == 'r' ? 20 : 100) // a: acquire lock, r: release lock, l/other: lock period + , m_lock(lock) +{ + start(lock); +} + +void TimeoutChecker::warning(int64_t t) +{ + // VS breakpoint, actions: $TICK {m_category} {t} {m_lock} $CALLSTACK + volatile bool breakpointHere = (t != 0); +} +#endif + +} // namespace DebugTools + +} // namespace xsens diff --git a/extern/xspublic/xscommon/xsens_math_throw.h b/extern/xspublic/xscommon/xsens_math_throw.h new file mode 100644 index 0000000..235f175 --- /dev/null +++ b/extern/xspublic/xscommon/xsens_math_throw.h @@ -0,0 +1,95 @@ + +// Copyright (c) 2003-2022 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 XSENS_MATH_THROW_H +#define XSENS_MATH_THROW_H + +#include + +#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 + #ifdef XSENS_FW_ASSERT_FUNC + #define XM_THROW(a) XSENS_FW_ASSERT_FUNC(a, __FILE__, (unsigned) __LINE__) + #else + #define XM_THROW(a) assert(a) + #endif + #define XM_THROW_DEFINED 1 + #else + #define XM_THROW(a) ((void) 0) + #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)) + #elif defined __GNUC__ + #define XETHROW(a) throw XsException(XRV_ERROR, XsString(__PRETTY_FUNCTION__) << " " << XsString(a)) + #else + #define XETHROW(a) throw XsException(XRV_ERROR, XsString(__func__) << " " << XsString(a)) + #endif + #else + #define XETHROW(a) throw (a) + #endif + + #if defined(XSENS_DEBUG) && defined(_WIN32) // && !defined(_WIN64) unclear why this clause was added in xsens_math.h rev 871 by RZA + #define XM_THROW(a) do { xsens::DebugTools::mathThrowBreakFunc(); XETHROW(a); } while(0) + #else + #define XM_THROW(a) do { XETHROW(a); } while(0) + #endif +#endif + +namespace xsens +{ +namespace DebugTools +{ +void mathThrowBreakFunc(); +void enableFloatingPointExceptions(); +void disableFloatingPointExceptions(); +} // namespace DebugTools +} // namespace xsens + +#endif diff --git a/extern/xspublic/xscommon/xsens_mutex.h b/extern/xspublic/xscommon/xsens_mutex.h new file mode 100644 index 0000000..c4f55b5 --- /dev/null +++ b/extern/xspublic/xscommon/xsens_mutex.h @@ -0,0 +1,1859 @@ + +// Copyright (c) 2003-2022 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 XSENS_MUTEX_H +#define XSENS_MUTEX_H + +#include +#include +#include +#include +#include + +#ifndef __GNUC__ + #pragma warning(disable: 4127) +#endif + +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 +{ +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() + { + 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 + +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 + } + ~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; + } + + /*! \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() + { + 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(); + } + + inline bool claimMutex(bool write) + { + XsThreadId cid = xsGetCurrentThreadId(); + // write lock + if (write) + { + enterAtomic(); + ++m_writeLocksPending; + while (1) + { + 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)) + { + leaveAtomic(); + xsYield(); // wait for other locks to end + enterAtomic(); + } + else + { + ++m_writeRef; + --m_writeLocksPending; + m_writeLocked = cid; + leaveAtomic(); + return true; + } + } + } + // read lock + else + { + while (true) + { + enterAtomic(); + 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(); + } + } + } + + /*! \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; + } + + inline bool releaseMutex(bool write) + { + XsThreadId cid = xsGetCurrentThreadId(); + enterAtomic(); + if (write) + { + if (m_writeLocked == cid) + { + if (--m_writeRef == 0) + m_writeLocked = 0; + } + else + { + 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; + + // write lock + if (write) + { + enterAtomic(); + ++m_writeLocksPending; + leaveAtomic(); + do + { + 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)) + { + leaveAtomic(); + // wait for other locks to end + xsYield(); + } + else + { + ++m_writeRef; + --m_writeLocksPending; + m_writeLocked = cid; + leaveAtomic(); + 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 + { + 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 (true) + { + 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 (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) + { + if (m_readLocked[i] == cid) + { + readGot = true; + break; + } + } + } + + if (readGot) + { + addReadLock(cid); + leaveAtomic(); + return true; + } + leaveAtomic(); + // wait for write lock to end + xsYield(); + } + break; + + case Mode::SuspendedWrite: + enterAtomic(); + ++m_writeLocksPending; + while (1) + { + if (m_writeLocked != cid && m_writeLocked != 0) + { + leaveAtomic(); + xsYield(); // wait for other write locks to end + enterAtomic(); + } + else + { + ++m_writeRef; + --m_writeLocksPending; + m_writeLocked = cid; + ++m_writeLockSuspendCount; + leaveAtomic(); + return true; + } + } + break; + + 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); + } + + /*! \brief Downgrade an existing write lock to a read lock + */ + inline bool downgradeToRead(bool decreaseSuspend) + { + XsThreadId cid = xsGetCurrentThreadId(); + enterAtomic(); + if (m_writeLocked == cid) + { + if (decreaseSuspend) + --m_writeLockSuspendCount; + if (--m_writeRef == 0) + { + m_writeLocked = 0; + m_writeLockSuspendCount = 0; + } + addReadLock(cid); + leaveAtomic(); + 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() + { + XsThreadId cid = xsGetCurrentThreadId(); + enterAtomic(); + if (m_writeLocked == cid) + { + ++m_writeLockSuspendCount; + leaveAtomic(); + return true; + } + leaveAtomic(); + return false; + } + + /*! \brief Resumes an existing write lock so read locks are no longer allowed + */ + inline bool resumeWriteLock() + { + XsThreadId cid = xsGetCurrentThreadId(); + enterAtomic(); + if (m_writeLocked == cid) + { + if (m_writeLockSuspendCount-- == m_writeRef) + { + // wait for all read locks to end + while (true) + { + bool stillReading = false; + for (int i = 0; i < m_readLockCount; ++i) + { + if (m_readLocked[i] != cid) + { + stillReading = true; + break; + } + } + + if (!stillReading) + { + leaveAtomic(); + return true; + } + leaveAtomic(); + xsYield(); + enterAtomic(); + } + } + leaveAtomic(); + 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) + { + XsThreadId cid = xsGetCurrentThreadId(); + enterAtomic(); + if (write) + { + if (m_writeLocked == cid) + { + if (decreaseSuspendCount) + --m_writeLockSuspendCount; + + if (--m_writeRef == 0) + { + m_writeLockSuspendCount = 0; + m_writeLocked = 0; + } + } + else + { + 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; + } + + /*! \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) + { + rv |= MSF_WriteLocked; + if (m_writeLocked == cid) + rv |= MSF_HaveWriteLock; + if (m_writeLockSuspendCount == m_writeRef) + rv |= MSF_WriteLockSuspended; + } + if (m_writeLocksPending) + rv |= MSF_WriteLocksPending; + if (m_readLockCount) + rv |= MSF_ReadLocked; + for (int i = 0; i < m_readLockCount; ++i) + { + if (m_readLocked[i] == cid) + { + rv |= MSF_HaveReadLock; + break; + } + } + const_cast(this)->leaveAtomic(); + return rv; + } + +#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 + { + return (status() & MSF_HaveWriteLock) != 0; + } + + /*! \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; + } + return *this; + } + +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 + \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 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) + { + CHECK_LOCK_ACQUIRE(); + m_locked = m_mutex->claimMutex(); + } + } + inline ~Lock() + { + unlock(); + } + + /*! \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 Unlocks the locked mutex + \returns True if the mutex is successfully unlocked + */ + inline bool unlock() noexcept + { + if (m_locked) + { + XSENS_MSC_WARNING_SUPPRESS(4706) + CHECK_LOCK_RELEASE(); + return !(m_locked = !m_mutex->releaseMutex()); + } + 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 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; + } +}; + +/*! \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() + { + 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) + { + 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) + { + m_lockedR = m_mutex->downgradeToRead(); + m_lockedW = false; + assert(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) + { + 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(!m_lockedR); + m_lockedW = false; + return m_mutex->releaseMutex(true); + } + 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) + { + 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) + { + m_lockedR = m_mutex->downgradeToRead(); + m_lockedW = false; + assert(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 main 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() + { + } + + /*! \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 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; + } + + /*! \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(); + } +#endif + +#ifndef XSENS_DEBUG +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 +*/ +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 + + 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 + + if (lockState != LS_Unlocked) + lock(lockState); + } + + 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) + { + switch (desiredLockState) + { + case LS_Write: + if (m_lockedW) + return resume(); + if (m_lockedR) + unlock(); + m_lockedW = m_mutex->claimMutex(MutexReadWriteSuspendable::Mode::Write); + assert(m_lockedW); + return m_lockedW; + + case LS_SuspendedWrite: + if (m_lockedW) + return suspend(); + if (m_lockedR) + unlock(); + m_lockedW = m_mutex->claimMutex(MutexReadWriteSuspendable::Mode::SuspendedWrite); + m_iSuspended = m_lockedW; + assert(m_lockedW); + return m_lockedW; + + case LS_Read: + if (m_lockedW) + { + m_lockedR = m_mutex->downgradeToRead(m_iSuspended); + m_iSuspended = false; + m_lockedW = false; + assert(m_lockedR); + } + if (!m_lockedR) + m_lockedR = m_mutex->claimMutex(MutexReadWriteSuspendable::Mode::Read); + assert(m_lockedR); + return m_lockedR; + + case LS_Unlocked: + return unlock(); + + 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 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; + } + + /*! \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) + { + 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; + } + + /*! \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); + 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 + { + return m_mutex == mutex; + } + + /*! \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; + } +}; + +/*! \class LockGuarded + \brief A guarded lock class +*/ +class LockGuarded +{ +private: + GuardedMutex* m_mutex; + volatile std::atomic_bool m_locked; + + LockGuarded(LockGuarded const&) = delete; + LockGuarded& operator=(LockGuarded const&) = delete; + LockGuarded(LockGuarded&&) = delete; + LockGuarded& operator=(LockGuarded&&) = delete; + +public: + + /*! \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 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(); + } + + /*! \brief Destructor + */ + inline ~LockGuarded() + { + unlock(); + } + + /*! \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 Unlocks the locked mutex + \returns True if the mutex is successfully unlocked + */ + inline bool unlock() noexcept + { + if (m_locked) + { + XSENS_MSC_WARNING_SUPPRESS(4706) + return !(m_locked = !m_mutex->releaseMutex()); + } + return true; + } + + /*! \returns True if the mutex is locked by this lock + */ + inline bool isLocked() const + { + return m_locked; + } + + /*! \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 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 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 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 Destructor + */ + ~ProtectedValue() + { + if (m_lock) + delete m_lock; + } + + /*! \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; + + assert(pv.m_lock); + m_lock = pv.m_lock; + m_value = pv.m_value; + pv.m_lock = NULL; + } + + 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 +#else + 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; +#ifdef _WIN32 + /*! \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); +#endif + /*! \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); + + 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; +#else + pthread_cond_t m_cond; + pthread_condattr_t m_condattr; +#ifdef __APPLE__ + int m_clockId; +#else + 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: +#if defined(_WIN32) + HANDLE m_event; +#else + 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; +}; + +/*! \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) +#endif + +#endif diff --git a/extern/xspublic/xscommon/xsens_threadpool.cpp b/extern/xspublic/xscommon/xsens_threadpool.cpp new file mode 100644 index 0000000..deede90 --- /dev/null +++ b/extern/xspublic/xscommon/xsens_threadpool.cpp @@ -0,0 +1,826 @@ + +// Copyright (c) 2003-2022 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 "xsens_threadpool.h" +#ifndef _MSC_VER + #include + #include +#endif +#ifdef XSENS_DEBUG + #include +#endif + +#include +#include +#include "threading.h" +#include + +// Enable this if you want to disable multithreading +//#define ADD_TASK_EXECUTE_NOW + +namespace xsens +{ +///////////////////////////////////////////////////////////////////////////////////////// +///////////////////////////////////////////////////////////////////////////////////////// +///////////////////////////////////////////////////////////////////////////////////////// + +typedef std::map> TaskSet; +typedef std::set ThreadSet; +typedef std::vector> TaskList; + +/*! \brief Returns the number of processor cores in the current system +*/ +int processorCount() +{ +#if defined(_WIN32) + SYSTEM_INFO sysinfo; + ::GetSystemInfo(&sysinfo); + return (int) sysinfo.dwNumberOfProcessors; +#else + return sysconf(_SC_NPROCESSORS_ONLN); + // see http://stackoverflow.com/questions/150355/programmatically-find-the-number-of-cores-on-a-machine + // for OS-specific code +#endif +} + +///////////////////////////////////////////////////////////////////////////////////////// +///////////////////////////////////////////////////////////////////////////////////////// +///////////////////////////////////////////////////////////////////////////////////////// + +/*! \class ThreadPoolTask + \brief A generic task implementation for the thread pool +*/ + +/*! \brief This function gets called by PooledThread when the exec() function returns false to determine if we should wait for a specific task + \details The default implementation returns 0 (no id to wait for) + \returns False to determine if we should wait for a specific task +*/ +unsigned int ThreadPoolTask::needToWaitFor() +{ + return 0; +} + +///////////////////////////////////////////////////////////////////////////////////////// +///////////////////////////////////////////////////////////////////////////////////////// +///////////////////////////////////////////////////////////////////////////////////////// + +/*! \brief A class that contains a task and some administrative stuff +*/ +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 + std::vector> m_dependentTasks; //!< A list of tasks that are waiting for this task to complete + XsThreadId m_threadId; + volatile std::atomic m_canceling; + + PooledTask() + : m_task(nullptr) + , m_id(0) + , m_threadId(0) + , m_canceling(false) + , m_completed(false) + , m_completedMutex() + , m_completedCondition(m_completedMutex) + { + } + + ~PooledTask() + { + signalCompleted(); + delete m_task; + } + + bool waitForCompletion(uint32_t timeout = UINT32_MAX) + { + Lock locker(&m_completedMutex); + if (!m_completed) + { + // This may look like a dead-lock situation with signalCompleted(), but it isn't + if (timeout == UINT32_MAX) + m_completedCondition.wait(); + else + m_completedCondition.wait(timeout); + } + return m_completed; + } + + void signalCompleted() noexcept + { + Lock locker(&m_completedMutex); + if (!m_completed) + { + m_completed = true; + locker.unlock(); + m_completedCondition.broadcast(); + } + } + +private: + volatile std::atomic_bool m_completed; + Mutex m_completedMutex; + WaitCondition m_completedCondition; +}; + +/*! \brief Returns true if the task has been told to cancel itself */ +bool ThreadPoolTask::isCanceling() const +{ + if (m_container) + return m_container->m_canceling; + return false; +} + +/*! \brief Returns the task ID of the task or 0 if it doesn't have a proper ID (yet) */ +unsigned int ThreadPoolTask::taskId() const +{ + if (m_container) + return m_container->m_id; + return 0; +} + +///////////////////////////////////////////////////////////////////////////////////////// +///////////////////////////////////////////////////////////////////////////////////////// +///////////////////////////////////////////////////////////////////////////////////////// + +/*! \brief A class that contains a thread that runs in a ThreadPool to execute tasks + \details These threads are created by the ThreadPool. +*/ +class PooledThread : public StandardThread +{ +public: + PooledThread(ThreadPool* pool); + ~PooledThread(); + bool isIdle() const; + bool isBusy() const; + unsigned int executedCount() const; + unsigned int completedCount() const; + unsigned int failedCount() const; + +protected: + ThreadPool* m_pool; //!< The pool that contains this thread + std::shared_ptr m_task; //!< The task that is currently being executed or NULL if the thread is idle + unsigned int m_executed; //!< The number of tasks that this thread has executed s o far, including incomplete tasks + 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 + + int32_t innerFunction(void) override; +}; + +/*! \brief Constructor */ +PooledThread::PooledThread(ThreadPool* pool) + : StandardThread() + , m_pool(pool) + , m_task(nullptr) + , m_executed(0) + , m_completed(0) + , m_failed(0) +{ +} + +/*! \brief Destructor */ +PooledThread::~PooledThread() +{ + stopThread(); + m_pool = nullptr; +} + +/*! \brief The inner function of the pooled thread. + + The function will do tasks until the ThreadPool no longer supplies any tasks, at which point + it will return with a sleep time of 1ms. +*/ +int32_t PooledThread::innerFunction(void) +{ + if (!m_task) + m_task = m_pool->getNextTask(); + + while (m_task && !isTerminating()) + { + m_task->m_threadId = getThreadId(); + bool complete = false; + try + { + if (m_task->m_task->exec()) + { + ++m_completed; + complete = true; + } + else if (m_task->m_canceling) + { + ++m_failed; + complete = true; + } + } +#ifdef XSENS_DEBUG + catch (std::exception& e) + { + const std::type_info& et = typeid(e); +#ifdef __GNUC__ + int 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 + fprintf(stderr, "ThreadPool: Caught an unhandled %s: %s\n", et.name(), e.what()); +#endif + complete = true; + } +#endif + catch (...) + { +#ifdef XSENS_DEBUG + fprintf(stderr, "ThreadPool: Caught an unhandled unknown exception\n"); +#endif + m_task->m_task->onError(); + ++m_failed; + complete = true; + } + + ++m_executed; + if (complete) + m_pool->reportTaskComplete(m_task); + else + { + m_task->m_threadId = 0; + m_pool->reportTaskPaused(m_task); + } + + m_task = m_pool->getNextTask(); + } + return 1; +} + +/*! \brief Return whether the thread is currently executing a task (false) or not (true) +*/ +bool PooledThread::isIdle() const +{ + return (m_task == nullptr); +} + +/*! \brief Return whether the thread is currently executing a task (true) or not (false) +*/ +bool PooledThread::isBusy() const +{ + return !isIdle(); +} + +/*! \brief Return the number of tasks successfully or partially executed by the thread +*/ +unsigned int PooledThread::executedCount() const +{ + return m_executed; +} + +/*! \brief Return the number of tasks successfully executed by the thread +*/ +unsigned int PooledThread::completedCount() const +{ + return m_completed; +} + +/*! \brief Return the number of tasks that failed to execute properly +*/ +unsigned int PooledThread::failedCount() const +{ + return m_failed; +} + +///////////////////////////////////////////////////////////////////////////////////////// +///////////////////////////////////////////////////////////////////////////////////////// +///////////////////////////////////////////////////////////////////////////////////////// + +/*! \class ThreadPool + \brief This class creates and maintains a number of threads that can execute finite-length tasks + \note See the test cases for examples on how to use the class +*/ + +/*! \brief Construct a threadpool with a number of threads equal to the number of cores on the PC +*/ +ThreadPool::ThreadPool() + : m_nextId(1) + , m_suspended(false) + , m_terminating(false) +{ + setPoolSize(0); +} + +/*! \brief Destructor, clears any pending tasks and destroys the threads +*/ +ThreadPool::~ThreadPool() +{ + m_terminating = true; + suspend(true); + + // all threads should now be idle + m_tasks.clear(); + m_tasksSearch.clear(); + m_executing.clear(); + m_delaying.clear(); + + try + { + for (ThreadSet::iterator it = m_threads.begin(); it != m_threads.end(); ++it) + delete *it; + } + catch (...) + { + // nothing much we can do about this... + } +} + +/*! \brief Add a task to be executed by the threadpool + \param task The task object whose exec() function will be called + \param afterId When not 0, the task will not be started until the task with the given id has completed + + \returns The id of the task that was added or 0 if the task could not be added for some reason. +*/ +ThreadPool::TaskId ThreadPool::addTask(ThreadPoolTask* task, ThreadPool::TaskId afterId) +{ + assert(!m_terminating); + std::shared_ptr tmp = std::make_shared(); + tmp->m_task = task; + Lock safety(&m_safe); + tmp->m_id = m_nextId; + task->m_container = tmp.get(); + + // determine next ID + if (!++m_nextId) + m_nextId = 1; + +#ifndef ADD_TASK_EXECUTE_NOW + if (afterId) + { + std::shared_ptr after = findTask(afterId); + if (after) + { + after->m_dependentTasks.push_back(tmp); + m_delaying[tmp->m_id] = tmp; + return tmp->m_id; + } + } + + m_tasks.push_back(tmp); + m_tasksSearch[tmp->m_id] = tmp; +#else + task->exec(); + task->signalCompleted(); +#endif + + return tmp->m_id; +} + +/*! \brief Return the number of tasks that are currently in the queue or being executed +*/ +unsigned int ThreadPool::count() +{ + Lock safety(&m_safe); + return (unsigned int)(m_tasks.size() + m_delaying.size() + m_executing.size()); +} + +/*! \brief Set the number of threads in the ThreadPool + \param poolsize When 0 or less two threads will be created for each processor core in the system (with a minimum of 4 threads), otherwise the desired number of threads will be created +*/ +void ThreadPool::setPoolSize(unsigned int poolsize) +{ + if (poolsize == 0) + { + //int pc = processorCount(); + //poolsize = ::std::max(4, pc*2); + poolsize = 12; // fixed count to prevent thread starvation on low-core PCs and overthreading on high-core PCs + } + + bool wasSuspended = m_suspended; + suspend(poolsize < m_threads.size()); + Lock safety(&m_safe); + + // reduce size if pool is too large + if (poolsize < m_threads.size()) + { + while (poolsize < m_threads.size()) + { + auto it = m_threads.begin(); + auto thread = *it; + safety.unlock(); + thread->stopThread(); + safety.lock(); + delete thread; + m_threads.erase(it); + } + } + + // increase size if pool is too small + for (unsigned int i = (unsigned int) m_threads.size(); i < poolsize; ++i) + { + PooledThread* t = new PooledThread(this); + m_threads.insert(t); +#ifdef XSENS_DEBUG + char bufje[64]; + sprintf(bufje, "Pooled Thread %p", t); +#else + const char bufje[] = "Pooled Thread"; +#endif + if (!t->startThread(bufje)) + { + m_threads.erase(m_threads.find(t)); + delete t; + throw XsException(XRV_ERROR, "Could not start thread for ThreadPool"); + } + } + + if (!wasSuspended) + resume(); +} + +/*! \brief Return the number of threads in the pool */ +unsigned int ThreadPool::poolSize() const +{ + return (unsigned int) m_threads.size(); +} + +/*! \brief Find a task with the supplied \a id +*/ +std::shared_ptr ThreadPool::findTask(ThreadPool::TaskId id) +{ + Lock safety(&m_safe); + TaskSet::iterator it = m_executing.find(id); + if (it != m_executing.end()) + return it->second; + + it = m_delaying.find(id); + if (it != m_delaying.end()) + return it->second; + + it = m_tasksSearch.find(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(); +} + +/*! \brief Find an XsThread with the specified \a id +*/ +XsThreadId ThreadPool::taskThreadId(TaskId id) +{ + Lock safety(&m_safe); + TaskSet::iterator it = m_executing.find(id); + if (it != m_executing.end()) + return it->second->m_threadId; + return 0; +} + +/*! \brief Check if a task with the supplied \a id exists +*/ +bool ThreadPool::doesTaskExist(ThreadPool::TaskId id) +{ + return findTask(id) != nullptr; +} + +/*! \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) noexcept +{ + Lock safety(&m_safe); + TaskSet::iterator it = m_executing.find(id); + if (it != m_executing.end()) + { + it->second->m_canceling = true; + safety.unlock(); + if (wait) + waitForCompletion(id); + return; + } + + 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 +*/ +void ThreadPool::waitForCompletion(ThreadPool::TaskId id) +{ + std::shared_ptr task = findTask(id); + if (task != nullptr) + task->waitForCompletion(); +} + +/*! \brief Called by PooledThread to notify the ThreadPool that a task was completed + \note After this call, the supplied \a task is invalid +*/ +void ThreadPool::reportTaskComplete(std::shared_ptr task) +{ + Lock safety(&m_safe); + + // remove task from 'executing' list + TaskSet::iterator it = m_executing.find(task->m_id); + if (it != m_executing.end()) + m_executing.erase(it); + + // notify dependent tasks that their dependency has been fulfilled + for (TaskList::iterator dep = task->m_dependentTasks.begin(); dep != task->m_dependentTasks.end(); ++dep) + { + it = m_delaying.find((*dep)->m_id); + if (it != m_delaying.end()) + { + m_tasks.push_back(it->second); + m_tasksSearch[it->second->m_id] = it->second; + m_delaying.erase(it); + } + } + task->signalCompleted(); +} + +/*! \brief Return the next task that should be run and mark it as executing +*/ +std::shared_ptr ThreadPool::getNextTask() +{ + Lock safety(&m_safe); + if (!m_suspended && !m_tasks.empty()) + { + std::shared_ptr tmp = m_tasks.front(); + m_tasks.pop_front(); + m_tasksSearch.erase(tmp->m_id); + m_executing[tmp->m_id] = tmp; + return tmp; + } + + return std::shared_ptr(); +} + +/*! \brief Called by PooledThread to notify the ThreadPool that its running task has to wait for something + \details This function can be called when the task failed to run to completion. It will be rescheduled + at the end of the queue and its exec function will be called again. + To notify the threadpool, a task should return 'false' from its exec() function. + \param task The task that is to be paused + \note The task itself is responsible for maintaining its state between the exec calls + \note After calling this function the PooledThread should consider the supplied \a task as invalid since it's possible that it has been picked up, executed and deleted by another thread during the function return +*/ +void ThreadPool::reportTaskPaused(std::shared_ptr task) +{ + Lock safety(&m_safe); + + // remove task from 'executing' list + TaskSet::iterator it = m_executing.find(task->m_id); + if (it != m_executing.end()) + m_executing.erase(it); + + // add task back into either the delaying list if it should wait for something + unsigned int waitForId = task->m_task->needToWaitFor(); + if (waitForId) + { + std::shared_ptr after = findTask(waitForId); + if (after) + { + after->m_dependentTasks.push_back(task); + m_delaying[task->m_id] = task; + return; + } + } + + // add task into ready queue + m_tasks.push_back(task); + m_tasksSearch[task->m_id] = task; +} + +/*! \brief Suspend execution of tasks, any currently executing tasks will run to completion, + but queued tasks will not be started. + \param wait When set to true the function waits for all threads to finish their current task + \sa resume +*/ +void ThreadPool::suspend(bool wait) noexcept +{ + Lock safety(&m_safe); + m_suspended = true; + + if (wait) + { + safety.unlock(); + + for (ThreadSet::const_iterator it = m_threads.begin(); it != m_threads.end(); ++it) + while ((*it)->isBusy()) + xsYield(); + } +} + +/*! \brief Resume execution of tasks. + \sa suspend +*/ +void ThreadPool::resume() +{ + Lock safety(&m_safe); + m_suspended = false; +} + +/*! \brief Return the number of tasks executed (including paused) by the given thread +*/ +unsigned int ThreadPool::executedCount(unsigned int thread) const +{ + unsigned int i = 0; + for (ThreadSet::const_iterator it = m_threads.begin(); it != m_threads.end(); ++it, ++i) + if (i == thread) + return (*it)->executedCount(); + return 0; +} + +/*! \brief Return the number of tasks completed by the given thread +*/ +unsigned int ThreadPool::completedCount(unsigned int thread) const +{ + unsigned int i = 0; + for (ThreadSet::const_iterator it = m_threads.begin(); it != m_threads.end(); ++it, ++i) + if (i == thread) + return (*it)->completedCount(); + return 0; +} + +/*! \brief Return the number of tasks that failed to execute in the given thread +*/ +unsigned int ThreadPool::failedCount(unsigned int thread) const +{ + unsigned int i = 0; + for (ThreadSet::const_iterator it = m_threads.begin(); it != m_threads.end(); ++it, ++i) + if (i == thread) + return (*it)->failedCount(); + return 0; +} + +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() noexcept +{ + if (gPool) + return gPool; + try + { + gPool = new ThreadPool; + gManagePool = true; + } + catch (...) + { + gPool = nullptr; + gManagePool = false; + } + return gPool; +} + +/*! \brief Destroy the global thread pool object + \details If the pool was not created by this library, the current library will only stop using + the pool, but will leave the object intact. + The next call to instance() will create a new pool. + \sa instance +*/ +void ThreadPool::destroy() +{ + if (gPool && gManagePool) + delete gPool; + gPool = NULL; + gManagePool = true; +} + +/*! \brief Set the threadpool to use + \details This function allows the user to supply his own ThreadPool object to be used + by all subsequent operations. If another pool was created by this library, it will be + destroyed. + \param pool The new threadpool object to use. If NULL is supplied, it is simply destroyed + or stopped from being used. + \sa destroy +*/ +void ThreadPool::setPool(ThreadPool* pool) +{ + if (pool != gPool) + { + destroy(); + if (pool) + { + gPool = pool; + gManagePool = false; + } + } +} + +///////////////////////////////////////////////////////////////////////////////////////// +///////////////////////////////////////////////////////////////////////////////////////// +///////////////////////////////////////////////////////////////////////////////////////// + +/*! \class TaskCompletionWaiter + \brief A class that allows multiple-dependency waiting + \details A normal task in the ThreadPool can only wait for one task ID. This class allows waiting + for multiple IDs. The IDs to wait for can be supplied upon creation or added later, but they + should NOT be added once the object has been scheduled. + The TaskCompletionWaiter must be scheduled in the ThreadPool that contains the task IDs to wait + for so it can do its job. + + \note If you want a thread outside the ThreadPool to wait for multiple tasks to complete, create a + TaskCompletionWaiter, schedule it and wait for the TaskCompletionWaiter to complete through + the ThreadPool::waitForCompletion function. +*/ + +/*! \brief Constructor, sets up an empty waiter */ +TaskCompletionWaiter::TaskCompletionWaiter(ThreadPool* pool) + : ThreadPoolTask() + , m_pool(pool) +{} + +/*! \brief Constructor, sets up a waiter that waits for all items in the list to be completed */ +TaskCompletionWaiter::TaskCompletionWaiter(const std::list& waitlist, ThreadPool* pool) + : ThreadPoolTask() + , m_pool(pool) + , m_waitList(waitlist) +{} + +/*! \brief Destructor */ +TaskCompletionWaiter::~TaskCompletionWaiter() +{} + +/*! \brief task function, checks if there are tasks left that we should be waiting for + \details The function returns false if at least one of the wait tasks still exists + \returns True if successfully executed +*/ +bool TaskCompletionWaiter::exec() +{ + while (m_waitList.size()) + { + // if the task exists, reschedule this task to recheck after the task completes + if (m_pool->doesTaskExist(*m_waitList.begin())) + return false; + + m_waitList.erase(m_waitList.begin()); + } + return true; +} + +/*! \brief If there are wait tasks left, this function will return the id of the first in the list + \returns The id of the first task in the list +*/ +unsigned int TaskCompletionWaiter::needToWaitFor() +{ + if (m_waitList.size()) + return *m_waitList.begin(); + return 0; +} + +/*! \brief Add the id of a task to wiat for to the list. + \param id The ID to add + \note This function should NOT be called once the task has been scheduled! +*/ +void TaskCompletionWaiter::addWaitId(unsigned int id) +{ + m_waitList.push_back(id); +} +} // namespace xsens diff --git a/extern/xspublic/xscommon/xsens_threadpool.h b/extern/xspublic/xscommon/xsens_threadpool.h new file mode 100644 index 0000000..05df5c9 --- /dev/null +++ b/extern/xspublic/xscommon/xsens_threadpool.h @@ -0,0 +1,168 @@ + +// Copyright (c) 2003-2022 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 XSENS_THREADPOOL_H +#define XSENS_THREADPOOL_H + +#include "xsens_mutex.h" + +#include +#include +#include +#include +#include + +namespace xsens +{ + +int processorCount(); + +class PooledTask; +class ThreadPoolTask +{ +public: + virtual bool exec() = 0; //!< \returns True if the task completed or false to reschedule the task + virtual unsigned int needToWaitFor(); + ThreadPoolTask() : m_container(nullptr) {} + virtual ~ThreadPoolTask() {} + virtual void onError() {} //!< Callback for when an error occurred during task execution. Any implementation of this function should NEVER throw an exception. + bool isCanceling() const; + unsigned int taskId() const; + +private: + friend class ThreadPool; + PooledTask* m_container; +}; + +class PooledThread; +class ThreadPool +{ +public: + typedef unsigned int TaskId; //!< A type definition of a task ID + +private: + void reportTaskComplete(std::shared_ptr); + void reportTaskPaused(std::shared_ptr); + std::shared_ptr getNextTask(); + friend class PooledThread; + + std::set m_threads; + std::deque> m_tasks; + std::map> m_tasksSearch; + std::map> m_executing; + std::map> m_delaying; + Mutex m_safe; + TaskId m_nextId; + bool m_suspended; + volatile std::atomic_bool m_terminating; + + std::shared_ptr findTask(TaskId id); + +protected: + ThreadPool(); + ~ThreadPool(); + +public: + TaskId addTask(ThreadPoolTask* task, TaskId afterId = 0); + unsigned int count(); + void setPoolSize(unsigned int poolsize); + unsigned int poolSize() const; + bool doesTaskExist(TaskId id); + void cancelTask(TaskId id, bool wait = true) noexcept; + void waitForCompletion(TaskId id); + void suspend(bool wait = false) noexcept; + void resume(); + unsigned int executedCount(unsigned int thread) const; + unsigned int completedCount(unsigned int thread) const; + unsigned int failedCount(unsigned int thread) const; + XsThreadId taskThreadId(TaskId id); + + static ThreadPool* instance() noexcept; + static void destroy(); + static void setPool(ThreadPool* pool); +}; + +class TaskCompletionWaiter : public ThreadPoolTask +{ +public: + virtual bool exec(); + virtual unsigned int needToWaitFor(); + + TaskCompletionWaiter(ThreadPool* pool = ThreadPool::instance()); + TaskCompletionWaiter(const std::list& waitlist, ThreadPool* pool = ThreadPool::instance()); + virtual ~TaskCompletionWaiter(); + void addWaitId(unsigned int id); + +private: + ThreadPool* m_pool; + std::list m_waitList; +}; + +/*! \brief Task that will delete its object parameter + \details This class can be used to schedule a delete of a single object in the ThreadPool. + Normally you don't need to use this class directly, but can use the \ref deleteThreaded function. + \tparam T The type of the object to delete +*/ +template +class ThreadPoolObjectDeleter : public ThreadPoolTask +{ + T* m_object; + ThreadPoolObjectDeleter(ThreadPoolObjectDeleter const&) = delete; + ThreadPoolObjectDeleter const& operator = (ThreadPoolObjectDeleter const&) = delete; +public: + /*! \brief Constructor, creates the task to delete object a obj, but doesn't schedule it. + */ + ThreadPoolObjectDeleter(T* obj) : m_object(obj) {} + /*! \brief deletes the managed object */ + bool exec() override + { + delete m_object; + return true; + } +}; + +/*! \brief Delete \a obj in the ThreadPool. + \details This function will schedule a task to be executed by the ThreadPool which will delete obj + \param obj The object to be deleted + \tparam T The type of the object to delete. Typically not required as the compiler should detect it automatically. + \return The task ID of the deletion task + \relates ThreadPoolObjectDeleter +*/ +template +inline static ThreadPool::TaskId deleteThreaded(T* obj) +{ + return ThreadPool::instance()->addTask(new ThreadPoolObjectDeleter(obj)); +} + +} // namespace xsens + +#endif diff --git a/extern/xspublic/xscontroller/broadcastdevice.cpp b/extern/xspublic/xscontroller/broadcastdevice.cpp new file mode 100644 index 0000000..da180da --- /dev/null +++ b/extern/xspublic/xscontroller/broadcastdevice.cpp @@ -0,0 +1,480 @@ + +// Copyright (c) 2003-2022 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 "broadcastdevice.h" +#include "xscontrol_def.h" +#include +#include + +using namespace xsens; + +/*! \class BroadcastDevice + \brief Intimately entangled class with XsControl that allows broadcasting to all main devices + \details This class is intended to prevent interface duplication from XsDevice to XsControl. + When broadcasting, the user can use XsControl::findDevice(XsDeviceId::broadcast()) to get the + BroadcastDevice and use it as any other device. + + The BroadcastDevice is not actually part of the device hierarchy, but it pretends to be the + parent of all mian devices is XsControl. + + The functions that are broadcast make use of the BroadcastForwardFunc class. +*/ + +/*! \brief Pure virtual base class for N-argument specific function forwarding + \details The class provides the necessary locking to safely use XsControl. +*/ +class BroadcastForwardFunc +{ +public: + BroadcastForwardFunc(BroadcastDevice* bc) : m_broadcaster(bc) {} + virtual ~BroadcastForwardFunc() {} + bool operator()() + { + LockReadWrite portLock(&m_broadcaster->m_control->m_portMutex); + portLock.lock(true); + + std::vector& ch = m_broadcaster->m_control->m_deviceList; + + bool ok = true; + XsResultValue res = (ok ? XRV_OK : XRV_NOFILEORPORTOPEN); + for (std::vector::iterator it = ch.begin(); it != ch.end(); ++it) + { + if (!call(*it)) + ok = false; + } + m_broadcaster->m_control->m_lastResult = res; + return ok; + } + + virtual bool call(XsDevice* device) = 0; + + BroadcastDevice* const m_broadcaster; +}; + +//! \brief 0-argument const forwarding function class +class ForwardConstFunc : public BroadcastForwardFunc +{ +public: + typedef bool (XsDevice::*FuncType)() const; + inline ForwardConstFunc(BroadcastDevice* bc, FuncType func) + : BroadcastForwardFunc(bc), m_func(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 +{ +public: + typedef bool (XsDevice::*FuncType)(Arg1) const; + 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); + } +private: + FuncType m_func; + Arg1 m_arg1; +}; + +//! \brief 0-argument forwarding function class +class BroadcastForwardFunc0Arg : public BroadcastForwardFunc +{ +public: + typedef bool (XsDevice::*FuncType)(); + inline BroadcastForwardFunc0Arg(BroadcastDevice* bc, FuncType func) + : BroadcastForwardFunc(bc), m_func(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 +{ +public: + typedef bool (XsDevice::*FuncType)(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); + } +private: + FuncType m_func; + Arg1 m_arg1; +}; + +//! \brief 2-argument forwarding function class +template +class BroadcastForwardFunc2Arg : public BroadcastForwardFunc +{ +public: + typedef bool (XsDevice::*FuncType)(Arg1, 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); + } +private: + FuncType m_func; + Arg1 m_arg1; + Arg2 m_arg2; +}; + +//! \brief 3-argument forwarding function class +template +class BroadcastForwardFunc3Arg : public BroadcastForwardFunc +{ +public: + typedef bool (XsDevice::*FuncType)(Arg1, Arg2, 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); + } +private: + FuncType m_func; + Arg1 m_arg1; + Arg2 m_arg2; + Arg3 m_arg3; +}; + +//! \brief 0-argument forwarding function class, void return +class BroadcastForwardFunc0ArgVoid : public BroadcastForwardFunc +{ +public: + typedef void (XsDevice::*FuncType)(); + inline BroadcastForwardFunc0ArgVoid(BroadcastDevice* bc, FuncType func) + : BroadcastForwardFunc(bc), m_func(func) {} + virtual bool call(XsDevice* device) + { + (device->*m_func)(); + return true; + } +private: + FuncType m_func; +}; + +//! \brief 1-argument forwarding function class, void return +template +class BroadcastForwardFunc1ArgVoid : public BroadcastForwardFunc +{ +public: + typedef void (XsDevice::*FuncType)(Arg1); + inline BroadcastForwardFunc1ArgVoid(BroadcastDevice* bc, FuncType func, Arg1 arg1) + : BroadcastForwardFunc(bc), m_func(func), m_arg1(arg1) {} + virtual bool call(XsDevice* device) + { + (device->*m_func)(m_arg1); + return true; + } +private: + FuncType m_func; + Arg1 m_arg1; +}; + +//! \brief 2-argument forwarding function class, void return +template +class BroadcastForwardFunc2ArgVoid : public BroadcastForwardFunc +{ +public: + typedef void (XsDevice::*FuncType)(Arg1, 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) + { + (device->*m_func)(m_arg1, m_arg2); + return true; + } +private: + FuncType m_func; + Arg1 m_arg1; + Arg2 m_arg2; +}; + +/*! \brief Constructor, sets up the XsControl reference +*/ +BroadcastDevice::BroadcastDevice(XsControl* control_) + : XsDevice(XsDeviceId(0)) + , m_control(control_) +{ +} + +/*! \brief Destructor, no special actions taken +*/ +BroadcastDevice::~BroadcastDevice() +{ + setTerminationPrepared(true); + m_control = NULL; +} + +/*! \brief Return the children of this device, actually returns the main devices in the XsControl object +*/ +std::vector BroadcastDevice::children() const +{ + return m_control->m_deviceList; +} + +//! required function to prevent pure-virtualness +bool BroadcastDevice::initialize() +{ + return false; +} +//! required function to prevent pure-virtualness +std::vector BroadcastDevice::supportedUpdateRates(XsDataIdentifier dataType) const +{ + (void)dataType; + return std::vector(); +} +//! required function to prevent pure-virtualness +XsString BroadcastDevice::productCode() const +{ + return XsString("Broadcaster"); +} +//! required function to prevent pure-virtualness +XsVersion BroadcastDevice::hardwareVersion() const +{ + return XsVersion(); +} + +/*! \brief Sets the object alignment of a device to the given \a matrix. + \param matrix The matrix to set + \details If an error is encountered, the lastResult value is set and the function returns false. + \returns True if successful + \sa objectAlignmentMatrix(), headingOffset(), setHeadingOffset(), lastResult() +*/ +bool BroadcastDevice::setObjectAlignment(const XsMatrix& matrix) +{ + return BroadcastForwardFunc1Arg(this, &XsDevice::setObjectAlignment, matrix)(); +} + +/*! \brief Start recording data +*/ +bool BroadcastDevice::startRecording() +{ + JLDEBUGG(""); + bool rv = BroadcastForwardFunc0Arg(this, &XsDevice::startRecording)(); + m_control->updateRecordingState(); + return rv; +} + +/*! \brief Stop recording data +*/ +bool BroadcastDevice::stopRecording() +{ + JLDEBUGG(""); + bool rv = BroadcastForwardFunc0Arg(this, &XsDevice::stopRecording)(); + m_control->updateRecordingState(); + return rv; +} + +/*! \brief Change the serial baudrate to baudrate +*/ +bool BroadcastDevice::setSerialBaudRate(XsBaudRate baudrate) +{ + return BroadcastForwardFunc1Arg(this, &XsDevice::setSerialBaudRate, baudrate)(); +} + +bool BroadcastDevice::loadLogFile() +{ + return BroadcastForwardFunc0Arg(this, &XsDevice::loadLogFile)(); +} + +bool BroadcastDevice::closeLogFile() +{ + return BroadcastForwardFunc0Arg(this, &XsDevice::closeLogFile)(); +} + +bool BroadcastDevice::setNoRotation(uint16_t duration) +{ + return BroadcastForwardFunc1Arg(this, &XsDevice::setNoRotation, duration)(); +} + +bool BroadcastDevice::requestBatteryLevel() +{ + return BroadcastForwardFunc0Arg(this, &XsDevice::requestBatteryLevel)(); +} + +XsTimeStamp BroadcastDevice::batteryLevelTime() +{ + return XsDevice::batteryLevelTime(); +} + +bool BroadcastDevice::abortFlushing() +{ + return BroadcastForwardFunc0Arg(this, &XsDevice::abortFlushing)(); +} + +bool BroadcastDevice::isMeasuring() const +{ + return ForwardConstFunc(const_cast(this), &XsDevice::isMeasuring)(); +} + +bool BroadcastDevice::isRecording() const +{ + return ForwardConstFunc(const_cast(this), &XsDevice::isRecording)(); +} + +bool BroadcastDevice::isReadingFromFile() const +{ + return ForwardConstFunc(const_cast(this), &XsDevice::isReadingFromFile)(); +} + +bool BroadcastDevice::gotoConfig() +{ + // this one needs to go through the list in reverse order in case of multiple devices + // for synchronization reasons + std::vector& ch = m_control->m_deviceList; + + bool ok = true; + XsResultValue res = (ok ? XRV_OK : XRV_NOFILEORPORTOPEN); + for (std::vector::reverse_iterator it = ch.rbegin(); it != ch.rend(); ++it) + { + if (!(*it)->gotoConfig()) + ok = false; + } + m_control->m_lastResult = res; + return ok; +} + +bool BroadcastDevice::gotoMeasurement() +{ + if (!BroadcastForwardFunc0Arg(this, &XsDevice::gotoMeasurement)()) + { + gotoConfig(); + return false; + } + return true; +} + +bool BroadcastDevice::setSyncSettings(const XsSyncSettingArray& s) +{ + return BroadcastForwardFunc1Arg(this, &XsDevice::setSyncSettings, s)(); +} + +bool BroadcastDevice::setHeadingOffset(double offset) +{ + return BroadcastForwardFunc1Arg(this, &XsDevice::setHeadingOffset, offset)(); +} + +bool BroadcastDevice::setLocationId(int id) +{ + return BroadcastForwardFunc1Arg(this, &XsDevice::setLocationId, id)(); +} + +bool BroadcastDevice::resetOrientation(XsResetMethod resetmethod) +{ + return BroadcastForwardFunc1Arg(this, &XsDevice::resetOrientation, resetmethod)(); +} + +bool BroadcastDevice::restoreFactoryDefaults() +{ + return BroadcastForwardFunc0Arg(this, &XsDevice::restoreFactoryDefaults)(); +} + +bool BroadcastDevice::resetLogFileReadPosition() +{ + return BroadcastForwardFunc0Arg(this, &XsDevice::resetLogFileReadPosition)(); +} + +bool BroadcastDevice::reset(bool skipDeviceIdCheck) +{ + return BroadcastForwardFunc1Arg(this, &XsDevice::reset, skipDeviceIdCheck)(); +} + +bool BroadcastDevice::setTransportMode(bool transportModeEnabled) +{ + return BroadcastForwardFunc1Arg(this, &XsDevice::setTransportMode, transportModeEnabled)(); +} + +bool BroadcastDevice::updateCachedDeviceInformation() +{ + return BroadcastForwardFunc0Arg(this, &XsDevice::updateCachedDeviceInformation)(); +} + +bool BroadcastDevice::setXdaFilterProfile(int profileType) +{ + return BroadcastForwardFunc1Arg(this, &XsDevice::setXdaFilterProfile, profileType)(); +} + +bool BroadcastDevice::setXdaFilterProfile(XsString const& profileType) +{ + return BroadcastForwardFunc1Arg(this, &XsDevice::setXdaFilterProfile, profileType)(); +} + +bool BroadcastDevice::setOnboardFilterProfile(int profileType) +{ + return BroadcastForwardFunc1Arg(this, &XsDevice::setOnboardFilterProfile, profileType)(); +} + +bool BroadcastDevice::setOnboardFilterProfile(XsString const& profileType) +{ + return BroadcastForwardFunc1Arg(this, &XsDevice::setOnboardFilterProfile, profileType)(); +} + +bool BroadcastDevice::setGravityMagnitude(double mag) +{ + return BroadcastForwardFunc1Arg(this, &XsDevice::setGravityMagnitude, mag)(); +} + +bool BroadcastDevice::storeFilterState() +{ + return BroadcastForwardFunc0Arg(this, &XsDevice::storeFilterState)(); +} + +bool BroadcastDevice::setInitialPositionLLA(const XsVector& lla) +{ + return BroadcastForwardFunc1Arg(this, &XsDevice::setInitialPositionLLA, lla)(); +} + +void BroadcastDevice::setOptions(XsOption enable, XsOption disable) +{ + BroadcastForwardFunc2ArgVoid(this, &XsDevice::setOptions, enable, disable)(); +} + +void BroadcastDevice::flushInputBuffers() +{ + BroadcastForwardFunc0ArgVoid(this, &XsDevice::flushInputBuffers)(); +} diff --git a/extern/xspublic/xscontroller/broadcastdevice.h b/extern/xspublic/xscontroller/broadcastdevice.h new file mode 100644 index 0000000..9a7f36f --- /dev/null +++ b/extern/xspublic/xscontroller/broadcastdevice.h @@ -0,0 +1,95 @@ + +// Copyright (c) 2003-2022 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 BROADCASTDEVICE_H +#define BROADCASTDEVICE_H + +#include "xsdevice_def.h" +struct XsControl; + +class BroadcastDevice : public XsDevice +{ +public: + explicit BroadcastDevice(XsControl* control); + ~BroadcastDevice() override; + + std::vector children() const; + + bool initialize() override; + + std::vector supportedUpdateRates(XsDataIdentifier dataType) const override; + XsString productCode() const override; + XsVersion hardwareVersion() const override; + + bool startRecording() override; + bool stopRecording() override; + + bool isMeasuring() const override; + bool isRecording() const override; + bool isReadingFromFile() const override final; + + bool setSerialBaudRate(XsBaudRate baudrate) override; + bool setSyncSettings(const XsSyncSettingArray& s) override; + bool gotoMeasurement() override; + bool gotoConfig() override; + bool resetOrientation(XsResetMethod resetMethod) override; + bool restoreFactoryDefaults() override; + bool reset(bool skipDeviceIdCheck = false) override; + bool loadLogFile() override; + bool closeLogFile() override; + //bool gotoOperational() override; + bool abortFlushing() override; + bool resetLogFileReadPosition() override; + bool updateCachedDeviceInformation() override; + bool setHeadingOffset(double offset) override; + bool setLocationId(int id) override; + bool setObjectAlignment(const XsMatrix& matrix) override; + bool setGravityMagnitude(double mag) override; + bool setXdaFilterProfile(int profileType) override; + bool setXdaFilterProfile(XsString const& profileType) override; + bool setOnboardFilterProfile(int profileType) override; + bool setOnboardFilterProfile(XsString const& profileType) override; + bool setNoRotation(uint16_t duration) override; + bool setInitialPositionLLA(const XsVector& lla) override; + bool storeFilterState() override; + bool requestBatteryLevel() override; + XsTimeStamp batteryLevelTime() override; + bool setTransportMode(bool transportModeEnabled) override; + void setOptions(XsOption enable, XsOption disable) override; + void flushInputBuffers() override; + +private: + friend class BroadcastForwardFunc; + XsControl* m_control; +}; + +#endif diff --git a/extern/xspublic/xscontroller/callbackmanagerxda.cpp b/extern/xspublic/xscontroller/callbackmanagerxda.cpp new file mode 100644 index 0000000..3a37551 --- /dev/null +++ b/extern/xspublic/xscontroller/callbackmanagerxda.cpp @@ -0,0 +1,615 @@ + +// Copyright (c) 2003-2022 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 "callbackmanagerxda.h" +#include + +using namespace xsens; + +/*! \brief Linked list item that contains a registered XsCallback handler for CallbackManagerXda +*/ +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 +{ + CallbackManagerXda* m_manager; //!< The callback managger + CallbackManagerItem* m_next; //!< The next item in the list or NULL if this is the last item +}; + +/*! \class CallbackManagerXda + \brief Class that delegates callbacks to registered XsCallbackHandlerItems. + + CallbackManagerXda is itself an XsCallback implementer. When a callback is triggered it walks + through its linked list of registered callbacks and calls the appropriate function in each one. + + Adding and removing handlers is done through the addXsCallbackHandlerItem() and + removeXsCallbackHandlerItem() functions. + + The callback handler can contain so called chained managers. A chained manager receives any callback + handler add/remove request that is done to the parent. The chained managers do not automatically + execute all callbacks that the parent manager executes. So chaining only affects callback + administration, not callback execution. +*/ + +/*! \brief Constructor, initializes the callback list. +*/ +CallbackManagerXda::CallbackManagerXda() +{ + m_callbackMutex = new MutexReadWrite; + m_handlerList = NULL; + m_managerList = NULL; +} + +/*! \brief Destructor, clears the callback list. +*/ +CallbackManagerXda::~CallbackManagerXda() +{ + try + { + clearChainedManagers(); + clearCallbackHandlers(false); + delete m_callbackMutex; + } + catch (...) + { + } +} + +/*! \brief Clear the callback list + \param chain If set to true clears all callback handlers + \note The name is chosen like this since it is inherited and exposed by other objects +*/ +void CallbackManagerXda::clearCallbackHandlers(bool chain) +{ + LockReadWrite locky(m_callbackMutex, LS_Write); + CallbackHandlerXdaItem* currentHdlr = m_handlerList; + while (currentHdlr) + { + CallbackHandlerXdaItem* next = currentHdlr->m_next; + delete currentHdlr; + currentHdlr = next; + } + m_handlerList = NULL; + + if (chain) + { + CallbackManagerItem* currentMgr = m_managerList; + while (currentMgr) + { + currentMgr->m_manager->clearCallbackHandlers(true); + currentMgr = currentMgr->m_next; + } + } +} + +/*! \brief Add a handler to the list + \param cb The handler to add to the list. + \param chain When set to true (default) the callback is added to chained managers as well + \note NULL and duplicate handlers are ignored, but chaining is still done. + \note The name is chosen like this since it is inherited and exposed by other objects +*/ +void CallbackManagerXda::addCallbackHandler(XsCallbackPlainC* cb, bool chain) +{ + if (!cb) + return; + + LockReadWrite locky(m_callbackMutex, LS_Write); + + if (chain) + { + CallbackManagerItem* current = m_managerList; + while (current) + { + current->m_manager->addCallbackHandler(cb, true); + current = current->m_next; + } + } + + if (!m_handlerList) + { + CallbackHandlerXdaItem* current = new CallbackHandlerXdaItem; + current->m_handler = cb; + current->m_next = NULL; + m_handlerList = current; + return; + } + + CallbackHandlerXdaItem* last; + CallbackHandlerXdaItem* current = m_handlerList; + while (current) + { + if (current->m_handler == cb) + return; + last = current; + current = current->m_next; + } + + current = new CallbackHandlerXdaItem; + current->m_handler = cb; + current->m_next = NULL; + last->m_next = current; +} + +/*! \brief Remove a handler from the list + \param cb The handler to remove from the list. + \param chain When set to true (default) the callback is added to chained managers as well + \note If \a cb is not found in the list or if \a cb is NULL, the list is not changed, but + chaining is still done. + \note The name is chosen like this since it is inherited and exposed by other objects +*/ +void CallbackManagerXda::removeCallbackHandler(XsCallbackPlainC* cb, bool chain) +{ + if (!cb) + return; + + LockReadWrite locky(m_callbackMutex, LS_Write); + + if (chain) + { + CallbackManagerItem* current = m_managerList; + while (current) + { + current->m_manager->removeCallbackHandler(cb, true); + current = current->m_next; + } + } + + CallbackHandlerXdaItem* last = NULL; + CallbackHandlerXdaItem* current = m_handlerList; + while (current) + { + if (current->m_handler == cb) + { + if (last) + last->m_next = current->m_next; + else + m_handlerList = current->m_next; + + delete current; + return; + } + last = current; + current = current->m_next; + } +} + +/*! \brief Clear the chained manager list +*/ +void CallbackManagerXda::clearChainedManagers() +{ + LockReadWrite locky(m_callbackMutex, LS_Write); + CallbackManagerItem* current = m_managerList; + while (current) + { + CallbackManagerItem* next = current->m_next; + delete current; + current = next; + } + m_managerList = NULL; +} + +/*! \brief Add a chained manager to the list + \param cm The manager to add to the list. + \note NULL and duplicate managers are ignored. +*/ +void CallbackManagerXda::addChainedManager(CallbackManagerXda* cm) +{ + if (!cm || cm == this) + return; + + LockReadWrite locky(m_callbackMutex, LS_Write); + + if (!m_managerList) + { + CallbackManagerItem* current = new CallbackManagerItem; + current->m_manager = cm; + current->m_next = NULL; + m_managerList = current; + return; + } + + CallbackManagerItem* last; + CallbackManagerItem* current = m_managerList; + while (current) + { + if (current->m_manager == cm) + return; + last = current; + current = current->m_next; + } + + current = new CallbackManagerItem; + current->m_manager = cm; + current->m_next = NULL; + last->m_next = current; +} + +/*! \brief Remove achained manager from the list + \param cm The manager to remove from the list. + \note If \a cm is not found in the list or if \a cm is NULL, the list is not changed. +*/ +void CallbackManagerXda::removeChainedManager(CallbackManagerXda* cm) +{ + if (!cm) + return; + + LockReadWrite locky(m_callbackMutex, LS_Write); + + CallbackManagerItem* last = NULL; + CallbackManagerItem* current = m_managerList; + while (current) + { + if (current->m_manager == cm) + { + if (last) + last->m_next = current->m_next; + else + m_managerList = current->m_next; + + delete current; + return; + } + last = current; + current = current->m_next; + } +} + +/*! \brief Copy all handlers from this manager into \a cm + \param cm The CallbackManagerXda to copy the handlers to + \param chain Whether to propagate the added handlers to chained managers +*/ +void CallbackManagerXda::copyCallbackHandlersTo(CallbackManagerXda* cm, bool chain) +{ + cm->copyCallbackHandlersFrom(this, chain); +} + +/*! \brief Copy all handlers from \a cm into this manager + \param cm The CallbackManagerXda to copy the handlers from + \param chain Whether to propagate the added handlers to chained managers +*/ +void CallbackManagerXda::copyCallbackHandlersFrom(CallbackManagerXda* cm, bool chain) +{ + LockReadWrite locky(m_callbackMutex, LS_Write); + LockReadWrite locky2(cm->m_callbackMutex, LS_Read); + CallbackHandlerXdaItem* current = cm->m_handlerList; + while (current) + { + addCallbackHandler(current->m_handler, chain); + current = current->m_next; + } +} + +//! \brief The XsCallback::onDeviceStateChanged() callback forwarding function +void CallbackManagerXda::onDeviceStateChanged(XsDevice* dev, XsDeviceState newState, XsDeviceState oldState) +{ + LockReadWrite locky(m_callbackMutex, LS_Read); + CallbackHandlerXdaItem* current = m_handlerList; + while (current) + { + if (current->m_handler->m_onDeviceStateChanged) + current->m_handler->m_onDeviceStateChanged(current->m_handler, dev, newState, oldState); + current = current->m_next; + } +} + +//! \brief The XsCallback::onLiveDataAvailable() callback forwarding function +void CallbackManagerXda::onLiveDataAvailable(XsDevice* dev, const XsDataPacket* packet) +{ + LockReadWrite locky(m_callbackMutex, LS_Read); + CallbackHandlerXdaItem* current = m_handlerList; + while (current) + { + if (current->m_handler->m_onLiveDataAvailable) + current->m_handler->m_onLiveDataAvailable(current->m_handler, dev, packet); + current = current->m_next; + } +} + +//! \brief The XsCallback::onAllLiveDataAvailable() callback forwarding function +void CallbackManagerXda::onAllLiveDataAvailable(XsDevicePtrArray* devs, const XsDataPacketPtrArray* packets) +{ + LockReadWrite locky(m_callbackMutex, LS_Read); + CallbackHandlerXdaItem* current = m_handlerList; + while (current) + { + if (current->m_handler->m_onAllLiveDataAvailable) + current->m_handler->m_onAllLiveDataAvailable(current->m_handler, devs, packets); + current = current->m_next; + } +} + +//! \brief The XsCallback::onMissedPackets() callback forwarding function +void CallbackManagerXda::onMissedPackets(XsDevice* dev, int count, int first, int last) +{ + LockReadWrite locky(m_callbackMutex, LS_Read); + CallbackHandlerXdaItem* current = m_handlerList; + while (current) + { + if (current->m_handler->m_onMissedPackets) + current->m_handler->m_onMissedPackets(current->m_handler, dev, count, first, last); + current = current->m_next; + } +} + +//! \brief The XsCallback::onDataUnavailable() callback forwarding function +void CallbackManagerXda::onDataUnavailable(XsDevice* dev, int64_t packetId) +{ + LockReadWrite locky(m_callbackMutex, LS_Read); + CallbackHandlerXdaItem* current = m_handlerList; + while (current) + { + if (current->m_handler->m_onDataUnavailable) + current->m_handler->m_onDataUnavailable(current->m_handler, dev, packetId); + current = current->m_next; + } +} + +//! \brief The XsCallback::onWakeupReceived() callback forwarding function +void CallbackManagerXda::onWakeupReceived(XsDevice* dev) +{ + LockReadWrite locky(m_callbackMutex, LS_Read); + CallbackHandlerXdaItem* current = m_handlerList; + while (current) + { + if (current->m_handler->m_onWakeupReceived) + current->m_handler->m_onWakeupReceived(current->m_handler, dev); + current = current->m_next; + } +} + +//! \brief The XsCallback::onProgressUpdated() callback forwarding function +void CallbackManagerXda::onProgressUpdated(XsDevice* dev, int curr, int total, const XsString* identifier) +{ + LockReadWrite locky(m_callbackMutex, LS_Read); + CallbackHandlerXdaItem* current = m_handlerList; + while (current) + { + if (current->m_handler->m_onProgressUpdated) + current->m_handler->m_onProgressUpdated(current->m_handler, dev, curr, total, identifier); + current = current->m_next; + } +} + +//! \brief The XsCallback::onWriteMessageToLogFile() callback forwarding function +int CallbackManagerXda::onWriteMessageToLogFile(XsDevice* dev, const XsMessage* message) +{ + LockReadWrite locky(m_callbackMutex, LS_Read); + CallbackHandlerXdaItem* current = m_handlerList; + bool rv = true; + while (current) + { + if (current->m_handler->m_onWriteMessageToLogFile) + rv = current->m_handler->m_onWriteMessageToLogFile(current->m_handler, dev, message) && rv; + current = current->m_next; + } + return rv ? 1 : 0; +} + +//! \brief The XsCallback::onBufferedDataAvailable() callback forwarding function +void CallbackManagerXda::onBufferedDataAvailable(XsDevice* dev, const XsDataPacket* data) +{ + LockReadWrite locky(m_callbackMutex, LS_Read); + CallbackHandlerXdaItem* current = m_handlerList; + while (current) + { + if (current->m_handler->m_onBufferedDataAvailable) + current->m_handler->m_onBufferedDataAvailable(current->m_handler, dev, data); + current = current->m_next; + } +} + +//! \brief The XsCallback::onAllBufferedDataAvailable() callback forwarding function +void CallbackManagerXda::onAllBufferedDataAvailable(XsDevicePtrArray* devs, const XsDataPacketPtrArray* packets) +{ + LockReadWrite locky(m_callbackMutex, LS_Read); + CallbackHandlerXdaItem* current = m_handlerList; + while (current) + { + if (current->m_handler->m_onAllBufferedDataAvailable) + current->m_handler->m_onAllBufferedDataAvailable(current->m_handler, devs, packets); + current = current->m_next; + } +} + +//! \brief The XsCallback::onConnectivityChanged() callback forwarding function +void CallbackManagerXda::onConnectivityChanged(XsDevice* dev, XsConnectivityState newState) +{ + LockReadWrite locky(m_callbackMutex, LS_Read); + CallbackHandlerXdaItem* current = m_handlerList; + while (current) + { + if (current->m_handler->m_onConnectivityChanged) + current->m_handler->m_onConnectivityChanged(current->m_handler, dev, newState); + current = current->m_next; + } +} + +//! \brief The XsCallback::onInfoResponse() callback forwarding function +void CallbackManagerXda::onInfoResponse(XsDevice* dev, XsInfoRequest request) +{ + LockReadWrite locky(m_callbackMutex, LS_Read); + CallbackHandlerXdaItem* current = m_handlerList; + while (current) + { + if (current->m_handler->m_onInfoResponse) + current->m_handler->m_onInfoResponse(current->m_handler, dev, request); + current = current->m_next; + } +} + +//! \brief The Xscallback::onError() callback forwarding function +void CallbackManagerXda::onError(XsDevice* dev, XsResultValue error) +{ + LockReadWrite locky(m_callbackMutex, LS_Read); + CallbackHandlerXdaItem* current = m_handlerList; + while (current) + { + if (current->m_handler->m_onError) + current->m_handler->m_onError(current->m_handler, dev, error); + current = current->m_next; + } +} + +//! \brief The Xscallback::onNonDataMessage() callback forwarding function +void CallbackManagerXda::onNonDataMessage(XsDevice* dev, XsMessage const* message) +{ + LockReadWrite locky(m_callbackMutex, LS_Read); + CallbackHandlerXdaItem* current = m_handlerList; + while (current) + { + if (current->m_handler->m_onNonDataMessage) + current->m_handler->m_onNonDataMessage(current->m_handler, dev, message); + current = current->m_next; + } +} + +//! \brief The Xscallback::onMessageReceivedFromDevice() callback forwarding function +void CallbackManagerXda::onMessageDetected(XsDevice* dev, XsProtocolType type, XsByteArray const* rawMessage) +{ + LockReadWrite locky(m_callbackMutex, LS_Read); + CallbackHandlerXdaItem* current = m_handlerList; + while (current) + { + if (current->m_handler->m_onMessageDetected) + current->m_handler->m_onMessageDetected(current->m_handler, dev, type, rawMessage); + current = current->m_next; + } +} + +//! \brief The Xscallback::onMessageReceivedFromDevice() callback forwarding function +void CallbackManagerXda::onMessageReceivedFromDevice(XsDevice* dev, XsMessage const* message) +{ + LockReadWrite locky(m_callbackMutex, LS_Read); + CallbackHandlerXdaItem* current = m_handlerList; + while (current) + { + if (current->m_handler->m_onMessageReceivedFromDevice) + current->m_handler->m_onMessageReceivedFromDevice(current->m_handler, dev, message); + current = current->m_next; + } +} + +//! \brief The Xscallback::onMessageSentToDevice() callback forwarding function +void CallbackManagerXda::onMessageSentToDevice(XsDevice* dev, XsMessage const* message) +{ + LockReadWrite locky(m_callbackMutex, LS_Read); + CallbackHandlerXdaItem* current = m_handlerList; + while (current) + { + if (current->m_handler->m_onMessageSentToDevice) + current->m_handler->m_onMessageSentToDevice(current->m_handler, dev, message); + current = current->m_next; + } +} + +//! \brief The XsCallback::onDataAvailable() callback forwarding function +void CallbackManagerXda::onDataAvailable(XsDevice* dev, const XsDataPacket* packet) +{ + LockReadWrite locky(m_callbackMutex, LS_Read); + CallbackHandlerXdaItem* current = m_handlerList; + while (current) + { + if (current->m_handler->m_onDataAvailable) + current->m_handler->m_onDataAvailable(current->m_handler, dev, packet); + current = current->m_next; + } +} + +//! \brief The XsCallback::onAllDataAvailable() callback forwarding function +void CallbackManagerXda::onAllDataAvailable(XsDevicePtrArray* devs, const XsDataPacketPtrArray* packets) +{ + LockReadWrite locky(m_callbackMutex, LS_Read); + CallbackHandlerXdaItem* current = m_handlerList; + while (current) + { + if (current->m_handler->m_onAllDataAvailable) + current->m_handler->m_onAllDataAvailable(current->m_handler, devs, packets); + current = current->m_next; + } +} + +//! \brief The XsCallback::onRecordedDataAvailable() callback forwarding function +void CallbackManagerXda::onRecordedDataAvailable(XsDevice* dev, const XsDataPacket* packet) +{ + LockReadWrite locky(m_callbackMutex, LS_Read); + CallbackHandlerXdaItem* current = m_handlerList; + while (current) + { + if (current->m_handler->m_onRecordedDataAvailable) + current->m_handler->m_onRecordedDataAvailable(current->m_handler, dev, packet); + current = current->m_next; + } +} + +//! \brief The XsCallback::onAllRecordedDataAvailable() callback forwarding function +void CallbackManagerXda::onAllRecordedDataAvailable(XsDevicePtrArray* devs, const XsDataPacketPtrArray* packets) +{ + LockReadWrite locky(m_callbackMutex, LS_Read); + CallbackHandlerXdaItem* current = m_handlerList; + while (current) + { + if (current->m_handler->m_onAllRecordedDataAvailable) + current->m_handler->m_onAllRecordedDataAvailable(current->m_handler, devs, packets); + current = current->m_next; + } +} + +void CallbackManagerXda::onTransmissionRequest(int channelId, const XsByteArray* data) +{ + LockReadWrite locky(m_callbackMutex, LS_Read); + CallbackHandlerXdaItem* current = m_handlerList; + while (current) + { + if (current->m_handler->m_onTransmissionRequest) + current->m_handler->m_onTransmissionRequest(current->m_handler, channelId, data); + current = current->m_next; + } +} + +//! \brief The Xscallback::onRestoreCommunication callback forwarding function +void CallbackManagerXda::onRestoreCommunication(const XsString* portName, XsResultValue result) +{ + LockReadWrite locky(m_callbackMutex, LS_Read); + CallbackHandlerXdaItem* current = m_handlerList; + while (current) + { + if (current->m_handler->m_onRestoreCommunication) + current->m_handler->m_onRestoreCommunication(current->m_handler, portName, result); + current = current->m_next; + } +} diff --git a/extern/xspublic/xscontroller/callbackmanagerxda.h b/extern/xspublic/xscontroller/callbackmanagerxda.h new file mode 100644 index 0000000..8818b4f --- /dev/null +++ b/extern/xspublic/xscontroller/callbackmanagerxda.h @@ -0,0 +1,96 @@ + +// Copyright (c) 2003-2022 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 CALLBACKMANAGERXDA_H +#define CALLBACKMANAGERXDA_H + +#include "xscallback.h" + +struct CallbackHandlerXdaItem; +struct CallbackManagerItem; +namespace xsens +{ +class MutexReadWrite; +} +struct MtwInfo; + +class CallbackManagerXda : public XsCallback +{ +public: + void onDeviceStateChanged(XsDevice* dev, XsDeviceState newState, XsDeviceState oldState) override; + void onLiveDataAvailable(XsDevice* dev, const XsDataPacket* packet) override; + void onAllLiveDataAvailable(XsDevicePtrArray* devs, const XsDataPacketPtrArray* packets) override; + void onMissedPackets(XsDevice* dev, int count, int first, int last) override; + void onDataUnavailable(XsDevice* dev, int64_t packetId) override; + void onWakeupReceived(XsDevice* dev) override; + void onProgressUpdated(XsDevice* dev, int current, int total, const XsString* identifier) override; + int onWriteMessageToLogFile(XsDevice* dev, const XsMessage* message) override; + void onBufferedDataAvailable(XsDevice* dev, const XsDataPacket* data) override; + void onAllBufferedDataAvailable(XsDevicePtrArray* devs, const XsDataPacketPtrArray* packets) override; + 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 onDataAvailable(XsDevice* dev, const XsDataPacket* data) override; + void onAllDataAvailable(XsDevicePtrArray* devs, const XsDataPacketPtrArray* packets) override; + void onRecordedDataAvailable(XsDevice* dev, const XsDataPacket* data) override; + void onAllRecordedDataAvailable(XsDevicePtrArray* devs, const XsDataPacketPtrArray* packets) override; + void onTransmissionRequest(int channelId, const XsByteArray* data) override; + void onRestoreCommunication(const XsString* portName, XsResultValue result) override; + + CallbackManagerXda(); + ~CallbackManagerXda(); + + void clearCallbackHandlers(bool chain = true); + void addCallbackHandler(XsCallbackPlainC* cb, bool chain = true); + void removeCallbackHandler(XsCallbackPlainC* cb, bool chain = true); + + void clearChainedManagers(); + void addChainedManager(CallbackManagerXda* cm); + void removeChainedManager(CallbackManagerXda* cm); + + void copyCallbackHandlersTo(CallbackManagerXda* cm, bool chain = true); + void copyCallbackHandlersFrom(CallbackManagerXda* cm, bool chain = true); + +private: + xsens::MutexReadWrite* m_callbackMutex; //!< Administration mutex + CallbackHandlerXdaItem* m_handlerList; //!< The first item in the linked list of callback handlers + CallbackManagerItem* m_managerList; //!< The first item in the linked list of child callback managers + + CallbackManagerXda(CallbackManagerXda const&) = delete; + CallbackManagerXda const& operator=(CallbackManagerXda const&) = delete; +}; + +#endif diff --git a/extern/xspublic/xscontroller/clocksynccommand.h b/extern/xspublic/xscontroller/clocksynccommand.h new file mode 100644 index 0000000..b7b7d57 --- /dev/null +++ b/extern/xspublic/xscontroller/clocksynccommand.h @@ -0,0 +1,47 @@ + +// Copyright (c) 2003-2022 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 CLOCKSYNCCOMMAND_H +#define CLOCKSYNCCOMMAND_H + +/*! \brief Clock synchronization commands + \details To be used in XMID_ClockSyncCommand and XMID_ClockSyncCommandAck messages +*/ +enum ClockSyncCommand +{ + CSC_Request = 0x00, //!< Command to request current clock value + CSC_SetPending = 0x01, //!< Command to pend a new clock value + CSC_ConfirmPending = 0x02 //!< Command to confirm/apply the pending new clock value +}; + +#endif + diff --git a/extern/xspublic/xscontroller/communicator.cpp b/extern/xspublic/xscontroller/communicator.cpp new file mode 100644 index 0000000..4525892 --- /dev/null +++ b/extern/xspublic/xscontroller/communicator.cpp @@ -0,0 +1,326 @@ + +// Copyright (c) 2003-2022 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 "communicator.h" +#include "protocolhandler.h" +#include "replymonitor.h" +#include +#include "usbinterface.h" +#include +#include +#include "xsdeviceconfiguration.h" +#include "xsusbhubinfo.h" +#include "xsscanner.h" +#include +#include +#include "xsdevice_def.h" + +using namespace xsens; +using namespace XsTime; + +/*! \brief Constructor, creates some management objects and clears the rest by calling initialize() +*/ +Communicator::Communicator(void) + : m_preparedForDestruction(false) + , m_masterInfo(nullptr) + , m_protocolManager(new ProtocolManager(*this)) + , m_masterDeviceId(0) + , m_replyMonitor(new ReplyMonitor) + , m_lastResult(XRV_OK) + , m_defaultTimeout(500) +{ +#ifdef XSENS_DEBUG + ProtocolManager::value_type addOk = protocolManager()->add(new ProtocolHandler()); + assert(addOk); +#else + protocolManager()->add(new ProtocolHandler()); +#endif + + JLDEBUGG("Created " << (void*) this); +} + +/*! \brief Destructor, waits for the last scheduled task to complete and then cleans up the object by calling clear() +*/ +Communicator::~Communicator() +{ + JLDEBUGG("Destroyed " << (void*) this); + assert(m_preparedForDestruction); + m_masterInfo = nullptr; +} + +/*! \brief Prepares communicator for destruction +*/ +void Communicator::prepareForDestruction() +{ + m_replyMonitor.reset(nullptr); + m_protocolManager.reset(); + + m_preparedForDestruction = true; +} + + +/*! \brief Sets a master device. + \param masterDevice a master device. +*/ +void Communicator::setMasterDevice(XsDevice* masterDevice) +{ + JLDEBUGG(masterDevice); + assert(m_masterInfo == nullptr); + m_masterInfo = masterDevice; +} + +/*! \returns a master device +*/ +XsDevice* Communicator::masterDevice() const +{ + return m_masterInfo; +} + +/*! \returns a protocol manager +*/ +std::shared_ptr Communicator::protocolManager() const +{ + return m_protocolManager; +} + +/*! \brief Sets a master device ID. + \param id device ID. +*/ +void Communicator::setMasterDeviceId(const XsDeviceId& id) +{ + m_masterDeviceId = id; +} + +/*! \returns a master device ID +*/ +XsDeviceId Communicator::masterDeviceId() const +{ + return m_masterDeviceId; +} + +/*! \brief Get the result value of the last operation. + \details The result values are codes that describe a failure in more detail. + \returns the last known error code + \sa resultText(XsResultValue), lastResultText() +*/ +XsResultValue Communicator::lastResult() const +{ + return m_lastResult; +} + +/*! \brief Get the accompanying error text for the value returned by lastResult() + It may provide situation-specific information instead. + \returns a human readable error description + \sa resultText(XsResultValue), lastResult() +*/ +XsString Communicator::lastResultText() const +{ + return m_lastResultText; +} + +/*! \returns a child device count +*/ +XsSize Communicator::childDeviceCount() const +{ + return m_masterInfo ? m_masterInfo->deviceConfigurationConst().numberOfDevices() : 0; +} + +/*! \brief Handles a \a 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); + + //JLTRACEG("Received " << JLHEXLOG(message.getMessageId()) << " size " << message.getTotalMessageSize()); + if (message.getMessageId() == XMID_Error) + { + char buffer[256]; + 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; + JLALERTG("Error message received: " << buffer); + JLIF(gJournal, JLL_Alert, m_replyMonitor->dumpObjectList(gJournal, JLL_Alert)); + } + if (!m_replyMonitor->addReply(message) && m_masterInfo) + m_masterInfo->handleMessage(message); +} + +/*! \brief Write a message and await the reply +*/ +bool Communicator::doTransaction(const XsMessage& msg, uint32_t timeout) +{ + XsMessage rcv; + return doTransaction(msg, rcv, timeout); +} + +/*! \brief Write a message and await the reply +*/ +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) +{ + return doTransaction(msg, rcv, defaultTimeout()); +} + +/*! \brief Sets the last result + \param res a result value + \param text a text string +*/ +void Communicator::setLastResult(XsResultValue res, XsString const& text) const +{ + (void)setAndReturnLastResult(res, text); +} + +/*! \brief Sets the last result and returns it + \param res a result value + \param text a text string + \returns XRV_OK if successful +*/ +XsResultValue Communicator::setAndReturnLastResult(XsResultValue res, XsString const& text) const +{ + m_lastResultText = text; + return m_lastResult = res; +} + +/*! \brief Do a sanity check on a potential message + \param msg a message + \details This check is done to prevent message loss due to a 'valid' message in a message + Check if the busId can be valid. It can be XS_BID_MASTER and is must be below the number of devices + The XS_BID_BROADCAST cannot be checked due to a bug in AwindaStation firmware 1.0.9 which sends flushing indications with BID 0 + 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 +{ + if (m_masterInfo) + return m_masterInfo->messageLooksSane(msg); + + return true; +} + +/*! \brief Add a MidReplyObject + \param[in] mid the id of the message to wait for + \returns a shared pointer to a reply object +*/ +std::shared_ptr Communicator::addReplyObject(uint8_t mid) +{ + assert(m_replyMonitor != nullptr); + return m_replyMonitor->addReplyObject(new MidReplyObject(mid)); +} + +/*! \brief Add a MidAndDataReplyObject + \param[in] mid the message id of the message to wait for + \param[in] offset the offset in the data part of the message + \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) + \returns a shared pointer to a reply object +*/ +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)); +} + +/*! \brief Add a custom ReplyObject + \param[in] obj The reply object to add + \returns a shared pointer to the supplied reply object +*/ +std::shared_ptr Communicator::addReplyObject(ReplyObject* obj) +{ + assert(m_replyMonitor != nullptr && obj != nullptr); + return m_replyMonitor->addReplyObject(obj); +} + +void Communicator::addProtocolHandler(IProtocolHandler* handler) +{ + assert(handler != 0); +#ifdef XSENS_DEBUG + ProtocolManager::value_type addOk = protocolManager()->add(handler); + assert(addOk); +#else + protocolManager()->add(handler); +#endif +} + +/*! \brief Removes a protocol handler + \param type a \ref XsProtocolType +*/ +void Communicator::removeProtocolHandler(XsProtocolType type) +{ + bool removeOk = protocolManager()->remove(type); + assert(removeOk); + (void)removeOk; // Prevent unused variable warning in release builds +} + +/*! \returns true when a protocol with type \a type has been added + \param[in] type The protocol type to check +*/ +bool Communicator::hasProtocol(XsProtocolType type) const +{ + return protocolManager()->hasProtocol(type); +} + +/*! \brief Set the credentials required for using the device + \details Mostly used for network access + \param id The username or system ID or similar + \param key The authentication key or password or similar +*/ +void Communicator::setCredentials(XsString const& id, XsString const& key) +{ + (void) id; + (void) key; +} + +/*! \brief Destroys the communicator. */ +void Communicator::destroy() +{ + prepareForDestruction(); + delete this; +} + +bool Communicator::isLoadLogFileInProgress() const +{ + return false; +} + +bool Communicator::allowReprocessing() const +{ + return true; +} diff --git a/extern/xspublic/xscontroller/communicator.h b/extern/xspublic/xscontroller/communicator.h new file mode 100644 index 0000000..3ff2816 --- /dev/null +++ b/extern/xspublic/xscontroller/communicator.h @@ -0,0 +1,343 @@ + +// Copyright (c) 2003-2022 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 COMMUNICATOR_H +#define COMMUNICATOR_H + +#include "callbackmanagerxda.h" +#include "openportstage.h" +#include "protocolmanager.h" +#include "replyobject.h" + +#include +#include "iointerfacefile.h" +#include "iprotocolhandler.h" +#include "serialinterface.h" +#include +#include + +#include +#include + +struct XsByteArray; +struct XsString; +struct XsMessage; +struct XsDeviceConfiguration; +namespace xsens +{ +class ReplyMonitor; +} + +/*! \class Communicator + \brief A base struct for a communication interface +*/ +struct Communicator : public CallbackManagerXda +{ +public: + void destroy(); + + //! \brief The communicator deleter + struct Deleter + { + //! \brief A function that destroys communicator + void operator()(Communicator* c) + { + if (c != nullptr) + c->destroy(); + } + }; + + //! \brief Initializes of UniquePtr + template + using UniquePtr = std::unique_ptr; + + //! \brief Constructs a new Communicator of type T and returns it as a UniquePtr + template + static UniquePtr createUniquePtr() + { + return UniquePtr(new T(), Deleter()); + } + + //! \brief Creates a UniquePtr from a Communicator* + template + static UniquePtr createUniquePtr(T* communicator) + { + return UniquePtr(communicator, Deleter()); + } + + Communicator(void); + + 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; + + //! \brief Sets a default \a timeout + void setDefaultTimeout(uint32_t timeout) + { + m_defaultTimeout = timeout; + } + + //! \returns a default timeout + uint32_t defaultTimeout() const + { + return m_defaultTimeout; + } + + XsResultValue lastResult() const; + XsString lastResultText() const; + 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(ReplyObject* obj); + + // live stuff + + /*! \brief Request a device to go to config mode + \param detectRs485 when set to true it will try to detect and use an RS485 interface + \returns XRV_OK if succeeded + */ + virtual XsResultValue gotoConfig(bool detectRs485 = false) = 0; + + /*! \brief Request a device to go to measurement mode + \returns XRV_OK if succeeded + */ + virtual XsResultValue gotoMeasurement() = 0; + + /*! \brief Request a device to get device ID + \returns XRV_OK if succeeded + */ + virtual XsResultValue getDeviceId() = 0; + + /*! \brief Set the timeout for the gotoConfig function + \param timeout The desired timeout value in ms, if 0 the default value is used + */ + virtual void setGotoConfigTimeout(uint32_t timeout) = 0; + + /*! \brief Write message to the device + \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; + + /*! \brief Flushes all remaining data on the open port + */ + virtual void flushPort() = 0; + + /*! \brief Closes the open port + */ + virtual void closePort() = 0; + + /*! \brief Schedules to close the open port + */ + virtual void scheduleClosePort() + { + closePort(); + } + + /*! \returns true if the port is open + */ + virtual bool isPortOpen() const = 0; + + /*! \returns XsPortInfo of the current port + */ + virtual XsPortInfo portInfo() const = 0; + + /*! \brief Opens a port + \param portInfo A port information that you want to open + \param stage A openning stage of a communication port + \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; + + /*! \brief Reopens the port + \param stage A openning stage of a communication port + \param skipDeviceIdCheck When set to true it will skip device ID check + \returns true if port is successfully reopened + */ + virtual bool reopenPort(OpenPortStage stage = OPS_Full, bool skipDeviceIdCheck = false) = 0; + + /*! \brief Returns true if the \a other device is docked at this device + */ + virtual bool isDockedAt(Communicator* other) const = 0; + + /*! \brief Either disable or enable (default) the keep alive mechanism (if supported by the device) + */ + virtual void setKeepAlive(bool enable) = 0; + + // file stuff + + /*! \brief Close the log file + \returns true if the log file was successfully closed or never open + */ + virtual void closeLogFile() = 0; + + /*! \brief Read a message from the open file + \param msgId an ID of message + \returns The message that was read or if no matching message was found a cleared message + */ + virtual XsMessage readMessage(uint8_t msgId = 0) = 0; + + /*! \brief Read a message from the start of the open file + \param msgId an ID of message + \param maxMsgs a maximum of messages to read + \returns The message that was read or if no matching message was found a cleared message + */ + virtual XsMessage readMessageFromStartOfFile(uint8_t msgId, int maxMsgs = 0) = 0; + + /*! \brief Read multiple similar messages from the start of the open file + \param msgId an ID of message + \param maxMsgs a maximum of messages to read + \returns The message that was read or if no matching message was found a cleared message + */ + virtual std::deque readMessagesFromStartOfFile(uint8_t msgId, int maxMsgs = 0) = 0; + + /*! \brief Load a complete logfile + */ + virtual void loadLogFile(XsDevice* device) = 0; + + /*! \brief Aborts loading a logfile + */ + virtual void abortLoadLogFile() = 0; + + /*! \brief Open the log file + ` \param filename a name of file to open + \returns true if the file was opened successfully + */ + virtual bool openLogFile(const XsString& filename) = 0; + + /*! \returns The name of the logfile + */ + virtual XsString logFileName() const = 0; + + /*! \returns The size of the logfile + */ + virtual XsFilePos logFileSize() const = 0; + + /*! \returns The date of the logfile + */ + virtual XsTimeStamp logFileDate() const = 0; + + /*! \returns The read position of the logfile + */ + virtual XsFilePos logFileReadPosition() const = 0; + + /*! \brief Resets the logfile read position + */ + virtual void resetLogFileReadPosition(void) = 0; + + /*! \returns true if we are reading from the file + */ + virtual bool isReadingFromFile() const = 0; + + /*! \brief Wait for the last processing task to complete in the threadpool + */ + virtual void waitForLastTaskCompletion() = 0; + + /*! \returns true if the file operation started by loadLogFile is still in progress + */ + virtual bool isLoadLogFileInProgress() const; + + /*! \returns true if reprocessing is allowed + */ + virtual bool allowReprocessing() const; + + /*! \returns The maximum set of messages, from the beginning of a file, which must contain a Configuration message + \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 20; + } + + virtual void setCredentials(XsString const& id, XsString const& key); + + bool sanityCheck(XsMessage const& msg) const; + + /*! \brief Adds a protocol handler + \param handler an \ref IProtocolHandler + */ + virtual void addProtocolHandler(IProtocolHandler* handler); + void removeProtocolHandler(XsProtocolType type); + bool hasProtocol(XsProtocolType type) const; + + void setMasterDevice(XsDevice* masterDevice); + +protected: + virtual ~Communicator(); + virtual void prepareForDestruction(); + + 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); + + //! \brief Prepared for destruction boolean variable + bool m_preparedForDestruction; + + //! \brief A master device object + XsDevice* m_masterInfo; + + //! \brief A shared pointer to protocl manager + std::shared_ptr m_protocolManager; + + //! \brief A master device ID + XsDeviceId m_masterDeviceId; + + //! \brief An unique pointer to a reply monitor + std::unique_ptr m_replyMonitor; + + //! \brief A last result variable + mutable XsResultValue m_lastResult; + + //! \brief A last result string + mutable XsString m_lastResultText; + + //! \brief A handle mutex + mutable xsens::Mutex m_handleMux; + + //! \brief A default timeout variable + uint32_t m_defaultTimeout; +}; + +#endif diff --git a/extern/xspublic/xscontroller/communicatorfactory.cpp b/extern/xspublic/xscontroller/communicatorfactory.cpp new file mode 100644 index 0000000..1e4c989 --- /dev/null +++ b/extern/xspublic/xscontroller/communicatorfactory.cpp @@ -0,0 +1,82 @@ + +// Copyright (c) 2003-2022 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 "communicatorfactory.h" + +/*! \brief Create the factory +*/ +CommunicatorFactory::CommunicatorFactory() +{ +} + +/*! \brief Destory the factory +*/ +CommunicatorFactory::~CommunicatorFactory() +{ +} + +/*! \brief Register a communicator type with the factory +*/ +bool CommunicatorFactory::registerType(CommunicatorTypeId typeId, CommunicatorConstructFunc constructFunc, PortInfoMatchFunc matchFunc) +{ + return m_constructors.emplace(ConstructorsMap::value_type(typeId, std::make_pair(constructFunc, matchFunc))).second; +} + +/*! \brief Create a communicator based on a port info +*/ +Communicator* CommunicatorFactory::create(const XsPortInfo& portInfo) const +{ + CommunicatorTypeId id = portInfoToCommunicatorId(portInfo); + return construct(id); +} + +/*! \brief Create a communicator based on a filename +*/ +Communicator* CommunicatorFactory::create(const XsString& filename) const +{ + CommunicatorTypeId id = filenameToCommunicatorId(filename); + return construct(id); +} + +/*! \brief Construct a communicator based on \a typeId +*/ +Communicator* CommunicatorFactory::construct(CommunicatorTypeId typeId) const +{ + Communicator* c = nullptr; + if (typeId != CommunicatorType::INVALID) + { + ConstructorsMap::const_iterator i = m_constructors.find(typeId); + if (i != m_constructors.end()) + c = i->second.first(); + } + return c; +} diff --git a/extern/xspublic/xscontroller/communicatorfactory.h b/extern/xspublic/xscontroller/communicatorfactory.h new file mode 100644 index 0000000..7f10a23 --- /dev/null +++ b/extern/xspublic/xscontroller/communicatorfactory.h @@ -0,0 +1,114 @@ + +// Copyright (c) 2003-2022 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 COMMUNICATORFACTORY_H +#define COMMUNICATORFACTORY_H + +#include "compat.h" + +#include +#include + +struct Communicator; +struct XsString; +struct XsPortInfo; + +/*! \class CommunicatorFactory + \brief A Factory for the communicators +*/ +class CommunicatorFactory +{ +public: + /*! \brief Create a new CommunicatorFactory and register its basic types + \return A unique pointer to the created factory + */ + template + static std::unique_ptr createFactory() + { + auto factory = std::make_unique(); + factory->registerCommunicatorTypes(); + return factory; + } + + CommunicatorFactory(); + virtual ~CommunicatorFactory(); + + //! The typedef of the communicator type ID + typedef unsigned int CommunicatorTypeId; + + //! The typedef of the communicator constructor function + typedef Communicator* (*CommunicatorConstructFunc)(); + + //! The typedef of the port info match function + typedef bool (*PortInfoMatchFunc)(const XsPortInfo&); + + Communicator* create(const XsPortInfo& portInfo) const; + Communicator* create(const XsString& filename) const; + + bool registerType(CommunicatorTypeId typeId, CommunicatorConstructFunc constructFunc, PortInfoMatchFunc matchFunc); + + /*! \brief Match a filename to a communicator + \param filename A name of file + \returns A communicator type ID + */ + 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; + + /*! \brief Registrates communicator types */ + virtual void registerCommunicatorTypes() {} + +protected: + //! The typedef of a map for a communicator type ID and constructors map + 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; + +private: + ConstructorsMap m_constructors; +}; + +namespace CommunicatorType +{ +static const CommunicatorFactory::CommunicatorTypeId INVALID = 0; +} + +#endif diff --git a/extern/xspublic/xscontroller/compat.h b/extern/xspublic/xscontroller/compat.h new file mode 100644 index 0000000..ba6fb16 --- /dev/null +++ b/extern/xspublic/xscontroller/compat.h @@ -0,0 +1,53 @@ + +// Copyright (c) 2003-2022 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 __COMPAT_H +#define __COMPAT_H + +#ifdef __GNUC__ +#if __cplusplus < 201402L +/* + 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) +{ + return std::unique_ptr(new T(std::forward(args)...)); +} +} +#endif +#endif + +#endif diff --git a/extern/xspublic/xscontroller/datalogger.cpp b/extern/xspublic/xscontroller/datalogger.cpp new file mode 100644 index 0000000..4bbdf50 --- /dev/null +++ b/extern/xspublic/xscontroller/datalogger.cpp @@ -0,0 +1,41 @@ + +// Copyright (c) 2003-2022 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 "datalogger.h" + +DataLogger::DataLogger() +{ +} + +DataLogger::~DataLogger() +{ +} diff --git a/extern/xspublic/xscontroller/datalogger.h b/extern/xspublic/xscontroller/datalogger.h new file mode 100644 index 0000000..e8313c3 --- /dev/null +++ b/extern/xspublic/xscontroller/datalogger.h @@ -0,0 +1,54 @@ + +// Copyright (c) 2003-2022 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 DATALOGGER_H +#define DATALOGGER_H + +#include "callbackmanagerxda.h" + +/*! \class DataLogger + \brief An abstract class for data logging +*/ +class DataLogger : public CallbackManagerXda +{ +public: + DataLogger(); + virtual ~DataLogger(); + + //! \brief Write a message to the logging stream + virtual bool writeMessage(const XsMessage& message) = 0; + + //! \brief Closes a file + virtual void close() = 0; +}; + +#endif diff --git a/extern/xspublic/xscontroller/datapacketcache.cpp b/extern/xspublic/xscontroller/datapacketcache.cpp new file mode 100644 index 0000000..a02e3ec --- /dev/null +++ b/extern/xspublic/xscontroller/datapacketcache.cpp @@ -0,0 +1,39 @@ + +// Copyright (c) 2003-2022 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 "datapacketcache.h" +#include + +std::pair DataPacketCache::insert(int64_t id, XsDataPacket const& p) +{ + return insert(std::make_pair(id, new XsDataPacket(p))); +} diff --git a/extern/xspublic/xscontroller/datapacketcache.h b/extern/xspublic/xscontroller/datapacketcache.h new file mode 100644 index 0000000..7638729 --- /dev/null +++ b/extern/xspublic/xscontroller/datapacketcache.h @@ -0,0 +1,59 @@ + +// Copyright (c) 2003-2022 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 DATAPACKETCACHE_H +#define DATAPACKETCACHE_H + +#include +#include +struct XsDataPacket; + +/*! \class DataPacketCache + \brief A cache of data packets +*/ +class DataPacketCache : public std::map +{ +public: + + //! \brief A map typed of integer values and data packets + typedef std::map Base; + using Base::insert; + + /*! \brief Inserts a data packet + \param id The ID of data packet + \param p The data packet to insert + \returns A pair of iterator and boolean value + */ + std::pair insert(int64_t id, XsDataPacket const& p); +}; + +#endif diff --git a/extern/xspublic/xscontroller/dataparser.cpp b/extern/xspublic/xscontroller/dataparser.cpp new file mode 100644 index 0000000..7ff8812 --- /dev/null +++ b/extern/xspublic/xscontroller/dataparser.cpp @@ -0,0 +1,150 @@ + +// Copyright (c) 2003-2022 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 "dataparser.h" +#include "xscontrollerconfig.h" +#include + + +/*! \class DataParser + \brief A class for the data parsing on a separate thread +*/ + +/*! \brief Default constructor +*/ +DataParser::DataParser() +{ + JLDEBUGG("Starting DataParser " << this); + startThread(); +} + +DataParser::~DataParser() +{ + try + { + JLDEBUGG("Stopping DataParser " << this); + terminate(); + } + catch (...) + { + } +} + +/*! \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) +{ + xsens::Lock locky(&m_incomingMutex); + m_incoming.push(arr); + locky.unlock(); + m_newDataEvent.set(); +} + +/*! \brief The inner thread function +*/ +int32_t DataParser::innerFunction() +{ + // wait for new data + if (!m_newDataEvent.wait()) + return 1; // no new data available (so we are keeping up easily), give other threads a little more breathing room + + // get new data + XsByteArray raw; + xsens::Lock lockIncoming(&m_incomingMutex); + while (!m_incoming.empty() && !isTerminating()) + { + raw.append(m_incoming.front()); + m_incoming.pop(); + lockIncoming.unlock(); + + JLTRACEG("raw size: " << raw.size()); + + // process data + if (!raw.empty() && !isTerminating()) + { + std::deque msgs; + XsResultValue res = processBufferedData(raw, msgs); + JLTRACEG("Parse result " << res << ": " << msgs.size() << " messages"); + + if (res != XRV_TIMEOUT && res != XRV_TIMEOUTNODATA && !isTerminating()) + { + for (XsMessage const& msg : msgs) + { + handleMessage(msg); + if (isTerminating()) + break; + } + } + raw.clear(); + } + + lockIncoming.lock(); + } + m_newDataEvent.reset(); + return 0; // we handled all our data, but more can be waiting +} + +/*! \brief Initializes the thread +*/ +void DataParser::initFunction() +{ + setPriority(XS_THREAD_PRIORITY_HIGH); + sprintf(m_parserType, "XDA %s %p", parserType(), this); + //JLDEBUGG("Thread " << this << " " << buffer); + xsNameThisThread(m_parserType); +} + +/*! \brief Clears the data queue +*/ +void DataParser::clear() +{ + xsens::Lock lockIncoming(&m_incomingMutex); + + while (!m_incoming.empty()) + m_incoming.pop(); +} + +void DataParser::signalStopThread(void) +{ + StandardThread::signalStopThread(); + m_newDataEvent.terminate(); +} + +/*! \brief Terminates the thread +*/ +void DataParser::terminate() +{ + JLDEBUGG("Thread " << this << " type: " << m_parserType); + stopThread(); + clear(); +} diff --git a/extern/xspublic/xscontroller/dataparser.h b/extern/xspublic/xscontroller/dataparser.h new file mode 100644 index 0000000..189cbc1 --- /dev/null +++ b/extern/xspublic/xscontroller/dataparser.h @@ -0,0 +1,88 @@ + +// Copyright (c) 2003-2022 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 DATAPARSER_H +#define DATAPARSER_H + +#include +#include +#include +#include +#include + +struct XsMessage; + +class DataParser : protected xsens::StandardThread +{ +public: + DataParser(); + ~DataParser() override; + + /*! \brief Read available data from the open IO device + \param raw A buffer that will receive the read data. + \returns XRV_OK if successful + */ + virtual XsResultValue readDataToBuffer(XsByteArray& raw) = 0; + + /*! \brief Read all messages from the buffered read data after adding new data supplied in \a rawIn + \param rawIn The byte array with all data + \param messages The message to process + \returns The messages that were read. + */ + virtual XsResultValue processBufferedData(const XsByteArray& rawIn, std::deque& messages) = 0; + + //! \copybrief Communicator::handleMessage + virtual void handleMessage(const XsMessage& message) = 0; + + void addRawData(const XsByteArray& arr); + void clear(); + void terminate(); + + //! \returns The parser type + virtual const char* parserType() const + { + return "DataParser"; + } + +protected: + void initFunction() override; + int32_t innerFunction() override; + void signalStopThread(void) override; + +private: + xsens::Mutex m_incomingMutex; + std::queue m_incoming; + xsens::WaitEvent m_newDataEvent; + char m_parserType[128]; +}; + +#endif diff --git a/extern/xspublic/xscontroller/datapoller.cpp b/extern/xspublic/xscontroller/datapoller.cpp new file mode 100644 index 0000000..de520c8 --- /dev/null +++ b/extern/xspublic/xscontroller/datapoller.cpp @@ -0,0 +1,100 @@ + +// Copyright (c) 2003-2022 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 "datapoller.h" +#include "dataparser.h" + +/*! \brief Create a DataPoller with a \a parser */ + +DataPoller::DataPoller(DataParser& parser) + : m_parser(parser) +{ + JLDEBUGG("Starting DataPoller " << this << " for parser " << &parser); +} + +/*! \brief Destroy the data poller */ + +DataPoller::~DataPoller() +{ + try + { + JLDEBUGG("Stopping DataPoller " << this << " for parser " << &m_parser); + cleanup(); + } + 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 +{ + if (bytes.size() == 0) + return 3; + else if (bytes.size() < 256) + return 2; + return 0; +} + +/*! \brief Init function for the thread, sets the priority higher +*/ + +void DataPoller::initFunction() +{ + setPriority(XS_THREAD_PRIORITY_HIGHER); + char buffer[128]; + sprintf(buffer, "XDA %s Poller %p", m_parser.parserType(), &m_parser); + xsNameThisThread(buffer); +} + +/*! \brief Clean up the DataPoller */ + +void DataPoller::cleanup() +{ + assert(getThreadId() != xsGetCurrentThreadId()); + stopThread(); +} + +/*! \brief The inner thread function +*/ +int32_t DataPoller::innerFunction(void) +{ + XsByteArray ba; + if (m_parser.readDataToBuffer(ba) != XRV_OK) + return 1; + + int32_t retval = conjureUpWaitTime(ba); + if (ba.size()) + m_parser.addRawData(ba); + + return retval; +} diff --git a/extern/xspublic/xscontroller/datapoller.h b/extern/xspublic/xscontroller/datapoller.h new file mode 100644 index 0000000..2a8ddf3 --- /dev/null +++ b/extern/xspublic/xscontroller/datapoller.h @@ -0,0 +1,62 @@ + +// Copyright (c) 2003-2022 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 DATAPOLLER_H +#define DATAPOLLER_H + +#include + +struct XsMessage; +class DataParser; +class ParseMessageThread; +struct XsByteArray; + +/*! \brief A class implementing some basic data poller behavior +*/ +class DataPoller : public xsens::StandardThread +{ +public: + virtual ~DataPoller(); + explicit DataPoller(DataParser& parser); + +protected: + virtual int32_t conjureUpWaitTime(const XsByteArray& bytes) const; + void initFunction() override; + int32_t innerFunction() override; + + void cleanup(); + +private: + DataParser& m_parser; +}; + +#endif diff --git a/extern/xspublic/xscontroller/devicecommunicator.cpp b/extern/xspublic/xscontroller/devicecommunicator.cpp new file mode 100644 index 0000000..5f81a72 --- /dev/null +++ b/extern/xspublic/xscontroller/devicecommunicator.cpp @@ -0,0 +1,433 @@ + +// Copyright (c) 2003-2022 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 "devicecommunicator.h" +#include "xsdevice_def.h" + +#include +#include +#include +#include "protocolhandler.h" + +#ifdef LOG_COMMUNICATOR_RX_TX + #include +#endif + +/*! \class DeviceCommunicator + \brief A class that is used for the communcation with a device +*/ + +/*! \brief Default constructor +*/ +DeviceCommunicator::DeviceCommunicator(RxChannelId rxChannels) + : m_gotoConfigTimeout(m_defaultGotoConfigTimeout) + , m_nextRxChannelId(0) +#ifdef LOG_COMMUNICATOR_RX_TX_TIMESTAMPED + , m_logStart(XsTimeStamp::now()) +#endif +{ + assert(rxChannels > 0); + + for (XsSize i = 0; i < rxChannels; ++i) + addRxChannel(); + +#ifdef LOG_COMMUNICATOR_RX_TX + generateLogFiles(); +#endif +} + +DeviceCommunicator::~DeviceCommunicator() +{ +} + +/*! \brief Write a message and await the reply + \param msg The message to send + \param rcv The message to receive + \param timeout The timeout in ms + \returns True if successful +*/ +bool DeviceCommunicator::doTransaction(const XsMessage& msg, XsMessage& rcv, uint32_t timeout) +{ + XsXbusMessageId expected = static_cast(msg.getMessageId() + 1); + + std::shared_ptr reply = addReplyObject(expected); + if (!writeMessage(msg)) + { + rcv.clear(); + JLALERTG("Failed to write message because " << lastResult() << " " << lastResultText()); + return false; + } + + rcv = reply->message(timeout); + if (rcv.getMessageId() == expected) + return true; + + if (rcv.getMessageId() == XMID_Error) + { + setLastResult(static_cast(rcv.getDataByte(0))); + JLALERTG("Received error " << lastResult()); + } + else + { + setLastResult(XRV_TIMEOUT); + JLALERTG("Timeout waiting for reply to " << msg.getMessageId() << ", timeout = " << timeout << " ms."); + } + + return false; +} + +/*! \brief Does nothing + \param enable . +*/ +void DeviceCommunicator::setKeepAlive(bool enable) +{ + (void)enable; +} + +/*! \brief Set the timeout for the gotoConfig function. + + \details The goto config function will try to put the device in config mode, but if the communication + channel is less than 100% reliable (especially in half-duplex communication like RS485) this + may fail. For this reason, the function will keep trying until it succeeds or until the timeout + is reached. Every attempt (even a successful one) will take between 20 and 30ms, so the specified + timeout is a lower bound for the actual timeout. + + \param timeout The desired timeout value in ms, if 0 the default value is used +*/ +void DeviceCommunicator::setGotoConfigTimeout(uint32_t timeout) +{ + if (timeout) + m_gotoConfigTimeout = timeout; + else + m_gotoConfigTimeout = m_defaultGotoConfigTimeout; +} + +/*! \copybrief Communicator::getDeviceId +*/ +XsResultValue DeviceCommunicator::getDeviceId() +{ + XsMessage snd(XMID_ReqDid); + snd.setBusId(XS_BID_MASTER); + + XsMessage rcv_did; + if (!doTransaction(snd, rcv_did)) + return setAndReturnLastResult(XRV_COULDNOTREADSETTINGS); + + 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; + snd.setMessageId(XMID_ReqProductCode); + if (doTransaction(snd, rcv_pdc)) + { + const char* pc = (const char*) rcv_pdc.getDataBuffer(); + 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()); + productCode = result; + } + + XsMessage rcv_hw; + snd.setMessageId(XMID_ReqHardwareVersion); + uint16_t hardwareVersion = 0; + if (doTransaction(snd, rcv_hw)) + hardwareVersion = rcv_hw.getDataShort(); + + setMasterDeviceId(XsDeviceId(productCode.c_str(), hardwareVersion, 0, deviceId)); + + return XRV_OK; +} + +/*! \copybrief Communicator::gotoConfig +*/ +XsResultValue DeviceCommunicator::gotoConfig(bool) +{ + XsMessage snd(XMID_GotoConfig), rcv; + snd.setBusId(XS_BID_MASTER); + + JLDEBUGG("Sending gotoConfig"); + if (!doTransaction(snd, rcv, gotoConfigTimeout())) + { + JLALERTG("Failed to go to config, XRV: " << rcv.toResultValue()); + return setAndReturnLastResult(rcv.toResultValue()); + } + JLDEBUGG("Received gotoConfig ACK"); + return setAndReturnLastResult(XRV_OK); +} + +/*! \copybrief Communicator::gotoMeasurement +*/ +XsResultValue DeviceCommunicator::gotoMeasurement() +{ + JLDEBUGG(""); + XsMessage snd(XMID_GotoMeasurement); + snd.setBusId(XS_BID_MASTER); + + if (!doTransaction(snd, defaultTimeout())) + { + JLDEBUGG("Transaction failed"); + return setAndReturnLastResult(XRV_CONFIGCHECKFAIL); + } + JLDEBUGG("Now in measurement mode"); + return setAndReturnLastResult(XRV_OK); +} + +/*! \brief Write \a message to the device + \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) +{ + XsByteArray raw; + if (ProtocolHandler::composeMessage(raw, message) < 0) + { + setLastResult(XRV_INVALIDMSG); + return false; + } + JLTRACEG("did: " << masterDeviceId() << " writing message " << message.toHexString(10)); + setLastResult(writeRawData(raw)); + if (lastResult() == XRV_OK) + { +#ifdef LOG_COMMUNICATOR_RX_TX + logTxStream(message); +#endif + if (masterDevice() != nullptr) + masterDevice()->onMessageSent(message); + return true; + } + return false; +} + +/*! \brief Adds an RX (receive) channel to the device communicator. + Each channel maintains its own message parsing state + \returns The id of the added channel +*/ +DeviceCommunicator::RxChannelId DeviceCommunicator::addRxChannel() +{ + RxChannelId channel = m_nextRxChannelId++; + m_messageExtractors.push_back(MessageExtractor{protocolManager()}); + return channel; +} + +/*! \brief Read all messages available in the incoming data stream after adding new data supplied in \a rawIn + \details This function will read all messages present in the DeviceCommunicator's message extracting buffer after + appending it with the newly received data. + + \param[in] rawIn the newly incoming data + \param[out] messages the list of messages that was extracted + \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) +{ + if (channel >= m_messageExtractors.size()) + return XRV_ERROR; + + assert(protocolManager()); + + XsResultValue res = m_messageExtractors[channel].processNewData(masterDevice(), rawIn, messages); + + if (res == XRV_OK) + { + for (auto const& msg : messages) + { + if (masterDevice() != nullptr) + masterDevice()->onMessageReceived(msg); + } + } + 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) +{ + assert(channel < m_messageExtractors.size()); + return m_messageExtractors[channel]; +} + +/*! \copybrief Communicator::handleMessage +*/ +void DeviceCommunicator::handleMessage(const XsMessage& message) +{ +#ifdef LOG_COMMUNICATOR_RX_TX + logRxStream(message); +#endif + Communicator::handleMessage(message); +} + +/* Now the disabled stuff starts */ + +//! \cond DO_NOT_DOCUMENT +void DeviceCommunicator::closeLogFile() +{ +} + +XsMessage DeviceCommunicator::readMessageFromStartOfFile(uint8_t, int) +{ + return XsMessage(); +} + +std::deque DeviceCommunicator::readMessagesFromStartOfFile(uint8_t, int) +{ + return std::deque(); +} + +void DeviceCommunicator::loadLogFile(XsDevice*) +{ +} + +XsResultValue DeviceCommunicator::readLogFile(XsDevice*) +{ + return XRV_UNSUPPORTED; +} + +XsResultValue DeviceCommunicator::readSinglePacketFromFile() +{ + return XRV_UNSUPPORTED; +} + +void DeviceCommunicator::abortLoadLogFile() +{ +} + +bool DeviceCommunicator::openLogFile(const XsString&) +{ + return false; +} + +XsString DeviceCommunicator::logFileName() const +{ + return XsString(); +} + +XsFilePos DeviceCommunicator::logFileSize() const +{ + return 0; +} + +XsTimeStamp DeviceCommunicator::logFileDate() const +{ + return 0; +} + +XsFilePos DeviceCommunicator::logFileReadPosition() const +{ + return XsFilePos(0); +} + +void DeviceCommunicator::resetLogFileReadPosition(void) +{ +} + +bool DeviceCommunicator::isReadingFromFile() const +{ + return false; +} + +void DeviceCommunicator::waitForLastTaskCompletion() +{ +} + +XsMessage DeviceCommunicator::readMessage(uint8_t) +{ + return XsMessage(); +} + +//! \endcond + +#ifdef LOG_COMMUNICATOR_RX_TX +void DeviceCommunicator::generateLogFiles() +{ + XsString date = XsTime::getDateAsString(); + XsString time = XsTime::getTimeAsString(); + +#ifdef LOG_COMMUNICATOR_RX_TX_TIMESTAMPED + XsString extension("mts"); +#else + XsString extension("mtb"); +#endif + + 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); + }; + + m_rxLog.create(generateFilename("rx"), false); + m_txLog.create(generateFilename("tx"), false); +} + +void DeviceCommunicator::logTxStream(XsMessage const& msg) +{ + assert(m_txLog.isOpen()); + +#ifdef LOG_COMMUNICATOR_RX_TX_TIMESTAMPED + XsTimeStamp diff = XsTimeStamp::now() - m_logStart; + std::string timestamp = xprintf("%d ", diff.msTime()); + m_txLog.write(timestamp.c_str(), 1, timestamp.length()); +#endif + + m_txLog.write(msg.rawMessage().data(), 1, msg.rawMessage().size()); + m_txLog.flush(); +} + +void DeviceCommunicator::logRxStream(XsMessage const& msg) +{ + assert(m_rxLog.isOpen()); + +#ifdef LOG_COMMUNICATOR_RX_TX_TIMESTAMPED + XsTimeStamp diff = XsTimeStamp::now() - m_logStart; + std::string timestamp = xprintf("%d ", diff.msTime()); + m_rxLog.write(timestamp.c_str(), 1, timestamp.length()); +#endif + + m_rxLog.write(msg.rawMessage().data(), 1, msg.rawMessage().size()); + m_rxLog.flush(); +} + +#endif diff --git a/extern/xspublic/xscontroller/devicecommunicator.h b/extern/xspublic/xscontroller/devicecommunicator.h new file mode 100644 index 0000000..cc2a1ce --- /dev/null +++ b/extern/xspublic/xscontroller/devicecommunicator.h @@ -0,0 +1,137 @@ + +// Copyright (c) 2003-2022 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 DEVICECOMMUNICATOR_H +#define DEVICECOMMUNICATOR_H + +#include "communicator.h" +#include "messageextractor.h" + +#ifdef LOG_COMMUNICATOR_RX_TX + #include +#endif + + +class DeviceCommunicator : public Communicator +{ +public: + + //! \brief A typedef for Rx chanel ID + typedef XsSize RxChannelId; + + DeviceCommunicator(RxChannelId rxChannels = 1); + + // live stuff + XsResultValue getDeviceId() override; + XsResultValue gotoConfig(bool) override; + XsResultValue gotoMeasurement() override; + + //! \returns The timeout value for gotoConfig function + uint32_t gotoConfigTimeout() const + { + return m_gotoConfigTimeout; + } + void setGotoConfigTimeout(uint32_t timeout) override; + bool writeMessage(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; + + void setKeepAlive(bool enable) override; + + // file stuff + void closeLogFile() override; + XsMessage readMessage(uint8_t msgId = 0) override; + XsMessage readMessageFromStartOfFile(uint8_t msgId, int maxMsgs = 0) override; + std::deque readMessagesFromStartOfFile(uint8_t msgId, int maxMsgs = 0) override; + void loadLogFile(XsDevice* device) override; + void abortLoadLogFile() override; + bool openLogFile(const XsString& filename) override; + XsString logFileName() const override; + XsFilePos logFileSize() const override; + XsTimeStamp logFileDate() const override; + XsFilePos logFileReadPosition() const override; + void resetLogFileReadPosition(void) override; + bool isReadingFromFile() const override; + void waitForLastTaskCompletion() override; + + /*! \brief Read a log file into cache + \param device The device to read log from + \returns XRV_OK if successful + */ + virtual XsResultValue readLogFile(XsDevice* device); + + /*! \brief Read a single XsDataPacket from an open log file + \returns XRV_OK if successful + */ + virtual XsResultValue readSinglePacketFromFile(); + +protected: + ~DeviceCommunicator() override; + 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; + RxChannelId addRxChannel(); + XsSize messageExtractorCount() const; + MessageExtractor& messageExtractor(RxChannelId = 0); + +#ifdef LOG_COMMUNICATOR_RX_TX + void logTxStream(XsMessage const& msg); + void logRxStream(XsMessage const& msg); +#endif + +private: + static const uint32_t m_defaultGotoConfigTimeout = 200; // 1500 just after powerup, 100ms is not enough sometimes during tests + + uint32_t m_gotoConfigTimeout; + + RxChannelId m_nextRxChannelId; + std::vector m_messageExtractors; + +#ifdef LOG_COMMUNICATOR_RX_TX + XsFile m_rxLog; + XsFile m_txLog; +#ifdef LOG_COMMUNICATOR_RX_TX_TIMESTAMPED + XsTimeStamp m_logStart; +#endif + + void generateLogFiles(); +#endif +}; + +#endif diff --git a/extern/xspublic/xscontroller/devicefactory.cpp b/extern/xspublic/xscontroller/devicefactory.cpp new file mode 100644 index 0000000..692a9d1 --- /dev/null +++ b/extern/xspublic/xscontroller/devicefactory.cpp @@ -0,0 +1,225 @@ + +// Copyright (c) 2003-2022 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 "devicefactory.h" + +#include +#include "xsdevice_def.h" +#include "communicator.h" +#include "devicetypes.h" +#include "mtixdevice.h" +#include "mtix0device.h" +#include "mtix00device.h" +#include "mtigdevice.h" +#include "mti7_mti8device.h" +#include "mti3x0device.h" +#include "mti6x0device.h" +#include "mti8x0device.h" +#include "dotdevice.h" + +/*! \class DeviceFactory + \brief A Factory for the devices +*/ + +/*! \brief Default constructor +*/ +DeviceFactory::DeviceFactory() +{ +} + +/*! \brief Registers a master device type. + After registration, the factory is able to cerate an instance of the specified type using the deviceTypeId. + A master device is not (yet) owned by an object + \param[in] deviceTypeId the unique identifier for this device type + \param[in] constructFunc the function used to create an instance of the required type + \returns true when registration was successful +*/ +bool DeviceFactory::registerMasterDeviceType(DeviceTypeId deviceTypeId, MasterConstructFunc constructFunc) +{ + bool rv = m_masterConstructors.insert(std::make_pair(deviceTypeId, constructFunc)).second; + // it should be a master XOR a standalone (or neither) + assert(m_standaloneConstructors.find(deviceTypeId) == m_standaloneConstructors.end()); + return rv; +} + +/*! \brief Registers a standalone device type. + After registration, the factory is able to cerate an instance of the specified type using the deviceTypeId. + A master device is not (yet) owned by an object + \param[in] deviceTypeId the unique identifier for this device type + \param[in] constructFunc the function used to create an instance of the required type + \returns true when registration was successful +*/ +bool DeviceFactory::registerStandaloneDeviceType(DeviceTypeId deviceTypeId, StandaloneConstructFunc constructFunc) +{ + m_standaloneConstructors[deviceTypeId] = constructFunc; + // it should be a master XOR a standalone (or neither) + assert(m_masterConstructors.find(deviceTypeId) == m_masterConstructors.end()); + return true; +} + +/*! \brief Constructs a master device of a specified deviceTypeId with a specified communicator. + The created device takes ownership of the Communicator. + \param[in] deviceTypeId the unique identifier for this device type + \param[in] comm The communicator to used for this device + \returns the newly created device or null when the device was not created. +*/ +XsDevice* DeviceFactory::constructDevice(DeviceTypeId deviceTypeId, Communicator* comm) +{ + XsDevice* device = nullptr; + if (deviceTypeId != DeviceType::INVALID) + { + auto i = m_masterConstructors.find(deviceTypeId); + if (i != m_masterConstructors.end()) + { + device = (i->second)(*this, comm); + device->addRef(); + } + else + { + auto j = m_standaloneConstructors.find(deviceTypeId); + if (j != m_standaloneConstructors.end()) + { + device = (j->second)(comm); + device->addRef(); + } + } + } + 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 +*/ +XsDevice* DeviceFactory::createMasterDevice(Communicator* communicator, bool doInitialize) +{ + if (communicator != nullptr) + { + DeviceTypeId deviceTypeId = deviceToTypeId(communicator->masterDeviceId()); + XsDevice* constructedDevice = constructDevice(deviceTypeId, communicator); + + if (!doInitialize || initializeDevice(constructedDevice)) + { +#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()); +#endif + return constructedDevice; + } + + // construction failed + if (constructedDevice) + constructedDevice->removeRef(); + else + communicator->destroy(); + } + return nullptr; +} + +/*! \brief converts an XsDeviceId to an DeviceTypeId + \param[in] deviceId the deviceId + \returns the DeviceTypeId +*/ +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_MTI_8;//this is used for both the MTi-7 and MTi-8 + 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; +} + +/*! \brief Tell our device manager to remove any devices matching \a deviceId +*/ +void DeviceFactory::removeExistingDevice(const XsDeviceId& deviceId) +{ + (void) deviceId; +} + +/*! \brief Initializes a device. + 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()) + return true; + return false; +} + +/*! \brief Initializes a device (if not already initialized) + \param[in] device The device to initialize + \returns true when the device is properly initialized or already initialized, false if initialization failed +*/ +bool DeviceFactory::initializeDevice(XsDevice& device) const +{ + return device.isInitialized() ? true : initializeDevice(&device); +} + +/*! \brief register all known device types +*/ +void DeviceFactory::registerDevices() +{ + (void)registerStandaloneDeviceType(DeviceType::MTI_X, &MtiXDevice::constructStandalone); + (void)registerStandaloneDeviceType(DeviceType::MTI_X0, &MtiX0Device::constructStandalone); + (void)registerStandaloneDeviceType(DeviceType::MTI_X00, &MtiX00Device::constructStandalone); + (void)registerStandaloneDeviceType(DeviceType::MTIG, &MtigDevice::constructStandalone); + (void)registerStandaloneDeviceType(DeviceType::MTI_7_MTI_8, &MTi7_MTi8Device::constructStandalone); + (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/extern/xspublic/xscontroller/devicefactory.h b/extern/xspublic/xscontroller/devicefactory.h new file mode 100644 index 0000000..243f347 --- /dev/null +++ b/extern/xspublic/xscontroller/devicefactory.h @@ -0,0 +1,84 @@ + +// Copyright (c) 2003-2022 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 DEVICE_FACTORY_H +#define DEVICE_FACTORY_H + +#include +#include +#include + +struct Communicator; +struct XsDevice; + +class DeviceFactory +{ +public: + DeviceFactory(); + virtual ~DeviceFactory() {} + + //! \brief Provides the device type ID as unsigned int + typedef unsigned int DeviceTypeId; + + //! \brief A function prototype that provides the device factory and communicator + 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; + + 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 registerDevices(); + +protected: + virtual XsDevice* constructDevice(DeviceTypeId deviceTypeId, Communicator* comm); + + virtual bool initializeDevice(XsDevice* device) const; + +private: + std::map m_masterConstructors; + std::map m_standaloneConstructors; +}; + +namespace DeviceType +{ +static const DeviceFactory::DeviceTypeId INVALID = 0; +} +#endif diff --git a/extern/xspublic/xscontroller/deviceredetector.cpp b/extern/xspublic/xscontroller/deviceredetector.cpp new file mode 100644 index 0000000..e2890d8 --- /dev/null +++ b/extern/xspublic/xscontroller/deviceredetector.cpp @@ -0,0 +1,255 @@ + +// Copyright (c) 2003-2022 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 "xscontrollerconfig.h" +#include +#include +#include +#include +#include "xsscanner.h" +//#include "networkcommunicator.h" +#include "deviceredetector.h" + +/*! \class DeviceRedetector + \brief A class which re-detects a device with a certain device Id +*/ + +/*! \brief Construct an Device Redetector object */ +DeviceRedetector::DeviceRedetector(const XsPortInfo& portInfo) +{ +#ifdef XSENS_WINDOWS + if (portInfo.isUsb()) +#else + (void)portInfo; + if (1) +#endif + { + m_detectFunctions[XS_DID_TYPEH_MT_X_MPU] = &DeviceRedetector::redetectScanPorts; + m_detectFunctions[XS_DID_TYPEH_MT_X0] = &DeviceRedetector::redetectScanPorts; + m_detectFunctions[XS_DID_TYPEH_MT_X00] = &DeviceRedetector::redetectScanPorts; + m_detectFunctions[XS_DID_TYPEH_MT_3X0] = &DeviceRedetector::redetectScanPorts; + m_detectFunctions[XS_DID64_BIT] = &DeviceRedetector::redetectScanPorts; + m_detectFunctions[XsDeviceId("MTi-610", 0, 0, XS_DID64_BIT)] = &DeviceRedetector::redetectScanPorts; + m_detectFunctions[XsDeviceId("MTi-620", 0, 0, XS_DID64_BIT)] = &DeviceRedetector::redetectScanPorts; + m_detectFunctions[XsDeviceId("MTi-630", 0, 0, XS_DID64_BIT)] = &DeviceRedetector::redetectScanPorts; + m_detectFunctions[XsDeviceId("MTi-670", 0, 0, XS_DID64_BIT)] = &DeviceRedetector::redetectScanPorts; + m_detectFunctions[XsDeviceId("MTi-680", 0, 0, XS_DID64_BIT)] = &DeviceRedetector::redetectScanPorts; + } + else + { + m_detectFunctions[XS_DID_TYPEH_MT_X_MPU] = &DeviceRedetector::redetectOneComPort; + m_detectFunctions[XS_DID_TYPEH_MT_X0] = &DeviceRedetector::redetectOneComPort; + m_detectFunctions[XS_DID_TYPEH_MT_X00] = &DeviceRedetector::redetectOneComPort; + m_detectFunctions[XS_DID_TYPEH_MT_3X0] = &DeviceRedetector::redetectOneComPort; + m_detectFunctions[XS_DID64_BIT] = &DeviceRedetector::redetectOneComPort; + m_detectFunctions[XsDeviceId("MTi-610", 0, 0, XS_DID64_BIT)] = &DeviceRedetector::redetectOneComPort; + m_detectFunctions[XsDeviceId("MTi-620", 0, 0, XS_DID64_BIT)] = &DeviceRedetector::redetectOneComPort; + m_detectFunctions[XsDeviceId("MTi-630", 0, 0, XS_DID64_BIT)] = &DeviceRedetector::redetectOneComPort; + m_detectFunctions[XsDeviceId("MTi-670", 0, 0, XS_DID64_BIT)] = &DeviceRedetector::redetectOneComPort; + m_detectFunctions[XsDeviceId("MTi-680", 0, 0, XS_DID64_BIT)] = &DeviceRedetector::redetectOneComPort; + } +} + +/*! \brief Base redetect function which calls the appropriate redetect based on deviceId + \param deviceId The device ID to look for + \param portInfo The updated \a portinfo when the device is re-detected. + \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) +{ + FunctionPointer currentFunction; + currentFunction = m_detectFunctions[deviceId.deviceType(false)]; + + if (currentFunction) + return (this->*currentFunction)(deviceId, portInfo, skipDeviceIdCheck); + else + { + JLDEBUGG("No functions defined for device. Defaulting to OneComPort" << deviceId); + return redetectOneComPort(deviceId, portInfo, skipDeviceIdCheck); + } +} + +/*! \returns True + \details This function does nothing + \param deviceId Device ID + \param portInfo Port info + \param skipDeviceIdCheck Skip device ID check +*/ +bool DeviceRedetector::redetectNoScan(const XsDeviceId& deviceId, XsPortInfo& portInfo, bool skipDeviceIdCheck) +{ + (void)deviceId; + (void)portInfo; + (void)skipDeviceIdCheck; + return true; +} + +/*! \brief Scan for devices until the requested device is found + \param deviceId The device ID to look for + \param portInfo The updated \a portinfo when the device is re-detected. + \param skipDeviceIdCheck If set to true then it will skip device ID check + \details Scans for a certain period all available ports for the device. + \returns True if successful +*/ +bool DeviceRedetector::redetectScanPorts(const XsDeviceId& deviceId, XsPortInfo& portInfo, bool skipDeviceIdCheck) +{ + JLDEBUGG("Redetecting device " << deviceId); + + bool found = false; + int count = 0; + while (!found && count < 25) + { + XsTime::msleep(100); + XsPortInfoArray portInfoArray = XsScanner::scanPorts(XBR_Invalid, 100, false, true); + + JLDEBUGG("Attempt " << count << ". Detected " << portInfoArray.size() << " devices."); + + for (uint32_t i = 0; i < portInfoArray.size(); i++) + { + portInfo = portInfoArray[i]; + if ((skipDeviceIdCheck && !portInfo.empty()) || portInfo.deviceId() == deviceId) + { + portInfo = portInfoArray[i]; + JLDEBUGG("Found device: " << portInfo); + found = true; + break; + } + } + XsTime::msleep(1000); + count++; + } + JLDEBUGG("Redetecting device " << deviceId << " . Result: " << found << " after " << count << " tries"); + return found; +} + +/*! \brief Redectets and enumerates the serial ports for a given device + \param deviceId The device ID to look for + \param portInfo The updated \a portinfo when the device is re-detected. + \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) +{ + JLDEBUGG("Redetecting device " << deviceId); + + bool found = false; + int count = 0; + while (!found && count < 20) + { + XsTime::msleep(100); + XsPortInfoArray portInfoArray = XsScanner::enumerateSerialPorts(); + + JLDEBUGG("Attempt " << count << ". Detected " << portInfoArray.size() << " devices."); + + for (uint32_t i = 0; i < portInfoArray.size(); i++) + { + JLDEBUGG(portInfoArray[i]); + if ((skipDeviceIdCheck && !portInfoArray[i].empty()) || portInfoArray[i].deviceId() == deviceId) + { + XsTime::msleep(3000); + portInfo = portInfoArray[i]; + found = true; + break; + } + } + count++; + } + JLDEBUGG("Redetecting device " << deviceId << " . Result: " << found << " after " << count << " tries"); + return found; +} + +/*! \brief Redectes a device on one com port + \param deviceId The device ID to look for + \param portInfo The updated \a portinfo when the device is re-detected. + \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) +{ + JLDEBUGG("Redetecting device " << deviceId); + + bool found = false; + int count = 0; + + XsStringArray portList; + portList.push_back(portInfo.portName()); + + XsIntArray portLinesOptions; + portLinesOptions.push_back((int)portInfo.linesOptions()); + + while (!found && count < 20) + { + XsTime::msleep(100); + + XsPortInfoArray detectedPorts = XsScanner::scanComPortList(portList, portLinesOptions, XBR_Invalid, 100); + + assert(detectedPorts.size() <= 1); // at most one port found + + JLDEBUGG("Attempt " << count << "."); + + if (!detectedPorts.empty()) + { + const XsPortInfo portInfoDetected = detectedPorts[0]; + + JLDEBUGG(portInfoDetected); + + if ((skipDeviceIdCheck && !portInfoDetected.empty()) || portInfoDetected.deviceId() == deviceId) + { + uint16_t vid, pid; + portInfo.getVidPid(vid, pid); + portInfo = portInfoDetected; + portInfo.setVidPid(vid, pid); + found = true; + break; + } + } + count++; + } + + JLDEBUGG("Redetecting device " << deviceId << " . Result: " << found << " after " << count << " tries"); + JLDEBUGG("PortInfo " << portInfo); + return found; +} + +/*! \returns False + \details This function does nothing + \param deviceId Device ID + \param portInfo Port info + \param skipDeviceIdCheck Skip device ID check +*/ +bool DeviceRedetector::redetectEnumerateNetworkDevices(const XsDeviceId& deviceId, XsPortInfo& portInfo, bool skipDeviceIdCheck) +{ + (void)deviceId; + (void)portInfo; + (void)skipDeviceIdCheck; + return false; +} diff --git a/extern/xspublic/xscontroller/deviceredetector.h b/extern/xspublic/xscontroller/deviceredetector.h new file mode 100644 index 0000000..497adcd --- /dev/null +++ b/extern/xspublic/xscontroller/deviceredetector.h @@ -0,0 +1,64 @@ + +// Copyright (c) 2003-2022 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 DEVICEREDETECTOR_H +#define DEVICEREDETECTOR_H + +struct XsDeviceId; +struct XsPortInfo; + +#include + +class DeviceRedetector +{ +public: + DeviceRedetector(const XsPortInfo& portInfo); + virtual ~DeviceRedetector() {} + + 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); + +protected: + + //! A typedef for a function pointer + typedef bool (DeviceRedetector::*FunctionPointer)(const XsDeviceId& deviceId, XsPortInfo& portInfo, bool skipDeviceIdCheck); + + //! A map filled with detect functions + std::map m_detectFunctions; +}; + +#endif diff --git a/extern/xspublic/xscontroller/devicetypes.h b/extern/xspublic/xscontroller/devicetypes.h new file mode 100644 index 0000000..4c1d9ed --- /dev/null +++ b/extern/xspublic/xscontroller/devicetypes.h @@ -0,0 +1,64 @@ + +// Copyright (c) 2003-2022 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 DEVICETYPES_H +#define DEVICETYPES_H + +#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_MTI_8 = 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/extern/xspublic/xscontroller/dotdevice.cpp b/extern/xspublic/xscontroller/dotdevice.cpp new file mode 100644 index 0000000..cc3236c --- /dev/null +++ b/extern/xspublic/xscontroller/dotdevice.cpp @@ -0,0 +1,69 @@ + +// Copyright (c) 2003-2022 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/extern/xspublic/xscontroller/dotdevice.h b/extern/xspublic/xscontroller/dotdevice.h new file mode 100644 index 0000000..2e6f36c --- /dev/null +++ b/extern/xspublic/xscontroller/dotdevice.h @@ -0,0 +1,79 @@ + +// Copyright (c) 2003-2022 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/extern/xspublic/xscontroller/dummy.cpp b/extern/xspublic/xscontroller/dummy.cpp new file mode 100644 index 0000000..64edf63 --- /dev/null +++ b/extern/xspublic/xscontroller/dummy.cpp @@ -0,0 +1,38 @@ + +// Copyright (c) 2003-2022 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. +// + +// This exists just so the library isn't empty for the VS build +#ifdef _WIN32 + #include +#endif + +const int xsControllerDummy = 0; diff --git a/extern/xspublic/xscontroller/enumerateusbdevices.cpp b/extern/xspublic/xscontroller/enumerateusbdevices.cpp new file mode 100644 index 0000000..6b4696d --- /dev/null +++ b/extern/xspublic/xscontroller/enumerateusbdevices.cpp @@ -0,0 +1,276 @@ + +// Copyright (c) 2003-2022 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 "xscontrollerconfig.h" +#include "enumerateusbdevices.h" + +#ifdef _WIN32 + #include + #include + #include + #include + #include + #include + #include +#else + #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) +{ + char deviceInstanceID[MAX_DEVICE_ID_LEN]; + SetupDiGetDeviceInstanceIdA(hDevInfo, DeviceInfoData, deviceInstanceID, MAX_DEVICE_ID_LEN, NULL); + return std::string(deviceInstanceID); +} + + +/* Get the first match for _regex_ within _devpath_ */ +static inline std::string searchOne(std::string const& devpath, std::string const& regex) +{ + std::smatch sm; + std::regex_search(devpath, sm, std::regex(regex)); + if (sm.size() < 2) + return std::string(); + return sm[1]; +} + +/* Fetch the vendor ID from the given device path */ +static inline uint16_t vidFromDevPath(std::string const& devpath) +{ + try + { + auto t1 = searchOne(devpath, "VID_([0-9a-fA-F]{4}).*PID_.*"); + if (t1.empty()) // prevent unnecessary exceptions + return 0; + return static_cast(std::stoi(t1, nullptr, 16)); + } + catch (std::invalid_argument&) + { + return 0; + } +} + +/* Fetch the product ID from the given device path */ +static inline uint16_t pidFromDevPath(std::string const& devpath) +{ + try + { + auto t1 = searchOne(devpath, "VID_.*PID_([0-9a-fA-F]{4})"); + if (t1.empty()) // prevent unnecessary exceptions + return 0; + return static_cast(std::stoi(t1, nullptr, 16)); + } + catch (std::invalid_argument&) + { + return 0; + } +} +#endif + +/*! \brief Enumerate Xsens USB devices + + If the OS already has drivers running for a device, the device should already have been + found by xsEnumerateSerialPorts(). + + \param[in,out] ports The list of serial ports to append to + \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) +{ + XsPortInfo current; +#ifdef USE_WINUSB + BOOL bResult = FALSE; + ULONG length; + ULONG requiredLength = 0; + + // {FD51225C-700A-47e5-9999-B2D9031B88ED} + GUID guid = { 0xfd51225c, 0x700a, 0x47e5, { 0x99, 0x99, 0xb2, 0xd9, 0x3, 0x1b, 0x88, 0xed } }; + + HDEVINFO deviceInfo; + SP_DEVICE_INTERFACE_DATA interfaceData; + PSP_DEVICE_INTERFACE_DETAIL_DATA_A detailData = NULL; + + deviceInfo = SetupDiGetClassDevs(&guid, NULL, NULL, DIGCF_PRESENT | DIGCF_DEVICEINTERFACE); + + // Initialize variables. + interfaceData.cbSize = sizeof(SP_INTERFACE_DEVICE_DATA); + for (DWORD dwIndex = 0; true; ++dwIndex) //lint !e774 !e440 !e506 + { + BOOL bRet = SetupDiEnumDeviceInterfaces(deviceInfo, NULL, &guid, dwIndex, &interfaceData); + if (!bRet) + { + if (GetLastError() == ERROR_NO_MORE_ITEMS) + break; + } + else + { + if (!SetupDiGetDeviceInterfaceDetail(deviceInfo, &interfaceData, NULL, 0, &requiredLength, NULL)) + { + if (GetLastError() != ERROR_INSUFFICIENT_BUFFER) + { + SetupDiDestroyDeviceInfoList(deviceInfo); + return false; + } + } + detailData = (PSP_DEVICE_INTERFACE_DETAIL_DATA_A)LocalAlloc(LMEM_FIXED, requiredLength); + if (NULL == detailData) + { + SetupDiDestroyDeviceInfoList(deviceInfo); + return false; + } + + detailData->cbSize = sizeof(SP_DEVICE_INTERFACE_DETAIL_DATA_A); + length = requiredLength; + SP_DEVINFO_DATA DevInfoData; + DevInfoData.cbSize = sizeof(SP_DEVINFO_DATA); + bResult = SetupDiGetDeviceInterfaceDetailA(deviceInfo, &interfaceData, detailData, length, &requiredLength, &DevInfoData); + + if (!bResult) + { + LocalFree(detailData); + SetupDiDestroyDeviceInfoList(deviceInfo); + return false; + } + + unsigned char serialNumber[256]; + char* ptrEnd, *ptrStart = strchr(detailData->DevicePath, '#'); + if (!ptrStart) + continue; + ptrStart = strchr(ptrStart + 1, '#'); + if (!ptrStart) + continue; + ptrEnd = strchr(ptrStart + 1, '#'); + if (!ptrEnd) + continue; + + 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); + current.setDeviceId((uint32_t) id); + + std::string devpath = getDevicePath(deviceInfo, &DevInfoData); + uint16_t vid = vidFromDevPath(devpath); + uint16_t pid = pidFromDevPath(devpath); + current.setVidPid(vid, pid); + + ports.push_back(current); + } + } + + SetupDiDestroyDeviceInfoList(deviceInfo); + return true; +#elif defined(HAVE_LIBUSB) + XsLibUsb libUsb; + libusb_context* context; + int result = libUsb.init(&context); + if (result != LIBUSB_SUCCESS) + return true; + + 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_descriptor desc; + result = libUsb.get_device_descriptor(device, &desc); + if (result != LIBUSB_SUCCESS) + continue; + + if (desc.idVendor != XSENS_VENDOR_ID) + continue; + + libusb_device_handle* handle; + result = libUsb.open(device, &handle); + if (result != LIBUSB_SUCCESS) + continue; + + unsigned char serialNumber[256]; + result = libUsb.get_string_descriptor_ascii(handle, desc.iSerialNumber, serialNumber, 256); + + libusb_config_descriptor* configDesc; + result = libUsb.get_active_config_descriptor(device, &configDesc); + if (result != LIBUSB_SUCCESS) + { + libUsb.close(handle); + continue; + } + + bool kernelActive = false; + for (uint8_t ifCount = 0; ifCount < configDesc->bNumInterfaces; ++ifCount) + { + int res = libUsb.kernel_driver_active(handle, ifCount); + kernelActive |= (res == 1); + } + + libUsb.free_config_descriptor(configDesc); + + if (!kernelActive) + { + char name[256]; + sprintf(name, "USB%03u:%03u", libUsb.get_bus_number(device), + libUsb.get_device_address(device)); + current.setPortName(name); + + int id = 0; + sscanf((const char*)serialNumber, "%X", &id); + current.setDeviceId((uint32_t) id); + current.setVidPid(desc.idVendor, desc.idProduct); + ports.push_back(current); + } + else + { + JLDEBUGG("Kernel driver active on USB" << + libUsb.get_bus_number(device) << ":" << libUsb.get_device_address(device) << + " device " << serialNumber); + + } + libUsb.close(handle); + } + libUsb.free_device_list(deviceList, 1); + libUsb.exit(context); + return true; +#else + (void)ports; + return true; +#endif +} diff --git a/extern/xspublic/xscontroller/enumerateusbdevices.h b/extern/xspublic/xscontroller/enumerateusbdevices.h new file mode 100644 index 0000000..fed6873 --- /dev/null +++ b/extern/xspublic/xscontroller/enumerateusbdevices.h @@ -0,0 +1,40 @@ + +// Copyright (c) 2003-2022 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 XSENUMERATEUSBDEVICES_H +#define XSENUMERATEUSBDEVICES_H + +#include + +bool xsEnumerateUsbDevices(XsPortInfoArray& ports); + +#endif diff --git a/extern/xspublic/xscontroller/enumexpanders.cpp b/extern/xspublic/xscontroller/enumexpanders.cpp new file mode 100644 index 0000000..2e527aa --- /dev/null +++ b/extern/xspublic/xscontroller/enumexpanders.cpp @@ -0,0 +1,70 @@ + +// Copyright (c) 2003-2022 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 "enumexpanders.h" +#include "xscontrollerconfig.h" + +JLENUMEXPANDER(XsDeviceState, + JLENUMCASE(XDS_Initial) + JLENUMCASE(XDS_Config) + JLENUMCASE(XDS_Measurement) + JLENUMCASE(XDS_WaitingForRecordingStart) + JLENUMCASE(XDS_Recording) + JLENUMCASE(XDS_FlushingData) + JLENUMCASE(XDS_Destructing) +) + +JLENUMEXPANDER(XsOperationalMode, + JLENUMCASE(XOP_Unknown) + JLENUMCASE(XOP_LiveStream) + JLENUMCASE(XOP_Obr) + JLENUMCASE(XOP_Robr) +) + +JLENUMEXPANDER(XsConnectivityState, + JLENUMCASE(XCS_Disconnected) + JLENUMCASE(XCS_Rejected) + JLENUMCASE(XCS_PluggedIn) + JLENUMCASE(XCS_Wireless) + JLENUMCASE(XCS_WirelessOutOfRange) + JLENUMCASE(XCS_File) + JLENUMCASE(XCS_Unknown) +) + +static void forceEnumExpanderInclusion() +{ + volatile bool blah = false; + JLWRITEG("blah" << XDS_Initial << blah); + JLWRITEG("blah" << XOP_Unknown << blah); + JLWRITEG("blah" << XCS_File << blah); + (void)blah; +} diff --git a/extern/xspublic/xscontroller/enumexpanders.h b/extern/xspublic/xscontroller/enumexpanders.h new file mode 100644 index 0000000..5a8f281 --- /dev/null +++ b/extern/xspublic/xscontroller/enumexpanders.h @@ -0,0 +1,45 @@ + +// Copyright (c) 2003-2022 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 XDA_ENUMEXPANDERS_H +#define XDA_ENUMEXPANDERS_H + +#include +#include "xsdevicestate.h" +#include "xsoperationalmode.h" +#include "xsconnectivitystate.h" + +JLENUMEXPPROTO(XsDeviceState); +JLENUMEXPPROTO(XsOperationalMode); +JLENUMEXPPROTO(XsConnectivityState); + +#endif diff --git a/extern/xspublic/xscontroller/fileloader.h b/extern/xspublic/xscontroller/fileloader.h new file mode 100644 index 0000000..f104ff0 --- /dev/null +++ b/extern/xspublic/xscontroller/fileloader.h @@ -0,0 +1,58 @@ + +// Copyright (c) 2003-2022 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 FILELOADER_H +#define FILELOADER_H + +struct XsDevice; + +/*! \class FileLoader + \brief A class that loads file +*/ +class FileLoader +{ +public: + virtual ~FileLoader(); + + /*! \brief Reads a log file + \param device The device to read from + \returns XRV_OK if successful + */ + virtual XsResultValue readLogFile(XsDevice* device) = 0; + + /*! \brief Read a single packet from file + \returns XRV_OK is successful + */ + virtual XsResultValue readSinglePacketFromFile() = 0; +}; + +#endif diff --git a/extern/xspublic/xscontroller/gpsstatus.h b/extern/xspublic/xscontroller/gpsstatus.h new file mode 100644 index 0000000..a78ea7a --- /dev/null +++ b/extern/xspublic/xscontroller/gpsstatus.h @@ -0,0 +1,67 @@ + +// Copyright (c) 2003-2022 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 XSGPSSTATUS_H +#define XSGPSSTATUS_H + +#include "xscontrollerconfig.h" +#include +#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 }; +#endif + +#define XS_MAX_SVINFO 16 + +/*! \brief Storage for status of one GPS satellite, used by MTi-G only +*/ +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 + uint8_t m_signalStrength; //!< Signal strength +}; +typedef struct XsGpsSatelliteInfo XsGpsSatelliteInfo; + +/*! \brief Storage for all GPS satellites' information, used by MTi-G only +*/ +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 +}; +typedef struct GpsStatus GpsStatus; + +#endif diff --git a/extern/xspublic/xscontroller/idfetchhelpers.h b/extern/xspublic/xscontroller/idfetchhelpers.h new file mode 100644 index 0000000..badcd9d --- /dev/null +++ b/extern/xspublic/xscontroller/idfetchhelpers.h @@ -0,0 +1,98 @@ + +// Copyright (c) 2003-2022 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 IDFETCHHELPERS_H +#define IDFETCHHELPERS_H + +#include +#include +#include + +/* Helper functions for fetching of device/vendor/product ids from text strings */ + +/* Get the first match for _regex_ within _string_ */ +static inline std::string searchOne(std::string const& string, std::string const& regex) +{ + std::smatch sm; + std::regex_search(string, sm, std::regex(regex)); + if (sm.size() < 2) + return std::string(); + return sm[1]; +} + +/* Fetch the device ID from the given device path */ +static inline XsDeviceId deviceIdFromDevPath(std::string const& devpath) +{ + XsDeviceId deviceId; + std::string id = searchOne(devpath, "([0-9A-Fa-f]*)$"); + if (!id.empty()) + deviceId.fromString(XsString(id)); + return deviceId; +} + +/* Fetch the vendor ID from the given string */ +static inline uint16_t vidFromString(std::string const& string) +{ + try + { + std::string regex = string; + std::transform(regex.begin(), regex.end(), regex.begin(), ::toupper); + auto t1 = searchOne(regex, "VID_([0-9a-fA-F]{4}).*PID_.*"); + if (t1.empty()) // prevent unnecessary exceptions + return 0; + return static_cast(std::stoi(t1, nullptr, 16)); + } + catch (std::invalid_argument&) + { + return 0; + } +} + +/* Fetch the product ID from the given string */ +static inline uint16_t pidFromString(std::string const& string) +{ + try + { + std::string regex = string; + std::transform(regex.begin(), regex.end(), regex.begin(), ::toupper); + auto t1 = searchOne(regex, "VID_.*PID_([0-9a-fA-F]{4})"); + if (t1.empty()) // prevent unnecessary exceptions + return 0; + return static_cast(std::stoi(t1, nullptr, 16)); + } + catch (std::invalid_argument&) + { + return 0; + } +} + +#endif diff --git a/extern/xspublic/xscontroller/iointerface.cpp b/extern/xspublic/xscontroller/iointerface.cpp new file mode 100644 index 0000000..9438600 --- /dev/null +++ b/extern/xspublic/xscontroller/iointerface.cpp @@ -0,0 +1,171 @@ + +// Copyright (c) 2003-2022 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 +#include "iointerface.h" +#include +#ifndef _WIN32 + #include // close + #include // ioctl + #include // open, O_RDWR + #include // strcpy + #include + #include +#else + #include + #include +#endif + +#ifndef _CRT_SECURE_NO_DEPRECATE + #define _CRT_SECURE_NO_DEPRECATE + #ifdef _WIN32 + #pragma warning(disable:4996) + #endif +#endif + +/*! \class IoInterface + \brief An abstract IO interface + \details An %IoInterface provides a generic interface for dealing with an interface device such + as a file, a com port or a USB port. + \note The class is not thread safe. +*/ + +// IOInterface functions +/*! \copydoc SerialInterface::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) +{ + (void) ms; + return XRV_INVALIDOPERATION; +} +/*! \copydoc SerialInterface::waitForData(XsFilePos, XsByteArray&) */ +XsResultValue IoInterface::waitForData(XsFilePos maxLength, XsByteArray& data) +{ + (void) maxLength; + (void) data; + return XRV_INVALIDOPERATION; +} +/*! \copydoc SerialInterface::cancelIo(void) const */ +void IoInterface::cancelIo(void) const +{ + +} + +// IOInterfaceFile functions +/*! \copydoc IoInterfaceFile::appendData() */ +XsResultValue IoInterface::appendData(const XsByteArray& bdata) +{ + (void) bdata; + return XRV_INVALIDOPERATION; +} +/*! \copydoc IoInterfaceFile::closeAndDelete() */ +XsResultValue IoInterface::closeAndDelete(void) +{ + return XRV_INVALIDOPERATION; +} +/*! \copydoc IoInterfaceFile::create(const XsString&) */ +XsResultValue IoInterface::create(const XsString& filename) +{ + (void) filename; + return XRV_INVALIDOPERATION; +} +/*! \copydoc IoInterfaceFile::deleteData(XsFilePos, XsFilePos) */ +XsResultValue IoInterface::deleteData(XsFilePos start, XsFilePos length) +{ + (void) start; + (void) length; + return XRV_INVALIDOPERATION; +} +/*! \copydoc IoInterfaceFile::find(const XsByteArray&, XsFilePos&) */ +XsResultValue IoInterface::find(const XsByteArray& needleV, XsFilePos& pos) +{ + (void) needleV; + (void) pos; + return XRV_INVALIDOPERATION; +} +/*! \copydoc IoInterfaceFile::getFileSize() const */ +XsFilePos IoInterface::getFileSize(void) const +{ + return 0; +} +/*! \copydoc IoInterfaceFile::getName(XsString&) const */ +XsResultValue IoInterface::getName(XsString& filename) const +{ + (void) filename; + return XRV_INVALIDOPERATION; +} +/*! \copydoc IoInterfaceFile::getReadPosition() const */ +XsFilePos IoInterface::getReadPosition(void) const +{ + return 0; +} +/*! \copydoc IoInterfaceFile::getWritePosition() const */ +XsFilePos IoInterface::getWritePosition(void) const +{ + return 0; +} +/*! \copydoc IoInterfaceFile::insertData(XsFilePos, const XsByteArray&) */ +XsResultValue IoInterface::insertData(XsFilePos start, const XsByteArray& data) +{ + (void) start; + (void) data; + return XRV_INVALIDOPERATION; +} +/*! \copydoc IoInterfaceFile::isReadOnly() const */ +bool IoInterface::isReadOnly(void) const +{ + return true; +} +/*! \copydoc IoInterfaceFile::open(const XsString&, bool, bool) */ +XsResultValue IoInterface::open(const XsString& filename, bool createNew, bool readOnly) +{ + (void) filename; + (void) createNew; + (void) readOnly; + return XRV_INVALIDOPERATION; +} +/*! \copydoc IoInterfaceFile::setReadPosition(XsFilePos) */ +XsResultValue IoInterface::setReadPosition(XsFilePos pos) +{ + (void) pos; + return XRV_INVALIDOPERATION; +} +/*! \copydoc IoInterfaceFile::setWritePosition(XsFilePos) */ +XsResultValue IoInterface::setWritePosition(XsFilePos pos) +{ + (void) pos; + return XRV_INVALIDOPERATION; +} diff --git a/extern/xspublic/xscontroller/iointerface.h b/extern/xspublic/xscontroller/iointerface.h new file mode 100644 index 0000000..c285189 --- /dev/null +++ b/extern/xspublic/xscontroller/iointerface.h @@ -0,0 +1,137 @@ + +// Copyright (c) 2003-2022 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 IOINTERFACE_H +#define IOINTERFACE_H + +#include "xscontrollerconfig.h" +#include +#include +#include +#include +#include +#include +#include + +#include + +//! The default size of the serial read buffer in bytes +#define XS_DEFAULT_READ_BUFFER_SIZE (XS_MAXMSGLEN*8) +//! The default size of the serial write buffer in bytes +#define XS_DEFAULT_WRITE_BUFFER_SIZE XS_MAXMSGLEN + +struct XsPortInfo; + +struct IoInterface +{ +public: + /*! \brief Destructor */ + virtual ~IoInterface() {} + + /*! \brief Close the connection to the device. + \returns XRV_OK if the connection was closed successfully + */ + virtual XsResultValue close(void) = 0; + + /*! \brief Flush all data in the buffers to and from the device + \returns XRV_OK if the data was flushed successfully + */ + virtual XsResultValue flushData(void) = 0; + + /*! \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; + + /*! \brief Returns the last result value produced by this interface + \returns The last result value produced by this interface. + */ + virtual XsResultValue getLastResult(void) const = 0; + + /*! \brief Write the data contained in \a data to the device + \param data The data to write to the device. + \param written An optional %XsFilePos value that will receive the number of bytes that were + actually written. + \returns XRV_OK if the data was written successfully + */ + virtual XsResultValue writeData(const XsByteArray& data, XsFilePos* written = nullptr) = 0; + + /*! \brief Read at most \a maxLength bytes from the device into \a data + \param maxLength The maximum number of bytes to read. Depending on the device type and timeout + settings, the function may return with less than this number of bytes read. + \param data A buffer that will contain the read data. + \returns XRV_OK if all data was read successfully, XRV_TIMEOUT if some data was read, but not + \a maxLength, XRV_TIMEOUTNODATA if no data was read at all. + */ + virtual XsResultValue readData(XsFilePos maxLength, XsByteArray& data) = 0; + + //! Options for flow control and stopbits which must be used when opening a port + 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) + }; + // 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 void cancelIo(void) const; + + // IOInterfaceFile overridable functions + virtual XsResultValue appendData(const XsByteArray& bdata); + virtual XsResultValue closeAndDelete(void); + virtual XsResultValue create(const XsString& filename); + virtual XsResultValue deleteData(XsFilePos start, XsFilePos length); + virtual XsResultValue find(const XsByteArray& needleV, XsFilePos& pos); + virtual XsFilePos getFileSize(void) const; + virtual XsResultValue getName(XsString& filename) const; + virtual XsFilePos getReadPosition(void) const; + virtual XsFilePos getWritePosition(void) const; + virtual XsResultValue insertData(XsFilePos start, const XsByteArray& data); + virtual bool isReadOnly(void) const; + virtual XsResultValue open(const XsString& filename, bool createNew, bool readOnly); + virtual XsResultValue setReadPosition(XsFilePos pos); + virtual XsResultValue setWritePosition(XsFilePos pos = -1); + +protected: + /*! \brief Constructor */ + IoInterface() + {} + + XSENS_DISABLE_COPY(IoInterface); +}; +#endif diff --git a/extern/xspublic/xscontroller/iointerfacefile.cpp b/extern/xspublic/xscontroller/iointerfacefile.cpp new file mode 100644 index 0000000..01dcd04 --- /dev/null +++ b/extern/xspublic/xscontroller/iointerfacefile.cpp @@ -0,0 +1,830 @@ + +// Copyright (c) 2003-2022 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 "iointerfacefile.h" + +#include +#ifndef _WIN32 + #include // close + #include // ioctl + #include // open, O_RDWR + #include // strcpy + #include + #include + #include +#else + #include + #include + #include +#endif + +#include + +#ifndef _CRT_SECURE_NO_DEPRECATE + #define _CRT_SECURE_NO_DEPRECATE + #ifdef _WIN32 + #pragma warning(disable:4996) + #endif +#endif + +/*! Default constructor, initializes all members to their default values. +*/ +IoInterfaceFile::IoInterfaceFile() + : m_handle(NULL) + , m_fileSize(0) + , m_readPos(0) + , m_writePos(0) + , m_lastResult(XRV_OK) + , m_reading(true) + , m_readOnly(false) +{ +} + +/*! Destructor +*/ +IoInterfaceFile::~IoInterfaceFile() +{ + try + { + closeFile(); + } + catch (...) + { + } +} + +/*! \brief Write data to the end of the file. + \details The function writes the given data to the file at the end. The current write + position is also moved to the end of the file. + \param bdata The byte data to append to the file + \returns XRV_OK if the write was successful +*/ +XsResultValue IoInterfaceFile::appendData(const XsByteArray& bdata) +{ + if (!m_handle) + return m_lastResult = XRV_NOFILEOPEN; + if (m_readOnly) + return m_lastResult = XRV_READONLY; + if (!bdata.size()) + return m_lastResult = XRV_OK; + + if (m_reading || m_writePos != m_fileSize) + { + m_reading = false; + m_handle->seek_r(0); + } + m_handle->write(bdata.data(), 1, (XsFilePos) bdata.size()); + m_writePos = m_handle->tell(); + m_fileSize = m_writePos; + + return (m_lastResult = XRV_OK); +} + +////////////////////////////////////////////////////////////////////////////////////////// +/*! \brief Close the file, overrides IoInterface::close(). + \returns XRV_OK if the file was closed successfully +*/ +XsResultValue IoInterfaceFile::close() +{ + return closeFile(); +} + +/*! \brief Close the file. + \returns XRV_OK if the file was closed successfully +*/ +XsResultValue IoInterfaceFile::closeFile() +{ + if (m_handle) + { + m_handle->flush(); + m_handle->close(); + + delete m_handle; + m_handle = NULL; + } + + m_readPos = 0; + m_writePos = 0; + m_reading = true; + m_fileSize = 0; + m_readOnly = false; + + return m_lastResult = XRV_OK; +} + +/*! \brief Close the file and delete it. + \returns XRV_OK if the file was closed and deleted successfully +*/ +XsResultValue IoInterfaceFile::closeAndDelete() +{ + if (m_handle) + { + m_handle->flush(); + m_handle->close(); + + if (m_readOnly) + m_lastResult = XRV_READONLY; + else + m_lastResult = XsFile::erase(m_filename); + + delete m_handle; + m_handle = NULL; + } + else + m_lastResult = XRV_NOFILEOPEN; + + m_readPos = 0; + m_writePos = 0; + m_reading = true; + m_fileSize = 0; + m_readOnly = false; + + return m_lastResult; +} + +/*! \brief Create an empty file + \param filename The desired (path+)name of the file + \returns XRV_OK if the file was created successfully +*/ +XsResultValue IoInterfaceFile::create(const XsString& filename) +{ + if (m_handle) + return m_lastResult = XRV_ALREADYOPEN; + + m_handle = new XsFile(); + m_lastResult = m_handle->create(filename, false); + if (m_lastResult != XRV_OK) + { + delete m_handle; + m_handle = NULL; + return m_lastResult; + } + + m_lastResult = XsFile::fullPath(filename, m_filename); + + if (m_lastResult != XRV_OK) + { + m_handle->close(); + XsFile::erase(m_filename); + delete m_handle; + m_handle = NULL; + + return m_lastResult = XRV_INVALIDPARAM; + } + + m_readPos = 0; + m_writePos = 0; + m_fileSize = 0; + m_reading = true; + m_readOnly = false; + return m_lastResult = XRV_OK; +} + +/*! \brief Delete the given data from the file. + \details The function erases the given data from the file at the given write position. This + operation may take a while to complete, but is faster than insertData. + + The write position is not changed and the read position is checked for validity upon function exit. + \param start The offset of the first byte to delete + \param length The total number of bytes to delete + \returns XRV_OK if the data was deleted successfully +*/ +XsResultValue IoInterfaceFile::deleteData(XsFilePos start, XsFilePos length) +{ + if (!m_handle) + return m_lastResult = XRV_NOFILEOPEN; + if (m_readOnly) + return m_lastResult = XRV_READONLY; + + gotoWrite(); + + XsFilePos wPos = start; + XsFilePos rPos = wPos + length; + + XsFilePos read1; + XsFilePos endPos = (start + (XsFilePos) length); + if (endPos < m_fileSize) + { + XsFilePos remaining = m_fileSize - endPos; + char buffer[m_fileBlockSize]; + + // copy data + m_handle->seek(rPos); + + while (remaining > 0) + { + if (remaining >= m_fileBlockSize) + read1 = m_handle->read(buffer, 1, m_fileBlockSize); + else + read1 = m_handle->read(buffer, 1, remaining); + + remaining -= read1; + rPos += read1; + + // write block to the correct position + m_handle->seek(wPos); + wPos += m_handle->write(buffer, 1, read1); + m_handle->seek(rPos); + } + m_fileSize -= length; + } + else + m_fileSize = start; + + XsResultValue truncateResult = m_handle->truncate(m_fileSize); + + m_writePos = start; + m_handle->seek(wPos); + + return m_lastResult = truncateResult; +} + +/*! \brief Find a string of bytes in the file + \details The function searches from the current read position until the given \c needle is + found. If the needle is not found, XsResultValue::NOT_FOUND is returned. The function + will update the seek position to the first character of the found needle. + \param needleV The byte string to find. + \param pos The position where \a needleV was found. This will point to the first character + of the found \a needleV. + \returns XRV_OK if the data was found, XRV_ENDOFFILE if it wasn't found +*/ +XsResultValue IoInterfaceFile::find(const XsByteArray& needleV, XsFilePos& pos) +{ + if (!m_handle) + return m_lastResult = XRV_NOFILEOPEN; + + XsFilePos needleLength = (XsFilePos) needleV.size(); + + pos = 0; + if (needleLength == 0) + return m_lastResult = XRV_OK; + + const char* needle = (const char*) needleV.data(); + + gotoRead(); + + char buffer[m_fileBlockSize]; + 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)))); + else + readBytes = m_handle->read(buffer, 1, m_fileBlockSize); // read a block of data + + while (readBytes > 0) + { + m_readPos += readBytes; + bufferPos = 0; + + while (bufferPos < readBytes && needlePos < needleLength) + { + if (buffer[bufferPos] == needle[needlePos]) + { + // found a byte + ++needlePos; + } + else + { + if (needlePos > 0) + needlePos = 0; + else if (buffer[bufferPos] == needle[0]) + { + // found a byte + needlePos = 1; + } + } + ++bufferPos; + } + if (needlePos < needleLength) + readBytes = m_handle->read(buffer, 1, m_fileBlockSize); // read next block + else + { + m_readPos = (m_readPos + bufferPos - readBytes) - needleLength; + pos = m_readPos; + m_handle->seek(m_readPos); + return m_lastResult = XRV_OK; + } + } + return m_lastResult = XRV_ENDOFFILE; +} + +/*! \brief Return the size of the file. + \returns The size of the file. +*/ +XsFilePos IoInterfaceFile::getFileSize() const +{ + return m_fileSize; +} + +/*! \brief Return the creation date of the file + \returns The creation date of the file +*/ +XsTimeStamp IoInterfaceFile::getFileDate() const +{ +#ifdef _WIN32 + struct _stat stats; + if (_wstat(m_filename.toStdWString().c_str(), &stats) == 0) +#else + struct stat stats; + if (stat(m_filename.c_str(), &stats) == 0) +#endif + { + XsTimeStamp t = XsTimeStamp((int64_t)stats.st_mtime * 1000); + return t; + } + return XsTimeStamp(); +} + +/*! \copydoc IoInterface::flushData */ +XsResultValue IoInterfaceFile::flushData() +{ + m_handle->flush(); + + return m_lastResult = XRV_OK; +} + +/*! \brief Retrieve the filename that was last successfully opened. + + \param filename The XsString which will contain the filename. + \returns XRV_OK +*/ +XsResultValue IoInterfaceFile::getName(XsString& filename) const +{ + filename = m_filename; + return m_lastResult = XRV_OK; +} + +/*! \brief Return the filename that was last successfully opened. +*/ +XsString IoInterfaceFile::getFileName() const +{ + return m_filename; +} + +//! \brief Change from writing to reading mode +void IoInterfaceFile::gotoRead() +{ + if (m_reading) + return; + + m_handle->flush(); + m_handle->seek(m_readPos); + m_reading = true; +} + +//! \brief Change from reading to writing mode +void IoInterfaceFile::gotoWrite() +{ + if (!m_reading) + return; + + m_handle->flush(); + m_handle->seek(m_writePos); + m_reading = false; +} + +/*! \brief Insert the given data into the file. + \details The function writes the given data to the file at the current write position. This + operation may take a while to complete. + + The write position is placed at the end of the inserted data. + \param start The offset in the file to write the first byte + \param data The data to insert in the file + \returns XRV_OK if the data was inserted successfully +*/ +XsResultValue IoInterfaceFile::insertData(XsFilePos start, const XsByteArray& data) +{ + if (!m_handle) + return m_lastResult = XRV_NOFILEOPEN; + if (m_readOnly) + return m_lastResult = XRV_READONLY; + + gotoWrite(); + + XsFilePos length = (XsFilePos) data.size(); + XsFilePos rPos = start; + XsFilePos wPos = rPos + length; + + XsFilePos read1, read2; + XsFilePos remaining = m_fileSize - start; + XsFilePos bsize = (length > m_fileBlockSize) ? length : m_fileBlockSize; + char* bufferRoot = (char*) malloc((XsSize)(bsize * 2)); + if (!bufferRoot) + return XRV_OUTOFMEMORY; + char* buffer1 = bufferRoot; + char* buffer2 = bufferRoot + bsize; + char* btemp; + + // copy data + m_handle->seek(rPos); + + if (data.size() == 0) + return m_lastResult = XRV_OK; + + if (remaining >= bsize) + read1 = m_handle->read(buffer1, 1, bsize); + else + read1 = m_handle->read(buffer1, 1, remaining); + + remaining -= read1; + rPos += read1; + + while (remaining > 0) + { + // move data to correct buffer + read2 = read1; + btemp = buffer1; + buffer1 = buffer2; + buffer2 = btemp; + + // read next block + if (remaining >= bsize) + read1 = m_handle->read(buffer1, 1, bsize); + else + read1 = m_handle->read(buffer1, 1, remaining); + + remaining -= read1; + rPos += read1; + + // write block to the correct position + m_handle->seek(wPos); + wPos += m_handle->write(buffer2, 1, read2); + m_handle->seek(rPos); + } + + m_handle->seek(wPos); + /* wPos += */ m_handle->write(buffer1, 1, read1); + + m_handle->seek(start); + m_writePos = start + m_handle->write(data.data(), 1, length); + m_fileSize += length; + + free(bufferRoot); + return m_lastResult = XRV_OK; +} + +/*! \brief Open a file. + \param filename The name of the file to open + \param createNew When true, the file will be created if it doesn't exist yet + \param readOnly When true, the file will be marked as read only for %IoInterfaceFile, + preventing accidental writes to the file. + \returns XRV_OK if the file was opened successfully + \sa createFile +*/ +XsResultValue IoInterfaceFile::open(const XsString& filename, bool createNew, bool readOnly) +{ + if (m_handle) + return m_lastResult = XRV_ALREADYOPEN; + + m_handle = new XsFile(); + + //! \test does this work for non-existing files? Or do we need a check and create? + m_readOnly = readOnly; + XsResultValue localResult = m_handle->open(filename, readOnly); + + if (localResult != XRV_OK) + { + if (createNew) + localResult = m_handle->create(filename, false); + else + { + // final attempt: open it forced readonly + localResult = m_handle->open(filename, false); + m_readOnly = true; + } + } + if (localResult != XRV_OK) + { + delete m_handle; + m_handle = NULL; + return m_lastResult = XRV_INPUTCANNOTBEOPENED; // overrides a possible OUTPUTCANNOTBEOPENED + } + + bool fail = false; +#ifdef _WIN32 + wchar_t fullpath[XS_MAX_FILENAME_LENGTH]; + 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]; + if (realpath(filename.c_str(), fullpath) == NULL) + fail = true; +#endif + m_filename = XsString(fullpath); + + if (fail) + { + m_handle->close(); + delete m_handle; + m_handle = NULL; + return m_lastResult = XRV_INVALIDPARAM; + } + + m_readPos = 0; + m_writePos = 0; + m_reading = true; + m_handle->seek_r(0); + m_fileSize = m_handle->tell(); + m_handle->seek(0); + return (m_lastResult = XRV_OK); +} + +/*! \copydoc IoInterface::readData + \note This function reads exactly the number of bytes as requested from the file unless the end + of file boundary is encountered. +*/ +XsResultValue IoInterfaceFile::readData(XsFilePos maxLength, XsByteArray& data) +{ + if (!m_handle) + return m_lastResult = XRV_NOFILEOPEN; + + if (maxLength == 0) + { + data.clear(); + return m_lastResult = XRV_OK; + } + + XsFilePos length; + + gotoRead(); + data.setSize((XsSize) maxLength); + + length = m_handle->read(data.data(), 1, maxLength); + if (m_handle->eof() && length <= 0) + { + data.clear(); + return (m_lastResult = XRV_ENDOFFILE); + } + + m_readPos += length; + if (length < maxLength) + data.pop_back((XsSize)(maxLength - length)); + return m_lastResult = XRV_OK; +} + +/*! \brief This function will read blocks of data aligned to \a m_fileBlockSize + \details The Function will read as much data as is necessary to align to the block size + \a blockCount blocks. + So the given blockCount is an indication for the minimum amount of data read, unless the end of file is encountered. + \param blockCount The number of blocks to read. + \param data A buffer for the data that was read from the file + \returns XRV_OK if the data was read successfully +*/ +XsResultValue IoInterfaceFile::readDataBlocks(XsFilePos blockCount, XsByteArray& data) +{ + XsFilePos realign = (m_readPos & (m_fileBlockSize - 1)); + if (realign) + blockCount = m_fileBlockSize * blockCount + m_fileBlockSize - realign; + else + blockCount *= m_fileBlockSize; + if (blockCount == 0) + return XRV_OK; + return readData(blockCount, data); +} + +/*! \brief Read data from the file and put it into the data buffer. + + This function reads up to the number of bytes as requested from the file. + The function will also stop if the given terminator character is encountered. + The terminator is included in the output buffer. + \param maxLength The amount of data that will be read. + \param terminator A character that will end the read operation if encountered. + \param bdata A buffer that will store the read data. + \returns XRV_OK if the data was read successfully +*/ +XsResultValue IoInterfaceFile::readTerminatedData(XsFilePos maxLength, unsigned char terminator, XsByteArray& bdata) +{ + if (!m_handle) + return m_lastResult = XRV_NOFILEOPEN; + + if (maxLength == 0) + { + bdata.clear(); + return m_lastResult = XRV_OK; + } + + bdata.setSize((XsSize) maxLength); + char* data = (char*) bdata.data(); + + XsFilePos length; + int readChar; + + gotoRead(); + + length = 0; + readChar = m_handle->getc(); + + while (!m_handle->eof() && !m_handle->error()) + { + data[length] = (char) readChar; + ++length; + ++m_readPos; + + if (length >= maxLength) + return m_lastResult = XRV_OK; + if ((unsigned char) readChar == terminator) + { + bdata.pop_back((XsSize)(maxLength - length)); + return m_lastResult = XRV_OK; + } + } + bdata.pop_back((XsSize)(maxLength - length)); + return m_lastResult = XRV_ENDOFFILE; +} + +/*! \brief Set the new absolute read position + \details The read position is checked against the file size first. + \param pos The new read position + \returns XRV_OK if the read position was updated successfully +*/ +XsResultValue IoInterfaceFile::setReadPosition(XsFilePos pos) +{ + if (!m_handle) + return m_lastResult = XRV_NOFILEOPEN; + + if (m_readPos != pos) + { + m_readPos = pos; + if (m_reading) + m_handle->seek(m_readPos); + } + + return m_lastResult = XRV_OK; +} + +/*! \brief Set the new absolute write position + \details The write position is checked against the file size first. + \param pos The new write position + \returns XRV_OK if the write position was updated successfully +*/ +XsResultValue IoInterfaceFile::setWritePosition(XsFilePos pos) +{ + if (!m_handle) + return m_lastResult = XRV_NOFILEOPEN; + if (m_readOnly) + return m_lastResult = XRV_READONLY; + + if (pos == -1) + { + if (m_reading) + m_reading = false; + m_handle->seek_r(0); + m_writePos = m_handle->tell(); + } + else + { + if (m_writePos != pos) + { + m_writePos = pos; + if (!m_reading) + m_handle->seek(m_writePos); + } + } + + return m_lastResult = XRV_OK; +} + +/*! \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) +{ + if (!m_handle) + return m_lastResult = XRV_NOFILEOPEN; + if (m_readOnly) + return m_lastResult = XRV_READONLY; + + XsFilePos length = (XsFilePos) data.size(); + if (length == 0) + return m_lastResult = XRV_OK; + + gotoWrite(); + XsFilePos writeRes = m_handle->write(data.data(), 1, length); + if (writeRes == (XsFilePos)EOF || writeRes < length) + { + 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; + } + } + m_writePos += writeRes; + if (written) + *written = writeRes; + + if (m_writePos > m_fileSize) + m_fileSize = m_writePos; + + return m_lastResult = XRV_OK; +} + +/*! \brief Return the current read position. + \returns The current read position. +*/ +XsFilePos IoInterfaceFile::getReadPosition() const +{ + return m_readPos; +} + +/*! \brief Return the current write position. + \returns The current write position. +*/ +XsFilePos IoInterfaceFile::getWritePosition() const +{ + return m_writePos; +} + +/*! \brief Return the result code of the last operation. + \returns The result code of the last operation. +*/ +XsResultValue IoInterfaceFile::getLastResult() const +{ + return m_lastResult; +} + +/*! \brief Return whether the file is open or not. + \returns true if the file is open +*/ +bool IoInterfaceFile::isOpen() const +{ + return m_handle != NULL; +} + +/*! \brief Return whether the file is read-only or not. + \returns true if the file is read-only +*/ +bool IoInterfaceFile::isReadOnly(void) const +{ + return !isOpen() || m_readOnly; +} + +/*! \brief Make sure the file is at least \a minSize bytes big. + \param minSize The minimum file size required + \returns XRV_OK if the reserve succeeded + \details The file will not be truncated, any added bytes will be filled with '\0' characters +*/ +XsResultValue IoInterfaceFile::reserve(XsFilePos minSize) +{ + if (isReadOnly()) + return XRV_READONLY; + + if (minSize <= m_fileSize) + return XRV_OK; + + auto rv = m_handle->resize(minSize); + if (rv != XRV_OK) + return rv; + + m_fileSize = minSize; + return XRV_OK; +} + +/*! \brief Flushes the buffers of a specified file and causes all buffered data to be written to a file. + \details This will ensure that the metadata is written to the file. + \note Only for Windows! + \returns XRV_OK if the buffers were flushed successfully +*/ +XsResultValue IoInterfaceFile::flushFileBuffers() +{ +#ifdef XSENS_WINDOWS + return FlushFileBuffers((HANDLE)_get_osfhandle(_fileno(m_handle->handle()))) ? XRV_OK : XRV_ERROR; +#else + return XRV_OK; +#endif +} diff --git a/extern/xspublic/xscontroller/iointerfacefile.h b/extern/xspublic/xscontroller/iointerfacefile.h new file mode 100644 index 0000000..b22774b --- /dev/null +++ b/extern/xspublic/xscontroller/iointerfacefile.h @@ -0,0 +1,109 @@ + +// Copyright (c) 2003-2022 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 IOINTERFACEFILE_H +#define IOINTERFACEFILE_H + +#include +#include "iointerface.h" +#include + +/*! \brief The low-level file communication class. +*/ +class IoInterfaceFile : public IoInterface +{ +protected: + //! The file handle, also indicates if the file is open or not. + XsFile* m_handle; + //! Contains the size of the file + XsFilePos m_fileSize; + //! The last read position in the file + XsFilePos m_readPos; + //! The last write position in the file + XsFilePos m_writePos; + //! The last result of an operation + mutable XsResultValue m_lastResult; + //! 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. + + This value is used to check whether or not a seek is required to perform a + requested read or write operation. + */ + bool m_reading; + //! Indicates if the file was opened in read-only mode + bool m_readOnly; + + void gotoRead(); + void gotoWrite(); +public: + IoInterfaceFile(); + ~IoInterfaceFile(); + + // Function overrides + XsResultValue close() override; + XsResultValue closeFile(); + XsResultValue flushData() override; + bool isOpen() const override; + XsResultValue getLastResult() const 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); + + // Other functions + XsResultValue appendData(const XsByteArray& bdata); + XsResultValue closeAndDelete(); + XsResultValue create(const XsString& filename); + XsResultValue deleteData(XsFilePos start, XsFilePos length); + XsResultValue find(const XsByteArray& data, XsFilePos& pos); + XsFilePos getFileSize() const; + XsTimeStamp getFileDate() const; + XsResultValue getName(XsString& filename) const; + XsString getFileName() const; + XsFilePos getReadPosition() const; + XsFilePos getWritePosition() const; + XsResultValue insertData(XsFilePos start, const XsByteArray& data); + bool isReadOnly() const; + XsResultValue open(const XsString& filename, bool createNew, bool readOnly); + XsResultValue setReadPosition(XsFilePos pos); + XsResultValue setWritePosition(XsFilePos pos = -1); + XsResultValue reserve(XsFilePos minSize); + XsResultValue flushFileBuffers(); + + //! \brief The default file block size + static const XsFilePos m_fileBlockSize = 4096; + + XSENS_DISABLE_COPY(IoInterfaceFile); +}; + +#endif diff --git a/extern/xspublic/xscontroller/iprotocolhandler.h b/extern/xspublic/xscontroller/iprotocolhandler.h new file mode 100644 index 0000000..bb9205f --- /dev/null +++ b/extern/xspublic/xscontroller/iprotocolhandler.h @@ -0,0 +1,119 @@ + +// Copyright (c) 2003-2022 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 IPROTOCOLHANDLER_H +#define IPROTOCOLHANDLER_H + +#include "messagelocation.h" +#include "xsprotocoltype.h" +#include +#include + +//-------------------------------------------------------------------------------- +/*! \brief Interface class for protocol handlers + \details Describes the interfaces of the protocol handling classes. The protocol handlers are + used to convert a binary data stream into XsMessage objects. +*/ +class IProtocolHandler +{ +public: + //! \brief Destructor + virtual ~IProtocolHandler() {} + + /*! \brief Find the first message in the \a raw byte stream + \details This function scans \a raw for a sequence of bytes that can contain a full message. + It returns the location and total byte size of the message so that the + caller can remove those bytes from the stream. The return value can also describe that a + partial message has been found. Return values: + \li \a startpos >= 0 and \a size > 0: A full message with \a size has been found at \a startpos. + \li \a startpos >= 0 and \a size == 0: The start of a message has been found at \a startpos, but the size could not yet be determined. + \li \a startpos >= 0 and \a size < 0: The start of a message has been found at \a startpos, and the size of the full message is at least \a -size. + \li \a startpos < 0: No messages have been found. + + \param type The protocol type that was used. + \param raw The raw byte stream to analyze. + \returns A %MessageLocation object that describes what was found. + */ + virtual MessageLocation findMessage(XsProtocolType& type, const XsByteArray& raw) const = 0; + + /*! \brief Converts \a raw data using \a location into a %XsMessage object. + \param location The location of a message to convert from \a raw data. + \param raw The raw byte stream. + \returns A %XsMessage object that was converted from raw byte stream. + */ + virtual XsMessage convertToMessage(MessageLocation& location, const XsByteArray& raw) const = 0; + + /*! \brief Returns the minimum size of a valid message + \details This value may differ for different protocols, but is always at least 1. + \returns The minimum size of a valid message for the protocol. + */ + virtual int minimumMessageSize() const = 0; + + /*! \brief Returns the maximum size of a valid message + \details This value may differ for different protocols. + \returns The maximum size of a valid message for the protocol. + */ + virtual int maximumMessageSize() const = 0; + + /*! \brief Returns the type of the protocol handler + \details Each protocol handler has a locally unique id that can be used for instantiation of + the correct protocol handler. + \returns The type id of the protocol handler. + */ + virtual int type() const = 0; + + /*! \brief Tells the protocol handler to ignore/expand its maximum message size + \details This is mostly used when reading from a file that is known to contain correct data. + Please note that the protocol handler can decide to ignore this call, which is what the default + implementation does. + \param ignore Set to true to ignore the maximum message size check. + */ + virtual void ignoreMaximumMessageSize(bool ignore) + { + (void) ignore; + } + + /*! \brief Enables or disables the protocol handler if supported. + A disabled protocol handler ignores and discards all data provided through the \a findMessage function + \param enable : If true enables the protcol handler, if false it disables it. + \returns true if the protocol handler was correctly enabled or disabled, false if the protocol handler doesn't support this function + */ + virtual bool setEnable(bool enable) + { + (void) enable; + return false; + } +}; + +//-------------------------------------------------------------------------------- + +#endif diff --git a/extern/xspublic/xscontroller/iprotocolmanager.h b/extern/xspublic/xscontroller/iprotocolmanager.h new file mode 100644 index 0000000..83a4901 --- /dev/null +++ b/extern/xspublic/xscontroller/iprotocolmanager.h @@ -0,0 +1,72 @@ + +// Copyright (c) 2003-2022 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 IPROTOCOLMANAGER_H +#define IPROTOCOLMANAGER_H + +#include +#include "messagelocation.h" +#include "xsprotocoltype.h" + +/*! \brief Interface class for protocol manager + \details Describes the interfaces of a manager of different protocols \sa ProtocolHandler +*/ +class IProtocolManager +{ +public: + + //! \brief Destructor + virtual ~IProtocolManager() {} + + /*! \brief Will let all supported protocols attempt finding a raw message in the given raw data + \param type: The protocol type to us + \param raw: The input raw byte array in which to look for a message + \returns A MessageLocation object describing the best possible found message \sa MessageLocation + */ + virtual MessageLocation findMessage(XsProtocolType& type, const XsByteArray& raw) = 0; + + /*! \brief Converts \a raw data using \a location into a %XsMessage object. + \param type: The protocol type to use. + \param location: The location of a message to convert from \a raw data. + \param raw: The raw byte stream. + \returns A %XsMessage object that was converted from raw byte stream. + */ + virtual XsMessage convertToMessage(XsProtocolType& type, MessageLocation& location, const XsByteArray& raw) = 0; + + /*! \brief Performs a sanity check on the given message + \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; +}; + +#endif diff --git a/extern/xspublic/xscontroller/lastresultmanager.h b/extern/xspublic/xscontroller/lastresultmanager.h new file mode 100644 index 0000000..16ec46f --- /dev/null +++ b/extern/xspublic/xscontroller/lastresultmanager.h @@ -0,0 +1,95 @@ + +// Copyright (c) 2003-2022 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 LASTRESULTMANAGER_H +#define LASTRESULTMANAGER_H + +#include +#include + +/*! \class LastResultManager + \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 +{ + XsResultValue m_lastResult; + XsString m_lastResultText; +public: + LastResultManager() : m_lastResult(XRV_OK) {} + + /*! \brief Sets the last results + \param res : The result value to set + \param text : The result text to set + */ + void set(XsResultValue res, XsString const& text) + { + m_lastResult = res; + m_lastResultText = text; + } + + /*! \returns The last result value + */ + XsResultValue lastResult() const + { + return m_lastResult; + } + + /*! \returns The last result text + */ + XsString lastResultText() const + { + if (m_lastResultText.empty()) + return XsResultValue_toString(m_lastResult); + return m_lastResultText; + } + + /*! \brief Assignment operator, copies contents from the \a res result value + \param res : The result value to copy from + \returns The assigned result value + */ + XsResultValue operator = (XsResultValue res) + { + m_lastResult = res; + m_lastResultText.clear(); + return res; + } + + /*! \returns The last result value + */ + operator XsResultValue() const + { + return m_lastResult; + } +}; + +#endif diff --git a/extern/xspublic/xscontroller/messageextractor.cpp b/extern/xspublic/xscontroller/messageextractor.cpp new file mode 100644 index 0000000..786a472 --- /dev/null +++ b/extern/xspublic/xscontroller/messageextractor.cpp @@ -0,0 +1,196 @@ + +// Copyright (c) 2003-2022 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 "messageextractor.h" +#include "xscontrollerconfig.h" +#include + +/*! \class MessageExtractor + + Helper class that extracts XsMessages from a stream of data. The user must call the \a processNewData function every time a new block of data is available. + It is advised not to process too small blocks of data (e.g. per byte); Every single message must not span more than \a m_maxIncompleteRetryCount blocks + to guarantee correct operation. + + A MessageExtractor object maintains a buffer representing a sliding window over the data stream that is just big enough to contain any incompletely received + XsMessage. The user can explicitly clear this buffer using the \a clearBuffer function +*/ + + +/*! \brief Constructor + \param protocolManager: the protocol manager to use for finding messages in the buffered data +*/ +MessageExtractor::MessageExtractor(std::shared_ptr const& protocolManager) + : m_protocolManager(protocolManager) + , m_retryTimeout(0) + , m_buffer() + , m_maxIncompleteRetryCount(5) +{ +} + +/*! \brief Processes new incoming data for message extraction + + \param devicePtr: %XsDevice pointer to call a onMessageDetected2 callback + \param newData: Buffer that contains the newly arrived data + \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) +{ + if (!m_protocolManager) + return XRV_ERROR; + +#ifdef XSENS_DEBUG + XsSize prevSize = m_buffer.size(); +#endif + if (newData.size()) + m_buffer.append(newData); +#ifdef XSENS_DEBUG + assert(m_buffer.size() == newData.size() + prevSize); +#endif + + XsSize popped = 0; + messages.clear(); + + while (true) + { + assert(popped <= m_buffer.size()); + + XsByteArray raw(m_buffer.data() + popped, m_buffer.size() - popped, XSDF_None); + + XsProtocolType type; + MessageLocation location = m_protocolManager->findMessage(type, raw); + assert(location.m_startPos == -1 || location.m_incompletePos == -1 || location.m_incompletePos < location.m_startPos); + + if (location.isValid()) + { + XsByteArray detectedMessage(&raw[(XsSize)(ptrdiff_t) location.m_startPos], (XsSize)(ptrdiff_t) location.m_size, XSDF_None); + if (devicePtr != nullptr) + devicePtr->onMessageDetected2(type, detectedMessage); + + XsMessage message = m_protocolManager->convertToMessage(type, location, raw); + assert(location.m_startPos == -1 || location.m_incompletePos == -1 || location.m_incompletePos < location.m_startPos); + + if (location.isValid() && !message.empty() && m_protocolManager->validateMessage(message)) + { + // We are going to skip something + if (location.m_incompletePos != -1) + { + // We are going to skip an incomplete but potentially valid message + // First wait a couple of times to see if we can complete that message before skipping + if (m_retryTimeout++ < m_maxIncompleteRetryCount) + { + // wait a bit until we have more data + // but already pop the data that we know contains nothing useful + if (location.m_incompletePos > 0) + { + JLALERTG("Skipping " << location.m_incompletePos << " bytes from the input buffer"); + popped += (XsSize)(ptrdiff_t) location.m_incompletePos; + } + + break; + } + else + { + // We've waited a bit for the incomplete message to complete but it never completed + // So: We are going to skip an incomplete but potentially valid message + JLALERTG("Skipping " << location.m_startPos << " bytes from the input buffer that may contain an incomplete message at " << location.m_incompletePos + << " found: " << (int)message.getTotalMessageSize() + << std::hex << std::setfill('0') + << " First bytes " << std::setw(2) << (int)message.getMessageStart()[0] + << " " << std::setw(2) << (int)message.getMessageStart()[1] + << " " << std::setw(2) << (int)message.getMessageStart()[2] + << " " << std::setw(2) << (int)message.getMessageStart()[3] + << " " << std::setw(2) << (int)message.getMessageStart()[4] + << std::dec << std::setfill(' ')); + } + } + else if (location.m_startPos > 0) + { + // We are going to skip something but we are not going to skip an incomplete but potentially valid message + JLALERTG("Skipping " << location.m_startPos << " bytes from the input buffer"); + } + + if (m_retryTimeout) + { + JLTRACEG("Resetting retry count from " << m_retryTimeout); + m_retryTimeout = 0; + } + + // message is valid, remove data from cache + 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); + else + break; + } + } + else + { + int bestPosition = location.m_incompletePos >= 0 ? location.m_incompletePos : location.m_startPos; + if (bestPosition < 0) + popped = m_buffer.size(); + else + popped += (XsSize)(ptrdiff_t)bestPosition; + break; + } + } + + if (popped > 0) + m_buffer.pop_front(popped); + if (messages.empty()) + return XRV_TIMEOUTNODATA; + + return XRV_OK; +} + +/*! \brief Clears the processing buffer +*/ +void MessageExtractor::clearBuffer() +{ + JLDEBUGG(this); + m_buffer.clear(); +} + +/*! \brief Sets the maximum number of process attempts before advancing over an incompletely received message. + \param max The maximum number set + \return Old maximum number +*/ +int MessageExtractor::setMaxIncompleteRetryCount(int max) +{ + int rv = m_maxIncompleteRetryCount; + m_maxIncompleteRetryCount = max; + return rv; +} diff --git a/extern/xspublic/xscontroller/messageextractor.h b/extern/xspublic/xscontroller/messageextractor.h new file mode 100644 index 0000000..5e2f797 --- /dev/null +++ b/extern/xspublic/xscontroller/messageextractor.h @@ -0,0 +1,62 @@ + +// Copyright (c) 2003-2022 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 MESSAGEEXTRACTOR_H +#define MESSAGEEXTRACTOR_H + +#include +#include +#include +#include +#include "xsdevice_def.h" +#include "iprotocolmanager.h" + +class MessageExtractor +{ +public: + MessageExtractor(std::shared_ptr const& protocolManager); + + XsResultValue processNewData(XsDevice* devicePtr, XsByteArray const& newData, std::deque& messages); + + void clearBuffer(); + int setMaxIncompleteRetryCount(int max); + +private: + std::shared_ptr m_protocolManager; + int m_retryTimeout; + XsByteArray m_buffer; + int m_maxIncompleteRetryCount; +}; + + +#endif + diff --git a/extern/xspublic/xscontroller/messagelocation.h b/extern/xspublic/xscontroller/messagelocation.h new file mode 100644 index 0000000..e4d170f --- /dev/null +++ b/extern/xspublic/xscontroller/messagelocation.h @@ -0,0 +1,81 @@ + +// Copyright (c) 2003-2022 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 MESSAGELOCATION_H +#define MESSAGELOCATION_H + +/*! \brief Stores the location of a message in a buffer using a start position and a size +*/ +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 + + /*! The offset of the first incomplete potentially eventually valid message that has been + skipped to reach the first valid and complete message at m_startPos. + If no valid complete message has been found (m_startPos == -1), m_incompletePos still + contains the offset of the first incomplete message. + invariant: m_incompletePos == -1 || m_startPos == -1 || m_incompletePos < m_startPos + */ + int m_incompletePos; + + /*! The expected size of the first incomplete potentially eventually valid message that has been + skipped to reach the first valid and complete message at m_startPos. + If a valid complete message has been found at a non-0 start position and m_incompletePos is not -1, + m_incompleteSize will contain the expected size of the incomplete message. + */ + int m_incompleteSize; + + /*! \brief Constructor, initializes by default to an invalid message + \param start The offset of the first byte of the message + \param size The size of the message + \param incompletePos The offset of the first byte of an incomplete but potentially eventually valid message + \param incompleteSize The expected size of the message at \a incompletePos, if that is not -1 + \sa isValid() \sa m_startPos \sa m_size \sa IProtocolHandler::findMessage + */ + MessageLocation(int start = -1, int size = 0, int incompletePos = -1, int incompleteSize = 0) + : m_startPos(start) + , m_size(size) + , m_incompletePos(incompletePos) + , m_incompleteSize(incompleteSize) + {} + + /*! \brief Returns whether the stored message information describes a valid message + */ + inline bool isValid() const + { + return m_startPos >= 0 && m_size > 0; + } +}; + +#endif diff --git a/extern/xspublic/xscontroller/messageserializer.cpp b/extern/xspublic/xscontroller/messageserializer.cpp new file mode 100644 index 0000000..50243d2 --- /dev/null +++ b/extern/xspublic/xscontroller/messageserializer.cpp @@ -0,0 +1,402 @@ + +// Copyright (c) 2003-2022 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 "messageserializer.h" +#include +#include +#include +#include +#include +#include + + +/*! \class MessageSerializer + \brief A class that does the message serialization +*/ + +/*! \brief Default constructor +*/ +MessageSerializer::MessageSerializer(XsMessage& msg, XsSize offset) + : m_message(msg) + , m_index(offset) +{ + +} + +/*! \brief Destructor +*/ +MessageSerializer::~MessageSerializer() +{ + try + { + finalize(); + } + catch (...) + { + } +} + +/*! \brief Output stream operator that adds a XsDataIdentifier to the stream + \param id The data identifier + \returns A reference to this object +*/ +MessageSerializer& MessageSerializer::operator<<(XsDataIdentifier id) +{ + return operator<<((uint16_t)id); +} + +/*! \brief Output stream operator that adds a XsCanDataIdentifier to the stream + \param id The CAN data identifier + \returns A reference to this object +*/ +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 length enum value + \returns A reference to this object +*/ +MessageSerializer& MessageSerializer::operator<<(XsCanFrameFormat idl) +{ + 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) +{ + 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) +{ + m_message.setDataByte(value, m_index); + m_index += sizeof(value); + return *this; +} + +/*! \brief Output stream operator that adds a uint16_t to the stream + \param value The value to add to the stream + \returns A reference to this object +*/ +MessageSerializer& MessageSerializer::operator<<(uint16_t value) +{ + m_message.setDataShort(value, m_index); + m_index += sizeof(value); + return *this; +} + +/*! \brief Output stream operator that adds a uint32_t to the stream + \param value The value to add to the stream + \returns A reference to this object +*/ +MessageSerializer& MessageSerializer::operator<<(uint32_t value) +{ + m_message.setDataLong(value, m_index); + m_index += sizeof(value); + return *this; +} + +/*! \brief Output stream operator that adds a uint64_t to the stream + \param value The value to add to the stream + \returns A reference to this object +*/ +MessageSerializer& MessageSerializer::operator<<(uint64_t value) +{ + m_message.setDataLongLong(value, m_index); + m_index += sizeof(value); + return *this; +} + +/*! \brief Output stream operator that adds a XsOutputConfigurationArray to the stream + \param config The output configuration array + \returns A reference to this object +*/ +MessageSerializer& MessageSerializer::operator<<(const XsOutputConfigurationArray& config) +{ + if (config.size() == 0) + return (*this << (XsOutputConfiguration(XDI_None, 0))); + + for (auto& cfg : config) + *this << cfg; + return *this; +} + +/*! \brief Output stream operator that adds a XsOutputConfiguration to the stream + \param cfg The output configuration + \returns A reference to this object +*/ +MessageSerializer& MessageSerializer::operator<<(const XsOutputConfiguration& cfg) +{ + return (*this << cfg.m_dataIdentifier << cfg.m_frequency); +} + +/*! \brief Output stream operator that adds a XsOutputConfigurationArray to the stream + \param config The output configuration array + \returns A reference to this object +*/ +MessageSerializer& MessageSerializer::operator<<(const XsCanOutputConfigurationArray& config) +{ + if (config.size() == 0) + return (*this << (XsCanOutputConfiguration(XCFF_11Bit_Identifier, XCDI_Invalid, 0, 0))); + + for (auto& cfg : config) + *this << cfg; + return *this; +} + +/*! \brief Output stream operator that adds a XsCanOutputConfiguration to the stream + \param cfg The output configuration + \returns A reference to this object +*/ +MessageSerializer& MessageSerializer::operator<<(const XsCanOutputConfiguration& cfg) +{ + return (*this << cfg.m_dataIdentifier << cfg.m_frameFormat << cfg.m_id << cfg.m_frequency); +} + + +/*! \brief Appends the data to the message + \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) +{ + m_message.setDataBuffer(data, size, m_index); + m_index += size; +} + +/*! \brief Finalizes the message serialization +*/ +void MessageSerializer::finalize() +{ + m_message.resizeData(m_index); +} + + +/*! \class MessageDeserializer + \brief A class that does the message deserialization +*/ + +/*! \brief Default constructor +*/ +MessageDeserializer::MessageDeserializer(const XsMessage& msg, XsSize offset) + : m_message(msg) + , m_index(offset) +{ +} + +/*! \brief Destructor +*/ +MessageDeserializer::~MessageDeserializer() +{ +} + +/*! \brief Input stream operator that takes a XsDataIdentifier from the stream + \param value Reference in which the data identifier is stored + \returns A reference to this object +*/ +MessageDeserializer& MessageDeserializer::operator>>(XsDataIdentifier& value) +{ + uint16_t v; + operator>>(v); + value = static_cast(v); + return *this; +} + +/*! \brief Input stream operator that takes a XsCanDataIdentifier from the stream + \param value Reference in which the CAN data identifier is stored + \returns A reference to this object +*/ +MessageDeserializer& MessageDeserializer::operator>>(XsCanDataIdentifier& value) +{ + uint8_t v; + operator>>(v); + value = static_cast(v); + return *this; +} + +/*! \brief Input stream operator that takes a XsCanIdLength from the stream + \param value Reference in which the CAN ID length is stored + \returns A reference to this object +*/ +MessageDeserializer& MessageDeserializer::operator>>(XsCanFrameFormat& value) +{ + uint8_t v; + operator>>(v); + value = static_cast(v); + return *this; +} + +/*! \brief Input stream operator that takes a XsDeviceId from the stream + \param value Reference in which the device ID is stored + \returns A reference to this object +*/ +MessageDeserializer& MessageDeserializer::operator>>(XsDeviceId& value) +{ + uint32_t v; + operator>>(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; +} + +/*! \brief Input stream operator that takes a uint8_t from the stream + \param value Reference in which the read value is stored + \returns A reference to this object +*/ +MessageDeserializer& MessageDeserializer::operator>>(uint8_t& value) +{ + value = m_message.getDataByte(m_index); + m_index += sizeof(value); + return *this; +} + +/*! \brief Input stream operator that takes a int8_t from the stream + \param value Reference in which the read value is stored + \returns A reference to this object +*/ +MessageDeserializer& MessageDeserializer::operator>>(int8_t& value) +{ + value = (int8_t)m_message.getDataByte(m_index); + m_index += sizeof(value); + return *this; +} + +/*! \brief Input stream operator that takes a uint16_t from the stream + \param value Reference in which the read value is stored + \returns A reference to this object +*/ +MessageDeserializer& MessageDeserializer::operator>>(uint16_t& value) +{ + value = m_message.getDataShort(m_index); + m_index += sizeof(value); + return *this; +} + +/*! \brief Input stream operator that takes a uint32_t from the stream + \param value Reference in which the read value is stored + \returns A reference to this object +*/ +MessageDeserializer& MessageDeserializer::operator>>(uint32_t& value) +{ + value = m_message.getDataLong(m_index); + m_index += sizeof(value); + return *this; +} + +/*! \brief Input stream operator that takes a uint64_t from the stream + \param value Reference in which the read value is stored + \returns A reference to this object +*/ +MessageDeserializer& MessageDeserializer::operator>>(uint64_t& value) +{ + value = m_message.getDataLongLong(m_index); + m_index += sizeof(value); + return *this; +} + +/*! \brief Input stream operator that takes a XsOutputConfigurationArray from the stream + \param config Reference in which the output configuration array is stored + \returns A reference to this object +*/ +MessageDeserializer& MessageDeserializer::operator>>(XsOutputConfigurationArray& config) +{ + config.clear(); + while (!atEnd()) + { + XsOutputConfiguration cfg; + *this >> cfg; + config.push_back(cfg); + } + return *this; +} + +/*! \brief Input stream operator that takes a XsOutputConfiguration from the stream + \param cfg Reference in which the output configuration is stored + \returns A reference to this object +*/ +MessageDeserializer& MessageDeserializer::operator>>(XsOutputConfiguration& cfg) +{ + return (*this >> cfg.m_dataIdentifier >> cfg.m_frequency); +} + +/*! \brief Input stream operator that takes a XsCanOutputConfigurationArray from the stream + \param config Reference in which the CAN output configuration array is stored + \returns A reference to this object +*/ +MessageDeserializer& MessageDeserializer::operator>>(XsCanOutputConfigurationArray& config) +{ + config.clear(); + while (!atEnd()) + { + XsCanOutputConfiguration cfg; + *this >> cfg; + config.push_back(cfg); + } + return *this; +} + +/*! \brief Input stream operator that takes a XsCanOutputConfiguration from the stream + \param cfg Reference in which the CAN output configuration is stored + \returns A reference to this object +*/ +MessageDeserializer& MessageDeserializer::operator>>(XsCanOutputConfiguration& cfg) +{ + return (*this >> cfg.m_dataIdentifier >> cfg.m_frameFormat >> cfg.m_id >> cfg.m_frequency); +} + +/*! \brief Checks if we are at the end of message. + \returns true if we are at the end. +*/ +bool MessageDeserializer::atEnd() const +{ + assert(m_index <= m_message.getDataSize()); + return m_index == m_message.getDataSize(); +} diff --git a/extern/xspublic/xscontroller/messageserializer.h b/extern/xspublic/xscontroller/messageserializer.h new file mode 100644 index 0000000..2ffe923 --- /dev/null +++ b/extern/xspublic/xscontroller/messageserializer.h @@ -0,0 +1,185 @@ + +// Copyright (c) 2003-2022 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 MESSAGESERIALIZER_H +#define MESSAGESERIALIZER_H + +#include +#include +#include +#include + +struct XsDeviceId; +struct XsOutputConfigurationArray; +struct XsOutputConfiguration; +struct XsCanOutputConfigurationArray; +struct XsCanOutputConfiguration; + +class MessageSerializer +{ +public: + 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); + + /*! \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) + { + return (operator<<((uint64_t)value)); + } + + /*! \brief Output stream operator that adds a int32_t to the stream + \param value The value to add to the stream + \returns A reference to this object + */ + inline MessageSerializer& operator<<(int32_t value) + { + return (operator<<((uint32_t)value)); + } + + /*! \brief Output stream operator that adds a int16_t to the stream + \param value The value to add to the stream + \returns A reference to this object + */ + inline MessageSerializer& operator<<(int16_t value) + { + return (operator<<((uint16_t)value)); + } + + /*! \brief Output stream operator that adds a int8_t to the stream + \param value The value to add to the stream + \returns A reference to this object + */ + inline MessageSerializer& operator<<(int8_t value) + { + return (operator<<((uint8_t)value)); + } + + MessageSerializer& operator<<(const XsOutputConfigurationArray& config); + MessageSerializer& operator<<(const XsOutputConfiguration& cfg); + + MessageSerializer& operator<<(const XsCanOutputConfigurationArray& config); + MessageSerializer& operator<<(const XsCanOutputConfiguration& cfg); + + /*! \returns The current index */ + inline XsSize index() const + { + return m_index; + } + + void append(const uint8_t* data, XsSize size); + void finalize(); +private: + XsMessage& m_message; + XsSize m_index; +}; + +class MessageDeserializer +{ +public: + 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); + + /*! \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) + { + return (operator>>((uint64_t&)value)); + } + + /*! \brief Input stream operator that takes a int32_t from the stream + \param value Reference in which the read value is stored + \returns A reference to this object + */ + inline MessageDeserializer& operator>>(int32_t& value) + { + return (operator>>((uint32_t&)value)); + } + + /*! \brief Input stream operator that takes a int16_t from the stream + \param value Reference in which the read value is stored + \returns A reference to this object + */ + inline MessageDeserializer& operator>>(int16_t& value) + { + return (operator>>((uint16_t&)value)); + } + + bool atEnd() const; + + MessageDeserializer& operator>>(XsOutputConfigurationArray& config); + MessageDeserializer& operator>>(XsOutputConfiguration& cfg); + + MessageDeserializer& operator>>(XsCanOutputConfigurationArray& config); + MessageDeserializer& operator>>(XsCanOutputConfiguration& cfg); + + //! \returns The current message + const XsMessage& message() const + { + return m_message; + } + + //! \returns The current index + inline XsSize index() const + { + return m_index; + } + +private: + const XsMessage& m_message; + XsSize m_index; +}; + +#endif diff --git a/extern/xspublic/xscontroller/mtbdatalogger.cpp b/extern/xspublic/xscontroller/mtbdatalogger.cpp new file mode 100644 index 0000000..42c434e --- /dev/null +++ b/extern/xspublic/xscontroller/mtbdatalogger.cpp @@ -0,0 +1,161 @@ + +// Copyright (c) 2003-2022 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 "mtbdatalogger.h" +#include "iointerfacefile.h" +#include "protocolhandler.h" + +/*! \class MtbDataLogger + \brief A class for logging the mtb data +*/ + +/*! \brief Default constructor +*/ +MtbDataLogger::MtbDataLogger() : + m_lastResult(XRV_OK) +{ +} + +MtbDataLogger::~MtbDataLogger() +{ + try + { + close(false); + } + catch (...) + { + } +} + +/*! \brief Open a log file for output. + \details This function opens the supplied log file for writing. + \param filename The name of the file to open. It is recommended to use a fully qualified path+filename. + \note This function is only available in configuration mode. + \returns True if successful + \see close +*/ +bool MtbDataLogger::create(const XsString& filename) +{ + if (m_ioInterfaceFile) + { + m_lastResult = XRV_ALREADYOPEN; + return false; + } + + m_ioInterfaceFile = std::shared_ptr(new IoInterfaceFile); + m_lastResult = m_ioInterfaceFile->create(filename); + if (m_lastResult != XRV_OK) + { + m_ioInterfaceFile.reset(); + return false; + } + + //m_readOnly = false; + + // check if we can actually write to the file + char testData[] = "Xsens"; + XsByteArray test((unsigned char*) testData, 5, XSDF_None); + + m_lastResult = m_ioInterfaceFile->writeData(test, nullptr); + if (m_lastResult == XRV_OK) + m_lastResult = m_ioInterfaceFile->deleteData(0, 5); + if (m_lastResult != XRV_OK) + { + m_ioInterfaceFile->close(); + m_ioInterfaceFile.reset(); + } + return m_lastResult == XRV_OK; +} + +/*! \brief Closes the file +*/ +void MtbDataLogger::close() +{ + close(false); +} + +/*! \brief Closes and if requested deletes the file + \param deleteFile If set to true then deletes the file +*/ +void MtbDataLogger::close(bool deleteFile) +{ + if (m_ioInterfaceFile) + { + if (deleteFile) + m_ioInterfaceFile->closeAndDelete(); + else + m_ioInterfaceFile->close(); + //removeChainedManager(m_ioInterfaceFile); + m_ioInterfaceFile.reset(); + } +} + +/*! \brief Overloadable function to allow easier testing +*/ +bool MtbDataLogger::writeMessage(const XsMessage& message) +{ + if (!m_ioInterfaceFile) + { + m_lastResult = XRV_NOFILEOPEN; + return false; + } + + XsByteArray raw; + if (ProtocolHandler::composeMessage(raw, message) != -1) + m_lastResult = m_ioInterfaceFile->writeData(raw, nullptr); + else + m_lastResult = XRV_DATACORRUPT; + + 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 +{ + if (!m_ioInterfaceFile) + return XsString(); + return m_ioInterfaceFile->getFileName(); +} diff --git a/extern/xspublic/xscontroller/mtbdatalogger.h b/extern/xspublic/xscontroller/mtbdatalogger.h new file mode 100644 index 0000000..879d340 --- /dev/null +++ b/extern/xspublic/xscontroller/mtbdatalogger.h @@ -0,0 +1,60 @@ + +// Copyright (c) 2003-2022 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 MTBDATALOGGER_H +#define MTBDATALOGGER_H + +#include "datalogger.h" +#include + +class IoInterfaceFile; + +class MtbDataLogger : public DataLogger +{ +public: + MtbDataLogger(); + ~MtbDataLogger() override; + + bool writeMessage(const XsMessage& message) override; + bool writeRaw(const XsByteArray& message); + + bool create(const XsString& filename); + void close() override; + void close(bool deleteFile); + XsString filename() const; + +private: + XsResultValue m_lastResult; + std::shared_ptr m_ioInterfaceFile; +}; + +#endif diff --git a/extern/xspublic/xscontroller/mtbfilecommunicator.cpp b/extern/xspublic/xscontroller/mtbfilecommunicator.cpp new file mode 100644 index 0000000..c7145bb --- /dev/null +++ b/extern/xspublic/xscontroller/mtbfilecommunicator.cpp @@ -0,0 +1,766 @@ + +// Copyright (c) 2003-2022 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 "mtbfilecommunicator.h" +#include "xsdevice_def.h" +#include "xsdeviceconfiguration.h" + +#include +#include +#include +#include +#include +#include "iointerfacefile.h" +#include "messageextractor.h" + +using namespace xsens; + +/*! Destructor +*/ +FileLoader::~FileLoader() +{ +} + +/*! \class MtbFileCommunicator + \brief A class that is used for the communcation with a mtb file +*/ + +/*! \brief Constructs new MtbFileCommunicator +*/ +Communicator* MtbFileCommunicator::construct() +{ + return new MtbFileCommunicator; +} + +/*! \brief Default constructor +*/ +MtbFileCommunicator::MtbFileCommunicator() + : Communicator() + , m_abortLoadLogFile(false) + , m_loadFileTaskId(0) + , m_extractor(nullptr) + , m_extractedMessages(new std::deque) +{ + m_extractor = new MessageExtractor(protocolManager()); + for (auto p : *protocolManager()) + p->ignoreMaximumMessageSize(true); +} + +/*! \brief Constructor that uses \a ioInterfaceFile +*/ +MtbFileCommunicator::MtbFileCommunicator(std::shared_ptr const& ioInterfaceFile) + : Communicator() + , m_ioInterfaceFile(ioInterfaceFile) + , m_abortLoadLogFile(false) + , m_loadFileTaskId(0) + , m_extractor(nullptr) + , m_extractedMessages(new std::deque) +{ + m_extractor = new MessageExtractor(protocolManager()); + for (auto p : *protocolManager()) + p->ignoreMaximumMessageSize(true); +} + +/*! Destructor +*/ +MtbFileCommunicator::~MtbFileCommunicator() +{ + delete m_extractor; + delete m_extractedMessages; +} + +/*! \brief A rather stupid function that tries to convert a live timeout into a number of messages +*/ +uint32_t MtbFileCommunicator::timeoutToMaxMessages(uint32_t timeout) const +{ + return timeout / 20; // 100 ms corresponds to 5 messages. Reasonable, no? +} + +/*! \brief Pretend to be a live system + \param msg The message to send + \param rcv The message to receive + \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) +{ + XsXbusMessageId expected = static_cast(msg.getMessageId() + 1); + std::deque messages = readMessagesFromStartOfFile(expected, (int) timeoutToMaxMessages(timeout)); + rcv.clear(); + for (const XsMessage& r : messages) + { + if (r.getBusId() != msg.getBusId()) + continue; + + if (msg.getMessageId() == XMID_ReqFrameRates && msg.getDataShort() != r.getDataShort()) // data identifier + continue; + + rcv = r; + return true; + } + return (lastResult() == XRV_OK && !rcv.empty()); +} + +/*! \brief Prepares for the destruction +*/ +void MtbFileCommunicator::prepareForDestruction() +{ + abortLoadLogFile(); + waitForLastTaskCompletion(); + + m_abortLoadLogFile = true; + completeAllThreadedWork(); + + Communicator::prepareForDestruction(); +} + +/*! \brief Completes all threaded work +*/ +void MtbFileCommunicator::completeAllThreadedWork() +{ + ThreadPool::instance()->waitForCompletion(m_loadFileTaskId); +} + +/*! \brief Wait for the last processing task to complete in the threadpool + \details This function is usually called after abort() to make sure that no more processing + is going on. +*/ +void MtbFileCommunicator::waitForLastTaskCompletion() +{ + completeAllThreadedWork(); +} + +/* \brief Closes the log file +*/ +void MtbFileCommunicator::closeLogFile() +{ + m_ioInterfaceFile.reset(); +} + +/*! \brief Read a message from the start of the open file + \details This function will reset the read position in the file to the start and will + then search for the message with the given message ID. After the message has been found (or not) + the read position will be restored to its original position. + \param msgId The ID of the message to search for + \param maxMsgs Optional parameter to limit the maximum number of messages to search. When 0, the + function will continue until the message has been found or the end of the file has been reached. + \returns The messsage that was read +*/ +XsMessage MtbFileCommunicator::readMessageFromStartOfFile(uint8_t msgId, int maxMsgs) +{ + if (!m_ioInterfaceFile) + { + setLastResult(XRV_INVALIDOPERATION); + return XsMessage(); + } + + // store current read state and make sure we reset to it when we're done + auto oldPos = logFileReadPosition(); + MessageExtractor* ex = m_extractor; + auto exm = m_extractedMessages; + m_extractor = new MessageExtractor(protocolManager()); + m_extractedMessages = new std::deque; + JanitorStdFunc0<> resetExtractor([this, ex, exm, oldPos]() + { + delete this->m_extractor; + this->m_extractor = ex; + this->m_extractedMessages = exm; + this->m_ioInterfaceFile->setReadPosition(oldPos); + }); + + // start reading from start of file + m_ioInterfaceFile->setReadPosition(0); + + if (maxMsgs == 0) + return readMessage(msgId); + + for (int count = 0; count < maxMsgs; ++count) + { + XsMessage msg = readMessage(); + if (msgId == 0 || msg.getMessageId() == msgId) + return msg; + if (m_ioInterfaceFile->getLastResult() != XRV_OK) + { + setLastResult(m_ioInterfaceFile->getLastResult()); + return XsMessage(); + } + } + + setLastResult(XRV_OTHER); + return XsMessage(); +} + +/*! \brief Read multiple similar messages from the start of the open file + \details This function will reset the read position in the file to the start and will + then search for all messages with the given message ID. Afterwards the read position will + be restored to its original position. + \param msgId The ID of the message to search for. + \param maxMsgs Optional parameter to limit the maximum number of messages to search. When 0, the + function will continue until the end of the file has been reached. + \returns The messsage that was read +*/ +std::deque MtbFileCommunicator::readMessagesFromStartOfFile(uint8_t msgId, int maxMsgs) +{ + std::deque rv; + + if (!m_ioInterfaceFile) + { + setLastResult(XRV_INVALIDOPERATION); + return rv; + } + + // store current read state and make sure we reset to it when we're done + auto oldPos = logFileReadPosition(); + MessageExtractor* ex = m_extractor; + auto exm = m_extractedMessages; + m_extractor = new MessageExtractor(protocolManager()); + m_extractedMessages = new std::deque; + JanitorStdFunc0<> resetExtractor([this, ex, exm, oldPos]() + { + delete this->m_extractedMessages; + delete this->m_extractor; + this->m_extractor = ex; + this->m_extractedMessages = exm; + this->m_ioInterfaceFile->setReadPosition(oldPos); + }); + + // start reading from start of file + m_ioInterfaceFile->setReadPosition(0); + + if (maxMsgs) + { + for (int count = 0; count < maxMsgs; ++count) + { + XsMessage msg = readMessage(); + if (lastResult() != XRV_OK) + break; + else if (msgId == 0 || msg.getMessageId() == msgId) + rv.push_back(msg); + } + } + else + { + XsMessage msg = readMessage(msgId); + while (lastResult() == XRV_OK) + { + if (msgId == 0 || msg.getMessageId() == msgId) + rv.push_back(msg); + msg = readMessage(msgId); + } + } + + if (lastResult() == XRV_OTHER || (lastResult() == XRV_ENDOFFILE && m_ioInterfaceFile->getFileSize())) + setLastResult(XRV_OK); + if (lastResult() == XRV_ENDOFFILE) + setLastResult(XRV_NODATA); + + return rv; +} + +/*! \class Xs4FileTask + \brief A class for handling file loading process in a separate thread + \details This task will delegate the loading of the file to yet another thread, keeping the task in the + threadpool as a monitoring task. This is because ThreadPoolTask tasks should be small tasks and file loading + can take a while. +*/ +class Xs4FileTask : public ThreadPoolTask +{ +public: + + /*! \brief Construct a file task, but does not schedule it. */ + Xs4FileTask(FileLoader* inf, XsDevice* device) + : ThreadPoolTask() + , m_loader(inf) + , m_device(device) + , m_thread(this) + { + assert(m_loader != 0); + assert(m_device != 0); + } + + /*! \brief Check if the file load is complete + */ + bool exec() override + { + // short sleep to prevent thread starvation + XsTime::msleep(1); + + if (m_thread.m_done) + return true; + + if (!m_thread.isRunning()) + { + JLDEBUGG("Starting dedicated read thread"); + m_thread.startThread(); + } + + // reschedule + return false; + } + + /*! \brief Destroy this process task. */ + virtual ~Xs4FileTask() + { + if (m_thread.isAlive()) + m_thread.stopThread(); + } + +private: + FileLoader* m_loader; //!< The Communicator object that contains information on calibration and filtering + XsDevice* m_device; + + class ReaderThread : public StandardThread + { + public: + ReaderThread(Xs4FileTask* task); + int32_t innerFunction(void) override; + Xs4FileTask* m_task; + bool m_done; + } m_thread; + + friend class ReaderThread; +}; + +Xs4FileTask::ReaderThread::ReaderThread(Xs4FileTask* task) + : m_task(task) + , m_done(false) +{ +} + +int32_t Xs4FileTask::ReaderThread::innerFunction(void) +{ +#ifndef ANDROID // Invoking XsDevice::logFileName() results in a crash on Android for some reason... + { + XsString filename = m_task->m_device->logFileName(); + std::string name = xprintf("FileReader: %s", filename.c_str()); + xsNameThisThread(name.c_str()); + JLDEBUGG("Loading file " << filename); + } +#endif + + try + { + m_task->m_loader->readLogFile(m_task->m_device); + } + catch (XsException const& e) + { + m_task->m_device->onError(m_task->m_device, e.code()); + JLALERTG(e.what()); + } + m_done = true; + stopThread(); + return 0; +} + +/*! \brief Load a log file with thread pool +*/ +void MtbFileCommunicator::loadLogFile(XsDevice* device) +{ + assert(device != 0); + + abortLoadLogFile(); + waitForLastTaskCompletion(); + + m_abortLoadLogFile = false; + Xs4FileTask* tsk = new Xs4FileTask(this, device); + m_loadFileTaskId = ThreadPool::instance()->addTask(tsk, m_loadFileTaskId); +} + +/*! \brief Read a log file into cache + \param device : The device to read log from + \details Read all data for \a inf into the cache + \sa XsControl::loadLogFile + \returns XRV_OK if successful +*/ +XsResultValue MtbFileCommunicator::readLogFile(XsDevice* device) +{ + assert(device != 0); + JLDEBUGG(""); + + XsResultValue res = XRV_OK; + XsFilePos prevFilePos = -1; + XsString id = logFileName(); + + do + { + try + { + res = readSinglePacketFromFile(); + } + catch (...) + { + // Simply ignore the error and continue. + continue; + } + // Only emit progress updates at complete percentages, once per percentage + XsFilePos pos = logFileReadPosition(); + XsFilePos size = logFileSize(); + + if (size) + { + const int percentage = (int)(pos * 100 / size); + if (prevFilePos != percentage && percentage < 100) + { + onProgressUpdated(device, percentage < 100 ? percentage : 99, 100, &id); // never report 100% until we're really done + prevFilePos = percentage; + } + } + } while (!m_abortLoadLogFile && (res == XRV_OK || res == XRV_OTHER)); + + if (!m_abortLoadLogFile) + { + masterDevice()->onEofReached(); + onProgressUpdated(device, 100, 100, &id); + } + + return setAndReturnLastResult(res); +} + +/*! \brief Read a single XsDataPacket from an open log file + \details Read a single XsDataPacket from the log file and place it in the correct data cache(s) + \returns XRV_OK if successful +*/ +XsResultValue MtbFileCommunicator::readSinglePacketFromFile() +{ + JLTRACEG("Reading from file"); + XsMessage msg = readMessage(0); + if (lastResult()) + return lastResult(); + + handleMessage(msg); + return lastResult(); +} + +/*! \brief Abort a process that takes a long time to complete + \details This currently only includes readLogFile() +*/ +void MtbFileCommunicator::abortLoadLogFile() +{ + m_abortLoadLogFile = true; +} + +/*! \brief Open a log file for input. + \details This function opens the supplied log file for reading. The function will fail if a serial connection is currently open. + \param filename The name of the file to open. It is recommended to use a fully qualified path+filename. + \note This function is only available in configuration mode. + \see closeLogFile + \returns True if successful +*/ +bool MtbFileCommunicator::openLogFile(const XsString& filename) +{ + if (m_ioInterfaceFile) + { + setLastResult(XRV_ALREADYOPEN); + if (filename == logFileName()) + return true; + return false; + } + m_ioInterfaceFile = std::shared_ptr(new IoInterfaceFile, [](IoInterfaceFile * f) + { + f->close(); + }); + + setLastResult(m_ioInterfaceFile->open(filename, false, true)); + if (lastResult() != XRV_OK) + { + m_ioInterfaceFile.reset(); + return false; + } + + // start with a sanity check on the file + XsByteArray hdrBuf; + XsResultValue rv; + + // Is the file readable and does it at least start with 0xFA marker? + rv = m_ioInterfaceFile->readData(1, hdrBuf); + if (rv != XRV_OK || hdrBuf.size() < 1 || hdrBuf[0] != 0xFA) + { + setLastResult(XRV_DATACORRUPT); + m_ioInterfaceFile.reset(); + return false; + } + + resetLogFileReadPosition(); + + // try to read any message from the start + XsMessage check = readMessage(); + if (check.getMessageId() == XMID_InvalidMessage) + { + setLastResult(XRV_DATACORRUPT); + m_ioInterfaceFile.reset(); + return false; + } + + resetLogFileReadPosition(); + + // Now let's try to find the actual message we are looking for somewhere... + XsMessage rcv = readMessageFromStartOfFile(XMID_Configuration, Communicator::configurationMessageSearchLimit()); + + if (rcv.getMessageId() != XMID_Configuration) + { + setLastResult(XRV_DATACORRUPT); + m_ioInterfaceFile.reset(); + return false; + } + + XsDeviceConfiguration config; + config.readFromMessage(rcv); + setMasterDeviceId(XsDeviceId((char*)config.masterInfo().m_productCode, 0, 0, config.masterInfo().m_masterDeviceId)); + + return true; +} + +/*! \brief Retrieve the name of the open log file or an empty string if no log file is open +*/ +XsString MtbFileCommunicator::logFileName() const +{ + XsString retString; + if (!m_ioInterfaceFile) + return retString; + + if (m_ioInterfaceFile->getName(retString) != XRV_OK) + return XsString(); + return retString; +} + +/*! \brief Retrieve the size of the open log file in bytes +*/ +XsFilePos MtbFileCommunicator::logFileSize() const +{ + if (!m_ioInterfaceFile) + return 0; + + return m_ioInterfaceFile->getFileSize(); +} + +/*! \brief Retrieve the date of the open log file +*/ +XsTimeStamp MtbFileCommunicator::logFileDate() const +{ + if (!m_ioInterfaceFile) + return XsTimeStamp(); + + return m_ioInterfaceFile->getFileDate(); +} + +/*! \brief Retrieve the read position of the log file. + \details This function will return the current read position in the open log file in bytes from the start. + \note The read and write positions of log files are completely independent of each other. + \note There is a look-ahead cache in place so even when the read position is already at the end of the file, there may still be some messages left to be read. + For this reason, this function will at most return filesize-1 until the message queue is empty. + \remarks To reset the read position, use resetLogFileReadPosition. + \see resetLogFileReadPosition + \returns The file read position +*/ +XsFilePos MtbFileCommunicator::logFileReadPosition() const +{ + if (!m_ioInterfaceFile) + return 0; + + auto pos = m_ioInterfaceFile->getReadPosition(); + if (pos < m_ioInterfaceFile->getFileSize()) + return pos; + + if (m_extractedMessages->empty()) + return pos; + + return pos > 0 ? pos - 1 : 0; +} + +/*! \brief Restart reading from the start of the open log file. + \details This function resets the read position to the start of the open log file. Only the read position is affected, the write position remains the same. + \see openLogFile +*/ +void MtbFileCommunicator::resetLogFileReadPosition(void) +{ + if (m_ioInterfaceFile) + { + m_extractor->clearBuffer(); + m_extractedMessages->clear(); + setLastResult(m_ioInterfaceFile->setReadPosition(0)); + } + else + setLastResult(XRV_NOFILEOPEN); +} + +/*! \brief Return whether we are reading from file */ +bool MtbFileCommunicator::isReadingFromFile() const +{ + return true; +} + +/*! \brief Read a message from the open file + \details This function will attempt to read a full message from the open device (file or COM port + or USB port). If msgId is non-0, the function will look for a specific message ID. + The function will read from the device, but it won't wait for data to become available. + \param msgId Either 0 to read the first available message or non-0 to look for a specific message + with this ID. + \returns The message that was read or if no matching message was found a cleared message. +*/ +XsMessage MtbFileCommunicator::readMessage(uint8_t msgId) +{ + if (!m_ioInterfaceFile) + { + setLastResult(XRV_INVALIDOPERATION); + return XsMessage(); + } + + XsMessage msg; + + do + { + msg = readNextMessage(); + } while (!msg.empty() && msgId != 0 && msg.getMessageId() != msgId); + + if (msgId == 0 || msg.getMessageId() == msgId) + return msg; + + setLastResult(XRV_OTHER); + return XsMessage(); +} + +/*! \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). +*/ +XsMessage MtbFileCommunicator::readNextMessage() +{ + while (m_extractedMessages->empty()) + { + XsByteArray raw; + XsResultValue res = m_ioInterfaceFile->readDataBlocks(1, raw); + (void)res; + if (raw.empty()) + { + // end of file really reached + setLastResult(XRV_ENDOFFILE); + return XsMessage(); + } + m_extractor->processNewData(masterDevice(), raw, *m_extractedMessages); + } + + setLastResult(XRV_OK); + XsMessage msg = *m_extractedMessages->begin(); + m_extractedMessages->pop_front(); + return msg; +} + +/*! \returns True if load log file is in progress +*/ +bool MtbFileCommunicator::isLoadLogFileInProgress() const +{ + return ThreadPool::instance()->doesTaskExist(m_loadFileTaskId); +} + +/*! \brief Add the protocol handler + \param handler : The protocol hanlder to add +*/ +void MtbFileCommunicator::addProtocolHandler(IProtocolHandler* handler) +{ + handler->ignoreMaximumMessageSize(true); + Communicator::addProtocolHandler(handler); +} + +/* Now the disabled stuff starts */ + +//! \cond DO_NOT_DOCUMENT +XsResultValue MtbFileCommunicator::gotoConfig(bool detectRs485) +{ + (void)detectRs485; + return XRV_UNSUPPORTED; +} + +XsResultValue MtbFileCommunicator::gotoMeasurement() +{ + return XRV_UNSUPPORTED; +} + +XsResultValue MtbFileCommunicator::getDeviceId() +{ + return XRV_UNSUPPORTED; +} + +void MtbFileCommunicator::setGotoConfigTimeout(uint32_t timeout) +{ + (void)timeout; +} + +bool MtbFileCommunicator::writeMessage(const XsMessage& message) +{ + (void)message; + return false; +} + +void MtbFileCommunicator::flushPort() +{ +} + +void MtbFileCommunicator::closePort() +{ +} + +bool MtbFileCommunicator::isPortOpen() const +{ + return false; +} + +XsPortInfo MtbFileCommunicator::portInfo() const +{ + return XsPortInfo(); +} + +bool MtbFileCommunicator::openPort(const XsPortInfo& portInfo, OpenPortStage stage, bool detectRs485) +{ + (void)portInfo; + (void)stage; + (void)detectRs485; + return false; +} + +bool MtbFileCommunicator::reopenPort(OpenPortStage stage, bool skipDeviceIdCheck) +{ + (void)stage; + (void)skipDeviceIdCheck; + return false; +} + +bool MtbFileCommunicator::isDockedAt(Communicator* other) const +{ + (void)other; + return false; +} + +void MtbFileCommunicator::setKeepAlive(bool enable) +{ + (void)enable; +} +//! \endcond diff --git a/extern/xspublic/xscontroller/mtbfilecommunicator.h b/extern/xspublic/xscontroller/mtbfilecommunicator.h new file mode 100644 index 0000000..c61c18c --- /dev/null +++ b/extern/xspublic/xscontroller/mtbfilecommunicator.h @@ -0,0 +1,109 @@ + +// Copyright (c) 2003-2022 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 MTBFILECOMMUNICATOR_H +#define MTBFILECOMMUNICATOR_H + +#include "communicator.h" +#include "fileloader.h" + +#include +#include + +#include + +class IoInterfaceFile; +class MessageExtractor; + +class MtbFileCommunicator : public Communicator, protected FileLoader +{ +public: + static Communicator* construct(); + MtbFileCommunicator(); + + void closeLogFile() override; + XsMessage readMessage(uint8_t msgId = 0) override; + XsMessage readMessageFromStartOfFile(uint8_t msgId, int maxMsgs = 0) override; + std::deque readMessagesFromStartOfFile(uint8_t msgId, int maxMsgs = 0) override; + void loadLogFile(XsDevice* device) override; + void abortLoadLogFile() override; + bool openLogFile(const XsString& filename) override; + XsString logFileName() const override; + XsFilePos logFileSize() const override; + XsTimeStamp logFileDate() const override; + XsFilePos logFileReadPosition() const override; + void resetLogFileReadPosition(void) override; + bool isReadingFromFile() const override; + bool isLoadLogFileInProgress() const 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; + 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 reopenPort(OpenPortStage stage = OPS_Full, bool skipDeviceIdCheck = false) override; + bool isDockedAt(Communicator* other) const override; + void setKeepAlive(bool enable) override; + void addProtocolHandler(IProtocolHandler* handler) override; + +protected: + MtbFileCommunicator(std::shared_ptr const& ioInterfaceFile); + ~MtbFileCommunicator(); + void prepareForDestruction() override; + + virtual XsResultValue readLogFile(XsDevice* device) override; + virtual XsResultValue readSinglePacketFromFile() override; + + void waitForLastTaskCompletion() override; + + virtual XsMessage readNextMessage(); + +private: + uint32_t timeoutToMaxMessages(uint32_t timeout) const; + void completeAllThreadedWork(); + + std::shared_ptr m_ioInterfaceFile; + bool m_abortLoadLogFile; + xsens::ThreadPool::TaskId m_loadFileTaskId; + + MessageExtractor* m_extractor; + std::deque* m_extractedMessages; +}; + +#endif diff --git a/extern/xspublic/xscontroller/mtdevice.cpp b/extern/xspublic/xscontroller/mtdevice.cpp new file mode 100644 index 0000000..651a328 --- /dev/null +++ b/extern/xspublic/xscontroller/mtdevice.cpp @@ -0,0 +1,885 @@ + +// Copyright (c) 2003-2022 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 "mtdevice.h" +#include "xsdef.h" +#include +#include +#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) + : XsDeviceEx(comm) +{ +} + +/*! \brief Constructs a standalone MtDevice based on \a master and \a childDeviceId +*/ +MtDevice::MtDevice(XsDevice* master, const XsDeviceId& childDeviceId) + : XsDeviceEx(master, childDeviceId) +{ +} + +/*! \brief Destroys the MtDevice +*/ +MtDevice::~MtDevice() +{ +} + +/*! \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 +{ + return msg.getBusId() == 1 || XsDevice::messageLooksSane(msg); +} + +/*! \brief Initialize the Mt device using the supplied filter profiles + \returns True if successful +*/ +bool MtDevice::initialize() +{ + if (!XsDeviceEx::initialize()) + return false; + + // we must create the data caches first so they are available even if the rest of the init fails + // when reading from file almost all init can fail but we can still read data into the caches + if (!readDeviceConfiguration()) + { + setInitialized(false); + return false; + } + + fetchAvailableHardwareScenarios(); + MtDevice::updateFilterProfiles(); + + return true; +} + +/*! \brief Updates the scenarios +*/ +void MtDevice::updateFilterProfiles() +{ + const XsMtDeviceConfiguration& info = deviceConfigurationConst().deviceInfo(deviceId()); + 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); + } + + for (auto i = m_hardwareFilterProfiles.begin(); i != m_hardwareFilterProfiles.end(); ++i) + { + if (i->type() == m_hardwareFilterProfile.type() || i->label() == m_hardwareFilterProfile.label()) + { + m_hardwareFilterProfile.setLabel(i->label()); + m_hardwareFilterProfile.setKind(i->kind()); + m_hardwareFilterProfile.setVersion(i->version()); + break; + } + } +} + +/*! \returns True if this is a motion tracker +*/ +bool MtDevice::isMotionTracker() const +{ + return true; +} + +/*! \copybrief XsDevice::updateRateForDataIdentifier +*/ +int MtDevice::updateRateForDataIdentifier(XsDataIdentifier dataType) const +{ + return XsDevice::updateRateForDataIdentifier(dataType); +} + +/*! \copybrief XsDevice::stringOutputType +*/ +uint16_t MtDevice::stringOutputType() const +{ + XsMessage snd(XMID_ReqStringOutputType), rcv; + if (!doTransaction(snd, rcv)) + return 0; + + return rcv.getDataShort(); +} + +/*! \copybrief XsDevice::stringSamplePeriod +*/ +uint16_t MtDevice::stringSamplePeriod() const +{ + XsMessage snd(XMID_ReqPeriod), rcv; + if (!doTransaction(snd, rcv)) + return 0; + + return rcv.getDataShort(); +} + +/*! \copybrief XsDevice::stringSkipFactor +*/ +uint16_t MtDevice::stringSkipFactor() const +{ + XsMessage snd(XMID_ReqOutputSkipFactor), rcv; + if (!doTransaction(snd, rcv)) + return 0; + + return rcv.getDataShort(); +} + +/*! \copybrief XsDevice::deviceOptionFlags +*/ +XsDeviceOptionFlag MtDevice::deviceOptionFlags() const +{ + XsMessage snd(XMID_ReqOptionFlags), rcv; + if (doTransaction(snd, rcv)) + return (XsDeviceOptionFlag)rcv.getDataLong(); + return XDOF_None; +} + +/*! \copybrief XsDevice::ubloxGnssPlatform +*/ +XsUbloxGnssPlatform MtDevice::ubloxGnssPlatform() const +{ + XsMessage snd(XMID_ReqGnssPlatform), rcv; + if (doTransaction(snd, rcv)) + return static_cast(rcv.getDataShort()); + return XGP_Portable; +} + +/*! \copybrief XsDevice::setUbloxGnssPlatform +*/ +bool MtDevice::setUbloxGnssPlatform(XsUbloxGnssPlatform ubloxGnssPlatform) +{ + XsMessage snd(XMID_SetGnssPlatform, 1); + snd.setBusId(busId()); + snd.setDataShort((uint16_t)ubloxGnssPlatform); + if (!doTransaction(snd)) + return false; + return true; +} + +/*! \copydoc XsDevice::gnssReceiverSettings +*/ +XsIntArray MtDevice::gnssReceiverSettings() const +{ + XsMessage snd(XMID_ReqGnssReceiverSettings), rcv; + snd.setBusId(busId()); + if (doTransaction(snd, rcv)) + { + XsIntArray gnssReceiverSettings = XsIntArray(4); + gnssReceiverSettings[0] = (int)rcv.getDataShort(0);//receiver type + gnssReceiverSettings[1] = (int)rcv.getDataShort(2);//receiver baud rate + gnssReceiverSettings[2] = (int)rcv.getDataShort(4);//receiver input rate + gnssReceiverSettings[3] = (int)rcv.getDataLong(6);//receiver options + return gnssReceiverSettings; + } + return XsIntArray(); +} + +/*! \copydoc XsDevice::setGnssReceiverSettings +*/ +bool MtDevice::setGnssReceiverSettings(const XsIntArray& gnssReceiverSettings) +{ + XsMessage snd(XMID_SetGnssReceiverSettings, 10); + snd.setBusId(busId()); + + if (gnssReceiverSettings.size() > 0) + snd.setDataShort((uint16_t)gnssReceiverSettings[0], 0);//receiver type + if (gnssReceiverSettings.size() > 1) + snd.setDataShort((uint16_t)gnssReceiverSettings[1], 2);//receiver baud rate + if (gnssReceiverSettings.size() > 2) + snd.setDataShort((uint16_t)gnssReceiverSettings[2], 4);//receiver input rate + if (gnssReceiverSettings.size() > 3) + snd.setDataLong((uint32_t)gnssReceiverSettings[3], 6);//receiver options + + XsMessage ack; + if (!doTransaction(snd, ack)) + return false; + + return true; +} + +/*! \copybrief XsDevice::outputConfiguration +*/ +XsOutputConfigurationArray MtDevice::outputConfiguration() const +{ + return XsOutputConfigurationArray(); +} + +/*! \brief Checks if this device can do orientation reset in firmware + \param method The reset method + \returns True if successful +*/ +bool MtDevice::canDoOrientationResetInFirmware(XsResetMethod method) +{ + switch (method) + { + case XRM_DefaultAlignment: + case XRM_DefaultHeading: + case XRM_DefaultInclination: + return true; + + case XRM_None: + return false; + + default: + break; + } + + return updateRateForDataIdentifier(XDI_OrientationGroup) > 0; +} + +/*! \copybrief XsDevice::scheduleOrientationReset */ +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)) + return false; + + break; + + case XDS_Config: + if (method != XRM_StoreAlignmentMatrix) + 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; +} + +/*! \brief Store the current alignment matrix in the device. + \details The alignment matrix is computed when doing an orientation reset and needs to be stored + explicitly in the device or it will be forgotten when the device restarts. This function will tell + the device to store its alignment matrix or to write the locally computed alignment matrix to the + device when filtering is done on the PC side. + \returns true if the alignment matrix was successfully written to the non-volatile memory of the + device +*/ +bool MtDevice::storeAlignmentMatrix() +{ + if (!XsDevice::scheduleOrientationReset(XRM_StoreAlignmentMatrix)) + return false; + + return true; +} + +/*! \brief The heading offset set for this device +*/ +double MtDevice::headingOffset() const +{ + XsMessage snd(XMID_ReqHeading), rcv; + snd.setBusId(busId()); + + if (!doTransaction(snd, rcv)) + return 0; + + return (double)rcv.getDataFloat(); +} + +/*! \copybrief XsDevice::setLocationId +*/ +bool MtDevice::setLocationId(int id) +{ + XsMessage snd(XMID_SetLocationId, XS_LEN_LOCATIONID); + snd.setBusId(busId()); + snd.setDataShort((uint16_t)id); + + return doTransaction(snd); +} + +/*! \copybrief XsDevice::locationId +*/ +int MtDevice::locationId() const +{ + XsMessage snd(XMID_ReqLocationId), rcv; + snd.setBusId(busId()); + + if (!doTransaction(snd, rcv)) + return 0; + + return rcv.getDataShort(); +} + +/*! \copybrief XsDevice::serialBaudRate +*/ +XsBaudRate MtDevice::serialBaudRate() const +{ + XsMessage snd(XMID_ReqBaudrate), rcv; + snd.setBusId(busId()); + + if (!doTransaction(snd, rcv)) + return XBR_Invalid; + + return (XsBaudRate)rcv.getDataByte(); +} + +/*! \copybrief XsDevice::hardwareVersion +*/ +XsVersion MtDevice::hardwareVersion() const +{ + XsMessage snd(XMID_ReqHardwareVersion), rcv; + if (!doTransaction(snd, rcv)) + return XsVersion(); + uint16_t hwv = rcv.getDataShort(); + return XsVersion(hwv >> 8, hwv & 0xff); +} + +/*! \copybrief XsDevice::availableOnboardFilterProfiles +*/ +XsFilterProfileArray MtDevice::availableOnboardFilterProfiles() const +{ + return m_hardwareFilterProfiles; +} + +/*! \brief Request the filter profiles headers from the hardware device and returns a vector with the found profiles. + the order in the output vector is the same as the order in the hardware device. +*/ +XsFilterProfileArray MtDevice::readFilterProfilesFromDevice() const +{ + XsFilterProfileArray result; + + XsMessage snd(XMID_ReqAvailableFilterProfiles); + snd.setBusId(busId()); + + XsMessage rcv; + if (!doTransaction(snd, rcv)) + return result; + + const char filterType = deviceConfigurationConst().deviceInfo(deviceId()).m_filterType; + + XsSize nofScenarios = rcv.getDataSize() / (1 + 1 + XS_LEN_FILTERPROFILELABEL); + + result.resize(nofScenarios); + for (XsSize i = 0; i < nofScenarios; ++i) + { + 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].setFilterType(filterType); + XsString kind; + if (type == XFPK_Base) + kind = "base"; + else if (type == XFPK_Additional) + kind = "additional"; + else if (type == XFPK_Heading) + kind = "heading"; + result[i].setKind(kind.c_str()); + } + return result; +} + +/*! \brief Fetches available hardware scenarios +*/ +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(); + } + ); +} + +/*! \copybrief XsDevice::productCode +*/ +XsString MtDevice::productCode() const +{ + XsMessage snd(XMID_ReqProductCode), rcv; + if (!doTransaction(snd, rcv)) + return XsString(); + + const char* pc = (const char*) rcv.getDataBuffer(); + assert(pc); + 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()); + return XsString(result); +} + +/*! \copybrief XsDevice::reinitialize +*/ +bool MtDevice::reinitialize() +{ + if (!readDeviceConfiguration()) + return false; + + clearDataCache(); + fetchAvailableHardwareScenarios(); + return true; +} + +/*! \brief Restore to factory default settings +*/ +bool MtDevice::restoreFactoryDefaults() +{ + if (!XsDevice::restoreFactoryDefaults()) + return false; + + return reinitialize(); +} + +/*! \copybrief XsDevice::onboardFilterProfile +*/ +XsFilterProfile MtDevice::onboardFilterProfile() const +{ + 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 +*/ +bool MtDevice::setOnboardFilterProfile(int profileType) +{ + if (deviceState() != XDS_Config) + return false; + + XsFilterProfileArray::iterator item = std::find_if(m_hardwareFilterProfiles.begin(), m_hardwareFilterProfiles.end(), + [profileType](XsFilterProfile const & p) + { + return p.type() == profileType; + }); + if (item == m_hardwareFilterProfiles.end()) + return false; + + XsMessage snd(XMID_SetFilterProfile, XS_LEN_SETFILTERPROFILE); + snd.setBusId(busId()); + snd.setDataShort((uint16_t)profileType); + + if (!doTransaction(snd)) + return false; + + m_hardwareFilterProfile = *item; + return true; +} + +/*! \copybrief XsDevice::setOnboardFilterProfile +*/ +bool MtDevice::setOnboardFilterProfile(XsString const& profile) +{ + if (deviceState() != XDS_Config) + return false; + + XsStringArray profileList(profile, "/"); + + XsFilterProfileArray::iterator item[2]; + int i = 0; + for (auto currentProfile : profileList) + { + item[i++] = std::find_if(m_hardwareFilterProfiles.begin(), m_hardwareFilterProfiles.end(), + [currentProfile](XsFilterProfile const & p) + { + return currentProfile == p.label(); + }); + if (i == 2) + break; + } + if (i == 0 || item[0] == m_hardwareFilterProfiles.end()) + return false; + + XsMessage snd(XMID_SetFilterProfile, profile.size()); + snd.setBusId(busId()); + snd.setDataBuffer((const uint8_t*)profile.c_str(), profile.size()); + + if (!doTransaction(snd)) + return false; + + if (item[1] != m_hardwareFilterProfiles.end()) + { + m_hardwareFilterProfile = *item[0]; + m_hardwareFilterProfile.setLabel(profile.c_str()); // Use info from first item, but label can be 2 profiles + } + else + m_hardwareFilterProfile = *item[0]; + return true; +} + +/*! \returns The accelerometer range for this device + \details The range is an absolute maximum number. This means that if 100 is returned, the sensor range is (100, -100) +*/ +double MtDevice::accelerometerRange() const +{ + return ::accelerometerRange(productCode(), hardwareVersion().major()); +} + +/*! \returns The accelerometer range for this device + \details The range is an absolute maximum number. This means that if 300 is returned, the sensor range is (300, -300) +*/ +double MtDevice::gyroscopeRange() const +{ + return ::gyroscopeRange(productCode()); +} + +/*! \brief Write the emts of the device to the open logfile + \note The default implementation of MtDevice does not include any children +*/ +void MtDevice::writeDeviceSettingsToFile() +{ + writeMessageToLogFile(m_emtsBlob); +} + +/*! \copybrief XsDevice::setNoRotation +*/ +bool MtDevice::setNoRotation(uint16_t duration) +{ + XsMessage snd(XMID_SetNoRotation, 2); + snd.setBusId(busId()); + snd.setDataShort(duration); + + return doTransaction(snd); +} + +/*! \brief Set the current sensor position. + \details Use this function to set the current position in latitude, longitude, altitude. + \param lla The LLA vector + \returns True if successful + \sa initialPositionLLA +*/ +bool MtDevice::setInitialPositionLLA(const XsVector& lla) +{ + uint8_t bid = (uint8_t)busId(); + if (bid == XS_BID_INVALID || bid == XS_BID_BROADCAST || lla.size() != 3) + return false; + + XsMessage snd(XMID_SetLatLonAlt, XS_LEN_LATLONALT); + snd.setDataFloat((float) lla[0], 0); + snd.setDataFloat((float) lla[1], 4); + snd.setDataFloat((float) lla[2], 8); + snd.setBusId(bid); + + return doTransaction(snd); +} + +/*! \returns the current sensor position + \sa setInitialPositionLLA +*/ +XsVector MtDevice::initialPositionLLA() const +{ + XsVector vec; + XsMessage snd(XMID_ReqLatLonAlt), rcv; + if (doTransaction(snd, rcv)) + { + vec.setSize(3); + for (XsSize i = 0; i < 3; i++) + vec[i] = rcv.getDataDouble(i * 8); + } + return vec; +} + +/*! \brief Convert mt sync ticks to microseconds +*/ +uint32_t MtDevice::syncTicksToUs(uint32_t ticks) const +{ + 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)); +} + +/*! \returns the error mode of the device. + \see setErrorMode +*/ +XsErrorMode MtDevice::errorMode() const +{ + XsMessage snd(XMID_ReqErrorMode), rcv; + if (!doTransaction(snd, rcv)) + return XEM_Ignore; + return static_cast(rcv.getDataShort()); +} + +/*! \brief Set the error mode of the device + \details The error mode determines how the device handles errors. See + the low-level communication documentation for more details. + \param em The error mode + \returns True if successful + \see errorMode +*/ +bool MtDevice::setErrorMode(XsErrorMode em) +{ + XsMessage snd(XMID_SetErrorMode, 2); + snd.setBusId(busId()); + snd.setDataShort(em); + return doTransaction(snd); +} + +/*! \brief Return the RS485 acknowledge transmission delay of the device + \details The RS485 acknowledge transmission delay determines the minimal + delay to response on request messages. See the low-level communication + documentation for more details. + \returns The delay value + \see setRs485TransmissionDelay() +*/ +uint16_t MtDevice::rs485TransmissionDelay() const +{ + XsMessage snd(XMID_ReqTransmitDelay), rcv; + if (!doTransaction(snd, rcv)) + return 0; + + return rcv.getDataShort(); +} + +/*! \brief Set the RS485 acknowledge transmission delay of the device + \param delay The delay value + \returns True if successful + \see rs485TransmissionDelay() +*/ +bool MtDevice::setRs485TransmissionDelay(uint16_t delay) +{ + XsMessage snd(XMID_SetTransmitDelay, 2); + snd.setBusId(busId()); + snd.setDataShort(delay); + + return doTransaction(snd); +} + +/*! \brief Request data from the motion tracker + \details The reply is handled by the live data path. + This functionality is only available if outputSkipFactor() is set to 0xffff. + \returns True if successful +*/ +bool MtDevice::requestData() +{ + XsMessage snd(XMID_ReqData); + snd.setBusId(busId()); + + return sendRawMessage(snd); +} + +/*! \brief Run a self test + \returns The self test result +*/ +XsSelfTestResult MtDevice::runSelfTest() +{ + XsMessage snd(XMID_RunSelfTest, 0); + snd.setBusId(busId()); + XsMessage rcv; + if (!doTransaction(snd, rcv, 3000)) + return XsSelfTestResult(); + + return XsSelfTestResult::create(rcv.getDataShort()); +} + +/*! \copybrief XsDevice::storeFilterState +*/ +bool MtDevice::storeFilterState() +{ + if (deviceState() == XDS_Config) + { + XsMessage snd(XMID_StoreFilterState); + snd.setBusId(busId()); + + if (doTransaction(snd)) + return true; + } + return false; +} + +/*! \brief Calculates the frequency + \param baseFrequency The base frequency to calculate with + \param skipFactor The skip factor to calculate with + \returns The calculated frequency +*/ +int MtDevice::calcFrequency(int baseFrequency, uint16_t skipFactor) +{ + int result = baseFrequency / (skipFactor + 1); + return result; +} + +///@{ \name Log files +/*! + \brief Set the read position of the open log file to the start of the file. + + If software filtering is enabled, the appropriate filters will be restarted + as if the file was just opened. This function sets the lastResult value + to indicate success (XRV_OK) or failure. + + \return True if successful + \sa lastResult() + \note This is a low-level file operation. + \internal +*/ +bool MtDevice::resetLogFileReadPosition() +{ + JLDEBUGG(""); + + if (!XsDevice::resetLogFileReadPosition()) + return false; + + return true; +} + +///@} end Log files + +uint32_t MtDevice::supportedStatusFlags() const +{ + // essentially an unknown device, assume everything is supported + return ~(uint32_t)0; +} + +/*! \brief Helper function to strip the hardware type from the product code + \param code A productcode to be stripped + \returns Code without the hardware type postfix +*/ +XsString MtDevice::stripProductCode(const XsString& code) +{ + XsString hwtype = findHardwareType(code); + if (hwtype.empty()) + return code; + + ptrdiff_t offset = code.findSubStr(hwtype); + while (offset >= 0 && code[(unsigned int)offset] != '-') + --offset; + + if (offset < 0) + return 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/extern/xspublic/xscontroller/mtdevice.h b/extern/xspublic/xscontroller/mtdevice.h new file mode 100644 index 0000000..534ca50 --- /dev/null +++ b/extern/xspublic/xscontroller/mtdevice.h @@ -0,0 +1,171 @@ + +// Copyright (c) 2003-2022 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 MTDEVICE_H +#define MTDEVICE_H + +#include "xsdevice_def.h" +#include +#include +#include + +struct XsFilterProfile; + +namespace xsens +{ +class Emts5Public; +} + +/*! \class MtDevice + \brief MT device base class +*/ + +class MtDevice : public XsDeviceEx +{ +public: + 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; + uint16_t stringSkipFactor() const override; + + XsDeviceOptionFlag deviceOptionFlags() const override; + + XsUbloxGnssPlatform ubloxGnssPlatform() const override; + bool setUbloxGnssPlatform(XsUbloxGnssPlatform ubloxGnssPlatform) override; + + XsIntArray gnssReceiverSettings() const override; + bool setGnssReceiverSettings(const XsIntArray& gnssReceiverSettings) override; + + XsOutputConfigurationArray outputConfiguration() const override; + + double headingOffset() const; + + virtual bool canDoOrientationResetInFirmware(XsResetMethod method); + virtual bool scheduleOrientationReset(XsResetMethod method); + virtual bool storeAlignmentMatrix(); + + virtual bool setLocationId(int id); + int locationId() const; + + XsString productCode() const; + + XsBaudRate serialBaudRate() const override; + + bool reinitialize(); + + XsFilterProfile onboardFilterProfile() const override; + bool setOnboardFilterProfile(int profileType) override; + bool setOnboardFilterProfile(XsString const& profileType) override; + + XsVersion hardwareVersion() const; + + XsFilterProfileArray availableOnboardFilterProfiles() const override; + + bool resetLogFileReadPosition() override; + + bool restoreFactoryDefaults(); + + double accelerometerRange() const; + double gyroscopeRange() const; + + void writeDeviceSettingsToFile() override; + + bool setNoRotation(uint16_t duration); + + XsVector initialPositionLLA() const override; + bool setInitialPositionLLA(const XsVector& lla) override; + + XsErrorMode errorMode() const; + bool setErrorMode(XsErrorMode errorMode); + + uint16_t rs485TransmissionDelay() const; + bool setRs485TransmissionDelay(uint16_t delay); + + XsSelfTestResult runSelfTest(); + + bool requestData(); + bool storeFilterState() override; + + static int calcFrequency(int baseFrequency, uint16_t skipFactor); + + bool messageLooksSane(const XsMessage& msg) const override; + uint32_t supportedStatusFlags() const override; + +protected: + explicit MtDevice(XsDeviceId const& id); + explicit MtDevice(Communicator* comm); + explicit MtDevice(XsDevice*, const XsDeviceId&); + + virtual void updateFilterProfiles(); + + XsFilterProfileArray readFilterProfilesFromDevice() const; + virtual void fetchAvailableHardwareScenarios(); + + static XsString stripProductCode(const XsString& code); + + uint32_t syncTicksToUs(uint32_t ticks) const; + uint32_t usToSyncTicks(uint32_t us) const; + + //! A vector of hardware filter profiles + XsFilterProfileArray m_hardwareFilterProfiles; + + //! \brief A hardware filter profile + XsFilterProfile m_hardwareFilterProfile; +}; + +#ifndef XDA_PRIVATE_BUILD + +/*! \class MtDeviceEx + \brief An abstract struct of MT device +*/ +struct MtDeviceEx : public MtDevice +{ +protected: + + //! Construct a device using \a comm for communication + 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) {} +}; +#else +#include "mtdeviceex.h" +#endif + +#endif diff --git a/extern/xspublic/xscontroller/mti3x0device.cpp b/extern/xspublic/xscontroller/mti3x0device.cpp new file mode 100644 index 0000000..51dc068 --- /dev/null +++ b/extern/xspublic/xscontroller/mti3x0device.cpp @@ -0,0 +1,164 @@ + +// Copyright (c) 2003-2022 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 "mti3x0device.h" + +#include +#include "replyobject.h" +#include "communicator.h" +#include "scenariomatchpred.h" +#include + +#include +#include "synclinegmt.h" +#include + +using namespace xsens; + +/*! \brief Constructs a device + \param comm The communicator to construct with +*/ +Mti3X0Device::Mti3X0Device(Communicator* comm) + : MtiBaseDeviceEx(comm) +{ + if (comm) + comm->setDefaultTimeout(2000); //Increase the default timeout for MTi-3X0 devices because a settings write can occasionally take ~900ms +} + +/*! \brief Destroys a device +*/ +Mti3X0Device::~Mti3X0Device() +{ +} + +/*! \brief Returns the base update rate (Hz) corresponding to the dataType. Returns 0 if no update rate is available +*/ +MtiBaseDevice::BaseFrequencyResult Mti3X0Device::getBaseFrequencyInternal(XsDataIdentifier dataType) const +{ + MtiBaseDevice::BaseFrequencyResult result; + result.m_frequency = 0; + result.m_divedable = true; + + if ((dataType == XDI_FreeAcceleration && deviceId().isImu()) || + ((dataType & XDI_FullTypeMask) == XDI_LocationId) || + ((dataType & XDI_FullTypeMask) == XDI_DeviceId)) + return result; + + if ((dataType & XDI_FullTypeMask) == XDI_AccelerationHR || (dataType & XDI_FullTypeMask) == XDI_RateOfTurnHR) + { + result.m_frequency = hardwareVersion().major() == 2 ? 1000 : 800; + result.m_divedable = true; + + return result; + } + + auto baseFreq = [this](XsDataIdentifier dataType) + { + 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; + } + }; + result.m_frequency = baseFreq(dataType); + + if ((dataType & XDI_TypeMask) == XDI_TimestampGroup) + result.m_divedable = false; + + return result; +} + +/*! \returns True if this device has an ICC support +*/ +bool Mti3X0Device::hasIccSupport() const +{ + return true; +} + +uint32_t Mti3X0Device::supportedStatusFlags() const +{ + 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 + ); +} + +/*! \copybrief XsDevice::shortProductCode +*/ +XsString Mti3X0Device::shortProductCode() const +{ + XsString code = productCode(); + return stripProductCode(code); +} diff --git a/extern/xspublic/xscontroller/mti3x0device.h b/extern/xspublic/xscontroller/mti3x0device.h new file mode 100644 index 0000000..58c45c3 --- /dev/null +++ b/extern/xspublic/xscontroller/mti3x0device.h @@ -0,0 +1,84 @@ + +// Copyright (c) 2003-2022 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 XSMTI3X0DEVICE_H +#define XSMTI3X0DEVICE_H + +#include "mtibasedevice.h" + +/*! \class Mti3X0Device + \brief The MTi device used for the X-series +*/ +class Mti3X0Device : 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 Mti3X0Device(comm); + } + + explicit Mti3X0Device(Communicator* comm); + + //! \brief An empty constructor for a master device + explicit Mti3X0Device(XsDevice* master) : MtiBaseDeviceEx(master) {} + virtual ~Mti3X0Device(); + + uint32_t supportedStatusFlags() const override; + XsString shortProductCode() const override; + +protected: + bool hasIccSupport() const override; + + MtiBaseDevice::BaseFrequencyResult getBaseFrequencyInternal(XsDataIdentifier dataType = XDI_None) const override; +}; + +#ifndef XDA_PRIVATE_BUILD +/*! \class Mti3X0DeviceEx + \brief The internal base class for MTi-3X0 series devices +*/ +struct Mti3X0DeviceEx : public Mti3X0Device +{ + //! \copybrief Mti3X0Device::Mti3X0Device + explicit Mti3X0DeviceEx(Communicator* comm) : Mti3X0Device(comm) {}; + + //! \copybrief Mti3X0Device::Mti3X0Device + explicit Mti3X0DeviceEx(XsDevice* master) : Mti3X0Device(master) {}; +}; +#else +#include "mti3x0deviceex.h" +#endif + +#endif diff --git a/extern/xspublic/xscontroller/mti6x0device.cpp b/extern/xspublic/xscontroller/mti6x0device.cpp new file mode 100644 index 0000000..96be120 --- /dev/null +++ b/extern/xspublic/xscontroller/mti6x0device.cpp @@ -0,0 +1,417 @@ + +// Copyright (c) 2003-2022 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 "mti6x0device.h" + +#include +#include "replyobject.h" +#include "communicator.h" +#include "scenariomatchpred.h" +#include "messageserializer.h" +#include + +#include +#include "synclinegmt.h" +#include +#include +#include +#include +#include "xsgnssreceivertype.h" + +using namespace xsens; + +/*! \brief Constructs a device + \param comm The communicator to construct with +*/ +Mti6X0Device::Mti6X0Device(Communicator* comm) : + MtiBaseDeviceEx(comm) +{ +} + +/*! \brief Destroys a device +*/ +Mti6X0Device::~Mti6X0Device() +{ +} + +/*! \brief Returns the base update rate (Hz) corresponding to the dataType +*/ +MtiBaseDevice::BaseFrequencyResult Mti6X0Device::getBaseFrequencyInternal(XsDataIdentifier dataType) const +{ + MtiBaseDevice::BaseFrequencyResult result; + result.m_frequency = 0; + result.m_divedable = true; + + if ((dataType == XDI_FreeAcceleration && deviceId().isImu()) || + ((dataType & XDI_FullTypeMask) == XDI_LocationId) || + ((dataType & XDI_FullTypeMask) == XDI_DeviceId) || + ((dataType & XDI_FullTypeMask) == XDI_RawGyroTemp)) + return result; + + if ((dataType & XDI_FullTypeMask) == XDI_AccelerationHR) + { + result.m_frequency = 2000; + return result; + } + + if ((dataType & XDI_FullTypeMask) == XDI_RateOfTurnHR) + { + result.m_frequency = hardwareVersion().major() == 2 ? 2000 : 1600; + return result; + } + + auto baseFreq = [this](XsDataIdentifier dataType) + { + XsDataIdentifier fullType = (dataType & XDI_FullTypeMask); + 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_RawSensorGroup: + return 200; + + case XDI_GnssGroup: + if (fullType == XDI_GnssPvtPulse) + return deviceId().isRtk() ? XDI_MAX_FREQUENCY_VAL : 0; + return deviceId().isGnss() ? XDI_MAX_FREQUENCY_VAL : 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); + + if (((dataType & XDI_TypeMask) == XDI_TimestampGroup) || ((dataType & XDI_TypeMask) == XDI_GnssGroup)) + result.m_divedable = false; + + return result; +} + +/*! \returns The sync line for a mtitx0 device This overrides the base class method. + \param setting The sync setting to get a sync line from +*/ +uint8_t Mti6X0Device::syncLine(const XsSyncSetting& setting) const +{ + SyncLineGmt gmtLine = xslToXslgmt(setting.m_line); + assert(gmtLine != XSLGMT_Invalid); + return static_cast(gmtLine); +} + +/*! \returns True if this device has an ICC support +*/ +bool Mti6X0Device::hasIccSupport() const +{ + return (firmwareVersion() >= XsVersion(1, 1, 0)); +} + + +XsStringOutputTypeArray Mti6X0Device::supportedStringOutputTypes() const +{ + XsStringOutputTypeArray outputs; + + outputs.push_back(XSOT_PSONCMS); + outputs.push_back(XSOT_HCMTW); + outputs.push_back(XSOT_HEROT); + outputs.push_back(XSOT_PTCF); + outputs.push_back(XSOT_GPZDA); + outputs.push_back(XSOT_TSS2); + outputs.push_back(XSOT_PHTRO); + outputs.push_back(XSOT_PRDID); + outputs.push_back(XSOT_EM1000); + outputs.push_back(XSOT_HEHDT); + outputs.push_back(XSOT_HCHDM); + outputs.push_back(XSOT_GPGGA); + outputs.push_back(XSOT_GPRMC); + outputs.push_back(XSOT_XSVEL); + outputs.push_back(XSOT_HCHDG); + + return outputs; +} + +/*! \brief Sets the string output mode for this device + \param type The type to set + \param frequency The frequency to set + \returns True if the device was successfully updated +*/ +bool Mti6X0Device::setStringOutputMode6x0(uint32_t type, uint16_t frequency) +{ + XsMessage sndType(XMID_SetStringOutputConfig); + sndType.setBusId(XS_BID_MASTER); // Always send to master device + sndType.setDataLong(type); + sndType.setDataShort(frequency, 4); + + if (!doTransaction(sndType)) + return false; + + return true; +} + +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) + ); +} + +/*! \copybrief XsDevice::shortProductCode +*/ +XsString Mti6X0Device::shortProductCode() const +{ + XsString code = productCode(); + return stripProductCode(code); +} + +/*! \copybrief XsDevice::canOutputConfiguration +*/ +XsCanOutputConfigurationArray Mti6X0Device::canOutputConfiguration() const +{ + XsMessage snd(XMID_ReqCanOutputConfig), rcv; + if (!doTransaction(snd, rcv)) + return XsCanOutputConfigurationArray(); + + XsCanOutputConfigurationArray config; + MessageDeserializer serializer(rcv); + serializer >> config; + + return config; +} + +/*! \copydoc XsDevice::setCanOutputConfiguration +*/ +bool Mti6X0Device::setCanOutputConfiguration(XsCanOutputConfigurationArray& config) +{ + XsMessage snd(XMID_SetCanOutputConfig, 4); + snd.setBusId(busId()); + bool wasEmpty = config.empty(); + MessageSerializer(snd) << config; + + XsMessage rcv; + if (!doTransaction(snd, rcv)) + return false; + + MessageDeserializer(rcv) >> config; + if (wasEmpty && config.size() == 1 && config[0] == XsCanOutputConfiguration(XCFF_11Bit_Identifier, XCDI_Invalid, 0, 0)) + config.clear(); + return true; +} + +/*! \copybrief XsDevice::canConfiguration +*/ +uint32_t Mti6X0Device::canConfiguration() const +{ + XsMessage snd(XMID_ReqCanConfig), rcv; + if (!doTransaction(snd, rcv)) + return 0; + + return rcv.getDataLong(); +} + +/*! \copydoc XsDevice::portConfiguration +*/ +XsIntArray Mti6X0Device::portConfiguration() const +{ + XsMessage snd(XMID_ReqPortConfig), rcv; + if (!doTransaction(snd, rcv)) + return XsIntArray(); + + XsIntArray rv; + rv.push_back((int)rcv.getDataLong(0)); + rv.push_back((int)rcv.getDataLong(4)); + rv.push_back((int)rcv.getDataLong(8)); + return rv; +} + +/*! \copydoc XsDevice::setPortConfiguration +*/ +bool Mti6X0Device::setPortConfiguration(XsIntArray& config) +{ + XsIntArray currentConfig = portConfiguration(); + + XsMessage snd(XMID_SetPortConfig); + snd.setBusId(busId()); + + if (config.size() > 0) + snd.setDataLong((uint32_t)config[0], 0); + if (config.size() > 1) + snd.setDataLong((uint32_t)config[1], 4); + if (config.size() > 2) + snd.setDataLong((uint32_t)config[2], 8); + + XsMessage rcv; + if (!doTransaction(snd, rcv)) + return false; + + if (currentConfig != config) + return reset(); + + return true; +} + +/*! \copydoc XsDevice::setCanOutputConfiguration +*/ +bool Mti6X0Device::setCanConfiguration(uint32_t config) +{ + uint32_t currentConfig = canConfiguration(); + + XsMessage snd(XMID_SetCanConfig, 4); + snd.setBusId(busId()); + snd.setDataLong(config); + + XsMessage rcv; + if (!doTransaction(snd, rcv)) + return false; + + if ((currentConfig & 0xFF) != (config & 0xFF)) + return reset(); + + return true; +} + +/*! \copydoc XsDevice::setGnssLeverArm +*/ +bool Mti6X0Device::setGnssLeverArm(const XsVector& arm) +{ + if (!deviceId().isRtk()) + return false; + + XsMessage snd(XMID_SetGnssLeverArm, 3 * sizeof(float)); + snd.setBusId(busId()); + snd.setDataFloat((float)arm[0], 0/* sizeof(float)*/); + snd.setDataFloat((float)arm[1], 1 * sizeof(float)); + snd.setDataFloat((float)arm[2], 2 * sizeof(float)); + + XsMessage rcv; + if (!doTransaction(snd, rcv)) + return false; + + return true; +} + +/*! \copydoc XsDevice::gnssLeverArm +*/ +XsVector Mti6X0Device::gnssLeverArm() const +{ + if (!deviceId().isRtk()) + return XsVector(); + + XsMessage snd(XMID_ReqGnssLeverArm), rcv; + if (!doTransaction(snd, rcv)) + return XsVector(); + + XsVector arm(3); + arm[0] = rcv.getDataFloat(0/* sizeof(float)*/); + arm[1] = rcv.getDataFloat(1 * sizeof(float)); + arm[2] = rcv.getDataFloat(2 * sizeof(float)); + return arm; +} + +/*! \copydoc XsDevice::ubloxGnssPlatform +*/ +XsUbloxGnssPlatform Mti6X0Device::ubloxGnssPlatform() const +{ + XsUbloxGnssPlatform platform = XGP_Portable; + auto gnssReceivSett = gnssReceiverSettings(); + if (gnssReceivSett.size() > 3) + { + int gnssReceiverModel = gnssReceivSett[0]; + int gnssReceiverOptions = gnssReceivSett[3]; + + //only read the value for u-blox devices + XsGnssReceiverType actualType = (XsGnssReceiverType)gnssReceiverModel; + if (actualType == XGRT_Ublox_Max_M8Q || actualType == XGRT_Ublox_Neo_M8P || actualType == XGRT_Ublox_ZED_F9P) + platform = static_cast(gnssReceiverOptions & 0xFFFF); + } + return platform; +} + +/*! \copydoc XsDevice::setUbloxGnssPlatform +*/ +bool Mti6X0Device::setUbloxGnssPlatform(XsUbloxGnssPlatform ubloxGnssPlatform) +{ + bool result = false; + auto gnssReceivSett = gnssReceiverSettings(); + if (gnssReceivSett.size() > 3) + { + int gnssReceiverModel = gnssReceivSett[0]; + int gnssReceiverOptions = gnssReceivSett[3]; + + //only set the value for u-blox devices + XsGnssReceiverType actualType = (XsGnssReceiverType)gnssReceiverModel; + if (actualType == XGRT_Ublox_Max_M8Q || actualType == XGRT_Ublox_Neo_M8P || actualType == XGRT_Ublox_ZED_F9P) + { + gnssReceivSett[3] = (int) ((((unsigned int)gnssReceiverOptions) & 0xFFFF0000U) | uint16_t(ubloxGnssPlatform)); + result = setGnssReceiverSettings(gnssReceivSett); + } + } + return result; +} diff --git a/extern/xspublic/xscontroller/mti6x0device.h b/extern/xspublic/xscontroller/mti6x0device.h new file mode 100644 index 0000000..d3e1c66 --- /dev/null +++ b/extern/xspublic/xscontroller/mti6x0device.h @@ -0,0 +1,98 @@ + +// Copyright (c) 2003-2022 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 XSMTI6X0DEVICE_H +#define XSMTI6X0DEVICE_H + +#include "mtibasedevice.h" + +/*! \class Mti6X0Device + \brief The MTi device used for the 6X0-series +*/ +class Mti6X0Device : public MtiBaseDeviceEx +{ +public: + //! \copybrief MtiXDevice::constructStandalone + static XsDevice* constructStandalone(Communicator* comm) + { + return new Mti6X0Device(comm); + } + + explicit Mti6X0Device(Communicator* comm); + + //! \brief An empty constructor for a master device + explicit Mti6X0Device(XsDevice* master) : MtiBaseDeviceEx(master) {} + virtual ~Mti6X0Device(); + + XsStringOutputTypeArray supportedStringOutputTypes() const override; + bool setStringOutputMode6x0(uint32_t type, uint16_t frequency); + uint32_t supportedStatusFlags() const override; + XsString shortProductCode() const override; + XsCanOutputConfigurationArray canOutputConfiguration() const override; + bool setCanOutputConfiguration(XsCanOutputConfigurationArray& config) override; + + XsIntArray portConfiguration() const override; + bool setPortConfiguration(XsIntArray& config) override; + + uint32_t canConfiguration() const override; + bool setCanConfiguration(uint32_t config) override; + + bool setGnssLeverArm(const XsVector& arm) override; + XsVector gnssLeverArm() const override; + + XsUbloxGnssPlatform ubloxGnssPlatform() const override; + bool setUbloxGnssPlatform(XsUbloxGnssPlatform ubloxGnssPlatform) override; + +protected: + uint8_t syncLine(const XsSyncSetting& setting) const override; + bool hasIccSupport() const override; + + MtiBaseDevice::BaseFrequencyResult getBaseFrequencyInternal(XsDataIdentifier dataType = XDI_None) const override; +}; + +#ifndef XDA_PRIVATE_BUILD +/*! \class Mti6X0DeviceEx + \brief The internal base class for MTi-6X0 series devices +*/ +struct Mti6X0DeviceEx : public Mti6X0Device +{ + //! \copybrief Mti6X0Device::Mti6X0Device + explicit Mti6X0DeviceEx(Communicator* comm) : Mti6X0Device(comm) {}; + + //! \copybrief Mti6X0Device::Mti6X0Device + explicit Mti6X0DeviceEx(XsDevice* master) : Mti6X0Device(master) {}; +}; +#else +#include "mtix00deviceex.h" +#endif + +#endif diff --git a/extern/xspublic/xscontroller/mti7_mti8device.cpp b/extern/xspublic/xscontroller/mti7_mti8device.cpp new file mode 100644 index 0000000..1d3d51f --- /dev/null +++ b/extern/xspublic/xscontroller/mti7_mti8device.cpp @@ -0,0 +1,213 @@ + +// Copyright (c) 2003-2022 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 "mti7_mti8device.h" +#include +#include + +MTi7_MTi8Device::MTi7_MTi8Device(Communicator* comm) + : MtiBaseDeviceEx(comm) +{ + if (comm) + comm->setDefaultTimeout(2000); //Increase the default timeout for MTi-1 devices because a settings write can occasionally take ~900ms +} + +MTi7_MTi8Device::MTi7_MTi8Device(XsDevice* masterdevice) + : MtiBaseDeviceEx(masterdevice) +{ +} + +MTi7_MTi8Device::~MTi7_MTi8Device() +{ +} + +namespace +{ +//! \brief Returns the default frequency of the supplied \a dataType +int baseFreq(MTi7_MTi8Device const* device, XsDataIdentifier dataType) +{ + 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_GnssGroup: + { + XsDataIdentifier fullType = (dataType & XDI_FullTypeMask); + if (fullType == XDI_GnssPvtPulse) + return device->deviceId().isRtk() ? XDI_MAX_FREQUENCY_VAL : 0; + if (fullType == XDI_GnssGroup || fullType == XDI_GnssPvtData) + return XDI_MAX_FREQUENCY_VAL; + return 0; + } + default: + return 0; + } +} +} + +/*! \brief Returns the base update rate (hz) corresponding to the dataType +*/ +MtiBaseDevice::BaseFrequencyResult MTi7_MTi8Device::getBaseFrequencyInternal(XsDataIdentifier dataType) const +{ + BaseFrequencyResult result; + result.m_frequency = 0; + result.m_divedable = true; + + if ((dataType & XDI_FullTypeMask) == XDI_LocationId || (dataType & XDI_FullTypeMask) == XDI_DeviceId) + return result; + + 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_v1 ? false : true; + + return result; + } + + result.m_frequency = baseFreq(this, dataType); + + if (((dataType & XDI_TypeMask) == XDI_TimestampGroup) || ((dataType & XDI_TypeMask) == XDI_GnssGroup)) + result.m_divedable = false; + + return result; +} + +bool MTi7_MTi8Device::hasIccSupport() const +{ + return true; +} + +uint32_t MTi7_MTi8Device::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 + | (deviceId().isRtk() ? XSF_RtkStatus : 0) + ); +} + +bool MTi7_MTi8Device::setStringOutputMode(uint16_t /*type*/, uint16_t /*period*/, uint16_t /*skipFactor*/) +{ + return true; +} + +/*! \copybrief XsDevice::shortProductCode +*/ +XsString MTi7_MTi8Device::shortProductCode() const +{ + XsString code = productCode(); + + if (hardwareVersion() >= XsVersion(2, 0, 0)) + code = stripProductCode(code); + + return code; +} + +/*! \copydoc XsDevice::setGnssLeverArm +*/ +bool MTi7_MTi8Device::setGnssLeverArm(const XsVector& arm) +{ + if (!deviceId().isRtk()) + return false; + + XsMessage snd(XMID_SetGnssLeverArm, 3 * sizeof(float)); + snd.setBusId(busId()); + snd.setDataFloat((float)arm[0], 0/* sizeof(float)*/); + snd.setDataFloat((float)arm[1], 1 * sizeof(float)); + snd.setDataFloat((float)arm[2], 2 * sizeof(float)); + + XsMessage rcv; + if (!doTransaction(snd, rcv)) + return false; + + return true; +} + +/*! \copydoc XsDevice::gnssLeverArm +*/ +XsVector MTi7_MTi8Device::gnssLeverArm() const +{ + if (!deviceId().isRtk()) + return XsVector(); + + XsMessage snd(XMID_ReqGnssLeverArm), rcv; + if (!doTransaction(snd, rcv)) + return XsVector(); + + XsVector arm(3); + arm[0] = rcv.getDataFloat(0/* sizeof(float)*/); + arm[1] = rcv.getDataFloat(1 * sizeof(float)); + arm[2] = rcv.getDataFloat(2 * sizeof(float)); + return arm; +} diff --git a/extern/xspublic/xscontroller/mti7_mti8device.h b/extern/xspublic/xscontroller/mti7_mti8device.h new file mode 100644 index 0000000..c23a564 --- /dev/null +++ b/extern/xspublic/xscontroller/mti7_mti8device.h @@ -0,0 +1,84 @@ + +// Copyright (c) 2003-2022 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 XSMTI7MTI8DEVICE_H +#define XSMTI7MTI8DEVICE_H + +#include "mtibasedevice.h" +struct XsVector; +/*! \brief A class used for the MTi-7-and MTi-8 types +*/ +class MTi7_MTi8Device : public MtiBaseDeviceEx +{ +public: + //! \copybrief MtiXDevice::constructStandalone + static XsDevice* constructStandalone(Communicator* comm) + { + return new MTi7_MTi8Device(comm); + } + + //! \brief An empty constructor for a device + explicit MTi7_MTi8Device(Communicator* comm); + + //! \brief An empty constructor for a master device + explicit MTi7_MTi8Device(XsDevice* master); + virtual ~MTi7_MTi8Device(); + + bool hasIccSupport() const override; + uint32_t supportedStatusFlags() const override; + XsString shortProductCode() const override; + + bool setGnssLeverArm(const XsVector& arm) override; + XsVector gnssLeverArm() const override; + +protected: + BaseFrequencyResult getBaseFrequencyInternal(XsDataIdentifier dataType = XDI_None) const override; + bool setStringOutputMode(uint16_t type, uint16_t period, uint16_t skipFactor) override; +}; + +#ifndef XDA_PRIVATE_BUILD +/*! \class Mti7_MTi8DeviceEx + \brief The internal base class for MTi-7 and MTi-8 devices +*/ +struct Mti7_MTi8DeviceEx : public MTi7_MTi8Device +{ + //! \copybrief MtigDevice::MtigDevice + explicit Mti7_MTi8DeviceEx(Communicator* comm) : MTi7_MTi8Device(comm) {}; + + //! \copybrief MtigDevice::MtigDevice + explicit Mti7_MTi8DeviceEx(XsDevice* master) : MTi7_MTi8Device(master) {}; +}; +#else +#include "mti7_mti8deviceex.h" +#endif + +#endif diff --git a/extern/xspublic/xscontroller/mti7device.cpp b/extern/xspublic/xscontroller/mti7device.cpp new file mode 100644 index 0000000..ff0cafc --- /dev/null +++ b/extern/xspublic/xscontroller/mti7device.cpp @@ -0,0 +1,156 @@ + +// Copyright (c) 2003-2020 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 "mti7device.h" +#include + +Mti7Device::Mti7Device(Communicator* comm) + : MtiBaseDeviceEx(comm) +{ + if (comm) + comm->setDefaultTimeout(2000); //Increase the default timeout for MTi-1 devices because a settings write can occasionally take ~900ms +} + +Mti7Device::Mti7Device(XsDevice* masterdevice) + : MtiBaseDeviceEx(masterdevice) +{ +} + +Mti7Device::~Mti7Device() +{ +} + +namespace +{ + //! \brief Returns the default frequency of the supplied \a dataType + int baseFreq(XsDataIdentifier dataType) + { + 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_GnssGroup: + { + if ((dataType & XDI_FullTypeMask) == XDI_GnssPvtData) + return 4; + return 0; + } + default: return 0; + } + } +} + +/*! \brief Returns the base update rate (hz) corresponding to the dataType +*/ +MtiBaseDevice::BaseFrequencyResult Mti7Device::getBaseFrequencyInternal(XsDataIdentifier dataType) const +{ + BaseFrequencyResult result; + result.m_frequency = 0; + result.m_divedable = true; + + if ((dataType & XDI_FullTypeMask) == XDI_LocationId || (dataType & XDI_FullTypeMask) == XDI_DeviceId) + return result; + + if ((dataType & XDI_FullTypeMask) == XDI_AccelerationHR || (dataType & XDI_FullTypeMask) == XDI_RateOfTurnHR) + { + result.m_frequency = 800; + result.m_divedable = true; + + return result; + } + + result.m_frequency = baseFreq(dataType); + + if (((dataType & XDI_TypeMask) == XDI_TimestampGroup) || ((dataType & XDI_TypeMask) == XDI_GnssGroup)) + result.m_divedable = false; + + return result; +} + +bool Mti7Device::hasIccSupport() const +{ + return true; +} + +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 + ); +} + +bool Mti7Device::setStringOutputMode(uint16_t /*type*/, uint16_t /*period*/, uint16_t /*skipFactor*/) +{ + return true; +} + +/*! \copybrief XsDevice::shortProductCode +*/ +XsString Mti7Device::shortProductCode() const +{ + XsString code = productCode(); + + if (hardwareVersion() >= XsVersion(2, 0, 0)) + code = stripProductCode(code); + + return code; +} diff --git a/extern/xspublic/xscontroller/mti7device.h b/extern/xspublic/xscontroller/mti7device.h new file mode 100644 index 0000000..c6c7c3a --- /dev/null +++ b/extern/xspublic/xscontroller/mti7device.h @@ -0,0 +1,81 @@ + +// Copyright (c) 2003-2020 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 XSMTI7DEVICE_H +#define XSMTI7DEVICE_H + +#include "mtibasedevice.h" + +/*! \brief The MTi device used for the 7-series +*/ +class Mti7Device : public MtiBaseDeviceEx +{ +public: + //! \copybrief MtiXDevice::constructStandalone + static XsDevice* constructStandalone(Communicator* comm) + { + return new Mti7Device(comm); + } + + //! \brief An empty constructor for a device + explicit Mti7Device(Communicator* comm); + + //! \brief An empty constructor for a master device + explicit Mti7Device(XsDevice* master); + virtual ~Mti7Device(); + + bool hasIccSupport() const override; + uint32_t supportedStatusFlags() const override; + XsString shortProductCode() const override; + +protected: + BaseFrequencyResult getBaseFrequencyInternal(XsDataIdentifier dataType = XDI_None) const override; + bool setStringOutputMode(uint16_t type, uint16_t period, uint16_t skipFactor) override; +}; + +#ifndef XDA_PRIVATE_BUILD +/*! \class Mti7DeviceEx + \brief The internal base class for MTi-7 series devices +*/ +struct Mti7DeviceEx : public Mti7Device +{ + //! \copybrief MtigDevice::MtigDevice + explicit Mti7DeviceEx(Communicator* comm) : Mti7Device(comm) {}; + + //! \copybrief MtigDevice::MtigDevice + explicit Mti7DeviceEx(XsDevice* master) : Mti7Device(master) {}; +}; +#else +#include "mti7deviceex.h" +#endif + +#endif diff --git a/extern/xspublic/xscontroller/mti8x0device.cpp b/extern/xspublic/xscontroller/mti8x0device.cpp new file mode 100644 index 0000000..3da637c --- /dev/null +++ b/extern/xspublic/xscontroller/mti8x0device.cpp @@ -0,0 +1,333 @@ + +// Copyright (c) 2003-2022 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 "mti8x0device.h" + +#include +#include "replyobject.h" +#include "communicator.h" +#include "scenariomatchpred.h" +#include "messageserializer.h" +#include + +#include +#include "synclinegmt.h" +#include +#include +#include +#include + +using namespace xsens; + +/*! \brief Constructs a device + \param comm The communicator to construct with +*/ +Mti8X0Device::Mti8X0Device(Communicator* comm) : + MtiBaseDeviceEx(comm) +{ +} + +/*! \brief Destroys a device +*/ +Mti8X0Device::~Mti8X0Device() +{ +} + +/*! \brief Returns the base update rate (Hz) corresponding to the dataType +*/ +MtiBaseDevice::BaseFrequencyResult Mti8X0Device::getBaseFrequencyInternal(XsDataIdentifier dataType) const +{ + MtiBaseDevice::BaseFrequencyResult result; + result.m_frequency = 0; + result.m_divedable = true; + + if ((dataType == XDI_FreeAcceleration && deviceId().isImu()) || + ((dataType & XDI_FullTypeMask) == XDI_LocationId) || + ((dataType & XDI_FullTypeMask) == XDI_DeviceId)) + return result; + + if ((dataType & XDI_FullTypeMask) == XDI_AccelerationHR) + { + result.m_frequency = 2000; + return result; + } + + if ((dataType & XDI_FullTypeMask) == XDI_RateOfTurnHR) + { + result.m_frequency = 1600; + return result; + } + + auto baseFreq = [this](XsDataIdentifier dataType) + { + XsDataIdentifier fullType = (dataType & XDI_FullTypeMask); + 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: + if (fullType == XDI_GnssPvtPulse) + return deviceId().isRtk() ? XDI_MAX_FREQUENCY_VAL : 0; + return deviceId().isGnss() ? XDI_MAX_FREQUENCY_VAL : 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); + + if (((dataType & XDI_TypeMask) == XDI_TimestampGroup) || ((dataType & XDI_TypeMask) == XDI_GnssGroup)) + result.m_divedable = false; + + return result; +} + +/*! \copybrief XsDevice::shortProductCode +*/ +XsString Mti8X0Device::shortProductCode() const +{ + XsString code = productCode(); + return stripProductCode(code); +} + +/*! \returns The sync line for a mtitx0 device This overrides the base class method. + \param setting The sync setting to get a sync line from +*/ +uint8_t Mti8X0Device::syncLine(const XsSyncSetting& setting) const +{ + SyncLineGmt gmtLine = xslToXslgmt(setting.m_line); + assert(gmtLine != XSLGMT_Invalid); + return static_cast(gmtLine); +} + +/*! \returns True if this device has an ICC support +*/ +bool Mti8X0Device::hasIccSupport() const +{ + return (firmwareVersion() >= XsVersion(1, 1, 0)); +} + + +XsStringOutputTypeArray Mti8X0Device::supportedStringOutputTypes() const +{ + XsStringOutputTypeArray outputs; + + outputs.push_back(XSOT_PSONCMS); + outputs.push_back(XSOT_HCMTW); + outputs.push_back(XSOT_HEROT); + outputs.push_back(XSOT_PTCF); + outputs.push_back(XSOT_GPZDA); + outputs.push_back(XSOT_TSS2); + outputs.push_back(XSOT_PHTRO); + outputs.push_back(XSOT_PRDID); + outputs.push_back(XSOT_EM1000); + outputs.push_back(XSOT_HEHDT); + outputs.push_back(XSOT_HCHDM); + outputs.push_back(XSOT_GPGGA); + outputs.push_back(XSOT_GPRMC); + outputs.push_back(XSOT_XSVEL); + outputs.push_back(XSOT_HCHDG); + + return outputs; +} + +/*! \brief Sets the string output mode for this device + \param type The type to set + \param frequency The frequency to set + \returns True if the device was successfully updated +*/ +bool Mti8X0Device::setStringOutputMode6x0(uint32_t type, uint16_t frequency) +{ + XsMessage sndType(XMID_SetStringOutputConfig); + sndType.setBusId(XS_BID_MASTER); // Always send to master device + sndType.setDataLong(type); + sndType.setDataShort(frequency, 4); + + if (!doTransaction(sndType)) + return false; + + return true; +} + +uint32_t Mti8X0Device::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) + ); +} + +/*! \copybrief XsDevice::canOutputConfiguration +*/ +XsCanOutputConfigurationArray Mti8X0Device::canOutputConfiguration() const +{ + XsMessage snd(XMID_ReqCanConfig), rcv; + if (!doTransaction(snd, rcv)) + return XsCanOutputConfigurationArray(); + + XsCanOutputConfigurationArray config; + MessageDeserializer serializer(rcv); + serializer >> config; + + return config; +} + +/*! \copydoc XsDevice::setCanOutputConfiguration +*/ +bool Mti8X0Device::setCanOutputConfiguration(XsCanOutputConfigurationArray& config) +{ + XsMessage snd(XMID_SetCanOutputConfig, 4); + snd.setBusId(busId()); + bool wasEmpty = config.empty(); + MessageSerializer(snd) << config; + + XsMessage rcv; + if (!doTransaction(snd, rcv)) + return false; + + MessageDeserializer(rcv) >> config; + if (wasEmpty && config.size() == 1 && config[0] == XsCanOutputConfiguration(XCFF_11Bit_Identifier, XCDI_Invalid, 0, 0)) + config.clear(); + return true; +} + +/*! \copybrief XsDevice::canConfiguration +*/ +uint32_t Mti8X0Device::canConfiguration() const +{ + XsMessage snd(XMID_ReqCanConfig), rcv; + if (!doTransaction(snd, rcv)) + return 0; + + return rcv.getDataLong(); +} + +/*! \copydoc XsDevice::portConfiguration +*/ +XsIntArray Mti8X0Device::portConfiguration() const +{ + XsMessage snd(XMID_ReqPortConfig), rcv; + if (!doTransaction(snd, rcv)) + return XsIntArray(); + + XsIntArray rv; + rv.push_back((int)rcv.getDataLong(0)); + rv.push_back((int)rcv.getDataLong(4)); + rv.push_back((int)rcv.getDataLong(8)); + return rv; +} + +/*! \copydoc XsDevice::setPortConfiguration +*/ +bool Mti8X0Device::setPortConfiguration(XsIntArray& config) +{ + XsIntArray currentConfig = portConfiguration(); + + XsMessage snd(XMID_SetPortConfig); + snd.setBusId(busId()); + + if (config.size() > 0) + snd.setDataLong((uint32_t)config[0], 0); + if (config.size() > 1) + snd.setDataLong((uint32_t)config[1], 4); + if (config.size() > 2) + snd.setDataLong((uint32_t)config[2], 8); + + XsMessage rcv; + if (!doTransaction(snd, rcv)) + return false; + + if (currentConfig != config) + return reset(); + + return true; +} + +/*! \copydoc XsDevice::setCanOutputConfiguration +*/ +bool Mti8X0Device::setCanConfiguration(uint32_t config) +{ + uint32_t currentConfig = canConfiguration(); + + XsMessage snd(XMID_SetCanConfig, 4); + snd.setBusId(busId()); + snd.setDataLong(config); + + XsMessage rcv; + if (!doTransaction(snd, rcv)) + return false; + + if ((currentConfig & 0xFF) != (config & 0xFF)) + return reset(); + + return true; +} diff --git a/extern/xspublic/xscontroller/mti8x0device.h b/extern/xspublic/xscontroller/mti8x0device.h new file mode 100644 index 0000000..42332e1 --- /dev/null +++ b/extern/xspublic/xscontroller/mti8x0device.h @@ -0,0 +1,92 @@ + +// Copyright (c) 2003-2022 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 XSMTI8X0DEVICE_H +#define XSMTI8X0DEVICE_H + +#include "mtibasedevice.h" + +/*! \class Mti8X0Device + \brief The MTi device used for the 8X0-series +*/ +class Mti8X0Device : public MtiBaseDeviceEx +{ +public: + //! \copybrief MtiXDevice::constructStandalone + static XsDevice* constructStandalone(Communicator* comm) + { + return new Mti8X0Device(comm); + } + + explicit Mti8X0Device(Communicator* comm); + + //! \brief An empty constructor for a master device + explicit Mti8X0Device(XsDevice* master) : MtiBaseDeviceEx(master) {} + virtual ~Mti8X0Device(); + + XsStringOutputTypeArray supportedStringOutputTypes() const override; + bool setStringOutputMode6x0(uint32_t type, uint16_t frequency); + uint32_t supportedStatusFlags() const override; + XsString shortProductCode() const override; + XsCanOutputConfigurationArray canOutputConfiguration() const override; + bool setCanOutputConfiguration(XsCanOutputConfigurationArray& config) override; + + XsIntArray portConfiguration() const override; + bool setPortConfiguration(XsIntArray& config) override; + + uint32_t canConfiguration() const override; + bool setCanConfiguration(uint32_t config) override; + +protected: + uint8_t syncLine(const XsSyncSetting& setting) const override; + bool hasIccSupport() const override; + + MtiBaseDevice::BaseFrequencyResult getBaseFrequencyInternal(XsDataIdentifier dataType = XDI_None) const override; +}; + +#ifndef XDA_PRIVATE_BUILD +/*! \class Mti8X0DeviceEx + \brief The internal base class for MTi-8X0 series devices +*/ +struct Mti8X0DeviceEx : public Mti8X0Device +{ + //! \copybrief Mti8X0Device::Mti8X0Device + explicit Mti8X0DeviceEx(Communicator* comm) : Mti8X0Device(comm) {}; + + //! \copybrief Mti8X0Device::Mti8X0Device + explicit Mti8X0DeviceEx(XsDevice* master) : Mti8X0Device(master) {}; +}; +#else +#include "mtix00deviceex.h" +#endif + +#endif diff --git a/extern/xspublic/xscontroller/mtibasedevice.cpp b/extern/xspublic/xscontroller/mtibasedevice.cpp new file mode 100644 index 0000000..77f1f6c --- /dev/null +++ b/extern/xspublic/xscontroller/mtibasedevice.cpp @@ -0,0 +1,605 @@ + +// Copyright (c) 2003-2022 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 "mtibasedevice.h" +#include "xsdef.h" +#include "synclinemk4.h" +#include "synclinegmt.h" +#include +#include +#include +#include "xsicccommand.h" +#include +#include "xsiccrepmotionresult.h" +#include +#include +#include +#include + +using namespace xsens; + +/*! \brief Constructs a device + \param comm The communicator to construct with +*/ +MtiBaseDevice::MtiBaseDevice(Communicator* comm) + : MtDeviceEx(comm) +{ +} + +/*! \brief Constructs a child device for a master device + \param master The master device to construct for +*/ +MtiBaseDevice::MtiBaseDevice(XsDevice* master) + : MtDeviceEx(master, XsDeviceId()) +{ +} + +MtiBaseDevice::~MtiBaseDevice() +{ +} + +/*! \copybrief XsDevice::setHeadingOffset +*/ +bool MtiBaseDevice::setHeadingOffset(double) +{ + return false; +} + +/*! \copybrief XsDevice::outputConfiguration +*/ +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(); + + XsOutputConfigurationArray rv; + XsSize count = rcv.getDataSize() >> 2; + for (XsSize row = 0; row < count; row++) + { + uint16_t dataId, freq; + dataId = rcv.getDataShort(row * 4); + freq = rcv.getDataShort(2 + row * 4); + rv.push_back(XsOutputConfiguration((XsDataIdentifier)dataId, freq)); + } + + return rv; +} + +/*! \brief Set the output configuration for this device + \param config The desired output configuration + \returns XRV_OK on success \sa XsResultValue +*/ +XsResultValue MtiBaseDevice::setOutputConfigurationInternal(XsOutputConfigurationArray& config) +{ + if (!deviceId().isMti6X0()) + setStringOutputMode(0, 0, 0); + + return MtDeviceEx::setOutputConfigurationInternal(config); +} + +/*! \copybrief XsDevice::setAlignmentRotationMatrix +*/ +bool MtiBaseDevice::setAlignmentRotationQuaternion(XsAlignmentFrame frame, const XsQuaternion& quat) +{ + XsMessage snd(XMID_SetAlignmentRotation, 1 + XS_LEN_ALIGNMENTROTATION); + 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))); + return doTransaction(snd); +} + +/*! \copybrief XsDevice::alignmentRotationQuaternion +*/ +XsQuaternion MtiBaseDevice::alignmentRotationQuaternion(XsAlignmentFrame frame) const +{ + XsQuaternion quat; + XsMessage snd(XMID_ReqAlignmentRotation), rcv; + snd.setDataByte(frame); + if (doTransaction(snd, rcv)) + { + for (XsSize i = 0; i < 4; ++i) + quat[i] = rcv.getDataFloat(1 + i * 4); + } + return quat; +} + +/*! \copybrief XsDevice::setAlignmentRotationMatrix +*/ +bool MtiBaseDevice::setAlignmentRotationMatrix(XsAlignmentFrame frame, const XsMatrix& matrix) +{ + return setAlignmentRotationQuaternion(frame, XsQuaternion(matrix).normalized()); +} + +/*! \copybrief XsDevice::alignmentRotationMatrix +*/ +XsMatrix MtiBaseDevice::alignmentRotationMatrix(XsAlignmentFrame frame) const +{ + XsMatrix matrix; + matrix.fromQuaternion(alignmentRotationQuaternion(frame)); + return matrix; +} + +/*! \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 + */ + + unsigned int timeResolution = XsDevice::syncSettingsTimeResolutionInMicroSeconds(deviceId()); + + if (s.size() > 10) + 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); + snd.setBusId(busId()); + + /* create message but abort if we encounter anything out of the ordinary + each sync setting is: + 0: event / action (function) + 1: line + 2: polarity + 3: triggerOnce + 4-5: skipFirst + 6-7: skipFactor + 8-9: pulse width + 10-11: delay/offset/clock frequency + */ + for (size_t i = 0; i < s.size(); ++i) + { + const XsSyncSetting& setting = s[i]; + XsSize offset = i * 12; + + snd.setDataByte(setting.m_function, offset); + const uint8_t line = syncLine(setting); + 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); + uint16_t value = 0; + if (setting.m_function == XSF_ClockBiasEstimation || setting.m_function == XSF_SampleAndSend) + snd.setDataShort(setting.m_clockPeriod, offset + 10); + else + { + value = (uint16_t)(int16_t)(setting.m_offset / (int) timeResolution); + snd.setDataShort(value, offset + 10); + } + } + + return doTransaction(snd); +} + +/*! \copybrief XsDevice::syncSettings +*/ +XsSyncSettingArray MtiBaseDevice::syncSettings() const +{ + XsMessage snd(XMID_SyncConfiguration), rcv; + if (!doTransaction(snd, rcv)) + return XsSyncSettingArray(); + + const uint8_t* emtsBuffer = rcv.getDataBuffer(); + return syncSettingsFromBuffer(emtsBuffer); +} + +/*! \brief Create an XsSyncSttingsArray from the given buffer of sync configuration data */ +XsSyncSettingArray MtiBaseDevice::syncSettingsFromBuffer(const uint8_t* buffer) const +{ + unsigned int timeResolution = XsDevice::syncSettingsTimeResolutionInMicroSeconds(deviceId()); + + XsSyncSettingArray rv; + for (XsSize i = 0; i < 10; ++i) + { + uint16_t tmp_pulseWidth; + XsSize offset = i * 12; + XsSyncSetting ss; + ss.m_function = (XsSyncFunction) buffer[offset]; + 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_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)); + else + { + int16_t tmp_offset; + memcpy((void*) &tmp_offset, (void const*) &buffer[offset + 10], sizeof(int16_t)); + ss.m_offset = (int32_t) tmp_offset * (int32_t) timeResolution; + } + + rv.push_back(ss); + } + return rv; +} + +/*! \returns the update rate for the specified XsDataIdentifier in the configuration list or 0 if no such data available + \param dataType The data identifier for which the update rate is requested + \param configurations The configuration list in which the specified dataType must be looked up +*/ +int MtiBaseDevice::calculateUpdateRateImp(XsDataIdentifier dataType, const XsOutputConfigurationArray& configurations) const +{ + int matchLevel = 0; + int result = 0; + + bool groupCheck = ((dataType & XDI_TypeMask) == dataType); + for (XsOutputConfigurationArray::const_iterator i = configurations.begin(); i != configurations.end(); ++i) + { + int ml = 0; + if ((dataType & XDI_FullTypeMask) == (i->m_dataIdentifier & XDI_FullTypeMask)) + { + if (dataType == i->m_dataIdentifier) + ml = 3; + else + ml = 2; + } + else if (groupCheck && (dataType == (i->m_dataIdentifier & XDI_TypeMask))) + ml = 1; + + if (ml > matchLevel) + { + result = i->m_frequency; + if (ml == 3) + break; + matchLevel = ml; + } + } + + return result; +} + +/*! \returns the update rate for the specified XsDataIdentifier or 0 if no such data available + \details This function only looks at the output configuration configured in the device, not XDA calculated data + \param dataType The data identifier to use + \sa calculateUpdateRateImp +*/ +int MtiBaseDevice::calculateUpdateRate(XsDataIdentifier dataType) const +{ + return calculateUpdateRateImp(dataType, outputConfiguration()); +} + +/*! \returns the base update rate (Hz) corresponding to the dataType. Returns 0 if no update rate is available + \param dataType The data identifier to use +*/ +int MtiBaseDevice::getBaseFrequency(XsDataIdentifier dataType) const +{ + return getBaseFrequencyInternal(dataType).m_frequency; +} + +/*! \copybrief XsDevice::supportedUpdateRates +*/ +std::vector MtiBaseDevice::supportedUpdateRates(XsDataIdentifier dataType) const +{ + std::vector updateRates; + auto baseFreq = getBaseFrequencyInternal(dataType); + + if (baseFreq.m_frequency == 0) + return updateRates; + + if (!baseFreq.m_divedable) + { + updateRates.push_back(baseFreq.m_frequency); + return updateRates; + } + + 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) + updateRates.push_back(freq); + } + + return updateRates; +} + +/*! \copybrief XsDevice::setNoRotation +*/ +bool MtiBaseDevice::setNoRotation(uint16_t duration) +{ + return MtDeviceEx::setNoRotation(duration); +} + +/*! \copybrief XsDevice::setInitialPositionLLA +*/ +bool MtiBaseDevice::setInitialPositionLLA(const XsVector& lla) +{ + uint8_t bid = (uint8_t) busId(); + if (bid == XS_BID_INVALID || bid == XS_BID_BROADCAST || lla.size() != 3) + return false; + + XsMessage snd(XMID_SetLatLonAlt, 3 * sizeof(double)); + snd.setDataDouble(lla[0], 0); + snd.setDataDouble(lla[1], 8); + snd.setDataDouble(lla[2], 16); + snd.setBusId(bid); + + return doTransaction(snd); +} + +/*! \copybrief XsDevice::utcTime +*/ +XsTimeInfo MtiBaseDevice::utcTime() const +{ + uint8_t bid = (uint8_t) busId(); + if (bid == XS_BID_INVALID || bid == XS_BID_BROADCAST) + return XsTimeInfo(); + + XsMessage snd(XMID_ReqUtcTime, 0), rcv; + snd.setBusId(bid); + + if (!doTransaction(snd, rcv)) + return XsTimeInfo(); + + XsTimeInfo time; + time.m_nano = rcv.getDataLong(0); + time.m_year = rcv.getDataShort(4); + time.m_month = rcv.getDataByte(6); + time.m_day = rcv.getDataByte(7); + time.m_hour = rcv.getDataByte(8); + time.m_minute = rcv.getDataByte(9); + time.m_second = rcv.getDataByte(10); + time.m_valid = rcv.getDataByte(11); + time.m_utcOffset = 0; + + return time; +} + +/*! \copybrief XsDevice::setUtcTime +*/ +bool MtiBaseDevice::setUtcTime(const XsTimeInfo& time) +{ + uint8_t bid = (uint8_t) busId(); + if (bid == XS_BID_INVALID || bid == XS_BID_BROADCAST) + return false; + + XsMessage snd(XMID_SetUtcTime, XS_LEN_UTCTIME); + snd.setDataLong(time.m_nano, 0); + snd.setDataShort(time.m_year, 4); + snd.setDataByte(time.m_month, 6); + snd.setDataByte(time.m_day, 7); + snd.setDataByte(time.m_hour, 8); + snd.setDataByte(time.m_minute, 9); + snd.setDataByte(time.m_second, 10); + snd.setDataByte(time.m_valid, 11); + snd.setBusId(bid); + + if (!doTransaction(snd)) + return false; + + return true; +} + +/*! \copybrief XsDevice::setErrorMode +*/ +bool MtiBaseDevice::setErrorMode(XsErrorMode em) +{ + if (em == XEM_IncreasePacketCounterAndSendError) + return true; + return false; +} + +/*! \copybrief XsDevice::errorMode +*/ +XsErrorMode MtiBaseDevice::errorMode() const +{ + return XEM_IncreasePacketCounterAndSendError; +} + +/*! \copybrief XsDevice::rs485TransmissionDelay +*/ +uint16_t MtiBaseDevice::rs485TransmissionDelay() const +{ + XsMessage snd(XMID_ReqTransmitDelay), rcv; + if (!doTransaction(snd, rcv)) + return 0; + return rcv.getDataShort(); +} + +/*! \copybrief XsDevice::setRs485TransmissionDelay +*/ +bool MtiBaseDevice::setRs485TransmissionDelay(uint16_t delay) +{ + return delay == 0; +} + +/*! \copybrief XsDevice::resetRemovesPort +*/ +bool MtiBaseDevice::resetRemovesPort() const +{ + Communicator* comm = communicator(); + if (!comm) + return false; + + bool removesPort = false; + uint16_t vid, pid; + comm->portInfo().getVidPid(vid, pid); + // Direct connection, COM port will be removed + if (vid == XSENS_VENDOR_ID && pid < 0x00FF) + removesPort = true; + + return removesPort; +} + +/*! \brief Returns the sync settings line for a generic mti device +*/ +XsSyncLine MtiBaseDevice::syncSettingsLine(const uint8_t* buff, XsSize offset) const +{ + if (deviceId().isMtMark4() || deviceId().isMtMark5()) + return xsl4ToXsl((SyncLineMk4) buff[offset + 1]); + else + return xslgmtToXsl((SyncLineGmt) buff[offset + 1]); +} + +/*! \brief Returns the sync line for a generic mti device +*/ +uint8_t MtiBaseDevice::syncLine(const XsSyncSetting& setting) const +{ + if (deviceId().isMtMark4() || deviceId().isMtMark5()) + { + 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 + { + SyncLineGmt gmtLine = xslToXslgmt(setting.m_line); + assert(gmtLine != XSLGMT_Invalid); + if (gmtLine == XSLGMT_ClockIn) + assert(setting.m_function == XSF_SampleAndSend); + return static_cast(gmtLine); + } +} + +/*! \returns True if device uses on board filtering */ +bool MtiBaseDevice::deviceUsesOnBoardFiltering() +{ + return updateRateForDataIdentifier(XDI_OrientationGroup) > 0; +} + +/*! \brief Starts the representative motion + \returns True if successful +*/ +bool MtiBaseDevice::startRepresentativeMotion() +{ + if (!hasIccSupport()) + return false; + + if (!deviceUsesOnBoardFiltering()) + return false; + + XsMessage snd(XMID_IccCommand, 1); + snd.setBusId(busId()); + snd.setDataByte(XIC_StartRepMotion); + + if (!doTransaction(snd)) + return false; + + return true; +} + +/*! \returns True if the representative motion is running */ +bool MtiBaseDevice::representativeMotionState() +{ + if (!hasIccSupport()) + return false; + + if (!deviceUsesOnBoardFiltering()) + return false; + + bool enabled = false; + + XsMessage snd(XMID_IccCommand, 1); + snd.setBusId(busId()); + snd.setDataByte(XIC_RepMotionState); + XsMessage rcv; + if (doTransaction(snd, rcv)) + enabled = (rcv.getDataByte(1) != 0); + + return enabled; +} + +/*! \brief Stops the representative motion + \returns The results of the representative motion +*/ +XsIccRepMotionResult MtiBaseDevice::stopRepresentativeMotion() +{ + XsIccRepMotionResult result; + + if (!hasIccSupport()) + return result; + + if (!deviceUsesOnBoardFiltering()) + return result; + + XsMessage snd(XMID_IccCommand, 1), rcv; + snd.setBusId(busId()); + snd.setDataByte(XIC_StopRepMotion); + + if (doTransaction(snd, rcv, 3500)) + { + result.m_ddtAccuracy = rcv.getDataFloat(1); + result.m_dimension = rcv.getDataByte(5); + result.m_status = rcv.getDataByte(6); + } + + return result; +} + +/*! \brief Stores the ICC results + \returns True if successful +*/ +bool MtiBaseDevice::storeIccResults() +{ + if (!deviceUsesOnBoardFiltering()) + return false; + + XsMessage snd(XMID_IccCommand, 1); + snd.setBusId(busId()); + snd.setDataByte(XIC_StoreResults); + + return doTransaction(snd, 2000); +} + +/*! \returns True if this device has an ICC support +*/ +bool MtiBaseDevice::hasIccSupport() const +{ + return (firmwareVersion() >= XsVersion(1, 5, 0)); +} + +void MtiBaseDevice::fetchAvailableHardwareScenarios() +{ + if (deviceId().isImu()) // If we are a 100 type device, + m_hardwareFilterProfiles.clear(); // there are no filter profiles in the firmware. + else // For other device types, + MtDeviceEx::fetchAvailableHardwareScenarios(); // fetch the scenarios. +} diff --git a/extern/xspublic/xscontroller/mtibasedevice.h b/extern/xspublic/xscontroller/mtibasedevice.h new file mode 100644 index 0000000..525570a --- /dev/null +++ b/extern/xspublic/xscontroller/mtibasedevice.h @@ -0,0 +1,141 @@ + +// Copyright (c) 2003-2022 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 XSMTIBASEDEVICE_H +#define XSMTIBASEDEVICE_H + +#include "mtdevice.h" +#include + +/*! \class MtiBaseDevice + \brief The generic class for MTi devices +*/ +class MtiBaseDevice : public MtDeviceEx +{ +public: + /*! \brief Construct a device as a master + \param comm The communicator to use + \returns The constructed master device + */ + static XsDevice* constructAsMaster(Communicator* comm) + { + return new MtiBaseDevice(comm); + } + + explicit MtiBaseDevice(Communicator* comm); + explicit MtiBaseDevice(XsDevice* master); + virtual ~MtiBaseDevice(); + + XsOutputConfigurationArray outputConfiguration() const; + + XsResultValue setOutputConfigurationInternal(XsOutputConfigurationArray& o) override; + + 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; + XsMatrix alignmentRotationMatrix(XsAlignmentFrame frame) const override; + bool setAlignmentRotationQuaternion(XsAlignmentFrame frame, const XsQuaternion& quat) override; + XsQuaternion alignmentRotationQuaternion(XsAlignmentFrame frame) const override; + + bool setHeadingOffset(double offset); + + XsSyncSettingArray syncSettings() const override; + bool setSyncSettings(const XsSyncSettingArray& s) override; + + bool setNoRotation(uint16_t duration); + + bool setInitialPositionLLA(const XsVector& lla); + XsTimeInfo utcTime() const; + bool setUtcTime(const XsTimeInfo& time); + + XsErrorMode errorMode() const override; + bool setErrorMode(XsErrorMode errorMode) override; + uint16_t rs485TransmissionDelay() const; + bool setRs485TransmissionDelay(uint16_t delay); + + bool startRepresentativeMotion() override; + bool representativeMotionState() override; + XsIccRepMotionResult stopRepresentativeMotion() override; + bool storeIccResults() override; + + //! A struct for base frequency result + struct BaseFrequencyResult + { + int m_frequency; //!< A frequency value + bool m_divedable; //!< A divedable value + }; + +protected: + virtual int calculateUpdateRate(XsDataIdentifier dataType) const; + + virtual int calculateUpdateRateImp(XsDataIdentifier dataType, const XsOutputConfigurationArray& configurations) const; + + virtual XsSyncLine syncSettingsLine(const uint8_t* buff, XsSize offset) const; + virtual uint8_t syncLine(const XsSyncSetting& setting) const; + virtual XsSyncSettingArray syncSettingsFromBuffer(const uint8_t* buffer) const; + + bool resetRemovesPort() const override; + + /*! \brief An internal function that gets the base frequency + \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 bool hasIccSupport() const; + virtual bool deviceUsesOnBoardFiltering(); + + void fetchAvailableHardwareScenarios() override; +}; + +#ifndef XDA_PRIVATE_BUILD +/*! \class MtiBaseDeviceEx + \brief The internal base class for MTi devices +*/ +struct MtiBaseDeviceEx : public MtiBaseDevice +{ + //! \copybrief MtiBaseDevice::MtiBaseDevice + explicit MtiBaseDeviceEx(Communicator* comm) : MtiBaseDevice(comm) { }; + + //! \copybrief MtiBaseDevice::MtiBaseDevice + explicit MtiBaseDeviceEx(XsDevice* master) : MtiBaseDevice(master) { }; +}; +#else +#include "mtibasedeviceex.h" +#endif + +#endif diff --git a/extern/xspublic/xscontroller/mtigdevice.cpp b/extern/xspublic/xscontroller/mtigdevice.cpp new file mode 100644 index 0000000..738e307 --- /dev/null +++ b/extern/xspublic/xscontroller/mtigdevice.cpp @@ -0,0 +1,184 @@ + +// Copyright (c) 2003-2022 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 "mtigdevice.h" +#include +#include + +#define MTMK4_700_LEGACY_FW_VERSION_MAJOR 1 +#define MTMK4_700_LEGACY_FW_VERSION_MINOR 3 +#define MTMK4_700_LEGACY_FW_VERSION_REVISION 7 + +using namespace xsens; + +/*! \brief Constructs a device + \param comm The communicator to construct with +*/ +MtigDevice::MtigDevice(Communicator* comm) + : MtiBaseDeviceEx(comm) +{ +} + +/*! \brief An empty constructor + \param master The master device +*/ +MtigDevice::MtigDevice(XsDevice* master) + : MtiBaseDeviceEx(master) +{ +} + +/*! \brief Destroys a device +*/ +MtigDevice::~MtigDevice() +{ +} + +XsStringOutputTypeArray MtigDevice::supportedStringOutputTypes() const +{ + XsStringOutputTypeArray outputs; + outputs.push_back(XSOT_HCHDM); + outputs.push_back(XSOT_HCHDG); + outputs.push_back(XSOT_TSS2); + outputs.push_back(XSOT_PHTRO); + outputs.push_back(XSOT_PRDID); + outputs.push_back(XSOT_EM1000); + outputs.push_back(XSOT_PSONCMS); + outputs.push_back(XSOT_HCMTW); + outputs.push_back(XSOT_HEHDT); + outputs.push_back(XSOT_HEROT); + outputs.push_back(XSOT_GPGGA); + outputs.push_back(XSOT_PTCF); + outputs.push_back(XSOT_XSVEL); + outputs.push_back(XSOT_GPZDA); + outputs.push_back(XSOT_GPRMC); + + return outputs; +} + +/*! \brief Returns the base update rate (hz) corresponding to the dataType +*/ +MtiBaseDevice::BaseFrequencyResult MtigDevice::getBaseFrequencyInternal(XsDataIdentifier dataType) const +{ + MtiBaseDevice::BaseFrequencyResult result; + result.m_frequency = 0; + result.m_divedable = true; + + if ((dataType & XDI_FullTypeMask) == XDI_AccelerationHR || (dataType & XDI_FullTypeMask) == XDI_RateOfTurnHR) + { + result.m_frequency = 1000; + result.m_divedable = false; + + return result; + } + + auto baseFreq = [this](XsDataIdentifier dataType) + { + static const XsVersion legacyFwVersion(MTMK4_700_LEGACY_FW_VERSION_MAJOR, MTMK4_700_LEGACY_FW_VERSION_MINOR, MTMK4_700_LEGACY_FW_VERSION_REVISION); + bool const isLegacyFirmware = (firmwareVersion() <= legacyFwVersion); + + XsDataIdentifier fullType = (dataType & XDI_FullTypeMask); + + 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: + if (fullType == XDI_GnssPvtPulse) + return 0; // RTK only + return isLegacyFirmware ? 0 : 4; + default: + return 0; + } + }; + + result.m_frequency = baseFreq(dataType); + + if (((dataType & XDI_TypeMask) == XDI_TimestampGroup) || ((dataType & XDI_TypeMask) == XDI_GnssGroup)) + result.m_divedable = false; + + return result; +} + +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 + ); +} diff --git a/extern/xspublic/xscontroller/mtigdevice.h b/extern/xspublic/xscontroller/mtigdevice.h new file mode 100644 index 0000000..9e5fb70 --- /dev/null +++ b/extern/xspublic/xscontroller/mtigdevice.h @@ -0,0 +1,77 @@ + +// Copyright (c) 2003-2022 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 XSMTIGDEVICE_H +#define XSMTIGDEVICE_H + +#include "mtibasedevice.h" + +/*! \class MtigDevice + \brief The MTi device used for the 7X0-series +*/ +class MtigDevice : public MtiBaseDeviceEx +{ +public: + //! \copybrief MtiXDevice::constructStandalone + static XsDevice* constructStandalone(Communicator* comm) + { + return new MtigDevice(comm); + } + + explicit MtigDevice(Communicator* comm); + explicit MtigDevice(XsDevice* master); + virtual ~MtigDevice(); + + XsStringOutputTypeArray supportedStringOutputTypes() const override; + uint32_t supportedStatusFlags() const override; + +protected: + MtiBaseDevice::BaseFrequencyResult getBaseFrequencyInternal(XsDataIdentifier dataType = XDI_None) const override; +}; + +#ifndef XDA_PRIVATE_BUILD +/*! \class MtigDeviceEx + \brief The internal base class for MTi-7X0 series devices +*/ +struct MtigDeviceEx : public MtigDevice +{ + //! \copybrief MtigDevice::MtigDevice + explicit MtigDeviceEx(Communicator* comm) : MtigDevice(comm) {}; + + //! \copybrief MtigDevice::MtigDevice + explicit MtigDeviceEx(XsDevice* master) : MtigDevice(master) {}; +}; +#else +#include "mtigdeviceex.h" +#endif + +#endif diff --git a/extern/xspublic/xscontroller/mtix00device.cpp b/extern/xspublic/xscontroller/mtix00device.cpp new file mode 100644 index 0000000..27c9afa --- /dev/null +++ b/extern/xspublic/xscontroller/mtix00device.cpp @@ -0,0 +1,179 @@ + +// Copyright (c) 2003-2022 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 "mtix00device.h" +#include +#include +#include "replyobject.h" +#include "communicator.h" +#include "scenariomatchpred.h" +#include +#include + +using namespace xsens; + +/*! \brief Constructs a device + \param comm The communicator to construct with +*/ +MtiX00Device::MtiX00Device(Communicator* comm) : + MtiBaseDeviceEx(comm) +{ +} + +/*! \brief Destroys a device +*/ +MtiX00Device::~MtiX00Device() +{ +} + +XsStringOutputTypeArray MtiX00Device::supportedStringOutputTypes() const +{ + XsStringOutputTypeArray outputs; + + if (deviceId().isImu() || deviceId().isVru() || deviceId().isAhrs()) + { + outputs.push_back(XSOT_PSONCMS); + outputs.push_back(XSOT_HCMTW); + outputs.push_back(XSOT_HEROT); + outputs.push_back(XSOT_PTCF); + outputs.push_back(XSOT_GPZDA); + } + + if (deviceId().isVru() || deviceId().isAhrs()) + { + outputs.push_back(XSOT_TSS2); + outputs.push_back(XSOT_PHTRO); + outputs.push_back(XSOT_PRDID); + outputs.push_back(XSOT_EM1000); + outputs.push_back(XSOT_HEHDT); + } + + if (deviceId().isAhrs()) + { + outputs.push_back(XSOT_HCHDM); + outputs.push_back(XSOT_HCHDG); + } + + return outputs; +} + +/*! \brief Returns the base update rate (Hz) corresponding to the dataType +*/ +MtiBaseDevice::BaseFrequencyResult MtiX00Device::getBaseFrequencyInternal(XsDataIdentifier dataType) const +{ + MtiBaseDevice::BaseFrequencyResult result; + result.m_frequency = 0; + result.m_divedable = true; + + if (dataType == XDI_FreeAcceleration && deviceId().isImu()) + return result; + + if ((dataType & XDI_FullTypeMask) == XDI_AccelerationHR || (dataType & XDI_FullTypeMask) == XDI_RateOfTurnHR) + { + result.m_frequency = 1000; + result.m_divedable = false; + + return result; + } + + auto baseFreq = [this](XsDataIdentifier dataType) + { + 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; + } + }; + result.m_frequency = baseFreq(dataType); + + if ((dataType & XDI_TypeMask) == XDI_TimestampGroup) + result.m_divedable = false; + + return result; +} + +uint32_t MtiX00Device::supportedStatusFlags() const +{ + 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 + ); +} diff --git a/extern/xspublic/xscontroller/mtix00device.h b/extern/xspublic/xscontroller/mtix00device.h new file mode 100644 index 0000000..5b3b80f --- /dev/null +++ b/extern/xspublic/xscontroller/mtix00device.h @@ -0,0 +1,79 @@ + +// Copyright (c) 2003-2022 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 XSMTIX00DEVICE_H +#define XSMTIX00DEVICE_H + +#include "mtibasedevice.h" + +/*! \class MtiX00Device + \brief The MTi device used for the X00-series +*/ +class MtiX00Device : public MtiBaseDeviceEx +{ +public: + //! \copybrief MtiXDevice::constructStandalone + static XsDevice* constructStandalone(Communicator* comm) + { + return new MtiX00Device(comm); + } + + explicit MtiX00Device(Communicator* comm); + + //! \brief An empty constructor for a master device + explicit MtiX00Device(XsDevice* master) : MtiBaseDeviceEx(master) {} + virtual ~MtiX00Device(); + + XsStringOutputTypeArray supportedStringOutputTypes() const override; + uint32_t supportedStatusFlags() const override; + +protected: + MtiBaseDevice::BaseFrequencyResult getBaseFrequencyInternal(XsDataIdentifier dataType = XDI_None) const override; +}; + +#ifndef XDA_PRIVATE_BUILD +/*! \class MtiX00DeviceEx + \brief The internal base class for MTi-X00 series devices +*/ +struct MtiX00DeviceEx : public MtiX00Device +{ + //! \copybrief MtiX00Device::MtiX00Device + explicit MtiX00DeviceEx(Communicator* comm) : MtiX00Device(comm) {}; + + //! \copybrief MtiX00Device::MtiX00Device + explicit MtiX00DeviceEx(XsDevice* master) : MtiX00Device(master) {}; +}; +#else +#include "mtix00deviceex.h" +#endif + +#endif diff --git a/extern/xspublic/xscontroller/mtix0device.cpp b/extern/xspublic/xscontroller/mtix0device.cpp new file mode 100644 index 0000000..f74179e --- /dev/null +++ b/extern/xspublic/xscontroller/mtix0device.cpp @@ -0,0 +1,170 @@ + +// Copyright (c) 2003-2022 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 "mtix0device.h" +#include +#include +#include "replyobject.h" +#include "communicator.h" +#include "scenariomatchpred.h" +#include +#include + +using namespace xsens; + +/*! \brief Constructs a device + \param comm The communicator to construct with +*/ +MtiX0Device::MtiX0Device(Communicator* comm) : + MtiBaseDeviceEx(comm) +{ +} + +/*! \brief Destroys a device +*/ +MtiX0Device::~MtiX0Device() +{ +} + +XsStringOutputTypeArray MtiX0Device::supportedStringOutputTypes() const +{ + XsStringOutputTypeArray outputs; + + if (deviceId().isImu() || deviceId().isVru() || deviceId().isAhrs()) + { + outputs.push_back(XSOT_PSONCMS); + outputs.push_back(XSOT_HCMTW); + outputs.push_back(XSOT_HEROT); + outputs.push_back(XSOT_PTCF); + outputs.push_back(XSOT_GPZDA); + } + + if (deviceId().isVru() || deviceId().isAhrs()) + { + outputs.push_back(XSOT_TSS2); + outputs.push_back(XSOT_PHTRO); + outputs.push_back(XSOT_PRDID); + outputs.push_back(XSOT_EM1000); + outputs.push_back(XSOT_HEHDT); + } + + if (deviceId().isAhrs()) + { + outputs.push_back(XSOT_HCHDM); + outputs.push_back(XSOT_HCHDG); + } + + return outputs; +} + +/*! \brief Returns the base update rate (Hz) corresponding to the dataType. Returns 0 if no update rate is available +*/ +MtiBaseDevice::BaseFrequencyResult MtiX0Device::getBaseFrequencyInternal(XsDataIdentifier dataType) const +{ + MtiBaseDevice::BaseFrequencyResult result; + result.m_frequency = 0; + result.m_divedable = true; + + if (dataType == XDI_FreeAcceleration && deviceId().isImu()) + return result; + + if ((dataType & XDI_FullTypeMask) == XDI_AccelerationHR || (dataType & XDI_FullTypeMask) == XDI_RateOfTurnHR) + { + result.m_frequency = 1000; + result.m_divedable = false; + + return result; + } + + auto baseFreq = [this](XsDataIdentifier dataType) + { + 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; + } + }; + result.m_frequency = baseFreq(dataType); + + if ((dataType & XDI_TypeMask) == XDI_TimestampGroup) + result.m_divedable = false; + + return result; +} + +uint32_t MtiX0Device::supportedStatusFlags() const +{ + 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 + ); +} diff --git a/extern/xspublic/xscontroller/mtix0device.h b/extern/xspublic/xscontroller/mtix0device.h new file mode 100644 index 0000000..ee1575a --- /dev/null +++ b/extern/xspublic/xscontroller/mtix0device.h @@ -0,0 +1,79 @@ + +// Copyright (c) 2003-2022 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 XSMTIX0DEVICE_H +#define XSMTIX0DEVICE_H + +#include "mtibasedevice.h" + +/*! \class MtiX0Device + \brief The MTi device used for the X0-series +*/ +class MtiX0Device : public MtiBaseDeviceEx +{ +public: + //! \copybrief MtiXDevice::constructStandalone + static XsDevice* constructStandalone(Communicator* comm) + { + return new MtiX0Device(comm); + } + + explicit MtiX0Device(Communicator* comm); + + //! \brief An empty constructor for a master device + explicit MtiX0Device(XsDevice* master) : MtiBaseDeviceEx(master) {}; + virtual ~MtiX0Device(); + + XsStringOutputTypeArray supportedStringOutputTypes() const override; + uint32_t supportedStatusFlags() const override; + +protected: + MtiBaseDevice::BaseFrequencyResult getBaseFrequencyInternal(XsDataIdentifier dataType = XDI_None) const override; +}; + +#ifndef XDA_PRIVATE_BUILD +/*! \class MtiX0DeviceEx + \brief The internal base class for MTi-X0 series devices +*/ +struct MtiX0DeviceEx : public MtiX0Device +{ + //! \copybrief MtiX0Device::MtiX0Device + explicit MtiX0DeviceEx(Communicator* comm) : MtiX0Device(comm) {}; + + //! \copybrief MtiX0Device::MtiX0Device + explicit MtiX0DeviceEx(XsDevice* master) : MtiX0Device(master) {}; +}; +#else +#include "mtix0deviceex.h" +#endif + +#endif diff --git a/extern/xspublic/xscontroller/mtixdevice.cpp b/extern/xspublic/xscontroller/mtixdevice.cpp new file mode 100644 index 0000000..c557b14 --- /dev/null +++ b/extern/xspublic/xscontroller/mtixdevice.cpp @@ -0,0 +1,170 @@ + +// Copyright (c) 2003-2022 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 "mtixdevice.h" + +#include +#include "replyobject.h" +#include "communicator.h" +#include "scenariomatchpred.h" +#include + +#include +#include "synclinegmt.h" +#include + +using namespace xsens; + +/*! \brief Constructs a device + \param comm The communicator to construct with +*/ +MtiXDevice::MtiXDevice(Communicator* comm) + : MtiBaseDeviceEx(comm) +{ + if (comm) + comm->setDefaultTimeout(2000); //Increase the default timeout for MTi-1 devices because a settings write can occasionally take ~900ms +} + +/*! \brief Destroys a device +*/ +MtiXDevice::~MtiXDevice() +{ +} + +/*! \brief Returns the base update rate (Hz) corresponding to the dataType. Returns 0 if no update rate is available +*/ +MtiBaseDevice::BaseFrequencyResult MtiXDevice::getBaseFrequencyInternal(XsDataIdentifier dataType) const +{ + MtiBaseDevice::BaseFrequencyResult result; + result.m_frequency = 0; + result.m_divedable = true; + + if ((dataType == XDI_FreeAcceleration && deviceId().isImu()) || + ((dataType & XDI_FullTypeMask) == XDI_LocationId) || + ((dataType & XDI_FullTypeMask) == XDI_DeviceId)) + return result; + + 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_v1 ? false : true; + + return result; + } + + auto baseFreq = [this](XsDataIdentifier dataType) + { + 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; + } + }; + result.m_frequency = baseFreq(dataType); + + if ((dataType & XDI_TypeMask) == XDI_TimestampGroup) + result.m_divedable = false; + + return result; +} + +/*! \returns True if this device has an ICC support +*/ +bool MtiXDevice::hasIccSupport() const +{ + return (firmwareVersion() >= XsVersion(1, 1, 0)); +} + +uint32_t MtiXDevice::supportedStatusFlags() const +{ + 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 + ); +} + +/*! \copybrief XsDevice::shortProductCode +*/ +XsString MtiXDevice::shortProductCode() const +{ + XsString code = productCode(); + + if (hardwareVersion() >= XsVersion(2, 0, 0)) + code = stripProductCode(code); + + return code; +} diff --git a/extern/xspublic/xscontroller/mtixdevice.h b/extern/xspublic/xscontroller/mtixdevice.h new file mode 100644 index 0000000..ecbbfc3 --- /dev/null +++ b/extern/xspublic/xscontroller/mtixdevice.h @@ -0,0 +1,84 @@ + +// Copyright (c) 2003-2022 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 XSMTIXDEVICE_H +#define XSMTIXDEVICE_H + +#include "mtibasedevice.h" + +/*! \class MtiXDevice + \brief The MTi device used for the X-series +*/ +class MtiXDevice : 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 MtiXDevice(comm); + } + + explicit MtiXDevice(Communicator* comm); + + //! \brief An empty constructor for a master device + explicit MtiXDevice(XsDevice* master) : MtiBaseDeviceEx(master) {} + virtual ~MtiXDevice(); + + uint32_t supportedStatusFlags() const override; + XsString shortProductCode() const override; + +protected: + bool hasIccSupport() const override; + + MtiBaseDevice::BaseFrequencyResult getBaseFrequencyInternal(XsDataIdentifier dataType = XDI_None) const override; +}; + +#ifndef XDA_PRIVATE_BUILD +/*! \class MtiXDeviceEx + \brief The internal base class for MTi-X series devices +*/ +struct MtiXDeviceEx : public MtiXDevice +{ + //! \copybrief MtiXDevice::MtiXDevice + explicit MtiXDeviceEx(Communicator* comm) : MtiXDevice(comm) {}; + + //! \copybrief MtiXDevice::MtiXDevice + explicit MtiXDeviceEx(XsDevice* master) : MtiXDevice(master) {}; +}; +#else +#include "mtixdeviceex.h" +#endif + +#endif diff --git a/extern/xspublic/xscontroller/mtsyncsettings.h b/extern/xspublic/xscontroller/mtsyncsettings.h new file mode 100644 index 0000000..4095772 --- /dev/null +++ b/extern/xspublic/xscontroller/mtsyncsettings.h @@ -0,0 +1,63 @@ + +// Copyright (c) 2003-2022 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 MTSYNCSETTINGS_H +#define MTSYNCSETTINGS_H + +#include + +/*! \brief The sync settings of MT +*/ +struct MtSyncSettings +{ + + /*! \brief Sync in settings + */ + struct + { + uint16_t m_mode; + uint16_t m_skipFactor; + uint32_t m_offset; + } m_in; + + /*! \brief Sync out settings + */ + struct + { + uint16_t m_mode; + uint16_t m_skipFactor; + uint32_t m_offset; + uint32_t m_pulseWidth; + } m_out; +}; + +#endif diff --git a/extern/xspublic/xscontroller/mtthread.cpp b/extern/xspublic/xscontroller/mtthread.cpp new file mode 100644 index 0000000..6b21c16 --- /dev/null +++ b/extern/xspublic/xscontroller/mtthread.cpp @@ -0,0 +1,98 @@ + +// Copyright (c) 2003-2022 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 "mtthread.h" +#include "serialcommunicator.h" +#include +#include + +using namespace xsens; + +/*! \class MtThread + \brief A class that implements thread for MT +*/ + +/*! \brief Default constructor */ +MtThread::MtThread(DataParser& fetcher, SerialCommunicator& communicator) + : DataPoller(fetcher) + , m_doGotoConfig(false) + , m_communicator(&communicator) + , m_gotoConfigPlus(0) +{ + JLDEBUGG("Starting MtThread " << this << " with parser " << &fetcher); + + XsMessage gotoConfig(XMID_GotoConfig); + XsMessage largeGotoConfig(XMID_GotoConfig, 30); + m_gotoConfigPlus = new XsByteArray(largeGotoConfig.rawMessage()); + for (int i = 0; i < 6; ++i) + m_gotoConfigPlus->append(gotoConfig.rawMessage()); +} + +/*! \brief Destructor */ +MtThread::~MtThread(void) +{ + try + { + JLDEBUGG("Stopping MtThread " << this); + cleanup(); + if (m_gotoConfigPlus) + delete m_gotoConfigPlus; + } + catch (...) + { + } +} + +/*! \brief Set whether we should send gotoconfig here */ +void MtThread::setDoGotoConfig(bool doit) +{ + srand((unsigned int)XsTime_timeStampNow(0)); + m_doGotoConfig = doit; +} + +/*! \brief The inner thread function + \details This function handles port communication, delegating processing and calibration to its DataParser. + \returns A value from 0 to 3 +*/ +int32_t MtThread::innerFunction(void) +{ + if (m_doGotoConfig) + { + JLDEBUGG("Sending gotoConfig"); + + if (m_communicator->writeRawData(*m_gotoConfigPlus) != XRV_OK) + JLALERTG("Send gotoConfig failed"); + 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/extern/xspublic/xscontroller/mtthread.h b/extern/xspublic/xscontroller/mtthread.h new file mode 100644 index 0000000..f823ab9 --- /dev/null +++ b/extern/xspublic/xscontroller/mtthread.h @@ -0,0 +1,59 @@ + +// Copyright (c) 2003-2022 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 MTTHREAD_H +#define MTTHREAD_H + +#include "datapoller.h" + +class DataParser; +class SerialCommunicator; +struct XsByteArray; + +class MtThread : public DataPoller +{ +public: + explicit MtThread(DataParser& fetcher, SerialCommunicator& communicator); + virtual ~MtThread(void); + + void setDoGotoConfig(bool doit); + +protected: + virtual int32_t innerFunction(void); + +private: + bool m_doGotoConfig; + SerialCommunicator* m_communicator; + XsByteArray* m_gotoConfigPlus; +}; + +#endif diff --git a/extern/xspublic/xscontroller/nmea_protocolhandler.cpp b/extern/xspublic/xscontroller/nmea_protocolhandler.cpp new file mode 100644 index 0000000..140c30b --- /dev/null +++ b/extern/xspublic/xscontroller/nmea_protocolhandler.cpp @@ -0,0 +1,146 @@ + +// Copyright (c) 2003-2022 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 "nmea_protocolhandler.h" +#include "xscontrollerconfig.h" + +namespace nmea +{ + +const char PREAMBLE = '$'; +const char PREAMBLE_TSS2 = ':'; +const char PREAMBLE_EM1000 = '\0'; +const unsigned char AFTER_PREAMBLE_EM1000 = 0x90; +const char CR = '\r'; +const char LF = '\n'; + +/*! \class ProtocolHandler + \brief The protocol handler for a different NMEA data types +*/ + +/*! \brief Default constructor +*/ +ProtocolHandler::ProtocolHandler() +{ +} + +/*! \brief Deafult destructor +*/ +ProtocolHandler::~ProtocolHandler() throw() +{ +} + +/*! \copydoc IProtocolHandler::findMessage +*/ +MessageLocation ProtocolHandler::findMessage(XsProtocolType& type, const XsByteArray& raw) const +{ + type = XPT_Nmea; + MessageLocation location(-1, 0, -1, 0); + + int bufferSize = (int)raw.size(); + if (bufferSize == 0) + return location; + + const unsigned char* buffer = raw.data(); + + bool foundPreamble = false; + bool foundEnd = false; + int start = 0; + int end = 0; + for (int i = 0; i < bufferSize; i++) + { + if (!foundPreamble) + { + if ((buffer[i] == PREAMBLE) || (buffer[i] == PREAMBLE_TSS2) || (buffer[i] == PREAMBLE_EM1000)) + { + foundPreamble = true; + start = i; + } + } + if (foundPreamble) + { + if ((buffer[i] == LF) && (buffer[i - 1] == CR)) + { + foundEnd = true; + end = i + 1; + } + else if ((buffer[i] == AFTER_PREAMBLE_EM1000) && (buffer[i - 1] == PREAMBLE_EM1000)) // special case EM1000 + { + if ((i - start) == 1 && (i + 9) < bufferSize) + { + foundEnd = true; + end = i + 9; + } + else + foundPreamble = false; + } + } + if (foundPreamble && foundEnd) + break; + } + + location.m_startPos = start; + location.m_size = end - start; + + return location; +} + +/*! \copydoc IProtocolHandler::convertToMessage +*/ +XsMessage ProtocolHandler::convertToMessage(MessageLocation& location, const XsByteArray& raw) const +{ + (void)location; + (void)raw; + return XsMessage(); +} + +/*! \returns The minimum message size +*/ +int ProtocolHandler::minimumMessageSize() const +{ + return MINIMUM_MESSAGE_SIZE; +} + +/*! \returns The maximum message size +*/ +int ProtocolHandler::maximumMessageSize() const +{ + return MAXIMUM_MESSAGE_SIZE; +} + +/*! \returns The type of the protocol */ +int ProtocolHandler::type() const +{ + return XPT_Nmea; +} + +} diff --git a/extern/xspublic/xscontroller/nmea_protocolhandler.h b/extern/xspublic/xscontroller/nmea_protocolhandler.h new file mode 100644 index 0000000..fc924da --- /dev/null +++ b/extern/xspublic/xscontroller/nmea_protocolhandler.h @@ -0,0 +1,63 @@ + +// Copyright (c) 2003-2022 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 NMEA_PROTOCOLHANDLER_H +#define NMEA_PROTOCOLHANDLER_H + +#include "iprotocolhandler.h" +#include "xsprotocoltype.h" +#include +#include + +namespace nmea +{ + +class ProtocolHandler : public virtual IProtocolHandler +{ +public: + ProtocolHandler(); + virtual ~ProtocolHandler() throw(); + + MessageLocation findMessage(XsProtocolType& type, const XsByteArray& raw) const override; + XsMessage convertToMessage(MessageLocation& location, const XsByteArray& raw) const override; + int minimumMessageSize() const override; + int maximumMessageSize() const override; + int type() const override; + +private: + static const int MINIMUM_MESSAGE_SIZE = 16; + static const int MAXIMUM_MESSAGE_SIZE = 128; +}; + +} + +#endif diff --git a/extern/xspublic/xscontroller/openportstage.h b/extern/xspublic/xscontroller/openportstage.h new file mode 100644 index 0000000..471fa86 --- /dev/null +++ b/extern/xspublic/xscontroller/openportstage.h @@ -0,0 +1,56 @@ + +// Copyright (c) 2003-2022 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 OPENPORTSTAGE_H +#define OPENPORTSTAGE_H + +/*! \addtogroup enums Global enumerations + @{ +*/ +/*! \brief Port opening stages + \details Opening a communication port is done in 3 stages. In some cases not all stages should be + 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 +{ + OPS_OpenPort = 1, + //OPS_StartThread = 2, // obsolete + OPS_InitDevice = 4, + + OPS_InitStart = OPS_InitDevice, + OPS_Full = OPS_OpenPort | OPS_InitDevice, +}; +/*! @} */ +typedef enum OpenPortStage OpenPortStage; + +#endif diff --git a/extern/xspublic/xscontroller/packeterrorrateestimator.cpp b/extern/xspublic/xscontroller/packeterrorrateestimator.cpp new file mode 100644 index 0000000..16eb578 --- /dev/null +++ b/extern/xspublic/xscontroller/packeterrorrateestimator.cpp @@ -0,0 +1,132 @@ + +// Copyright (c) 2003-2022 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 "packeterrorrateestimator.h" +#include "xscontrollerconfig.h" +#include + +#define PER_UPDATE_PERIOD_MILLISECONDS (10000) +#define INVALID_PACKET_RATE (-1) + +/*! \class PacketErrorRateEstimator + \brief Thread to periodically estimate packet error rate based on expected packet rate + + The Packet Error Rate (PER) is estimated based on the ratio of the number of + received packets compared to the number of expected packets within a time + window. The estimator operates as a thread that periodically updates the + PER estimate. +*/ + +/*! \brief Constructor */ +PacketErrorRateEstimator::PacketErrorRateEstimator() + : m_expectedPacketsPerSecond(INVALID_PACKET_RATE) + , m_receivedPacketCount(0) + , m_packetErrorRate(0) + , m_previousUpdateTime(0) +{ +} + +/*! \brief Destructor */ +PacketErrorRateEstimator::~PacketErrorRateEstimator() +{ +} + +/*! \brief Set the expected packet reception rate in packets per second + \param packetsPerSecond The number of packets expected to be received per second +*/ +void PacketErrorRateEstimator::setExpectedPacketsPerSecond(int16_t packetsPerSecond) +{ + xsens::Lock lock(&m_mutex); + m_expectedPacketsPerSecond = packetsPerSecond; +} + +/*! \brief Indicate that a packet has been received + \details Should be called for every in order packet received by the device. Reception + of out of order packets, e.g. retransmissions, or late packets, should not + trigger a call to this function. +*/ +void PacketErrorRateEstimator::packetReceived(void) +{ + xsens::Lock lock(&m_mutex); + ++m_receivedPacketCount; +} + +/*! \brief Return the currently estimated packet error rate + \return The packet error rate as a percentage +*/ +uint8_t PacketErrorRateEstimator::packetErrorRate(void) const +{ + xsens::Lock lock(&m_mutex); + return m_packetErrorRate; +} + +/*! \brief Initializes the estimation parameters */ +void PacketErrorRateEstimator::initFunction(void) +{ + xsNameThisThread("Packet Error Rate Estimator"); + m_previousUpdateTime = XsTimeStamp::nowMs(); +} + +/*! \brief Updates the packet error rate estimate periodically */ +int32_t PacketErrorRateEstimator::innerFunction(void) +{ + xsens::Lock lock(&m_mutex); + + const int64_t now = XsTimeStamp::nowMs(); + + if (m_expectedPacketsPerSecond > 0) + { + const int64_t msSinceLastUpdate = now - m_previousUpdateTime; + + if (msSinceLastUpdate > 0) + { + const float expectedPackets = (float)floor(m_expectedPacketsPerSecond * (msSinceLastUpdate / 1000)); + float packetDeliveryRate = 100.0f * (m_receivedPacketCount / expectedPackets); + + if (packetDeliveryRate > 100.0f) + 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 << "%."); + } + else + { + m_packetErrorRate = 0; + JLDEBUGG("Received " << m_receivedPacketCount << " packets in " << msSinceLastUpdate << " ms. Delivery rate unknown (100%). Error rate unknown (" << (int)m_packetErrorRate << "%)."); + } + } + + m_receivedPacketCount = 0; + m_previousUpdateTime = now; + + return PER_UPDATE_PERIOD_MILLISECONDS; +} diff --git a/extern/xspublic/xscontroller/packeterrorrateestimator.h b/extern/xspublic/xscontroller/packeterrorrateestimator.h new file mode 100644 index 0000000..c04eba5 --- /dev/null +++ b/extern/xspublic/xscontroller/packeterrorrateestimator.h @@ -0,0 +1,61 @@ + +// Copyright (c) 2003-2022 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 __PACKETERRORRATEESTIMATOR_H +#define __PACKETERRORRATEESTIMATOR_H + +#include +#include + +class PacketErrorRateEstimator : public xsens::StandardThread +{ +public: + PacketErrorRateEstimator(); + virtual ~PacketErrorRateEstimator(); + + void setExpectedPacketsPerSecond(int16_t packetsPerSecond); + void packetReceived(void); + uint8_t packetErrorRate(void) const; + +protected: + virtual void initFunction(void); + virtual int32_t innerFunction(void); + +private: + mutable xsens::Mutex m_mutex; + int32_t m_expectedPacketsPerSecond; + uint16_t m_receivedPacketCount; + uint8_t m_packetErrorRate; + int64_t m_previousUpdateTime; +}; + +#endif diff --git a/extern/xspublic/xscontroller/packetstamper.cpp b/extern/xspublic/xscontroller/packetstamper.cpp new file mode 100644 index 0000000..c8638b7 --- /dev/null +++ b/extern/xspublic/xscontroller/packetstamper.cpp @@ -0,0 +1,360 @@ + +// Copyright (c) 2003-2022 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 +#include "packetstamper.h" + +/*! \class PacketStamper + \brief Supplies functionality for timestamping data packets. + \details This class can analyze a data packet and create a proper packet id for it. +*/ + +//! \brief 32 bit MT Sample Counter boundary +const int64_t PacketStamper::AWINDABOUNDARY = 0x100000000LL; + +//! \brief 16 bit MT Sample Counter boundary +const int64_t PacketStamper::MTSCBOUNDARY = 0x00010000LL; + +//! \brief 8 bit Sample Counter boundary +const int64_t PacketStamper::SC8BOUNDARY = 0x00000100LL; + +//! \brief Default constructor +PacketStamper::PacketStamper() +{ + resetTosEstimation(); +} + +//! \brief Reset the Time Of Sampling estimation parameters +void PacketStamper::resetTosEstimation() +{ + m_latest = DataPair{-1, 0}; + m_rejectionCountdown = 0; + m_dataPoints.clear(); + m_rate = 0; + m_toa0 = 0.0; +} + +/*! \brief Calculate the new large packet counter value based on \a frameCounter and the \a lastCounter + \details Wraparound is at the given \a boundary + \param[in] frameCounter The frame counter + \param[in] lastCounter The last counter + \param[in] boundary the boundary at which to assume a wrap-around + \returns The computed packet counter value + \note If lastCounter < 0, returns frameCounter +*/ +int64_t PacketStamper::calculateLargePacketCounter(int64_t frameCounter, int64_t lastCounter, int64_t boundary) +{ + if (lastCounter < 0) + return frameCounter; + + const int64_t lowMask = boundary - 1; + const int64_t boundaryHalf = boundary / 2; + + int64_t low = lastCounter & lowMask; + int64_t dt = frameCounter - low; + if (dt < -boundaryHalf) + return lastCounter + dt + boundary; // positive wraparound + if (dt < boundaryHalf) + return lastCounter + dt; // normal increment + + return lastCounter + dt - boundary; // negative wraparound +} + +/*! \brief Create 64 bit counter for a packet. + \details Wrap when new XsDataPacket is too far away from the previous XsDataPacket in time. + Use half cache size as reasonable time difference + When infinite cache, simply wrap when new is lower than old + \param pack The XsDataPacket that needs its 64-bit sample counter updated + \param highestPacket The highest packet available for the current device, it will be updated if + the new counter is higher than the stored value. + \returns The computed counter for the packet. +*/ +int64_t PacketStamper::stampPacket(XsDataPacket& pack, XsDataPacket const& highestPacket) +{ + pack.setTimeOfArrival(XsTimeStamp::now()); + int64_t newCounter, lastCounter = -1; + + if (!highestPacket.empty()) + lastCounter = highestPacket.packetId(); + + if (pack.packetId() > 0) + newCounter = pack.packetId(); + else if (pack.containsPacketCounter()) + newCounter = calculateLargePacketCounter(pack.packetCounter(), lastCounter, MTSCBOUNDARY); + else if (pack.containsSampleTimeFine()) + { + newCounter = lastCounter + 1; + //if (pack.containsSampleTimeCoarse()) + // newCounter = (int64_t) pack.sampleTime64(); + //else + // newCounter = calculateLargeSampleTime((int32_t) pack.sampleTimeFine(), lastCounter); + } + else if (pack.containsPacketCounter8()) + newCounter = calculateLargePacketCounter(pack.packetCounter8(), lastCounter, SC8BOUNDARY); + else if (pack.containsAwindaSnapshot()) + newCounter = calculateLargePacketCounter(pack.awindaSnapshot().m_frameNumber, lastCounter, AWINDABOUNDARY); + else + newCounter = lastCounter + 1; + + // JLDEBUGG("XsensDeviceAPI", "%s [%08x] old = %I64d new = %I64d diff = %I64d", __FUNCTION__, did, lastCounter, newCounter, (newCounter-lastCounter)); + + pack.setPacketId(newCounter); + estimateTos(pack); + + return newCounter; +} + +/*! \brief Calculate the new large sample time value based on \a frameTime and the \a lastTime + \details Wraparound is at 864000000 (1 day @ 10kHz) + \param[in] frameTime The frame time + \param[in] lastTime The last time + \returns The computed packet counter value + \note If lastTime < 0, returns frameTime +*/ +int64_t PacketStamper::calculateLargeSampleTime(int64_t frameTime, int64_t lastTime) +{ + if (lastTime < 0) + return frameTime; + + int64_t low = lastTime % 864000000; + int64_t dt = frameTime - low; + if (dt < (-864000000 / 2)) + return lastTime + dt + 864000000; // positive wraparound + if (dt < (864000000 / 2)) + return lastTime + dt; // normal increment + + return lastTime + dt - 864000000; // negative wraparound +} + +/*! \brief Estimate the time of sampling for the supplied packet \a pack and update it */ +void PacketStamper::estimateTos(XsDataPacket& pack) +{ + if (pack.containsSampleTime64()) + pack.setEstimatedTimeOfSampling(XsTimeStamp((int64_t) pack.sampleTime64())); + else + pack.setEstimatedTimeOfSampling(estimateTosInternal(pack.packetId(), pack.timeOfArrival().msTime())); +} + +/*! \brief Estimate the clock parameters based on the available data points + \details Uses a least-square estimation to fit a line through the known dataset + and then shifts the line down to ensure the TOA >= ETOS constraint holds +*/ +void PacketStamper::estimateClockParameters() +{ + // now we need to find the most consistent rate by doing a least square best fit + // which we then shift down to match the fastest toa + double avgPid = 0.0; + double avgToa = 0.0; + + // if we have enough data we exclude the last item from the averages since it is volatile + auto last = *m_dataPoints.rbegin(); + bool popit = (m_dataPoints.size() >= 5); + if (popit) + m_dataPoints.pop_back(); + for (auto const& d : m_dataPoints) + { + avgPid += d.m_pid; + avgToa += d.m_toa; + } + avgPid /= m_dataPoints.size(); + avgToa /= m_dataPoints.size(); + + double fracTop = 0.0, fracBot = 0.0; + for (auto const& d : m_dataPoints) + { + double dpid = d.m_pid - avgPid; + double dtoa = d.m_toa - avgToa; + fracTop += dpid * dtoa; + fracBot += dpid * dpid; + } + m_rate = fracTop / fracBot; + m_toa0 = avgToa - m_rate * avgPid; + + // put last item back + if (popit) + m_dataPoints.push_back(last); + + // shift down + for (auto const& d : m_dataPoints) + { + double diff = d.m_pid * m_rate + m_toa0 - d.m_toa; + if (diff > 0.0) + m_toa0 -= diff; + } +} + +/*! \brief Remove the worst outlier from the known data points. + \details Only items that are more than the estimated rate off from the current estimation + are considered outliers + \return true if anything was rejected, false otherwise +*/ +bool PacketStamper::rejectOutlier() +{ + auto reject = m_dataPoints.end(); + double diffMin = 0.0; + for (auto d = m_dataPoints.begin(); d != m_dataPoints.end(); ++d) + { + double diff = d->m_pid * m_rate + m_toa0 - d->m_toa; + if (diff < -m_rate && diff < diffMin) + { + diffMin = diff; + reject = d; + } + } + if (reject != m_dataPoints.end()) + { + m_dataPoints.erase(reject); + return true; + } + return false; +} + +/*! \brief Estimate the time of sampling for the supplied \a pid + \details This function will estimate the time of samplinmg based on the supplied PID. If both + PID and TOA are acceptable values, they will be used to update the estimation parameters. + + The current algorithm expects the values to be estimated to be constant for the lifetime of the + object. + + \param pid The packet ID to estimate the time of sampling for. + \param toa The time of arrival associated with this pid + \return The estimated time of sampling. +*/ +int64_t PacketStamper::estimateTosInternal(int64_t pid, int64_t toa) +{ + if (m_dataPoints.size() < 2) + { + if (m_dataPoints.empty()) + { + m_linearize = DataPair{pid, toa}; + m_dataPoints.push_back(DataPair{0, 0}); + m_toa0 = 0; + m_rate = 0; + m_rejectionCountdown = 0; + } + else if (pid > m_latest.m_pid && toa > m_latest.m_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_dataPoints.push_back(last); + } + m_latest = DataPair{pid, toa}; // non-linearized values! + return toa; + } + if (pid > m_latest.m_pid && toa > m_latest.m_toa) + { + // we might do an update + while (pid - m_latest.m_pid == 1) // 'while' so we can break out of this scope if necessary + { + // do sanity check on the data point before adding it + bool enough = (m_dataPoints.size() >= 5 && toa - m_linearize.m_toa >= 1000); + 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) + { + // 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 + m_rejectionCountdown = 5; + break; + } + } + if (m_rejectionCountdown > 0) + { + --m_rejectionCountdown; + break; + } + + // 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 + toa line spanned by the neighbouring points + */ + if (enough) + { + auto next = m_dataPoints.begin(); + auto prev = next++; + 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 itoa = (it->m_pid - prev->m_pid) * rate + prev->m_toa; + if ((double) it->m_toa >= itoa) + { + // useless data point, discard + it = m_dataPoints.erase(it); + if (it == m_dataPoints.end()) + break; + prev = it; + --prev; + } + else + { + // useful data point, keep it + prev = it++; + if (it == m_dataPoints.end()) + break; + } + next = it; + ++next; + } + } + + // forget too old data if we have enough data left afterwards + estimateClockParameters(); + if (enough && m_dataPoints.size() >= 16) + { + bool reestimate = rejectOutlier(); + if (m_dataPoints.size() >= 16) + { + auto it = m_dataPoints.begin(); + ++it; + if ((toa - m_linearize.m_toa) - it->m_toa >= 30000) + { + m_dataPoints.pop_front(); + reestimate = true; + } + } + if (reestimate) + estimateClockParameters(); + } + break; + } + m_latest = DataPair{pid, toa}; + } + + // estimate tos from last known values + return std::min(toa, XsMath_doubleToInt64((pid - m_linearize.m_pid) * m_rate + m_toa0) + m_linearize.m_toa); +} diff --git a/extern/xspublic/xscontroller/packetstamper.h b/extern/xspublic/xscontroller/packetstamper.h new file mode 100644 index 0000000..8ad93e0 --- /dev/null +++ b/extern/xspublic/xscontroller/packetstamper.h @@ -0,0 +1,82 @@ + +// Copyright (c) 2003-2022 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 PACKETSTAMPER_H +#define PACKETSTAMPER_H + +#include +#include + +struct XsDataPacket; + +class PacketStamper +{ +public: + PacketStamper(); + void resetTosEstimation(); + + static const int64_t AWINDABOUNDARY; + static const int64_t MTSCBOUNDARY; + static const int64_t SC8BOUNDARY; + + static int64_t calculateLargePacketCounter(int64_t frameCounter, int64_t lastCounter, int64_t boundary); + static int64_t calculateLargeSampleTime(int64_t frameTime, int64_t lastTime); + int64_t stampPacket(XsDataPacket& pack, XsDataPacket const& highest); + +protected: + /*! Holds a data point for the clock estimation algorithm */ + struct DataPair + { + int64_t m_pid; //!< Packet ID of data item + int64_t m_toa; //!< Time Of Arrival of data item + + //! Returns true if the items are equal + bool operator == (DataPair const& other) const + { + return m_pid == other.m_pid && m_toa == other.m_toa; + } + }; + DataPair m_latest; //!< Latest known data (later data may arrive with a lower pid, which will not be put in this item) + DataPair m_linearize; //!< The very first item received, used to normalize to 0,0 so we have less computational issues with large numbers + std::list m_dataPoints; //!< The filtered history of interesting data items + + double m_toa0; //!< The recomputed Time Of Arrival of PID 0 + double m_rate; //!< The estimated clock rate per pid + int m_rejectionCountdown; //!< A countdown value that is used after the input sanity check rejects input to reject subsequent samples as well. + + void estimateTos(XsDataPacket& pack); + int64_t estimateTosInternal(int64_t pid, int64_t toa); + void estimateClockParameters(); + bool rejectOutlier(); +}; + +#endif diff --git a/extern/xspublic/xscontroller/protocolhandler.cpp b/extern/xspublic/xscontroller/protocolhandler.cpp new file mode 100644 index 0000000..4a5d89d --- /dev/null +++ b/extern/xspublic/xscontroller/protocolhandler.cpp @@ -0,0 +1,263 @@ + +// Copyright (c) 2003-2022 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 "protocolhandler.h" +#include +#include +#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 +#endif + +/*! \class ProtocolHandler + \brief Message protocol handling class + + This class' purpose is to get valid messages according to its protocol from the raw + data that is supplied to it. The default implementation (ProtocolHandler) implements + the Xsens message protocol. To use a different protocol, overload the findMessage function. + + The class is intended to be state-less with respect to the data it handles. +*/ + +//! Default constructor +ProtocolHandler::ProtocolHandler() + : m_ignoreMaxMsgSize(false) +{} + +//! Destructor +ProtocolHandler::~ProtocolHandler() +{} + +/*! \brief Compute the expected message size given a possibly incomplete message + \details When analyzing a message with incomplete size information, the function + will return the minimum size of the message given the information that is available +*/ +static int expectedMessageSize(const unsigned char* buffer, int sz) +{ + const XsMessageHeader* hdr = (const XsMessageHeader*) buffer; + if (sz < 4) + return XS_LEN_MSGHEADERCS; // no size information available at all, return a minimum message + + // we have size information + if (hdr->m_length == XS_EXTLENCODE) + { + 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_MSGHEADERCS + (int)(hdr->m_length); +} + +/*! \brief Write the contents of a uint8 buffer to string as hex characters */ +inline std::string dumpBuffer(const uint8_t* buff, XsSize sz) +{ +#ifdef DUMP_BUFFER_ON_ERROR + std::ostringstream ostr; + ostr << std::hex << std::setfill('0'); +#if DUMP_BUFFER_ON_ERROR > 0 + sz = std::min(sz, DUMP_BUFFER_ON_ERROR); +#endif + for (XsSize i = 0; i < sz; ++i) + ostr << " " << std::setw(2) << (int) buff[i]; + return ostr.str(); +#else + return std::string(); +#endif +} + +/*! \copydoc IProtocolHandler::findMessage +*/ +MessageLocation ProtocolHandler::findMessage(XsProtocolType& type, const XsByteArray& raw) const +{ + JLTRACEG("Entry"); + type = static_cast(ProtocolHandler::type()); + MessageLocation rv; + + int bufferSize = (int)raw.size(); + if (bufferSize == 0) + return rv; + + const unsigned char* buffer = raw.data(); + + // loop through the buffer to find a preamble + for (int pre = 0; pre < bufferSize; ++pre) + { + if (buffer[pre] == XS_PREAMBLE) + { + JLTRACEG("Preamble found at " << pre); + int remaining = bufferSize - pre; // remaining bytes in buffer INCLUDING preamble + + if (remaining < XS_LEN_MSGHEADERCS) + { + JLTRACEG("Not enough header data read"); + if (rv.m_incompletePos == -1) + { + rv.m_incompletePos = pre; + rv.m_incompleteSize = XS_LEN_MSGHEADERCS; + } + break; + } + + // read header + const uint8_t* msgStart = &(buffer[pre]); + const XsMessageHeader* hdr = (const XsMessageHeader*)msgStart; + if (hdr->m_busId == 0 && hdr->m_messageId == 0) + { + // found 'valid' message that isn't actually valid... happens inside GPS raw data + // skip to next preamble + // and completely ignore this message, since it cannot be valid + //JLDEBUGG("Found invalid valid message"); + continue; + } + + // check the reported size + int target = expectedMessageSize(&buffer[pre], remaining); + + JLTRACEG("Bytes in buffer=" << remaining << ", full target = " << target); + + if (!m_ignoreMaxMsgSize && target > (XS_LEN_MSGEXTHEADERCS + XS_MAXDATALEN)) + { + // skip current preamble + JLALERTG("Invalid message length: " << target); + continue; + } + + if (remaining < target) + { + // not enough data read, skip current preamble + JLTRACEG("Not enough data read: " << remaining << " / " << target); + if (rv.m_incompletePos == -1) + { + rv.m_incompletePos = pre; + rv.m_incompleteSize = target; + } + continue; + } + + XsMessage rcv; + // we have read enough data to fulfill our target so we'll try to parse the message + // and check the checksum + if (rcv.loadFromString(msgStart, (uint16_t)target)) + { + JLTRACEG("OK, size = " << (int) rcv.getTotalMessageSize() << " buffer: " << dumpBuffer(msgStart, target)); + rv.m_size = (int) rcv.getTotalMessageSize(); + rv.m_startPos = pre; +#if 0 + JLDEBUGG("OK: rv.m_size = " << rv.m_size << + " rv.m_startPos = " << rv.m_startPos << + " rv.m_incompletePos = " << rv.m_incompletePos << + " pre = " << pre << " msg " << dumpBuffer(msgStart, target) << + " buffer " << dumpBuffer(buffer, bufferSize)); +#endif + break; + } + + // Only alert the checksum error if this is not an embedded message + if (rv.m_incompletePos == -1) + { + JLALERTG( + "Invalid checksum for msg at offset " << pre << " bufferSize = " << bufferSize + << " buffer at offset: " << dumpBuffer(raw.data() + pre, raw.size() - pre)); + } + else + { + JLTRACEG("Invalid checksum, size = " << (int)rcv.getTotalMessageSize() << " buffer: " << dumpBuffer(msgStart, target)); + } + } + } + + JLTRACEG("Exit"); + return rv; +} + +/*! \copydoc IProtocolHandler::convertToMessage +*/ +XsMessage ProtocolHandler::convertToMessage(MessageLocation& location, const XsByteArray& raw) const +{ + XsMessage message; + + const unsigned char* buffer = raw.data(); + const uint8_t* msgStart = &(buffer[location.m_startPos]); + + if (message.loadFromString(msgStart, (uint16_t)location.m_size)) + { + JLTRACEG("OK, size = " << (int)message.getTotalMessageSize() << " buffer: " << dumpBuffer(msgStart, location.m_size)); + location.m_size = (int)message.getTotalMessageSize(); + + return message; + } + + message.clear(); + location.m_startPos = -1; + location.m_incompletePos = -1; + + return message; +} + +/*! \brief Returns the minimum size of a valid message of this protocol including preambles and checksums */ +int ProtocolHandler::minimumMessageSize() const +{ + return XS_LEN_MSGHEADERCS; // minimum size of xsens xbus protocol message +} + +/*! \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 +} + +/*! \brief Compose a message for transmission + \param raw The raw byte array to be constructed from the message + \param msg The message to translate into a raw byte array + \returns The size of the generated byte array +*/ +int ProtocolHandler::composeMessage(XsByteArray& raw, const XsMessage& msg) +{ + if (msg.getTotalMessageSize() < 5) // minimum size of an xsens message including envelope is 5 bytes + return -1; + + raw.assign(msg.getTotalMessageSize(), msg.getMessageStart()); + return (int) raw.size(); +} + +int ProtocolHandler::type() const +{ + return XPT_Xbus; +} + +void ProtocolHandler::ignoreMaximumMessageSize(bool ignore) +{ + m_ignoreMaxMsgSize = ignore; +} diff --git a/extern/xspublic/xscontroller/protocolhandler.h b/extern/xspublic/xscontroller/protocolhandler.h new file mode 100644 index 0000000..3c2c248 --- /dev/null +++ b/extern/xspublic/xscontroller/protocolhandler.h @@ -0,0 +1,57 @@ + +// Copyright (c) 2003-2022 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 PROTOCOLHANDLER_H +#define PROTOCOLHANDLER_H + +#include "xscontrollerconfig.h" +#include "iprotocolhandler.h" + +class ProtocolHandler : public virtual IProtocolHandler +{ + bool m_ignoreMaxMsgSize; +public: + ProtocolHandler(); + virtual ~ProtocolHandler(); + + MessageLocation findMessage(XsProtocolType& type, const XsByteArray& raw) const override; + XsMessage convertToMessage(MessageLocation& location, const XsByteArray& raw) const override; + int minimumMessageSize() const override; + int maximumMessageSize() const override; + int type() const override; + static int composeMessage(XsByteArray& raw, const XsMessage& msg); + void ignoreMaximumMessageSize(bool ignore) override; + + XSENS_DISABLE_COPY(ProtocolHandler); +}; + +#endif diff --git a/extern/xspublic/xscontroller/protocolmanager.cpp b/extern/xspublic/xscontroller/protocolmanager.cpp new file mode 100644 index 0000000..f61cefd --- /dev/null +++ b/extern/xspublic/xscontroller/protocolmanager.cpp @@ -0,0 +1,233 @@ + +// Copyright (c) 2003-2022 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 "protocolmanager.h" +#include "iprotocolhandler.h" +#include "communicator.h" +#include + +// Remove windows global min/max macros +#undef min +#undef max + +/*! \class ProtocolManager + \brief Supplies multi-protocol data stream parsing + \details This class provides functionality for dealing with multiple or changing communication + protocols in a data stream. The XCommunicator class uses this class to parse a raw byte stream + into XsMessage objects. +*/ + +/*! \brief Default constructor +*/ +ProtocolManager::ProtocolManager(Communicator const& communicator) + : m_communicator(communicator) +{ +} + +ProtocolManager::~ProtocolManager() throw() +{ +} + +/*! \returns A const interator of the protocol handlers list beginning +*/ +ProtocolManager::const_iterator ProtocolManager::begin() const +{ + return m_protocolHandlers.begin(); +} + +/*! \returns A const interator of the protocol handlers list end +*/ +ProtocolManager::const_iterator ProtocolManager::end() const +{ + return m_protocolHandlers.end(); +} + +/*! \brief Searches for a raw message in a raw data + \param[out] type The protocol type that was used. + \param[in] raw The byte array to search in. + \returns The found message location +*/ +MessageLocation ProtocolManager::findMessage(XsProtocolType& type, const XsByteArray& raw) +{ + MessageLocation bestMessageLocation; + XsProtocolType bestProtocolType = XPT_Xbus; + container_type::iterator bestHandlerIter = m_protocolHandlers.end(); + + for (container_type::iterator i = m_protocolHandlers.begin(); i != m_protocolHandlers.end(); ++i) + { + assert((*i).operator->() != 0); + IProtocolHandler const& handler = **i; + XsProtocolType currentProtocolType = static_cast(handler.type()); + MessageLocation currentMessageLocation = handler.findMessage(type, raw); + + int currentPosition = currentMessageLocation.m_incompletePos >= 0 ? currentMessageLocation.m_incompletePos : currentMessageLocation.m_startPos; + if (currentPosition < 0) + continue; + + int bestPosition = bestMessageLocation.m_incompletePos >= 0 ? bestMessageLocation.m_incompletePos : bestMessageLocation.m_startPos; + if ( (bestPosition < 0) + || (currentPosition < bestPosition) + || (currentPosition == bestPosition && currentMessageLocation.m_startPos >= 0 && (bestMessageLocation.m_startPos < 0 || currentMessageLocation.m_startPos < bestMessageLocation.m_startPos)) + ) + { + // Message is a better match + bestMessageLocation = currentMessageLocation; + bestProtocolType = currentProtocolType; + bestHandlerIter = i; + } + + // Stop searching if the location is as good as it gets + if (bestMessageLocation.m_startPos == 0) + break; + } + + // Move the best handler to the front of the list to speed up future searches + if (bestHandlerIter != m_protocolHandlers.end() && bestHandlerIter != m_protocolHandlers.begin()) + { + value_type bestHandler = *bestHandlerIter; + container_type& cont = m_protocolHandlers; + cont.erase(bestHandlerIter); + cont.push_front(bestHandler); + } + type = bestProtocolType; + return bestMessageLocation; +} + +/*! \brief Converts \a raw data using \a location into a %XsMessage object. + \param[in] type The protocol type to use. + \param[out] location The location of a message to convert from \a raw data. + \param[in] raw The raw byte stream. + \returns A %XsMessage object that was converted from raw byte stream. +*/ +XsMessage ProtocolManager::convertToMessage(XsProtocolType& type, MessageLocation& location, const XsByteArray& raw) +{ + for (auto const& handler : m_protocolHandlers) + if (handler->type() == type) + return handler->convertToMessage(location, raw); + + return XsMessage(); +} + +/*! \brief Removes a protocol handler of a specified type + \param[in] type The type of the protocol handler to remove + \returns True if successful +*/ +bool ProtocolManager::remove(XsProtocolType type) +{ + bool result = false; + container_type::iterator i = m_protocolHandlers.begin(); + 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; + ++i; + m_protocolHandlers.erase(toErase); + result = true; + } + else + ++i; + } + return result; +} + + +/*! \brief Searches the registered protocol handlers for a handler that matches the given protocol type and returns that handler if present +* \param type : The protocol type to search for +* \returns : (shared) pointer to the matching handler, an invalid shared pointer if there is no matching protocol +*/ +ProtocolManager::value_type ProtocolManager::find(XsProtocolType type) +{ + for (auto i : m_protocolHandlers) + { + if (i->type() == type) + return i; + } + return value_type(); +} + + +/*! \returns true when a protocol with type \a type has been added + \param[in] type The protocol type to check +*/ +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) + result = true; + } + return result; +} + + +ProtocolManager::value_type ProtocolManager::add(IProtocolHandler* handler) +{ + assert(handler != 0); + // check for duplicates first + for (container_type::iterator i = m_protocolHandlers.begin(); i != m_protocolHandlers.end(); ++i) + if (handler->type() == (*i)->type()) + return (*i); + + m_protocolHandlers.emplace_back(value_type(handler)); + value_type inserted = m_protocolHandlers.back(); + return inserted; +} + +/*! \brief Clears the protocol handlers list +*/ +void ProtocolManager::clear() +{ + m_protocolHandlers.clear(); +} + +/*! \brief Check a message for a validity + \param[in] message The message to check + \returns True if valid +*/ +bool ProtocolManager::validateMessage(XsMessage const& message) const +{ + return m_communicator.sanityCheck(message); +} + +/*! \returns the minimum message size of the last successful protocol handler +*/ +int ProtocolManager::likelyMinimumMessageSize() const +{ + if (m_protocolHandlers.empty()) + return 0; + return (*m_protocolHandlers.begin())->minimumMessageSize(); +} + diff --git a/extern/xspublic/xscontroller/protocolmanager.h b/extern/xspublic/xscontroller/protocolmanager.h new file mode 100644 index 0000000..eb95d81 --- /dev/null +++ b/extern/xspublic/xscontroller/protocolmanager.h @@ -0,0 +1,91 @@ + +// Copyright (c) 2003-2022 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 PROTOCOLMANAGER_H +#define PROTOCOLMANAGER_H + +#include "messagelocation.h" +#include "iprotocolmanager.h" +#include +#include +#include "xsprotocoltype.h" +#include + +class IProtocolHandler; +struct Communicator; +struct XsByteArray; +struct XsMessage; + + +class ProtocolManager : public IProtocolManager +{ +public: + + //! \brief A typedef for a value type + typedef std::shared_ptr value_type; + + //! \brief A typedef for a container type + typedef std::list container_type; + + //! \brief A typedef for a const iterator + typedef container_type::const_iterator const_iterator; + + explicit ProtocolManager(Communicator const&); + virtual ~ProtocolManager() throw(); + + int likelyMinimumMessageSize() const; + + const_iterator begin() const; + 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; + + /*! \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 find(XsProtocolType type); + virtual bool remove(XsProtocolType type); + virtual bool hasProtocol(XsProtocolType type) const; + virtual void clear(); + +private: + 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 + container_type m_protocolHandlers; +}; + +#endif diff --git a/extern/xspublic/xscontroller/proxycommunicator.cpp b/extern/xspublic/xscontroller/proxycommunicator.cpp new file mode 100644 index 0000000..5b4960e --- /dev/null +++ b/extern/xspublic/xscontroller/proxycommunicator.cpp @@ -0,0 +1,213 @@ + +// Copyright (c) 2003-2022 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 "proxycommunicator.h" +#include "callbackmanagerxda.h" +#include +#include +#include "deviceredetector.h" + + +/*! \class ProxyCommunicator + + The ProxyCommunicator is a communicator that uses an external user defined communication channel. Any data + to be transmitted by XDA to a device is given to the user in a callback (\a onTransmissionRequest) and it is + up to the user how that data is transported. Data coming back from a device is inserted into the ProxyCommunicator + by calling the \a handleReceivedData function. There can be multiple ProxyCommunicators as long as their user-provided + channel identifiers are different. +*/ + +/*! \brief Constructor + \param channelId The user-provided identifier associated with this communicator + \param channelLatency The latency of channel +*/ +ProxyCommunicator::ProxyCommunicator(int channelId, uint32_t channelLatency) + : m_channelId(channelId) + , m_channelLatency(channelLatency) +{ +} + +/*! \brief Destructor +*/ +ProxyCommunicator::~ProxyCommunicator() +{ +} + +/*! \brief Flushes all remaining data on the open port. Has no effect for the ProxyCommunicator +*/ +void ProxyCommunicator::flushPort() +{ + //ProxyCommunicator maintains no stream. No action required +} + +/*! \brief Closes the port +*/ +void ProxyCommunicator::closePort() +{ + m_activePortInfo = XsPortInfo(); +} + +/*! \brief Checks if the associated port is open +*/ +bool ProxyCommunicator::isPortOpen() const +{ + return !m_activePortInfo.portName().empty(); +} + +/*! \brief Opens a proxy port + \param portInfo The port information + \param stage The open port stage + \param detectRs485 Enable more extended scan to detect rs485 devices + \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) +{ + auto result = [this]() + { + bool ok = (lastResult() == XRV_OK); + if (!ok) + prepareForDestruction(); + return ok; + }; + + if (stage & OPS_OpenPort) + { + if (isPortOpen()) + { + setLastResult(XRV_ALREADYOPEN); + return (this->portInfo() == portInfo); + } + } + + if (stage & OPS_InitDevice) + { + if (gotoConfig(detectRs485) != XRV_OK) + return result(); + + if (getDeviceId() != XRV_OK) + return result(); + } + + setLastResult(XRV_OK); + m_activePortInfo = portInfo; + m_activePortInfo.setDeviceId(masterDeviceId()); + return result(); +} + +/*! \brief Closes and tries to reopen the port + \param stage The current stage of port + \param skipDeviceIdCheck If set to true the it will skip device id check + \returns true if the the port could be successfully reopened +*/ +bool ProxyCommunicator::reopenPort(OpenPortStage stage, bool skipDeviceIdCheck) +{ + XsPortInfo pi = portInfo(); + + closePort(); + DeviceRedetector redetector(pi); + if (!redetector.redetect(masterDeviceId(), pi, skipDeviceIdCheck)) + return false; + + return openPort(pi, stage); +} + +/*! \brief Has no effect for the ProxyCommunicator. Always returns false +*/ +bool ProxyCommunicator::isDockedAt(Communicator*) const +{ + return false; +} + +/*! \brief Writes raw data to the communication channel +*/ +XsResultValue ProxyCommunicator::writeRawData(const XsByteArray& data) +{ + onTransmissionRequest(m_channelId, &data); + return XRV_OK; +} + +/*! \brief Handles data received from the communication channel +*/ +void ProxyCommunicator::handleReceivedData(const XsByteArray& data) +{ + addRawData(data); +} + +/*! \brief Write a message and await the reply */ +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&) +{ + //No action required as data is pushed (no polling required) in the buffer by the handleReceivedData function + return XRV_OK; +} + +/*! \brief Handles a message received on the communication channel +*/ +void ProxyCommunicator::handleMessage(const XsMessage& msg) +{ + DeviceCommunicator::handleMessage(msg); +} + +/*! \brief Read all messages from the buffered read data after adding new data supplied in \a rawIn + \param rawIn The byte array with all data + \param messages The message to process + \returns The messages that were read. +*/ +XsResultValue ProxyCommunicator::processBufferedData(const XsByteArray& rawIn, std::deque& messages) +{ + return extractMessages(rawIn, messages); +} + +/*! \brief Creates a default port info object based on the given user-provided channel identifier +*/ +XsPortInfo ProxyCommunicator::createPortInfo(int channelId) +{ + XsPortInfo portInfo; + char tmp[16]; + snprintf(tmp, sizeof(tmp), "PROXY#%d", channelId); + portInfo.setPortName(tmp); + return portInfo; +} + +/*! \brief Returns the default timeout needed for this interface +*/ +uint32_t ProxyCommunicator::defaultInterfaceTimeout() const +{ + return m_channelLatency; +} diff --git a/extern/xspublic/xscontroller/proxycommunicator.h b/extern/xspublic/xscontroller/proxycommunicator.h new file mode 100644 index 0000000..586c496 --- /dev/null +++ b/extern/xspublic/xscontroller/proxycommunicator.h @@ -0,0 +1,82 @@ + +// Copyright (c) 2003-2022 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 PROXYCOMMUNICATOR_H +#define PROXYCOMMUNICATOR_H + +#include "serialcommunicator.h" +#include "dataparser.h" +#include + +class CallbackManagerXda; +class ProxyCommunicator : public SerialCommunicator +{ +public: + ProxyCommunicator(int channelId, uint32_t channelLatency); + ~ProxyCommunicator() override; + + XsResultValue readDataToBuffer(XsByteArray& raw) override; + XsResultValue processBufferedData(const XsByteArray& rawIn, std::deque& messages) 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 reopenPort(OpenPortStage stage = OPS_Full, bool skipDeviceIdCheck = false) override; + 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; + + 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(); + } + + uint32_t defaultInterfaceTimeout() const override; + +private: + int m_channelId; + uint32_t m_channelLatency; +}; + +#endif diff --git a/extern/xspublic/xscontroller/rangequeue.h b/extern/xspublic/xscontroller/rangequeue.h new file mode 100644 index 0000000..988403c --- /dev/null +++ b/extern/xspublic/xscontroller/rangequeue.h @@ -0,0 +1,304 @@ + +// Copyright (c) 2003-2022 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 RANGE_QUEUE_H +#define RANGE_QUEUE_H + +#include +#include + +/*! \class RangeQueue + \brief A range queue +*/ +template +class RangeQueue +{ +public: + /*! \brief Default constructor + */ + RangeQueue() : m_count(0) + { + } + + virtual ~RangeQueue() + { + } + + /*! \brief Clears the queue + */ + void clear() + { + xsens::Lock locky(&m_mutex); + m_queue.clear(); + m_count = 0; + } + + /*! \brief Adds a range to the back of the queue + Only a range newer than the most recent range in the queue is added. Older (also partial) are ignored + Also \a end must be larger or equal than \a start + \param start : The first index of the range + \param end: The last index of the range + */ + void pushBack(T start, T end) + { + if (end < start) + return; + + xsens::Lock locky(&m_mutex); + if (!empty() && (start <= last())) + return; + + Range newRange(start, end); + JLTRACEG("Adding range: [" << start << " - " << end << "]"); + m_queue.push_back(newRange); + m_count += 1 + end - start; + } + + /*! \brief Removes a specific index from the queue + \param index The index to remove + \details This function removes the given index from the queue by splitting the containing range at the \a index + */ + void remove(T index) + { + xsens::Lock locky(&m_mutex); + if (empty()) + return; + + for (auto r = m_queue.rbegin(); r != m_queue.rend(); ++r) + { + 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); + r->m_start = index + 1; + m_queue.insert(--(r.base()), split); + } + return; + } + } + + /*! \brief Removes all the indices upto and including \a upto from the queue + */ + void popFront(T upto) + { + xsens::Lock locky(&m_mutex); + + while (!m_queue.empty() && (m_queue.front().m_end <= upto)) + { + auto const& i = m_queue.front(); + m_count -= (i.m_end - i.m_start) + 1; + m_queue.pop_front(); + } + + if (empty()) + { + JLDEBUGG("Removed upto: " << upto << ", now empty"); + m_count = 0; + return; + } + if (m_queue.front().m_start <= upto) + { + m_count -= 1 + upto - m_queue.front().m_start; + m_queue.front().m_start = upto + 1; + } + JLDEBUGG("Removed upto: " << upto << ", new first = " << m_queue.front().m_start); + } + + /*! \brief Removes all the indices upto and including \a upto from the queue + */ + void popBack(T upto) + { + xsens::Lock locky(&m_mutex); + + while (!m_queue.empty() && (m_queue.back().m_start >= upto)) + { + auto const& i = m_queue.back(); + m_count -= (i.m_end - i.m_start) + 1; + m_queue.pop_back(); + } + + if (empty()) + { + JLDEBUGG("Removed upto: " << upto << ", now empty"); + m_count = 0; + return; + } + if (m_queue.back().m_end >= upto) + { + m_count -= 1 + m_queue.back().m_end - upto; + m_queue.back().m_end = upto - 1; + } + JLDEBUGG("Removed upto: " << upto << ", new last = " << m_queue.back().m_end); + } + + /*! \brief Returns the first index of the first range in the queue + */ + T first() const + { + xsens::Lock locky(&m_mutex); + if (empty()) + return illegalIndex(); + return m_queue.front().m_start; + } + + /*! \brief Returns the last (highest) index in the queue + */ + T last() const + { + xsens::Lock locky(&m_mutex); + if (empty()) + return illegalIndex(); + + return m_queue.back().m_end; + } + + /*! \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 + - ... + */ + const T operator[](std::size_t index) const + { + xsens::Lock locky(&m_mutex); + + std::size_t range = index / 2; + uint32_t end = index % 2; + + if (m_queue.size() < range) + return illegalIndex(); + + auto it = m_queue.begin(); + + for (uint32_t i = 0; i < range; ++i) + ++it; + + if (end) + return it->m_end; + else + return it->m_start; + } + + /*! \brief Checks if the given index is contained in the queue + */ + bool contains(uint32_t index) const + { + 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)) + return true; + } + return false; + } + + /*! \brief Returns the total number of indices captured by the ranges in the queue + */ + T count() const + { + xsens::Lock locky(&m_mutex); + return m_count; + } + + /*! \brief Returns the recounted total number of indices captured by the ranges in the queue*/ + T recount() + { + 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; + } + + /*! \brief Returns true if the queue is empty + */ + bool empty() const + { + return m_queue.empty(); + } + + /*! \brief Returns the value for an illegal index + */ + static const T illegalIndex() + { + 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. + */ + void copy(RangeQueue& destination, T start, T end) + { + xsens::Lock locky(&m_mutex); + + 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)); + } + } + +private: + struct Range + { + T m_start; + T m_end; + Range(T start, T end) + : m_start(start) + , m_end(end) + {} + }; + T m_count; + + std::list m_queue; + mutable xsens::Mutex m_mutex; +}; + +#endif diff --git a/extern/xspublic/xscontroller/replymonitor.cpp b/extern/xspublic/xscontroller/replymonitor.cpp new file mode 100644 index 0000000..a8ef32e --- /dev/null +++ b/extern/xspublic/xscontroller/replymonitor.cpp @@ -0,0 +1,148 @@ + +// Copyright (c) 2003-2022 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 "replymonitor.h" +#include "replyobject.h" +#include + +namespace xsens +{ + +/*! \class ReplyObjectDeleter + \brief A class that deletes a reply object and removes it from the given monitor +*/ +class ReplyObjectDeleter +{ +public: + // create a reply object deleter + 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 + { + if (m_monitor) + m_monitor->removeObject(p); + delete p; + } +private: + ReplyObjectRemover* m_monitor; +}; + + +/*! \class ReplyMonitor + \brief A monitor class for receiving replies messages in a thread + \details This class monitors for a desired message, and releases a semaphore when the message is received +*/ + +/*! \brief Default constructor */ +ReplyMonitor::ReplyMonitor(void) +{ +} + +ReplyMonitor::~ReplyMonitor(void) +{ +} + +/*! \brief Add a reply object to the reply monitor + \param[in] replyObject the ReplyObject to add + \returns a shared pointer to this object +*/ +std::shared_ptr ReplyMonitor::addReplyObject(ReplyObject* replyObject) +{ + xsens::Lock locky(&m_mutex); + m_objectList.push_back(replyObject); + return std::shared_ptr(replyObject, ReplyObjectDeleter(this)); +} + +/*! \brief Remove an object from the list + \param obj The shared pointer to the object to remove + \note This does not delete the object +*/ +void ReplyMonitor::removeObject(std::shared_ptr const& obj) +{ + removeObject(obj.get()); +} + +/*! \brief Remove an object from the list + \param obj The object to remove + \note This does not delete the object +*/ +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); + if (it == m_objectList.end()) + return; + + m_objectList.erase(it); +} + +/*! \brief Put a reply in the monitor + \param message The message to add as a reply + \returns true if the message is delivered +*/ +bool ReplyMonitor::addReply(const XsMessage& message) +{ + xsens::Lock locky(&m_mutex); + size_t numElements = m_objectList.size(); + for (size_t i = 0; i < numElements; i++) + { + if (m_objectList[i]->isReplyFor(message)) + { + ReplyObject* tmp = m_objectList[i]; + m_objectList.erase(m_objectList.begin() + (ptrdiff_t) i); + tmp->setMessage(message); + return true; + } + } + return false; +} + +/*! \brief Dumps the current list of objects to wait for to the supplied journaller +*/ +void ReplyMonitor::dumpObjectList(Journaller* journal, JournalLogLevel level) const +{ + xsens::Lock locky(&m_mutex); + 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/extern/xspublic/xscontroller/replymonitor.h b/extern/xspublic/xscontroller/replymonitor.h new file mode 100644 index 0000000..499ae6d --- /dev/null +++ b/extern/xspublic/xscontroller/replymonitor.h @@ -0,0 +1,78 @@ + +// Copyright (c) 2003-2022 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 REPLYMONITOR_H +#define REPLYMONITOR_H + +#include +#include +#include + +#include + +struct XsMessage; +class ReplyObject; + +namespace xsens +{ + +/*! \class ReplyObjectRemover + \brief A class that removes a reply object from the given monitor +*/ +class ReplyObjectRemover +{ +public: + //! \copybrief ReplyMonitor::removeObject + virtual void removeObject(ReplyObject* obj) = 0; + virtual ~ReplyObjectRemover() {} +}; + +class ReplyMonitor : private ReplyObjectRemover +{ +public: + ReplyMonitor(); + virtual ~ReplyMonitor(); + + 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); + + std::vector m_objectList; + mutable Mutex m_mutex; +}; +} // namespace xsens + +#endif diff --git a/extern/xspublic/xscontroller/replyobject.cpp b/extern/xspublic/xscontroller/replyobject.cpp new file mode 100644 index 0000000..c206cd3 --- /dev/null +++ b/extern/xspublic/xscontroller/replyobject.cpp @@ -0,0 +1,196 @@ + +// Copyright (c) 2003-2022 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 "replyobject.h" +#include "replymonitor.h" +#include +#include + +using namespace xsens; + +/*! \brief Default constructor */ +ReplyObject::ReplyObject() + : m_mutex(new Mutex) + , m_waitCondition(new WaitCondition(*m_mutex)) + , m_delivered(false) +{ +} + +ReplyObject::~ReplyObject() +{ + try + { + delete m_waitCondition; + delete m_mutex; + } + catch (...) + { + } +} + +/*! \brief Sets a message as reply message and trigger the semaphore which will unblock any waiting message() calls +*/ +void ReplyObject::setMessage(const XsMessage& msg) +{ + xsens::Lock locker(m_mutex); + + m_message = msg; + m_delivered = true; + m_waitCondition->signal(); +} + +/*! \brief Blocks until a message has been set by setMessage() then returns that message +*/ +XsMessage ReplyObject::message(uint32_t timeout) +{ + xsens::Lock locker(m_mutex); + + if (!m_delivered) + m_waitCondition->wait(timeout); + if (m_delivered) + return m_message; + else + return XsMessage(); +} + +/*! \brief MidReplyObject constructor + \param[in] messageId the id of the message to wait for +*/ +MidReplyObject::MidReplyObject(uint8_t messageId) + : ReplyObject() + , m_messageId(messageId) +{ +} + +/*! \brief MidReplyObject destructor +*/ +MidReplyObject::~MidReplyObject() +{ +} + +/*! \returns the message ID that this reply object is waiting for +*/ +uint8_t MidReplyObject::msgId() const +{ + return m_messageId; +} + +/*! \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) +{ + if (m_messageId == msg.getMessageId()) + return true; + + if (msg.getMessageId() == XMID_Error) + { + assert(msg.getDataSize()); + return (static_cast(msg.getDataByte()) != XRV_DATAOVERFLOW); + } + return false; +} + +/*! \brief MidAndDataReplyObject constructor + \param[in] messageId the message id of the message to wait for + \param[in] offset the offset in the data part of the message + \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) + : ReplyObject() + , m_messageId(messageId) + , m_dataOffset(offset) + , m_dataSize(size) + , m_data(0) +{ + assert(m_dataSize > 0); + setData(data); +} + +/*! \brief MidAndDataReplyObject destructor +*/ +MidAndDataReplyObject::~MidAndDataReplyObject() +{ + try + { + freeData(); + } + catch (...) + { + assert(false); + } +} + +/*! \returns the message ID that this reply object is waiting for +*/ +uint8_t MidAndDataReplyObject::msgId() const +{ + return m_messageId; +} + +/*! \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) +{ + if (msg.getMessageId() == XMID_Error) + return true; + if (m_messageId != msg.getMessageId()) + return false; + return memcmp(msg.getDataBuffer(m_dataOffset), m_data, m_dataSize) == 0; +} + +/*! \brief Frees allocated data (if any) +*/ +void MidAndDataReplyObject::freeData() +{ + if (m_data != 0) + { + free(m_data); + m_data = 0; + } +} + +/*! \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) +{ + freeData(); + if (data != 0) + { + m_data = reinterpret_cast(malloc(m_dataSize)); + if (m_data != 0) + memcpy(m_data, data, m_dataSize); + } +} diff --git a/extern/xspublic/xscontroller/replyobject.h b/extern/xspublic/xscontroller/replyobject.h new file mode 100644 index 0000000..728bc1d --- /dev/null +++ b/extern/xspublic/xscontroller/replyobject.h @@ -0,0 +1,108 @@ + +// Copyright (c) 2003-2022 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 REPLYOBJECT_H +#define REPLYOBJECT_H + +#include +#include + +namespace xsens +{ +class ReplyMonitor; +class Mutex; +class WaitCondition; +} + +/*! \class ReplyObject + \brief Abstract reply object. Blocks on a semaphore when requesting the message until the message has been set by the reply monitor. +*/ +class ReplyObject +{ +public: + explicit ReplyObject(); + virtual ~ReplyObject(); + + void setMessage(const XsMessage& msg); + XsMessage message(uint32_t timeout); + + /*! \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; + + //! \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; + XsMessage m_message; + bool m_delivered; +}; + +/*! \class MidReplyObject + \brief Reply object that only checks the message identifier. +*/ +class MidReplyObject : public ReplyObject +{ +public: + MidReplyObject(uint8_t messageId); + ~MidReplyObject(void); + + virtual bool isReplyFor(XsMessage const& message); + virtual uint8_t msgId() const; +private: + uint8_t m_messageId; +}; + +/*! \class MidAndDataReplyObject + \brief Reply object that checks the message identifier and data in the data field +*/ +class MidAndDataReplyObject : public ReplyObject +{ +public: + 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); + virtual uint8_t msgId() const; +private: + void freeData(); + uint8_t m_messageId; + XsSize m_dataOffset; + XsSize m_dataSize; + uint8_t* m_data; +}; + +#endif + diff --git a/extern/xspublic/xscontroller/restorecommunication.cpp b/extern/xspublic/xscontroller/restorecommunication.cpp new file mode 100644 index 0000000..8df1f4c --- /dev/null +++ b/extern/xspublic/xscontroller/restorecommunication.cpp @@ -0,0 +1,175 @@ + +// Copyright (c) 2003-2022 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 "restorecommunication.h" +#include "xscontrol_def.h" +#include "serialinterface.h" +#include + + +/*! \class RestoreCommunication + \brief Performs restore communication procedure with a provided COM port. + \details When started it will repeatedly send the reset byte (0xDE) at 115k2 until a specific answer is received. + \note The procedure is executed on a separate thread and call a callback on completion/error. +*/ + +/*! \brief Constructor. + \details Uses XsControl object to call a callback and creates a serial interface. + \param control A pointer to XsControl object. +*/ +RestoreCommunication::RestoreCommunication(XsControl* control) + : m_control(control) + , m_serialInterface(new SerialInterface()) + , m_isRestoring(false) +{ +} + +/*! \brief Destructor. + \details Stops restoring the communication before destruction. +*/ +RestoreCommunication::~RestoreCommunication() +{ + try + { + stop(); + } + catch (...) + { + } + delete m_serialInterface; +} + +/*! \brief Starts the restore communication procedure. + \details Tries to open COM port and starts a thread for the execution. + \param portName A string with a port name. + \returns XRV_OK if the procedure started successfully +*/ +XsResultValue RestoreCommunication::start(const XsString& portName) +{ + m_portName = portName; + XsPortInfo portInfo(portName, XBR_115k2); + XsResultValue result; + if (!m_isRestoring) + { + result = openComPort(portInfo); + if (result == XRV_OK) + { + if (!startThread("RestoreCommunication")) + result = XRV_ERROR; + else + m_isRestoring = true; + } + } + else + { + result = XRV_ERROR; + stop(); + } + return result; +} + +/*! \brief Stops the restore communication procedure. + \details Stops the thread and closes a serial interface. +*/ +void RestoreCommunication::stop() +{ + m_isRestoring = false; + m_serialInterface->close(); +} + +/*! \brief Inner function for an execution on a thread. + \returns 0. +*/ +int32_t RestoreCommunication::innerFunction(void) +{ + m_serialInterface->setTimeout(0); + + bool success = false; + const size_t readDataSize = 4; + unsigned char resetByte = (unsigned char)0xDE; // reset byte + unsigned char preambleBuf[1] = { 0 }; + unsigned char readBuf[readDataSize] = { 0, 0, 0, 0 }; + + + XsByteArray resetByteArray(1, &resetByte); + XsByteArray preambleArray(1, &preambleBuf[0]); + XsByteArray readArray(readDataSize, &readBuf[0]); + + while (!success && m_isRestoring) + { + XsResultValue writeResult = m_serialInterface->writeData(resetByteArray); + XsResultValue readResult = m_serialInterface->readData(1, preambleArray); + + if (writeResult == XRV_OK && readResult == XRV_OK) + { + if (preambleArray[0] == XS_PREAMBLE) + success = true; + } + + XsTime::udelay(100); + } + + bool stoppedByUser = !success; + + if (!stoppedByUser) + { + m_serialInterface->setTimeout(1000); + m_serialInterface->readData((XsFilePos) readDataSize, readArray); + bool restoreOk = (preambleArray[0] == XS_PREAMBLE && readArray[0] == 0xFF && readArray[1] == 0x3E && readArray[2] == 0x00 && readArray[3] == 0xC3); + + XsResultValue result = restoreOk ? XRV_OK : XRV_RESTORE_COMMUNICATION_FAILED; + m_control->onRestoreCommunication(&m_portName, result); + } + else + m_control->onRestoreCommunication(&m_portName, XRV_RESTORE_COMMUNICATION_STOPPED); + + m_isRestoring = false; + stopThread(); + return 0; +} + +/*! \brief Tries to open COM port. + \details If the port was open it will retry. + \param portInfo A port info to open. + \return XRV_OK if was successful. +*/ +XsResultValue RestoreCommunication::openComPort(const XsPortInfo& portInfo) +{ + XsResultValue result = m_serialInterface->open(portInfo); + if (result == XRV_ALREADYOPEN) + { + result = m_serialInterface->close(); + if (result == XRV_OK) + result = m_serialInterface->open(portInfo); + } + return result; +} diff --git a/extern/xspublic/xscontroller/restorecommunication.h b/extern/xspublic/xscontroller/restorecommunication.h new file mode 100644 index 0000000..b22f1bf --- /dev/null +++ b/extern/xspublic/xscontroller/restorecommunication.h @@ -0,0 +1,65 @@ + +// Copyright (c) 2003-2022 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 XSRESTORECOMMUNICATION_H +#define XSRESTORECOMMUNICATION_H + +#include "callbackmanagerxda.h" +#include + +struct XsControl; +struct XsPortInfo; +class SerialInterface; + +class RestoreCommunication : public CallbackManagerXda, protected xsens::StandardThread +{ +public: + RestoreCommunication(XsControl* control); + ~RestoreCommunication(); + + XsResultValue start(const XsString& portName); + void stop(); + +protected: + int32_t innerFunction(void) override; + +private: + XsResultValue openComPort(const XsPortInfo& portInfo); + + XsControl* m_control; + SerialInterface* m_serialInterface; + XsString m_portName; + + volatile std::atomic m_isRestoring; +}; + +#endif diff --git a/extern/xspublic/xscontroller/rx_tx_log.h b/extern/xspublic/xscontroller/rx_tx_log.h new file mode 100644 index 0000000..9de7651 --- /dev/null +++ b/extern/xspublic/xscontroller/rx_tx_log.h @@ -0,0 +1,178 @@ + +// Copyright (c) 2003-2022 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 RX_TX_LOG_H +#define RX_TX_LOG_H + +#if defined(XSENS_DEBUG) + //// Debug or RelWithDeb build + //#define LOG_RX_TX // Lowest level byte receive and send (binary log) + //#define LOG_RX_TX_UNIQUE // Use unique file names for rx/tx logs + //#define LOG_RX_TX_FLUSH // Flush after each log operation (can cause hickups in timing, 300ms is not unheard of) + //#define LOG_RX_TX_PER_STATE // Use new unique log file after switching to a new state (config/measurement/operational/recording) for rx/tx logs, override LOG_RX_TX_UNIQUE, automatically disabled if LOG_RX_TX is disabled +#endif + +#ifdef LOG_RX_TX +#if defined(LOG_RX_TX_PER_STATE) || defined(LOG_RX_TX_UNIQUE) +#include +#include + +/*! \brief Helper function for making filename of log file unique +*/ +inline static void makeFilenameUnique(char* filename, char const* state) +{ +#if defined(LOG_RX_TX_PER_STATE) || defined(LOG_RX_TX_UNIQUE) + char basename[XS_MAX_FILENAME_LENGTH]; + strcpy(basename, filename); + basename[strlen(basename) - 4] = 0; // remove .log extension + sprintf(filename, "%s_%" PRINTF_INT64_MODIFIER "u%s.log", basename, XsTimeStamp::nowMs(), state); +#else + (void)filename; +#endif +} + + +#ifdef LOG_RX_TX_PER_STATE +inline static void checkStateRx(char* state, int length, XsByteArray const& data, XsFile& rx_log) +{ + state[0] = 0; + if (length >= 4) + { + // find preamble + int idx = 0; + while (idx < length - 4) + { + if (data[idx] == 0xFA && (data[idx + 1] == 0xFF || data[idx + 1] == 0) && data[idx + 3] == 0) + { + 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; + } + break; + } + ++idx; + } + } + if (rx_log.isOpen() && state[0] != 0) + { + rx_log.flush(); + rx_log.close(); + } +} +inline static void checkStateRx(char* state, int length, void const* data, XsFile& rx_log) +{ + XsByteArray tmp((uint8_t*) data, length); + checkStateRx(state, length, tmp, rx_log); +} + +#define CHECK_STATE_RX(length, data, logfile) \ + char state[16] = "";\ + checkStateRx(state, (int) (length), (data), (logfile)) + +inline static void checkStateTx(char* state, int length, XsByteArray const& data, XsFile& tx_log) +{ + state[0] = 0; + if (length >= 4) + { + // find preamble + int idx = 0; + while (idx < length - 4) + { + if (data[idx] == 0xFA && (data[idx + 1] == 0xFF || data[idx + 1] == 0) && data[idx + 3] == 0) + { + 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; + } + break; + } + ++idx; + } + } + if (tx_log.isOpen() && state[0] != 0) + { + tx_log.flush(); + tx_log.close(); + } +} +inline static void checkStateTx(char* state, int length, void const* data, XsFile& tx_log) +{ + XsByteArray tmp((uint8_t*) data, length); + checkStateTx(state, length, tmp, tx_log); +} +#define CHECK_STATE_TX(length, data, logfile) \ + char state[16] = "";\ + checkStateTx(state, (int) (length), (data), (logfile)) + +#else +static const char state[1] = ""; +#define CHECK_STATE_RX(...) ((void)0) +#define CHECK_STATE_TX(...) ((void)0) +#endif + +#endif +#endif +#endif diff --git a/extern/xspublic/xscontroller/scanner.cpp b/extern/xspublic/xscontroller/scanner.cpp new file mode 100644 index 0000000..9064bc7 --- /dev/null +++ b/extern/xspublic/xscontroller/scanner.cpp @@ -0,0 +1,983 @@ + +// Copyright (c) 2003-2022 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 "scanner.h" + +#ifdef _WIN32 + #include + #include + #include + #include + #include + #include + #include "idfetchhelpers.h" +#else + #include + #include + #include + #include "xslibusb.h" + #include "udev.h" +#endif +#include + +#include "serialportcommunicator.h" +#include "usbcommunicator.h" +#include +#include +#include +#include +#include +#include +#include +#include "enumerateusbdevices.h" +#include + +namespace XsScannerNamespace +{ +volatile std::atomic_bool abortPortScan{false}; +Scanner* gScanner = nullptr; +XsScanLogCallbackFunc gScanLogCallback = nullptr; +} +using namespace XsScannerNamespace; + +/*! \class Scanner::Accessor + \brief An accessor class for scanner +*/ + +/*! \returns The reference to a scanner object +*/ +Scanner& Scanner::Accessor::scanner() const +{ + if (!gScanner) + gScanner = new Scanner(); + return *gScanner; +} + +/*! \class Scanner + \brief Provides static functionality for scanning for Xsens devices. +*/ + +/*! \brief Destructor */ +Scanner::~Scanner() +{ +} + +/*! \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 Scanner::setScanLogCallback(XsScanLogCallbackFunc cb) +{ + gScanLogCallback = cb; +} + +/*! \brief Fetch basic device information + + \param[in, out] portInfo The name of the port to fetch from + \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 + + \returns XRV_OK if successful +*/ +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()); + + port->setGotoConfigTimeout(singleScanTimeout); + LOGXSSCAN("Opening port " << portInfo.portName() << " (" << portInfo.portNumber() << ") @ " << portInfo.baudrate() << " baud, expected device " << portInfo.deviceId()); + if (!port->openPort(portInfo, OPS_OpenPort)) + { + LOGXSSCAN("Failed to open port because: " << XsResultValue_toString(port->lastResult())); + return port->lastResult(); + } + + if (!port->openPort(portInfo, OPS_InitStart, detectRs485)) + { + LOGXSSCAN("Failed to initialize port because: " << XsResultValue_toString(port->lastResult())); + if (port->lastResult() < XRV_ERROR) + { + LOGXSSCAN("Attempting to reset device"); + // we got an xbus protocol error message, attempt to reset the device and try again (once) + XsMessage snd(XMID_Reset); + snd.setBusId(XS_BID_MASTER); + if (!port->writeMessage(snd)) + { + LOGXSSCAN("Failed to wriite reset to device because: " << XsResultValue_toString(port->lastResult())); + return port->lastResult(); + } + LOGXSSCAN("Reopening port after reset"); + port->closePort(); + XsTime::msleep(2000); + if (!port->openPort(portInfo, OPS_Full, detectRs485)) + { + LOGXSSCAN("Failed to reopen port after reset because: " << XsResultValue_toString(port->lastResult())); + return port->lastResult(); + } + } + else + return port->lastResult(); + } + + LOGXSSCAN("Port " << portInfo.portName() << " opened successfully, device is " << port->masterDeviceId()); + portInfo.setDeviceId(port->masterDeviceId()); + + // Enable flow control for Awinda2 stations/dongles which support this: + if (port->masterDeviceId().isAwinda2()) + { + XsPortLinesOptions portLinesOptions = portInfo.linesOptions(); + XsVersion fwVersion = port->firmwareRevision(); + XsVersion hwVersion = port->hardwareRevision(); + + if (port->masterDeviceId().isAwinda2Station() && fwVersion.major() != 255 && fwVersion >= XsVersion(4, 2, 1)) + portLinesOptions = (XsPortLinesOptions)(portLinesOptions | XPLO_RtsCtsFlowControl); + else if (port->masterDeviceId().isAwinda2Dongle() && fwVersion.major() != 255 && fwVersion >= XsVersion(4, 3, 2) && hwVersion >= XsVersion(2, 3)) + portLinesOptions = (XsPortLinesOptions)(portLinesOptions | XPLO_RtsCtsFlowControl); + else + portLinesOptions = (XsPortLinesOptions)(portLinesOptions & ~XPLO_RtsCtsFlowControl); + LOGXSSCAN("Setting flow control options for Awinda device to " << JLHEXLOG(portLinesOptions)); + portInfo.setLinesOptions(portLinesOptions); + } + + return XRV_OK; +} + +/*! \brief Scan a single COM port for connected Xsens devices + + \details The xsScanPort function will scan a single port for connected Xsens devices. If the + baudrate parameter is 0 (default), it will try to connect at all supported baud + rates, starting with the most common 115k2, 460k8 and 58k6. If the baudrate parameter + is non-zero, only the specified baud rate is tried. Any detected devices are returned + in the portInfo parameter. + + \param[in, out] portInfo The name of the port to scan should be in this parameter, the other contents will be filled by the function + \param[in] baud 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 + + \returns true if a device was found, false otherwise +*/ +bool Scanner::xsScanPort(XsPortInfo& portInfo, XsBaudRate baud, uint32_t singleScanTimeout, bool detectRs485) +{ + LOGXSSCAN("Scanning port " << portInfo.portName() << " at baudrate " << baud << " with timeout " << singleScanTimeout << " detectRs485 " << detectRs485); + XsResultValue res; + XsBaudRate baudrate; + + if (baud == 0) + baudrate = XBR_115k2; + else + baudrate = baud; + + while (!abortPortScan) + { + portInfo.setBaudrate(baudrate); + res = fetchBasicInfo(portInfo, singleScanTimeout, detectRs485); + if (res == XRV_OK) + { + LOGXSSCAN("Scan successfully found device " << portInfo.deviceId() << " on port " << portInfo.portName()); + portInfo.setBaudrate(baudrate); + return true; + } + + // failed, determine if we need to scan other baudrates or not + if (res != XRV_TIMEOUT && res != XRV_TIMEOUTNODATA && res != XRV_CONFIGCHECKFAIL) + { + LOGXSSCAN("Failed to fetch basic info: " << XsResultValue_toString(res)); + return false; + } + LOGXSSCAN("Failed to fetch basic info within timeout"); + + // not detected, try next baudrate + if (baud != 0) + { + LOGXSSCAN("Failed to find device"); + return false; + } + 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 + } + LOGXSSCAN("Checking next baudrate: " << baudrate); + } + LOGXSSCAN("Port scan aborted by external trigger"); + return false; +} + +/*! \brief Scan serial ports for connected Xsens devices. + + \details The xsScanPorts function will scan registered Xsens USB converters and serial COM ports + for connected Xsens devices. If the baudrate parameter is 0 (default), it will try to + connect at all supported baud rates, starting with the most common 115k2, 460k8 and + 58k6. If the baudrate parameter is non-zero, only the specified baudrate is tried. + Any detected devices are returned in the ports list, which is sorted by port nr. + + \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 + + \returns true if at least one device was found, false otherwise +*/ +bool Scanner::xsScanPorts(XsPortInfoArray& ports, XsBaudRate baudrate, uint32_t singleScanTimeout, bool ignoreNonXsensDevices, bool detectRs485) +{ + ports.clear(); + + if (!xsEnumerateSerialPorts(ports, ignoreNonXsensDevices)) + return false; + + if (!xsEnumerateUsbDevices(ports)) + return false; + + return xsFilterResponsiveDevices(ports, baudrate, singleScanTimeout, detectRs485); +} + +/*! \brief Filter responsive devices + + \details Serial ports that do not have a responsive Xsens device connected are removed from \a ports. + + \param[in,out] ports The list of ports to filter + \param[in] baudrate The baud rate used for scanning. If \a baudrate equals XBR_Invalid, all rates are scanned. + \param[in] singleScanTimeout The maximum time allowed for response + \param[in] detectRs485 Enable more extended scan to detect rs485 devices + + \returns true if successful +*/ +bool Scanner::xsFilterResponsiveDevices(XsPortInfoArray& ports, XsBaudRate baudrate, uint32_t singleScanTimeout, bool detectRs485) +{ + // try to connect so we can detect if there really is an MT / XM attached + unsigned p = 0; + while (!abortPortScan && p < ports.size()) + { + if (ports[p].isNetwork() || xsScanPort(ports[p], baudrate, singleScanTimeout, detectRs485)) + ++p; + else + { + LOGXSSCAN("Port : " << ports[p].portName() << " is not responsive, discarding"); + ports.erase(ports.begin() + p); + } + } + + if (abortPortScan) + { + abortPortScan = false; + return false; + } + + // Now sort the final list by ascending port nr + std::sort(ports.begin(), ports.end()); + abortPortScan = false; + return true; +} + +#ifdef _WIN32 +/*! \returns The device path for given windows device + \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) +{ + char deviceInstanceID[MAX_DEVICE_ID_LEN]; + SetupDiGetDeviceInstanceIdA(hDevInfo, DeviceInfoData, deviceInstanceID, MAX_DEVICE_ID_LEN, NULL); + return std::string(deviceInstanceID); +} +#endif + +/*! \returns true if the vendor/product combination may point to an xsens device + \param[in] vid The vendor ID + \param[in] pid the product ID +*/ +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); + + // FTDI reserved PIDs + case FTDI_VENDOR_ID: + return (pid >= 0xd388 && pid <= 0xd38f); + + default: + return false; + } +} + +/*! \brief Enumerate the serial ports + \param[in,out] ports The list of ports to append to + \param[in] ignoreNonXsensDevices If set to true (default), ignore serial ports that aren't Xsens USB devices + \returns True if successful +*/ +bool Scanner::xsEnumerateSerialPorts(XsPortInfoArray& ports, bool ignoreNonXsensDevices) +{ + LOGXSSCAN("Enumerating USB devices"); + XsPortInfo current; + +#ifdef _WIN32 + HDEVINFO hDevInfo; + SP_DEVINFO_DATA DeviceInfoData; + DWORD i; + + // Create a HDEVINFO with all present devices. + hDevInfo = SetupDiGetClassDevs(&GUID_DEVCLASS_PORTS, 0, 0, DIGCF_PRESENT | DIGCF_PROFILE | DIGCF_ALLCLASSES); + + if (hDevInfo == INVALID_HANDLE_VALUE) + { + LOGXSSCAN("Failed to get any USB device information, check permissions"); + return false; + } + + // Enumerate through all devices in Set. + DeviceInfoData.cbSize = sizeof(SP_DEVINFO_DATA); + 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); + if (hDeviceKey == INVALID_HANDLE_VALUE) + continue; + + xsens::JanitorFuncStdCall devkeyCleaner(&RegCloseKey, hDeviceKey, true); + + // Read in the name of the port + char pszPortName[256]; + DWORD dwSize = 256; + DWORD dwType = 0; + if ((RegQueryValueExA(hDeviceKey, "PortName", NULL, &dwType, (LPBYTE) pszPortName, &dwSize) != ERROR_SUCCESS) || (dwType != REG_SZ)) + continue; + + // If it looks like "COMX" then + // add it to the array which will be returned + if (_strnicmp(pszPortName, "COM", 3)) + continue; + int32_t nPort = atoi(&pszPortName[3]); + if (nPort == 0) + continue; + + std::string devpath = getDevicePath(hDevInfo, &DeviceInfoData); + uint16_t vid = vidFromString(devpath); + uint16_t pid = pidFromString(devpath); + + LOGXSSCAN("Found USB device " << devpath); + if (ignoreNonXsensDevices) + { + if (!isXsensUsbDevice(vid, pid)) + { + LOGXSSCAN("Ignoring non-Xsens device " << devpath); + continue; + } + } + + current.setPortName(pszPortName); + current.setBaudrate(XBR_Invalid); + current.setDeviceId(deviceIdFromDevPath(devpath)); + current.setVidPid(vid, pid); + ports.push_back(current); + + current.clear(); + } + + // Cleanup + + SetupDiDestroyDeviceInfoList(hDevInfo); + + // Now sort the list by ascending port nr + std::sort(ports.begin(), ports.end()); + +#else // !_WIN32 + Udev xsudev; + struct udev* udev = xsudev.unew(); + if (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; + 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) + { + xsudev.device_unref(d); + }; + std::unique_ptr + device(xsudev.device_new_from_syspath(udev, path), devcleaner); + if (!device) + continue; + + if (!xsudev.device_get_parent(device.get())) + // 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"); + 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"); + + if (vendor && product) + { + sscanf(vendor, "%x", &vid); + sscanf(product, "%x", &pid); + } + } + + LOGXSSCAN("Found USB device " << path); + + if (ignoreNonXsensDevices) + { + if (!isXsensUsbDevice(vid, pid)) + { + LOGXSSCAN("Ignoring non-Xsens device " << path); + continue; + } + } + + XsPortInfo portInfo; + + const char* devnode = xsudev.device_get_devnode(device.get()); + if (strlen(devnode) > 255 || strncmp(devnode, "/dev/ttyS", 9) == 0) + continue; + portInfo.setPortName(devnode); + portInfo.setVidPid(vid, pid); + + if (usbParentDevice) + { + const char* deviceidstring; + deviceidstring = xsudev.device_get_sysattr_value(usbParentDevice, "serial"); + if (deviceidstring) + { + int deviceId = 0; + sscanf(deviceidstring, "%08X", &deviceId); + portInfo.setDeviceId(deviceId); + } + } + + ports.push_back(portInfo); + } + xsudev.enumerate_unref(enumerate); + xsudev.unref(udev); + } + else + { + (void)ignoreNonXsensDevices; + DIR* dir; + struct dirent* entry; + + if ((dir = opendir("/dev/")) == NULL) + return false; + + while ((entry = readdir(dir))) + { + if (strncmp("ttyUSB", entry->d_name, 6) == 0) + { + char name[261]; + sprintf(name, "/dev/%s", entry->d_name); + current.setPortName(name); + ports.push_back(current); + LOGXSSCAN("Found USB device " << name); + } + } + closedir(dir); + } +#endif // _WIN32 + return true; +} + +/*! \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::xsEnumerateBluetoothDevices(XsPortInfoArray& ports) +{ + (void)ports; + return false; +} + +////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////// + +#define ILLEGAL_HUB (0) +#ifdef _WIN32 +#define HUB_SEARCH_STRING ("Hub_#") + +/*! \brief Retrieves the USB Hub number for a device + \param[in] hDevInfo The refernce to a device information + \param[in] deviceInfoData The pointer to a device information data + \returns Non-zero if successful +*/ +int Scanner::xsScanGetHubNumber(HDEVINFO hDevInfo, SP_DEVINFO_DATA* deviceInfoData) +{ + DWORD DataT; + char buffer[256]; + int result = ILLEGAL_HUB; + + if (SetupDiGetDeviceRegistryPropertyA(hDevInfo, + deviceInfoData, + SPDRP_LOCATION_INFORMATION, + &DataT, + (PBYTE)buffer, + 256, + NULL)) + { + LOGXSSCAN("Registry access successful: \"" << buffer << "\""); + 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()); + + return result; +} + +/*! \brief Scans a port by an Xsens hub ID (e.g. Awinda Station) + \param[in] id The id of a hub + \returns The found port info +*/ +XsPortInfo Scanner::xsScanPortByHubId(const char* id) +{ + assert(id != nullptr); + if (!id) + return XsPortInfo(); + + LOGXSSCAN("xsScanPortByHubId with id " << 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 */ + ); + + if (hDevInfo == INVALID_HANDLE_VALUE) + { + LOGXSSCAN("Failed to get any USB device information, check permissions"); + return XsPortInfo(); + } + + // Retrieve a context structure for a device interface of a device + // information set. + + SP_DEVICE_INTERFACE_DATA devInterfaceData; + ZeroMemory(&devInterfaceData, sizeof(SP_DEVICE_INTERFACE_DATA)); + devInterfaceData.cbSize = sizeof(SP_DEVICE_INTERFACE_DATA); + XsPortInfo portInfo; + int port = 0; + 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 */ + ); + if (!bRet) + { + if (GetLastError() == ERROR_NO_MORE_ITEMS) + break; + } + else + { + 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); + SP_DEVICE_INTERFACE_DETAIL_DATA_A& ifdData = *(SP_DEVICE_INTERFACE_DETAIL_DATA_A*) buffer; + ifdData.cbSize = sizeof(SP_DEVICE_INTERFACE_DETAIL_DATA_A); + DWORD reqSize; + if (SetupDiGetDeviceInterfaceDetailA(hDevInfo, &devInterfaceData, &ifdData, 1020, &reqSize, &diData)) + { + // determine my own id + // strip A#0000#* from path + char* end = strrchr(ifdData.DevicePath, '#'); + if (end) + end[0] = 0; + end = strrchr(ifdData.DevicePath, '#'); + if (end) + end[0] = 0; + char* start = strrchr(ifdData.DevicePath, '+'); + if (!start || !end) + continue; + ++start; + + // compare IDs + if (_strnicmp(id, start, strlen(id)) == 0) + { + // we found a match, determine port nr + // Get the registry key which stores the ports settings + HKEY hDeviceKey = SetupDiOpenDevRegKey(hDevInfo, &diData, DICS_FLAG_GLOBAL, 0, DIREG_DEV, KEY_QUERY_VALUE); + if (hDeviceKey != INVALID_HANDLE_VALUE) + { + // Read in the name of the port + char pszPortName[256] = ""; + DWORD dwSize = 256; + DWORD dwType = 0; + if ((RegQueryValueExA(hDeviceKey, "PortName", NULL, &dwType, (LPBYTE) pszPortName, &dwSize) == ERROR_SUCCESS) && (dwType == REG_SZ)) + { + // If it looks like "COMX" then + // add it to the array which will be returned + if (!_strnicmp(pszPortName, "COM", 3)) + port = atoi(&pszPortName[3]); + portInfo.setPortName(pszPortName); + std::string devpath = getDevicePath(hDevInfo, &diData); + uint16_t vid = vidFromString(devpath); + uint16_t pid = pidFromString(devpath); + LOGXSSCAN("Adding " << pszPortName << " with vid " << JLHEXLOG(vid) << " and pid " << JLHEXLOG(pid)); + portInfo.setVidPid(vid, pid); + } + } + // Close the key now that we are finished with it + RegCloseKey(hDeviceKey); + } + } + } + } + + SetupDiDestroyDeviceInfoList(hDevInfo); + + return portInfo; +} + +/*! \brief Scans all USB controllers to search for an Xsens hubs (e.g. Awinda Station) + \param[out] hubs The list of hubs found + \param[out] ports The list of ports associated with the found hubs + \returns True if successful +*/ +bool Scanner::xsScanXsensUsbHubs(XsIntArray& hubs, XsPortInfoArray& ports) +{ + LOGXSSCAN("xsScanXsensUsbHubs"); + + // clear the lists + hubs.clear(); + ports.clear(); + + // 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 */ + ); + + if (hDevInfo == INVALID_HANDLE_VALUE) + { + LOGXSSCAN("Failed to get any USB device information, check permissions"); + return false; + } + + // Retrieve a context structure for a device interface of a device + // information set. + + SP_DEVICE_INTERFACE_DATA devInterfaceData; + ZeroMemory(&devInterfaceData, sizeof(SP_DEVICE_INTERFACE_DATA)); + devInterfaceData.cbSize = sizeof(SP_DEVICE_INTERFACE_DATA); + 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 */ + ); + if (!bRet) + { + if (GetLastError() == ERROR_NO_MORE_ITEMS) + break; + continue; + } + + 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); + + SP_DEVICE_INTERFACE_DETAIL_DATA_A& ifdData = *(SP_DEVICE_INTERFACE_DETAIL_DATA_A*) tmp2; + ifdData.cbSize = sizeof(SP_DEVICE_INTERFACE_DETAIL_DATA_A); + DWORD reqSize; + XsPortInfo port; + int hubNr = 0; + while (SetupDiGetDeviceInterfaceDetailA(hDevInfo, &devInterfaceData, &ifdData, 1020, &reqSize, &diData)) + { + 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 + break; + } + else + break; + + hubNr = xsScanGetHubNumber(hDevInfo, &diData); + // we found something, determine its port + if (hubNr) + { + char* hubEnd = strrchr(ifdData.DevicePath, '#'); + if (!hubEnd || hubEnd == ifdData.DevicePath) + break; + hubEnd[0] = 0; + char* hubStart = strrchr(ifdData.DevicePath, '#'); + if (!hubStart || hubStart == ifdData.DevicePath) + break; + ++hubStart; + port = xsScanPortByHubId(hubStart); + } + break; + } + if (port.portName().size()) + { + LOGXSSCAN("xsScanXsensUsbHubs Adding hub " << hubNr << " and port " << port.portName()); + hubs.push_back(hubNr); + ports.push_back(port); + } + } + + SetupDiDestroyDeviceInfoList(hDevInfo); + + return true; +} +#endif // _WIN32 + +/*! \brief Get information about the hub configuration + \param portInfo Descriptor of the USB port to scan + \returns An XsUsbHubInfo object that provides platform independent comparison + + \details This function behaves slightly different on different platforms. On Windows + only hubs that are Xsens devices are taken into account. On platforms supporting udev + the information is always filled in if \a port exists. This means that + \code + if (xsScanUsbHub(port).isValid()) + { + // device is docked + } + \endcode + + is not enough to accurately determine whether a device was docked in a cross-platform + and future proof application. + Instead use the recommended approach to check whether two devices share the same hub: + + \code + XsUsbHubInfo hubInfo1 = xsScanUsbHub(port1); + XsUsbHubInfo hubInfo2 = xsScanUsbHub(port2); + + hubInfo1.parentPathMatches(hubInfo2); + \endcode +*/ +XsUsbHubInfo Scanner::xsScanUsbHub(const XsPortInfo& portInfo) +{ +#ifdef _WIN32 + int behindXsensHub = ILLEGAL_HUB; + XsIntArray hubs; + XsPortInfoArray ports; + if (!xsScanXsensUsbHubs(hubs, ports)) + return XsUsbHubInfo(); + + // check if it's in the ports list before going through all THAT trouble... + for (size_t i = 0; i < ports.size(); ++i) + if (_stricmp(ports[i].portName_c_str(), portInfo.portName_c_str()) == 0) + return XsUsbHubInfo(hubs[i]); + + HDEVINFO hDevInfo; + SP_DEVINFO_DATA devInfoData; + + // Create a HDEVINFO with all present devices. + hDevInfo = SetupDiGetClassDevs(&GUID_DEVCLASS_PORTS, 0, 0, DIGCF_PRESENT | DIGCF_PROFILE); + + if (hDevInfo == INVALID_HANDLE_VALUE) + return XsUsbHubInfo(); + + // Enumerate through all devices in Set. + devInfoData.cbSize = sizeof(SP_DEVINFO_DATA); + for (DWORD i = 0; behindXsensHub == ILLEGAL_HUB && SetupDiEnumDeviceInfo(hDevInfo, i, &devInfoData); ++i) + { + DWORD dataT; + char buffer[256]; + + // Call function with null to begin with, + // then use the returned buffer size + // to Alloc the buffer. Keep calling until + // success or an unknown failure. + // + + if (!SetupDiGetDeviceRegistryPropertyA(hDevInfo, + &devInfoData, + SPDRP_MFG, + &dataT, + (PBYTE)buffer, + 256, + NULL)) + continue; + + // Get the registry key which stores the ports settings + HKEY hDeviceKey = SetupDiOpenDevRegKey(hDevInfo, &devInfoData, DICS_FLAG_GLOBAL, 0, DIREG_DEV, KEY_QUERY_VALUE); + if (hDeviceKey != INVALID_HANDLE_VALUE) + { + // Read in the name of the port + char pszPortName[256]; + DWORD dwSize = 256; + DWORD dwType = 0; + if ((RegQueryValueExA(hDeviceKey, "PortName", NULL, &dwType, (LPBYTE) pszPortName, &dwSize) == ERROR_SUCCESS) && (dwType == REG_SZ)) + { + // If it looks like "COMX" then + // add it to the array which will be returned + int32_t nLen = (int32_t) strlen(pszPortName); + if (nLen > 3) + { + if (_strnicmp(pszPortName, "COM", 3)) + continue; + //int32_t nPort = atoi(&pszPortName[3]); + if (_stricmp(pszPortName, portInfo.portName_c_str()) == 0) + { + //This is the port we are looking for + //check if the hub number is in the hubs list + int hub = xsScanGetHubNumber(hDevInfo, &devInfoData); + JLDEBUGG("Got hub " << hub << " for port " << std::string(pszPortName)); + + if (hub != ILLEGAL_HUB) + { + for (XsIntArray::const_iterator hi = hubs.begin(); hi != hubs.end(); ++hi) + { + if (*hi == hub) + { + behindXsensHub = hub; + break; + } + } + } + } + } + } + } + // Close the key now that we are finished with it + RegCloseKey(hDeviceKey); + } + + // Cleanup + + SetupDiDestroyDeviceInfoList(hDevInfo); + return XsUsbHubInfo(behindXsensHub); +#else + XsUsbHubInfo inf; + + Udev xsudev; + + udev* udevInstance = xsudev.unew(); + if (udevInstance == NULL) + { + fprintf(stderr, "Unable to create udev object\n"); + return XsUsbHubInfo(); + } + + 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; + 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); + if (!device) + return XsUsbHubInfo(); + + 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"); + if (!parent) + break; + + const char* devpath = xsudev.device_get_sysattr_value(parent, "devpath"); + if (!devpath) + break; + + inf = XsUsbHubInfo(devpath); + break; + } + xsudev.enumerate_unref(enumerate); + xsudev.unref(udevInstance); + return inf; +#endif +} diff --git a/extern/xspublic/xscontroller/scanner.h b/extern/xspublic/xscontroller/scanner.h new file mode 100644 index 0000000..27b2a7d --- /dev/null +++ b/extern/xspublic/xscontroller/scanner.h @@ -0,0 +1,106 @@ + +// Copyright (c) 2003-2022 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 SCANNER_H +#define SCANNER_H + +#include +#include +#include +#include +#include "xsusbhubinfo.h" +#include "xsscanner.h" + +#include + +struct XsIntArray; + +#ifdef _WIN32 + #include +#endif + +class Scanner +{ +public: + virtual ~Scanner(); + + class Accessor + { + public: + Scanner& scanner() const; + }; + + 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); + 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); + + static void setScanLogCallback(XsScanLogCallbackFunc cb); +}; + +namespace XsScannerNamespace +{ +extern volatile std::atomic_bool abortPortScan; +extern Scanner* gScanner; +extern XsScanLogCallbackFunc gScanLogCallback; +} + +#if 1 +#define LOGXSSCAN(msg)\ + do {\ + JLDEBUGG(msg); \ + if (XsScannerNamespace::gScanLogCallback) \ + { \ + std::ostringstream os; \ + os << msg; \ + const XsString cbVal(os.str()); \ + XsScannerNamespace::gScanLogCallback(&cbVal); \ + } \ + } while(0) +#else +#define LOGXSSCAN(msg) ((void)0) +#endif + +#endif diff --git a/extern/xspublic/xscontroller/scenariomatchpred.h b/extern/xspublic/xscontroller/scenariomatchpred.h new file mode 100644 index 0000000..335cabc --- /dev/null +++ b/extern/xspublic/xscontroller/scenariomatchpred.h @@ -0,0 +1,96 @@ + +// Copyright (c) 2003-2022 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 FILTERPROFILEMATCHPRED_H +#define FILTERPROFILEMATCHPRED_H + +#include +#include +#include + +/*! \brief Returns true if \a lhs has the same type as \a 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) +{ + return scenarioTypeMatches(lhs, rhs); +} + +/*! \class ScenarioMatchPred + \brief The class that compares two scenarios for matching +*/ +class ScenarioMatchPred +{ +public: + + /*! \brief Default constructor + */ + ScenarioMatchPred(const XsFilterProfile& scenario) + : m_scenarioType(scenario.type()) + { + } + + /*! \brief Explicit constructor + */ + explicit ScenarioMatchPred(uint32_t profileType) + : m_scenarioType(profileType) + { + + } + + /*! \param s : The filter profile to compare + \returns true if \a s matches + */ + bool operator()(const XsFilterProfile& s) const + { + return s.type() == m_scenarioType; + } + + /*! \param s : The filter profile to compare + \returns true if \a s matches + */ + bool operator()(const XsFilterProfile* s) const + { + return s->type() == m_scenarioType; + } + +private: + uint32_t m_scenarioType; +}; + +#endif diff --git a/extern/xspublic/xscontroller/serialcommunicator.cpp b/extern/xspublic/xscontroller/serialcommunicator.cpp new file mode 100644 index 0000000..23c9ed8 --- /dev/null +++ b/extern/xspublic/xscontroller/serialcommunicator.cpp @@ -0,0 +1,454 @@ + +// Copyright (c) 2003-2022 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 "serialcommunicator.h" +#include +#include +#include "protocolhandler.h" +#include "serialinterface.h" +#include +#include +#include +#include "xsscanner.h" +#include "mtthread.h" +#include "deviceredetector.h" +#include +#include "xsdevice_def.h" + + +/*! \class SerialCommunicator + \brief A class that uses serial communication +*/ + +/*! \brief Default constructor +*/ +SerialCommunicator::SerialCommunicator() + : m_thread(*this, *this) + , m_firmwareRevision(0, 0, 0) + , m_hardwareRevision(0, 0) +{ + messageExtractor().clearBuffer(); + startPollThread(); +} + +SerialCommunicator::~SerialCommunicator() +{ +} + +/*! \brief Prepares for a destruction +*/ +void SerialCommunicator::prepareForDestruction() +{ + JLDEBUGG(""); + + stopPollThread(); + terminate(); + + messageExtractor().clearBuffer(); + if (m_streamInterface) + { + m_streamInterface->cancelIo(); + m_streamInterface->close(); + m_streamInterface.reset(); + } + + DeviceCommunicator::prepareForDestruction(); +} + +/*! \copybrief Communicator::gotoMeasurement +*/ +XsResultValue SerialCommunicator::gotoMeasurement() +{ + JLDEBUGG("entry"); + + if (!isActive()) + return setAndReturnLastResult(XRV_INVALIDOPERATION); + + setDoGotoConfig(false); + + XsResultValue r = DeviceCommunicator::gotoMeasurement(); + if (r != XRV_OK) + gotoConfig(); + return setAndReturnLastResult(r); +} + +/*! \copybrief Communicator::gotoConfig +*/ +XsResultValue SerialCommunicator::gotoConfig(bool detectRs485) +{ + JLDEBUGG("entry"); + + bool rs485Config = ((masterDeviceId().toInt() & XS_DID_TYPEL_COMM_MASK) == XS_DID_TYPEL_RS485) || detectRs485; + if (!rs485Config) + return DeviceCommunicator::gotoConfig(false); + + uint32_t interfaceTimeout = defaultInterfaceTimeout(); + if (m_streamInterface) + interfaceTimeout = m_streamInterface->getTimeout(); + + std::shared_ptr reply = addReplyObject(XMID_GotoConfigAck); + + int oldCount = messageExtractor().setMaxIncompleteRetryCount(0); + setDoGotoConfig(true); + XsMessage rcv = reply->message(gotoConfigTimeout() + interfaceTimeout + 1000); // set higher timeout for RS485 + setDoGotoConfig(false); + + if (rcv.getMessageId() != XMID_GotoConfigAck) + { + messageExtractor().setMaxIncompleteRetryCount(oldCount); + JLDEBUGG("Fail: " << rcv.getMessageId()); + return (setAndReturnLastResult(XRV_CONFIGCHECKFAIL)); + } + + // wait a bit longer for all potentially incoming gotoConfigAcks to be received and processed + while (true) + { + std::shared_ptr reply2 = addReplyObject(XMID_GotoConfigAck); + rcv = reply2->message(100); + if (rcv.getMessageId() != XMID_GotoConfigAck) + break; + } + + messageExtractor().setMaxIncompleteRetryCount(oldCount); + JLTRACEG("Ok: " << rcv.getMessageId()); + return (setAndReturnLastResult(XRV_OK)); +} + +/*! \brief Write raw data to the open COM or USB port +*/ +XsResultValue SerialCommunicator::writeRawData(const XsByteArray& data) +{ + if (!isPortOpen()) + return XRV_NOPORTOPEN; + return m_streamInterface->writeData(data, nullptr); +} + +/*! \brief Flushes all remaining data on the open port +*/ +void SerialCommunicator::flushPort() +{ + if (m_streamInterface) + m_streamInterface->flushData(); +} + +/*! \brief Stops polling the thread +*/ +void SerialCommunicator::stopPollThread() +{ + m_thread.stopThread(); +} + +/*! \brief Starts polling the thread +*/ +void SerialCommunicator::startPollThread() +{ + m_thread.startThread(); +} +/*! \brief Closes the port +*/ +void SerialCommunicator::closePort() +{ + JLDEBUGG("entry"); + XSEXITLOGN(gJournal); + + if (!m_streamInterface) + return; + + stopPollThread(); + + if (m_streamInterface) + { + m_streamInterface->close(); + m_streamInterface.reset(); + } +} + +/*! \returns whether the communicator has an open COM or USB port + \*/ +bool SerialCommunicator::isPortOpen() const +{ + return m_streamInterface && m_streamInterface->isOpen(); +} + +/*! \returns The port information +*/ +XsPortInfo SerialCommunicator::portInfo() const +{ + return m_activePortInfo; +} + +/*! \brief Open a serial port and return the main device connected to it +*/ +bool SerialCommunicator::openPort(const XsPortInfo& portInfo, OpenPortStage stage, bool detectRs485) +{ + JLDEBUGG("Opening " << portInfo << " stage " << stage << " configtimeout " << gotoConfigTimeout()); + XSEXITLOGN(gJournal); + + if (stage & OPS_OpenPort) + { + if (m_streamInterface) + { + setLastResult(XRV_ALREADYOPEN); + return (this->portInfo() == portInfo); + } + + m_streamInterface = createStreamInterface(portInfo); + + if (lastResult() != XRV_OK) + { + stopPollThread(); // it's possible that the poll thread has been started, but the result is still not ok + m_streamInterface.reset(); + return false; + } + startPollThread(); + } + if (!m_streamInterface) + { + setLastResult(XRV_NOPORTOPEN); + return false; + } + + if (stage & OPS_InitDevice) + { + // Retrieve deviceid, which is always needed with an open port + if (gotoConfig(detectRs485) != XRV_OK) + { + prepareForDestruction(); + return false; + } + + if (getDeviceId() != XRV_OK) + { + prepareForDestruction(); + return false; + } + + // Also retrieve the firmware revision, which may be needed for determining optional settings of a port + if (getFirmwareRevision() != XRV_OK) + { + prepareForDestruction(); + return false; + } + + // Get the hardware version only for Awinda Dongle. Issue: MTSDK-3999 + if (masterDeviceId().isAwinda2Dongle()) + { + if (getHardwareRevision() != XRV_OK) + { + prepareForDestruction(); + return false; + } + } + } + + setLastResult(m_streamInterface->setTimeout(0)); + + if (lastResult() == XRV_OK) + { + m_activePortInfo = portInfo; + return true; + } + else + return false; + +} + +/*! \copybrief Communicator::reopenPort +*/ +bool SerialCommunicator::reopenPort(OpenPortStage stage, bool skipDeviceIdCheck) +{ + JLDEBUGG(""); + XsPortInfo pi = portInfo(); + + uint32_t oldTimeOut = defaultTimeout() + defaultInterfaceTimeout(); + if (m_streamInterface) + oldTimeOut = m_streamInterface->getTimeout(); + + closePort(); + DeviceRedetector redetector(pi); + bool detected = false; + int count = 0; + while (!detected && count < 3) + { + if (!redetector.redetect(masterDeviceId(), pi, skipDeviceIdCheck)) + { + JLDEBUGG("Redetect failed"); + return false; + } + + if (!openPort(pi, stage)) + { + JLDEBUGG("openPort failed, attempt: " << count); + count++; + continue; + } + detected = true; + } + if (!detected) + return false; + + if (oldTimeOut && m_streamInterface) + m_streamInterface->setTimeout(oldTimeOut); + + return true; +} + +/*! \returns True if the thread is alive*/ +bool SerialCommunicator::isActive() const +{ + return ((masterDevice() != nullptr) && (m_thread.isAlive())); +} + +/*! \brief Sets do go to config in a thread + \param doit The boolean value to set +*/ +void SerialCommunicator::setDoGotoConfig(bool doit) +{ + m_thread.setDoGotoConfig(doit); +} + + +/*! \returns True if the \a other device is docked at this device + \param other The communicator of an other device + \note This is true if the USB hub matches. +*/ +bool SerialCommunicator::isDockedAt(Communicator* other) const +{ + XsUsbHubInfo thisHub = XsScanner::scanUsbHub(XsPortInfo(portInfo().portName())); + XsUsbHubInfo otherHub = XsScanner::scanUsbHub(XsPortInfo(other->portInfo().portName())); + return thisHub.parentPathMatches(otherHub); +} + +/*! \brief Read available data from the open IO device + \details This function will attempt to read all available data from the open device (COM port + or USB port). + The function will read from the device, but it won't wait for data to become available. + \param raw A buffer that will receive the read data. + \note This function does not set the lastResult value since this could cause threading issues. + \returns The result of the operation +*/ +XsResultValue SerialCommunicator::readDataToBuffer(XsByteArray& raw) +{ + if (!m_streamInterface) + return XRV_NOPORTOPEN; + + // always read data and append it to the cache before doing analysis + const int maxSz = 8192; + XsResultValue res = m_streamInterface->readData(maxSz, raw); + if (raw.size()) + return XRV_OK; + + switch (res) + { + // all intended fall-throughs + case XRV_UNEXPECTED_DISCONNECT: + if (masterDevice() != nullptr) + masterDevice()->onConnectionLost(); + //lint -fallthrough + case XRV_NOFILEORPORTOPEN: + closePort(); + break; + + default: + break; + } + + return res; +} + +/*! \copybrief Communicator::handleMessage + \note Overridden here for implementation of DataParser::handleMessage + \param msg The XsMessage to handle +*/ +void SerialCommunicator::handleMessage(const XsMessage& msg) +{ + DeviceCommunicator::handleMessage(msg); +} + +/*! \brief Read all messages from the buffered read data after adding new data supplied in \a rawIn + \param rawIn The byte array with all data + \param messages The message to process + \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); +} + +/*! \brief Requests the firmware revision from the connected device +*/ +XsResultValue SerialCommunicator::getFirmwareRevision() +{ + XsMessage snd(XMID_ReqFirmwareRevision); + snd.setBusId(XS_BID_MASTER); + + XsMessage rcv; + if (!doTransaction(snd, rcv)) + return setAndReturnLastResult(XRV_COULDNOTREADSETTINGS); + m_firmwareRevision = XsVersion(rcv.getDataByte(0), rcv.getDataByte(1), rcv.getDataByte(2)); + + return setAndReturnLastResult(XRV_OK); +} + +/*! \brief Requests the hardware revision from the connected device. + \note Actually it requests master settings, but gets only hardware revision. + \returns The hardware revision +*/ +XsResultValue SerialCommunicator::getHardwareRevision() +{ + XsMessage snd(XMID_ReqMasterSettings, 1); + snd.setBusId(XS_BID_MASTER); + snd.setDataByte(0, 0); + + XsMessage rcv; + if (!doTransaction(snd, rcv)) + return setAndReturnLastResult(XRV_COULDNOTREADSETTINGS); + + m_hardwareRevision = XsVersion(rcv.getDataShort(24) >> 8, rcv.getDataShort(24) & 0xff); + return setAndReturnLastResult(XRV_OK); +} + +/*! \returns the firmware revision which was found during the OPS_InitDevice stage +*/ +XsVersion SerialCommunicator::firmwareRevision() +{ + return m_firmwareRevision; +} + +/*! \returns the hardware revision only for Awinda Dongle. +*/ +XsVersion SerialCommunicator::hardwareRevision() +{ + return m_hardwareRevision; +} diff --git a/extern/xspublic/xscontroller/serialcommunicator.h b/extern/xspublic/xscontroller/serialcommunicator.h new file mode 100644 index 0000000..f7ba0d1 --- /dev/null +++ b/extern/xspublic/xscontroller/serialcommunicator.h @@ -0,0 +1,103 @@ + +// Copyright (c) 2003-2022 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 SERIALCOMMUNICATOR_H +#define SERIALCOMMUNICATOR_H + +#include "devicecommunicator.h" +#include +#include +#include +#include "dataparser.h" +#include "mtthread.h" + +class SerialCommunicator : public DeviceCommunicator, public DataParser +{ +public: + SerialCommunicator(); + + XsResultValue gotoConfig(bool detectRs485 = false) override; + XsResultValue gotoMeasurement() 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 reopenPort(OpenPortStage stage = OPS_Full, bool skipDeviceIdCheck = false) override; + bool isDockedAt(Communicator* other) const override; + + XsResultValue writeRawData(const XsByteArray& data) override; + + XsVersion firmwareRevision(); + XsVersion hardwareRevision(); +protected: + virtual ~SerialCommunicator(); + virtual void prepareForDestruction() override; + + /*! \brief Creates a stream interface + \param pi The port information to use + \returns The shared pointer to a stream interface + */ + virtual std::shared_ptr createStreamInterface(const XsPortInfo& pi) = 0; + + XsResultValue readDataToBuffer(XsByteArray& raw) override; + XsResultValue processBufferedData(const XsByteArray& rawIn, std::deque& messages) override; + + bool isActive() const; + + void setDoGotoConfig(bool doit); + void stopPollThread(); // made protected for TestCommunicator testing + virtual XsResultValue getFirmwareRevision(); + virtual XsResultValue getHardwareRevision(); + + /*! \returns The default interface timeout + */ + virtual uint32_t defaultInterfaceTimeout() const + { + return 0; + } + + XsPortInfo m_activePortInfo; //!< The information about the port this communicator is currently connected to + +private: + MtThread m_thread; + std::shared_ptr m_streamInterface; + XsVersion m_firmwareRevision; + XsVersion m_hardwareRevision; + + void startPollThread(); +}; + +#endif diff --git a/extern/xspublic/xscontroller/serialinterface.cpp b/extern/xspublic/xscontroller/serialinterface.cpp new file mode 100644 index 0000000..d944f42 --- /dev/null +++ b/extern/xspublic/xscontroller/serialinterface.cpp @@ -0,0 +1,973 @@ + +// Copyright (c) 2003-2022 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 "serialinterface.h" +#include +#include +#include "rx_tx_log.h" + +#include +#ifndef _WIN32 + #include // close + #include // ioctl + #include // open, O_RDWR + #include // strcpy + #include + #include + #include + #if !defined(__APPLE__) && !defined(ANDROID) + #include + #endif +#else + #include + #include +#endif + +#ifndef _CRT_SECURE_NO_DEPRECATE + #define _CRT_SECURE_NO_DEPRECATE + #ifdef _WIN32 + #pragma warning(disable:4996) + #endif +#endif + +////////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////// SerialInterface //////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////// + +//! \brief Default constructor, initializes all members to their default values. +SerialInterface::SerialInterface() +{ + m_port = 0; + m_lastResult = XRV_OK; + m_timeout = 0; + m_endTime = 0; + m_baudrate = XBR_Invalid; + m_portname[0] = 0; +#ifdef _WIN32 + m_handle = INVALID_HANDLE_VALUE; +#else + m_handle = -1; + memset(&m_commState, 0, sizeof(m_commState)); +#endif +} + +//! Destructor, de-initializes, frees memory allocated for buffers, etc. +SerialInterface::~SerialInterface() +{ + try + { + closeLive(); + } + catch (...) + { + } +} + +//! \brief Close the serial communication port. +XsResultValue SerialInterface::close(void) +{ + return closeLive(); +} + +//! \brief Close the serial communication port. +XsResultValue SerialInterface::closeLive(void) +{ +#ifdef LOG_RX_TX + rx_log.close(); + tx_log.close(); +#endif + if (!isOpen()) + return m_lastResult = XRV_NOPORTOPEN; + + m_lastResult = XRV_OK; +#ifdef _WIN32 + if (::FlushFileBuffers(m_handle)) + { + // 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)) + { + cto.ReadIntervalTimeout = MAXDWORD; + cto.ReadTotalTimeoutConstant = 0; + cto.ReadTotalTimeoutMultiplier = 0; + if (::SetCommTimeouts(m_handle, &cto)) + { + char buffer[1024]; + DWORD length; + do + { + if (!::ReadFile(m_handle, buffer, 1024, &length, NULL)) + break; + } while (length > 0); + } + else + m_lastResult = XRV_ERROR; + } + else + m_lastResult = XRV_ERROR; + } + if (!::CloseHandle(m_handle)) + m_lastResult = XRV_ERROR; + m_handle = INVALID_HANDLE_VALUE; +#else + flushData(); + ::close(m_handle); + m_handle = -1; +#endif + m_endTime = 0; + + return m_lastResult; +} + +/*! \brief Manipulate the Serial control lines + + The function manipulates the serial control lines that are indicated by the + mask parameter. Note that only the DTR and RTS lines can be set by win32. + \param mask Indicates which lines are to be manipulated and which should be + left alone. + \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) +{ + if (!isOpen()) + return (m_lastResult = XRV_NOPORTOPEN); +#ifdef _WIN32 + BOOL rv = 0; + if (mask & XCL_DTR) + { + if (state & XCL_DTR) + rv = EscapeCommFunction(m_handle, SETDTR); + else + rv = EscapeCommFunction(m_handle, CLRDTR); + } + + if (mask & XCL_RTS) + { + if (state & XCL_RTS) + rv = EscapeCommFunction(m_handle, SETRTS); + else + rv = EscapeCommFunction(m_handle, CLRRTS); + } + if (rv) + return m_lastResult = XRV_OK; + else + return m_lastResult = XRV_ERROR; +#else + bool rv = true; + int32_t status; + if (mask & XCL_DTR) + { + if (ioctl(m_handle, TIOCMGET, &status) == -1) + { + if (state & XCL_DTR) + status |= TIOCM_DTR; + else + status &= ~TIOCM_DTR; + rv = (ioctl(m_handle, TIOCMSET, &status) == -1); + } + else + rv = false; + } + if (rv && (mask & XCL_RTS)) + { + if (ioctl(m_handle, TIOCMGET, &status) == -1) + { + if (state & XCL_RTS) + status |= TIOCM_RTS; + else + status &= ~TIOCM_RTS; + rv = (ioctl(m_handle, TIOCMSET, &status) == -1); + } + else + rv = false; + } + if (rv) + return m_lastResult = XRV_OK; + else + return m_lastResult = XRV_ERROR; +#endif +} + +/*! \copydoc IoInterface::flushData + \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) +{ + m_lastResult = XRV_OK; +#ifdef _WIN32 + // Remove any 'old' data in buffer + if (!PurgeComm(m_handle, PURGE_TXCLEAR | PURGE_RXCLEAR)) + m_lastResult = XRV_ERROR; +#else + tcflush(m_handle, TCIOFLUSH); +#endif + m_endTime = 0; + return m_lastResult; +} + +//! Return the baudrate that is currently being used by the port +XsBaudRate SerialInterface::getBaudrate(void) const +{ + if (isOpen()) + return m_baudrate; + return XBR_Invalid; +} + +//! Return the handle of the port +XsIoHandle SerialInterface::getHandle(void) const +{ + return m_handle; +} + +//! Retrieve the port number that was last successfully opened. +uint16_t SerialInterface::getPortNumber(void) const +{ + return m_port; +} + +//! Retrieve the port name that was last successfully opened. +void SerialInterface::getPortName(XsString& portname) const +{ + portname = m_portname; +} + +//! Return the error code of the last operation. +XsResultValue SerialInterface::getLastResult(void) const +{ + return m_lastResult; +} + +//! Return the current timeout value +uint32_t SerialInterface::getTimeout(void) const +{ + return m_timeout; +} + +//! Return whether the communication port is open or not. +bool SerialInterface::isOpen(void) const +{ +#ifdef _WIN32 + return m_handle != INVALID_HANDLE_VALUE; +#else + return m_handle >= 0; +#endif +} + +#ifndef _WIN32 +template +static T setBitsEnabled(T field, T bits, bool cond) +{ + if (cond) + field |= bits; + else + field &= (~bits); + return field; +} +#endif + +/*! \brief Open a communication channel to the given port info. + \details If the baudrate in \a portInfo is set to XBR_Invalid, the baud rate is automatically + detected if possible. + \param portInfo The details of the port that should be opened. Depending on the type of interface, + parts of this parameter may be ignored. + \param readBufSize The size of the read buffer in bytes (if appliccable to the device) + \param writeBufSize The size of the write buffer in bytes (if appliccable to the device) + \param options The options to enable (flow control, stop bits) + \returns XRV_OK if the device was opened successfully +*/ +XsResultValue SerialInterface::open(const XsPortInfo& portInfo, + XsFilePos readBufSize, + XsFilePos writeBufSize, + PortOptions options) +{ + m_endTime = 0; + + JLDEBUGG(portInfo); + + if (isOpen()) + { + JLALERTG("Port " << portInfo.portName() << " is already open"); + return (m_lastResult = XRV_ALREADYOPEN); + } + m_baudrate = portInfo.baudrate(); + + if (options & PO_RtsCtsFlowControl || (portInfo.linesOptions() & XPLO_RtsCtsFlowControl)) + JLTRACEG("Requested RTS/CTS flow control"); + if (options & PO_DtrDsrFlowControl) + JLTRACEG("Requested DTR/DSR flow control"); + if (options & PO_XonXoffFlowControl) + JLTRACEG("Requested Xon/Xoff flow control"); + +#ifdef _WIN32 + XsResultValue fail = XRV_OK; + char winPortName[256]; + + // Open port + sprintf(winPortName, "\\\\.\\%s", portInfo.portName_c_str()); + m_handle = CreateFileA(winPortName, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL); + if (m_handle == INVALID_HANDLE_VALUE) + { + JLDEBUGG("Port " << portInfo.portName() << " cannot be opened"); + return (m_lastResult = XRV_INPUTCANNOTBEOPENED); + } + + DCB commState; //!< Stored settings about the serial port + + commState.DCBlength = sizeof(DCB); + + //Get the current state & then change it + if (!GetCommState(m_handle, &commState)) // Get current state + fail = XRV_ERROR; + + 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; + + // Setup flow control + 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.fInX = commState.fOutX; + + if (!SetCommState(m_handle, (LPDCB)&commState)) // Set new state + { + // Bluetooth ports cannot always be opened with 2 stopbits + // Now try to open port with 1 stopbit. + commState.StopBits = ONESTOPBIT; + if (!SetCommState(m_handle, (LPDCB)&commState)) + fail = XRV_INPUTCANNOTBEOPENED; + } + char const* tmp = portInfo.portName_c_str(); + m_port = (uint16_t) atoi(&tmp[3]); + sprintf(m_portname, "%s", tmp); + + if (setTimeout(20)) + fail = m_lastResult; + + // Other initialization functions + int tmpFail = fail; + applyHwControlLinesOptions(options, portInfo.linesOptions(), tmpFail); + fail = static_cast(tmpFail); + + if (!SetupComm(m_handle, (DWORD) readBufSize, (DWORD) writeBufSize)) // Set queue size + fail = XRV_ERROR; + + // Remove any 'old' data in buffer + //PurgeComm(m_handle, PURGE_TXCLEAR | PURGE_RXCLEAR); + if (!PurgeComm(m_handle, PURGE_TXABORT | PURGE_RXABORT | PURGE_TXCLEAR | PURGE_RXCLEAR)) + fail = XRV_ERROR; + + if (fail != XRV_OK) + { + CloseHandle(m_handle); + m_handle = INVALID_HANDLE_VALUE; + return (m_lastResult = fail); + } + +#else // !_WIN32 + (void)readBufSize; + (void)writeBufSize; + // Open port + std::string pn = portInfo.portName().toStdString(); + m_handle = ::open(pn.c_str(), O_RDWR | O_NOCTTY); + + // O_RDWR: Read+Write + // O_NOCTTY: Raw input, no "controlling terminal" + // O_NDELAY: Don't care about DCD signal + + if (m_handle < 0) + { + // Port not open + return m_lastResult = XRV_INPUTCANNOTBEOPENED; + } + + // Check if the file is already opened by someome else (other thread/process) + if (flock(m_handle, LOCK_EX | LOCK_NB)) + { + closeLive(); + return m_lastResult = XRV_INPUTCANNOTBEOPENED; + } + + /* Start configuring of port for non-canonical transfer mode */ + // Get current options for the port + if (tcgetattr(m_handle, &m_commState) != 0) + return XRV_ERROR; + + // Set baudrate. + if (cfsetispeed(&m_commState, portInfo.baudrate()) != 0) + return XRV_ERROR; + + if (cfsetospeed(&m_commState, portInfo.baudrate()) != 0) + return XRV_ERROR; + + // 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 |= CS8; // Select 8 data bits + + 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); +#ifdef CDTRDSR + 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); + // 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); + // 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 + + // Set the new options for the port + if (tcsetattr(m_handle, TCSANOW, &m_commState) != 0) + return XRV_INPUTCANNOTBEOPENED; + +#if defined(JLDEF_BUILD) && JLDEF_BUILD <= JLL_ALERT + termios checkCommState; + if (tcgetattr(m_handle, &checkCommState) != 0) + return XRV_ERROR; + + if (cfgetispeed(&checkCommState) != portInfo.baudrate()) + JLALERTG("Set baudrate doesn't match requested baudrate"); + + if (cfgetospeed(&checkCommState) != portInfo.baudrate()) + JLALERTG("Set baudrate doesn't match requested baudrate"); + + if (options & PO_RtsCtsFlowControl && !(checkCommState.c_cflag & CRTSCTS)) + JLALERTG("Requested RTS/CTS flow control, but could not be set."); + + if (options & PO_DtrDsrFlowControl && +#ifdef 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))) + JLALERTG("Requested Xon/Xoff flow control, but could not be set."); +#endif // JLDEF_BUILD < JLL_ALERT + +#if defined(JLDEF_BUILD) && JLDEF_BUILD <= JLL_DEBUG +#define CHECK_COMMSTATE(req, res, field)\ + if (req.field != res.field) \ + {\ + JLDEBUGG("field " << #field << " does not match");\ + JLDEBUGG("actual : " << std::oct << (uint64_t)res.field);\ + JLDEBUGG("expected: " << std::oct << (uint64_t)req.field);\ + } +#else +#define CHECK_COMMSTATE(req, res, field) +#endif + CHECK_COMMSTATE(m_commState, checkCommState, c_cflag); + CHECK_COMMSTATE(m_commState, checkCommState, c_iflag); + CHECK_COMMSTATE(m_commState, checkCommState, c_oflag); + CHECK_COMMSTATE(m_commState, checkCommState, c_cc[VMIN]); + CHECK_COMMSTATE(m_commState, checkCommState, c_cc[VTIME]); + + m_port = 1; + sprintf(m_portname, "%s", pn.c_str()); + + tcflush(m_handle, TCIOFLUSH); + + // 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"); + return (m_lastResult = XRV_OK); +} + +/*! \brief Read data from the serial port and put it into the data buffer. + \details This function reads up to \a maxLength bytes from the port (non-blocking) and + puts it into the \a data buffer. + \param maxLength The maximum amount of data read. + \param data The buffer that will store the received data. + \returns XRV_OK if no error occurred. It can be that no data is available and XRV_OK will be + returned. Check data.size() for the number of bytes that were read. +*/ +XsResultValue SerialInterface::readData(XsFilePos maxLength, XsByteArray& data) +{ + if (!isOpen()) + return (m_lastResult = XRV_NOPORTOPEN); + +#ifdef _WIN32 + DWORD length; + data.setSize((XsSize) maxLength); + BOOL rres = ::ReadFile(m_handle, data.data(), (DWORD) maxLength, &length, NULL); + data.pop_back((XsSize)(maxLength - length)); + JLTRACEG("ReadFile result " << rres << ", length " << length); + + if (!rres) + { + DWORD wErr = ::GetLastError(); + JLALERTG("ReadFile returned windows error " << wErr); + if (wErr == ERROR_ACCESS_DENIED) + return (m_lastResult = XRV_UNEXPECTED_DISCONNECT); + if (wErr >= ERROR_INVALID_FUNCTION && wErr <= ERROR_INVALID_HANDLE) + return (m_lastResult = XRV_NOFILEORPORTOPEN); + return (m_lastResult = XRV_ERROR); + } + + if (length == 0) + return (m_lastResult = XRV_TIMEOUT); +#else + fd_set fd; + fd_set err; + timeval timeout; + FD_ZERO(&fd); + FD_ZERO(&err); + FD_SET(m_handle, &fd); + FD_SET(m_handle, &err); + + 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); + if (res < 0 || FD_ISSET(m_handle, &err)) + { + data.clear(); + return (m_lastResult = XRV_ERROR); + } + else if (res == 0) + { + data.clear(); + return (m_lastResult = XRV_TIMEOUT); + } + + data.setSize(maxLength); + int length = read(m_handle, (void*)data.data(), maxLength); + if (length > 0) + data.pop_back(maxLength - length); + else + { + int err = errno; + + data.clear(); + switch (err) + { + case EAGAIN: +#if defined(EWOULDBLOCK) && EWOULDBLOCK != EAGAIN + case EWOULDBLOCK: +#endif + return XRV_TIMEOUT; + + case EIO: + return XRV_UNEXPECTED_DISCONNECT; + + default: + break; + } + } +#if defined(JLDEF_BUILD) && JLDEF_BUILD <= JLL_TRACE && !defined(ANDROID) + serial_icounter_struct ic; + res = ioctl(m_handle, TIOCGICOUNT, &ic); + if (res == 0) + { + JLTRACEG("rx: " << ic.rx); + JLTRACEG("tx: " << ic.tx); + JLTRACEG("frame " << ic.frame); + JLTRACEG("overrun " << ic.overrun); + JLTRACEG("buf_overrun " << ic.buf_overrun); + } +#endif +#endif + +#ifdef LOG_RX_TX + if (length > 0) + { + CHECK_STATE_RX(length, data, rx_log); + + if (!rx_log.isOpen()) + { + char fname[XS_MAX_FILENAME_LENGTH]; +#ifdef _WIN32 + sprintf(fname, "rx_%03d_%d.log", (int32_t) m_port, m_baudrate); +#else + char* devname = strrchr(m_portname, '/'); + sprintf(fname, "rx_%s_%d.log", devname + 1, XsBaud::rateToNumeric(m_baudrate)); +#endif + makeFilenameUnique(fname, state); + + rx_log.create(XsString(fname), true); + } + rx_log.write(data.data(), 1, length); +#ifdef LOG_RX_TX_FLUSH + rx_log.flush(); +#endif + } +#endif + + JLTRACEG("returned success, read " << length << " of " << maxLength << " bytes, first: " << JLHEXLOG(data[0])); + return (m_lastResult = XRV_OK); +} + +/*! \brief Set the default timeout value to use in blocking operations. + \details This function sets the value of m_timeout. There is no infinity value. The value 0 + means that all blocking operations now become polling (non-blocking) operations. + If the value is set to or from 0, the low-level serial port settings may be + changed in addition to the m_timeout value. + \param ms The new timeout in milliseconds + \returns XRV_OK if the function succeeded +*/ +XsResultValue SerialInterface::setTimeout(const uint32_t ms) +{ + JLDEBUGG("Setting timeout to " << ms << " ms"); + + m_timeout = ms; +#ifdef _WIN32 + // Set COM timeouts + COMMTIMEOUTS commTimeouts; + + if (!GetCommTimeouts(m_handle, &commTimeouts)) // Fill CommTimeouts structure + return m_lastResult = XRV_ERROR; + + // immediate return if data is available, wait 1ms otherwise + if (m_timeout > 0) + { + commTimeouts.ReadIntervalTimeout = 0; + commTimeouts.ReadTotalTimeoutConstant = m_timeout; // ms time + commTimeouts.ReadTotalTimeoutMultiplier = 0; + commTimeouts.WriteTotalTimeoutConstant = m_timeout; + commTimeouts.WriteTotalTimeoutMultiplier = 0; + } + else + { + // immediate return whether data is available or not + commTimeouts.ReadIntervalTimeout = MAXDWORD; + commTimeouts.ReadTotalTimeoutConstant = 0; + commTimeouts.ReadTotalTimeoutMultiplier = 0; + commTimeouts.WriteTotalTimeoutConstant = 0; + commTimeouts.WriteTotalTimeoutMultiplier = 0; + } + + if (!SetCommTimeouts(m_handle, &commTimeouts)) // Set CommTimeouts structure + return m_lastResult = XRV_ERROR; +#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 + + // Set the new options for the port if it is open + if (isOpen()) + tcsetattr(m_handle, TCSANOW, &m_commState); +#endif + return (m_lastResult = XRV_OK); +} + +/*! \brief Wait for data to arrive or a timeout to occur. + \details The function waits until \c maxLength data is available or until a timeout occurs. + The function returns success if data is available or XsResultValue::TIMEOUT if a + timeout occurred. A timeout value of 0 indicates that the default timeout stored + in the class should be used. + \param maxLength The maximum number of bytes to read before returning + \param data The buffer to put the read data in. + \returns XRV_OK if \a maxLength bytes were read, XRV_TIMEOUT if less was read, XRV_TIMEOUTNODATA if nothing was read +*/ +XsResultValue SerialInterface::waitForData(XsFilePos maxLength, XsByteArray& data) +{ + data.clear(); + data.reserve((XsSize) maxLength); + + //char *data = (char *)&_data[0]; + JLTRACEG("timeout=" << m_timeout << ", maxLength=" << maxLength); + uint32_t timeout = m_timeout; + + uint32_t eTime = XsTime_getTimeOfDay(NULL, NULL) + timeout; + // uint32_t newLength = 0; + + while (((XsFilePos) data.size() < maxLength) && (XsTime_getTimeOfDay(NULL, NULL) <= eTime)) + { + XsByteArray raw; + + if (readData(maxLength - (XsFilePos) data.size(), raw) != XRV_OK) + return m_lastResult; + data.append(raw); + } + JLTRACEG("Read " << data.size() << " of " << maxLength << " bytes"); + + if ((XsFilePos) data.size() < maxLength) + return (m_lastResult = XRV_TIMEOUT); + else + return (m_lastResult = XRV_OK); +} + +/*! \copydoc IoInterface::writeData + \note The default timeout is respected in this operation. +*/ +XsResultValue SerialInterface::writeData(const XsByteArray& data, XsFilePos* written) +{ + XsFilePos bytes; + if (written == NULL) + written = &bytes; + + if (!isOpen()) + return (m_lastResult = XRV_NOPORTOPEN); + + *written = 0; + +#ifdef _WIN32 + DWORD lwritten = 0; + if (WriteFile(m_handle, data.data(), (DWORD) data.size(), &lwritten, NULL) == 0) + { + DWORD wErr = ::GetLastError(); + JLALERTG("WriteFile returned windows error " << wErr); + if (wErr == ERROR_ACCESS_DENIED) + return (m_lastResult = XRV_UNEXPECTED_DISCONNECT); + return (m_lastResult = XRV_ERROR); + } + + *written = lwritten; +#else + ssize_t result = write(m_handle, (const void*)data.data(), data.size()); + if (result <= 0) + { + int err = errno; + *written = 0; + switch (err) + { + case EAGAIN: +#if defined(EWOULDBLOCK) && EWOULDBLOCK != EAGAIN + case EWOULDBLOCK: +#endif + return XRV_TIMEOUT; + + case EIO: + return XRV_UNEXPECTED_DISCONNECT; + + /* we don't expect any other errors to actually occur */ + default: + break; + } + } + + if (result < 0) + *written = 0; + else + *written = result; +#endif + +#ifdef LOG_RX_TX + if (written[0] > 0) + { + CHECK_STATE_TX(written[0], data, tx_log); + if (!tx_log.isOpen()) + { + char fname[XS_MAX_FILENAME_LENGTH]; +#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)); +#endif + makeFilenameUnique(fname, state); + + tx_log.create(XsString(fname), true); + } + tx_log.write(data.data(), 1, *written); +#ifdef LOG_RX_TX_FLUSH + tx_log.flush(); +#endif + } +#endif + + return (m_lastResult = XRV_OK); +} + +/*! \brief Cancel any pending io requests */ +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. + */ + //CancelIoEx(m_handle, NULL); +#endif +} + +/*! \brief Logical \a or operator for flow controls */ +SerialInterface::PortOptions operator|(SerialInterface::PortOptions lhs, SerialInterface::PortOptions rhs) +{ + return static_cast(static_cast(lhs) | static_cast(rhs)); +} + +/*! \brief Logical \a and operator for flow controls */ +SerialInterface::PortOptions operator&(SerialInterface::PortOptions lhs, SerialInterface::PortOptions rhs) +{ + return static_cast(static_cast(lhs) & static_cast(rhs)); +} + +/*! \brief Logical inversion operator for flow controls */ +SerialInterface::PortOptions operator~(SerialInterface::PortOptions lhs) +{ + return static_cast(~static_cast(lhs)); +} + +/*! \brief Apply the specified options for the hardware control lines. + \param options The options to enable (flow control, stop bits) + \param portLinesOptions The options for the hardware control lines (RTS/DTR levels) + \param p The reference to an enabled bits +*/ +void SerialInterface::applyHwControlLinesOptions(PortOptions options, int portLinesOptions, int& p) +{ + const XsPortLinesOptions pLinesOpts = static_cast(portLinesOptions); + +#ifdef _WIN32 + + // DTR Line Options + if (!(options & PO_DtrDsrFlowControl)) + { + // Flow Control is disabled + if ((pLinesOpts == XPLO_Invalid) || (pLinesOpts & XPLO_DTR_Ignore)) + { + // Default behaviour + if (!EscapeCommFunction(m_handle, SETDTR)) // Set DTR (Calibration sensors need DTR to startup, won't hurt otherwise) + p = XRV_ERROR; + } + else + { + // Handle flow control line options + if (((pLinesOpts & (XPLO_DTR_Set | XPLO_DTR_Clear)) == (XPLO_DTR_Set | XPLO_DTR_Clear)) + || ((pLinesOpts & (XPLO_DTR_Set | XPLO_DTR_Clear)) == 0)) + { + // Line options are not valid, both Set and Clear are high or down + if (!EscapeCommFunction(m_handle, SETDTR)) // Default behaviour + p = XRV_ERROR; + } + else if (pLinesOpts & XPLO_DTR_Set) + { + if (!EscapeCommFunction(m_handle, SETDTR)) // Set DTR + p = XRV_ERROR; + } + else + { + // XPLO_DTR_Clear is Set + assert(pLinesOpts & XPLO_DTR_Clear); + + if (!EscapeCommFunction(m_handle, CLRDTR)) // Clear DTR + p = XRV_ERROR; + } + } + } + + // RTS Line Options + if (!(options & PO_RtsCtsFlowControl)) + { + // Flow Control is disabled + if ((pLinesOpts != XPLO_Invalid) && !(pLinesOpts & XPLO_RTS_Ignore)) + { + bool set = !!(pLinesOpts & XPLO_RTS_Set); + bool clr = !!(pLinesOpts & XPLO_RTS_Clear); + if (set != clr) + { + // Only one between Set and Clear is high + if (set) + { + if (!EscapeCommFunction(m_handle, SETRTS)) // Set RTS + p = XRV_ERROR; + } + else + { + // XPLO_RTS_Clear is Set + assert(pLinesOpts & XPLO_RTS_Clear); + + if (!EscapeCommFunction(m_handle, CLRRTS)) // Clear RTS + p = XRV_ERROR; + } + } + } + } + +#else + + // DTR Line + if (!(options & PO_DtrDsrFlowControl)) + { + // Flow control is disabled + if ((pLinesOpts != XPLO_Invalid) && !(pLinesOpts & XPLO_DTR_Ignore)) + { + if (((pLinesOpts & (XPLO_DTR_Set | XPLO_DTR_Clear)) == (XPLO_DTR_Set | XPLO_DTR_Clear)) + || ((pLinesOpts & (XPLO_DTR_Set | XPLO_DTR_Clear)) == 0)) + { + // Line options are not valid, both Set and Clear are high or down + p = setBitsEnabled(p, TIOCM_DTR, true); // Default behaviour + } + else + p = setBitsEnabled(p, TIOCM_DTR, (pLinesOpts & XPLO_DTR_Set)); + } + else + p = setBitsEnabled(p, TIOCM_DTR, true); // Default behaviour + } + + // RTS Line + if (!(options & PO_RtsCtsFlowControl)) + { + if ((pLinesOpts != XPLO_Invalid) && !(pLinesOpts & XPLO_RTS_Ignore)) + { + if (((pLinesOpts & (XPLO_RTS_Set | XPLO_RTS_Clear)) == (XPLO_RTS_Set | XPLO_RTS_Clear)) + || ((pLinesOpts & (XPLO_RTS_Set | XPLO_RTS_Clear)) == 0)) + { + // Line options are not valid, both Set and Clear are high or down + p = setBitsEnabled(p, TIOCM_RTS, true); // Default behaviour + } + else + p = setBitsEnabled(p, TIOCM_RTS, (pLinesOpts & XPLO_RTS_Set)); + } + else + p = setBitsEnabled(p, TIOCM_RTS, true); // Default behaviour + } + +#endif +} diff --git a/extern/xspublic/xscontroller/serialinterface.h b/extern/xspublic/xscontroller/serialinterface.h new file mode 100644 index 0000000..0f53afa --- /dev/null +++ b/extern/xspublic/xscontroller/serialinterface.h @@ -0,0 +1,114 @@ + +// Copyright (c) 2003-2022 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 SERIALINTERFACE_H +#define SERIALINTERFACE_H + +#include "streaminterface.h" +#include +#include +#include +#include + +struct XsPortInfo; + +////////////////////////////////////////////////////////////////////////////////////////// +///////////////////////////////////////// SerialInterface ///////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////// + +/*! \brief The low-level serial communication class. +*/ +class SerialInterface : public StreamInterface +{ +private: + void applyHwControlLinesOptions(PortOptions options, int portLinesOptions, int& p); + + // these are only used when LOG_RX_TX is defined in rx_tx_log.h + XsFile rx_log; + XsFile tx_log; +protected: + //! 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. + uint32_t m_endTime; + //! The last result of an operation + mutable XsResultValue m_lastResult; + //! The opened COM port nr + uint16_t m_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 +public: + SerialInterface(); + virtual ~SerialInterface(); + + // Function overrides + XsResultValue close(void) override; + XsResultValue closeLive(void); + XsResultValue flushData(void) override; + bool isOpen(void) const; + XsResultValue getLastResult(void) const; + XsResultValue writeData(const XsByteArray& data, XsFilePos* written = 0) override; + XsResultValue readData(XsFilePos maxLength, XsByteArray& data) override; + + void cancelIo(void) const; + + // Other functions + XsResultValue escape(XsControlLine mask, XsControlLine state); + XsBaudRate getBaudrate(void) const; + XsIoHandle getHandle(void) const; + uint16_t getPortNumber(void) const; + void getPortName(XsString& portname) 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; + + XSENS_DISABLE_COPY(SerialInterface); +}; + +SerialInterface::PortOptions operator|(SerialInterface::PortOptions lhs, SerialInterface::PortOptions rhs); +SerialInterface::PortOptions operator&(SerialInterface::PortOptions lhs, SerialInterface::PortOptions rhs); +SerialInterface::PortOptions operator~(SerialInterface::PortOptions lhs); + +#endif diff --git a/extern/xspublic/xscontroller/serialportcommunicator.cpp b/extern/xspublic/xscontroller/serialportcommunicator.cpp new file mode 100644 index 0000000..3eec597 --- /dev/null +++ b/extern/xspublic/xscontroller/serialportcommunicator.cpp @@ -0,0 +1,72 @@ + +// Copyright (c) 2003-2022 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 "serialportcommunicator.h" +#include + +/*! \class SerialPortCommunicator + \brief A class that uses serial port to communicate +*/ + +/*! \brief Constructs new SerialPortCommunicator +*/ +Communicator* SerialPortCommunicator::construct() +{ + return new SerialPortCommunicator; +} + +/*! Default constructor +*/ +SerialPortCommunicator::SerialPortCommunicator() +{ +} + +SerialPortCommunicator::~SerialPortCommunicator() +{ +} + +/*! \brief Creates a stream interface + \param pi The port to use + \returns The shared pointer to a stream interface +*/ +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; + setLastResult(stream->open(pi, 65536, 65536, options)); + + return stream; +} diff --git a/extern/xspublic/xscontroller/serialportcommunicator.h b/extern/xspublic/xscontroller/serialportcommunicator.h new file mode 100644 index 0000000..ecf95db --- /dev/null +++ b/extern/xspublic/xscontroller/serialportcommunicator.h @@ -0,0 +1,49 @@ + +// Copyright (c) 2003-2022 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 SERIALPORTCOMMUNICATOR_H +#define SERIALPORTCOMMUNICATOR_H + +#include "serialcommunicator.h" + +class SerialPortCommunicator : public SerialCommunicator +{ +public: + static Communicator* construct(); + SerialPortCommunicator(); + +protected: + ~SerialPortCommunicator(); + std::shared_ptr createStreamInterface(const XsPortInfo& pi) override; +}; + +#endif diff --git a/extern/xspublic/xscontroller/simpleprotocolmanager.h b/extern/xspublic/xscontroller/simpleprotocolmanager.h new file mode 100644 index 0000000..bf18cfb --- /dev/null +++ b/extern/xspublic/xscontroller/simpleprotocolmanager.h @@ -0,0 +1,70 @@ + +// Copyright (c) 2003-2022 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 SIMPLEPROTOCOLMANAGER_H +#define SIMPLEPROTOCOLMANAGER_H + +#include "iprotocolmanager.h" +#include "protocolhandler.h" + +/*! \brief A very basic protocol manager + \details This protocol manager supports only the xbus protocol +*/ +class SimpleProtocolManager : public IProtocolManager +{ +public: + /*! \copydoc IProtocolManager::findMessage + */ + MessageLocation findMessage(XsProtocolType& type, const XsByteArray& raw) override + { + return m_protocolHandler.findMessage(type, raw); + }; + + /*! \copydoc IProtocolManager::convertToMessage + */ + XsMessage convertToMessage(XsProtocolType& type, MessageLocation& location, const XsByteArray& raw) override + { + (void)type; + return m_protocolHandler.convertToMessage(location, raw); + }; + + /*! \copydoc IProtocolManager::validateMessage + */ + bool validateMessage(XsMessage const& msg) const override + { + return msg.isChecksumOk(); + } +private: + ProtocolHandler m_protocolHandler; //!< Standard protocol handler; used for finding messages. +}; + +#endif diff --git a/extern/xspublic/xscontroller/streaminterface.cpp b/extern/xspublic/xscontroller/streaminterface.cpp new file mode 100644 index 0000000..5479a96 --- /dev/null +++ b/extern/xspublic/xscontroller/streaminterface.cpp @@ -0,0 +1,39 @@ + +// Copyright (c) 2003-2022 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 "streaminterface.h" + +/*! \brief Destroy the stream interface +*/ +StreamInterface::~StreamInterface() +{ +} diff --git a/extern/xspublic/xscontroller/streaminterface.h b/extern/xspublic/xscontroller/streaminterface.h new file mode 100644 index 0000000..f443b03 --- /dev/null +++ b/extern/xspublic/xscontroller/streaminterface.h @@ -0,0 +1,66 @@ + +// Copyright (c) 2003-2022 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 STREAMINTERFACE_H +#define STREAMINTERFACE_H + +#include "iointerface.h" +#include + +/*! \brief A stream interface + \details This class provides an interface for dealing with streaming I/O devices, such as a COM + port or a USB port. +*/ +class StreamInterface : public IoInterface +{ +public: + /*! \brief Destroy the stream interface + */ + ~StreamInterface(); + + /*! \brief Set the read/write timeout to \a ms + A timeout of 0 means non-blocking operation of writeData() and readData(). + */ + XsResultValue setTimeout(uint32_t ms) override = 0; + + /*! \brief The timeout used for read/write operations + */ + virtual uint32_t getTimeout() const = 0; + + XSENS_DISABLE_COPY(StreamInterface); +protected: + /*! \brief Create a stream interface + */ + inline StreamInterface() {} +}; + +#endif diff --git a/extern/xspublic/xscontroller/supportedsyncsettings.cpp b/extern/xspublic/xscontroller/supportedsyncsettings.cpp new file mode 100644 index 0000000..e8445be --- /dev/null +++ b/extern/xspublic/xscontroller/supportedsyncsettings.cpp @@ -0,0 +1,687 @@ + +// Copyright (c) 2003-2022 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 "supportedsyncsettings.h" + +#include "synclinemk4.h" +#include "synclinegmt.h" +#include +#include + +namespace Synchronization +{ + +/*! \brief Returns the list of the supported synchronizations settings for the specified device id + Each item in the list represent one possible function-line settings. +*/ +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 supportedSyncSettingsForMti7AndMTi8Devices();//this used for both the MTi-7 and MTi-8 + else if (deviceId.isMti6X0() && deviceId.isGnss() && deviceId.hasInternalGnss()) + return supportedSyncSettingsForMt6x0IntGnssDevice(); + else if (deviceId.isMti6X0() && deviceId.isGnss()) + 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 +*/ +bool supportsSyncSettings(XsDeviceId const& deviceId) +{ + if (supportedSyncSettings(deviceId).size()) + return true; + else + return false; +} + +/*! \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) +{ + if (deviceId.isAwindaX()) + return isAwindaSettingCompatible(setting1, setting2); + else if (deviceId.isSyncStationX()) + return isSyncStationSettingCompatible(setting1, setting2); + else + return true; // Always compatible for devices other than awinda for now +} + +/*! \returns the time resolution in microseconds for a device with device id \a deviceId + For example if the precision is 1 millisecond, 1000 is returned + \note Currently MVN studio only supports 1 or 1000 +*/ +unsigned int timeResolutionInMicroseconds(XsDeviceId const& deviceId) +{ + if (deviceId.isAwindaX()) + return awindaTimeResolutionInMicroseconds(); + else if (deviceId.isSyncStationX()) + return syncStationTimeResolutionInMicroseconds(); + else if (deviceId.isMti() || deviceId.isMtig()) + return mtiTimeResolutionInMicroseconds(); + else if (deviceId.isMtx2()) + return mtx2TimeResolutionInMicroseconds(); + else + return 1; +} + +/*! \brief get list of supported synchronizations settings for an Mtx2Device */ +XsSyncSettingArray supportedSyncSettingsForMtx2Device() +{ + XsSyncSettingArray settings; + return settings; +} + +/*! \brief get list of supported synchronizations settings for an MtiBaseDevice */ +#define SUPPORT_XSL4_BiIn 0 // Bidirectional 1 <- In is not supported anymore +XsSyncSettingArray supportedSyncSettingsForMtiDevice() +{ + XsSyncSetting s; + XsSyncSettingArray settings; + + //enable/disabled parameters per functions + s.m_polarity = XSP_RisingEdge; + s.m_offset = 1; + s.m_skipFactor = 1; + s.m_triggerOnce = 0; + s.m_skipFirst = 0; + s.m_pulseWidth = 0; + s.m_clockPeriod = 0; + + s.m_function = XSF_TriggerIndication; + s.m_line = xsl4ToXsl(XSL4_In); + settings.push_back(s); + +#if SUPPORT_XSL4_BiIn == 1 + s.m_line = xsl4ToXsl(XSL4_BiIn); + settings.push_back(s); +#endif + + s.m_function = XSF_SendLatest; + s.m_line = xsl4ToXsl(XSL4_In); + settings.push_back(s); + + //----- + s.m_function = XSF_SendLatest; + s.m_line = xsl4ToXsl(XSL4_ReqData); + // Disable polarity, offset and skip factor for ReqData + s.m_polarity = XSP_None; + s.m_skipFactor = 0; + s.m_offset = 0; + settings.push_back(s); + + // Enable polarity, offset and skip factor for next settings + s.m_polarity = XSP_RisingEdge; + s.m_skipFactor = 1; + s.m_offset = 1; + //----- + +#if SUPPORT_XSL4_BiIn == 1 + s.m_line = xsl4ToXsl(XSL4_BiIn); + settings.push_back(s); +#endif + + s.m_function = XSF_IntervalTransitionMeasurement; + s.m_line = xsl4ToXsl(XSL4_BiOut); + s.m_pulseWidth = 1; + settings.push_back(s); + + s.m_function = XSF_ClockBiasEstimation; + s.m_line = xsl4ToXsl(XSL4_ClockIn); + s.m_offset = 0; + s.m_pulseWidth = 0; + s.m_clockPeriod = 1; + settings.push_back(s); + + s.m_function = XSF_StartSampling; + s.m_line = xsl4ToXsl(XSL4_In); + s.m_skipFactor = 0; + s.m_clockPeriod = 0; + s.m_pulseWidth = 0; + s.m_offset = 1; + settings.push_back(s); + + return settings; +} + +/*! \brief get list of supported synchronizations settings for an Mti6x0Device */ +XsSyncSettingArray supportedSyncSettingsForMt6x0Device() +{ + XsSyncSetting s; + XsSyncSettingArray settings; + + //enable/disabled parameters per functions + s.m_polarity = XSP_RisingEdge; + s.m_offset = 0; + s.m_skipFactor = 1; + s.m_triggerOnce = 1; + s.m_skipFirst = 1; + s.m_pulseWidth = 0; + s.m_clockPeriod = 0; + + s.m_function = XSF_TriggerIndication; + s.m_line = XSL_In1; + settings.push_back(s); + s.m_line = XSL_In2; + settings.push_back(s); + + s.m_function = XSF_SendLatest; + s.m_line = XSL_In1; + settings.push_back(s); + s.m_line = XSL_In2; + settings.push_back(s); + + //----- + s.m_function = XSF_SendLatest; + s.m_line = XSL_ReqData; + // Disable polarity, offset and skip factor for ReqData + s.m_polarity = XSP_None; + s.m_skipFactor = 0; + s.m_offset = 0; + s.m_skipFirst = 0; + s.m_triggerOnce = 0; + settings.push_back(s); + + // Enable polarity, offset and skip factor for next settings + s.m_polarity = XSP_RisingEdge; + s.m_skipFactor = 1; + s.m_offset = 1; + //----- + + s.m_function = XSF_IntervalTransitionMeasurement; + s.m_line = XSL_Out1; + s.m_pulseWidth = 1; + settings.push_back(s); + 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_In1; + settings.push_back(s); + s.m_line = XSL_In2; + settings.push_back(s); + + s.m_function = XSF_StartSampling; + s.m_skipFactor = 0; + s.m_clockPeriod = 0; + s.m_pulseWidth = 0; + s.m_offset = 1; + s.m_skipFirst = 1; + s.m_line = XSL_In1; + settings.push_back(s); + s.m_line = XSL_In2; + settings.push_back(s); + + return settings; +} + +/*! \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(); + + XsSyncSetting s; + + // add XSF_Gnss1Pps with gnss clock in setting + s.m_function = XSF_Gnss1Pps; + s.m_polarity = XSP_None; + s.m_offset = 0; + s.m_pulseWidth = 1; + s.m_clockPeriod = 0; + s.m_skipFactor = 0; + s.m_line = XSL_In1; + settings.push_back(s); + s.m_line = XSL_In2; + settings.push_back(s); + + return settings; +} + +/*! \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; + + //enable/disabled parameters per functions + s.m_polarity = XSP_RisingEdge; + s.m_offset = 0; + s.m_skipFactor = 1; + s.m_triggerOnce = 1; + s.m_skipFirst = 1; + s.m_pulseWidth = 0; + s.m_clockPeriod = 0; + + s.m_function = XSF_TriggerIndication; + s.m_line = XSL_In1; + settings.push_back(s); + s.m_line = XSL_In2; + settings.push_back(s); + + s.m_function = XSF_SendLatest; + s.m_line = XSL_In1; + settings.push_back(s); + s.m_line = XSL_In2; + settings.push_back(s); + + //----- + s.m_function = XSF_SendLatest; + s.m_line = XSL_ReqData; + // Disable polarity, offset and skip factor for ReqData + s.m_polarity = XSP_None; + s.m_skipFactor = 0; + s.m_offset = 0; + s.m_skipFirst = 0; + s.m_triggerOnce = 0; + settings.push_back(s); + + // Enable polarity, offset and skip factor for next settings + s.m_polarity = XSP_RisingEdge; + s.m_skipFactor = 1; + s.m_offset = 1; + //----- + + s.m_function = XSF_IntervalTransitionMeasurement; + s.m_line = XSL_Out1; + s.m_pulseWidth = 1; + settings.push_back(s); + s.m_skipFirst = 0; + s.m_triggerOnce = 0; + + s.m_function = XSF_StartSampling; + s.m_skipFactor = 0; + s.m_clockPeriod = 0; + s.m_pulseWidth = 0; + s.m_offset = 1; + s.m_skipFirst = 1; + s.m_line = XSL_In1; + settings.push_back(s); + s.m_line = XSL_In2; + settings.push_back(s); + + // add XSF_Gnss1Pps with gnss clock in setting + s.m_function = XSF_Gnss1Pps; + s.m_polarity = XSP_None; + s.m_offset = 0; + s.m_pulseWidth = 1; + s.m_clockPeriod = 0; + s.m_skipFactor = 0; + s.m_line = XSL_In3; + settings.push_back(s); + + 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); + + return settings; +} + +XsSyncSettingArray supportedSyncSettingsForMark5MtigX10Device() +{ + XsSyncSettingArray settings = supportedSyncSettingsForMtigDevice(); + + XsSyncSetting timePulse1Pps(xsl4ToXsl(XSL4_BiOut), XSF_Gnss1Pps, XSP_None, 0, 0, 0, 0, 0, 0); + settings.push_back(timePulse1Pps); + + return settings; +} + +/*! \brief get list of supported synchronizations settings for an MtigDevice */ +XsSyncSettingArray supportedSyncSettingsForMtigDevice() +{ + //extend ClockBiasEstimation to support GPS clock line + XsSyncSettingArray settings = supportedSyncSettingsForMtiDevice(); + 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) + { + XsSyncSetting s = *it; + s.m_line = xsl4ToXsl(XSL4_GnssClockIn); + // Disable all parameters + s.m_polarity = XSP_None; + s.m_pulseWidth = 0; + s.m_offset = 0; + s.m_skipFactor = 0; + s.m_clockPeriod = 0; + settings.insert(s, it); + break; + } + } + + return settings; +} + +/*! \brief get list of supported synchronizations settings for an Mti7 or MTi8 Device */ +XsSyncSettingArray supportedSyncSettingsForMti7AndMTi8Devices() +{ + XsSyncSettingArray settings = supportedSyncSettingsForMtiXDevice(); + + XsSyncSetting s; + + // add XSF_ClockBiasEstimation with clock line in setting + s.m_function = XSF_ClockBiasEstimation; + s.m_line = xsl4ToXsl(XSL4_In); + s.m_offset = 0; + s.m_pulseWidth = 0; + s.m_clockPeriod = 1; + s.m_skipFactor = 1; + settings.push_back(s); + + // add XSF_ClockBiasEstimation with gps line in setting + s.m_function = XSF_ClockBiasEstimation; + s.m_line = xsl4ToXsl(XSL4_GnssClockIn); + s.m_polarity = XSP_None; + s.m_offset = 0; + s.m_pulseWidth = 0; + s.m_clockPeriod = 0; + s.m_skipFactor = 0; + settings.push_back(s); + + // add XSF_Gnss1Pps with gnss clock in setting + s.m_function = XSF_Gnss1Pps; + s.m_line = xsl4ToXsl(XSL4_Gnss1Pps); + s.m_polarity = XSP_None; + s.m_offset = 0; + s.m_pulseWidth = 1; + s.m_clockPeriod = 0; + s.m_skipFactor = 0; + settings.push_back(s); + + return settings; +} + +/*! \brief get list of supported synchronizations settings for an MtDevice */ +XsSyncSettingArray supportedSyncSettingsForMtDevice() +{ + XsSyncSetting s; + XsSyncSettingArray settings; + + //enable/disabled options + s.m_polarity = XSP_RisingEdge; + s.m_skipFactor = 1; + s.m_offset = 1; + s.m_triggerOnce = 0; + s.m_skipFirst = 0; + s.m_pulseWidth = 0; + s.m_clockPeriod = 0; + + s.m_function = XSF_TriggerIndication; + s.m_line = XSL_In1; + settings.push_back(s); + + s.m_function = XSF_SendLatest; + s.m_line = XSL_In1; + settings.push_back(s); + + s.m_function = XSF_IntervalTransitionMeasurement; + s.m_line = XSL_Out1; + s.m_pulseWidth = 1; + settings.push_back(s); + + s.m_function = XSF_ClockBiasEstimation; + s.m_line = XSL_ClockIn; + s.m_pulseWidth = 0; + s.m_clockPeriod = 1; + s.m_offset = 0; + settings.push_back(s); + + + return settings; +} + +/*! \brief get list of supported synchronizations settings for MtDevice with GNSS */ +XsSyncSettingArray supportedSyncSettingsForGnssDevice() +{ + XsSyncSetting s; + XsSyncSettingArray settings = supportedSyncSettingsForMtDevice(); + + s.m_function = XSF_ClockBiasEstimation; + s.m_line = XSL_GnssClockIn; + // Disable all parameters + s.m_polarity = XSP_None; + s.m_pulseWidth = 0; + s.m_offset = 0; + s.m_skipFactor = 0; + s.m_clockPeriod = 0; + settings.push_back(s); + + return settings; +} + +/*! \brief get list of supported synchronization settings for an MtiX Device */ +XsSyncSettingArray supportedSyncSettingsForMtiXDevice() +{ + XsSyncSetting s; + XsSyncSettingArray settings; + + //enable/disabled parameters per functions + s.m_polarity = XSP_RisingEdge; + s.m_offset = 1; + s.m_skipFactor = 1; + s.m_triggerOnce = 0; + s.m_skipFirst = 0; + s.m_pulseWidth = 0; + s.m_clockPeriod = 0; + + s.m_function = XSF_SendLatest; + s.m_line = xslgmtToXsl(XSLGMT_In1); + settings.push_back(s); + + //----- + s.m_function = XSF_SendLatest; + s.m_line = xslgmtToXsl(XSLGMT_ReqData); + // Disable polarity, offset and skip factor for ReqData + s.m_polarity = XSP_None; + s.m_skipFactor = 0; + s.m_offset = 0; + settings.push_back(s); + + return settings; +} + +/*! \brief get list of supported synchronizations settings for an Awinda base station */ +XsSyncSettingArray supportedSyncSettingsForAwindaBaseStation() +{ + XsSyncSetting s; + XsSyncSettingArray settings; + + // ----- Input triggers ----- + std::set inputSyncFunctions; + inputSyncFunctions.insert(XSF_StartRecordingIn); + inputSyncFunctions.insert(XSF_StopRecordingIn); + inputSyncFunctions.insert(XSF_ResetTimer); + inputSyncFunctions.insert(XSF_TriggerIndication); + + for (XsSyncFunction const& syncFunction : inputSyncFunctions) + { + s.m_line = XSL_In1; + s.m_polarity = XSP_RisingEdge; + s.m_function = syncFunction; + s.m_pulseWidth = 0; + s.m_offset = 0; + s.m_skipFirst = 1; + s.m_skipFactor = 1; + s.m_clockPeriod = 0; + s.m_triggerOnce = 1; + + settings.push_back(s); + + s.m_line = XSL_In2; + s.m_polarity = XSP_RisingEdge; + s.m_function = syncFunction; + s.m_pulseWidth = 0; + s.m_offset = 0; + s.m_skipFirst = 1; + s.m_skipFactor = 1; + s.m_clockPeriod = 0; + s.m_triggerOnce = 1; + + settings.push_back(s); + } + + // ----- Output triggers ----- + std::set outputSyncFunctions; + outputSyncFunctions.insert(XSF_StartRecordingOut); + outputSyncFunctions.insert(XSF_StopRecordingOut); + outputSyncFunctions.insert(XSF_GotoOperational); + outputSyncFunctions.insert(XSF_IntervalTransitionMeasurement); + outputSyncFunctions.insert(XSF_IntervalTransitionRecording); + for (XsSyncFunction const& syncFunction : outputSyncFunctions) + { + s.m_line = XSL_Out1; + s.m_polarity = XSP_RisingEdge; + s.m_function = syncFunction; + s.m_pulseWidth = 1; + s.m_offset = 0; + s.m_skipFirst = 1; + s.m_skipFactor = 1; + s.m_clockPeriod = 0; + s.m_triggerOnce = 1; + + settings.push_back(s); + + s.m_line = XSL_Out2; + s.m_polarity = XSP_RisingEdge; + s.m_function = syncFunction; + s.m_pulseWidth = 1; + s.m_offset = 0; + s.m_skipFirst = 1; + s.m_skipFactor = 1; + s.m_clockPeriod = 0; + s.m_triggerOnce = 1; + + settings.push_back(s); + } + + return settings; +} + +/*! \returns true if the awinda settings \a setting1 and \a setting2 are compatible + \note This *very* specific for awinda + 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 isCompatible = true; + if (setting1.m_function == setting2.m_function) + { + bool areInputs = (setting1.m_line == XSL_In1 || setting1.m_line == XSL_In2) && (setting2.m_line == XSL_In1 || setting2.m_line == XSL_In2); + bool areOutputs = (setting1.m_line == XSL_Out1 || setting1.m_line == XSL_Out2) && (setting2.m_line == XSL_Out1 || setting2.m_line == XSL_Out2); + if (areInputs || areOutputs) + { + if (setting1.m_line != setting2.m_line) + { + if (setting1.m_polarity != setting2.m_polarity || + setting1.m_pulseWidth != setting2.m_pulseWidth || + setting1.m_offset != setting2.m_offset || + setting1.m_skipFirst != setting2.m_skipFirst || + setting1.m_skipFactor != setting2.m_skipFactor || + setting1.m_clockPeriod != setting2.m_clockPeriod || + setting1.m_triggerOnce != setting2.m_triggerOnce) + isCompatible = false; + } + } + } + return isCompatible; +} + +/*! \returns true if the sync station settings \a setting1 and \a setting2 are compatible + \note This *very* specific for a sync station + \sa isAwindaSettingCompatible +*/ +bool isSyncStationSettingCompatible(XsSyncSetting const& setting1, XsSyncSetting const& setting2) +{ + return isAwindaSettingCompatible(setting1, setting2); +} + +/*! \returns the time resolution in microseconds for an awinda device +*/ +unsigned int awindaTimeResolutionInMicroseconds() +{ + return 1000; +} + +/*! \returns the time resolution in microseconds for an sync station device +*/ +unsigned int syncStationTimeResolutionInMicroseconds() +{ + return awindaTimeResolutionInMicroseconds();; +} + +/*! \returns the time resolution in microseconds for an Mtmk4 device +*/ +unsigned int mtiTimeResolutionInMicroseconds() +{ + return 100; +} + +/*! \returns the time resolution in microseconds for an Mtx2 device +*/ +unsigned int mtx2TimeResolutionInMicroseconds() +{ + return 100; +} + +} // End namespace diff --git a/extern/xspublic/xscontroller/supportedsyncsettings.h b/extern/xspublic/xscontroller/supportedsyncsettings.h new file mode 100644 index 0000000..4bcc4f9 --- /dev/null +++ b/extern/xspublic/xscontroller/supportedsyncsettings.h @@ -0,0 +1,72 @@ + +// Copyright (c) 2003-2022 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 SUPPORTEDSYNCSETTINGS_H +#define SUPPORTEDSYNCSETTINGS_H + +#include +#include + +struct XsSyncSetting; +struct XsSyncSettingArray; + +namespace Synchronization +{ +bool supportsSyncSettings(XsDeviceId const& deviceId); +XsSyncSettingArray supportedSyncSettings(XsDeviceId const& deviceId); + +XsSyncSettingArray supportedSyncSettingsForMtx2Device(); +XsSyncSettingArray supportedSyncSettingsForMtiDevice(); +XsSyncSettingArray supportedSyncSettingsForMti7AndMTi8Devices(); +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 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(); +} + +#endif diff --git a/extern/xspublic/xscontroller/synclinegmt.c b/extern/xspublic/xscontroller/synclinegmt.c new file mode 100644 index 0000000..1701227 --- /dev/null +++ b/extern/xspublic/xscontroller/synclinegmt.c @@ -0,0 +1,107 @@ + +// Copyright (c) 2003-2022 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 "synclinegmt.h" + +/*! \addtogroup cinterface C Interface + @{ +*/ + +//! \brief Translate an SyncLineGmt into a generic XsSyncLine +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_Invalid: + default: + return XSL_Invalid; + } +} + +//! \brief Translate an XsSyncLine into a SyncLineGmt +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_CtsIn: + case XSL_Invalid: + case XSL_ExtTimepulseIn: + case XSL_Out2: + case XSL_RtsOut: + default: + return XSLGMT_Invalid; + } +} + +/*! @} */ diff --git a/extern/xspublic/xscontroller/synclinegmt.h b/extern/xspublic/xscontroller/synclinegmt.h new file mode 100644 index 0000000..2e14707 --- /dev/null +++ b/extern/xspublic/xscontroller/synclinegmt.h @@ -0,0 +1,75 @@ + +// Copyright (c) 2003-2022 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 XSSYNCLINEGMT_H +#define XSSYNCLINEGMT_H + +#include + +/*! \addtogroup enums Global enumerations + @{ +*/ +/*! \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 +*/ +enum SyncLineGmt +{ + XSLGMT_ClockIn = 0, //!< External clock sync \ref XSL_ClockIn + XSLGMT_GnssClockIn = 1, //!< GNSS clock sync \ref XSL_GnssClockIn + XSLGMT_In1 = 2, //!< Sync in 1 \ref XSL_In1 + XSLGMT_BiIn = 3, //!< Bidirectional sync line, configured as input \ref XSL_Bi1In + XSLGMT_BiOut = 4, //!< Bidirectional sync line, configured as output \ref XSL_Bi1Out + XSLGMT_ReqData = 6, //!< Serial data sync option, use XMID_ReqData message id for this \ref XSL_ReqData + XSLGMT_Out = 7, //!< Sync out 1 \ref XSL_Out1 + XSLGMT_Gnss1Pps = 8, //!< GNSS 1PPS pulse sync in line \ref XSL_Gnss1Pps + XSLGMT_In2 = 9, //!< Sync in 2 \ref XSL_In2 + XSLGMT_In3 = 10, //!< Sync in 3 + XSLGMT_Invalid +}; +/*! @} */ +typedef enum SyncLineGmt SyncLineGmt; + +#ifdef __cplusplus +extern "C" { +#endif + +XsSyncLine xslgmtToXsl(SyncLineGmt mk4Line); +SyncLineGmt xslToXslgmt(XsSyncLine line); + +#ifdef __cplusplus +} // extern "C" +#endif + +#endif diff --git a/extern/xspublic/xscontroller/synclinemk4.c b/extern/xspublic/xscontroller/synclinemk4.c new file mode 100644 index 0000000..cf331f5 --- /dev/null +++ b/extern/xspublic/xscontroller/synclinemk4.c @@ -0,0 +1,101 @@ + +// Copyright (c) 2003-2022 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 "synclinemk4.h" + +/*! \addtogroup cinterface C Interface + @{ +*/ + +//! \brief Translate an SyncLineMk4 into a generic XsSyncLine +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_Invalid: + default: + return XSL_Invalid; + } +} + +//! \brief Translate an XsSyncLine into a Mk4-specififc SyncLineMk4 +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_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/extern/xspublic/xscontroller/synclinemk4.h b/extern/xspublic/xscontroller/synclinemk4.h new file mode 100644 index 0000000..42d933c --- /dev/null +++ b/extern/xspublic/xscontroller/synclinemk4.h @@ -0,0 +1,69 @@ + +// Copyright (c) 2003-2022 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 XSSYNCLINEMK4_H +#define XSSYNCLINEMK4_H + +#include + +/*! \addtogroup enums Global enumerations + @{ +*/ +/*! \brief Synchronization line identifiers for the Mk4 devices, only to be used directly in Xbus messages */ +enum SyncLineMk4 +{ + XSL4_ClockIn = 0, //!< External clock sync \ref XSL_ClockIn + XSL4_GnssClockIn = 1, //!< GNSS clock sync \ref XSL_GnssClockIn + XSL4_In = 2, //!< Send data line \ref XSL_In1 + XSL4_BiIn = 3, //!< Bidirectional sync line, configured as input \ref XSL_Bi1In + XSL4_BiOut = 4, //!< Bidirectional sync line, configured as output \ref XSL_Bi1Out + XSL4_ExtTimepulseIn = 5,//!< External Timepulse input \ref XSL_ExtTimepulseIn + XSL4_ReqData = 6, //!< Serial data sync option, use XMID_ReqData message id for this \ref XSL_ReqData + XSL4_Gnss1Pps = 8, //!< GNSS 1PPS sync line \ref XSL_Gnss1Pps + + XSL4_Invalid +}; +/*! @} */ +typedef enum SyncLineMk4 SyncLineMk4; + +#ifdef __cplusplus +extern "C" { +#endif + +XsSyncLine xsl4ToXsl(SyncLineMk4 mk4Line); +SyncLineMk4 xslToXsl4(XsSyncLine line); + +#ifdef __cplusplus +} // extern "C" +#endif + +#endif diff --git a/extern/xspublic/xscontroller/udev.cpp b/extern/xspublic/xscontroller/udev.cpp new file mode 100644 index 0000000..8a04cb8 --- /dev/null +++ b/extern/xspublic/xscontroller/udev.cpp @@ -0,0 +1,298 @@ + +// Copyright (c) 2003-2022 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 "udev.h" +#include + +/*! \class Udev + \brief Class for dynamic loading of winusb +*/ +Udev::Udev(void) +{ + m_libraryLoader = new XsLibraryLoader(); + initLibrary(); +} + +Udev::~Udev(void) +{ + delete m_libraryLoader; +} + +void Udev::initLibrary() +{ + if (!m_libraryLoader->isLoaded()) + m_libraryLoader->load("libudev.so"); + + memset(&m_uDev, 0, sizeof(m_uDev)); + + if (m_libraryLoader->isLoaded()) + { + m_uDev.unew = (uDEV_new*)m_libraryLoader->resolve("udev_new"); + m_uDev.unref = (uDEV_unref*)m_libraryLoader->resolve("udev_unref"); + m_uDev.device_unref = (uDEV_device_unref*)m_libraryLoader->resolve("udev_device_unref"); + m_uDev.enumerate_new = (uDEV_enumerate_new*)m_libraryLoader->resolve("udev_enumerate_new"); + m_uDev.enumerate_add_match_subsystem = (uDEV_enumerate_add_match_subsystem*)m_libraryLoader->resolve("udev_enumerate_add_match_subsystem"); + m_uDev.enumerate_scan_devices = (uDEV_enumerate_scan_devices*)m_libraryLoader->resolve("udev_enumerate_scan_devices"); + m_uDev.enumerate_get_list_entry = (uDEV_enumerate_get_list_entry*)m_libraryLoader->resolve("udev_enumerate_get_list_entry"); + m_uDev.enumerate_unref = (uDEV_enumerate_unref*)m_libraryLoader->resolve("udev_enumerate_unref"); + m_uDev.list_entry_get_next = (uDEV_list_entry_get_next*)m_libraryLoader->resolve("udev_list_entry_get_next"); + m_uDev.list_entry_get_name = (uDEV_list_entry_get_name*)m_libraryLoader->resolve("udev_list_entry_get_name"); + m_uDev.device_new_from_syspath = (uDEV_device_new_from_syspath*)m_libraryLoader->resolve("udev_device_new_from_syspath"); + m_uDev.device_get_parent = (uDEV_device_get_parent*)m_libraryLoader->resolve("udev_device_get_parent"); + m_uDev.device_get_devnode = (uDEV_device_get_devnode*)m_libraryLoader->resolve("udev_device_get_devnode"); + m_uDev.device_get_parent_with_subsystem_devtype = (uDEV_device_get_parent_with_subsystem_devtype*)m_libraryLoader->resolve("udev_device_get_parent_with_subsystem_devtype"); + m_uDev.device_get_sysattr_value = (uDEV_device_get_sysattr_value*)m_libraryLoader->resolve("udev_device_get_sysattr_value"); + } +} + +/*! \brief Create udev library context. + + This reads the udev configuration file, and fills in the default values. + + The initial refcount is 1, and needs to be decremented to release the resources of the udev library context. + + \returns a new udev library context +*/ +udev* Udev::unew(void) +{ + if (m_uDev.unew) + return m_uDev.unew(); + else + return NULL; +} + +/*! \brief Drop a reference of the udev library context. + + \param udev udev library context + + If the refcount reaches zero, the resources of the context will be released. +*/ +udev* Udev::unref(struct udev* udev) +{ + if (m_uDev.unref) + return m_uDev.unref(udev); + else + return NULL; +} + +/*! \brief Drop a reference of a udev device. + + If the refcount reaches zero, the resources of the device will be released. + + \param udev_device udev device + \return NULL +*/ +udev_device* Udev::device_unref(struct udev_device* udev_device) +{ + if (m_uDev.device_unref) + return m_uDev.device_unref(udev_device); + else + return NULL; +} + +/*! \brief Create an enumeration context to scan. + \param udev udev library context + + \return an enumeration context. +*/ +udev_enumerate* Udev::enumerate_new(struct udev* udev) +{ + if (m_uDev.enumerate_new) + return m_uDev.enumerate_new(udev); + else + return NULL; +} + +/*! \brief Match only devices belonging to a certain kernel subsystem. + \param udev_enumerate context + \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) +{ + if (m_uDev.enumerate_add_match_subsystem) + return m_uDev.enumerate_add_match_subsystem(udev_enumerate, subsystem); + else + return -1; +} + +/*! \brief Scan /sys for all devices which match the given filters. No matches will return all currently available devices. + \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) +{ + if (m_uDev.enumerate_scan_devices) + return m_uDev.enumerate_scan_devices(udev_enumerate); + else + return -1; +} + +/*! \brief Get the next entry from the list. + + \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) +{ + if (m_uDev.list_entry_get_next) + return m_uDev.list_entry_get_next(list_entry); + else + return NULL; +} + +/*! \brief Get the first entry of the sorted list of device paths. + \param udev_enumerate context + \return a udev_list_entry. +*/ +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); + else + return NULL; +} + +/*! \brief Drop a reference of an enumeration context. + + If the refcount reaches zero, all resources of the enumeration context will be released. + + \param udev_enumerate context + + \return: NULL +*/ +udev_enumerate* Udev::enumerate_unref(struct udev_enumerate* udev_enumerate) +{ + if (m_uDev.enumerate_unref) + return m_uDev.enumerate_unref(udev_enumerate); + else + return NULL; +} + +/*! \brief Get the name of a list entry. + \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) +{ + if (m_uDev.list_entry_get_name) + return m_uDev.list_entry_get_name(list_entry); + else + return ""; +} + +/*! \brief Create new udev device, and fill in information from the sys device and the udev database entry. + + The syspath is the absolute path to the device, including the sys mount point. + + \param udev udev library context + \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) +{ + if (m_uDev.device_new_from_syspath) + return m_uDev.device_new_from_syspath(udev, syspath); + else + return NULL; +} + +/*! \brief Find the next parent device, and fill in information from the sys device and the udev database entry. + + Returned device is not referenced. It is attached to the child device, and will be cleaned up when the child device is cleaned up. + It is not necessarily just the upper level directory, empty or not recognized sys directories are ignored. + + It can be called as many times as needed, without caring about references. + + \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) +{ + if (m_uDev.device_get_parent) + return m_uDev.device_get_parent(udev_device); + else + return NULL; +} + +/*! \brief Retrieve the device node file name belonging to the udev device. + + The path is an absolute path, and starts with the device directory. + + \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) +{ + if (m_uDev.device_get_devnode) + return m_uDev.device_get_devnode(udev_device); + else + return ""; +} + +/*! \brief Find the next parent device, with a matching subsystem and devtypevalue, and fill in information from the sys device and the udev database entry. + + If devtype is NULL, only subsystem is checked, and any devtype will match. + + Returned device is not referenced. It is attached to the child device, and will be cleaned up when the child device is cleaned up. + + It can be called as many times as needed, without caring about references. + + \param udev_device udev device to start searching from + \param subsystem the subsystem of the 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) +{ + if (m_uDev.device_get_parent_with_subsystem_devtype) + return m_uDev.device_get_parent_with_subsystem_devtype(udev_device, subsystem, devtype); + else + return NULL; +} + +/*! \brief Get a sys attribute value + + The retrieved value is cached in the device. Repeated calls will return the same value and not open the attribute again. + + \param udev_device udev device + \param sysattr attribute name + + \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) +{ + if (m_uDev.device_get_sysattr_value) + return m_uDev.device_get_sysattr_value(udev_device, sysattr); + else + return ""; +} diff --git a/extern/xspublic/xscontroller/udev.h b/extern/xspublic/xscontroller/udev.h new file mode 100644 index 0000000..2b269bc --- /dev/null +++ b/extern/xspublic/xscontroller/udev.h @@ -0,0 +1,111 @@ + +// Copyright (c) 2003-2022 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 UDEV +#define UDEV + +struct udev; +struct udev_device; +struct udev_enumerate; +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); + +#ifdef __cplusplus +class Udev +{ +public: + Udev(void); + ~Udev(void); + + 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; + +private: + + 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; + } m_uDev; + + XsLibraryLoader* m_libraryLoader; + + void initLibrary(); + Udev(Udev const&) = delete; + Udev const& operator = (Udev const&) = delete; +}; +#endif + +#endif diff --git a/extern/xspublic/xscontroller/usbcommunicator.cpp b/extern/xspublic/xscontroller/usbcommunicator.cpp new file mode 100644 index 0000000..ee007ac --- /dev/null +++ b/extern/xspublic/xscontroller/usbcommunicator.cpp @@ -0,0 +1,105 @@ + +// Copyright (c) 2003-2022 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 "usbcommunicator.h" +#include "usbinterface.h" + +/*! \class UsbCommunicator + \brief A class that uses USB to communicate +*/ + +/*! \brief Construct and returns new USB communicator + \returns The new USB communicator +*/ +Communicator* UsbCommunicator::construct() +{ + return new UsbCommunicator; +} + +/*! \brief Default constructor +*/ +UsbCommunicator::UsbCommunicator() + : m_usbInterface(nullptr) +{ +} + +UsbCommunicator::~UsbCommunicator() +{ +} + +/*! \copybrief Communicator::gotoConfig +*/ +XsResultValue UsbCommunicator::gotoConfig(bool detectRs485) +{ + XsResultValue r = SerialPortCommunicator::gotoConfig(detectRs485); + + if (r == XRV_OK) + { + m_usbInterface->setRawIo(false); + m_usbInterface->setTimeout(0); + } + return r; +} + +/*! \copybrief Communicator::gotoMeasurement +*/ +XsResultValue UsbCommunicator::gotoMeasurement() +{ + XsResultValue r = SerialPortCommunicator::gotoMeasurement(); + if (r == XRV_OK) + { + m_usbInterface->setRawIo(true); + m_usbInterface->setTimeout(2000); + } + return r; +} + +/*! \brief Creates a stream interface + \param pi The port to use + \returns The shared pointer to a stream interface +*/ +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; + } + ); + + setLastResult(m_usbInterface->open(pi)); + + return stream; +} diff --git a/extern/xspublic/xscontroller/usbcommunicator.h b/extern/xspublic/xscontroller/usbcommunicator.h new file mode 100644 index 0000000..49b7f2b --- /dev/null +++ b/extern/xspublic/xscontroller/usbcommunicator.h @@ -0,0 +1,57 @@ + +// Copyright (c) 2003-2022 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 USBCOMMUNICATOR_H +#define USBCOMMUNICATOR_H + +#include "serialportcommunicator.h" + +class UsbInterface; + +class UsbCommunicator : public SerialPortCommunicator +{ +public: + static Communicator* construct(); + UsbCommunicator(); + + virtual XsResultValue gotoConfig(bool detectRs485) override; + virtual XsResultValue gotoMeasurement() override; + +protected: + virtual ~UsbCommunicator(); + std::shared_ptr createStreamInterface(const XsPortInfo& pi) override; + +private: + UsbInterface* m_usbInterface; +}; + +#endif diff --git a/extern/xspublic/xscontroller/usbinterface.cpp b/extern/xspublic/xscontroller/usbinterface.cpp new file mode 100644 index 0000000..796b7a1 --- /dev/null +++ b/extern/xspublic/xscontroller/usbinterface.cpp @@ -0,0 +1,1083 @@ + +// Copyright (c) 2003-2022 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 +#include +#include "usbinterface.h" +#include + +#include "xswinusb.h" +#include "xslibusb.h" +#include "rx_tx_log.h" + +#ifndef _WIN32 + #include // strcpy +#else + #include + #include +#endif +#include + +#ifndef _CRT_SECURE_NO_DEPRECATE + #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 + + //! The time at which an operation will end in ms, used by several functions. + uint32_t m_endTime; + //! 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. + */ + uint32_t m_timeout; + + int m_dataInEndPoint; + int m_dataOutEndPoint; + char m_portname[256]; + +#ifdef USE_WINUSB + XsWinUsb m_winUsb; + WINUSB_INTERFACE_HANDLE m_usbHandle[2]; + uint8_t m_bulkInPipe, m_bulkOutPipe, m_interruptPipe, m_deviceSpeed; + uint16_t m_bulkInPipePacketSize; + XsIoHandle m_deviceHandle; + + XsThreadId m_threadId; + XsThread m_threadHandle; + static const int m_oCount = MAXIMUM_WAIT_OBJECTS - 1; + OVERLAPPED m_overlapped[m_oCount]; + XsByteArray m_varBuffer; + static const int m_fixedBufferSize = 8192; + uint8_t m_fixedBuffer[m_oCount][m_fixedBufferSize]; + //int m_offset; + CRITICAL_SECTION m_mutex; + HANDLE m_quitEvent; + volatile std::atomic_bool m_running; + HANDLE m_waitEvents[m_oCount]; + int m_readIdx; + XsResultValue m_threadedResult; + + void threadFunc(); + +#elif defined(HAVE_LIBUSB) + int m_interfaceCount; + int m_interface; + + 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. + */ + class UsbContext + { + public: + /*! \brief Create the USB context + */ + UsbContext() + { + if (!m_claimedContexts) + m_libUsb.init(&m_usbContext); + m_claimedContexts++; + //libusb_set_debug(m_usbContext, 3); + } + + /*! \brief Destroy the USB context */ + ~UsbContext() + { + m_claimedContexts--; + if (!m_claimedContexts) + m_libUsb.exit(m_usbContext); + } + static libusb_context* m_usbContext; // needed for proper use of libusb + XsLibUsb m_libUsb; + private: + static int m_claimedContexts; + }; + UsbContext m_contextManager; + + /*! \brief Map a libusb_error to XsResultValue + + \a param libusbError [in] the result code to convert + \a param hint give a hint for the code to return when in doubt + */ + XsResultValue libusbErrorToXrv(int libusbError, XsResultValue hint = XRV_ERROR) + { + switch (libusbError) + { + case LIBUSB_SUCCESS: + return XRV_OK; + + case LIBUSB_ERROR_IO: + return hint; + + case LIBUSB_ERROR_INVALID_PARAM: + return XRV_INVALIDPARAM; + + case LIBUSB_ERROR_ACCESS: + return hint; + + case LIBUSB_ERROR_NO_DEVICE: + return XRV_UNEXPECTED_DISCONNECT; + + case LIBUSB_ERROR_NOT_FOUND: + return XRV_NOTFOUND; + + case LIBUSB_ERROR_BUSY: + return XRV_INVALIDOPERATION; + + case LIBUSB_ERROR_TIMEOUT: + return XRV_TIMEOUT; + + case LIBUSB_ERROR_OVERFLOW: + return hint; + + case LIBUSB_ERROR_PIPE: + return hint; + + case LIBUSB_ERROR_INTERRUPTED: + return XRV_UNEXPECTEDMSG; + + case LIBUSB_ERROR_NO_MEM: + return XRV_OUTOFMEMORY; + + case LIBUSB_ERROR_NOT_SUPPORTED: + return XRV_NOTIMPLEMENTED; + + case LIBUSB_ERROR_OTHER: + return hint; + } + return hint; + } + + /*! \brief Convert a libusb error to a human-readable string */ + const char* libusbErrorToString(int libusbError) + { + switch (libusbError) + { + case LIBUSB_SUCCESS: + return "LIBUSB_SUCCESS"; + + case LIBUSB_ERROR_IO: + return "LIBUSB_ERROR_IO"; + + case LIBUSB_ERROR_INVALID_PARAM: + return "LIBUSB_ERROR_INVALID_PARAM"; + + case LIBUSB_ERROR_ACCESS: + return "LIBUSB_ERROR_ACCESS"; + + case LIBUSB_ERROR_NO_DEVICE: + return "LIBUSB_ERROR_NO_DEVICE"; + + case LIBUSB_ERROR_NOT_FOUND: + return "LIBUSB_ERROR_NOT_FOUND"; + + case LIBUSB_ERROR_BUSY: + return "LIBUSB_ERROR_BUSY"; + + case LIBUSB_ERROR_TIMEOUT: + return "LIBUSB_ERROR_TIMEOUT"; + + case LIBUSB_ERROR_OVERFLOW: + return "LIBUSB_ERROR_OVERFLOW"; + + case LIBUSB_ERROR_PIPE: + return "LIBUSB_ERROR_PIPE"; + + case LIBUSB_ERROR_INTERRUPTED: + return "LIBUSB_ERROR_INTERRUPTED"; + + case LIBUSB_ERROR_NO_MEM: + return "LIBUSB_ERROR_NO_MEM"; + + case LIBUSB_ERROR_NOT_SUPPORTED: + return "LIBUSB_ERROR_NOT_SUPPORTED"; + + case LIBUSB_ERROR_OTHER: + return "LIBUSB_ERROR_OTHER"; + } + return "Unknown"; + } +#else + void* m_deviceHandle; +#endif +}; + +#ifdef USE_WINUSB +static DWORD usbReadThreadFunc(void* obj) +{ + UsbInterfacePrivate* d = (UsbInterfacePrivate*) obj; + d->m_running = true; + xsSetThreadPriority(::GetCurrentThread(), XS_THREAD_PRIORITY_HIGHEST); // not using xsGetCurrentThreadHandle here because we can directly use GetCurrentThread(), which is simpler + xsNameThisThread("USB reader"); + try + { + d->threadFunc(); + } + catch (...) + { +#ifdef XSENS_DEBUG + assert(0); +#endif + } + d->m_running = false; + return 0; +} + +void UsbInterfacePrivate::threadFunc() +{ + HANDLE handles[1 + m_oCount]; + handles[0] = m_quitEvent; + 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) + { + 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]; + m_winUsb.ReadPipe(m_usbHandle[1], m_bulkInPipe, m_fixedBuffer[m_readIdx], (ULONG)m_fixedBufferSize, 0, &m_overlapped[m_readIdx]); + } + bool run = true; + while (run) + { + // start follow-up read operation + m_overlapped[m_readIdx] = OVERLAPPED(); + ::ResetEvent(m_waitEvents[m_readIdx]); + 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); + + // 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)); + break; + } + +#ifndef XSENS_RELEASE + 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) + { + 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); + } + } + break; + } + } +} + +#elif defined(HAVE_LIBUSB) +int UsbInterfacePrivate::UsbContext::m_claimedContexts = 0; +libusb_context* UsbInterfacePrivate::UsbContext::m_usbContext = NULL; +#endif + +/*! \class UsbInterface + \brief An IoInterface for dealing specifically with Xsens USB devices +*/ + +/*! \brief Default constructor, initializes all members to their default values. +*/ +UsbInterface::UsbInterface() + : d(new UsbInterfacePrivate) +{ + d->m_lastResult = XRV_OK; + d->m_timeout = 20; + d->m_endTime = 0; + d->m_dataInEndPoint = -1; + d->m_dataOutEndPoint = -1; + d->m_deviceHandle = 0; + d->m_portname[0] = 0; + +#ifdef USE_WINUSB + d->m_threadHandle = INVALID_HANDLE_VALUE; + ::InitializeCriticalSection(&d->m_mutex); + d->m_usbHandle[0] = 0; + d->m_usbHandle[1] = 0; + for (int i = 0; i < UsbInterfacePrivate::m_oCount; ++i) + { + d->m_waitEvents[i] = ::CreateEvent(NULL, TRUE, FALSE, NULL); + ::ResetEvent(d->m_waitEvents[i]); + } + d->m_quitEvent = ::CreateEvent(NULL, TRUE, FALSE, NULL); + ::ResetEvent(d->m_quitEvent); + d->m_readIdx = 0; + d->m_threadedResult = XRV_OK; +#endif +} + +//! Destructor, de-initializes, frees memory allocated for buffers, etc. +UsbInterface::~UsbInterface() +{ + try + { + closeUsb(); +#ifdef USE_WINUSB + ::CloseHandle(d->m_quitEvent); + for (int i = 0; i < UsbInterfacePrivate::m_oCount; ++i) + ::CloseHandle(d->m_waitEvents[i]); + ::DeleteCriticalSection(&d->m_mutex); +#endif + delete d; + } + catch (...) + { + } +} + +/*! \brief Close the USB communication port. +*/ +XsResultValue UsbInterface::close(void) +{ + return closeUsb(); +} + +/*! \brief Close the USB communication port. + \returns XRV_OK if the port was closed successfully + \note Linux:\n + If a kernel driver was detached when communication with the device started, + attach it again. No guarantee is given that udev will pick up on it though. +*/ +XsResultValue UsbInterface::closeUsb(void) +{ +#ifdef LOG_RX_TX + d->rx_log.close(); + d->tx_log.close(); + +#endif + if (!isOpen()) + return d->m_lastResult = XRV_NOPORTOPEN; + + d->m_lastResult = XRV_OK; +#ifdef USE_WINUSB + if (d->m_threadHandle != INVALID_HANDLE_VALUE) + { + while (d->m_running) + { + ::SetEvent(d->m_quitEvent); + XsTime::msleep(10); + } + ::CloseHandle(d->m_threadHandle); + } + + flushData(); + if (d->m_usbHandle[0]) + { + d->m_winUsb.Free(d->m_usbHandle[0]); + d->m_usbHandle[0] = NULL; + } + if (d->m_usbHandle[1]) + { + d->m_winUsb.Free(d->m_usbHandle[1]); + d->m_usbHandle[1] = NULL; + } + if (d->m_deviceHandle) + { + CloseHandle(d->m_deviceHandle); + d->m_deviceHandle = NULL; + } +#elif defined(HAVE_LIBUSB) + flushData(); + 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; + while (result != LIBUSB_SUCCESS) + { + result = d->m_contextManager.m_libUsb.release_interface(d->m_deviceHandle, i); + if (result == LIBUSB_SUCCESS) + d->m_contextManager.m_libUsb.attach_kernel_driver(d->m_deviceHandle, i); + } + } + + d->m_contextManager.m_libUsb.close(d->m_deviceHandle); + d->m_deviceHandle = NULL; + + d->m_contextManager.m_libUsb.unref_device(dev); + d->m_interface = -1; + d->m_dataInEndPoint = -1; + d->m_dataOutEndPoint = -1; +#else + d->m_lastResult = XRV_NOTIMPLEMENTED; +#endif + d->m_endTime = 0; + + return d->m_lastResult; +} + +////////////////////////////////////////////////////////////////////////////////////////// +// Flush all data to be transmitted / received. +XsResultValue UsbInterface::flushData(void) +{ + d->m_lastResult = XRV_OK; +#ifdef USE_WINUSB + if (d->m_usbHandle[0]) + { + d->m_winUsb.AbortPipe(d->m_usbHandle[0], d->m_bulkInPipe); + d->m_winUsb.FlushPipe(d->m_usbHandle[0], d->m_bulkInPipe); + d->m_winUsb.AbortPipe(d->m_usbHandle[0], d->m_bulkOutPipe); + d->m_winUsb.FlushPipe(d->m_usbHandle[0], d->m_bulkOutPipe); + } + if (d->m_usbHandle[1]) + { + d->m_winUsb.AbortPipe(d->m_usbHandle[1], d->m_bulkInPipe); + d->m_winUsb.FlushPipe(d->m_usbHandle[1], d->m_bulkInPipe); + d->m_winUsb.AbortPipe(d->m_usbHandle[1], d->m_bulkOutPipe); + d->m_winUsb.FlushPipe(d->m_usbHandle[1], d->m_bulkOutPipe); + } +#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) + break; + if (actual == 0) + break; + } +#else + d->m_lastResult = XRV_NOTIMPLEMENTED; +#endif + d->m_endTime = 0; + return d->m_lastResult; +} + +//! Return the error code of the last operation. +XsResultValue UsbInterface::getLastResult(void) const +{ + return d->m_lastResult; +} + +//! Return the current timeout value +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 +{ + 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) +{ + d->m_endTime = 0; + +#ifdef USE_WINUSB + JLDEBUGG("Open usb port " << portInfo.portName()); +#else + JLDEBUGG("Open usb port " << portInfo.usbBus() << ":" << portInfo.usbAddress()); +#endif + + if (isOpen()) + { + JLALERTG("Port " << portInfo.portName() << " already open"); + return (d->m_lastResult = XRV_ALREADYOPEN); + } + +#ifdef USE_WINUSB + d->m_deviceHandle = CreateFileA(portInfo.portName_c_str(), GENERIC_WRITE | GENERIC_READ, FILE_SHARE_WRITE | FILE_SHARE_READ, NULL, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL | FILE_FLAG_OVERLAPPED, NULL); + + if (d->m_deviceHandle == INVALID_HANDLE_VALUE) + { + d->m_deviceHandle = NULL; + return (d->m_lastResult = XRV_PORTNOTFOUND); + } + + BOOL result; + UCHAR speed = 0; + ULONG length = 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]); + else + { +#ifdef XSENS_DEBUG + DWORD err = GetLastError(); + assert(result); +#endif + return (d->m_lastResult = XRV_ERROR); + } + + for (int k = 0; k < 2; k++) + { + if (result) + { + assert(d->m_usbHandle[k] != 0); + length = sizeof(UCHAR); + result = d->m_winUsb.QueryDeviceInformation(d->m_usbHandle[k], DEVICE_SPEED, &length, &speed); + } + + if (result) + { + d->m_deviceSpeed = speed; + result = d->m_winUsb.QueryInterfaceSettings(d->m_usbHandle[k], 0, &interfaceDescriptor); + } + if (result) + { + for (int i = 0; i < interfaceDescriptor.bNumEndpoints; i++) + { + result = d->m_winUsb.QueryPipe(d->m_usbHandle[k], 0, (UCHAR) i, &pipeInfo); + + if (pipeInfo.PipeType == UsbdPipeTypeBulk && USB_ENDPOINT_DIRECTION_IN(pipeInfo.PipeId)) + { + d->m_bulkInPipe = pipeInfo.PipeId; + d->m_bulkInPipePacketSize = + 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; + break; + } + } + } + } + + setTimeout(0); + flushData(); + + sprintf(d->m_portname, "%s", portInfo.portName_c_str()); + + // d->m_offset = 0; + ::ResetEvent(&d->m_quitEvent); + d->m_threadHandle = xsStartThread(usbReadThreadFunc, d, &d->m_threadId); + if (d->m_threadHandle == XSENS_INVALID_THREAD) + { +#ifdef XSENS_DEBUG + assert(0); +#endif + return (d->m_lastResult = XRV_ERROR); + } + +#elif defined(HAVE_LIBUSB) + 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); + + // "USBxxx:yyy" + uint8_t bus = XsPortInfo_usbBus(&portInfo); + uint8_t address = XsPortInfo_usbAddress(&portInfo); + + XsResultValue xrv = XRV_OK; + int result; + libusb_device* device = NULL; + for (int i = 0; i < listLength && device == NULL; ++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; + + libusb_device_descriptor desc; + result = d->m_contextManager.m_libUsb.get_device_descriptor(dev, &desc); + if (result != LIBUSB_SUCCESS) + break; + + libusb_config_descriptor* configDesc; + result = d->m_contextManager.m_libUsb.get_active_config_descriptor(dev, &configDesc); + if (result != LIBUSB_SUCCESS) + break; + + d->m_interface = -1; + d->m_interfaceCount = configDesc->bNumInterfaces; + // find the bulk transfer endpoints + for (uint8_t ifCount = 0; ifCount < configDesc->bNumInterfaces && d->m_interface == -1; ++ifCount) + { + for (uint8_t altsettingCount = 0; altsettingCount < configDesc->interface[ifCount].num_altsetting; altsettingCount++) + { + 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) + continue; + + switch (endpoints[i].bEndpointAddress & LIBUSB_ENDPOINT_DIR_MASK) + { + 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; + } + + } + + if (outEndpoint == -1 || inEndpoint == -1) + continue; + + d->m_interface = ifCount; + d->m_dataOutEndPoint = outEndpoint; + d->m_dataInEndPoint = inEndpoint; + } + } + if (d->m_interface == -1) + { + xrv = XRV_INPUTCANNOTBEOPENED; + break; + } + + d->m_contextManager.m_libUsb.free_config_descriptor(configDesc); + d->m_contextManager.m_libUsb.ref_device(dev); + device = dev; + result = LIBUSB_SUCCESS; + } + + d->m_contextManager.m_libUsb.free_device_list(deviceList, 1); + if (result != LIBUSB_SUCCESS) + { + d->m_contextManager.m_libUsb.unref_device(device); + return d->m_lastResult = d->libusbErrorToXrv(result); + } + + if (xrv != XRV_OK) + { + d->m_contextManager.m_libUsb.unref_device(device); + return d->m_lastResult = xrv; + } + + libusb_device_handle* handle; + result = d->m_contextManager.m_libUsb.open(device, &handle); + if (result != LIBUSB_SUCCESS) + { + d->m_contextManager.m_libUsb.unref_device(device); + return d->m_lastResult = d->libusbErrorToXrv(result); + } + + // be rude and claim all interfaces + for (int i = 0; i < d->m_interfaceCount; i++) + { + result = d->m_contextManager.m_libUsb.kernel_driver_active(handle, i); + if (result > 0) + result = d->m_contextManager.m_libUsb.detach_kernel_driver(handle, i); + if (result == LIBUSB_SUCCESS) + result = d->m_contextManager.m_libUsb.claim_interface(handle, i); + if (result != LIBUSB_SUCCESS) + { + for (int j = 0; j < i; j++) + { + while (result != LIBUSB_SUCCESS) + { + result = d->m_contextManager.m_libUsb.release_interface(handle, j); + d->m_contextManager.m_libUsb.attach_kernel_driver(handle, j); + } + } + + d->m_contextManager.m_libUsb.close(handle); + d->m_contextManager.m_libUsb.unref_device(device); + return d->m_lastResult = d->libusbErrorToXrv(result); + } + } + + d->m_deviceHandle = handle; + sprintf(d->m_portname, "%s", portInfo.portName_c_str()); + + flushData(); +#else // HAVE_LIBUSB + (void)portInfo; + d->m_lastResult = XRV_NOTIMPLEMENTED; +#endif + JLDEBUGG("USB Port opened"); + return (d->m_lastResult = XRV_OK); +} + +/*! \brief Read data from the USB port and put it into the data buffer. + \details This function reads up to \a maxLength bytes from the port (non-blocking) and + puts it into the \a data buffer. + \param maxLength The maximum amount of data read. + \param data The buffer that will store the received data. + \returns XRV_OK if no error occurred. It can be that no data is available and XRV_OK will be + returned. Check data.size() for the number of bytes that were read. +*/ +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)); + return res; +} + +/*! \brief Read data from the serial port and put it into the data buffer. + \details This function reads up to \a maxLength bytes from the USB port (non-blocking) and + puts it into the \a data buffer. + \param maxLength The maximum number of bytes to read. + \param data Pointer to a buffer that will store the received data. + \param length The number of bytes placed into \c 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) +{ + JLTRACEG("maxLength=" << maxLength << ", data=" << JLHEXLOG(data) << ", length=" << JLHEXLOG(length)); + XsFilePos ln; + if (length == NULL) + length = &ln; + + if (!isOpen()) + return (d->m_lastResult = XRV_NOPORTOPEN); + +#ifdef USE_WINUSB + ::EnterCriticalSection(&d->m_mutex); + XsFilePos remaining = *length = (XsFilePos) d->m_varBuffer.size(); + if (*length > maxLength) + *length = maxLength; + if (*length) + { + memcpy(data, (void*) d->m_varBuffer.data(), (XsSize) *length); + d->m_varBuffer.erase(0, (XsSize) *length); + remaining = (XsFilePos) d->m_varBuffer.size(); + } + + if (d->m_threadedResult != XRV_OK) + return (d->m_lastResult = d->m_threadedResult); + + ::LeaveCriticalSection(&d->m_mutex); + JLTRACEG("returned success, read " << *length << " of " << maxLength << " bytes, first: " << JLHEXLOG(((char*)data)[0]) << ", " << remaining << " remaining in buffer"); + (void) remaining; + +#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); + 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); + + *length = actual; +#else + (void)maxLength; + (void)data; +#endif + +#ifdef LOG_RX_TX + if (*length > 0) + { + CHECK_STATE_RX(*length, data, d->rx_log); + + if (!d->rx_log.isOpen()) + { + char fname[XS_MAX_FILENAME_LENGTH]; + sprintf(fname, "rx_USB%03u_%03u.log", usbBus(), usbAddress()); + d->rx_log.create(XsString(fname), true); + } + d->rx_log.write(data, 1, *length); +#ifdef LOG_RX_TX_FLUSH + d->rx_log.flush(); +#endif + } +#endif + + return (d->m_lastResult = XRV_OK); +} + +/*! \brief Set the default timeout value to use in blocking operations. + \details This function sets the value of m_timeout. There is no infinity value. The value 0 + means that the default value is used. + \param ms The desired timeout in milliseconds + \returns XRV_OK if the timeout value was successfully updated +*/ +XsResultValue UsbInterface::setTimeout(uint32_t ms) +{ +#ifdef USE_WINUSB + JLDEBUGG("Request to set timeout to " << ms << " ms overridden, setting to 0 ms instead"); + ms = 0; // no timeout ever + UCHAR enable = FALSE; + + d->m_winUsb.SetPipePolicy(d->m_usbHandle[1], d->m_bulkInPipe, IGNORE_SHORT_PACKETS, sizeof(UCHAR), &enable); + d->m_winUsb.SetPipePolicy(d->m_usbHandle[1], d->m_bulkInPipe, PIPE_TRANSFER_TIMEOUT, sizeof(ULONG), &ms); + + d->m_timeout = ms; +#else + JLDEBUGG("Setting timeout to " << ms); + if (ms == 0) + d->m_timeout = 1; + else + d->m_timeout = ms; +#endif + return (d->m_lastResult = XRV_OK); +} + +/*! \brief Sets the RAWIO mode of the USB interface + \note Only applies to WinUSB implementations + \param enable : If true will enable RAW IO mode +*/ +void UsbInterface::setRawIo(bool enable) +{ + JLDEBUGG("Setting RAWIO mode to " << enable); + +#ifdef USE_WINUSB + enable = false; // never use raw IO + UCHAR rawIo = (UCHAR)enable; + d->m_winUsb.SetPipePolicy(d->m_usbHandle[1], d->m_bulkInPipe, RAW_IO, sizeof(UCHAR), &rawIo); +#else + (void)enable; +#endif + d->m_lastResult = XRV_OK; +} + +/*! \brief Retrieves the state of the RAWIO mode of the USB interface + \returns true if raw IO mode is enabled + \note Only applies to WinUSB implementations +*/ +bool UsbInterface::getRawIo(void) +{ +#ifdef USE_WINUSB + UCHAR rawIo = 0; + ULONG someSize = sizeof(UCHAR); + d->m_winUsb.GetPipePolicy(d->m_usbHandle[1], d->m_bulkInPipe, RAW_IO, &someSize, &rawIo); + return (rawIo != 0); +#else + return false; +#endif +} + +/*! \brief Wait for data to arrive or a timeout to occur. + \details The function waits until \c maxLength data is available or until a timeout occurs. + The function returns success if data is available or XsResultValue::TIMEOUT if a + timeout occurred. A timeout value of 0 indicates that the default timeout stored + in the class should be used. + \param maxLength The maximum number of bytes to wait for + \param data A buffer that will be filled with the read data. It must be able to contain at + least \a maxLength bytes. + \param length An optional pointer to storage for the actual number of bytes read. + \returns XRV_OK if the requested data was read +*/ +XsResultValue UsbInterface::waitForData(XsFilePos maxLength, void* data, XsFilePos* length) +{ + JLTRACEG("timeout=" << d->m_timeout << ", data=" << data << ", length=" << length); + uint32_t timeout = d->m_timeout; + + char* bdata = (char*)data; + + XsFilePos ln; + if (length == NULL) + length = &ln; + uint32_t eTime = XsTime::getTimeOfDay() + timeout; + XsFilePos newLength = 0; + + *length = 0; + while ((*length < maxLength) && (XsTime::getTimeOfDay() <= eTime)) + { + if (readData(maxLength - *length, bdata + *length, &newLength)) + return d->m_lastResult; + *length += newLength; + } + JLTRACEG("read " << length[0] << " of " << maxLength << " bytes"); + + if (length[0] < maxLength) + return (d->m_lastResult = XRV_TIMEOUT); + else + return (d->m_lastResult = XRV_OK); +} + +/*! \brief Write the data to the USB port. + \details The function writes the given data to the connected USB port. + The default timeout is respected in this operation. + \param data The data to be written + \param written An optional pointer to storage for the actual number of bytes that were written + \returns XRV_OK if the data was successfully written + \sa writeData(XsFilePos, const void *, XsFilePos*) +*/ +XsResultValue UsbInterface::writeData(const XsByteArray& data, XsFilePos* written) +{ + return writeData((XsFilePos) data.size(), data.data(), written); +} + +/*! \brief Write the data to the USB port. + \details The function writes the given data to the connected USB port. + The default timeout is respected in this operation. + \param length The number of bytes to write. + \param data A pointer to a memory buffer that contains the bytes to send + \param written An optional pointer to storage for the actual number of bytes that were written + \returns XRV_OK if the data was successfully written + \sa writeData(const XsByteArray&, XsFilePos*) +*/ +XsResultValue UsbInterface::writeData(XsFilePos length, const void* data, XsFilePos* written) +{ + XsFilePos bytes; + if (written == NULL) + written = &bytes; + + if (!isOpen()) + return (d->m_lastResult = XRV_NOPORTOPEN); + + *written = 0; + +#ifdef USE_WINUSB + ULONG dataWritten; + d->m_winUsb.WritePipe(d->m_usbHandle[1], d->m_bulkOutPipe, static_cast(const_cast(data)), (ULONG)length, &dataWritten, NULL); + + *written = dataWritten; +#elif defined(HAVE_LIBUSB) + *written = 0; + 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); + *written += actual; + if (result != LIBUSB_SUCCESS) + { + JLALERTG("bulk write failed: " << d->libusbErrorToString(result) << ". " << actual << " bytes sent"); + return (d->m_lastResult = d->libusbErrorToXrv(result)); + } + } +#else + (void)length; + (void)data; +#endif + +#ifdef LOG_RX_TX + if (*written > 0) + { + CHECK_STATE_TX(*written, data, d->tx_log); + + if (!d->tx_log.isOpen()) + { + char fname[XS_MAX_FILENAME_LENGTH]; + sprintf(fname, "tx_USB%03u_%03u.log", usbBus(), usbAddress()); + d->tx_log.create(XsString(fname), true); + } + d->tx_log.write(data, 1, *written); +#ifdef LOG_RX_TX_FLUSH + d->tx_log.flush(); +#endif + } +#endif + + return (d->m_lastResult = XRV_OK); +} + +/*! \brief The USB bus number this device is on (libusb/linux only) */ +uint8_t UsbInterface::usbBus() const +{ +#if defined(HAVE_LIBUSB) + if (!d->m_deviceHandle) + return 0; + + 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; +#endif +} + +/*! \brief The address of the device (libusb/linux only) */ +uint8_t UsbInterface::usbAddress() const +{ +#if defined(HAVE_LIBUSB) + if (!d->m_deviceHandle) + return 0; + + 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; +#endif +} + +//! Retrieve the port name that was last successfully opened. +void UsbInterface::getPortName(XsString& portname) const +{ + portname = d->m_portname; +} diff --git a/extern/xspublic/xscontroller/usbinterface.h b/extern/xspublic/xscontroller/usbinterface.h new file mode 100644 index 0000000..861f9f7 --- /dev/null +++ b/extern/xspublic/xscontroller/usbinterface.h @@ -0,0 +1,95 @@ + +// Copyright (c) 2003-2022 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 USBINTERFACE_H +#define USBINTERFACE_H + +#include + +#include +#include +#ifdef _WIN32 + #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 +#endif + +#include "streaminterface.h" +#include + +struct XsPortInfo; + +class UsbInterfacePrivate; + +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 close(void); + XsResultValue closeUsb(void); + XsResultValue flushData(void); + + 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; + + void setRawIo(bool enable); + bool getRawIo(void); + + XsResultValue writeData(const XsByteArray& data, XsFilePos* written = NULL) override; + 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); + + void getPortName(XsString& portname) const; + + XSENS_DISABLE_COPY(UsbInterface); +}; + +#endif diff --git a/extern/xspublic/xscontroller/xdacommunicatorfactory.cpp b/extern/xspublic/xscontroller/xdacommunicatorfactory.cpp new file mode 100644 index 0000000..f60a723 --- /dev/null +++ b/extern/xspublic/xscontroller/xdacommunicatorfactory.cpp @@ -0,0 +1,101 @@ + +// Copyright (c) 2003-2022 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 "xdacommunicatorfactory.h" +#include "mtbfilecommunicator.h" +#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; +} + +/*! \class XdaCommunicatorFactory + \brief XDA communication factory +*/ + +/*! \brief Construct this factory */ +XdaCommunicatorFactory::XdaCommunicatorFactory() +{ +} + +/*! \copydoc CommunicatorFactory::filenameToCommunicatorId */ +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. + */ + return CommunicatorType::MTBFILE; +} + +/*! \copydoc CommunicatorFactory::portInfoToCommunicatorId */ +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)) + return it->first; + + 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(); +} + +/*! \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 */ +void XdaCommunicatorFactory::registerCommunicatorTypes() +{ + (void)registerType(CommunicatorType::MTBFILE, &MtbFileCommunicator::construct, nullptr); + (void)registerType(CommunicatorType::SERIALPORT, &SerialPortCommunicator::construct, &isSerialPort); + (void)registerType(CommunicatorType::USB, &UsbCommunicator::construct, &isUsb); +} diff --git a/extern/xspublic/xscontroller/xdacommunicatorfactory.h b/extern/xspublic/xscontroller/xdacommunicatorfactory.h new file mode 100644 index 0000000..ab58768 --- /dev/null +++ b/extern/xspublic/xscontroller/xdacommunicatorfactory.h @@ -0,0 +1,52 @@ + +// Copyright (c) 2003-2022 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 XDACOMMUNICATORFACTORY_H +#define XDACOMMUNICATORFACTORY_H + +#include "communicatorfactory.h" + +class XdaCommunicatorFactory : public CommunicatorFactory +{ +public: + XdaCommunicatorFactory(); + + using CommunicatorFactory::create; + void registerCommunicatorTypes() override; + +protected: + virtual CommunicatorTypeId filenameToCommunicatorId(const XsString& filename) const; + virtual CommunicatorTypeId portInfoToCommunicatorId(const XsPortInfo& portInfo) const; + +}; + +#endif diff --git a/extern/xspublic/xscontroller/xsaccesscontrolmode.h b/extern/xspublic/xscontroller/xsaccesscontrolmode.h new file mode 100644 index 0000000..18ca6e3 --- /dev/null +++ b/extern/xspublic/xscontroller/xsaccesscontrolmode.h @@ -0,0 +1,53 @@ + +// Copyright (c) 2003-2022 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 XSACCESSCONTROLMODE_H +#define XSACCESSCONTROLMODE_H + +/*! \addtogroup enums Global enumerations + @{ +*/ + +//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 +}; +/*! @} */ +typedef enum XsAccessControlMode XsAccessControlMode; + +//AUTO } + +#endif diff --git a/extern/xspublic/xscontroller/xsalignmentframe.h b/extern/xspublic/xscontroller/xsalignmentframe.h new file mode 100644 index 0000000..629949d --- /dev/null +++ b/extern/xspublic/xscontroller/xsalignmentframe.h @@ -0,0 +1,52 @@ + +// Copyright (c) 2003-2022 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 XSALIGNMENTFRAME_H +#define XSALIGNMENTFRAME_H + +/*! \addtogroup enums Global enumerations + @{ +*/ +//AUTO namespace xscontroller { +/*! \brief Alignment frame. +*/ +enum XsAlignmentFrame +{ + XAF_Sensor, //!< Sensor alignment frame + XAF_Local //!< Local alignment frame +}; +/*! @} */ +typedef enum XsAlignmentFrame XsAlignmentFrame; + +//AUTO } + +#endif diff --git a/extern/xspublic/xscontroller/xscalibrateddatamode.h b/extern/xspublic/xscontroller/xscalibrateddatamode.h new file mode 100644 index 0000000..312fc3d --- /dev/null +++ b/extern/xspublic/xscontroller/xscalibrateddatamode.h @@ -0,0 +1,56 @@ + +// Copyright (c) 2003-2022 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 XSCALIBRATEDDATAMODE_H +#define XSCALIBRATEDDATAMODE_H + +/*! \addtogroup enums Global enumerations + @{ +*/ +/*! \brief Legacy calibrated data output selection flags */ +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_All = XCDM_Acceleration | XCDM_GyroscopeData | XCDM_MagneticField +}; +/*! @} */ +typedef enum XsCalibratedDataMode XsCalibratedDataMode; + +#endif diff --git a/extern/xspublic/xscontroller/xscallback.h b/extern/xspublic/xscontroller/xscallback.h new file mode 100644 index 0000000..f145a60 --- /dev/null +++ b/extern/xspublic/xscontroller/xscallback.h @@ -0,0 +1,363 @@ + +// Copyright (c) 2003-2022 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 XSCALLBACK_H +#define XSCALLBACK_H + +#include "xscallbackplainc.h" + +#ifdef __cplusplus + +/*! \brief Structure that contains callback functions for the Xsens Device API + \details When programming in C++, simply overload the callback that you want to use and supply + your XsCallbackPlainC-overloaded class to one of the setCallback functions + + When programming in C, create an XsCallbackPlainC structure and initialize each function pointer to + the right function to call. The supplied first parameter is the address of the XsCallbackPlainC + object that you supplied to the setCallback function. If you do not wish to receive a specific + callback in C, set the function pointer to 0. + + In both cases, the calling application remains in control of the XsCallbackPlainC object and thus + remains responsible for cleaning it up when the it is no longer necessary. + + \note Any callback function in %XsCallback that is not overloaded will only be called once to + minimize callback overhead. +*/ +class XsCallback : public XsCallbackPlainC +{ +public: + /*! \brief Constructor */ + XsCallback() + { + m_onDeviceStateChanged = sonDeviceStateChanged; + m_onLiveDataAvailable = sonLiveDataAvailable; + m_onMissedPackets = sonMissedPackets; + m_onWakeupReceived = sonWakeupReceived; + m_onProgressUpdated = sonProgressUpdated; + m_onWriteMessageToLogFile = sonWriteMessageToLogFile; + m_onBufferedDataAvailable = sonBufferedDataAvailable; + m_onConnectivityChanged = sonConnectivityChanged; + m_onInfoResponse = sonInfoResponse; + m_onError = sonError; + m_onNonDataMessage = sonNonDataMessage; + m_onMessageDetected = sonMessageDetected; + m_onMessageReceivedFromDevice = sonMessageReceivedFromDevice; + m_onMessageSentToDevice = sonMessageSentToDevice; + m_onAllLiveDataAvailable = sonAllLiveDataAvailable; + m_onAllBufferedDataAvailable = sonAllBufferedDataAvailable; + m_onDataUnavailable = sonDataUnavailable; + m_onDataAvailable = sonDataAvailable; + m_onAllDataAvailable = sonAllDataAvailable; + m_onRecordedDataAvailable = sonRecordedDataAvailable; + m_onAllRecordedDataAvailable = sonAllRecordedDataAvailable; + m_onTransmissionRequest = sonTransmissionRequest; + m_onRestoreCommunication = sonRestoreCommunication; + } + + /*! \brief Destructor + \note Make sure that the object is removed from callback generating objects before destroying it! + */ + virtual ~XsCallback() {} + + // Swig needs these functions to be protected, not private, otherwise they are ignored. +protected: + /*! \protectedsection + \addtogroup Callbacks + @{ + */ + //! \copybrief m_onDeviceStateChanged + virtual void onDeviceStateChanged(XsDevice* dev, XsDeviceState newState, XsDeviceState oldState) + { + (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; + } + //! \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; + } + //! \copybrief m_onWakeupReceived + virtual void onWakeupReceived(XsDevice* dev) + { + (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; + } + //! \copydoc m_onWriteMessageToLogFile + virtual int onWriteMessageToLogFile(XsDevice* dev, const XsMessage* message) + { + (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; + } + //! \copydoc m_onConnectivityChanged + virtual void onConnectivityChanged(XsDevice* dev, XsConnectivityState newState) + { + (void) dev; + (void) newState; + m_onConnectivityChanged = 0; + } + //! \copydoc m_onInfoResponse + virtual void onInfoResponse(XsDevice* dev, XsInfoRequest request) + { + (void) dev; + (void) request; + m_onInfoResponse = 0; + } + //! \copydoc m_onError + virtual void onError(XsDevice* dev, XsResultValue error) + { + (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; + } + //! \copydoc m_onMessageDetected + 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; + } + //! \copydoc m_onMessageSentToDevice + 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; + } + //! \copydoc m_onAllBufferedDataAvailable + virtual void onAllBufferedDataAvailable(XsDevicePtrArray* devs, const XsDataPacketPtrArray* packets) + { + (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; + } + //! \copydoc m_onDataAvailable + virtual void onDataAvailable(XsDevice* dev, const XsDataPacket* packet) + { + (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; + } + //! \copydoc m_onRecordedDataAvailable + virtual void onRecordedDataAvailable(XsDevice* dev, const XsDataPacket* packet) + { + (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; + } + //! \copydoc m_onTransmissionRequest + virtual void onTransmissionRequest(int channelId, const XsByteArray* data) + { + (void) channelId; + (void)data; + } + //! \copydoc m_onRestoreCommunication + virtual void onRestoreCommunication(const XsString* portName, XsResultValue result) + { + (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); + } +}; +#endif + +#endif diff --git a/extern/xspublic/xscontroller/xscallbackplainc.h b/extern/xspublic/xscontroller/xscallbackplainc.h new file mode 100644 index 0000000..bd8d55f --- /dev/null +++ b/extern/xspublic/xscontroller/xscallbackplainc.h @@ -0,0 +1,282 @@ + +// Copyright (c) 2003-2022 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 XSCALLBACKPLAINC_H +#define XSCALLBACKPLAINC_H + +#include +#include +#include +#include "xsdevicestate.h" +#include "xsconnectivitystate.h" +#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 } +#endif + +struct XsDevice; +struct XsDevicePtrArray; +struct XsDataPacket; +struct XsDataPacketPtrArray; +struct XsString; +struct XsMessage; +struct XsByteArray; + +/*! \brief Structure that contains callback functions for the Xsens Device API + \details When using C++, please use the overloaded class XsCallback instead. + + This structure contains pointers to functions that will be called by XDA when certain + events occur. To use it in C, set any callback you do not wish to use to 0 and put a valid + function pointer in the others. Then pass the object to an XsControl or XsDevice object's + addCallbackHandler function. + + \note XDA does not copy the structure contents and does not take ownership of it. So make sure it + is allocated on the heap or at least removed from wherever it was added by calling + removeCallbackHandler before it is destroyed. +*/ +typedef struct XsCallbackPlainC +{ + /*! \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 + \param oldState The old device state + */ + void (*m_onDeviceStateChanged)(struct XsCallbackPlainC* thisPtr, struct XsDevice* dev, XsDeviceState newState, XsDeviceState oldState); + + /*! \brief Called when new data has been received from a device or read from a file. 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. + \param dev The device that initiated the callback. + \param packet The data packet that has been received (and processed). This may be 0 when the callback originates from a non-device, such as the XsDataBundler. + \note For most applications, attaching to the m_onDataAvailable callback is sufficient, the specific stream callbacks are only provided for exceptional cases + \sa m_onAllLiveDataAvailable \sa m_onBufferedDataAvailable \sa m_onAllBufferedDataAvailable \sa m_onDataAvailable + */ + void (*m_onLiveDataAvailable)(struct XsCallbackPlainC* thisPtr, struct XsDevice* dev, const struct XsDataPacket* packet); + + /*! \brief Called when XDA detects that packets have been missed. + \param dev The device that initiated the callback. + \param count The number of samples that were missed + \param first The sample counter / packet identifier of the first missed sample + \param last The sample counter / packet identifier of the last missed sample + */ + void (*m_onMissedPackets)(struct XsCallbackPlainC* thisPtr, struct XsDevice* dev, int count, int first, int last); + + /*! \brief Called when a wakeup message has been received from a device. This indicates that the device has just been reset or plugged in. + \param dev The device that initiated the callback. + */ + void (*m_onWakeupReceived)(struct XsCallbackPlainC* thisPtr, struct XsDevice* dev); + + /*! \brief Called when a long-duration operation has made some progress or has completed. Examples include loadLogFile and flushing of retransmissions (Awinda). When \a current == \a total the operation has completed. + \param dev The device that initiated the callback. + \param current The current progress. + \param total The total work to be done. When \a current equals \a total, the task is completed. + \param identifier An identifier for the task. This may for example be a filename for file read operations. + */ + void (*m_onProgressUpdated)(struct XsCallbackPlainC* thisPtr, struct XsDevice* dev, int current, int total, const struct XsString* identifier); + + /*! \brief Called when XDA has a message that could be written to a log file. + \param dev The device that initiated the callback. + \param message The message that will be written. + \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); + + /*! \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. + \param dev The device that initiated the callback. + \param packet The data message that is ready to be written to file \sa onWriteMessageToLogFile + \sa m_onAllLiveDataAvailable \sa m_onLiveDataAvailable \sa m_onAllBufferedDataAvailable \sa m_onDataAvailable + \note For most applications, attaching to the m_onDataAvailable callback is sufficient, the specific stream callbacks are only provided for exceptional cases + */ + void (*m_onBufferedDataAvailable)(struct XsCallbackPlainC* thisPtr, struct XsDevice* dev, const struct XsDataPacket* packet); + + /*! \brief Called when XDA has detected a change in the connectivity state of a device + \param dev The device that initiated the callback. + \param newState The new connectivity state + */ + void (*m_onConnectivityChanged)(struct XsCallbackPlainC* thisPtr, struct XsDevice* dev, XsConnectivityState newState); + + /*! \brief Called when an information request has resulted in a response + \details When the information request has completed, the data can be retrieved through the usual + functions. Ie. when a requestBatteryLevel() resulted in an onInfoResponse(.., XIR_BatteryLevel), + the XsDevice::batteryLevel function will return the received battery level. + \param dev The device that initiated the callback. + \param request The type of request that was completed + */ + void (*m_onInfoResponse)(struct XsCallbackPlainC* thisPtr, struct XsDevice* dev, XsInfoRequest request); + + /*! \brief Called when an error has occurred while handling incoming data + \param dev The device that generated the error message + \param error The error code that specifies exactly what problem occurred + */ + void (*m_onError)(struct XsCallbackPlainC* thisPtr, struct XsDevice* dev, XsResultValue error); + + /*! \brief Called when a non data, non reply message has been received + \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); + + /*! \brief Called just after a message is detected in raw data from the device. + \param dev The device that sent the message + \param type The protocol type that detected a message + \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); + + /*! \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); + + /*! \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); + + /*! \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. + \param devs A managed array of devices for which data is available. The array will be cleaned up by XDA after the callback returns. + \param packets A plain array of pointers to data packets matching the order of \a devs. The array will be cleaned up by XDA after the callback returns. + \sa m_onLiveDataAvailable \sa m_onBufferedDataAvailable \sa m_onAllBufferedDataAvailable \sa m_onDataAvailable + \note For most applications, attaching to the m_onAllDataAvailable callback is sufficient, the specific stream callbacks are only provided for exceptional cases + */ + void (*m_onAllLiveDataAvailable)(struct XsCallbackPlainC* thisPtr, struct XsDevicePtrArray* devs, const struct XsDataPacketPtrArray* packets); + + /*! \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 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. This stream will only interpolate data that is knon to be permanently unavailable. + \param devs A managed array of devices for which data is available. The array will be cleaned up by XDA after the callback returns. + \param packets A plain array of pointers to data packets matching the order of \a devs. The array will be cleaned up by XDA after the callback returns. + \sa m_onAllLiveDataAvailable \sa m_onLiveDataAvailable \sa m_onBufferedDataAvailable \sa m_onDataAvailable + \note For most applications, attaching to the m_onAllDataAvailable callback is sufficient, the specific stream callbacks are only provided for exceptional cases + */ + void (*m_onAllBufferedDataAvailable)(struct XsCallbackPlainC* thisPtr, struct XsDevicePtrArray* devs, const struct XsDataPacketPtrArray* packets); + + /*! \brief Called when XDA detects that data is forever unavailable + \details This differs from onMissedPackets, since missed packets may be retransmitted, while unavailable + data can no longer be retransmitted. + \param dev The device that initiated the callback. + \param packetId The sample counter / packet identifier of the unavailable sample + */ + void (*m_onDataUnavailable)(struct XsCallbackPlainC* thisPtr, struct XsDevice* dev, int64_t packetId); + + /*! \brief Called when new data has been received from a device or read from a file. 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 dynamically attached to either the Live or the Buffered stream and behaves accordingly. When reading from a device, it will be attached to the Live stream, while it will be attached to the Buffered stream when reading from a file. + \param dev The device that initiated the callback. + \param packet The data packet that has been received (and processed). This may be 0 when the callback originates from a non-device, such as the XsDataBundler. + \sa m_onAllLiveDataAvailable \sa m_onLiveDataAvailable \sa m_onAllBufferedDataAvailable \sa m_onBufferedDataAvailable \sa m_onAllDataAvailable + */ + void (*m_onDataAvailable)(struct XsCallbackPlainC* thisPtr, struct XsDevice* dev, const struct XsDataPacket* packet); + + /*! \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 dynamically attached to either the Live or the Buffered stream and behaves accordingly. When reading from a device, it will be attached to the Live stream, while it will be attached to the Buffered stream when reading from a file. + \param devs A managed array of devices for which data is available. The array will be cleaned up by XDA after the callback returns. + \param packets A plain array of pointers to data packets matching the order of \a devs. The array will be cleaned up by XDA after the callback returns. + \sa m_onAllLiveDataAvailable \sa m_onAllBufferedDataAvailable \sa m_onDataAvailable + */ + void (*m_onAllDataAvailable)(struct XsCallbackPlainC* thisPtr, struct XsDevicePtrArray* devs, const struct XsDataPacketPtrArray* packets); + + /*! \brief Called when new data has been received from a device in a recording state or read from a file. 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 attached to the Buffered stream and behaves accordingly, it will only be called with data that should be recorded. + \param dev The device that initiated the callback. + \param packet The data packet that has been received (and processed). This may be 0 when the callback originates from a non-device, such as the XsDataBundler. + \sa m_onAllLiveDataAvailable \sa m_onLiveDataAvailable \sa m_onAllBufferedDataAvailable \sa m_onBufferedDataAvailable \sa m_onAllDataAvailable + */ + void (*m_onRecordedDataAvailable)(struct XsCallbackPlainC* thisPtr, struct XsDevice* dev, const struct XsDataPacket* packet); + + /*! \brief Called when new data has been received for devices connected to the same main device in a recording state or read from file. 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 attached to the Buffered stream and behaves accordingly, it will only be called with data that should be recorded. + \param devs A managed array of devices for which data is available. The array will be cleaned up by XDA after the callback returns. + \param packets A plain array of pointers to data packets matching the order of \a devs. The array will be cleaned up by XDA after the callback returns. + \sa m_onAllLiveDataAvailable \sa m_onAllBufferedDataAvailable \sa m_onDataAvailable + */ + void (*m_onAllRecordedDataAvailable)(struct XsCallbackPlainC* thisPtr, struct XsDevicePtrArray* devs, const struct XsDataPacketPtrArray* packets); + + /*! \brief Called when XDA needs to send raw data to a device connected using a custom communication channel. + \param channelId The user provided identifier associated with the custom channel. + \param data The array of bytes that must be forwarded to the device + */ + void (*m_onTransmissionRequest)(struct XsCallbackPlainC* thisPtr, int channelId, const struct XsByteArray* data); + + /*! \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; +protected: + XsCallbackPlainC() + : m_onDeviceStateChanged(nullptr) + , m_onLiveDataAvailable(nullptr) + , m_onMissedPackets(nullptr) + , m_onWakeupReceived(nullptr) + , m_onProgressUpdated(nullptr) + , m_onWriteMessageToLogFile(nullptr) + , m_onBufferedDataAvailable(nullptr) + , m_onConnectivityChanged(nullptr) + , m_onInfoResponse(nullptr) + , m_onError(nullptr) + , m_onNonDataMessage(nullptr) + , m_onMessageDetected(nullptr) + , m_onMessageReceivedFromDevice(nullptr) + , m_onMessageSentToDevice(nullptr) + , m_onAllLiveDataAvailable(nullptr) + , m_onAllBufferedDataAvailable(nullptr) + , m_onDataUnavailable(nullptr) + , m_onDataAvailable(nullptr) + , m_onAllDataAvailable(nullptr) + , m_onRecordedDataAvailable(nullptr) + , m_onAllRecordedDataAvailable(nullptr) + , m_onTransmissionRequest(nullptr) + , m_onRestoreCommunication(nullptr) + {} + ~XsCallbackPlainC() throw() {} +private: + XsCallbackPlainC(XsCallbackPlainC const&); + XsCallbackPlainC& operator = (XsCallbackPlainC const&); +#endif + +} XsCallbackPlainC; + +#endif diff --git a/extern/xspublic/xscontroller/xsconnectivitystate.c b/extern/xspublic/xscontroller/xsconnectivitystate.c new file mode 100644 index 0000000..b9ae32e --- /dev/null +++ b/extern/xspublic/xscontroller/xsconnectivitystate.c @@ -0,0 +1,58 @@ + +// Copyright (c) 2003-2022 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 "xsconnectivitystate.h" +#include + +/*! \addtogroup cinterface C Interface + @{ +*/ + +/*! \brief Convert the device state to a human readable string */ +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: + ; + } + return "UnknownState"; +} + +/*! @} */ diff --git a/extern/xspublic/xscontroller/xsconnectivitystate.h b/extern/xspublic/xscontroller/xsconnectivitystate.h new file mode 100644 index 0000000..2c900c4 --- /dev/null +++ b/extern/xspublic/xscontroller/xsconnectivitystate.h @@ -0,0 +1,84 @@ + +// Copyright (c) 2003-2022 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 XSCONNECTIVITYSTATE_H +#define XSCONNECTIVITYSTATE_H + +#ifdef __cplusplus + #include +#endif + +/*! \addtogroup enums Global enumerations + @{ +*/ + +//AUTO namespace xscontroller { +/*! \brief XsDevice connectivity state identifiers */ +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. */ + XCS_Wireless, /*!< Device is connected wirelessly. */ + XCS_WirelessOutOfRange, /*!< Device was connected wirelessly and is currently out of range. */ + XCS_File, /*!< Device is reading from a file. */ + XCS_Unknown, /*!< Device is in an unknown state. */ +}; + +/*! @} */ +typedef enum XsConnectivityState XsConnectivityState; +//AUTO } + +#ifdef __cplusplus +extern "C" { +#endif + +/*! \brief Convert the device state to a human readable string */ +XDA_DLL_API const char* XsConnectivityState_toString(XsConnectivityState s); + +#ifdef __cplusplus +} // extern "C" + +#ifndef XSENS_NO_STL +namespace std +{ +/*! \brief Stream output operator for XsConnectivityState */ +template +basic_ostream<_CharT, _Traits>& operator<<(basic_ostream<_CharT, _Traits>& o, XsConnectivityState const& xs) +{ + return (o << XsConnectivityState_toString(xs)); +} +} +#endif +#endif + +#endif diff --git a/extern/xspublic/xscontroller/xscontrol_def.cpp b/extern/xspublic/xscontroller/xscontrol_def.cpp new file mode 100644 index 0000000..91de35d --- /dev/null +++ b/extern/xspublic/xscontroller/xscontrol_def.cpp @@ -0,0 +1,1154 @@ + +// Copyright (c) 2003-2022 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 "xscontrol_def.h" +#include "xdacommunicatorfactory.h" +#include +#include +#include "xsdeviceconfiguration.h" +#include +#include + +#include "proxycommunicator.h" +#include "restorecommunication.h" + +#include + +#include "mtix0device.h" +#include "mtix00device.h" +#include "mtigdevice.h" +#include // xddid and xsens_private for imar device id checks +#include "xsdeviceptrarray.h" +#include +#include +#include +#include +#include +#include +#include "broadcastdevice.h" +#include "idfetchhelpers.h" + +using namespace xsens; +using namespace XsMath; + +/*! + \class XsControl + \brief High level Motion Tracker (MT) management class + + CMT version 2 and higher do not use the explicit error codes that CMT version 1 used. Most functions + return a boolean indicating success, a null-object, empty lists or nothing at all. In some cases more + in-depth knowledge of the last error is required. For these occasions, use lastResult() or lastResultText() + to find out what exactly went wrong. + + \note This object cannot be copied. The copy constructor has been disabled. + \sa cinterface For the C interface functions. +*/ + +/*! \fn XsControl::clearCallbackHandlers(bool chain = true) + \brief Clear the callback handler list + \param chain Whether to clear the callback handlers of all connected devices as well (true, default) + or just the callback handlers of the %XsControl object (false) +*/ + +/*! \fn XsControl::addCallbackHandler(XsCallbackPlainC* cb, bool chain = true) + \brief Add a callback handler to the list + \param cb The handler to add to the list. + \param chain When set to true (default) the callback is added to connected devices as well + \note NULL and duplicate handlers are ignored, but chaining is still done. +*/ + +/*! \fn XsControl::removeCallbackHandler(XsCallbackPlainC* cb, bool chain = true) + \brief Remove a handler from the list + \param cb The handler to remove from the list. + \param chain When set to true (default) the callback is added to connected devices as well + \note If \a cb is not found in the list or if \a cb is NULL, the list is not changed, but + chaining is still done. +*/ + +/*! \fn XsControl::broadcast() const + \brief Returns the broadcast device + \details The broadcast device can be used to apply an operation to all connected devices at once (if + they support it) + \returns An XsDevice pointer representing the broadcast device +*/ + +/*! \brief Construct a new Xsens Device API control object. + \details Construct a new Xsens Device API control object that can be used to open ports, files, etc. + \return The newly constructed XsControl object +*/ +XsControl::XsControl() + : m_useFakeMessages(true) + , m_lastHwError(XRV_OK) + , m_lastHwErrorDeviceId(0) + , m_recording(false) + , m_broadcaster(0) + , m_optionsEnable(XSO_Calibrate | XSO_Orientation) + , m_optionsDisable(XSO_None) + , m_latLonAlt(XsVector()) + , m_deviceFactory(new DeviceFactory) + , m_communicatorFactory(new XdaCommunicatorFactory) + , m_restoreCommunication(nullptr) +{ + m_communicatorFactory->registerCommunicatorTypes(); + m_deviceFactory->registerDevices(); + + m_broadcaster = new BroadcastDevice(this); + m_restoreCommunication = new RestoreCommunication(this); +} + + +/*! \brief Destroy this XsControl object. + \details All connected devices are put in config mode. All serial ports and files are subsequently closed. + \sa close() +*/ +XsControl::~XsControl() +{ + try + { + close(); + delete m_broadcaster; + delete m_restoreCommunication; + } + catch (...) + {} + + delete m_deviceFactory; + delete m_communicatorFactory; + m_broadcaster = nullptr; + m_restoreCommunication = nullptr; +} + +/*! \brief Get a descriptive text for the given \a resultCode. + \param resultCode The result code to translate + \returns The \a resultCode translated into an %XsString +*/ +XsString XsControl::resultText(XsResultValue resultCode) +{ + return XsResultValue_toString(resultCode); +} + +/*! \brief Close all ports and files. + \details All devices are put in config mode before the serial port is closed. +*/ +void XsControl::close() +{ + JLDEBUGG(""); + 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) + { + removeChainedManager(*it); + + // prepareForTermination is called automatically when the reference is removed + (*it)->prepareForTermination(); + (*it)->removeRef(); + } + m_deviceList.clear(); + + m_lastHwError = XRV_OK; + m_lastHwErrorDeviceId = 0; + + m_lastResult = XRV_OK; +} + +#ifndef XSENS_NO_PORT_NUMBERS +/*! \brief Close the serial port with the given \a portNr. + \details All connected devices are put in config mode before the port is closed. + \param portNr The COM port number of the port that should be closed + \note This function is only available on Windows systems +*/ +void XsControl::closePort(int portNr) +{ + JLDEBUGG(portNr); + XSEXITLOGD(gJournal); + + closePort(XsPortInfo(portNr).portName()); +} +#endif + +/*! \brief Close the device port with the given XsDevice + \details All connected devices are put in config mode before the port is closed. + \param device the XsDevice obtained with the device() function + \sa device() +*/ +void XsControl::closePort(XsDevice* device) +{ + JLDEBUGG(device); + XSEXITLOGD(gJournal); + + for (uint16_t i = 0; i < m_deviceList.size(); ++i) + { + if (device == m_deviceList[i]) + { + m_deviceList.erase(m_deviceList.begin() + i); + removeChainedManager(device); + device->prepareForTermination(); + device->removeRef(); + } + } +} + +/*! \brief Close the serial port with the given \a portname. + \details All connected devices are put in config mode before the port is closed. + \param portname the name of the port to close (e.g. COM1 on Windows, /dev/ttyUSB0 on Linux) +*/ +void XsControl::closePort(const XsString& portname) +{ + JLDEBUGG(portname.toStdString()); + XSEXITLOGD(gJournal); + + // find appropriate port + LockReadWrite portLock(&m_portMutex); + portLock.lock(true); + + for (uint16_t i = 0; i < m_deviceList.size(); ++i) + { + if (portname == m_deviceList[i]->portName()) + closePort(m_deviceList[i]); + } +} + +/*! \brief Close the port that is used for communication with the given \a deviceId + \param deviceId The device ID to clos eth port for. When 0, the first available port is closed. + \note When the port hosts multiple devices, this function will make all devices connected to the port + invalid. +*/ +void XsControl::closePort(const XsDeviceId& deviceId) +{ + XsDevice* inf = findDevice(deviceId); + if (inf) + closePort(inf); +} + +/*! \brief Close the serial port that matches \a portinfo. + \details All connected devices are put in config mode before the port is closed. + \param portinfo A port information structure that contains the name of the port to close + \sa closePort(const XsString&) +*/ +void XsControl::closePort(const XsPortInfo& portinfo) +{ + closePort(portinfo.portName()); +} + +/*! \brief Clear the inbound data buffers of all devices + \details + \copydetails XsDevice::flushInputBuffers +*/ +void XsControl::flushInputBuffers() +{ + JLDEBUGG(""); + XSEXITLOGD(gJournal); + + for (uint32_t i = 0; i < m_deviceList.size(); i++) // loop over all devices, + { + XsDevice* dev = m_deviceList[i]; + dev->flushInputBuffers(); // flush data buffer of each device. + } +} + +/*! \brief Get the number of connected devices. + \returns The number of connected devices +*/ +int XsControl::deviceCount() const +{ + int count = 0; + for (uint32_t i = 0; i < m_deviceList.size(); i++) + { + XsDevice* dev = m_deviceList[i]; + count += 1 + (int) dev->childCount(); + } + m_lastResult = XRV_OK; + return count; +} + +/*! + \brief Get the device IDs of all the connected devices. + \returns Vector containing the device IDs of all the connected devices. +*/ +std::vector XsControl::deviceIds() const +{ + LockReadWrite portLock(&m_portMutex, LS_Read); + + std::vector result; + for (uint32_t index = 0; index < m_deviceList.size(); ++index) + { + XsDevice const* dev = m_deviceList[index]; + result.push_back(dev->deviceId()); + auto childrn = dev->children(); + for (auto child : childrn) + if (child) + result.push_back(child->deviceId()); + } + return result; +} + +/*! + \brief Get the device of the device on the given \a locationId. + + If the location ID is not found, the lastResult value is set and the + function returns a nullptr. + + \param locationId the location ID of the device we're looking for + + \returns a pointer to the device for \a locationId + + \sa lastResult() +*/ +XsDevice* XsControl::getDeviceFromLocationId(uint16_t locationId) const +{ + JLDEBUGG((int) locationId); + XSEXITLOGD(gJournal); + for (uint16_t i = 0; i < m_deviceList.size(); ++i) + { + XsDevice* d = m_deviceList[i]->getDeviceFromLocationId(locationId); + if (!d) + continue; + m_lastResult = XRV_OK; + return d; + } + m_lastResult = XRV_NOTFOUND; + return nullptr; +} + +/*! \brief Clear the last hardware error. + \sa lastHardwareError(); +*/ +void XsControl::clearHardwareError() +{ + m_lastHwErrorDeviceId = 0; + m_lastHwError = XRV_OK; +} + +/*! \brief Get the last hardware error code + \returns The last hardware error + \sa lastResult() + \sa resultText() +*/ +XsResultValue XsControl::lastHardwareError() const +{ + return m_lastHwError; +} + +/*! \returns The device ID that caused the last hardware error. +*/ +XsDeviceId XsControl::lastHardwareErrorDeviceId() const +{ + return m_lastHwErrorDeviceId; +} + +/*! \brief Get the result value of the last operation. + \details The result values are codes that describe a failure in more detail. + \returns the last known error code + \sa resultText(XsResultValue), lastResultText() +*/ +XsResultValue XsControl::lastResult() const +{ + return m_lastResult.lastResult(); +} + +/*! \brief Get the accompanying error text for the value returned by lastResult() + \details This is more than a convenience function for + \code + XsString lastResultText = XsControl::resultText(xscontrol->lastResult()); + \endcode + It may provide situation-specific information instead. + \returns a human readable error description + \sa resultText(XsResultValue), lastResult() +*/ +XsString XsControl::lastResultText() const +{ + return m_lastResult.lastResultText(); +} + +/*! \brief Get the number of main devices. + \returns the number of main devices + \sa mainDeviceIds(), mainDeviceId(const XsDeviceId&) +*/ +int XsControl::mainDeviceCount() const +{ + // m_lastResult = XRV_OK; + return (int) m_deviceList.size(); +} + +/*! + \brief Get the number of connected MTs. + + \returns the number of MTs, including both main and child devices. + + \sa mtDeviceIds() +*/ +/* We assume motion trackers cannot have children and if a device is a child device, it is a motion tracker */ +int XsControl::mtCount() const +{ + int count = 0; + for (uint32_t i = 0; i < m_deviceList.size(); i++) + { + XsDevice* dev = m_deviceList[i]; + if (dev->isMotionTracker()) + count++; + } + m_lastResult = XRV_OK; + return count; +} + +/*! + \brief Get the device IDs of the available main devices. + + Main devices are the devices communicating with the serial port, typically Bodypacks, + Awinda Stations and stand-alone MTis or MTxs. + \returns a std::vector with the device IDs. +*/ +std::vector XsControl::mainDeviceIds() const +{ + LockReadWrite portLock(&m_portMutex); + portLock.lock(false); + + m_lastResult = XRV_OK; + std::vector ids; + ids.reserve(m_deviceList.size()); + for (uint32_t index = 0; index < m_deviceList.size(); ++index) + ids.push_back(m_deviceList[index]->deviceId()); + return ids; +} + +/*! + \brief Get the device IDs of the available MTs. + \returns A std::vector with the device IDs. + \sa mtCount + \internal +*/ +std::vector XsControl::mtDeviceIds() const +{ + m_lastResult = XRV_OK; + + LockReadWrite portLock(&m_portMutex); + portLock.lock(false); + + std::vector result; + for (uint32_t i = 0; i < m_deviceList.size(); ++i) + { + XsDevice const* main = m_deviceList.at(i); + if (main->isMotionTracker()) + result.push_back(main->deviceId()); + } + return result; +} + +/*! \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() + + \internal + The function places the sensors in configuration mode in the appropriate order as they are sorted by sortBySync. +*/ +void XsControl::gotoConfig(void) +{ + JLDEBUGG(""); + XSEXITLOGD(gJournal); + + m_broadcaster->gotoConfig(); +} + +/*! + \brief Place all sensors connected through a serial port into Measurement Mode. + + The function places the sensors in measurement mode in the appropriate order + as they are sorted by sortBySync. +*/ +void XsControl::gotoMeasurement() +{ + JLDEBUGG(""); + XSEXITLOGD(gJournal); + + m_broadcaster->gotoMeasurement(); +} + +/*! \brief Starts restore communication procedure. + \details Restores the communication settings to the default factory settings. + \note Works with RS422 and legacy products only. + \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) +{ + return m_restoreCommunication->start(portName); +} + +/*! \brief Stops restore communication procedure. +*/ +void XsControl::stopRestoreCommunication() +{ + m_restoreCommunication->stop(); +} + +/*! \brief Test if the given \a deviceId is docked + + Only wireless devices can be regarded as docked. + + \param deviceId the ID of the device to investigate + + \returns true if the device is docked, false otherwise +*/ +bool XsControl::isDeviceDocked(const XsDeviceId& deviceId) const +{ + (void)deviceId; + return false; +} + +/*! + \brief Test if the given \a deviceId is an MTw and if it is wirelessly connected. + \details If the device ID is not found, the function returns false and the lastResult value is set. + + \param deviceId the ID of the device to investigate + + \returns true if the device is wirelessly connected, false otherwise +*/ +bool XsControl::isDeviceWireless(const XsDeviceId& deviceId) const +{ + (void)deviceId; + return false; +} + +/*! \cond XS_INTERNAL */ +/*! \brief Creates and adds a device as master. The new master will be owned by this. + \param[in] communicator the Communicator for the created device. + \note communicator is a pointer to a dynamically allocated Communicator. + The created device will take ownership of the communicator. + \returns the newly created device. +*/ +XsDevice* XsControl::addMasterDevice(Communicator* communicator) +{ + XsDevice* result = nullptr; + XsDevice* dev = m_deviceFactory->createMasterDevice(communicator); + if (dev != nullptr) + { + setPersistentSettings(dev); + addChainedManager(dev); + m_deviceList.push_back(dev); + result = dev; + } + return result; +} +/*! \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. + + \param filename the name of the file to open + + \returns true on success, false on failure + + \sa lastResult(), loadLogFile(), logFileName() +*/ +bool XsControl::openLogFile(const XsString& filename) +{ + JLDEBUGG(filename.toStdString()); + XSEXITLOGD(gJournal); + + LockReadWrite portLock(&m_portMutex); + portLock.lock(true); + + // try opening the file as a regular mtb file + auto object = Communicator::createUniquePtr(m_communicatorFactory->create(filename)); + + copyCallbackHandlersTo(object.get()); + //object->addCallbackHandler(d); + if (!object->openLogFile(filename)) + { + m_lastResult = object->lastResult(); + return false; + } + + // 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(); + + m_lastResult = XRV_OK; + return true; + +} + +#ifndef XSENS_NO_PORT_NUMBERS +/*! \brief Open a communication channel to the given COM \a portNr. + + This is a convenience overload for openPort(const XsString&, XsBaudRate, bool). + This function is available on Microsoft Windows only due to the ambiguous nature of port numbers on other platforms. + + \param baudrate The baudrate used on the port. + \param portNr The port number. + \param timeout The maximum number of ms to try to put the device in config mode before giving up, if 0 the default value is used + \param[in] detectRs485 Enable more extended scan to detect rs485 devices + + \returns true if the port was opened successfully, false otherwise + + \sa openPort(const XsString&, XsBaudRate, uint32_t, bool) \sa lastResult() +*/ +bool XsControl::openPort(int portNr, XsBaudRate baudrate, uint32_t timeout, bool detectRs485) +{ + JLDEBUGG("port " << (int32_t) portNr << " baudrate " << baudrate << " timeout " << timeout << " detectRs485 " << (int32_t)detectRs485); + XSEXITLOGD(gJournal); + + XsPortInfo pinfo(portNr, baudrate); + return openPort(pinfo, timeout, detectRs485); +} +#endif + +/*! + \brief Open a communication channel on serial port with the given \a portname. + + If opening the port is successful, the connected devices are available through the XsControl interface. + + The expected value for \a portname on Microsoft Windows platforms is "COMx" where x is the port number. + + \param baudrate The baudrate used on the port. + \param portname The name of the port. + \param timeout The maximum number of ms to try to put the device in config mode before giving up, if 0 the default value is used + \param[in] detectRs485 Enable more extended scan to detect rs485 devices + + \returns true on success, false otherwise + + \sa openPort(int, XsBaudRate, uint32_t, bool) +*/ +bool XsControl::openPort(const XsString& portname, XsBaudRate baudrate, uint32_t timeout, bool detectRs485) +{ + XsPortInfo localPortInfo = XsPortInfo(portname, baudrate); + uint16_t vid = vidFromString(portname.toStdString()); + uint16_t pid = pidFromString(portname.toStdString()); + localPortInfo.setVidPid(vid, pid); + return openPort(localPortInfo, timeout, detectRs485); +} + +/*! \cond XS_INTERNAL */ +/*! \brief Finalize opening the port + + Takes ownership of the passed Communicator. +*/ +bool XsControl::finalizeOpenPort(Communicator* communicator, XsPortInfo& portinfo, uint32_t timeout, bool detectRs485) +{ + XSEXITLOGD(gJournal); + if (!communicator) + { + m_lastResult = XRV_INVALIDPARAM; + return false; + } + + auto serialPort = Communicator::createUniquePtr(communicator); + + copyCallbackHandlersTo(serialPort.get()); + + if (timeout) + serialPort->setGotoConfigTimeout(timeout); + + bool retval = serialPort->openPort(portinfo, OPS_Full, detectRs485); + if (serialPort->masterDeviceId().isValid()) + portinfo.setDeviceId(serialPort->masterDeviceId()); + + if (!retval) + { + m_lastResult.set(serialPort->lastResult(), serialPort->lastResultText()); + return false; + } + + // note that addMasterDevice ALWAYS takes ownership of communicator + if (!addMasterDevice(serialPort.release())) + return false; + + return true; +} +/*! \endcond */ + +/*! \brief Open a communication channel using the details in the supplied %XsPortInfo structure + \param portinfo Contains the details of the port to open. The \a portinfo may be updated with a detected deviceid + \param timeout The maximum number of ms to try to put the device in config mode before giving up, if 0 the default value is used + \param[in] detectRs485 Enable more extended scan to detect rs485 devices. Only necessary if \a portInfo does not contain a device ID of an RS485 device + \returns true on success, false otherwise + \sa openPort(const XsString &, XsBaudRate, uint32_t) +*/ +bool XsControl::openPort(XsPortInfo& portinfo, uint32_t timeout, bool detectRs485) +{ + JLDEBUGG("port " << portinfo << " timeout " << timeout << " detectRs485 " << (int32_t)detectRs485); + XSEXITLOGD(gJournal); + + Communicator* xs3info = findXbusInterface(portinfo); + if (xs3info) + { + if (xs3info->masterDeviceId().isValid() && !portinfo.deviceId().isValid()) + portinfo.setDeviceId(xs3info->masterDeviceId()); + m_lastResult = XRV_ALREADYOPEN; + return true; + } + + return finalizeOpenPort(m_communicatorFactory->create(portinfo), portinfo, timeout, detectRs485); +} + +/*! \brief Open a communication channel using the details in the supplied %XsPortInfo structure using the supplied credentials + \param portinfo Contains the details of the connection to open. The device ID in the structure may be updated by this function. + \param id The user ID to be supplied for the authentication + \param key The key to be supplied for the authentication + \param timeout The maximum number of ms to try to put the device in config mode before giving up, if 0 the default value is used + \returns true on success, false otherwise + \sa openPort(const XsPortInfo &, uint32_t, bool) + \sa openPort(const XsString &, XsBaudRate, uint32_t) +*/ +bool XsControl::openPortWithCredentials(XsPortInfo& portinfo, XsString const& id, XsString const& key, uint32_t timeout) +{ + JLDEBUGG("port " << portinfo << " id " << id << " key " << key << " timeout " << timeout); + XSEXITLOGD(gJournal); + + Communicator* xs3info = findXbusInterface(portinfo); + if (xs3info) + { + if (xs3info->masterDeviceId().isValid()) + portinfo.setDeviceId(xs3info->masterDeviceId()); + m_lastResult = XRV_ALREADYOPEN; + return true; + } + + xs3info = m_communicatorFactory->create(portinfo); + xs3info->setCredentials(id, key); + return finalizeOpenPort(xs3info, portinfo, timeout, false); +} + +/*! \brief Open a custom communication channel + \param channelId: User-provided identifier for the custom channel. Supplying the same channel Id more than once gives a XV_ALREADYOPEN result (\sa getLastResult) + \param channelLatency: The worst-case round-trip delay in milliseconds induced by the custom channel. XDA will add this latency to its communication timeout values + \param detectRs485 Enable more extended scan to detect rs485 devices + \returns true if the port was successfully opened. + \sa closeCustomPort + \sa transmissionReceived +*/ +bool XsControl::openCustomPort(int channelId, uint32_t channelLatency, bool detectRs485) +{ + if (m_proxyChannels.find(channelId) != m_proxyChannels.end()) + { + // We are assuming that the device is there in this case + m_lastResult = XRV_ALREADYOPEN; + return true; + } + + ProxyCommunicator* proxy = new ProxyCommunicator(channelId, channelLatency); + m_proxyChannels[channelId] = proxy; + + XsPortInfo portInfo = ProxyCommunicator::createPortInfo(channelId); + if (!finalizeOpenPort(proxy, portInfo, 0, detectRs485)) + { + //Please note that finalizeOpenPort takes ownership of the Communicator pointer and will clean it up on error + m_proxyChannels.erase(channelId); + return false; + } + + return true; +} + +/*! \brief Returns the port information for a custom communication channel + \param channelId: The user-provided identifier of the channel + \returns The port info for the given channel + \sa openCustomPort +*/ +XsPortInfo XsControl::customPortInfo(int channelId) const +{ + if (m_proxyChannels.find(channelId) == m_proxyChannels.end()) + return XsPortInfo(); + + return m_proxyChannels.at(channelId)->portInfo(); +} + +/*! \brief Closes a custom communication channel + \note When closing a custom channel the device will not be switched to config mode. This must be done manually \sa XsDevice::gotoConfig + \param channelId: The user-provided identifier of the channel to close. This identifier must match with the one used to open the port + \sa openCustomPort +*/ +void XsControl::closeCustomPort(int channelId) +{ + auto it = m_proxyChannels.find(channelId); + if (it == m_proxyChannels.end()) + return; + + ProxyCommunicator* p = it->second; + if (p) + { + XsDevice* dev = findDevice(p->masterDeviceId()); + if (dev) + dev->setGotoConfigOnClose(false); + m_proxyChannels.erase(it); + closePort(p->masterDeviceId()); + } + else + m_proxyChannels.erase(it); +} + +/*! \brief Feed data coming back from an Xsens device over a custom channel into XDA + \note For correct operation of XDA it is key that the transmissionReceived function is called from a separate thread + \param channelId: The user-provided identifier of the custom channel + \param data: The data to feed back into XDA +*/ +void XsControl::transmissionReceived(int channelId, const XsByteArray& data) +{ + if (m_proxyChannels.find(channelId) == m_proxyChannels.end()) + return; + m_proxyChannels[channelId]->handleReceivedData(data); +} + +/*! + \brief Open a communication channel on serial port with the given \a portname. + + If opening the port is successful, the connected devices are available through the XsControl interface. + + The expected value for \a portname on Microsoft Windows platforms is "COMx" where x is the port number. + + \param baudrate The baudrate used on the port. + \param portname The name of the port. + \param imarType The type of iMAR device that is used + \param timeout The maximum number of ms to try to put the device in config mode before giving up, if 0 the default value is used + + \returns true on success, false otherwise + + \sa openPort(int, XsBaudRate, bool) +*/ +bool XsControl::openImarPort_internal(const XsString&, XsBaudRate, int, uint32_t) +{ + return false; +} + +/*! + \brief Get the device ID of the dock device for the given \a deviceId. + + This function returns the ID of the docking station that deviceId is plugged into. + If the docking station itself is not an open port in this XsControl or the device is not + plugged into a docking station, the function will return a 0 id. + + \param deviceId the ID of the device to find the dock parent for + + \returns the ID of the device that has \a deviceId docked + + \sa isDeviceDocked +*/ +XsDeviceId XsControl::dockDeviceId(const XsDeviceId& deviceId) const +{ + JLDEBUGG(deviceId.toString().toStdString()); + + LockReadWrite portLock(&m_portMutex); + portLock.lock(false); + + m_lastResult = XRV_OK; + if (!deviceId.isMtw()) + return XsDeviceId(); + + XsDevice* dev = findDevice(deviceId); + if (!dev) + { + m_lastResult = XRV_INVALIDID; + return XsDeviceId(); + } + + for (uint32_t index = 0; index < m_deviceList.size(); ++index) + { + if (m_deviceList[index]->deviceIsDocked(dev)) + return m_deviceList[index]->deviceId(); + } + + return XsDeviceId(); +} + +/*! \brief Sets the current GNSS position of the system + \details This function will update the Latitude, Longitude and Altitude of the system and all + connected devices. This differs from broadcast()->setInitialPositionLLA() in that the setting is persistent + for the XsControl and will be applied to devices connected after the setting has been made. + Note: this XDA data type is the setting initialPositionLLA, which is set by setInitialPositionLLA. + It's value is therefore static. Use LatitudeLongitude to retrieve the live position data from the MTi. + \param lla A vector containing the desired Latitude, Longitude and Altitude + \returns true since the function always succeeds, the boolean return value is for consistency in + the interface. +*/ +bool XsControl::setInitialPositionLLA(const XsVector& lla) +{ + m_latLonAlt = lla; + m_broadcaster->setInitialPositionLLA(lla); + return true; +} + +/*! \brief Returns the XsDevice interface object associated with the supplied \a deviceId + \param deviceId The ID of the device to return, 0 to return the first available main device + \returns The XsDevice attached to the \a deviceId or 0 if the device was not found + \sa broadcast() +*/ +XsDevice* XsControl::device(const XsDeviceId& deviceId) const +{ + return findDevice(deviceId); +} + +/*! \brief Returns all main XsDevice interface objects + \returns A list containing pointers to the main device XsDevice objects +*/ +XsDevicePtrArray XsControl::mainDevices() const +{ + XsDevicePtrArray rv; + for (std::vector::const_iterator it = m_deviceList.begin(); it != m_deviceList.end(); ++it) + rv.push_back(*it); + return rv; +} + +/*! \brief Returns the broadcast device + \details The broadcast device can be used to apply an operation to all connected devices at once (if + they support it) + \returns An XsDevice pointer representing the broadcast device +*/ +XsDevice* XsControl::broadcast() const +{ + return m_broadcaster; +} + +/*! \cond XS_INTERNAL */ +/*! \brief Check if there are devices in a recording state and update m_recording accordingly +*/ +void XsControl::updateRecordingState() +{ + for (size_t i = 0 ; i < m_deviceList.size(); ++i) + if (m_deviceList[i]->isRecording()) + m_recording = true; + + m_recording = false; +} + +/*! \brief Find the device info of the supplied \a deviceId and return a pointer to the item in the list + + \param deviceId the device ID to look for + + \returns the device matching \a deviceId, NULL otherwise. On failure lastResult() is set. +*/ +XsDevice* XsControl::findDevice(const XsDeviceId& deviceId) const +{ + if (m_deviceList.empty()) + { + m_lastResult = XRV_NOFILEORPORTOPEN; + return NULL; + } + + m_lastResult = XRV_OK; + + if (deviceId.toInt() == 0) + return m_deviceList[0]; // simply return the first item in the list + + for (size_t i = 0; i < m_deviceList.size(); i++) + { + XsDevice* dev = m_deviceList[i]; + + if (!dev) + break; + + if (dev->deviceId() == deviceId) + return dev; + + dev = dev->findDevice(deviceId); + if (dev) + return dev; + } + + m_lastResult = XRV_INVALIDID; + return NULL; +} + +/*! \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) +{ + XsDevice* dev = findDevice(deviceId); + if (!dev) + return; + + if (dev->isMasterDevice()) + closePort(deviceId); +} + +/*! \brief Searches for the XBUS interface in a device object + \param deviceId The ID of device object to search in + \returns The found communication interface +*/ +Communicator* XsControl::findXbusInterface(const XsDeviceId& deviceId) const +{ + XsDevice* dev = findDevice(deviceId); + if (!dev) + return NULL; + + return dev->communicator(); +} + +/*! \brief Searches for the XBUS interface in a port information object + \param portInfo The port information object to search in + \returns The found communication interface +*/ +Communicator* XsControl::findXbusInterface(const XsPortInfo& portInfo) const +{ + return findXbusInterface(portInfo.portName()); +} + +/*! \brief Searches for the XBUS interface in a port + \param portName The name of port to search in + \returns The found communication interface +*/ +Communicator* XsControl::findXbusInterface(const XsString& portName) const +{ + for (size_t i = 0; i < m_deviceList.size(); ++i) + { + assert(m_deviceList[i]->communicator()); + if (portName == m_deviceList[i]->communicator()->portInfo().portName()) + return m_deviceList[i]->communicator(); + } + return NULL; +} +/*! \endcond */ + +/*! \brief Load filter profile definitions from a settings file with the given \a filename + + To use the filtering properly, XDA requires filter settings usually stored in a + scenarios.xsb file. XSB is the Xsens Settings Binary format. This function allows you to + specify the full path + filename to use instead of the default filter profiles embedded in the dll. + + \param filename The full path+filename to use for loading the parameters. When empty, the default + filter profiles are loaded. On Linux this is "./scenarios.xsb", on Windows it is read + from the dll resource. + + \returns true on success, false on failure +*/ +bool XsControl::loadFilterProfiles(const XsString&) +{ + return false; +} + +/*! \cond XS_INTERNAL */ +/*! \brief Close a serial communication port by \a index + \param i The index of a device from a device list +*/ +void XsControl::closePortByIndex(uint32_t i) +{ + JLDEBUGG(i); + // XSEXITLOGD(gJournal); + + XsDevice* dev = m_deviceList[i]; + closePort(dev); +} +/*! \endcond */ + +/*! \brief Return the currently enabled options + \return The options that are set to be enabled + \sa setOptions \sa disabledOptions +*/ +XsOption XsControl::enabledOptions() const +{ + return m_optionsEnable; +} + +/*! \brief Return the currently explicitly disabled options + \return The options that are set to be explicitly disabled + \sa setOptions \sa enabledOptions +*/ +XsOption XsControl::disabledOptions() const +{ + return m_optionsDisable; +} + +/*! \brief Peristently enable or disable options + \details These options are used to specify whether XDA should compute certain kinds of data from + available other data and what data-retention policy to use. On a system with limited + resources it may be useful to limit the processing and data retention done by XDA. By default XDA will + do all processing it can do, but retain as little data as possible. + This function remembers the setting and applies it to new devices when they are created as well + as broadcasting it to existing devices. + In case of conflict, \a enable supersedes \a disable. + \param enable A logically OR'ed combination of XsOptions to enable + \param disable A logically OR'ed combination of XsOptions to disable + \note While XsDevice uses these options in an additional manner, remembering whatever was enabled/disabled before, + calling the XsControl version replaces all its remembered enable/disable values. + \sa setOptionsForce +*/ +void XsControl::setOptions(XsOption enable, XsOption disable) +{ + m_optionsEnable = XsOption_purify(enable); + m_optionsDisable = disable; + m_broadcaster->setOptions(m_optionsEnable, m_optionsDisable); +} + +/*! \brief Peristently enable or disable options + \details These options are used to specify whether XDA should compute certain kinds of data from + available other data and what data-retention policy to use. On a system with limited + resources it may be useful to limit the processing and data retention done by XDA. By default XDA will + do all processing it can do, but retain as little data as possible. + This function remembers the setting and applies it to new devices when they are created as well + as broadcasting it to existing devices. + Any non-enabled options are explicitly disabled. + \param enabled A logically OR'ed combination of the desired enabled XsOptions + \note Contrary to \a setOptions this function will do a hard override of all options of the child devices as it will + assume that any non-enabled option should be specifically disabled. + \sa setOptions +*/ +void XsControl::setOptionsForce(XsOption enabled) +{ + m_optionsEnable = XsOption_purify(enabled); + m_optionsDisable = (XsOption)((~m_optionsEnable) & XSO_All); + m_broadcaster->setOptions(m_optionsEnable, m_optionsDisable); +} + +/*! \cond XS_INTERNAL */ +/*! \brief Write the persistent settings to a (newly created) device + \details These settings include whether to perform filtering, explicit initialPositionLLA values, etc + \param dev The device to write the settings to +*/ +void XsControl::setPersistentSettings(XsDevice* dev) +{ + dev->setOptions(m_optionsEnable, m_optionsDisable); + + if (!m_latLonAlt.empty()) + dev->setInitialPositionLLA(m_latLonAlt); +} +/*! \endcond */ + +#ifndef XDA_PRIVATE_BUILD + #include "xscontrol_public.h" +#else + #include "xscontrolex.h" +#endif +XsControl* XsControl::construct() +{ + return new XsControlEx; +} diff --git a/extern/xspublic/xscontroller/xscontrol_def.h b/extern/xspublic/xscontroller/xscontrol_def.h new file mode 100644 index 0000000..dd4f21d --- /dev/null +++ b/extern/xspublic/xscontroller/xscontrol_def.h @@ -0,0 +1,264 @@ + +// Copyright (c) 2003-2022 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 XSCONTROL_DEF_H +#define XSCONTROL_DEF_H + +#include "xsdef.h" +#include +#include "xscallback.h" +#include + +#include +#include +#include +#include +#include +#include +#include +#include "devicefactory.h" +#include "callbackmanagerxda.h" +#include +#include "lastresultmanager.h" +#include + +struct XSNOEXPORT Communicator; +class XSNOEXPORT BroadcastDevice; +class XSNOEXPORT XdaCommunicatorFactory; +class XSNOEXPORT ProxyCommunicator; +class XSNOEXPORT RestoreCommunication; + +struct XSNOEXPORT XsDevice; +class XSNOEXPORT EmtsManager; +class XSNOEXPORT NetworkScanner; + +//AUTO namespace xstypes { +//AUTO+chdr struct PstdInt; +//AUTO+chdr struct XsTypeDefs; +//AUTO+chdr struct XsException; +struct XsString; +struct XsPortInfo; +struct XsPortInfoArray; +struct XsSyncSetting; +struct XsDataPacket; +struct XsMessage; +struct XsFilterProfile; + +//AUTO enum XsResultValue; +//AUTO enum XsBaud; +//AUTO enum XsXbusMessageId; +//AUTO struct XsDeviceIdArray; +//AUTO struct XsIntArray; +//AUTO struct XsSyncSettingArray; +//AUTO typename XsDevice; +//AUTO enum XsFilePos; +//AUTO struct XsFilterProfileArray; +//AUTO enum XsOption; + +//AUTO } + +//AUTO namespace xscontroller { +struct XsDeviceConfiguration; +struct XsDevicePtrArray; +//AUTO } + +#ifndef XsensThreadReturn + #define XsensThreadReturn XSENS_THREAD_RETURN // for generator + #define XsensThreadParam XSENS_THREAD_PARAM // for generator +#endif + +struct XsControl : public CallbackManagerXda +{ +public: + XsControl(); + ~XsControl(); + + void flushInputBuffers(); + + static XsString resultText(XsResultValue resultCode); + + void clearHardwareError(); + void close(); + + bool openPort(const XsString& portname, XsBaudRate baudrate, uint32_t timeout = 0, bool detectRs485 = false); + bool openPort(XsPortInfo& portinfo, uint32_t timeout = 0, bool detectRs485 = false); + bool openPortWithCredentials(XsPortInfo& portinfo, XsString const& id, XsString const& key, uint32_t timeout = 0); + bool openCustomPort(int channelId, uint32_t channelLatency, bool detectRs485 = false); + virtual bool openImarPort_internal(const XsString& portname, XsBaudRate baudrate, int imarType, uint32_t timeout = 0); +#ifndef XSENS_NO_PORT_NUMBERS + XSNOLINUXEXPORT bool openPort(int portNr, XsBaudRate baudrate, uint32_t timeout = 0, bool detectRs485 = false); +#endif + void closePort(const XsString& portname); + void closePort(const XsDeviceId& deviceId); + void closePort(const XsPortInfo& portinfo); + void closeCustomPort(int channelId); +#ifndef XSENS_NO_PORT_NUMBERS + XSNOLINUXEXPORT void closePort(int portNr); +#endif + void closePort(XsDevice* device); + + XsPortInfo customPortInfo(int channelId) const; + + bool openLogFile(const XsString& filename); + + XsResultValue lastResult() const; + XsString lastResultText() const; + XsResultValue lastHardwareError() const; + XsDeviceId lastHardwareErrorDeviceId() const; + + int deviceCount() const; + int mainDeviceCount() const; + std::vector mainDeviceIds() const; + virtual int mtCount() const; + virtual std::vector mtDeviceIds() const; + virtual std::vector deviceIds() const; + XsDevice* getDeviceFromLocationId(uint16_t locationId) const; + XsDeviceId dockDeviceId(const XsDeviceId& deviceId) const; + + virtual bool isDeviceWireless(const XsDeviceId& deviceId) const; + virtual bool isDeviceDocked(const XsDeviceId& deviceId) const; + + virtual bool loadFilterProfiles(const XsString& filename); + + XsOption enabledOptions() const; + XsOption disabledOptions() const; + void setOptions(XsOption enable, XsOption disable); + void setOptionsForce(XsOption enabled); + + bool setInitialPositionLLA(const XsVector& lla); + + XsDevice* device(const XsDeviceId& deviceId) const; + XsDevicePtrArray mainDevices() const; + XsDevice* broadcast() const; + + void transmissionReceived(int channelId, const XsByteArray& data); +#ifdef DOXYGEN + // Explicit inheritance for generator and doxygen + void XSNOCOMEXPORT clearCallbackHandlers(bool chain = true); + void XSNOCOMEXPORT addCallbackHandler(XsCallbackPlainC* cb, bool chain = true); + void XSNOCOMEXPORT removeCallbackHandler(XsCallbackPlainC* cb, bool chain = true); +#endif + + // 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; + } + + virtual bool XSNOEXPORT finalizeOpenPort(Communicator* communicator, XsPortInfo& portinfo, uint32_t timeout, bool detectRs485); + void gotoConfig(); + void gotoMeasurement(); + + XsResultValue startRestoreCommunication(const XsString& portName); + void stopRestoreCommunication(); + +protected: + virtual XsDevice* XSNOCOMEXPORT addMasterDevice(Communicator* communicator); + +#ifndef DOXYGEN + XSNOEXPORT XsControl(const XsControl&) = delete; +#endif + + //void gotoOperational(const XsDeviceId& stationId = XsDeviceId()); + + //! Boolean variable for enabling/disabling the use of fake messages + bool m_useFakeMessages; + + //! This list contains device-information and cached data per device. + std::vector m_deviceList; + + //! This map contains the proxy channels + std::map m_proxyChannels; + + //! The last result of an operation + mutable LastResultManager m_lastResult; + + //! Contains the last serious error reported by CMT3 + XsResultValue m_lastHwError; + + //! Contains the XsDevice ID of the device that caused the last hardware error + XsDeviceId m_lastHwErrorDeviceId; + + //////////////////// + // thread management, multiple locks should always use this same order + //! Controls access to the serial ports, also used to suspend the thread. + mutable xsens::MutexReadWrite m_portMutex; + //! Always held by the thread when it is running + mutable xsens::Mutex m_runMutex; + + //! AwindaStationIndication of threads started or not + volatile std::atomic_bool m_recording; + + void updateRecordingState(); + + XsDevice* findDevice(const XsDeviceId& deviceId) const; + + 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; + + void closePortByIndex(uint32_t index); + + //! The broadcast device object + BroadcastDevice* m_broadcaster; + + //! Contains all enable options + XsOption m_optionsEnable; + + //! Contsins all disabled options + XsOption m_optionsDisable; + + //! This vector contains the latitude, longitude and altitude + XsVector3 m_latLonAlt; + + void setPersistentSettings(XsDevice* dev); + + //! The device factory object + DeviceFactory* m_deviceFactory; + + //! The communicator factory object + XdaCommunicatorFactory* m_communicatorFactory; + + //! The restore communication object + RestoreCommunication* m_restoreCommunication; + + /*! \cond XS_INTERNAL */ + friend class BroadcastDevice; + friend class BroadcastForwardFunc; + /*! \endcond */ // XS_INTERNAL +}; + +#endif diff --git a/extern/xspublic/xscontroller/xscontrol_public.h b/extern/xspublic/xscontroller/xscontrol_public.h new file mode 100644 index 0000000..625175a --- /dev/null +++ b/extern/xspublic/xscontroller/xscontrol_public.h @@ -0,0 +1,45 @@ + +// Copyright (c) 2003-2022 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 XSCONTROL_PUBLIC_H +#define XSCONTROL_PUBLIC_H + +#include "xscontrol_def.h" + +/*! \class XsControlEx + \brief An abstract internal struct of a control object +*/ +struct XsControlEx : public XsControl +{ +}; + +#endif diff --git a/extern/xspublic/xscontroller/xscontrollerconfig.h b/extern/xspublic/xscontroller/xscontrollerconfig.h new file mode 100644 index 0000000..f02db28 --- /dev/null +++ b/extern/xspublic/xscontroller/xscontrollerconfig.h @@ -0,0 +1,224 @@ + +// Copyright (c) 2003-2022 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. +// + +#define XSENS_NO_AUTOLIB +// include this file in Visual Studio using C/C++->Advanced->Force Includes (the /FI option) +#ifndef XSCONTROLLER_CONFIG_H +#define XSCONTROLLER_CONFIG_H + +// define to build with Journaller +#ifndef HAVE_JOURNALLER + //#define HAVE_JOURNALLER +#endif + +////////////////////////////////////////////////// +// generic preprocessor defines + +// make sure both _WIN32 and WIN32 are defined if either of them is. +#if defined(_WIN32) || defined(_M_IX86) + #ifndef WIN32 + #define WIN32 + #endif + #ifndef XSENS_WINDOWS + #define XSENS_WINDOWS + #endif +#endif + +#ifdef WIN32 + #ifndef _WIN32 + #define _WIN32 + #define XSENS_WINDOWS + #endif +#endif + +#ifdef _WIN32 + #define USE_WINUSB +#else + #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 +#endif +#ifndef _CRT_SECURE_NO_WARNINGS + #define _CRT_SECURE_NO_WARNINGS +#endif + +#ifdef __GNUC__ + #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 +#else + #ifndef XSENS_32BIT + #define XSENS_32BIT + #endif +#endif + +// all xsens libraries should use unicode +#ifndef 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 +#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) + //// Debug or RelWithDeb build + #if defined(XSENS_RELEASE) + //// RelWithDeb build + #else + //// Debug build + #endif + #else + //// Release build + #endif +*/ + +////////////////////////////////////////////////// +// more generic preprocessor 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: \ + className(className const &) = delete; \ + className &operator = (className const &) = delete +#endif + +#ifdef __cplusplus + #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 +#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 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 +#endif +#ifndef XSNOLINUXEXPORT + #define XSNOLINUXEXPORT +#endif + +#endif diff --git a/extern/xspublic/xscontroller/xscoordinatesystem.h b/extern/xspublic/xscontroller/xscoordinatesystem.h new file mode 100644 index 0000000..da68d84 --- /dev/null +++ b/extern/xspublic/xscontroller/xscoordinatesystem.h @@ -0,0 +1,48 @@ + +// Copyright (c) 2003-2022 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 XSCOORDINATESYSTEM_H +#define XSCOORDINATESYSTEM_H + +/*! \addtogroup enums Global enumerations + @{ +*/ +/*! \brief Coordinate system definition flags */ +enum XsCoordinateSystem +{ + CS_NorthWestUp, + CS_NorthEastDown, +}; +/*! @} */ +typedef enum XsCoordinateSystem XsCoordinateSystem; + +#endif diff --git a/extern/xspublic/xscontroller/xsdef.cpp b/extern/xspublic/xscontroller/xsdef.cpp new file mode 100644 index 0000000..a722995 --- /dev/null +++ b/extern/xspublic/xscontroller/xsdef.cpp @@ -0,0 +1,40 @@ + +// Copyright (c) 2003-2022 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 "xsdef.h" +#include + +//*! \brief Scale the battery level to a percentage */ +int xsScaleBatteryLevel(int batteryLevel) +{ + return (int) std::min(batteryLevel / 2.0, 100.0); +} diff --git a/extern/xspublic/xscontroller/xsdef.h b/extern/xspublic/xscontroller/xsdef.h new file mode 100644 index 0000000..f0473e3 --- /dev/null +++ b/extern/xspublic/xscontroller/xsdef.h @@ -0,0 +1,336 @@ + +// Copyright (c) 2003-2022 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. +// + +/*! \file + \brief Macros and types for use in the Xsens communication protocol and Xsens Device API classes + + This file contains useful macros for the Xsens Device API classes. These include + Message Ids, sizes of messages, result/error codes, etc. + + The macros and enumerators in this file are either used on multiple levels (XsResultValue) + or so numerous (XMID_...) that it would not be justifiable to put them in a header + file of a particular level. +*/ + +#ifndef XSDEF_H +#define XSDEF_H + +#include "xscontrollerconfig.h" + +////////////////////////////////////////////////////////////////////////////////////////// +// 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_DATAEXT0 6 + +#define XS_SELFTEST_OK 0x1FF + +// message data lengths +#define XS_LEN_TRANSPORTMODE 1 +#define XS_LEN_DEVICEID 4 +#define XS_LEN_INITBUSRESULTS 4 +#define XS_LEN_PERIOD 2 +#define XS_LEN_BUSPWR 2 +#define XS_LEN_DATALENGTH 2 +#define XS_LEN_CONFIGURATION 118 +#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_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_SYNCINMODE 2 +#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 +#define XS_LEN_SYNCOUTPULSEWIDTH 4 +#define XS_LEN_ERRORMODE 2 +#define XS_LEN_TRANSMITDELAY 2 +#define XS_LEN_OBJECTALIGNMENT 36 +#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_LOCATIONID 2 +#define XS_LEN_EXTOUTPUTMODE 2 +#define XS_LEN_INITTRACKMODE 2 +#define XS_LEN_STOREFILTERSTATE 0 +#define XS_LEN_UTCTIME 12 +#define XS_LEN_FILTERPROFILELABEL 20 +#define XS_LEN_FILTERPROFILEFULL (1+1+XS_LEN_FILTERPROFILELABEL) +#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_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_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_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_ORIENT_EULERDATA 12 +#define XS_LEN_ORIENT_MATRIXSTA 36 +#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_ORIENT_QUATDATA_FLT 4 +#define XS_LEN_ORIENT_EULERDATA_FLT 3 +#define XS_LEN_ORIENT_MATRIXSTA_FLT 9 + +#define XS_INVALIDSETTINGVALUE 0xFFFFFFFF + + + +// Configuration message defines +#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_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_DATALENGTH 102 +#define XS_CONF_OUTPUTMODE 104 +#define XS_CONF_OUTPUTSETTINGS 106 +#define XS_CONF_BLOCKLEN 20 +// To calculate the offset in data field for output mode of sensor #2 use +// CONF_OUTPUTMODE + 1*CONF_BLOCKLEN +#define XS_CONF_MASTERDIDLEN 4 +#define XS_CONF_PERIODLEN 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_RESERVED_CLIENTLEN 32 +#define XS_CONF_RESERVED_HOSTLEN 32 +#define XS_CONF_NUMDEVICESLEN 2 +// Configuration sensor block properties +#define XS_CONF_DIDLEN 4 +#define XS_CONF_DATALENGTHLEN 2 +#define XS_CONF_OUTPUTMODELEN 2 +#define XS_CONF_OUTPUTSETTINGSLEN 4 + +// Clock frequency for offset & pulse width +#define XS_SYNC_CLOCKFREQMHZ 29.4912 +#define XS_SYNC_CLOCK_NS_TO_TICKS (XS_SYNC_CLOCKFREQMHZ * 1.0e-3) +#define XS_SYNC_CLOCK_TICKS_TO_NS (1.0e3 / XS_SYNC_CLOCKFREQMHZ) +#define XS_SYNC_CLOCK_US_TO_TICKS (XS_SYNC_CLOCKFREQMHZ * 1.0) +#define XS_SYNC_CLOCK_TICKS_TO_US (1.0 / XS_SYNC_CLOCKFREQMHZ) + +// SyncIn params +#define XS_PARAM_SYNCIN_MODE 0x00 +#define XS_PARAM_SYNCIN_SKIPFACTOR 0x01 +#define XS_PARAM_SYNCIN_OFFSET 0x02 + +// SyncIn mode +#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_TYPE_DOSAMPLING 0x0000 +#define XS_SYNCIN_TYPE_SENDLASTDATA 0x0004 +#define XS_SYNCIN_TYPE_CLOCK 0x0010 +#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 + +// SyncOut mode +#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 + +// Initial tracking mode (SetInitTrackMode) +#define XS_INITTRACKMODE_DISABLED 0x0000 +#define XS_INITTRACKMODE_ENABLED 0x0001 + +// Filter settings params +#define XS_PARAM_FILTER_GAIN 0x00 +#define XS_PARAM_FILTER_RHO 0x01 +#define XS_DONOTSTORE 0x00 +#define XS_STORE 0x01 + +// AMDSetting (SetAMD) +#define XS_AMDSETTING_DISABLED 0x0000 +#define XS_AMDSETTING_ENABLED 0x0001 + +#define XS_PARAM_ROTSENSOR 0x00 +#define XS_PARAM_ROTLOCAL 0x01 + +// Send raw string mode +#define XS_SENDRAWSTRING_INIT 0 +#define XS_SENDRAWSTRING_DEFAULT 1 +#define XS_SENDRAWSTRING_SEND 2 + +// Timeouts +#define XS_TO_DEFAULT 500 +#define XS_TO_INIT 250 +#define XS_TO_RETRY 50 + +#define XS_PERIOD_10HZ 11520 // invalid with gps pulse time correction +#define XS_PERIOD_12HZ 9600 +#define XS_PERIOD_15HZ 7680 // invalid with gps pulse time correction +#define XS_PERIOD_16HZ 7200 +#define XS_PERIOD_18HZ 6400 // invalid with gps pulse time correction +#define XS_PERIOD_20HZ 5760 +#define XS_PERIOD_24HZ 4800 +#define XS_PERIOD_25HZ 4608 // invalid with gps pulse time correction +#define XS_PERIOD_30HZ 3840 +#define XS_PERIOD_32HZ 3600 +#define XS_PERIOD_36HZ 3200 +#define XS_PERIOD_40HZ 2880 +#define XS_PERIOD_45HZ 2560 // invalid with gps pulse time correction +#define XS_PERIOD_48HZ 2400 +#define XS_PERIOD_50HZ 2304 // invalid with gps pulse time correction +#define XS_PERIOD_60HZ 1920 +#define XS_PERIOD_64HZ 1800 +#define XS_PERIOD_72HZ 1600 +#define XS_PERIOD_75HZ 1536 // invalid with gps pulse time correction +#define XS_PERIOD_80HZ 1440 +#define XS_PERIOD_90HZ 1280 // invalid with gps pulse time correction +#define XS_PERIOD_96HZ 1200 + +#define XS_PERIOD_100HZ 1152 +#define XS_PERIOD_120HZ 960 +#define XS_PERIOD_128HZ 900 +#define XS_PERIOD_144HZ 800 +#define XS_PERIOD_150HZ 768 // invalid with gps pulse time correction +#define XS_PERIOD_160HZ 720 +#define XS_PERIOD_180HZ 640 +#define XS_PERIOD_192HZ 600 +#define XS_PERIOD_200HZ 576 +#define XS_PERIOD_225HZ 512 // invalid with gps pulse time correction +#define XS_PERIOD_240HZ 480 +#define XS_PERIOD_256HZ 450 +#define XS_PERIOD_288HZ 400 +#define XS_PERIOD_300HZ 384 +#define XS_PERIOD_320HZ 360 +#define XS_PERIOD_360HZ 320 +#define XS_PERIOD_384HZ 300 +#define XS_PERIOD_400HZ 288 +#define XS_PERIOD_450HZ 256 // invalid with gps pulse time correction +#define XS_PERIOD_480HZ 240 +#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 +#endif + +/*! \cond XS_INTERNAL */ + +int XDA_DLL_API xsScaleBatteryLevel(int batteryLevel); + +#define XS_AUTO_SAVE_FRAMES 5000 +#define XS_FILE_LAST_FRAME 0xFFFFFFFF + +#define XEMTS_SIZE 1320 + +#define XS_MAX_VPORTNAME_LEN 32 + +/*! \endcond */ // XS_INTERNAL +#endif diff --git a/extern/xspublic/xscontroller/xsdevice_def.cpp b/extern/xspublic/xscontroller/xsdevice_def.cpp new file mode 100644 index 0000000..9782094 --- /dev/null +++ b/extern/xspublic/xscontroller/xsdevice_def.cpp @@ -0,0 +1,4623 @@ + +// Copyright (c) 2003-2022 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 "xsdevice_def.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "protocolhandler.h" +#include "communicator.h" +#include "mtbdatalogger.h" +#include +#include +#include "xsselftestresult.h" +#include +#include +#include +#include +#include "nmea_protocolhandler.h" +#include +#include "supportedsyncsettings.h" +#include "messageserializer.h" +#include "xsdeviceptrarray.h" +#include +#include +#include +#include +#include "xsdef.h" +#include "xsiccrepmotionresult.h" +#include + +//! \cond DOXYGEN_SHOULD_SKIP_THIS +using namespace xsens; +using namespace XsTime; + +#if 1 && defined(XSENS_RELEASE) && defined(XSENS_DEBUG) + // prevent spamming logs with identical "packet missed" loglines + #define ONLYFIRSTMTX2 if (!deviceId().isMtx2() || firstChild() == this) +#else + #define ONLYFIRSTMTX2 +#endif +//! \endcond + +#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) +#else + #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) + \brief Clear the callback handler list + \param chain Whether to clear the callback handlers of all child devices as well (true, default) + or just the callback handlers of this %XsDevice object (false) +*/ + +/*! \fn XsDevice::addCallbackHandler(XsCallbackPlainC* cb, bool chain = true) + \brief Add a callback handler to the list + \param cb The handler to add to the list. + \param chain When set to true (default) the callback is added to child devices as well + \note NULL and duplicate handlers are ignored, but chaining is still done. +*/ + +/*! \fn XsDevice::removeCallbackHandler(XsCallbackPlainC* cb, bool chain = true) + \brief Remove a handler from the list + \param cb The handler to remove from the list. + \param chain When set to true (default) the callback is added to child devices as well + \note If \a cb is not found in the list or if \a cb is NULL, the list is not changed, but + 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 +*/ +XsDevice::XsDevice(XsDeviceId const& id) + : m_latestLivePacket(new XsDataPacket) + , m_latestBufferedPacket(new XsDataPacket) + , m_unavailableDataBoundary(-1) + , m_deviceId(id) + , m_state(XDS_Initial) + , m_connectivity(XCS_Disconnected) + , m_communicator(nullptr) + , m_logFileInterface(nullptr) + , m_master(this) + , m_refCounter(0) + , m_writeToFile(false) + , m_isInitialized(false) + , m_terminationPrepared(false) + , m_gotoConfigOnClose(true) + , m_justWriteSetting(false) + , m_skipEmtsReadOnInit(true) + , m_options(XSO_None) + , m_startRecordingPacketId(-1) + , m_stopRecordingPacketId(-1) + , m_stoppedRecordingPacketId(-1) + , m_lastAvailableLiveDataCache(new XsDataPacket) + , m_toaDumpFile(nullptr) +{ + CREATETOADUMPFILE(); + JLDEBUGG(this << " Created device " << deviceId()); +} + +/*! \brief Construct a device using \a inf for communication + \details Using this constructor implies that this device is a master device (master() returns this). + \param comm The Communicator to use for this device +*/ +XsDevice::XsDevice(Communicator* comm) + : m_latestLivePacket(new XsDataPacket) + , m_latestBufferedPacket(new XsDataPacket) + , m_unavailableDataBoundary(-1) + , m_deviceId(0) + , m_state(XDS_Initial) + , m_connectivity(XCS_Disconnected) + , m_communicator(comm) + , m_logFileInterface(nullptr) + , m_master(this) + , m_refCounter(0) + , m_writeToFile(false) + , m_isInitialized(false) + , m_terminationPrepared(false) + , m_gotoConfigOnClose(true) + , m_justWriteSetting(false) + , m_skipEmtsReadOnInit(false) + , m_options(XSO_None) + , m_startRecordingPacketId(-1) + , m_stopRecordingPacketId(-1) + , m_stoppedRecordingPacketId(-1) + , m_lastAvailableLiveDataCache(new XsDataPacket) + , m_toaDumpFile(nullptr) +{ + // put callback managers and callbacks in place + copyCallbackHandlersFrom(m_communicator); + addChainedManager(m_communicator); + + m_deviceId = m_communicator->masterDeviceId(); + + m_communicator->setMasterDevice(this); + + if (m_communicator->isPortOpen()) + m_connectivity = XCS_PluggedIn; + else if (m_communicator->isReadingFromFile()) + { + m_state = XDS_Measurement; + m_connectivity = XCS_File; + } + CREATETOADUMPFILE(); + JLDEBUGG(this << " Created device " << deviceId()); +} + +/*! \brief Construct a device with device id \a childDeviceId for master \a masterDevice + \param masterDevice The master device ID to construct for + \param childDeviceId The child device ID to construct with + \details Communication uses \a masterDevice's channel +*/ +XsDevice::XsDevice(XsDevice* masterDevice, const XsDeviceId& childDeviceId) + : m_latestLivePacket(new XsDataPacket) + , m_latestBufferedPacket(new XsDataPacket) + , m_unavailableDataBoundary(-1) + , m_deviceId(childDeviceId) + , m_state(XDS_Initial) + , m_connectivity(XCS_Unknown) + , m_communicator(nullptr) + , m_logFileInterface(nullptr) + , m_master(masterDevice) + , m_refCounter(0) + , m_writeToFile(false) + , m_isInitialized(false) + , m_terminationPrepared(false) + , m_gotoConfigOnClose(true) + , m_justWriteSetting(false) + , m_skipEmtsReadOnInit(true) + , m_options(XSO_None) + , m_startRecordingPacketId(-1) + , m_stopRecordingPacketId(-1) + , m_stoppedRecordingPacketId(-1) + , m_lastAvailableLiveDataCache(new XsDataPacket) + , m_toaDumpFile(nullptr) +{ + (void)masterDevice; + JLDEBUGG(this << " Created device " << deviceId() << " child of " << masterDevice << " " << (masterDevice ? masterDevice->deviceId() : XsDeviceId())); +} + +/*! \brief Destroy the device */ +XsDevice::~XsDevice() +{ + JLDEBUGG(this << " did " << deviceId() << " refcounter " << m_refCounter.load()); + LockSuspendable locky(&m_deviceMutex, LS_Write); // This is to make sure that we're not deleting the object while some other thread is using it. Especially the communicator threads tend to still be active at this point. + + assert(m_refCounter.load() == 0); + assert(m_terminationPrepared); + + try + { + if (m_latestLivePacket) + { + delete m_latestLivePacket; + m_latestLivePacket = nullptr; + } + if (m_latestBufferedPacket) + { + delete m_latestBufferedPacket; + m_latestBufferedPacket = nullptr; + } + if (m_lastAvailableLiveDataCache) + { + delete m_lastAvailableLiveDataCache; + m_lastAvailableLiveDataCache = nullptr; + } + + locky.unlock(); // the lock will interfere with thread termination + if (isMasterDevice() && m_communicator) + { + removeChainedManager(m_communicator); + m_communicator->destroy(); + m_communicator = nullptr; + } + + if (m_logFileInterface) + { + m_logFileInterface->close(); + delete m_logFileInterface; + } + + JLDEBUGG(this << " destroyed"); + + if (m_toaDumpFile) + { + fflush(m_toaDumpFile); + fclose(m_toaDumpFile); + m_toaDumpFile = nullptr; + } + } + catch (...) + { + } +} + +/*! \brief Return the master device of this device + \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 +{ + return m_master; +} + +/*! \cond XS_INTERNAL */ +//! \brief Remove all defined data processing components for this device +void XsDevice::clearProcessors() +{ +} + +/*! \brief Stop the processing thread before beginning to destroy the object + \details This should be called at the start of every inheriting destructor +*/ +void XsDevice::prepareForTermination() +{ + if (!m_terminationPrepared) + { + JLDEBUGG("Preparing " << deviceId() << " for termination"); + + updateDeviceState(XDS_Destructing); + + if (isMasterDevice() && m_communicator != nullptr && m_communicator->isPortOpen()) + { + if (m_gotoConfigOnClose) + gotoConfig(); + m_communicator->closePort(); + } + // finishLastProcessingTask(); // make sure no processing is going on in the background either... + m_terminationPrepared = true; + } +} + +/*! \brief Update the device state immediately. + \details The function marks the completion of a device state change. The callbacks that mark the end + of a state transition should only be called from within this function or its overrides. + \param newState The desired state + \note The function can be overridden because some devices support different states +*/ +void XsDevice::updateDeviceState(XsDeviceState newState) +{ + LockSuspendable locky(&m_deviceMutex, LS_Write); + LockGuarded lockG(&m_deviceMutex); + + // we don't allow any transitions out of the (final) destructing state + if (m_state == XDS_Destructing) + return; + + XsDeviceState oldState = m_state; + if (oldState != newState) + { + ONLYFIRSTMTX2 + JLDEBUGG("did: " << m_deviceId << " new: " << newState << " old: " << m_state); + // some special case handling + switch (newState) + { + 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_WaitingForRecordingStart: // from + updateDeviceState(XDS_Measurement); + return; + + default: + 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: + 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: + 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); + 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; + lockG.unlock(); + locky.unlock(); + onDeviceStateChanged(this, newState, oldState); + } +} + +/*! \brief Returns true when software filtering is enabled for this device */ +bool XsDevice::isSoftwareFilteringEnabled() const +{ + return false; +} + +/*! \brief Returns true when software calibration is enabled for this device */ +bool XsDevice::isSoftwareCalibrationEnabled() const +{ + return false; +} + +/*! \endcond */ + +/*! \brief Find the child device with \a deviceid + \details This function returns the child device of the current device that matches the given ID. + \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) const +{ + if (deviceid == m_deviceId) + return const_cast(this); + return nullptr; +} + +/*! \brief Find the child device with \a deviceid + \details This function returns the child device of the current device that matches the given ID. + \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 +{ + return const_cast(const_cast(this)->findDevice(deviceid)); +} + +/*! \brief On closePort the device will go to config by default, with this function it is possible to prevent that. + \param gotoConfigOnClose boolean */ +void XsDevice::setGotoConfigOnClose(bool gotoConfigOnClose) +{ + m_gotoConfigOnClose = gotoConfigOnClose; +} +/*! \brief Get the batterylevel of this device + The battery level is a value between 0 and 100 that indicates the remaining capacity as a percentage. + Due to battery characteristics, this is not directly the remaining time, but just a rough indication. + + Bodypack: The amount of time remaining for measurement given any battery level greatly depends on the type of batteries used, + the number of sensors attached to the Bodypack and the used output options. + Mtw: The last known battery level for this motion tracker. First call \sa requestBatteryLevel to have a battery level available. + The callback \sa onInfoResponse with ID XIR_BatteryLevel will indicate when the requested battery level is available. + This function is available in both config and measurement mode. + For devices in wired mode this function can be called without calling \sa requestBatteryLevel first + \returns The battery level in the range 0-100 +*/ +int XsDevice::batteryLevel() const +{ + return 0; +} + +/*! \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; +} + +/*! \brief Set the legacy update rate of the device + \param[in] rate The desired legacy update rate for the device + \returns true if the update rate was successfully set + \sa deviceMode \sa setDeviceMode +*/ +bool XsDevice::setUpdateRate(int) +{ + return false; +} + +/*! \brief Returns the device option flags + \returns The current configured device option flags +*/ +XsDeviceOptionFlag XsDevice::deviceOptionFlags() const +{ + return XDOF_None; +} + +/*! \cond XS_INTERNAL */ +/*! \brief Find the output configuration for \a dataType +*/ +XsOutputConfiguration XsDevice::findConfiguration(XsDataIdentifier dataType) const +{ + XsDataIdentifier mask; + if ((dataType & XDI_TypeMask) == dataType) + mask = XDI_TypeMask; + else + mask = XDI_FullTypeMask; + + XsOutputConfigurationArray cfg = outputConfiguration(); + auto item = std::find_if(cfg.begin(), cfg.end(), + [&](const XsOutputConfiguration & cfg) + { + return (cfg.m_dataIdentifier & mask) == dataType; + } + + ); + if (item == cfg.end()) + return XsOutputConfiguration(); + return *item; +} +/*! \endcond */ + +/*! \brief Returns the currently configured update rate for the supplied \a dataType + \details This function checks if the configured output contains \a dataType and returns the + associated update rate. In some cases 65535 (0xFFFF) will be returned, which means 'as fast + as possible'. This applies to things like a packet counter, which is sent with every packet + and can therefore have an unpredictable update rate. + This function only checks the output configured in the device, not possible computed data + \param dataType The type of data to get the update rate for. + \returns The requested update rate or 0 if the type is not configured for output +*/ +int XsDevice::updateRateForDataIdentifier(XsDataIdentifier dataType) const +{ + XsOutputConfiguration cfg = findConfiguration(dataType); + + if (cfg.m_dataIdentifier == XDI_None) + return updateRate(); + + return cfg.m_frequency; +} + +/*! \brief Returns the currently configured update rate for the supplied \a dataType + \details This function checks if the configured output contains \a dataType and returns the + associated update rate. In some cases 65535 (0xFFFF) will be returned, which means 'as fast + as possible'. This applies to things like a packet counter, which is sent with every packet + and can therefore have an unpredictable update rate. + Where updateRateForDataIdentifier only checks the outputs configured in the device, this function also + checks what can and will be computed from the data. + \see updateRateForDataIdentifier + \param dataType The type of data to get the update rate for. + \returns The requested update rate or 0 if the type is not configured for output +*/ +int XsDevice::updateRateForProcessedDataIdentifier(XsDataIdentifier dataType) const +{ + return updateRateForDataIdentifier(dataType); +} + +/*! \brief Returns if the currently configured output contains \a dataType + \param dataType The type of data to check the output for. + \returns true if \a dataType is configured for output + \sa outputConfiguration \sa hasProcessedDataEnabled \sa updateRateForDataIdentifier +*/ +bool XsDevice::hasDataEnabled(XsDataIdentifier dataType) const +{ + return checkDataEnabled(dataType, outputConfiguration()); +} + +/*! \cond XS_INTERNAL */ +/*! \brief Returns true if the supplied \a configurations contains \a dataType */ +bool XsDevice::checkDataEnabled(XsDataIdentifier dataType, XsOutputConfigurationArray const& configurations) +{ + XsDataIdentifier mask; + if ((dataType & XDI_TypeMask) == dataType) + mask = XDI_TypeMask; + else // if ((dataType & XDI_FullTypeMask) == dataType) + mask = XDI_FullTypeMask; + + dataType = dataType & mask; + + for (XsOutputConfigurationArray::const_iterator i = configurations.begin(); i != configurations.end(); ++i) + { + if (dataType == (i->m_dataIdentifier & mask)) + return true; + } + return false; +} +/*! \endcond */ + +/*! \brief Returns if the currently configured output contains \a dataType after processing on the host + \details Where hasDataEnabled() only checks the outputs configured in the device, this function also + checks what can and will be computed from the data. + \param dataType The type of data to check the output for. + \returns true if \a dataType is configured for output + \sa hasDataEnabled \sa processedOutputConfiguration \sa updateRateForDataIdentifier +*/ +bool XsDevice::hasProcessedDataEnabled(XsDataIdentifier) const +{ + return false; +} + +/*! \brief Return the firmware version + \returns The firmware version of the live device + \note The firmware version is not stored in mtb files, so when reading from file this function will + return an empty %XsVersion object +*/ +XsVersion XsDevice::firmwareVersion() const +{ + return m_firmwareVersion; +} + +/*! \cond XS_INTERNAL */ +/*! \brief Set the cached firmware version of the device +*/ +void XsDevice::setFirmwareVersion(const XsVersion& version) +{ + if (m_firmwareVersion != version) +#ifdef XSENS_DEBUG + JLDEBUGG("Device " << deviceId() << " has firmware version " << version.toString()); +#else + JLWRITEG("Device " << deviceId() << " has firmware version " << version.toString()); +#endif + m_firmwareVersion = version; +} + +/*! \brief Extracts the firmware version of the device. + \param message: XMID_FirmwareRevision message. +*/ +void XsDevice::extractFirmwareVersion(XsMessage const& message) +{ + XsVersion old = m_firmwareVersion; + m_firmwareVersion = XsVersion(message.getDataByte(0), message.getDataByte(1), message.getDataByte(2)); + if (message.getDataSize() > 3) + m_firmwareVersion.setBuild((int)(int32_t) message.getDataLong(3)); + if (message.getDataSize() > 7) + m_firmwareVersion.setReposVersion((int)(int32_t) message.getDataLong(7)); + if (old != m_firmwareVersion) +#ifdef XSENS_DEBUG + JLDEBUGG("Device " << deviceId() << " has firmware version " << m_firmwareVersion.toString()); +#else + JLWRITEG("Device " << deviceId() << " has firmware version " << m_firmwareVersion.toString()); +#endif +} + +/*! \brief Return the related Communicator */ +Communicator* XsDevice::communicator() const +{ + return m_communicator; +} + +/*! \brief Return the related Communicator for logging */ +DataLogger* XsDevice::logFileInterface(std::unique_ptr& myLock) const +{ + if (!myLock) + myLock.reset(new xsens::Lock(&m_logFileMutex, true)); + else if (!myLock->isLocked()) + myLock->lock(); + return m_logFileInterface; +} + +/*! \brief Set the device id of this device +*/ +void XsDevice::setDeviceId(const XsDeviceId& deviceid) +{ + m_deviceId = deviceid; +} +/*! \endcond */ + +/*! \brief Return the device ID of the device + \details Each Xsens device has a unique ID. The ID identifies the device as well as the + product family it belongs to. + \returns The device ID +*/ +XsDeviceId const& XsDevice::deviceId() const +{ + return m_deviceId; +} + +/*! \brief returns whether this device is in a master role regarding the device synchronization + \returns true if the device has a synchronization master role +*/ +bool XsDevice::isSyncMaster() const +{ + return false; +} + +/*! \brief returns whether this device is in a slave role regarding the device synchronization + \returns true if the device has a synchronization slave role +*/ +bool XsDevice::isSyncSlave() const +{ + return false; +} + +/*! \brief Return the device with bus ID \a busid + \param busid The busid to serach for + \returns The XsDevice corresponding to the supplied \a busid +*/ +XsDevice* XsDevice::deviceAtBusId(int busid) +{ + if (isMasterDevice() && (busid == XS_BID_MASTER || busid == 1)) + return this; + return nullptr; +} + +/*! \brief Return the device with bus ID \a 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 +{ + // prevent code duplication, go through the non-const implementation + return const_cast(const_cast(this)->deviceAtBusId(busid)); +} + +/*! \brief Restart the software filter used by this device */ +void XsDevice::restartFilter() +{ +} + +/*! \brief Get the result value of the last operation. + \details The result values are codes that describe a failure in more detail. + \returns the last known error code + \sa resultText(XsResultValue), lastResultText() +*/ +XsResultValue XsDevice::lastResult() const +{ + return m_lastResult; +} + +/*! \brief Get the accompanying error text for the value returned by lastResult() + It may provide situation-specific information instead. + \returns a human readable error description + \sa resultText(XsResultValue), lastResult() +*/ +XsString XsDevice::lastResultText() const +{ + return m_lastResult.lastResultText(); +} + +/*! \brief Returns true if this is a motion tracker + \returns true if this is a motion tracker or false if it is a master device such as an Awinda + Station or a Bodypack +*/ +bool XsDevice::isMotionTracker() const +{ + return false; +} + +/*! \brief Returns the length of the data in the legacy MTData packets that the device will send in + measurement mode. + \details This function will only return a value when the device is configured for legacy output, + otherwise it will return 0. + \returns The data size of the MTData packets that will be sent by the device + \sa setDeviceMode \sa deviceMode +*/ +int XsDevice::dataLength() const +{ + return 0; +} + +/*! \brief The baud rate configured for cabled connection + \details This differs from the baudRate() function in that it will return the configured value for + a serial connection even if the device is currently not configured for serial communication (ie + when it is connected with a direct USB cable or wirelessly), whereas the baudRate() function + will return the baud rate of the current connection. + \returns The configured baud rate +*/ +XsBaudRate XsDevice::serialBaudRate() const +{ + return XBR_Invalid; +} + +/*! \brief Get the baud rate (communication speed) of the serial port on which the given \a deviceId is connected. + \details This differs from the serialBaudRate() function in that it will only return the baud rate + of the current connection, whereas the serialBaudRate() function will return the configured value for + a serial connection even if the device is currently not configured for serial communication (ie + when it is connected with a direct USB cable or wirelessly). + \returns The baud rate of the serial connection or XBR_Invalid +*/ +XsBaudRate XsDevice::baudRate() const +{ + Communicator* comm = communicator(); + if (!comm) + return XBR_Invalid; + return comm->portInfo().baudrate(); +} + +/*! \brief The bus ID for this device + \returns The bus ID of the device +*/ +int XsDevice::busId() const +{ + return XS_BID_MASTER; +} + +/*! \brief Change the serial baudrate to \a baudrate + \details This function is only useful when using a serial communication channel, such as a + serial-USB converter or a direct COM port. It is advised to make the baud rate as high as your + platform allows, to minimize latency and problems with bandwidth. + + After setting the baudrate and communicating over the same communication channel, it is required to + reset the device. + \param baudrate The desired serial baudrate + \returns true if the baud rate was successfully updated +*/ +bool XsDevice::setSerialBaudRate(XsBaudRate baudrate) +{ + if (!isMasterDevice()) + return false; + + Communicator* comm = communicator(); + if (!comm) + return false; + + if (comm->isReadingFromFile()) + return false; + + XsMessage snd(XMID_SetBaudrate, XS_LEN_BAUDRATE); + snd.setBusId(XS_BID_MASTER); + snd.setDataByte(XsBaud::rateToCode(baudrate)); + + if (!doTransaction(snd, 1000))//increased from 500 to 1000, since the One Series timed out very rarely with the old value + return false; + + if (comm->portInfo().baudrate() == XBR_Invalid || + comm->portInfo().baudrate() == baudrate || + m_justWriteSetting) + return true; + + if (!resetRemovesPort()) + return reset(); + else + return true; +} + +/*! \brief Get the current port configuration of a device + \remarks Only Mti6x0 devices supported + \returns The current port configurations of the device + \sa setPortConfiguration, XsBaudCode +*/ +XsIntArray XsDevice::portConfiguration() const +{ + return XsIntArray(); +} + +/*! \brief Change the port configuration of a device + \details Configures the 2 ports of the 6x0 device + The integers consist of: + - bits 0:7 XsBaudcode + - bit 8 Enable flow control + - bit 9 Use 2nd stop bit + - bit 10 Use Parity bit + - bit 11 Even/odd parity + - bit 12:15 reserved + - bits 16:19 bits XsProtocol + \param config An array of elements containing a configuration for each port (UART and RS232) + \remarks Only Mti6x0 devices supported + \returns true if the port configuration was successfully updated + \sa portConfiguration, XsBaudCode +*/ +bool XsDevice::setPortConfiguration(XsIntArray& config) +{ + (void)config; + return false; +} + +/*! \cond XS_INTERNAL */ +/*! \brief Reset packet stamping by re-initializing the highestpacket + \details Used after config-measurement cycling +*/ +void XsDevice::resetPacketStamping() +{ + LockGuarded lockG(&m_deviceMutex); + JLDEBUGG("did: " << deviceId()); + m_latestLivePacket->clear(); + m_latestBufferedPacket->clear(); + m_lastDataOkStamp = 0; + m_unavailableDataBoundary = -1; + m_packetStamper.resetTosEstimation(); + CREATETOADUMPFILE(); +} +/*! \endcond */ + +/*! \brief Put this device in measurement mode + \details Measurement mode is where the device is sampling data and producing inertial and orientation + output. + \returns true if the device was successfully put in measurement mode or was already in measurement + mode + \sa gotoConfig +*/ +bool XsDevice::gotoMeasurement() +{ + JLDEBUGG(deviceId()); + + if (!isMasterDevice()) + { + m_lastResult.set(XRV_OTHER, deviceId().toString() << " is not a master device, can't switch to measurement mode"); + JLERRORG(m_lastResult.lastResultText()); + return false; + } + + if (m_state == XDS_Measurement) // if we're already in measurement mode, we can ignore this command + { + m_lastResult.set(XRV_OK, deviceId().toString() << " was already in measurement mode"); + JLDEBUGG(m_lastResult.lastResultText()); + return true; + } + + Communicator* comm = communicator(); + if (!comm) + { + m_lastResult.set(XRV_OTHER, deviceId().toString() << " doesn't have a communicator, can't switch to measurement mode"); + JLERRORG(m_lastResult.lastResultText()); + return false; + } + + if (comm->isReadingFromFile()) + { + m_lastResult.set(XRV_OTHER, deviceId().toString() << " is reading from file, can't switch to measurement mode"); + JLERRORG(m_lastResult.lastResultText()); + return false; + } + + XsResultValue res = comm->gotoMeasurement(); + if (res == XRV_OK) + { + // switch device (and children) to measuring state + setDeviceState(XDS_Measurement); + m_lastResult.set(XRV_OK, deviceId().toString() << " now in measurement mode"); + JLDEBUGG(m_lastResult.lastResultText()); + return true; + } + + m_lastResult.set(XRV_OTHER, deviceId().toString() << " communicator refused with code " << res << " (" << XsResultValue_toString(res) << "), switching children back to config mode"); + JLERRORG(m_lastResult.lastResultText()); + return false; +} + +/*! \brief Put the device in config mode + \details Device settings can only be changed in config mode, since changing anything during + measurement would mess up the sample timing. + \returns true if the device was successfully put in config mode or was already in config mode + \sa gotoMeasurement +*/ +bool XsDevice::gotoConfig() +{ + JLDEBUGG(deviceId()); + if (!isMasterDevice()) + { + m_lastResult.set(XRV_OTHER, deviceId().toString() << " is not a master device, can't switch to config mode"); + JLERRORG(m_lastResult.lastResultText()); + return false; + } + + if (m_state == XDS_Config) // if we're already in config mode, we can ignore this command + { + m_lastResult.set(XRV_OK, deviceId().toString() << " was already in config mode"); + JLDEBUGG(m_lastResult.lastResultText()); + return true; + } + + Communicator* comm = communicator(); + if (!comm) + { + m_lastResult.set(XRV_OTHER, deviceId().toString() << " doesn't have a communicator, can't switch to config mode"); + JLERRORG(m_lastResult.lastResultText()); + return false; + } + + if (comm->isReadingFromFile()) + { + m_lastResult.set(XRV_INVALIDOPERATION, deviceId().toString() << " is reading from file, can't switch to config mode"); + JLDEBUGG(m_lastResult.lastResultText()); + return false; + } + + XsResultValue res = comm->gotoConfig(); + if (res == XRV_OK) + { + setDeviceState(XDS_Config); + m_lastResult.set(XRV_OK, deviceId().toString() << " now in config mode"); + JLDEBUGG(m_lastResult.lastResultText()); + return true; + } + + m_lastResult.set(XRV_OTHER, deviceId().toString() << " communicator refused with code " << res << " (" << XsResultValue_toString(res) << "), can't switch to config mode"); + JLERRORG(m_lastResult.lastResultText()); + return false; +} + +/*! \brief Return the state of this device + \details The device state indiciates whether the device is in config mode, measuring, recording, etc + \returns The state of the device +*/ +XsDeviceState XsDevice::deviceState() const +{ + return m_state; +} + +/*! \cond XS_INTERNAL */ +/*! \brief Set the device state for this device (triggering callbacks) and relay it to all children + \param state The device state to set + \details To ensure proper switching, the switch is done threaded. To override functionality, use updateDeviceState() + \returns The id of the task that marks the state change completion when \a waitForCompletion is false +*/ +void XsDevice::setDeviceState(XsDeviceState state) +{ + // LockSuspendable locky(&m_deviceMutex, LS_Write); + // lock is already acquired by updateDeviceState, + updateDeviceState(state); +} +/*! \endcond */ + +/*! \brief Returns true if this is the master device (not a child of another device) + \returns true if this is the master device +*/ +bool XsDevice::isMasterDevice() const +{ + return this == master(); +} + +/*! \brief Returns true if this device can have child devices + \returns true if this is a container device +*/ +bool XsDevice::isContainerDevice() const +{ + return false; +} + +/*! \brief Returns true if this is a standalone device (not a child of another device and not a container device) + \returns true if this is a standalone device, equivalent to !isContainerDevice() && isMasterDevice() +*/ +bool XsDevice::isStandaloneDevice() const +{ + return isMasterDevice() && !isContainerDevice(); +} + +/*! \brief Set the device option flags + \param setFlags The option flags that must be set. Set to XDOF_None if no flags need to be set + \param clearFlags The option flags that must be cleared. Set to XDOF_None if no flags need to be cleared + \returns true if the device option flags were successfully altered +*/ +bool XsDevice::setDeviceOptionFlags(XsDeviceOptionFlag setFlags, XsDeviceOptionFlag clearFlags) +{ + XsMessage snd(XMID_SetOptionFlags, 8); + snd.setBusId(busId()); + MessageSerializer(snd) << (uint32_t)setFlags << (uint32_t)clearFlags; + + XsMessage rcv; + if (!doTransaction(snd, rcv)) + return false; + + return true; +} + +/*! \brief Set the output configuration for this device + \details When the function exits with a true value \a config will contain the actual configuration in the + device after configuration. When it exits with false the contents of \a config are undefined. + + \note The \a config is updated to reflect frequency mismatches in desired configuration and actually possible + configuration. As input, a frequency of 65535 (0xFFFF) may be supplied to indicate 'maximum output + rate', but after configuration XDA will have put the actual maximum value in \a config. + Similarly, some data types may not have a real update rate (ie. packet counter) and will return + an update rate of 65535 (0xFFFF) when configured at any rate other than 0. + + \param config The desired output configuration + \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()); + bool wasEmpty = config.empty(); + MessageSerializer(snd) << config; + + XsMessage rcv; + if (!doTransaction(snd, rcv)) + { + // 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(); + + 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 + device after configuration. When it exits with false the contents of \a config are undefined. + + \note The \a config is updated to reflect frequency mismatches in desired configuration and actually possible + configuration. As input, a frequency of 65535 (0xFFFF) may be supplied to indicate 'maximum output + rate', but after configuration XDA will have put the actual maximum value in \a config. + Similarly, some data types may not have a real update rate (ie. packet counter) and will return + an update rate of 65535 (0xFFFF) when configured at any rate other than 0. + + \param config The desired output configuration + \returns true if the output configuration was successfully updated +*/ +bool XsDevice::setCanOutputConfiguration(XsCanOutputConfigurationArray& config) +{ + (void)config; + return false; +} + +/*! \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 +*/ +bool XsDevice::setCanConfiguration(uint32_t config) +{ + (void)config; + return false; +} + +/*! \brief Return the full output configuration including post processing outputs + \details This function return the list returned by outputConfiguration() and adds outputs + that become available during post-processing. + \returns The requested output configuration list +*/ +XsOutputConfigurationArray XsDevice::processedOutputConfiguration() const +{ + return outputConfiguration(); +} + +/*! \brief Sets the string output mode for this device + \param type The type to set + \param period The period to set + \param skipFactor The skipFactor to set + \returns True if the device was successfully updated +*/ +bool XsDevice::setStringOutputMode(uint16_t type, uint16_t period, uint16_t skipFactor) +{ + XsMessage sndType(XMID_SetStringOutputType); + sndType.setBusId(XS_BID_MASTER); // Always send to master device + sndType.resizeData(2); + sndType.setDataShort(type); + + if (!doTransaction(sndType)) + return false; + + if (type == 0) + return true; + + XsMessage sndSkip(XMID_SetOutputSkipFactor); + sndSkip.setBusId(busId()); + sndSkip.resizeData(2); + sndSkip.setDataShort(skipFactor); + + if (!doTransaction(sndSkip)) + return false; + + m_config.masterInfo().m_outputSkipFactor = skipFactor; + + XsMessage sndPer(XMID_SetPeriod); + sndPer.setBusId(XS_BID_MASTER); // Always send to master device + sndPer.resizeData(2); + sndPer.setDataShort(period); + + if (!doTransaction(sndPer)) + return false; + + m_config.masterInfo().m_samplingPeriod = period; + + return true; +} + +/*! \brief Ask the device for its supported string output types. + \returns A list with the supported string output types. +*/ +XsStringOutputTypeArray XsDevice::supportedStringOutputTypes() const +{ + return XsStringOutputTypeArray(); +} + +/*! \cond XS_INTERNAL */ +/*! \brief Schedules the orientation reset + \param method The reset method to use + \returns True if successful +*/ +bool XsDevice::scheduleOrientationReset(XsResetMethod method) +{ + if (method == XRM_StoreAlignmentMatrix && deviceState() != XDS_Config) + return false; + + if (method != XRM_StoreAlignmentMatrix && deviceState() != XDS_Measurement && deviceState() != XDS_Recording) + return false; + + XsMessage snd(XMID_ResetOrientation); + snd.setBusId(busId()); + snd.resizeData(2); + snd.setDataShort((uint16_t)method); + + if (!doTransaction(snd)) + return false; + return true; +} +/*! \endcond */ + +/*! \brief Send a custom message \a messageSend to the device and possibly wait for a result. + \details If \a waitForResult is true, the function will wait for a result and put it in the given + \a messageReceive. Otherwise the contents of messageReceive will not be altered. + If an error message is received or the wait times out, \a messageReceive will contain an error + message. + + \param messageSend The message to send to the device + \param waitForResult true if it is required that the function waits for the appropriate reply. A + valid reply always has a message ID that is one higher than the sent message ID. + \param messageReceive When \a waitForResult is true, the reply will be put in this object. + \param timeout Optional timeout in ms. When 0 is supplied (the default), the default timeout is used. + \returns true if the message was successfully sent and when \a waitForResult is true the correct + reply has been received +*/ +bool XsDevice::sendCustomMessage(const XsMessage& messageSend, bool waitForResult, XsMessage& messageReceive, int timeout) +{ + return sendCustomMessage(messageSend, waitForResult, static_cast(messageSend.getMessageId() + 1), messageReceive, timeout); +} + +/*! \brief Send a message directly to the communicator + \param[in] message The message that will be sent + \returns true if the message was successfully sent +*/ +bool XsDevice::sendRawMessage(const XsMessage& message) +{ + Communicator* comm = communicator(); + if (!comm) + return false; + TRANSACTIONLOG(m_deviceId << " SND: " << msgToString(message)); + return comm->writeMessage(message); +} + +/*! \cond XS_INTERNAL */ +/*! \brief Send a custom message \a messageSend to the device and possibly wait for a result. + + \details If \a waitForResult is true, the function will wait for a result and put it in the given + \a messageReceive. Otherwise the contents of messageReceive will not be altered. + If an error message is received or the wait times out, \a messageReceive will contain an error + message. + + \param messageSend The message to send to the device + \param waitForResult true if it is required that the function waits for the appropriate reply. + A valid reply always has a message ID that is one higher than the sent message ID. + \param messageId The message Id to wait for. Use this if the expected messageId is not the standard input messageid + 1 + \param messageReceive When \a waitForResult is true, the reply will be put in this object. + \param timeout Optional timeout in ms. When 0 is supplied (the default), the default timeout is used. + + \returns true if the message was successfully sent and when \a waitForResult is true the correct + reply has been received +*/ +bool XsDevice::sendCustomMessage(const XsMessage& messageSend, bool waitForResult, XsXbusMessageId messageId, XsMessage& messageReceive, int timeout) +{ + Communicator* comm = communicator(); + if (!comm) + return false; + + TRANSACTIONLOG(m_deviceId << " SND: " << msgToString(messageSend)); + std::shared_ptr reply = comm->addReplyObject(messageId); + if (!comm->writeMessage(messageSend)) + return false; + + if (waitForResult) + { + 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; + } + + return true; +} + +/*! \brief Let the communicator wait for for a message + \param messageId The message Id to wait for. + \param messageReceive The reply will be put in this object. + \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) +{ + Communicator* comm = communicator(); + if (!comm) + return false; + + std::shared_ptr reply = comm->addReplyObject(messageId); + + if (!timeout) + timeout = (int)(int32_t) comm->defaultTimeout(); + messageReceive = reply->message((uint32_t)(unsigned int)timeout); + + if (messageReceive.getMessageId() != messageId) + return false; + + return true; +} + +/*! \brief Let the communicator wait for for a message + \param reply The reply object to wait for + \param messageReceive The reply will be put in this object. + \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) +{ + if (!timeout && communicator()) + timeout = (int)(int32_t) communicator()->defaultTimeout(); + messageReceive = reply->message((uint32_t)(unsigned int)timeout); + + if (messageReceive.getMessageId() != reply->msgId()) + return false; + + return true; +} + +/*! \brief Add a reply object to the communicator to wait for a specific message + \param messageId The message Id to wait for. + \param data The data to check for in the first data byte location + \returns A shared pointer to this object +*/ +std::shared_ptr XsDevice::addReplyObject(XsXbusMessageId messageId, uint8_t data) +{ + Communicator* comm = communicator(); + if (!comm) + return std::shared_ptr(); + + return comm->addReplyObject(messageId, 0, 1, &data); +} + +/*! \returns true if the supplied data message should be recorded to file + \param msg The message to use + \details This function should be overridden on a master device so we can check its device-specific messages. + 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 +{ + (void)msg; + return (deviceState() == XDS_Recording); +} + +/*! \brief Handle an XbusMessage +*/ +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); + break; + + case XMID_Warning: + handleWarningMessage(msg); + break; + + case XMID_MasterIndication: + handleMasterIndication(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) +{ + onNonDataMessage(this, &msg); +} + +/*! \brief Process an error message +*/ +void XsDevice::handleErrorMessage(const XsMessage& msg) +{ + uint8_t errorCode = msg.getDataByte(0); + XsResultValue xsResultValue = static_cast(errorCode); + onNonDataMessage(this, &msg); + onError(this, xsResultValue); +} + +/*! \brief Process a warning message +*/ +void XsDevice::handleWarningMessage(const XsMessage& msg) +{ + onNonDataMessage(this, &msg); +} + +/*! \brief Process a wakeup message +*/ +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 + \param pid The packet ID to instert + \param pack The data packet to insert +*/ +void XsDevice::insertIntoDataCache(int64_t pid, XsDataPacket* pack) +{ + LockGuarded lockG(&m_deviceMutex); + + if (m_dataCache.empty()) + m_dataCache.insert(std::make_pair(pid, pack)); + else + { + auto it = m_dataCache.lower_bound(pid); + if (it == m_dataCache.end()) + m_dataCache.insert(it, std::make_pair(pid, pack)); + else + { + if (it->first != pid) + m_dataCache.insert(it, std::make_pair(pid, pack)); + else + { + it->second->merge(*pack, true); + delete pack; + } + } + } +} + +/*! \brief Clears the data cache +*/ +void XsDevice::clearDataCache() +{ + LockGuarded lockG(&m_deviceMutex); + + for (auto& it : m_dataCache) + delete it.second; + m_dataCache.clear(); + //m_latestLivePacket->clear(); + m_latestBufferedPacket->clear(); + m_unavailableDataBoundary = -1; +} + +/*! \brief Handles the inability to obtain specific measurement data + \details Should be called when it is not possible to retransmit specific data. The data should be considered lost + \param frameNumber The frame number of the data that is lost +*/ +void XsDevice::handleUnavailableData(int64_t frameNumber) +{ + LockGuarded lockG(&m_deviceMutex); + + if (frameNumber < latestBufferedPacketId()) + return; // The recording stream already advanced itself beyond this frame. We don't need it + if (latestLivePacketId() == -1) + return; + if (m_stopRecordingPacketId >= 0 && frameNumber > m_stopRecordingPacketId) + return; // we're missing data past the end of the recording, ignore + + //JLDEBUGG("Device " << m_deviceId << " Updating m_unavailableDataBoundary from " << m_unavailableDataBoundary << " to " << (std::max)(m_unavailableDataBoundary, frameNumber)); + m_unavailableDataBoundary = (std::max)(m_unavailableDataBoundary, frameNumber); // Note: (std::max) to prevent a macro for max() to be substituted + checkDataCache(); +} + +/*! \brief Returns true if it should do a recorded callback for a given \a data packet. + \param p The reference to a data packet. + \returns True if successful +*/ +bool XsDevice::shouldDoRecordedCallback(XsDataPacket const& p) const +{ + if (p.empty()) + return false; + + if (isReadingFromFile()) + return true; + + switch (deviceState()) + { + case XDS_Recording: + case XDS_FlushingData: + break; + + default: + return false; + } + + if (isStandaloneDevice()) + return true; + + if (m_stopRecordingPacketId >= 0 && p.packetId() > m_stopRecordingPacketId) + return false; + + if (!p.containsFrameRange()) + return p.packetId() >= m_startRecordingPacketId; + + int64_t startPid = p.packetId() - p.frameRange().interval(); + return startPid >= m_startRecordingPacketId; +} + +/*! \brief Handle an XbusDataPacket + \param packet The data packet to handle +*/ +void XsDevice::handleDataPacket(const XsDataPacket& packet) +{ + LockGuarded locky(&m_deviceMutex); + if (m_terminationPrepared) + return; // we're being destroyed, abort handling of datapacket, state may be invalid + + m_lastDataOkStamp = XsTimeStamp::now(); + int64_t fastest = latestLivePacketConst().packetId(); + int64_t slowest = latestBufferedPacketConst().packetId(); + + std::unique_ptr pack(new XsDataPacket(packet)); + master()->m_packetStamper.stampPacket(*pack, latestLivePacket()); // always go through master for stamping packets so we have consistent timing + int64_t current = pack->packetId(); + +#if TOADUMP + fprintf(master()->m_toaDumpFile, "%llu,%llu,%llu\n", current, pack->timeOfArrival().msTime(), pack->estimatedTimeOfSampling().msTime()); +#endif + +#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()); + JLDEBUGG("stamped: " << current << " new latestlive: " << latestLivePacketConst().packetId()); +#endif + + bool interpolate = false; + if (fastest >= 0) + { + int64_t dpc = current - fastest; + + if (dpc > 1) + { + 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); + + for (int64_t i = firstMissed; i <= lastMissed; ++i) + { + if (!expectingRetransmissionForPacket(i)) + { + //JLDEBUGG("not expecting retransmission for packet " << i); + handleUnavailableData(i); + if (m_options & XSO_InterpolateMissingData) + interpolate = true; + } + } + } + } + else if (isReadingFromFile() && getStartRecordingPacketId() == -1) + setStartRecordingPacketId(current); + + if (current >= fastest) + { + JLTRACEG("Processing (live) packet " << current); + std::unique_ptr copy(new XsDataPacket(*pack)); + processLivePacket(*copy); + + if (interpolate) + { + // 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; + })) + { + return; + } + } + + // store result + latestLivePacket().swap(*copy); + + // do callbacks + if (!latestLivePacketConst().empty()) + { + if (m_options & XSO_KeepLastLiveData) + updateLastAvailableLiveDataCache(latestLivePacketConst()); + if (m_options & XSO_RetainLiveData) + retainPacket(latestLivePacketConst()); + + //if (isRecording()) + // JLDEBUGG("Device " << m_deviceId << " triggering onLiveDataAvailable " << latestLivePacketConst().packetId()); + onLiveDataAvailable(this, &latestLivePacketConst()); + if (!isReadingFromFile()) + onDataAvailable(this, &latestLivePacketConst()); + if (isStandaloneDevice()) + { + XsDevicePtrArray devs; + const XsDataPacket* ppack = &latestLivePacketConst(); + devs.push_back(this); + XsDataPacketPtrArray packs; + packs.push_back(const_cast(ppack)); + onAllLiveDataAvailable(&devs, &packs); + if (!isReadingFromFile()) + onAllDataAvailable(&devs, &packs); + } + } + } + else + { + // probably a retransmission + //JLDEBUGG("Device " << deviceId() << " ignoring (live) packet " << current << " because it is older than " << fastest); + } + + if (latestBufferedPacketConst().empty() || current > slowest) + { + // insert into cache + //JLDEBUGG("Device " << deviceId() << " Adding (buffered) packet " << current); + insertIntoDataCache(current, pack.get()); + pack.release(); + checkDataCache(); + } + else + { + // not correct, weird old retransmission + ONLYFIRSTMTX2 + JLDEBUGG("Device " << deviceId() << " ignoring (buffered) packet " << current << " because it is not newer than " << slowest); + } +} +/*! \endcond */ + +/*! \brief Returns true if the device is currently in a measuring state + \returns true if the device is currently in a measuring state +*/ +bool XsDevice::isMeasuring() const +{ + switch (deviceState()) + { + 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; + } +} + +/*! \brief Returns true if the device is currently in a recording state + \returns true if the device is currently in a recording state +*/ +bool XsDevice::isRecording() const +{ + 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; + } +} + +/*! \brief Returns true if the device is reading from a file + \returns true if the device is reading from a file +*/ +bool XsDevice::isReadingFromFile() const +{ + if (!communicator()) + return false; + assert(communicator()); + return communicator()->isReadingFromFile(); +} + +/*! \cond XS_INTERNAL */ +/*! \brief Process a XsDataPacket (calibrate and filter) and put the results in the appropriate cache +*/ +void XsDevice::processLivePacket(XsDataPacket&) +{ +} + +/*! \brief Process a XsDataPacket (calibrate and filter) and put the results in the appropriate cache +*/ +void XsDevice::processBufferedPacket(XsDataPacket&) +{ +} +/*! \endcond */ + +/*! \brief Clear the inbound data buffers of the device +*/ +void XsDevice::flushInputBuffers() +{ + JLDEBUGG(this); + if (isMasterDevice() && communicator()) + communicator()->flushPort(); + clearDataCache(); + resetPacketStamping(); +} + +/*! \brief Returns the synchronization role of the device + \details For synchronization purposes a device can be a master, a slave, both or neither. This + function returns the way the device is currently configured. + \returns The synchronization role of the device +*/ +XsSyncRole XsDevice::syncRole() const +{ + return XSR_None; +} + +/*! \cond XS_INTERNAL */ +/*! \brief Reads the device configuration + \returns True if successful +*/ +bool XsDevice::readDeviceConfiguration() +{ + JLDEBUGG(""); + if (isMasterDevice()) + { + Communicator* comm = communicator(); + if (!comm) + return false; + + if (comm->isReadingFromFile()) + { + XsMessage rcv; + + rcv = comm->readMessageFromStartOfFile(XMID_Configuration, Communicator::configurationMessageSearchLimit()); + if (comm->lastResult() != XRV_OK) + return false; + m_config.readFromMessage(rcv); + + 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) + auto const& tmp = m_config.deviceInfo((XsSize)(ptrdiff_t)busId()); + m_firmwareVersion = XsVersion(tmp.m_fwRevMajor, tmp.m_fwRevMinor, tmp.m_fwRevRevision); + } + else + extractFirmwareVersion(rcv); + } + else + { + if (!gotoConfig()) + { + JLALERTG("Failed to go to config"); + return false; + } + + XsMessage snd(XMID_Initbus); + snd.setBusId(busId()); + + if (!doTransaction(snd, 500)) + { + JLALERTG("Failed to init bus"); + return false; + } + + snd.setMessageId(XMID_ReqConfiguration); + snd.setBusId(busId()); + + XsMessage rcv; + if (!doTransaction(snd, rcv, 5000)) + { + JLALERTG("Failed to req configuration"); + return false; + } + + m_config.readFromMessage(rcv); + m_firmwareVersion = XsVersion(); + snd.setMessageId(XMID_ReqFirmwareRevision); + snd.setBusId(busId()); + + if (doTransaction(snd, rcv) && rcv.getMessageId() == XMID_FirmwareRevision) + extractFirmwareVersion(rcv); + + m_outputConfiguration = outputConfiguration(); + } + return true; + } + else + { + try + { + const XsMtDeviceConfiguration& devinf = deviceConfigurationConst().deviceInfo(deviceId()); + XsVersion old = m_firmwareVersion; + m_firmwareVersion = XsVersion(devinf.m_fwRevMajor, devinf.m_fwRevMinor, devinf.m_fwRevRevision); +#ifndef XSENS_DEBUG + // this is support debug info, we don't want to spam the debugger windows + if (old != m_firmwareVersion) + JLWRITEG("Device " << deviceId() << " has firmware version " << m_firmwareVersion.toString()); +#endif + return true; + } + catch (XsDeviceConfigurationException&) + { + return false; + } + } + return false; +} + +/*! \brief Returns a const reference to the device configuration + \details The device configuration contains a summary of the devices connected to the same port. + The function will always return the configuration for the port's main device. + \returns A copy of the device configuration of the port +*/ +XsDeviceConfiguration const& XsDevice::deviceConfigurationConst() const +{ + return m_config; +} + +/*! \brief Returns a reference to the device configuration + \details The device configuration contains a summary of the devices connected to the same port. + The function will always return the configuration for the port's main device. + \returns A reference to the device configuration of the port +*/ +XsDeviceConfiguration& XsDevice::deviceConfigurationRef() +{ + return const_cast(deviceConfigurationConst()); +} +/*! \endcond */ + +/*! \brief Returns the device configuration + \details The device configuration contains a summary of the devices connected to the same port. + The function will always return the configuration for the port's main device. + \returns A copy of the device configuration of the port +*/ +XsDeviceConfiguration XsDevice::deviceConfiguration() const +{ + return deviceConfigurationConst(); +} + +/*! \brief Restore the device to its factory default settings + \returns true if the settings have been successfully restored +*/ +bool XsDevice::restoreFactoryDefaults() +{ + if (deviceState() == XDS_Measurement || deviceState() == XDS_Recording) + return false; + + XsMessage snd(XMID_RestoreFactoryDef); + snd.setBusId(busId()); + + if (!doTransaction(snd, 2000)) + return false; + + return true; +} + +/*! \brief Reset the device + \details This function tells the device to reboot itself. + \returns true if the device was successfully reset +*/ +bool XsDevice::reset() +{ + return reset(false); +} + +/*! \cond XS_INTERNAL */ +/*! \brief Reset the device + \details This function tells the device to reboot itself. + \param skipDeviceIdCheck Set to true if the rescan should not verify the device id (not recommended) + \returns true if the device was successfully reset +*/ +bool XsDevice::reset(bool skipDeviceIdCheck) +{ + if (!isMasterDevice()) + return false; + + Communicator* comm = communicator(); + if (!comm) + return false; + + if (!gotoConfig()) + return false; + + 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); + + if (!reopenPort(false, skipDeviceIdCheck)) + return false; + + if (!gotoConfig()) + return false; + + return true; +} +/*! \endcond */ + +/*! \brief Reopens a port + Uses rescan method to redetect a device. Also if USB descriptor has changed + \param gotoConfig Set to true if the device should be put to config before port closure + \param skipDeviceIdCheck Set to true if the rescan should not verify the device id (not recommended) + \returns true if successful +*/ +bool XsDevice::reopenPort(bool gotoConfig, bool skipDeviceIdCheck) +{ + setGotoConfigOnClose(gotoConfig); + + Communicator* comm = communicator(); + if (!comm) + return false; + + return comm->reopenPort(OPS_OpenPort, skipDeviceIdCheck); +} + +/*! \brief Write the emts/wms/xms of the device and all its children to the open logfile +*/ +void XsDevice::writeDeviceSettingsToFile() +{ +} + +/*! \brief Create a log file for logging + \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) +{ + JLDEBUGG(filename); + Communicator* comm = communicator(); + if (!comm || !comm->isPortOpen()) + { + JLALERTG("No port open"); + return XRV_NOPORTOPEN; + } + + std::unique_ptr myLock(new xsens::Lock(&m_logFileMutex, true)); + if (logFileInterface(myLock)) + { + JLERRORG("A file is already open"); + return XRV_ALREADYOPEN; + } + + std::unique_ptr newfile(new MtbDataLogger); + if (newfile->create(filename)) + { + m_logFileInterface = newfile.release(); + JLDEBUGG("Creation ok"); + tm dateTime; + getDateTime(&dateTime); + getDateAsString((char*) m_config.masterInfo().m_date, &dateTime); + getTimeAsString((char*) m_config.masterInfo().m_time, &dateTime); + + XsMessage msg; + deviceConfiguration().writeToMessage(msg); + m_logFileInterface->writeMessage(msg); + writeDeviceSettingsToFile(); + return XRV_OK; + } + JLDEBUGG("Creation failed"); + newfile->close(true); + removeChainedManager(m_logFileInterface); + newfile.reset(); + return XRV_OUTPUTCANNOTBEOPENED; +} + +/*! \brief Stores the current device configuration in a config file(.xsa) + \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&) +{ + return XRV_NOTIMPLEMENTED; +} + +/*! \brief Set an arbitrary alignment rotation quaternion. + Use to rotate either L to the chosen frame L' or S to the chosen frame S' + \param frame The frame to rotate + \param quat The desired alignment rotation setting of the device. + \returns true if the alignment rotation has been set successfully + \see alignmentRotationQuaternion, setAlignmentRotationMatrix, alignmentRotationMatrix +*/ +bool XsDevice::setAlignmentRotationQuaternion(XsAlignmentFrame frame, const XsQuaternion& quat) +{ + (void)frame; + (void)quat; + return false; +} + +/*! \brief Retrieve the alignment rotation quaternion + \param frame The frame of which to return the alignment rotation + \returns The alignment rotation + \see setAlignmentRotationQuaternion, setAlignmentRotationMatrix, alignmentRotationMatrix +*/ +XsQuaternion XsDevice::alignmentRotationQuaternion(XsAlignmentFrame frame) const +{ + (void)frame; + return XsQuaternion(); +} + +/*! \brief Set an arbitrary alignment rotation matrix + Use to rotate either L to the chosen frame L' or S to the chosen frame S' + \param frame The frame to rotate + \param matrix The desired alignment rotation setting of the device. This should be an + orthonormal 3x3 matrix. + \returns true if the alignment rotation has been set successfully + \see setAlignmentRotationQuaternion, alignmentRotationQuaternion, alignmentRotationMatrix +*/ +bool XsDevice::setAlignmentRotationMatrix(XsAlignmentFrame frame, const XsMatrix& matrix) +{ + (void)frame; + (void)matrix; + return false; +} + +/*! \brief Retrieve the alignment rotation matrix to rotate S to the chosen frame S' + \param frame The frame of which to return the alignment rotation + \returns The alignment rotation + \see setAlignmentRotationQuaternion, alignmentRotationQuaternion, setAlignmentRotationMatrix +*/ +XsMatrix XsDevice::alignmentRotationMatrix(XsAlignmentFrame frame) const +{ + (void)frame; + return XsMatrix(); +} + +/*! \brief Loads a config file(.xsa) and configures the device accordingly + \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&) +{ + return XRV_NOTIMPLEMENTED; +} + +/*! \brief Close the log file + \returns true if the log file was successfully closed or never open +*/ +bool XsDevice::closeLogFile() +{ + xsens::Lock myLock(&m_logFileMutex, true); + if (m_logFileInterface) + { + JLDEBUGG(m_logFileInterface); + m_logFileInterface->close(); + removeChainedManager(m_logFileInterface); + delete m_logFileInterface; + m_logFileInterface = NULL; + } + return true; +} + +/*! \brief Start recording incoming data + \details To record successfully, a log file should be created by calling createLogFile() before + this function is called. startRecording(XsString, XsDeviceId) can be used to achieve the same result. + \returns true if recording was successfully started + \note Starting recording for a single non-main device will start a recording for the entire system. + \sa createLogFile() \sa stopRecording() +*/ +bool XsDevice::startRecording() +{ + JLDEBUGG(""); + if (!isMasterDevice()) + return false; + + if (deviceState() != XDS_Measurement) + return false; + + writeFilterStateToFile(); + setDeviceState(XDS_Recording); + + return true; +} + +/*! \brief Start recording incoming data through generating a virtual input trigger + \note On devices without support for a start recording input trigger this function will default to XsDevice::startRecording + \returns true if recording was successfully started + \sa createLogFile() \sa stopRecording() \sa startRecording +*/ +bool XsDevice::triggerStartRecording() +{ + return startRecording(); +} + +/*! \brief Stop recording incoming data + \returns true if recording was successfully stopped + \note Stopping recording for a single non-main device will stop a recording for the entire system. + \sa createLogFile() \sa startRecording() +*/ +bool XsDevice::stopRecording() +{ + JLDEBUGG(""); + if (!isMasterDevice()) + return false; + + if (deviceState() != XDS_Recording) + return false; + + setDeviceState(XDS_Measurement); + return true; +} + +/*! \cond XS_INTERNAL */ +/*! \brief Returns whether the packet contains a retransmission or not */ +bool XsDevice::packetContainsRetransmission(XsDataPacket const& packet) +{ + if (packet.containsAwindaSnapshot()) + return packet.isAwindaSnapshotARetransmission(); + + return false; +} + +/*! \brief Handles the end of the recording stream +*/ +void XsDevice::endRecordingStream() +{ + LockGuarded lockG(&m_deviceMutex); + + if (m_dataCache.empty() || (m_startRecordingPacketId < 0)) + return; + + if (m_stopRecordingPacketId >= 0) + { + //The first packetId following the latestBufferedPacket (head of recording stream) is what is blocking possible cached data + //If the recording stream is empty the beginning of the recording is unavailable (from start recording until the first packet in the cache) + int64_t blockingPacketId = (latestBufferedPacketId() < 0) ? m_startRecordingPacketId : latestBufferedPacketId() + 1; + + while (blockingPacketId <= m_stopRecordingPacketId) + { + //Mark blocking packet as unavailable. + handleUnavailableData(blockingPacketId); + //If marking the blocking packet unavailable did not advance the head of the recording stream the next blocking packet follows the current one immediately + blockingPacketId = (std::max)(blockingPacketId + 1, latestBufferedPacketId() + 1); // Note: (std::max) to prevent a macro for max() to be substituted + } + } + + checkDataCache(); + // clearDataCache(); +} + +/*! \brief Check if cache contains data to process */ +void XsDevice::checkDataCache() +{ + LockGuarded lockG(&m_deviceMutex); + // process available data + while (!m_dataCache.empty()) + { + auto it = m_dataCache.begin(); + //JLWRITEG("pid: " << it->second->packetId() << " range? " << it->second->containsFrameRange() << " retransmission? " << it->second->isAwindaSnapshotARetransmission() << " snapshotA,F? " << it->second->containsAwindaSnapshot() << "," << it->second->containsFullSnapshot()); + + int64_t expectedPacketId = latestBufferedPacketId() < 0 ? -1 : latestBufferedPacketId() + 1; + if (expectedPacketId < m_startRecordingPacketId) + { + //If there is a frame range (i.e. we get an interval) the packetId is equal to the end of that interval + //The startRecordingPacketId always is equal to the start value of an interval. Therefore if using the startRecordingPacketId to + //calculate the expected packetId for an ideal interval (no missing data) is to +1 the startRecordingPacketId + //For an ideal (expected) situation this would be 1 higher than the startRecordingPacketId + expectedPacketId = it->second->containsFrameRange() ? m_startRecordingPacketId + 1 : getStartRecordingPacketId(); + expectedPacketId = PacketStamper::calculateLargePacketCounter(expectedPacketId, latestLivePacketId(), PacketStamper::MTSCBOUNDARY); + } + int64_t packetId = it->first; + + auto missingDataIsUnavailable = [this](int64_t rFirst, int64_t rLast) + { + if (m_options & XSO_ExpectNoRetransmissionsInFile) + return true; + if ((m_stopRecordingPacketId >= 0 && rFirst > m_stopRecordingPacketId) || + (rLast - 1 <= m_unavailableDataBoundary)) + { + // notify unavailable data in recording states + // when not recording, unavailable data is expected and should not be reported + if (isRecording()) + { + //JLDEBUGG("Device " << m_deviceId << " data unavailable in range " << rFirst << " - " << rLast); + if (m_stopRecordingPacketId != -1 && rLast > m_stopRecordingPacketId) + rLast = m_stopRecordingPacketId; + if (rFirst < rLast) + { + ONLYFIRSTMTX2 + JLDEBUGG("Device " << m_deviceId << " Data is unavailable in reported range " << rFirst << " - " << rLast << " Stop Recording Packet ID " << m_stopRecordingPacketId << " Unavailable Data Boundary " << m_unavailableDataBoundary); + for (int64_t r = rFirst; r < rLast; ++r) + onDataUnavailable(this, r); + } + else + { + //ONLYFIRSTMTX2 + //JLDEBUGG("Device " << m_deviceId << " NOT Reporting data unavailable"); + } + } + + return true; + } + return false; + }; + + int64_t rFirst = expectedPacketId >= 0 ? expectedPacketId : packetId; + int64_t rLast = packetId; + if (it->second->containsFrameRange()) + { + XsRange rng = it->second->frameRange(); + rFirst = rng.first() + 1; + } + + if ((expectedPacketId >= 0) && (packetId > expectedPacketId)) + { + if (!missingDataIsUnavailable(rFirst, rLast)) + { + //JLDEBUGG("Device " << m_deviceId << " Waiting for retransmissions in range " << rFirst << " - " << rLast); + break; // waiting for retransmissions + } + } + // we need to 'else' here to avoid duplicate and erroneous missed packet handling + else if (it->second->containsFrameRange()) + { + if (rLast > rFirst) + { + if (!missingDataIsUnavailable(rFirst, rLast)) + break; // waiting for retransmissions + } + } + + // do 'buffered' processing + processBufferedPacket(*it->second); + + // store result + latestBufferedPacket().swap(*it->second); + // ONLYFIRSTMTX2 + // JLDEBUGG("latestBufferedPacket is now " << latestBufferedPacket().packetId() << " old: " << it->second->packetId()); + delete it->second; + m_dataCache.erase(it); + + if (latestBufferedPacketConst().empty()) + continue; + + if (m_options & XSO_RetainBufferedData) + retainPacket(latestBufferedPacketConst()); + + // do callbacks + const XsDataPacket& buf = latestBufferedPacketConst(); + //if (isRecording()) + // JLDEBUGG("Device " << m_deviceId << " triggering onBufferedDataAvailable " << buf.packetId()); + onBufferedDataAvailable(this, &buf); + bool doRecCallback = shouldDoRecordedCallback(buf); + if (isReadingFromFile()) + onDataAvailable(this, &buf); + if (doRecCallback) + onRecordedDataAvailable(this, &buf); + + if (isStandaloneDevice()) + { + XsDevicePtrArray devs; + const XsDataPacket* ppack = &latestBufferedPacketConst(); + devs.push_back(this); + XsDataPacketPtrArray packs; + packs.push_back((XsDataPacket*)ppack); + onAllBufferedDataAvailable(&devs, &packs); + if (isReadingFromFile()) + onAllDataAvailable(&devs, &packs); + if (doRecCallback) + onAllRecordedDataAvailable(&devs, &packs); + } + } +} + +/*! \brief Store a message in the log file if a log file is open */ +void XsDevice::writeMessageToLogFile(const XsMessage& message) +{ + if (communicator()->isReadingFromFile()) + return; + + if (!onWriteMessageToLogFile(this, &message)) + return; + + std::unique_ptr myLock; + DataLogger* logInt = logFileInterface(myLock); + if (!logInt) + return; + + logInt->writeMessage(message); +} + +/*! \brief Write filter states to file */ +void XsDevice::writeFilterStateToFile() +{ +} +/*! \endcond */ + +/*! \brief Ask the device for its supported update rates for the given \a dataType + \param dataType The type of data to get the supported update rates for + \returns A list with the supported update rates or an empty list in case of an error +*/ +std::vector XsDevice::supportedUpdateRates(XsDataIdentifier dataType) const +{ + (void)dataType; + return std::vector(); +} + +/*! \brief Returns true if the device has its BlueTooth radio enabled + \returns true if the device has its BlueTooth radio enabled + \sa setBlueToothEnabled +*/ +bool XsDevice::isBlueToothEnabled() const +{ + return false; +} +/*! \brief Enable or disable the BlueTooth radio of the device + \param enabled Set to true to enable the BlueTooth radio + \returns true if the device was successfully updated + \sa isBlueToothEnabled +*/ +bool XsDevice::setBlueToothEnabled(bool enabled) +{ + (void)enabled; + return false; +} + +/*! \brief Returns if the Xbus is powering its child devices or not + \details When the bus power is off, the child devices are disabled + \returns true If the Xbus is currently providing power to its child devices +*/ +bool XsDevice::isBusPowerEnabled() const +{ + return false; +} +/*! \brief Tell the Xbus to provide power to its child devices or not + \details This function can be used to tell the Xbus to stop and start powering its child + devices. By default when the Xbus starts up it will provide power to its child devices. + Switching the power off can save a lot of energy, but powering the system up again will take some + time, depending on the number of connected devices. + \param enabled true to enable bus power, false to disable the bus power + \returns true If the Xbus' bus power state was successfully updated. +*/ +bool XsDevice::setBusPowerEnabled(bool enabled) +{ + (void)enabled; + return false; +} +/*! \brief Tell the device to power down completely + \details This function can be used to tell the device to shut down completely, requiring a physical + button press on the device to power up again. + \returns true if the device was successfully powered down +*/ +bool XsDevice::powerDown() +{ + return false; +} +/*! \brief Returns the error mode of the device + \details The error mode tells the device what to do if a problem occurs. + \returns The currently configured error mode of the device + \sa setErrorMode +*/ +XsErrorMode XsDevice::errorMode() const +{ + return XEM_Invalid; +} +/*! \brief Sets the error mode of the device + \details The error mode tells the device what to do if a problem occurs. + \param errormode The desired error mode of the device + \returns true if the device was successfully updated + \sa errorMode +*/ +bool XsDevice::setErrorMode(XsErrorMode errormode) +{ + (void)errormode; + return false; +} + +// MT Device functions + +/*! \brief Set the 'heading offset' setting of the device + \param offset The desired heading offset of the device in degrees + \returns true if the device was successfully updated + \sa headingOffset +*/ +bool XsDevice::setHeadingOffset(double offset) +{ + (void)offset; + return false; +} + +/*! \brief Return the 'heading offset' setting of the device + \returns The currently configured heading offset in degrees +*/ +double XsDevice::headingOffset() const +{ + return 0; +} + +/*! \brief Set the location ID of the device + \details The location ID is a custom 16-bit ID that can be assigned to a device. + \param id The desired location ID for the device + \returns true if the Location ID was successfully updated + \sa locationId +*/ +bool XsDevice::setLocationId(int id) +{ + (void)id; + return false; +} + +/*! \brief Get the location ID of the device + \details The location ID is a custom 16-bit ID that can be assigned to a device. + \returns The current location ID stord in the device + \sa setLocationId +*/ +int XsDevice::locationId() const +{ + return 0; +} + +/*! \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. +*/ +XsDevice* XsDevice::getDeviceFromLocationId(uint16_t locId) +{ + if (locationId() == locId) + return this; + + return nullptr; +} + +/*! \brief Returns the object alignment matrix of the device + \returns The current 'object alignment matrix' setting of the device. + \note This is legacy functionality to support backwards compatibility with older devices. For + MT Mk4 devices it is suggested to use alignmentRotationQuaternion or alignmentRotationMatrix instead. + \sa setObjectAlignmentMatrix() \sa headingOffset() \sa setHeadingOffset() \sa alignmentRotationQuaternion \sa alignmentRotationMatrix +*/ +XsMatrix XsDevice::objectAlignment() const +{ + return XsMatrix(); +} + +/*! \brief Sets the object alignment of the device to the given \a matrix. + \param matrix The desired 'object alignment matrix' setting of the device. This should be an + orthonormal 3x3 matrix. + \returns true if the object alignment matrix was successfully written + \note This is legacy functionality to support backwards compatibility with older devices. For + 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) +{ + (void)matrix; + return false; +} + +/*! \brief Returns the 'Gravity Magnitude' of the device + \details The Gravity Magnitude is the strength of the gravity where the measurements are done. + Setting this value precisely allows for more accurate measurements. + \returns The current 'Gravity Magnitude' setting of the device. + \sa setGravityMagnitude \sa setInitialPositionLLA \sa initialPositionLLA +*/ +double XsDevice::gravityMagnitude() const +{ + return 0; +} + +/*! \brief Sets the 'Gravity Magnitude' of the device to the given value \a mag. + \details The Gravity Magnitude is the strength of the gravity where the measurements are done. + Setting this value precisely allows for more accurate measurements. + \param mag The desired 'Gravity Magnitude' setting of the device. + \returns true if the Gravity Magnitude was successfully written + \note The default value is usually computed from the last known Lat Lon Alt value + \sa gravityMagnitude \sa setInitialPositionLLA \sa initialPositionLLA +*/ +bool XsDevice::setGravityMagnitude(double mag) +{ + (void)mag; + return false; +} + +/*! \brief Reinitialize the XsDevice + \details This function will read all configuration details freshly from the device and will + reinitialize all filters. Especially when you have made changes to the device configuration outside + XDA or through sendCustomMessage() it is advisable to call this function so XDA will show the + correct state of the device. + \returns true if the device was successfully reinitialized + \note The device itself is not reset, but will be put in config mode while the settings are being + updated. +*/ +bool XsDevice::reinitialize() +{ + return false; +} + +/*! \brief Gets the filter profile in use for computing orientations on the host PC + \returns The filter profile in use when computing orientations is done on the PC + \sa setXdaFilterProfile \sa onboardFilterProfile +*/ +XsFilterProfile XsDevice::xdaFilterProfile() const +{ + return XsFilterProfile(); +} + +/*! \brief Sets the filter profile to use for computing orientations on the host PC + \details When computing orientation data, there is a choice of filter profiles. This function can + be used to select the appropriate one. By default XDA will attempt to match the software filter profile + to the configured hardware filter profile when detecting a new device. + \param profileType The filter profile type to use. This can be chosen from the list returned by + availableXdaFilterProfiles() + \returns true if the filter profile was successfully changed + \note When reading from a file, make sure to call resetLogFileReadPosition() and possibly + loadLogFile() after changing the filter profile to make sure all cached data is recomputed. + \sa availableXdaFilterProfiles \sa xdaFilterProfile \sa setOnboardFilterProfile +*/ +bool XsDevice::setXdaFilterProfile(int profileType) +{ + (void)profileType; + return false; +} + +/*! \brief Sets the filter profile to use for computing orientations on the host PC + \details When computing orientation data, there is a choice of filter profiles. This function can + be used to select the appropriate one. By default XDA will attempt to match the software filter profile + to the configured hardware filter profile when detecting a new device. + \param profileType The filter profile type to use. This can be chosen from the list returned by + availableXdaFilterProfiles() + \returns true if the filter profile was successfully changed + \note When reading from a file, make sure to call resetLogFileReadPosition() and possibly + loadLogFile() after changing the filter profile to make sure all cached data is recomputed. + \sa availableXdaFilterProfiles \sa xdaFilterProfile \sa setOnboardFilterProfile +*/ +bool XsDevice::setXdaFilterProfile(XsString const& profileType) +{ + (void)profileType; + return false; +} + +/*! \brief Gets the filter profile in use by the device for computing orientations + \returns The filter profile in use when computing orientations is done on the device + \sa setOnboardFilterProfile \sa xdaFilterProfile +*/ +XsFilterProfile XsDevice::onboardFilterProfile() const +{ + return XsFilterProfile(); +} + +/*! \brief Sets the filter profile to use for computing orientations on the device + \details When computing orientation data, there is a choice of filter profiles. This function can + be used to select the appropriate one. + \param profileType The filter profile type to use. This can be chosen from the list returned by + availableOnboardFilterProfiles() + \returns true if the filter profile was successfully changed + \sa availableOnboardFilterProfiles \sa onboardFilterProfile \sa setXdaFilterProfile +*/ +bool XsDevice::setOnboardFilterProfile(int profileType) +{ + (void)profileType; + return false; +} + +/*! \brief Sets the filter profile to use for computing orientations on the device + \details When computing orientation data, there is a choice of filter profiles. This function can + be used to select the appropriate one. + \param profileType The filter profile type to use. This can be chosen from the list returned by + availableOnboardFilterProfiles() + \returns true if the filter profile was successfully changed + \sa availableOnboardFilterProfiles \sa onboardFilterProfile \sa setXdaFilterProfile +*/ +bool XsDevice::setOnboardFilterProfile(XsString const& profileType) +{ + (void)profileType; + return false; +} + +/*! \brief Replaces profileCurrent by profileNew in the device + \param profileCurrent The profile that should be replaced + \param profileNew The new profile + \return true if successful, false otherwise + \note The default implementation does nothing as this feature requires the full XDA +*/ +bool XsDevice::replaceFilterProfile(XsFilterProfile const& profileCurrent, XsFilterProfile const& profileNew) +{ + (void)profileCurrent; + (void)profileNew; + return false; +} + +/*! \brief Return the list of filter profiles available on the device + \returns The list of filter profiles available for computing orientations on the device + \sa availableXdaFilterProfiles \sa onboardFilterProfile \sa setOnboardFilterProfile +*/ +XsFilterProfileArray XsDevice::availableOnboardFilterProfiles() const +{ + return XsFilterProfileArray(); +} + +/*! \brief Return the list of filter profiles available on the host PC + \returns The list of filter profiles available for computing orientations on the host PC + \sa availableOnboardFilterProfiles \sa xdaFilterProfile \sa setXdaFilterProfile +*/ +XsFilterProfileArray XsDevice::availableXdaFilterProfiles() const +{ + return XsFilterProfileArray(); +} + +/*! \brief Returns the maximum official value of the accelerometers in the device + \details The actual official range is -accelerometerRange() .. accelerometerRange(). The device may + send out higher values than this for extreme movements, but then the data quality can not be + guaranteed. + \returns The maximum value of the accelerometers in m/s^2 +*/ +double XsDevice::accelerometerRange() const +{ + return 0; +} + +/*! \brief Returns the maximum official value of the gyroscopes in the device + \details The actual official range is -gyroscopeRange() .. gyroscopeRange(). The device may + send out higher values than this for extreme movements, but then the data quality can not be + guaranteed. + \returns The maximum value of the gyroscopes in degrees/s +*/ +double XsDevice::gyroscopeRange() const +{ + return 0; +} + +/*! \brief Set the no rotation period to \a duration + \details This function can be called in both config and measurement modes. In config mode it + specifies the duration that the device is considered to be stationary as soon as it enters + measurement mode. In measurement mode, it specifies the duration that the device is considered to + be stationary, starting immediately. + + During the stationary period, the gyroscope biases are measured, giving better performance. + \param duration The desired stationary duration in seconds + \returns true if the no rotation command was accepted by the device + \sa isInitialBiasUpdateEnabled \sa setInitialBiasUpdateEnabled +*/ +bool XsDevice::setNoRotation(uint16_t duration) +{ + (void)duration; + return false; +} + +/*! \brief Let the user indicate that he is starting the representative motion + for the In-Run Compass Calibration + \returns true if the start indication was successfully sent to the device +*/ +bool XsDevice::startRepresentativeMotion() +{ + return false; +} + +/*! \brief Retrieves the active representative motion state + for the In-Run Compass Calibration + \returns true if the reprensentation motion state is active, false otherwise +*/ +bool XsDevice::representativeMotionState() +{ + return false; +} + +/*! \brief Let the user indicate that he stopped the representative motion + + for the In-Run Compass Calibration + \returns A struct containing the results of the last In-Run Compass Calibration +*/ +XsIccRepMotionResult XsDevice::stopRepresentativeMotion() +{ + return XsIccRepMotionResult(); +} + +/*! \brief Store the onboard ICC results for use by the device + \returns true if the store was successful +*/ +bool XsDevice::storeIccResults() +{ + return false; +} + +/*! \brief Gets the 'Latitude Longitude Altitude' setting of the device + \details The Latitude Longitude Altitude contains the location on earth where the measurements are + done. Setting this value allows for more accurate measurements. + Note: this XDA data type is the setting initialPositionLLA, which is set by setInitialPositionLLA. + It's value is therefore static. Use LatitudeLongitude to retrieve the live position data from the MTi. + \returns lla The desired 'Latitude Longitude Altitude' setting for the device. + \sa setInitialPositionLLA \sa gravityMagnitude +*/ +XsVector XsDevice::initialPositionLLA() const +{ + return XsVector(); +} + +/*! \brief Sets the 'Latitude Longitude Altitude' setting of the device to the given \a vector. + \details The Latitude Longitude Altitude contains the location on earth where the measurements are + done. Setting this value allows for more accurate measurements. The default gravity magnitude and + earth magnetic field are computed form this value. + \param lla The desired 'Latitude Longitude Altitude' setting for the device. This should be a + 3-element vector. + \returns true if the Latitude Longitude Altitude was successfully written + \note When GNSS is available, this value is automatically updated with the last known position when + the device is put in config mode after measurement. + \sa initialPositionLLA \sa labMagneticField \sa gravityMagnitude +*/ +bool XsDevice::setInitialPositionLLA(const XsVector& lla) +{ + (void)lla; + return false; +} + +/*! \brief Gets the 'UTC Time' setting of the device. + \details Gets the UTC time in the device. + \returns The current UTC time of the sensor +*/ +XsTimeInfo XsDevice::utcTime() const +{ + return XsTimeInfo(); +} + +/*! \brief Sets the 'UTC Time' setting of the device to the given \a time. + \details Sets the UTC time in the device. Setting this value allows for more accurate measurements. + \param time The current time in UTC format + \returns true if the UTC Time was successfully written + \note When GNSS is available, this value is automatically updated with the last known UTC time when + the device is put in config mode after measurement. +*/ +bool XsDevice::setUtcTime(const XsTimeInfo& time) +{ + (void)time; + return false; +} + +/*! \brief Returns the transmission delay used for RS485 transmissions + \details See the low level documentation for more information on this function. + \returns The currently configured RS485 transmission delay +*/ +uint16_t XsDevice::rs485TransmissionDelay() const +{ + return 0; +} + +/*! \brief Set the transmission delay used for RS485 transmissions + \details See the low level documentation for more information on this function. + \param delay The desired delay + \returns true if the device was successfully updated +*/ +bool XsDevice::setRs485TransmissionDelay(uint16_t delay) +{ + (void)delay; + return false; +} + +/*! \brief Run the self test for the device + \details All Xsens devices have limited self-diagnostic functionality, which can be triggered + by calling this function. The device automatically does some self tests during startup, but + this function returns more information. + \returns Results of the test + \note This function is blocking and can take a few 100 ms +*/ +XsSelfTestResult XsDevice::runSelfTest() +{ + return XsSelfTestResult(); +} + +/*! \brief Request data when configured in legacy mode with infinite skip factor + \details When configured in legacy mode and an output skip factor of 0xFFFF, the device will not + send data by itself, but will instead send the latest data after receiving an explicit request. + This function is that request. After the request, the normal callback mechanism will take over. + \returns true if the message was successfully sent. +*/ +bool XsDevice::requestData() +{ + return false; +} + +/*! \brief Store orientation filter state in the device + \details Use this function when the filters for the device have stabilized to store the + current biases in the device. The benefit is that on the next startup the filter + will stabilize quicker. However, the stored biases depend on temperature and + other external parameters, so the stored values will remain correct for only a + short time. + \returns true if the filter state was saved, false otherwise +*/ +bool XsDevice::storeFilterState() +{ + return false; +} + +// MTix device +/*! \brief Returns if the device does gyroscope bias estimation when switching to measurement mode + \details When this option is enabled, the device will automatically run the 'no rotation' + algorithm every time it switches to measurement mode. + \returns true if the option is enabled + \sa setNoRotation \sa setInitialBiasUpdateEnabled +*/ +bool XsDevice::isInitialBiasUpdateEnabled() const +{ + return false; +} + +/*! \brief Set if the device does gyroscope bias estimation when switching to measurement mode + \details When this option is enabled, the device will automatically run the 'no rotation' + algorithm every time it switches to measurement mode. + \param enable true to enable the option, false to disable it + \returns true if the device was successfully update + \sa setNoRotation \sa isInitialBiasUpdateEnabled +*/ +bool XsDevice::setInitialBiasUpdateEnabled(bool enable) +{ + (void)enable; + return false; +} + +/*! \brief Returns if the fixed gravity value should be used or if it should be computed from the + initialPositionLLA value. + \returns true if the option is enabled + \sa setFixedGravityEnabled \sa gravityMagnitude \sa initialPositionLLA +*/ +bool XsDevice::isFixedGravityEnabled() const +{ + return false; +} + +/*! \brief Sets whether the fixed gravity value should be used or if it should be computed from the + initialPositionLLA value. + \param enable true to use fixed gravity, false to compute from initialPositionLLA + \returns true if the device was successfully update + \sa isFixedGravityEnabled \sa setGravityMagnitude \sa setInitialPositionLLA +*/ +bool XsDevice::setFixedGravityEnabled(bool enable) +{ + (void)enable; + return false; +} + +/*! \brief Sets the GNSS Lever Arm vector + \details GNSS Lever arm is the length/pos of the antenna w.r.t. the center of the sensor + It is a 3-vector: x,y,z + \param arm The lever arm vector + \returns True if successful +*/ +bool XsDevice::setGnssLeverArm(const XsVector& arm) +{ + (void)arm; + return false; +} + +/*! \returns The GNSS Lever Arm vector. */ +XsVector XsDevice::gnssLeverArm() const +{ + return XsVector(); +} + +/*! \cond XS_INTERNAL */ +/*! \brief Requests UTC time. */ +bool XsDevice::requestUtcTime() +{ + return false; +} +/*! \endcond */ + +// Mtw device +/*! \brief Request the battery level from the device + \details This is an asynchronous operation. The Awinda station or MTw sends the battery level when + possible. For devices in wired mode the \sa batteryLevel() function can be called without calling this + function first. + \returns true If the battery level request was successfully sent +*/ +bool XsDevice::requestBatteryLevel() +{ + return false; +} + +/*! \brief Requests the time the battery level was last updated + \returns the XsTimeStamp the battery level was last set +*/ +XsTimeStamp XsDevice::batteryLevelTime() +{ + return 0; +} + +/*! \brief Enable or disable the transport mode for the device + \details The MTw has a "wake up by motion" feature that requires some power and can cause + unnecessary wakeups when transporting the device. This function can be used to put the device in + "transport mode", which effectively disables the motion wake up feature until the device is plugged + into something or the transport mode is explicitly disabled by this function again. + \param transportModeEnabled true to enable transport mode (which disables the motion wakeup) + \returns true if the device was successfully put in transport mode (or taken out of it) + \note MTw only +*/ +bool XsDevice::setTransportMode(bool transportModeEnabled) +{ + (void)transportModeEnabled; + return false; +} + +/*! \brief Returns the current state of the transport mode feature + \returns true if tranport mode is currently enabled + \sa setTransportMode +*/ +bool XsDevice::transportMode() +{ + return false; +} + +/*! \brief Returns if the device is outputting data in string mode + \details In string mode only NMEA packets are transmitted at the legacy update rate + \returns true if the device is configured for string mode output. +*/ +bool XsDevice::isInStringOutputMode() const +{ + return stringOutputType() != 0; +} + +/*! \brief Returns whether the device uses legacy device mode + \returns True if the legacy period, outputmode, outputsettings or string reports are used +*/ +bool XsDevice::usesLegacyDeviceMode() const +{ + return isInStringOutputMode(); +} + +/*! \cond XS_INTERNAL */ +/*! \brief Override the log interface + \note XsDevice takes control of the supplied pointer +*/ +void XsDevice::useLogInterface(DataLogger* logger) +{ + xsens::Lock myLock(&m_logFileMutex, true); + m_logFileInterface = logger; +} +/*! \endcond */ + +/*! \brief Load a complete logfile + \details Load the opened log file completely. This function loads all data from + the open logfile in a separate thread, generating onProgressUpdated callbacks. + This function will return true if the reading was scheduled. + \returns true if the threaded loading was successfully started + \sa onProgressUpdated +*/ +bool XsDevice::loadLogFile() +{ + Communicator* comm = communicator(); + if (!comm) + return false; + + if (!comm->isReadingFromFile()) + return false; + + comm->loadLogFile(this); + return true; +} + +/*! \brief Aborts loading a logfile + \returns true if loading is aborted successfully + \note if no file was currently loading returns false +*/ +bool XsDevice::abortLoadLogFile() +{ + Communicator* comm = communicator(); + if (comm && comm->isReadingFromFile()) + { + comm->abortLoadLogFile(); + comm->waitForLastTaskCompletion(); + return true; + } + return false; +} + +/*! \brief Get the name of the log file the device is reading from. + \details Returns an empty string when not in file mode. + \returns The name of the logfile +*/ +XsString XsDevice::logFileName() const +{ + if (!isReadingFromFile()) + { + std::unique_ptr myLock; + if (logFileInterface(myLock)) + { + MtbDataLogger const* mtb = dynamic_cast(logFileInterface(myLock)); + if (mtb != nullptr) + return mtb->filename(); + } + return XsString(); + } + + Communicator* object = communicator(); + if (!object) + return XsString(); + return object->logFileName(); +} + +/*! \brief Get the maximum update rate for the device + \returns The maximum update rate of the device + \sa supportedUpdateRates +*/ +int XsDevice::maximumUpdateRate() const +{ + // Get the supported update rates + std::vector frameRates = supportedUpdateRates(); + + // Determine the maximum value + std::vector::iterator p = std::max_element(frameRates.begin(), frameRates.end()); + if (p != frameRates.end()) + return *p; + else + return 0; +} + +/*! \brief Get the number of packets currently waiting in the slow data cache for the device based + \returns The highest received packet ID minus the last reported packet id in the slow data callback +*/ +int XsDevice::recordingQueueLength() const +{ + LockGuarded lockG(&m_deviceMutex); + + if (m_dataCache.empty()) + return 0; + + return (int)(m_dataCache.rbegin()->first - latestBufferedPacketId()); +} + +/*! \brief Get the number of items currently in the slow data cache for the device + \returns The actual number of items in the cache, which may contain huge gaps in packet ids +*/ +int XsDevice::cacheSize() const +{ + LockGuarded lockG(&m_deviceMutex); + return (int) m_dataCache.size(); +} + + +/*! \brief Get all the current synchronization settings of the device + \details This function is a generic way of requesting the synchonization options of a device, + since not all devices support the same synchronization functionality. + \returns The list of synchronization settings configured for the device + \sa setSyncSettings +*/ +XsSyncSettingArray XsDevice::syncSettings() const +{ + return XsSyncSettingArray(); +} + +/*! \brief Set the synchronization settings of the device + \details This function can be used to set all the synchronization options of the device at once. + It is translated into device-specific commands by XDA, since not all devices support the same + synchronization functionality. + \param settingList The list of synchronization settings to set. An empty list will clear all + synchronization settings. + \returns true if the device was successfully updated + \sa syncSettings +*/ +bool XsDevice::setSyncSettings(const XsSyncSettingArray& settingList) +{ + (void)settingList; + return false; +} + +/*! \brief Get all supported synchronization settings available on the device. + \details This function provides a list of the available synchronization settings of the device, + since not all devices support the same synchronization functionality. + Every XsSyncSetting element in the list defines one function and line setting, with supported parameters. + If the same function support multiple settings (i.e. multiple lines), + then the list will contains multiple items with the same function name, but with different line settings. + For easier use, same functions must be listed next to eachother, so each function settings + in the list will be grouped. + Properties, others then m_function and m_line are set to 0 if not supported or 1 if supported by the device. + \returns The list of synchronization settings supported by the device. Each settings grouped by functions. +*/ +XsSyncSettingArray XsDevice::supportedSyncSettings() const +{ + return Synchronization::supportedSyncSettings(m_deviceId); +} + +/*! \brief Perform an orientation reset on the device using the given \a resetMethod + \details This function schedules an orientation reset command to be applied in the first available + orientation filter update. + \param resetmethod The requested orientation reset method. + \returns true if the orientation reset was successfully scheduled + \note XRM_StoreAlignmentMatrix can only be used in config mode, the others only in measurement mode +*/ +bool XsDevice::resetOrientation(XsResetMethod resetmethod) +{ + return scheduleOrientationReset(resetmethod); +} + +/*! \brief Set the read position of the open log file to the start of the file. + \details If software filtering is enabled, the appropriate filters will be restarted + as if the file was just opened. + \returns true if the read position was successfully reset to the start of the file + \note This is a low-level file operation. +*/ +bool XsDevice::resetLogFileReadPosition() +{ + JLDEBUGG(""); + + Communicator* comm = communicator(); + if (!comm || !comm->isReadingFromFile()) + return false; + + comm->resetLogFileReadPosition(); + + clearDataCache(); + resetPacketStamping(); + return true; +} + +/*! \brief Get the size of the log file the device is reading from + \details If the function encounters an error the function returns 0. + \returns The size of the log file or 0 + \note This is a low-level file operation. + \sa logFileReadPosition +*/ +XsFilePos XsDevice::logFileSize() const +{ + JLDEBUGG(""); + + Communicator* comm = communicator(); + if (!comm || !comm->isReadingFromFile()) + return 0; + + return comm->logFileSize(); +} + +/*! \brief Get the current read position of the open log file + \details If the function encounters an error the function returns -1. + \returns The current read position (in bytes) from the start of the file or -1 + \note This is a low-level file operation. + \sa logFileSize +*/ +XsFilePos XsDevice::logFileReadPosition() const +{ + JLDEBUGG(""); + + Communicator* comm = communicator(); + if (!comm || !comm->isReadingFromFile()) + return -1; + + return comm->logFileReadPosition(); +} + +/*! \brief Updates the cached device information for all devices connected to this port + \details This function can only be called in config mode. XDA caches all device information to + prevent unnecessary communication with the device. When some configuration has changed without + XDA knowing about it (through sendCustomMessage() for example), it may be necessary to tell XDA + to refresh its cached information by calling this function. + \returns true if the cached information was updated successfully +*/ +bool XsDevice::updateCachedDeviceInformation() +{ + JLDEBUGG(""); + + if (isMeasuring()) + return false; + + return initialize(); +} + +/*! \cond XS_INTERNAL */ +/*! \brief Initializes the device using the supplied filter profiles + \returns True if successful +*/ +bool XsDevice::initialize() +{ + m_isInitialized = true; + return true; +} +/*! \endcond */ + +/*! \brief Enable an additional communication protocol when reading messages + \param protocol The type of protocol-support to add. + \returns true if the addition was successful + \note This is a per port or per file setting +*/ +bool XsDevice::enableProtocol(XsProtocolType protocol) +{ + Communicator* comm = communicator(); + if (!comm) + return false; + + 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 + } +} + +/*! \brief Disable a communication protocol previously added by XsDevice::enableProtocol + \param protocol The type of protocol-support to remove. + \returns true if the removal was successful + \note This is a per port or per file setting +*/ +bool XsDevice::disableProtocol(XsProtocolType protocol) +{ + Communicator* comm = communicator(); + if (!comm) + return false; + + 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 + } +} + +/*! \returns true when a protocol with type \a type has been added + \param[in] protocol The protocol type to check +*/ +bool XsDevice::isProtocolEnabled(XsProtocolType protocol) const +{ + Communicator* comm = communicator(); + if (!comm) + return false; + return comm->hasProtocol(protocol); +} + +/*! \brief Returns the sample period for string output + \returns The 'sample period' setting of the device for string output +*/ +uint16_t XsDevice::stringSamplePeriod() const +{ + return 0xFFFF; +} + +/*! \brief Returns the skipfactor for string output + \returns The 'output skip factor' setting of the device for string output +*/ +uint16_t XsDevice::stringSkipFactor() const +{ + return 0xFFFF; +} + +/*! \brief Returns the string output type + \returns The 'string output type' setting of the device + \sa setStringOutputType +*/ +uint16_t XsDevice::stringOutputType() const +{ + return 0x0000; +} + +/*! \brief Returns the operational mode + \returns The current operational mode of the device +*/ +XsOperationalMode XsDevice::operationalMode() const +{ + return XOP_LiveStream; +} + +/*! \brief Set the device in the given operational mode. + \param mode: Desired operional mode. + \returns True when successful, false otherwise. +*/ +bool XsDevice::setOperationalMode(XsOperationalMode mode) +{ + (void)mode; + return true; +} + +/*! \brief Returns the currently configured output of the device + \returns The output configuration of the device +*/ +XsOutputConfigurationArray XsDevice::outputConfiguration() const +{ + return XsOutputConfigurationArray(); +} + +/*! \brief Returns the currently configured CAN output of the device + \returns The can output configuration of the device +*/ +XsCanOutputConfigurationArray XsDevice::canOutputConfiguration() const +{ + return XsCanOutputConfigurationArray(); +} + +/*! \brief Returns the currently configured CAN configuration of the device + \return The can configuration of the device +*/ +uint32_t XsDevice::canConfiguration() const +{ + return 0; +} + +/*! \brief Enable and disable processing options + \details These options are used to specify whether XDA should compute certain kinds of data from + available other data and what data-retention policy to use. On a system with limited + resources it may be useful to limit the processing and data retention done by XDA. By default XDA will + do all processing it can do, but retain as little data as possible. + In case of conflict, \a enable supersedes \a disable. + \param enable A logically OR'ed combination of XsOptions to enable + \param disable A logically OR'ed combination of XsOptions to disable + \sa areOptionsEnabled +*/ +void XsDevice::setOptions(XsOption enable, XsOption disable) +{ + 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)); + static const XsOption mask = XSO_KeepLastLiveData | XSO_RetainLiveData | XSO_RetainBufferedData; + + // this has to be done in this order to prevent timing issues + bool clr = ((m_options & mask) != (upd & mask)); + m_options = upd; + if (clr) + clearExternalPacketCaches(); +} + +/*! \brief Returns true when all the specified processing options are enabled + \param options The options to check + \returns true if the options are enabled + \sa setOptions +*/ +bool XsDevice::areOptionsEnabled(XsOption options) const +{ + return (m_options & options) == options; +} + +/*! \brief Return the currently enabled options + \return The options that are enabled for this device and its child devices (if any) + \sa setOptions \sa areOptionsEnabled +*/ +XsOption XsDevice::getOptions() const +{ + return m_options; +} + +/*! \brief Return the product code of the device + \returns The product code of the device +*/ +XsString XsDevice::productCode() const +{ + return XsString(); +} + +/*! \brief Return the shortened product code of the device suitable for display + \returns The short product code of the device +*/ +XsString XsDevice::shortProductCode() const +{ + return productCode(); +} + +/*! \brief Return the hardware version of the device + \returns The hardware version of the device +*/ +XsVersion XsDevice::hardwareVersion() const +{ + return XsVersion(); +} + +/*! \brief Set the radio channel to use for wireless communication + \details This function can be used to enable or disable the radio of an Awinda Station. + \param channel A valid channel number in the range [11..25] or -1 to disable the radio + \returns true if the radio was successfully reconfigured + \note Awinda Station only +*/ +bool XsDevice::enableRadio(int channel) +{ + (void)channel; + return false; +} + +/*! \brief Disables the radio for this station, resetting all children to disconnected state + \returns true if the radio was successfully disabled + \note Awinda Station only +*/ +bool XsDevice::disableRadio() +{ + return false; +} + +/*! \brief Returns if the radio is enabled + \returns true if the radio is enabled + \note Awinda Sation only +*/ +bool XsDevice::isRadioEnabled() const +{ + return false; +} + +/*! \brief Returns the radio channel used for wireless communication + \returns The radio channel used for wireless communication or -1 if the radio is disabled + \note Awinda Sation only +*/ +int XsDevice::radioChannel() const +{ + return -1; +} + +#if 0 +/*! \brief Returns a quality indication for the current wireless communication + \returns A quality indication in the range [0-100] where 100 is the best quality. If the function + return -1, no radio quality indication could be given. + \note Awinda Station only +*/ +int XsDevice::radioQualityIndication() const +{ + return -1; +} +#endif + +/*! \cond XS_INTERNAL */ +/*! \brief Handles master indication message. + \param message The message to handle. +*/ +void XsDevice::handleMasterIndication(const XsMessage& message) +{ + onNonDataMessage(this, &message); +} +/*! \endcond */ + +/*! \brief Abort the wireless flushing operation and finalize the recording + \returns true if no flushing is in progress when the function exits + \note Awinda Station only +*/ +bool XsDevice::abortFlushing() +{ + return true; +} + +/*! \brief Accept connections from the device on the parent/master device + \details This function can be used to accept connections from a device that has been rejected. + Call this function from within the onConnectivityChanged callback. + \returns true if the device will be accepted next time it tries to connect + \note MTw rejected to Awinda Station only +*/ +bool XsDevice::acceptConnection() +{ + return false; +} + +/*! \brief Reject connections from the device on the parent/master device + \details This function can be used to reject connections from a device that has connected. + This function can be called from within the onConnectivityChanged callback or at other times + when a device is connected. + \returns true if the device will be rejected next time it tries to connect + \note After the function returns, this XsDevice should no longer be used. + \note MTw connected to Awinda Station only + \sa rejectReason +*/ +bool XsDevice::rejectConnection() +{ + return false; +} + +/*! \brief Returns the wireless priority of the device + \returns The wireless priority of the device or 0 if it has none. + \note MTw connected to Awinda Station only +*/ +int XsDevice::wirelessPriority() const +{ + return 0; +} + +/*! \brief Sets the wireless priority of the device + \param priority The desired wireless priority of the device in the range 0-255. + \returns true if the wireless priority has been successfully updated + \note MTw connected to Awinda Station only +*/ +bool XsDevice::setWirelessPriority(int priority) +{ + (void)priority; + return false; +} + +/*! \cond XS_INTERNAL */ +/*! \brief Sets the connectivity state to \a newState if different than the old state. + \param[in] newState the new state +*/ +void XsDevice::updateConnectivityState(XsConnectivityState newState) +{ + if (newState != m_connectivity) + { + JLDEBUGG(deviceId() << " newState " << newState << " oldState " << m_connectivity); + m_connectivity = newState; + onConnectivityChanged(this, newState); + } +} + +/*! \brief The default connectivity state for newly created devices +*/ +XsConnectivityState XsDevice::defaultChildConnectivityState() const +{ + return m_connectivity; +} +/*! \endcond */ + +/*! \brief Returns the reason why a device's connection was rejected. + \details This function is typically called from within the onConnectivityChanged callback when + the connectivity has changed to XCS_Rejected. + \returns The reason why the connection was rejected +*/ +XsRejectReason XsDevice::rejectReason() const +{ + return XRR_Unknown; +} + +/*! \brief Returns the last known RSSI value of the device. + \details RSSI values are only relevant for wireless devices. Since the value is measured passively, + any time an RSSI value is received by XDA, the last known value is updated. + \returns The last known biased RSSI value or XS_RSSI_UNKNOWN if no RSSI value is available (yet) +*/ +int16_t XsDevice::lastKnownRssi() const +{ + return XS_RSSI_UNKNOWN; +} + +/*! \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; +} +/*! \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. +*/ +int XsDevice::packetErrorRate() const +{ + return 0; +} + +/*! \brief Returns the connectivity state of the device + \details The connectivity describes how and if the device is connected to XDA. + \returns The current connectivity of the device + \sa XsConnectivityState +*/ +XsConnectivityState XsDevice::connectivityState() const +{ + return m_connectivity; +} + +/*! \brief Check if the device is docked + \details Checks if device \a dev is docked in this device + \param dev The device to check + \return true if the device is docked in this device +*/ +bool XsDevice::deviceIsDocked(XsDevice* dev) const +{ + (void)dev; + return false; +} + +/*! \brief The port name of the connection + \returns The name of the communication port or an empty string if not connected to a communication + port +*/ +XsString XsDevice::portName() const +{ + Communicator* comm = communicator(); + if (!comm) + return XsString(); + + return comm->portInfo().portName(); +} + +/*! \brief The port information of the connection + \returns The port information object containing the information of the communication port or an empty structure if not connected to a communication + port +*/ +XsPortInfo XsDevice::portInfo() const +{ + Communicator* comm = communicator(); + if (!comm) + return XsPortInfo(); + + return comm->portInfo(); +} + +#ifndef XSENS_NO_PORT_NUMBERS +/*! \brief Return the port number of the connection + \returns The port number or 0 if no port is open that can be identified by a single number +*/ +int XsDevice::portNumber() const +{ + Communicator* comm = communicator(); + if (!comm) + return 0; + return comm->portInfo().portNumber(); +} +#endif + +/*! \brief Returns true when the device is initialized + \returns true when the device has been initialized +*/ +bool XsDevice::isInitialized() const +{ + return m_isInitialized; +} + +/*! \brief Accepts a device + \param[in] deviceId The device to accept + \returns true when the device has been successfully accepted +*/ +bool XsDevice::setDeviceAccepted(const XsDeviceId&) +{ + return false; +} + +/*! \brief Rejects a device + \param[in] deviceId The device to reject + \returns true when the device has been successfully rejected +*/ +bool XsDevice::setDeviceRejected(const XsDeviceId&) +{ + return false; +} + +/*! \brief Set the access control mode of the master device + \details The access control mode determines which connections are allowed. + \param mode The access control mode to use, the choice is between blacklist or whitelist + \param initialList The initial list to use for the selected access control mode + \returns true if the access control mode was successfully changed + \sa accessControlMode() \sa currentAccessControlList +*/ +bool XsDevice::setAccessControlMode(XsAccessControlMode, const XsDeviceIdArray&) +{ + return false; +} + +/*! \brief Request the access control mode of the master device + \returns The currently configured access control mode + \sa setAccessControlMode \sa currentAccessControlList +*/ +XsAccessControlMode XsDevice::accessControlMode() const +{ + return XACM_None; +} + +/*! \brief Request the access control list of the master device + \returns The currently configured access control list. This can be either a blacklist or a whitelist. + \sa setAccessControlMode \sa accessControlMode +*/ +XsDeviceIdArray XsDevice::currentAccessControlList() const +{ + return XsDeviceIdArray(); +} + +/*! \brief Sets the Awinda station to operational state + \note this is considered an extension to the config state, not a new state. + \returns true when the awindastation is put in operational mode. +*/ +bool XsDevice::makeOperational() +{ + return false; +} + +/*! \returns true when the device is operational + \sa AwindaStationDevice::makeOperational() +*/ +bool XsDevice::isOperational() const +{ + return false; +} + +/*! \returns true when the device is in Sync Station mode (Awinda Station and Sync Station only) */ +bool XsDevice::isInSyncStationMode() +{ + return false; +} + +/*! \brief Set the Sync Station mode of the Awinda Station device + \param enabled true to enable Sync Station mode, false to disable it + \returns true if successful +*/ +bool XsDevice::setSyncStationMode(bool) +{ + return false; +} + +/*! \brief Increase reference count of XsDevice pointer + XsDevice pointers stay alive while reference counter is not zero + Also increases the reference count of each child device with 1 +*/ +void XsDevice::addRef() +{ + JLTRACEG(this << " pre: " << m_refCounter.load()); + ++m_refCounter; +} + +/*! \brief The current reference counter + \returns The current reference count +*/ +XsSize XsDevice::refCounter() const +{ + return (XsSize)(ptrdiff_t) m_refCounter.load(); +} + +/*! \brief Decrease this XsDevices reference counter with 1 + \details Also decreases the reference count of each child with 1 + - If it is a child device, it will delete itself when the reference count reaches zero. + It will also remove itself from its master's child list and ask the master if it can be deleted + - If it is a master device, it will delete itself when the reference count reaches zero and the reference count + of all children is zero. +*/ +void XsDevice::removeRef() +{ + JLTRACEG(this << " pre: " << m_refCounter.load()); + + // Do not decrease when already at 0 ref count + // In this case, the device is just waiting for all children to be dereferenced + if (m_refCounter.load() != 0) + { + --m_refCounter; + assert(m_refCounter.load() >= 0); + } + + // Always check if this device can be removed + removeIfNoRefs(); +} + +/*! \cond XS_INTERNAL */ +/*! \brief Writes an EMTS page to the device + \param data The data buffer + \param pageNr The page number + \param bankNr The bank number + \returns True if successful +*/ +bool XsDevice::writeEmtsPage(uint8_t const* data, int pageNr, int bankNr) +{ + (void)data; + (void)pageNr; + (void)bankNr; + return false; +} + +/*! \brief Delete itself when all references and child references are zero +*/ +void XsDevice::removeIfNoRefs() +{ + // If any child has refs, no need to delete this device + LockSuspendable lock(&m_deviceMutex, LS_Write); + if (childCount()) + return; + //for (XsDevice* dev:m_children) + // if (dev->refCounter() > 0) + // return; + + if (m_refCounter.load() == 0) + { + lock.unlock(); + prepareForTermination(); + delete this; + } +} + +/*! \brief Return true if the message looks sane */ +bool XsDevice::messageLooksSane(const XsMessage& msg) const +{ + return msg.getBusId() == XS_BID_MASTER; +} + +void XsDevice::onMessageSent(const XsMessage& msg) +{ + onMessageSentToDevice(this, &msg); +} + +void XsDevice::onMessageReceived(const XsMessage& msg) +{ + onMessageReceivedFromDevice(this, &msg); +} + +void XsDevice::onMessageDetected2(XsProtocolType type, const XsByteArray& rawMessage) +{ + CallbackManagerXda::onMessageDetected(this, type, &rawMessage); +} + +void XsDevice::onSessionRestarted() +{ +} + +void XsDevice::onConnectionLost() +{ + updateConnectivityState(XCS_Disconnected); +} + +void XsDevice::onEofReached() +{ + LockGuarded locky(&m_deviceMutex); + setStopRecordingPacketId(latestLivePacketId()); + endRecordingStream(); +} + +void XsDevice::onWirelessConnectionLost() +{ + updateConnectivityState(XCS_Disconnected); +} + +/*! \return The packet ID of the cached latest Live packet */ +int64_t XsDevice::latestLivePacketId() const +{ + LockGuarded locky(&m_deviceMutex); + return latestLivePacketConst().packetId(); +} + +/*! \return The packet ID of the cached latest Buffered packet */ +int64_t XsDevice::latestBufferedPacketId() const +{ + LockGuarded locky(&m_deviceMutex); + return latestBufferedPacketConst().packetId(); +} + +/*! \return true if the message should be written to log file + \param msg The message to write + \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 +{ + return shouldWriteMessageToLogFile(this, msg); +} + +/*! \return true if \a child should be writing to log file + \param dev The child device + \param message The message to write + \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 +{ + (void)dev; + (void)message; + return true; +} + +/*! \brief Send a message and wait for its reply + \details The expected reply is always the Ack to \a snd's message + \param snd the message to send + \returns true if the message was sent and acknowledged, false otherwise +*/ +bool XsDevice::doTransaction(const XsMessage& snd) const +{ + 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 + \details The expected reply is always the Ack to \a snd's message + \param snd the message to send + \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 +{ + 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 + \details The expected reply is always the Ack to \a snd's message + \param snd the message to send + \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 +{ + 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 + \details The expected reply is always the Ack to \a snd's message + \param snd the message to send + \param rcv a pointer to a receive message + \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 +{ + 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 */ + +/*! \brief Enable or disable stealth mode + \details In stealth mode, the MVN hardware will be silent and all LEDs will be dimmed or disabled. + Some minimal user feedback will remain enabled. The change will be applied immediately to all detected + systems, resetting the mocap data stream. + \param enabled Set to true if you wish to enable stealth mode, false to disable it. + \return true if the setting was successfully updated +*/ +bool XsDevice::setStealthMode(bool) +{ + return false; +} + +/*! \brief Return the state of the stealth mode setting. + \return the state of the stealth mode setting. + \sa setStealthMode +*/ +bool XsDevice::stealthMode() const +{ + return false; +} + +/*! \brief Return the supported synchronization settings for a specified \a deviceId or deviceId mask + \param[in] deviceId The device id to request the supported synchronization settings for + \returns the supported synchronization settings for the specified deviceId +*/ +XsSyncSettingArray XsDevice::supportedSyncSettings(XsDeviceId const& deviceId) +{ + XsSyncSettingArray settings = Synchronization::supportedSyncSettings(deviceId); + XsSyncSettingArray result; + for (XsSyncSetting const& setting : settings) + result.push_back(setting); + + return result; +} + +/*! \brief Determines whether the device specified by \a deviceId supports sync settings + \param[in] deviceId The device id to check + \returns true when the device specified by \a deviceId supports sync settings +*/ +bool XsDevice::supportsSyncSettings(XsDeviceId const& deviceId) +{ + return Synchronization::supportsSyncSettings(deviceId); +} + +/*! \brief Determines whether \a setting1 is compatible with \a setting2 for deviceId \a deviceId + \param[in] deviceId The device id + \param[in] setting1 Setting 1 + \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) +{ + return Synchronization::isCompatibleSyncSetting(deviceId, setting1, setting2); +} + +/*! \returns the time resolution in microseconds for a device with device id \a deviceId + For example if the precision is 1 millisecond, 1000 is returned + \param[in] deviceId The deviceId +*/ +unsigned int XsDevice::syncSettingsTimeResolutionInMicroSeconds(XsDeviceId const& deviceId) +{ + return Synchronization::timeResolutionInMicroseconds(deviceId); +} + +/*! \brief Return the number of child-devices this device has. For standalone devices this is always 0 + \returns The number of child devices of the device +*/ +int XsDevice::childCount() const +{ + return 0; +} + +/*! \brief Return a managed array containing the child-devices this device has. For standalone devices this is always an empty array + \returns An array of pointers to the child devices of the device +*/ +std::vector XsDevice::children() const +{ + return std::vector(); +} + +/*! \cond XS_INTERNAL */ +/*! \brief Sets a communicator for a device + \param comm a communicator +*/ +void XsDevice::setCommunicator(Communicator* comm) +{ + if (m_communicator) + m_communicator->destroy(); + m_communicator = comm; +} + +/*! \brief Set the frame at which the recording is actually started +*/ +void XsDevice::setRecordingStartFrame(uint16_t startFrame) +{ + LockGuarded lock(&m_deviceMutex); + m_stopRecordingPacketId = -1; + m_startRecordingPacketId = PacketStamper::calculateLargePacketCounter(startFrame, latestLivePacketId(), PacketStamper::MTSCBOUNDARY); + JLDEBUGG(this << " " << deviceId() << " m_startRecordingPacketId = " << m_startRecordingPacketId << " m_stopRecordingPacketId = " << m_stopRecordingPacketId << " m_stoppedRecordingPacketId = " << m_stoppedRecordingPacketId); +} + +/*! \brief Set the frame at which the recording is actually stopped +*/ +void XsDevice::setRecordingStopFrame(uint16_t stopFrame) +{ + LockGuarded lock(&m_deviceMutex); + JLDEBUGG(this << " " << deviceId() << " Setting recording stop frame to " << stopFrame << " last known frame is " << latestBufferedPacketId() << " (" << (latestBufferedPacketId() & 0xffff) << ")"); + m_stopRecordingPacketId = PacketStamper::calculateLargePacketCounter(stopFrame, latestBufferedPacketId(), PacketStamper::MTSCBOUNDARY); + m_stoppedRecordingPacketId = m_stopRecordingPacketId; + JLDEBUGG(this << " " << deviceId() << " m_startRecordingPacketId = " << m_startRecordingPacketId << " m_stopRecordingPacketId = " << m_stopRecordingPacketId << " m_stoppedRecordingPacketId = " << m_stoppedRecordingPacketId); +} + +/*! \brief Clear the external packet cache +*/ +void XsDevice::clearExternalPacketCaches() +{ + LockGuarded lock(&m_deviceMutex); + for (auto item : m_linearPacketCache) + delete item; + m_linearPacketCache.clear(); + m_lastAvailableLiveDataCache->clear(); +} + +/*! \brief Clear the data cache up till the packet indicated by the start recording frame +*/ +void XsDevice::clearCacheToRecordingStart() +{ + LockGuarded lockG(&m_deviceMutex); + for (auto item : m_dataCache) + { + if (item.first < m_startRecordingPacketId) + { + delete item.second; + m_dataCache.erase(item.first); + } + } +} + +/*! \brief Merge the supplied \a pack into m_lastAvailableLiveDataCache so its data is now available when calling lastAvailableLiveData() +*/ +void XsDevice::updateLastAvailableLiveDataCache(XsDataPacket const& pack) +{ + LockGuarded lockG(&m_deviceMutex); + m_lastAvailableLiveDataCache->merge(pack, true); +} + +/*! \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); + if (!m_linearPacketCache.empty() && m_linearPacketCache.back()->packetId() == pack.packetId()) + m_linearPacketCache.back()->merge(pack, true); + else + m_linearPacketCache.push_back(new XsDataPacket(pack)); +} + +/*! \brief Return whether a device reset will remove the COM port connection + \return True or false + \note When connected directly to USB, the device cannot reset without losing connection. Then a rescan should be done +*/ +bool XsDevice::resetRemovesPort() const +{ + return true; +} +/*! \endcond */ + +/*! \brief Return the cached data packet with \a index + \param index The requested index, this does not have to be the same as the packet counter + \return The requested packet or an empty packet if \a index is out of range + \note This only works if XSO_RetainLiveData or XSO_RetainBufferedData was set before the data was read + \sa getDataPacketCount + \sa setOptions +*/ +XsDataPacket XsDevice::getDataPacketByIndex(XsSize index) const +{ + LockGuarded lockG(&m_deviceMutex); + if (index < m_linearPacketCache.size()) + return *m_linearPacketCache.at(index); + return XsDataPacket(); +} + +/*! \brief Return the current size of the retained data packet cache + \return The current size of the cache + \sa getDataPacketCount + \sa setOptions +*/ +XsSize XsDevice::getDataPacketCount() const +{ + LockGuarded lockG(&m_deviceMutex); + return m_linearPacketCache.size(); +} + +/*! \brief Return the last available live data + \return A packet containing the latest available live data. This packet will contain the latest data of each + appropriate type, so it may contain old data mixed with new data if different data comes in at different speeds. + \note This only works if XSO_KeepLastLiveData is set + \sa setOptions +*/ +XsDataPacket XsDevice::lastAvailableLiveData() const +{ + LockGuarded lockG(&m_deviceMutex); + return *m_lastAvailableLiveDataCache; +} + +/*! \brief Return the first packet in the packet queue or an empty packet if the queue is empty + \details This function will only return a packet when XSO_RetainLiveData or XSO_RetainBufferedData is specified for the + device. It will return the first packet in the queue and remove the packet from the queue. + \return The first packet in the queue or an empty packet if the queue is empty + \sa setOptions +*/ +XsDataPacket XsDevice::takeFirstDataPacketInQueue() +{ + LockGuarded lockG(&m_deviceMutex); + if (m_linearPacketCache.empty()) + return XsDataPacket(); + + std::unique_ptr rv(m_linearPacketCache.at(0)); + m_linearPacketCache.pop_front(); + return *rv; // copy and delete, hopefully optimizing the copy into a move operation +} + +/*! \cond XS_INTERNAL */ +/*! \brief Sets start recording packet id + \param startFrame a start frame value +*/ +void XsDevice::setStartRecordingPacketId(int64_t startFrame) +{ + LockGuarded lockG(&m_deviceMutex); + m_startRecordingPacketId = startFrame; + ONLYFIRSTMTX2 + JLDEBUGG(this << " " << deviceId() << " m_startRecordingPacketId = " << m_startRecordingPacketId << " m_stopRecordingPacketId = " << m_stopRecordingPacketId << " m_stoppedRecordingPacketId = " << m_stoppedRecordingPacketId); +} + +/*! \brief Sets stop recording packet id + \param stopFrame a stop frame value +*/ +void XsDevice::setStopRecordingPacketId(int64_t stopFrame) +{ + LockGuarded lockG(&m_deviceMutex); + m_stopRecordingPacketId = stopFrame; + m_stoppedRecordingPacketId = m_stopRecordingPacketId; + ONLYFIRSTMTX2 + JLDEBUGG(this << " " << deviceId() << " m_startRecordingPacketId = " << m_startRecordingPacketId << " m_stopRecordingPacketId = " << m_stopRecordingPacketId << " m_stoppedRecordingPacketId = " << m_stoppedRecordingPacketId); +} + +/*! \brief Sets skip emts read on initialization + \param skip Set to true if you want to skip it +*/ +void XsDevice::setSkipEmtsReadOnInit(bool skip) +{ + m_skipEmtsReadOnInit = skip; +} + +/*! \brief Reads EMTS and device configuration. + \details EMTS will be stored in \ref m_emtsBlob. + \note Used in public source to have this data cached for log file header, so MTM can read the file. + \returns True if successful. +*/ +bool XsDevice::readEmtsAndDeviceConfiguration() +{ + if (!gotoConfig()) + return false; + + XsMessage snd(XMID_ReqEmts, 2); + snd.setBusId(busId()); + snd.setDataByte(0, 0); + snd.setDataByte(255, 1); + + XsMessage rcv; + if (!doTransaction(snd, rcv, 10000)) + return false; + + m_emtsBlob = rcv; + + snd.setMessageId(XMID_ReqConfiguration); + snd.setBusId(busId()); + + rcv.clear(); + if (!doTransaction(snd, rcv, 5000)) + return false; + + m_config.readFromMessage(rcv); + + return true; +} + +/*! \brief Gets a device recording buffer item count. + \param lastCompletePacketId an id of last complete packet. +*/ +int64_t XsDevice::deviceRecordingBufferItemCount(int64_t& lastCompletePacketId) const +{ + lastCompletePacketId = latestLivePacketId(); + return 0; +} +/*! \endcond */ + +/*! \brief Request the size of the interal buffer + \return Buffer size in number of frames +*/ +uint32_t XsDevice::deviceBufferSize() +{ + return 0; +} + +/*! \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; + return true; +} + +/*! \brief Wait until are known devices are initialized + \details Container devices such as Awinda Master and Bodypack can have (slightly) delayed initialization + of child devices after they have been detected. This function can be used to wait for all currently + detected trackers to have been properly initialized. +*/ +void XsDevice::waitForAllDevicesInitialized() +{ + while (!isInitialized()) + XsTime::msleep(0); +} + +/*! \brief Returns true if the file operation started by loadLogFile is still in progress + \return true if the file operation started by loadLogFile is still in progress + \sa loadLogFile \sa waitForLoadLogFileDone +*/ +bool XsDevice::isLoadLogFileInProgress() const +{ + Communicator const* comm = communicator(); + if (!comm) + return false; + + if (!comm->isReadingFromFile()) + return false; + + return comm->isLoadLogFileInProgress(); +} + +/*! \brief Wait for the file operation started by loadLogFile to complete + \sa loadLogFile \sa isLoadLogFileInProgress +*/ +void XsDevice::waitForLoadLogFileDone() const +{ + while (isLoadLogFileInProgress() && !m_terminationPrepared) + XsTime::msleep(10); +} + +/*! \brief Return the ID of the first packet that should be recorded + \details This is only valid in Recording or Flushing states + \return The ID of the first packet that should be recorded +*/ +int64_t XsDevice::getStartRecordingPacketId() const +{ + return m_startRecordingPacketId; +} + +/*! \brief Return the ID of the last packet that should be recorded + \details This is only valid in Recording or Flushing states or in Measurement after a recording has finished + \return The ID of the last packet that should be / was recorded +*/ +int64_t XsDevice::getStopRecordingPacketId() const +{ + return m_stopRecordingPacketId == -1 ? m_stoppedRecordingPacketId : m_stopRecordingPacketId; +} + +/*! \brief Sets the given parameter for the device + \details Settings device parameters is only valid after initialization and before switching the device to be operational. + \param parameter: a parameter object, corresponding to a row in the table below. + \return Result value indicating success (XRV_OK) or unsupported with current device or firmware version (XRV_UNSUPPORTED). + + For Awinda stations this function needs to be called before enabling the radio (\a XsDevice::enableRadio). + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
XsDeviceParameterIdentifierValue type/rangeDescriptionSupported devices
Awinda 2 (Station, Dongle)BodyPackMTmk4MTw2MTx2SyncStation
XDPI_PacketErrorRatebooleanReport packet error rate for each child device.Y-----
XDPI_SyncLossTimeoutuint16_tNetwork timeout in seconds used by connected child devices.
For Awinda2: this timeout is measured by connected MTw2 devices based on received data from the station.		Y-----
XDPI_UplinkTimeoutuint16_tNetwork timeout in seconds used by the master devices.
For Awinda2: this timeout is measured based on received data from each connected MTw2 individually.		Y-----
XDPI_ExtendedBufferboolIf set, enables the extended buffer on connected MTw2 devices.Y-----
+*/ +XsResultValue XsDevice::setDeviceParameter(XsDeviceParameter const& parameter) +{ + (void)parameter; + return XRV_UNSUPPORTED; +} + +/*! \brief Retrieves the requested parameter's current value + \details Retrieving device parameters is only valid after initialization + \param parameter: a parameter object, corresponding to a row in the table listed under \a XsDevice::setParameter + \return Result value indicating success (XRV_OK) or unsupported with current or current firmware version (XRV_UNSUPPORTED) + \sa XsDevice::setParameter +*/ +XsResultValue XsDevice::deviceParameter(XsDeviceParameter& parameter) const +{ + (void)parameter; + return XRV_UNSUPPORTED; +} + +/*! \cond XS_INTERNAL */ +/*! \brief Returns the device GNSS platform + \returns The current device GNSS platform + \deprecated GNSS platform is a u-blox specific feature, please use the u-blox specific functions + \sa XsDevice::ubloxGnssPlatform +*/ +XsGnssPlatform XsDevice::gnssPlatform() const +{ + return static_cast(ubloxGnssPlatform()); +} + +/*! \brief Set the device GNSS platform + \param gnssPlatform The GNSS platform that must be set + \returns true if the device GNSS platform was successfully set + \deprecated GNSS platform is a u-blox specific feature, please use the u-blox specific functions + \sa XsDevice::setUbloxGnssPlatform +*/ +bool XsDevice::setGnssPlatform(XsGnssPlatform gnssPlatform) +{ + return setUbloxGnssPlatform(static_cast(gnssPlatform)); +} +/*! \endcond */ + +/*! \brief Returns the device GNSS platform for u-blox GNSS receivers. + \warning This function has an undefined meaning when used on non u-blox GNSS receivers. + \returns The current device GNSS platform +*/ +XsUbloxGnssPlatform XsDevice::ubloxGnssPlatform() const +{ + return XGP_Portable; +} + +/*! \brief Set the device GNSS platform for u-blox GNSS receivers. + \warning This function produces undefined behaviour when used on non u-blox GNSS receivers. + \param ubloxGnssPlatform The GNSS platform that must be set + \returns true if the device GNSS platform was successfully set +*/ +bool XsDevice::setUbloxGnssPlatform(XsUbloxGnssPlatform ubloxGnssPlatform) +{ + (void) ubloxGnssPlatform; + return false; +} + +/*! \brief Gets some GNSS receiver settings + \returns XsIntArray containing the receiver settings +*/ +XsIntArray XsDevice::gnssReceiverSettings() const +{ + return XsIntArray(); +} + +/*! \brief Sets some GNSS receiver settings + \param gnssReceiverSettings XsIntArray containing the receiver settings, that are to be set + \returns true if the settings were successfully set; false otherwise +*/ +bool XsDevice::setGnssReceiverSettings(const XsIntArray& gnssReceiverSettings) +{ + (void)gnssReceiverSettings; + return false; +} + +/*! \cond XS_INTERNAL */ +//! \brief Reinitialize all defined data processing components for this device +void XsDevice::reinitializeProcessors() +{ + // intentionally empty, private implementation +} + +/*! \brief This function indicates if the device supports retransmissions (at all) + \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! +*/ +bool XsDevice::expectingRetransmissionForPacket(int64_t packetId) const +{ + if (isMasterDevice()) + return false; + + // child devices use their parent's decision + return master()->expectingRetransmissionForPacket(packetId); +} + +bool XsDevice::initializeSoftwareCalibration() +{ + // intentionally empty, private implementation + return false; +} + +void XsDevice::deinitializeSoftwareCalibration() +{ + // intentionally empty, private implementation +} + +/*! \brief Returns the first child device (if any) of the master. If there are no children, it will return the master device itself. +*/ +XsDevice const* XsDevice::firstChild() const +{ + return this; +} + +/*! \brief Tell XDA to interpolate missing items from \a prev to \a pack + \param pack The latest received packet + \param prev The previously received packet + \param packetHandler The function to call with all newly created intermediate and the final packet. packetHandler is expected to take control of its argument. + \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) +{ + (void) pack; + (void) prev; + (void) packetHandler; + return false; +} +/*! \endcond */ + +/*! \brief Tell XDA and the device that any data from before \a firstNewPacketId may be lossy + \details Tell the device to not request retransmissions of missed data older than the supplied \a firstNewPacketId. + If \a firstNewPacketId is beyond the end of the recording or beyond the highest received packet ID, + the lower value is used instead. This means that you can't set this for future packets. + \param firstNewPacketId The first packet that (if missing) should be retransmitted. + \note This applies to master devices that support retransmissions only: Awinda and Bodypack. +*/ +void XsDevice::discardRetransmissions(int64_t firstNewPacketId) +{ + (void) firstNewPacketId; +} + +/*! \brief Returns a bitmask with all the status flags supported by this device + \details Not all devices support all status flags. When receiving an XsDataPacket with a status in it, + this can affect how to interpret the flags. Especially with a flag like the self-test it's important + not to conclude that a device is defective because it is not set when the device doesn't actually support + this feature. + \return The bitmask with all the status flags that this device supports +*/ +uint32_t XsDevice::supportedStatusFlags() const +{ + // The basic device supports nothing + return (uint32_t) 0; +} + +/*! \brief Update the connection information, this is only useful for devices communicating over a dynamic link such as a TCP/IP connection + \param newInfo The new port information to use in the next (re)connect attempts + \return Whether the operation succeeded or not +*/ +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/extern/xspublic/xscontroller/xsdevice_def.h b/extern/xspublic/xscontroller/xsdevice_def.h new file mode 100644 index 0000000..fbd63cc --- /dev/null +++ b/extern/xspublic/xscontroller/xsdevice_def.h @@ -0,0 +1,767 @@ + +// Copyright (c) 2003-2022 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 XSDEVICE_DEF_H +#define XSDEVICE_DEF_H + +#include +#include +#include "packetstamper.h" +#include "xsdeviceconfiguration.h" +#include "xserrormode.h" +#include +#include "callbackmanagerxda.h" +#include +#include "xsrejectreason.h" +#include "communicator.h" +#include "xsalignmentframe.h" +#include "xsaccesscontrolmode.h" +#include "datapacketcache.h" +#include +#include +#include "lastresultmanager.h" +#include "xsgnssplatform.h" +#include +#include "xsoperationalmode.h" + +class XSNOEXPORT MtContainer; +class XSNOEXPORT DataLogger; +class XSNOEXPORT PacketProcessor; + +//AUTO namespace xstypes { +struct XsString; +struct XsDeviceId; +struct XsPortInfo; +struct XsDataPacket; +struct XsSyncSetting; +struct XsVersion; +struct XsScrData; +struct XsCalibratedData; +struct XsTimeInfo; +struct XsOutputConfigurationArray; +struct XsCanOutputConfigurationArray; +struct XsIntArray; +struct XsMatrix3x3; +struct XsDeviceIdArray; +struct XsDataPacket; +struct XsMessage; +struct XsSyncSettingArray; +struct XsMatrix; +struct XsVector; +struct XsStringArray; +struct XsQuaternion; +struct XsFilterProfileArray; +struct XsOutputConfiguration; +struct XsStringOutputTypeArray; +//AUTO+chdr struct XsException; +//AUTO } + +//AUTO namespace xscontroller { +struct XsCallbackPlainC; +struct XsSelfTestResult; +struct XsDeviceParameter; +struct XsIccRepMotionResult; +//AUTO } + +#define DebugFileType FILE // required for autogenerate to correctly parse this +#define NOEXCEPT noexcept // required for autogenerate to correctly parse this + +//AUTO namespace xstypes { +struct XsFilterProfile; +//AUTO enum XsBaud; +//AUTO enum XsResultValue; +//AUTO enum XsSyncRole; +//AUTO struct XsIntArray; +//AUTO struct XsOutputConfigurationArray; +//AUTO struct XsCanOutputConfigurationArray; +//AUTO struct XsSyncSettingArray; +//AUTO enum XsXbusMessageId; +//AUTO enum XsFilePos; +//AUTO struct XsFilterProfileArray; +//AUTO enum XsDeviceOptionFlag; +//AUTO enum XsResetMethod; +//AUTO enum XsOption; +//AUTO } + +//AUTO namespace xscontroller { +//AUTO struct XsDevicePtrArray; +//AUTO enum XsDeviceState; +//AUTO enum XsErrorMode; +//AUTO struct XsDeviceConfiguration; +//AUTO enum XsProtocolType; +//AUTO enum XsRejectReason; +//AUTO enum XsAlignmentFrame; +//AUTO enum XsOperationalMode; +//AUTO enum XsAccessControlMode; +//AUTO struct XsDeviceParameter; +//AUTO enum XsUbloxGnssPlatform; +//AUTO struct XsIccRepMotionResult; +//AUTO } + +//AUTO struct XdaConfig; + +struct XsDevice : public CallbackManagerXda +{ +public: + XSNOEXPORT virtual ~XsDevice(); + + virtual void addRef(); + virtual void removeRef(); + XsSize refCounter() const; + + virtual XsDevice* master() 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; + bool isMasterDevice() const; + virtual bool isContainerDevice() const; + bool isInitialized() const; + bool isStandaloneDevice() const; + + const XsDevice* deviceAtBusIdConst(int busid) const; + virtual XsDevice* deviceAtBusId(int busid); + + 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); + virtual bool closeLogFile(); + + virtual bool isMeasuring() const; + virtual bool isRecording() const; + virtual bool isReadingFromFile() const; + + virtual void XSNOEXPORT checkDataCache(); + virtual void restartFilter(); + + XsResultValue lastResult() const; + XsString lastResultText() const; + + int recordingQueueLength() const; + int cacheSize() const; + + virtual XsDeviceState deviceState() const; + + static bool supportsSyncSettings(XsDeviceId const& deviceId); + static bool isCompatibleSyncSetting(XsDeviceId const& deviceId, XsSyncSetting const& setting1, XsSyncSetting const& setting2); + static unsigned int syncSettingsTimeResolutionInMicroSeconds(XsDeviceId const& deviceId); + +#ifdef DOXYGEN + // Explicit inheritance for generator + void XSNOCOMEXPORT clearCallbackHandlers(bool chain = true); + void XSNOCOMEXPORT addCallbackHandler(XsCallbackPlainC* cb, bool chain = true); + void XSNOCOMEXPORT removeCallbackHandler(XsCallbackPlainC* cb, bool chain = true); +#endif + + //! \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(); + } + //! \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(); + } + //! \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(); + } + //! \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(); + } + + XsDeviceConfiguration deviceConfiguration() const; + XsDeviceConfiguration& XSNOEXPORT deviceConfigurationRef(); + virtual XsDeviceConfiguration const& XSNOEXPORT deviceConfigurationConst() const; + + //! \internal + XSNOEXPORT template T* toType() + { + return dynamic_cast(this); + } + + virtual bool XSNOEXPORT initialize(); + virtual bool XSNOEXPORT initializeSoftwareCalibration(); + virtual void XSNOEXPORT deinitializeSoftwareCalibration(); + + virtual int batteryLevel() const; + virtual int updateRateForDataIdentifier(XsDataIdentifier dataType) const; + virtual int updateRateForProcessedDataIdentifier(XsDataIdentifier dataType) const; + virtual std::vector supportedUpdateRates(XsDataIdentifier dataType = XDI_None) const; + virtual int maximumUpdateRate() const; + virtual bool hasDataEnabled(XsDataIdentifier dataType) const; + virtual bool hasProcessedDataEnabled(XsDataIdentifier dataType) const; + virtual XsString productCode() const; + virtual XsString shortProductCode() const; + virtual XsString portName() const; + virtual XsPortInfo portInfo() const; + virtual XsBaudRate baudRate() const; + virtual XsBaudRate serialBaudRate() const; + virtual XsVersion hardwareVersion() const; +#ifndef XSENS_NO_PORT_NUMBERS + virtual int XSNOLINUXEXPORT portNumber() const; +#endif + virtual bool startRecording(); + virtual bool triggerStartRecording(); + virtual bool stopRecording(); + int64_t getStartRecordingPacketId() const; + int64_t getStopRecordingPacketId() const; + + virtual void setOptions(XsOption enable, XsOption disable); + virtual bool areOptionsEnabled(XsOption options) const; + XsOption getOptions() const; + + virtual bool sendCustomMessage(const XsMessage& messageSend, bool waitForResult, XsMessage& messageReceive, int timeout = 0); + virtual bool sendRawMessage(const XsMessage& message); + + 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); + virtual std::shared_ptr addReplyObject(XsXbusMessageId messageId, uint8_t data); + + 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 handleWakeupMessage(const XsMessage& msg); + + virtual bool setSerialBaudRate(XsBaudRate baudrate); + + virtual XsIntArray portConfiguration() const; + virtual bool setPortConfiguration(XsIntArray& config); + + virtual bool isMotionTracker() const; + + virtual XsOperationalMode operationalMode() const; + virtual bool setOperationalMode(XsOperationalMode mode); + + virtual int updateRate() const; + virtual bool setUpdateRate(int rate); + + virtual XsDeviceOptionFlag deviceOptionFlags() const; + virtual bool setDeviceOptionFlags(XsDeviceOptionFlag setFlags, XsDeviceOptionFlag clearFlags); + + virtual XsOutputConfigurationArray outputConfiguration() const; + virtual XsOutputConfigurationArray processedOutputConfiguration() const; + bool setOutputConfiguration(XsOutputConfigurationArray& config); + virtual bool isInStringOutputMode() const; + virtual XsCanOutputConfigurationArray canOutputConfiguration() const; + virtual bool setCanOutputConfiguration(XsCanOutputConfigurationArray& config); + virtual uint32_t canConfiguration() const; + virtual bool setCanConfiguration(uint32_t config); + + virtual bool usesLegacyDeviceMode() const; + + virtual uint16_t stringOutputType() const; + virtual uint16_t stringSamplePeriod() const; + virtual uint16_t stringSkipFactor() const; + + virtual bool setStringOutputMode(uint16_t type, uint16_t period, uint16_t skipFactor); + virtual XsStringOutputTypeArray supportedStringOutputTypes() const; + + virtual int dataLength() const; + + virtual XsSyncSettingArray syncSettings() const; + virtual bool setSyncSettings(const XsSyncSettingArray& settingList); + virtual bool isSyncMaster() const; + virtual bool isSyncSlave() const; + virtual XsSyncSettingArray supportedSyncSettings() const; + static XsSyncSettingArray supportedSyncSettings(XsDeviceId const& deviceId); + + virtual bool gotoMeasurement(); + virtual bool gotoConfig(); + + virtual bool restoreFactoryDefaults(); + + virtual bool reset(); + XSNOEXPORT virtual bool reset(bool skipDeviceIdCheck); + + virtual bool reopenPort(bool gotoConfig, bool skipDeviceIdCheck = false); + + virtual void writeDeviceSettingsToFile(); + + virtual void flushInputBuffers(); + + virtual XsSyncRole syncRole() const; + virtual bool loadLogFile(); + virtual bool abortLoadLogFile(); + virtual XsString logFileName() const; + virtual bool resetOrientation(XsResetMethod resetmethod); + virtual bool resetLogFileReadPosition(); + XsFilePos logFileSize() const; + XsFilePos logFileReadPosition() const; + virtual bool updateCachedDeviceInformation(); + virtual bool enableProtocol(XsProtocolType protocol); + virtual bool disableProtocol(XsProtocolType protocol); + virtual bool isProtocolEnabled(XsProtocolType protocol) const; + + virtual uint32_t deviceBufferSize(); + virtual bool setDeviceBufferSize(uint32_t frames); + + virtual XsConnectivityState connectivityState() const; + virtual void waitForAllDevicesInitialized(); + + // MtContainer + virtual std::vector children() const; + virtual int childCount() const; + + // Awinda Station + virtual bool enableRadio(int channel); + virtual bool disableRadio(); + virtual int radioChannel() const; + virtual bool isRadioEnabled() const; + virtual bool makeOperational(); + virtual bool isOperational() const; + virtual bool isInSyncStationMode(); + virtual bool setSyncStationMode(bool enabled); + + virtual bool stealthMode() const; + virtual bool setStealthMode(bool enabled); + + virtual void discardRetransmissions(int64_t firstNewPacketId); + + //virtual int radioQualityIndication() const; + XSNOEXPORT virtual void handleMasterIndication(const XsMessage& message); + virtual bool abortFlushing(); + virtual bool setDeviceAccepted(const XsDeviceId& deviceId); + virtual bool setDeviceRejected(const XsDeviceId& deviceId); + virtual bool setAccessControlMode(XsAccessControlMode mode, const XsDeviceIdArray& initialList); + virtual XsAccessControlMode accessControlMode() const; + virtual XsDeviceIdArray currentAccessControlList() const; + + virtual XsResultValue setDeviceParameter(XsDeviceParameter const& parameter); + virtual XsResultValue deviceParameter(XsDeviceParameter& parameter) const; + + XSNOEXPORT XSDEPRECATED XsGnssPlatform gnssPlatform() const; + XSNOEXPORT XSDEPRECATED virtual bool setGnssPlatform(XsGnssPlatform gnssPlatform); + virtual XsUbloxGnssPlatform ubloxGnssPlatform() const; + virtual bool setUbloxGnssPlatform(XsUbloxGnssPlatform ubloxGnssPlatform); + virtual XsIntArray gnssReceiverSettings() const; + virtual bool setGnssReceiverSettings(const XsIntArray& gnssReceiverSettings); + + // MTw + virtual bool acceptConnection(); + virtual bool rejectConnection(); + virtual int wirelessPriority() const; + virtual bool setWirelessPriority(int priority); + virtual XsRejectReason rejectReason() const; + + virtual bool requestBatteryLevel(); + virtual XsTimeStamp batteryLevelTime(); + virtual bool setTransportMode(bool transportModeEnabled); + virtual bool transportMode(); + virtual int16_t lastKnownRssi() const; + XSNOEXPORT virtual void setPacketErrorRate(int per); + virtual int packetErrorRate() const; + + virtual bool isBlueToothEnabled() const; + virtual bool setBlueToothEnabled(bool enabled); + virtual bool isBusPowerEnabled() const; + virtual bool setBusPowerEnabled(bool enabled); + virtual bool powerDown(); + virtual XsErrorMode errorMode() const; + virtual bool setErrorMode(XsErrorMode errormode); + + // MT device + virtual bool setHeadingOffset(double offset); + virtual double headingOffset() const; + virtual bool setLocationId(int id); + virtual int locationId() const; + virtual XsDevice* getDeviceFromLocationId(uint16_t locId); + virtual XsMatrix objectAlignment() const; + virtual bool setObjectAlignment(const XsMatrix& matrix); + virtual double gravityMagnitude() const; + virtual bool setGravityMagnitude(double mag); + virtual XsVector initialPositionLLA() const; + virtual bool setInitialPositionLLA(const XsVector& lla); + virtual XsTimeInfo utcTime() const; + virtual bool setUtcTime(const XsTimeInfo& time); + virtual bool reinitialize(); + + virtual XsFilterProfile xdaFilterProfile() const; + virtual bool setXdaFilterProfile(int profileType); + virtual bool setXdaFilterProfile(XsString const& profileType); + virtual XsFilterProfile onboardFilterProfile() const; + virtual bool setOnboardFilterProfile(int profileType); + virtual bool setOnboardFilterProfile(XsString const& profileType); + virtual bool replaceFilterProfile(XsFilterProfile const& profileCurrent, XsFilterProfile const& profileNew); + virtual XsFilterProfileArray availableOnboardFilterProfiles() const; + virtual XsFilterProfileArray availableXdaFilterProfiles() const; + virtual double accelerometerRange() const; + virtual double gyroscopeRange() const; + virtual bool setNoRotation(uint16_t duration); + virtual bool startRepresentativeMotion(); + virtual bool representativeMotionState(); + virtual XsIccRepMotionResult stopRepresentativeMotion(); + virtual bool storeIccResults(); + virtual uint16_t rs485TransmissionDelay() const; + virtual bool setRs485TransmissionDelay(uint16_t delay); + virtual XsSelfTestResult runSelfTest(); + virtual bool requestData(); + virtual bool storeFilterState(); + + virtual XsDataPacket getDataPacketByIndex(XsSize index) const; + XsSize getDataPacketCount() const; + XsDataPacket lastAvailableLiveData() const; + XsDataPacket takeFirstDataPacketInQueue(); + + // MTix device + virtual bool isInitialBiasUpdateEnabled() const; + virtual bool setInitialBiasUpdateEnabled(bool enable); + virtual bool isFixedGravityEnabled() const; + virtual bool setFixedGravityEnabled(bool enable); + + virtual XsResultValue createConfigFile(const XsString& filename); + virtual XsResultValue applyConfigFile(const XsString& filename); + + // MtMk4 device + virtual bool setAlignmentRotationMatrix(XsAlignmentFrame frame, const XsMatrix& matrix); + virtual XsMatrix alignmentRotationMatrix(XsAlignmentFrame frame) const; + virtual bool setAlignmentRotationQuaternion(XsAlignmentFrame frame, const XsQuaternion& quat); + virtual XsQuaternion alignmentRotationQuaternion(XsAlignmentFrame frame) const; + + //! \returns The device mutex. + xsens::GuardedMutex* mutex() const + { + return &m_deviceMutex; + } + + virtual bool deviceIsDocked(XsDevice* dev) const; + + bool isLoadLogFileInProgress() const; + void waitForLoadLogFileDone() const; + + XSNOEXPORT virtual bool messageLooksSane(const XsMessage& msg) const; + XSNOEXPORT virtual void prepareForTermination(); + + // MTi with RTK support + virtual bool setGnssLeverArm(const XsVector& arm); + virtual XsVector gnssLeverArm() const; + XSNOEXPORT virtual bool requestUtcTime(); + + // Snapshot related + XSNOEXPORT virtual void handleUnavailableData(int64_t frameNumber); + + XSNOEXPORT virtual bool writeEmtsPage(uint8_t const* data, int pageNr, int bankNr); + XSNOEXPORT void setSkipEmtsReadOnInit(bool skip); + + XSNOEXPORT virtual bool readEmtsAndDeviceConfiguration(); + + virtual uint32_t supportedStatusFlags() const; + virtual XsByteArray readMetaDataFromLogFile(); + +protected: + virtual XsDevice const* firstChild() const; + virtual void setRecordingStartFrame(uint16_t startFrame); + virtual void setRecordingStopFrame(uint16_t stopFrame); + + /*! \brief Uses log interface for a given data logger + \param logger The data logger + \details For testing purposes + */ + void useLogInterface(DataLogger* logger); + + friend struct XsDeviceEx; + /*! \private @{ */ + void setFirmwareVersion(const XsVersion& version); + void extractFirmwareVersion(XsMessage const& message); + + friend class MtContainer; + virtual void setDeviceState(XsDeviceState state); + virtual void updateDeviceState(XsDeviceState state); + + void removeIfNoRefs(); + + virtual bool scheduleOrientationReset(XsResetMethod method); + + explicit XsDevice(XsDeviceId const& id); + explicit XsDevice(Communicator* comm); + explicit XsDevice(XsDevice* master, const XsDeviceId& childDeviceId); + + /*! \return A const reference to the cached device configuration */ + inline const XsDeviceConfiguration& deviceConfig() const + { + return m_config; + } + + void setDeviceId(const XsDeviceId& deviceId); + + XsOutputConfiguration findConfiguration(XsDataIdentifier dataType) const; + + virtual void writeMessageToLogFile(const XsMessage& message); + + virtual void writeFilterStateToFile(); + + virtual void processLivePacket(XsDataPacket& pack); + virtual void processBufferedPacket(XsDataPacket& pack); + + virtual bool readDeviceConfiguration(); + + /*! \return A reference to the cached latest received packet */ + inline XsDataPacket& latestLivePacket() + { + 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()); + 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()); + 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()); + return *m_latestBufferedPacket; + } + virtual int64_t latestLivePacketId() const; + virtual int64_t latestBufferedPacketId() const; + + virtual void resetPacketStamping(); + + void updateConnectivityState(XsConnectivityState newState); + virtual XsConnectivityState defaultChildConnectivityState() const; + + //! \brief Set the initialized state to \a 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; } + + 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 onSessionRestarted(); + XSNOEXPORT virtual void onConnectionLost(); + XSNOEXPORT virtual void onEofReached(); + XSNOEXPORT virtual void onWirelessConnectionLost(); + 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; + + //! \return The value of the m_justWriteSetting flag, which is used in file-based processing + bool justWriteSetting() const + { + return m_justWriteSetting; + } + + virtual void clearProcessors(); + virtual void clearDataCache(); + virtual void insertIntoDataCache(int64_t pid, XsDataPacket* pack); + virtual void reinitializeProcessors(); + virtual bool expectingRetransmissionForPacket(int64_t packetId) const; + + virtual bool resetRemovesPort() const; + + virtual bool isSoftwareFilteringEnabled() const; + virtual bool isSoftwareCalibrationEnabled() const; + + + virtual void setStartRecordingPacketId(int64_t startFrame); + virtual void setStopRecordingPacketId(int64_t stopFrame); + virtual void endRecordingStream(); + virtual void clearCacheToRecordingStart(); + + /*! @} */ + + //! The mutex for guarding state changes of the device + mutable xsens::GuardedMutex m_deviceMutex; + + //! The mutex for guarding access to the log file + mutable xsens::Mutex m_logFileMutex; + + //! \brief A copy of the latest ready live packet. This is the packet with the highest 64-bit sample counter so far. Use latestLivePacket() to access. + XsDataPacket* m_latestLivePacket; + + //! \brief A copy of the latest ready recording packet. This is the last packet that was popped off the front of m_dataCache. Use latestBufferedPacket() to access. + XsDataPacket* m_latestBufferedPacket; + + 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; + virtual bool shouldDoRecordedCallback(XsDataPacket const& packet) const; + virtual bool interpolateMissingData(XsDataPacket const& pack, XsDataPacket const& prev, std::function packetHandler); + + //! \brief A data cache + DataPacketCache m_dataCache; + + //! \brief A packet ID of the last sample we know to be unavailable + int64_t m_unavailableDataBoundary; + + //! \brief An ID of the device + XsDeviceId m_deviceId; + + //! \brief The last result of an operation + mutable LastResultManager m_lastResult; + + //! \brief A current device state + XsDeviceState m_state; + + //! \brief A current device connectivity state + XsConnectivityState m_connectivity; + + //! \brief A time stamp for an OK last data + XsTimeStamp m_lastDataOkStamp; + + //! \brief A device configuration + XsDeviceConfiguration m_config; + + //! \brief A devices output configuration + XsOutputConfigurationArray m_outputConfiguration; + + //! \brief A firmware version + XsVersion m_firmwareVersion; + + //! \brief A communicator + Communicator* m_communicator; + + //! \brief A data logger for a file interface + DataLogger* m_logFileInterface; + + //! \brief A device object + XsDevice* m_master; + + //! \brief A reference counter + volatile std::atomic_int m_refCounter; + + //! \brief Write to file boolean variable + bool m_writeToFile; + + //! \brief Is intialized boolean variable + bool m_isInitialized; + + //! \brief Termination prepared boolean variable + bool m_terminationPrepared; + + //! \brief Go to confing on close boolean variable + bool m_gotoConfigOnClose; + + //! \brief Just write setting boolean variable + bool m_justWriteSetting; + + /*! \brief Skip EMTS read on init boolean variable + \details Required for the firmware updater to retain EMTS while rebooting devices + */ + bool m_skipEmtsReadOnInit; + + //! \brief A packet stamper + PacketStamper m_packetStamper; + + //! \brief The options + XsOption m_options; + + /*! \brief An EMTS blob from device. + \note Used in public source for storing EMTS for log file header. + */ + XsMessage m_emtsBlob; + + //These members are protected instead of private to give direct access to tests + + //! \brief The ID of the first packet that should be / was recorded + int64_t m_startRecordingPacketId; + + //! \brief The ID of the last packet that should be / was recorded. Only valid in Recording/Flushing states + int64_t m_stopRecordingPacketId; + + //! \brief The ID of the last packet that was recorded. Remains valid after Flushing has ended, until a new recording is started. + int64_t m_stoppedRecordingPacketId; + + virtual void clearExternalPacketCaches(); + void updateLastAvailableLiveDataCache(XsDataPacket const& pack); + void retainPacket(XsDataPacket const& pack); + static bool packetContainsRetransmission(XsDataPacket const& pack); + + //! \brief A linear data packet cache + std::deque m_linearPacketCache; + + //! \brief A last available live data cache + XsDataPacket* m_lastAvailableLiveDataCache; + + /*! \brief To a dump file. + \details For debugging purposes only, but doesn't do any harm to always be there. + */ + DebugFileType* m_toaDumpFile; + + XSENS_DISABLE_COPY(XsDevice); +}; + +#ifndef XDA_PRIVATE_BUILD + #include "xsdevice_public.h" +#else + #include "xsdeviceex.h" +#endif + +#endif diff --git a/extern/xspublic/xscontroller/xsdevice_public.h b/extern/xspublic/xscontroller/xsdevice_public.h new file mode 100644 index 0000000..e4b8a70 --- /dev/null +++ b/extern/xspublic/xscontroller/xsdevice_public.h @@ -0,0 +1,62 @@ + +// Copyright (c) 2003-2022 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 XSDEVICE_PUBLIC_H +#define XSDEVICE_PUBLIC_H + +#include "xsdevice_def.h" + +namespace xsens +{ +class XSNOEXPORT SettingsFile; +} + +/*! \class XsDeviceEx + \brief An abstract internal struct of a device +*/ +struct XsDeviceEx : public XsDevice +{ +protected: + //! Construct a device using \a comm for communication + explicit XsDeviceEx(Communicator* comm) : XsDevice(comm) {} + + //! Construct a device using a device id \a id + 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) {} + + //! Destroy the device + virtual ~XsDeviceEx() {} +}; + +#endif diff --git a/extern/xspublic/xscontroller/xsdeviceconfiguration.c b/extern/xspublic/xscontroller/xsdeviceconfiguration.c new file mode 100644 index 0000000..8da47b4 --- /dev/null +++ b/extern/xspublic/xscontroller/xsdeviceconfiguration.c @@ -0,0 +1,261 @@ + +// Copyright (c) 2003-2022 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 "xsdeviceconfiguration.h" +#include +#include +#include "xsdef.h" +#include +#include + +#define INC_ALLOC ((void) 0) +#define INC_FREE ((void) 0) + +/*! \addtogroup cinterface C Interface + @{ +*/ + +/*! \relates XsDeviceConfiguration + \brief Initializes the %XsDeviceConfiguration object +*/ +void XsDeviceConfiguration_construct(XsDeviceConfiguration* thisPtr) +{ + memset(thisPtr, 0, sizeof(XsDeviceConfiguration)); +} + +/*! \relates XsDeviceConfiguration + \brief Reinitializes the %XsDeviceConfiguration with space for \a numberOfDevices devices and copies them from \a src + \details This function reinitializes the object reserving space for \a numberOfDevices devices in the buffer. \a size may + be 0. If \a src is not null, \a numberOfDevices bytes from \a src will be copied. + Previous data will be cleared and freed automatically. + + \param numberOfDevices : The number of devices for which memory should be allocated in the XsDeviceConfiguration + \param src : A source XsDeviceConfiguration object that will be used to initialize the new object. May be null +*/ +void XsDeviceConfiguration_assign(XsDeviceConfiguration* thisPtr, XsSize numberOfDevices, const XsDeviceConfiguration* src) +{ + if (numberOfDevices > thisPtr->m_numberOfDevices || numberOfDevices == 0) + { + XsDeviceConfiguration_destruct(thisPtr); + if (numberOfDevices) + { + // init to size + *((XsMtDeviceConfiguration**) &thisPtr->m_deviceInfo) = (XsMtDeviceConfiguration*) malloc(numberOfDevices * sizeof(XsMtDeviceConfiguration)); + INC_ALLOC; + } + } + assert(numberOfDevices != 0 || thisPtr->m_deviceInfo == 0); + + *((uint16_t*) &thisPtr->m_numberOfDevices) = (uint16_t) numberOfDevices; + + if (src) + { + memcpy(&thisPtr->m_masterInfo, &src->m_masterInfo, sizeof(XsMasterDeviceConfiguration)); + if (numberOfDevices) + memcpy(thisPtr->m_deviceInfo, src->m_deviceInfo, numberOfDevices * sizeof(XsMtDeviceConfiguration)); + } +} + +/*! \relates XsDeviceConfiguration + \brief Clears and frees data in the %XsDeviceConfiguration */ +void XsDeviceConfiguration_destruct(XsDeviceConfiguration* thisPtr) +{ + if (thisPtr->m_deviceInfo) + { + // clear contents + free(thisPtr->m_deviceInfo); + INC_FREE; + } + // init to 0 + *((XsMtDeviceConfiguration**) &thisPtr->m_deviceInfo) = 0; + *((uint16_t*) &thisPtr->m_numberOfDevices) = 0; +} + +/*! \relates XsDeviceConfiguration + \brief Copy the %XsDeviceConfiguration to \a copy */ +void XsDeviceConfiguration_copy(XsDeviceConfiguration* copy, XsDeviceConfiguration const* src) +{ + if (copy == src) + return; + XsDeviceConfiguration_assign(copy, src->m_numberOfDevices, src); +} + +/*! \relates XsDeviceConfiguration + \brief Returns true if the XsDeviceConfiguration is empty + \returns Returns true if the XsDeviceConfiguration is empty +*/ +int XsDeviceConfiguration_empty(const XsDeviceConfiguration* thisPtr) +{ + return thisPtr->m_numberOfDevices == 0; +} + +/*! \relates XsDeviceConfiguration + \brief Sets up a XsDeviceConfiguration based upon \a msg + + \param msg : The XsMessage to use to set up the XsDeviceConfiguration object +*/ +void XsDeviceConfiguration_readFromMessage(XsDeviceConfiguration* thisPtr, const XsMessage* msg) +{ + uint16_t i, nDevs; + + assert(XsMessage_getConstHeader(msg)->m_messageId == XMID_Configuration); + + 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, 0}; + if (XsDeviceId_isLegacyDeviceId(&deviceId)) + { + thisPtr->m_masterInfo.m_samplingPeriod = XsMessage_getDataShort(msg, 4); + thisPtr->m_masterInfo.m_outputSkipFactor = XsMessage_getDataShort(msg, 6); + memset(thisPtr->m_masterInfo.m_productCode, 0, XS_LEN_PRODUCTCODE * sizeof(thisPtr->m_masterInfo.m_productCode[0])); + } + else + { + thisPtr->m_masterInfo.m_masterDeviceId |= ((uint64_t)XsMessage_getDataLong(msg, 4) << 32); + memcpy(thisPtr->m_masterInfo.m_productCode, XsMessage_getDataBuffer(msg, 32), 20); + } + } + memcpy(thisPtr->m_masterInfo.m_reserved1, XsMessage_getDataBuffer(msg, 8), 8); + memcpy(thisPtr->m_masterInfo.m_date, XsMessage_getDataBuffer(msg, 16), 8); + memcpy(thisPtr->m_masterInfo.m_time, XsMessage_getDataBuffer(msg, 24), 8); + memcpy(thisPtr->m_masterInfo.m_reserved2, XsMessage_getDataBuffer(msg, 52), 44); + + 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, 0}; + if (XsDeviceId_isLegacyDeviceId(&deviceId)) + { + 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); + 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); + } +} + +/*! \relates XsDeviceConfiguration + \brief Creates a message \a msg that represents the %XsDeviceConfiguration + + \param msg : An XsMessage that will represent the %XsDeviceConfiguration +*/ +void XsDeviceConfiguration_writeToMessage(const XsDeviceConfiguration* thisPtr, XsMessage* msg) +{ + uint16_t i; + XsMessageHeader* msgHeader; + + XsMessage_constructSized(msg, 98 + thisPtr->m_numberOfDevices * 20); + msgHeader = XsMessage_getHeader(msg); + msgHeader->m_messageId = XMID_Configuration; + msgHeader->m_busId = XS_BID_MASTER; + + { + 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); + } + 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_setDataBuffer(msg, thisPtr->m_masterInfo.m_reserved1, 8, 8); + XsMessage_setDataBuffer(msg, thisPtr->m_masterInfo.m_date, 8, 16); + XsMessage_setDataBuffer(msg, thisPtr->m_masterInfo.m_time, 8, 24); + XsMessage_setDataBuffer(msg, thisPtr->m_masterInfo.m_productCode, 20, 32); + XsMessage_setDataBuffer(msg, thisPtr->m_masterInfo.m_reserved2, 44, 52); + XsMessage_setDataShort(msg, thisPtr->m_numberOfDevices, 96); + + for (i = 0; i < thisPtr->m_numberOfDevices; ++i) + { + 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); + } + 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_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); +} + +/*! \relates XsDeviceConfiguration + \brief Returns a (naive) bus ID if \a deviceId is available in this configuration, 0 otherwise + + \note The bus ID may not be valid for device communications + + \param deviceId the device ID to look for + \returns the bus ID that matches \a deviceId +*/ +XsSize XsDeviceConfiguration_findDevice(const XsDeviceConfiguration* thisPtr, const XsDeviceId* deviceId) +{ + uint16_t i; + for (i = 0; i < thisPtr->m_numberOfDevices; ++i) + { + if (thisPtr->m_deviceInfo[i].m_deviceId == deviceId->m_deviceId) + return i + 1; + } + return 0; +} +/*! @} */ diff --git a/extern/xspublic/xscontroller/xsdeviceconfiguration.h b/extern/xspublic/xscontroller/xsdeviceconfiguration.h new file mode 100644 index 0000000..936e4fd --- /dev/null +++ b/extern/xspublic/xscontroller/xsdeviceconfiguration.h @@ -0,0 +1,325 @@ + +// Copyright (c) 2003-2022 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 XSDEVICECONFIGURATION_H +#define XSDEVICECONFIGURATION_H + +#include "xscontrollerconfig.h" +#include +#include +#include +#ifdef __cplusplus + #include +#endif + +struct MtwInfo; +struct XsDeviceConfiguration; +struct XsMessage; +struct XsDeviceId; + +#ifdef __cplusplus +extern "C" { +#else +#define XSDEVICEINFO_INITIALIZER { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 } +#define XSDEVICECONFIGURATION_INITIALIZER { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 } +#endif + +XDA_DLL_API void XsDeviceConfiguration_construct(struct XsDeviceConfiguration* thisPtr); +XDA_DLL_API void XsDeviceConfiguration_assign(struct XsDeviceConfiguration* thisPtr, XsSize numberOfDevices, const struct XsDeviceConfiguration* src); +XDA_DLL_API void XsDeviceConfiguration_destruct(struct XsDeviceConfiguration* thisPtr); +XDA_DLL_API void XsDeviceConfiguration_copy(struct XsDeviceConfiguration* copy, struct XsDeviceConfiguration const* src); +XDA_DLL_API int XsDeviceConfiguration_empty(const struct XsDeviceConfiguration* thisPtr); +XDA_DLL_API void XsDeviceConfiguration_readFromMessage(struct XsDeviceConfiguration* thisPtr, const struct XsMessage* msg); +XDA_DLL_API void XsDeviceConfiguration_writeToMessage(const struct XsDeviceConfiguration* thisPtr, struct XsMessage* msg); +XDA_DLL_API XsSize XsDeviceConfiguration_findDevice(const struct XsDeviceConfiguration* thisPtr, const struct XsDeviceId* deviceId); + +#ifdef __cplusplus +} // extern "C" +#endif + +/*! \brief %Device information for MT devices in an XsDeviceConfiguration. */ +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 */ + uint8_t m_fwRevMajor; /*!< \brief The major version of the firmware */ + uint8_t m_fwRevMinor; /*!< \brief The minor version of the firmware */ + uint8_t m_fwRevRevision; /*!< \brief The revision version of the firmware */ + char m_filterType; /*!< \brief The filter type */ + uint8_t m_filterMajor; /*!< \brief The filter major version */ + uint8_t m_filterMinor; /*!< \brief The filter minor version */ +}; + +typedef struct XsMtDeviceConfiguration XsMtDeviceConfiguration; + +/*! \brief Device information for the main device in an XsDeviceConfiguration. */ +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 */ + uint8_t m_reserved1[8]; /*!< \brief Reserved space */ + uint8_t m_date[8]; /*!< \brief The date */ + uint8_t m_time[8]; /*!< \brief The time */ + uint8_t m_productCode[20]; /*!< \brief The master product code */ + uint8_t m_reserved2[44]; /*!< \brief Reserved space */ +}; +typedef struct XsMasterDeviceConfiguration XsMasterDeviceConfiguration; + +#ifdef __cplusplus +/*! \class XsDeviceConfigurationException + Exception class thrown when an exception occurred inside the XsDeviceConfiguration +*/ +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 +{ +#ifdef __cplusplus + /*! \brief Constructor + + \param numberOfDevs : The number of devices for which memory should be allocated in the XsDeviceConfiguration + + \sa XsDeviceConfiguration_construct + */ + explicit XsDeviceConfiguration(uint16_t numberOfDevs = 0) + : m_numberOfDevices(0) + , m_deviceInfo(0) + { + memset(this, 0, sizeof(XsDeviceConfiguration)); + if (numberOfDevs) + XsDeviceConfiguration_assign(this, numberOfDevs, 0); + } + + /*! \brief Copy constructor + \param other the object to copy + \sa XsDeviceConfiguration_copy + */ + XsDeviceConfiguration(const XsDeviceConfiguration& other) + : m_numberOfDevices(0) + , m_deviceInfo(0) + { + memset(this, 0, sizeof(XsDeviceConfiguration)); + XsDeviceConfiguration_copy(this, &other); + } + + /*! \brief Assign \a other to this device configuaration + + \param other the object to copy + + \returns a const reference to this object + \sa XsDeviceConfiguration_copy + */ + inline const XsDeviceConfiguration& operator = (const XsDeviceConfiguration& other) + { + XsDeviceConfiguration_copy(this, &other); + return *this; + } + + /*! \brief Destructor \sa XsDeviceConfiguration_destruct */ + inline ~XsDeviceConfiguration() + { + XsDeviceConfiguration_destruct(this); + } + + /*! \brief Clears and frees data \sa XsDeviceConfiguration_destruct */ + inline void clear() + { + XsDeviceConfiguration_destruct(this); + } + + /*! \brief Test if this object is empty + \returns true if this object is empty + */ + inline bool empty() const + { + return m_numberOfDevices == 0; + } + + /*! \brief \copybrief XsDeviceConfiguration_readFromMessage + + \param msg the message to read the device configuration from + + \sa XsDeviceConfiguration_readFromMessage + */ + inline void readFromMessage(const XsMessage& msg) + { + XsDeviceConfiguration_readFromMessage(this, &msg); + } + + /*! \brief \copybrief XsDeviceConfiguration_writeToMessage + + \param msg the message to write the device configuration to + + \sa XsDeviceConfiguration_writeToMessage + */ + inline void writeToMessage(XsMessage& msg) const + { + XsDeviceConfiguration_writeToMessage(this, &msg); + } + + /*! \brief Return true if this contains device info for \a deviceId + + \param deviceId the device ID to find in this configuration + \returns true if the device is present in the configuration + */ + inline bool containsDevice(const XsDeviceId& deviceId) const + { + return XsDeviceConfiguration_findDevice(this, &deviceId) != 0; + } + + /*! \brief The deviceInfo for the \a deviceId + \param deviceId the device ID to identify with + \returns the deviceInfo structure for \a deviceId + */ + inline XsMtDeviceConfiguration const& deviceInfo(const XsDeviceId& deviceId) const + { + XsSize busId = XsDeviceConfiguration_findDevice(this, &deviceId); + if (busId == 0) + throw XsDeviceConfigurationException(); + + return deviceInfo(busId); + } + + /*! \brief The deviceInfo for the \a deviceId + \param deviceId the device ID to identify with + \returns the deviceInfo structure for \a deviceId + */ + inline XsMtDeviceConfiguration& deviceInfo(const XsDeviceId& deviceId) + { + XsSize busId = XsDeviceConfiguration_findDevice(this, &deviceId); + if (busId == 0) + throw XsDeviceConfigurationException(); + + return deviceInfo(busId); + } + + /*! \brief The device info for the device at \a busId + + \param busId the bus ID of the device for which to return data for + + \returns a reference to the device configuration for the device at \a busId + */ + inline XsMtDeviceConfiguration& deviceInfo(XsSize busId) + { + if (!m_numberOfDevices) + throw XsDeviceConfigurationException(); + + if (busId == XS_BID_MASTER) + return m_deviceInfo[0]; + + if (busId > m_numberOfDevices) + throw XsDeviceConfigurationException(); + + return m_deviceInfo[busId - 1]; + } + + /*! \brief The device info for the device at \a busId + + \param busId the bus ID of the device for which to return data for + + \returns a const reference to the device configuration for the device at \a busId + */ + inline const XsMtDeviceConfiguration& deviceInfo(XsSize busId) const + { + if (!m_numberOfDevices) + throw XsDeviceConfigurationException(); + + if (busId == XS_BID_MASTER) + return m_deviceInfo[0]; + + if (busId > m_numberOfDevices) + throw XsDeviceConfigurationException(); + + return m_deviceInfo[busId - 1]; + } + + + /*! \brief The device info for the master device + + \returns a reference to the device configuration for the master device + */ + inline XsMasterDeviceConfiguration& masterInfo() + { + return m_masterInfo; + } + + /*! \brief The device info for the master device + + \returns a const reference to the device configuration for the master device + */ + inline const XsMasterDeviceConfiguration& masterInfo() const + { + return m_masterInfo; + } + + /*! \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); + } + + /*! \brief The current number of devices + \returns the number of devices + */ + inline XsSize numberOfDevices() const + { + return (XsSize) m_numberOfDevices; + } + + /*! \brief \copybrief numberOfDevices + \copydetails numberOfDevices + */ + inline XsSize deviceCount() const + { + return numberOfDevices(); + } + +private: + //! \protectedsection +#endif + XsMasterDeviceConfiguration m_masterInfo; //!< \brief The master info + const uint16_t m_numberOfDevices; //!< \brief The currently allocated number of devices + XsMtDeviceConfiguration* const m_deviceInfo; //!< \brief The list of device infos +}; + +typedef struct XsDeviceConfiguration XsDeviceConfiguration; + +#endif diff --git a/extern/xspublic/xscontroller/xsdeviceparameter.h b/extern/xspublic/xscontroller/xsdeviceparameter.h new file mode 100644 index 0000000..440224e --- /dev/null +++ b/extern/xspublic/xscontroller/xsdeviceparameter.h @@ -0,0 +1,168 @@ + +// Copyright (c) 2003-2022 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 XSDEVICEPARAMETER_H +#define XSDEVICEPARAMETER_H + +#include "xsdeviceparameteridentifier.h" +#include + +#ifdef __cplusplus +extern "C" { +struct XsDeviceParameter; +} // extern "C" +#endif + +/*! + \class XsDeviceParameter + \brief Class to set and retrieve parameters from a XsDevice object. +*/ +struct XsDeviceParameter +{ +#ifdef __cplusplus + //! \brief Constructor, initializes the object with a parameter \a id. + explicit XsDeviceParameter(XsDeviceParameterIdentifier id) + : m_id(id) + , m_value(0) + { + } + + //! \brief Constructor, initializes the object with a parameter \a id and desired \a value. + explicit XsDeviceParameter(XsDeviceParameterIdentifier id, int value) + : m_id(id) + , m_value(static_cast(value)) + { + } + + //! \brief Returns the current parameter identifier. + XsDeviceParameterIdentifier id() const + { + return m_id; + } + + /*! + \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. + */ + template + void setValue(T value); + +private: +#endif + XsDeviceParameterIdentifier m_id; + int m_value; +}; + + +#ifdef __cplusplus +template<> +//! \copydoc XsDeviceParameter::getValue +inline bool XsDeviceParameter::getValue() const +{ + return m_value > 0; +} + +//! \copydoc XsDeviceParameter::getValue +template<> +inline uint8_t XsDeviceParameter::getValue() const +{ + return static_cast(m_value); +} + +//! \copydoc XsDeviceParameter::getValue +template<> +inline uint16_t XsDeviceParameter::getValue() const +{ + return static_cast(m_value); +} + +//! \copydoc XsDeviceParameter::getValue +template<> +inline uint32_t XsDeviceParameter::getValue() const +{ + return static_cast(m_value); +} + +//! \copydoc XsDeviceParameter::getValue +template<> +inline int XsDeviceParameter::getValue() const +{ + return m_value; +} + +//! \copydoc XsDeviceParameter::setValue +template<> +inline void XsDeviceParameter::setValue(bool value) +{ + m_value = value ? 1 : 0; +} + +//! \copydoc XsDeviceParameter::setValue +template<> +inline void XsDeviceParameter::setValue(uint8_t value) +{ + m_value = static_cast(value); +} + +//! \copydoc XsDeviceParameter::setValue +template<> +inline void XsDeviceParameter::setValue(uint16_t value) +{ + m_value = static_cast(value); +} + +//! \copydoc XsDeviceParameter::setValue +template<> +inline void XsDeviceParameter::setValue(uint32_t value) +{ + m_value = static_cast(value); +} + +//! \copydoc XsDeviceParameter::setValue +template<> +inline void XsDeviceParameter::setValue(int value) +{ + m_value = value; +} +#endif + +typedef struct XsDeviceParameter XsDeviceParameter; + +#endif diff --git a/extern/xspublic/xscontroller/xsdeviceparameteridentifier.h b/extern/xspublic/xscontroller/xsdeviceparameteridentifier.h new file mode 100644 index 0000000..b6a49de --- /dev/null +++ b/extern/xspublic/xscontroller/xsdeviceparameteridentifier.h @@ -0,0 +1,57 @@ + +// Copyright (c) 2003-2022 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 XSDEVICEPARAMETERIDENTIFIER_H +#define XSDEVICEPARAMETERIDENTIFIER_H + +////////////////////////////////////////////////////////////////////////////////////////// +/*! \addtogroup enums Global enumerations + @{ +*/ + +/*! \enum XsDeviceParameterIdentifier + \brief Defines the device parameter identifiers. + \sa XsDevice::setDeviceParameter for a detailed description of the following identifiers. +*/ +enum XsDeviceParameterIdentifier +{ + XDPI_Unknown = 0x0000, /*!< Invalid value, set if the desired parameter is not supported. */ + XDPI_PacketErrorRate = 0x0001, + XDPI_ExtendedBuffer = 0x0002, + XDPI_UplinkTimeout = 0x0003, + XDPI_SyncLossTimeout = 0x0004, +}; +/*! @} */ + +typedef enum XsDeviceParameterIdentifier XsDeviceParameterIdentifier; + +#endif diff --git a/extern/xspublic/xscontroller/xsdeviceptr.h b/extern/xspublic/xscontroller/xsdeviceptr.h new file mode 100644 index 0000000..ca1b705 --- /dev/null +++ b/extern/xspublic/xscontroller/xsdeviceptr.h @@ -0,0 +1,42 @@ + +// Copyright (c) 2003-2022 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 XSDEVICEPTR_H +#define XSDEVICEPTR_H + +struct XsDevice; + +/*! \brief A pointer to an XsDevice object +*/ +typedef struct XsDevice* XsDevicePtr; + +#endif diff --git a/extern/xspublic/xscontroller/xsdeviceptrarray.c b/extern/xspublic/xscontroller/xsdeviceptrarray.c new file mode 100644 index 0000000..ce15cda --- /dev/null +++ b/extern/xspublic/xscontroller/xsdeviceptrarray.c @@ -0,0 +1,95 @@ + +// Copyright (c) 2003-2022 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 "xsdeviceptrarray.h" + +/*! \struct XsDevicePtrArray + \brief A list of XsDevicePtr values + \sa XsArray +*/ + +/*! \copydoc XsArrayDescriptor::itemSwap + \note Specialization for XsDevicePtr +*/ +void swapDevicePtr(XsDevicePtr* a, XsDevicePtr* b) +{ + XsDevicePtr tmp = *a; + *a = *b; + *b = tmp; +} + +/*! \copydoc XsArrayDescriptor::itemCopy + \note Specialization for XsDevicePtr +*/ +void copyDevicePtr(XsDevicePtr* to, XsDevicePtr const* from) +{ + *to = *from; +} + +/*! \copydoc XsArrayDescriptor::itemCompare + \note Specialization for XsDevicePtr +*/ +int compareDevicePtr(XsDevicePtr const* a, XsDevicePtr const* b) +{ + if (*a < *b) + return -1; + if (*a > *b) + return 1; + return 0; +} + +//! \brief zero the pointer value +void zeroDevicePtr(XsDevicePtr* a) +{ + *a = 0; +} + +//! \brief Descriptor for XsDevicePtrArray +XsArrayDescriptor const g_xsDevicePtrArrayDescriptor = +{ + sizeof(XsDevicePtr), + XSEXPCASTITEMSWAP swapDevicePtr, // swap + XSEXPCASTITEMMAKE zeroDevicePtr, // construct + (XsArrayItemCopyFunc) copyDevicePtr, // copy construct + XSEXPCASTITEMMAKE zeroDevicePtr, // destruct + (XsArrayItemCopyFunc) copyDevicePtr, // copy + (XsArrayItemCompareFunc) compareDevicePtr, // compare + XSEXPCASTRAWCOPY XsArray_rawCopy +}; + +/*! \copydoc XsArray_constructDerived + \note Specialization for XsDevicePtrArray +*/ +void XsDevicePtrArray_construct(XsDevicePtrArray* thisPtr, XsSize count, XsDevicePtr const* src) +{ + XsArray_construct(thisPtr, &g_xsDevicePtrArrayDescriptor, count, src); +} diff --git a/extern/xspublic/xscontroller/xsdeviceptrarray.h b/extern/xspublic/xscontroller/xsdeviceptrarray.h new file mode 100644 index 0000000..8c466f8 --- /dev/null +++ b/extern/xspublic/xscontroller/xsdeviceptrarray.h @@ -0,0 +1,88 @@ + +// Copyright (c) 2003-2022 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 XSDEVICEPTRARRAY_H +#define XSDEVICEPTRARRAY_H + +#include "xscontrollerconfig.h" +#include "xsdeviceptr.h" +#include + +#ifdef __cplusplus +extern "C" { +#endif + +extern XsArrayDescriptor const XDA_DLL_API g_xsDevicePtrArrayDescriptor; + +#ifndef __cplusplus +#define XSDEVICEPTRARRAY_INITIALIZER XSARRAY_INITIALIZER(&g_xsDevicePtrArrayDescriptor) + +XSARRAY_STRUCT(XsDevicePtrArray, XsDevicePtr); +typedef struct XsDevicePtrArray XsDevicePtrArray; + +XDA_DLL_API void XsDevicePtrArray_construct(XsDevicePtrArray* thisPtr, XsSize count, XsDevicePtr const* src); +#else +} // extern "C" +#endif + +#ifdef __cplusplus + 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 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) + { + } +#endif + }; +#endif +#endif diff --git a/extern/xspublic/xscontroller/xsdevicestate.c b/extern/xspublic/xscontroller/xsdevicestate.c new file mode 100644 index 0000000..4127454 --- /dev/null +++ b/extern/xspublic/xscontroller/xsdevicestate.c @@ -0,0 +1,60 @@ + +// Copyright (c) 2003-2022 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 "xsdevicestate.h" +#include +#include + +/*! \addtogroup cinterface C Interface + @{ +*/ + +/*! \brief Convert the device state to a human readable string */ +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; + } +} + +/*! @} */ diff --git a/extern/xspublic/xscontroller/xsdevicestate.h b/extern/xspublic/xscontroller/xsdevicestate.h new file mode 100644 index 0000000..d0ca3a4 --- /dev/null +++ b/extern/xspublic/xscontroller/xsdevicestate.h @@ -0,0 +1,79 @@ + +// Copyright (c) 2003-2022 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 XSDEVICESTATE_H +#define XSDEVICESTATE_H + +#include "xscontrollerconfig.h" +#include + +/*! \addtogroup enums Global enumerations + @{ +*/ + +//AUTO namespace xscontroller { +/*! \brief XsDevice state identifiers */ +enum XsDeviceState +{ + XDS_Initial, /*!< Initial unknown state */ + XDS_Config, /*!< Configuration mode. */ + XDS_Measurement, /*!< Measurement mode, devices are transmitting data. */ + XDS_WaitingForRecordingStart, /*!< The device is in measurement mode and waiting for an external trigger to go to recording state. \note Awinda Station only */ + XDS_Recording, /*!< Same as measurement mode, but on Awinda systems retransmissions now also occur. \note Only on an Awinda Station is this an actual state in the device. For other devices, the state exists only in XDA. */ + XDS_FlushingData, /*!< The device has been notified that it should stop recording. It is still measuring data and may flush retransmitted data to XDA. When XDA decides that it will not receive any more data that should be recorded, the state will be switched to XDS_Measurement automatically */ + XDS_Destructing /*!< The device is being destructed. After this callback, the device and any references to it are invalid. */ +}; +/*! @} */ +typedef enum XsDeviceState XsDeviceState; +//AUTO } + +#ifdef __cplusplus +extern "C" { +#endif + +/*! \brief Convert the device state to a human readable string */ +XDA_DLL_API const char* XsDeviceState_toString(XsDeviceState s); + +#ifdef __cplusplus +} // extern "C" + +#ifndef XSENS_NO_STL +template +std::basic_ostream<_CharT, _Traits>& operator<<(std::basic_ostream<_CharT, _Traits>& o, XsDeviceState const& xds) +{ + return (o << XsDeviceState_toString(xds)); +} +#endif // XSENS_NO_STL + +#endif //__cplusplus + +#endif diff --git a/extern/xspublic/xscontroller/xserrormode.h b/extern/xspublic/xscontroller/xserrormode.h new file mode 100644 index 0000000..ab213c6 --- /dev/null +++ b/extern/xspublic/xscontroller/xserrormode.h @@ -0,0 +1,53 @@ + +// Copyright (c) 2003-2022 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 XSERRORMODE_H +#define XSERRORMODE_H + +/*! \addtogroup enums Global enumerations + @{ +*/ +/*! \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 +}; +/*! @} */ +typedef enum XsErrorMode XsErrorMode; + +#endif diff --git a/extern/xspublic/xscontroller/xsfloatformat.h b/extern/xspublic/xscontroller/xsfloatformat.h new file mode 100644 index 0000000..82ed7f3 --- /dev/null +++ b/extern/xspublic/xscontroller/xsfloatformat.h @@ -0,0 +1,49 @@ + +// Copyright (c) 2003-2022 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 XSFLOATFORMAT_H +#define XSFLOATFORMAT_H + +/*! \addtogroup enums Global enumerations + @{ +*/ +/*! \brief Legacy floating point / fixed point format options */ +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) +}; +/*! @} */ +typedef enum XsFloatFormat XsFloatFormat; + +#endif diff --git a/extern/xspublic/xscontroller/xsgnssplatform.h b/extern/xspublic/xscontroller/xsgnssplatform.h new file mode 100644 index 0000000..3bdc497 --- /dev/null +++ b/extern/xspublic/xscontroller/xsgnssplatform.h @@ -0,0 +1,41 @@ + +// Copyright (c) 2003-2022 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 XSGNSSPLATFORM_H +#define XSGNSSPLATFORM_H + +#include "xsubloxgnssplatform.h" + +//! \deprecated GNSS platform is a u-blox specific feature, please use the u-blox specific functions +typedef enum XsUbloxGnssPlatform XsGnssPlatform; + +#endif diff --git a/extern/xspublic/xscontroller/xsgnssreceivertype.h b/extern/xspublic/xscontroller/xsgnssreceivertype.h new file mode 100644 index 0000000..4d7cea1 --- /dev/null +++ b/extern/xspublic/xscontroller/xsgnssreceivertype.h @@ -0,0 +1,51 @@ + +// Copyright (c) 2003-2022 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 XSGNSSRECEIVERTYPE_H +#define XSGNSSRECEIVERTYPE_H + +/*! \brief These enums represent various GNSS receiver types that are supported. +*/ +enum XsGnssReceiverType +{ + XGRT_Ublox_Max_M8Q = 0, + XGRT_Generic_Nmea = 1, + XGRT_Ublox_Neo_M8P = 2, + XGRT_Ublox_ZED_F9P = 3, + XGRT_Septentrio_SBF = 4, + XGRT_Trimble_BX992 = 5, + XGRT_None = 0xFFFF +}; + +typedef enum XsGnssReceiverType XsGnssReceiverType; + +#endif diff --git a/extern/xspublic/xscontroller/xsgnssstatus.h b/extern/xspublic/xscontroller/xsgnssstatus.h new file mode 100644 index 0000000..e515c18 --- /dev/null +++ b/extern/xspublic/xscontroller/xsgnssstatus.h @@ -0,0 +1,117 @@ + +// Copyright (c) 2003-2022 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/extern/xspublic/xscontroller/xsicccommand.h b/extern/xspublic/xscontroller/xsicccommand.h new file mode 100644 index 0000000..f378b46 --- /dev/null +++ b/extern/xspublic/xscontroller/xsicccommand.h @@ -0,0 +1,63 @@ + +// Copyright (c) 2003-2022 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 XSICCCOMMAND_H +#define XSICCCOMMAND_H + +#include + +/*! \brief ICC (Inrun Compass Calibration) commands. + \details To be used inside XMID_IccCommand and XMID_IccCommandAck messages +*/ +enum XsIccCommand +{ + XIC_StartRepMotion = 0x00, //!< Indicate to ICC the start of representative motion + XIC_StopRepMotion = 0x01, //!< Indicate to ICC the end of representative motion + XIC_StoreResults = 0x02, //!< Update the stored magnetometer calibration using the ICC estimated calibration values + XIC_RepMotionState = 0x03, //!< Retrieve the current state of the representative motion + XIC_Status = 0x04 //!< Retrieve the current ICC status +}; +typedef enum XsIccCommand XsIccCommand; + +/*! \brief ICC status flag + \details Used for status fields in XMID_IccCommand and XMID_IccCommandAck messages +*/ +enum XsIccStatusFlag +{ + XISF_ddtWarning = 0x01, //!< Indicates magnetic disturbance + XISF_notEnoughData = 0x02, //!< Indicates data during representative motion does not have enough observability for an estimate + XISF_OutputStable = 0x10, //!< Indicates the ICC output is stable and used by the filter + XISF_RepMoActive = 0x20 //!< Indicates ICC is recording a representative motion +}; +typedef enum XsIccStatusFlag XsIccStatusFlag; + +#endif diff --git a/extern/xspublic/xscontroller/xsiccrepmotionresult.h b/extern/xspublic/xscontroller/xsiccrepmotionresult.h new file mode 100644 index 0000000..682e9ec --- /dev/null +++ b/extern/xspublic/xscontroller/xsiccrepmotionresult.h @@ -0,0 +1,84 @@ + +// Copyright (c) 2003-2022 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 XSICCREPMOTIONRESULT_H +#define XSICCREPMOTIONRESULT_H + +#include + +/*! \brief Contains the result of the representative motion processed by ICC +*/ +struct XsIccRepMotionResult +{ + float m_ddtAccuracy; //!< The ddtAccuracy of the In-Run Compass Calibration + uint8_t m_dimension; //!< The dimension of the In-Run Compass Calibration + uint8_t m_status; //!< The status of the In-Run Compass Calibration + +#ifdef __cplusplus + XsIccRepMotionResult() : m_ddtAccuracy(0.0), m_dimension(0), m_status(0) + { + + } + + /*! \brief Copy constructor for a filter profile object + \param other the filter profile object to construct a copy of + */ + XsIccRepMotionResult(const XsIccRepMotionResult& other) + : m_ddtAccuracy(other.m_ddtAccuracy) + , m_dimension(other.m_dimension) + , m_status(other.m_status) + { + } + + //! \returns the ddtAccuracy + inline float ddtAccuracy() const + { + return m_ddtAccuracy; + } + + //! \returns the dimension + inline uint8_t dimension() const + { + return m_dimension; + } + + //! \returns the status + inline uint8_t status() const + { + return m_status; + } +#endif +}; + +typedef struct XsIccRepMotionResult XsIccRepMotionResult; + +#endif diff --git a/extern/xspublic/xscontroller/xslibusb.cpp b/extern/xspublic/xscontroller/xslibusb.cpp new file mode 100644 index 0000000..b51356a --- /dev/null +++ b/extern/xspublic/xscontroller/xslibusb.cpp @@ -0,0 +1,522 @@ + +// Copyright (c) 2003-2022 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 "xslibusb.h" +#include + +#ifdef HAVE_LIBUSB + +/*! \class XsLibUsb + \brief Class for dynamic loading of winusb +*/ +XsLibUsb::XsLibUsb(void) +{ + m_libraryLoader = new XsLibraryLoader(); + initLibrary(); +} + +XsLibUsb::~XsLibUsb(void) +{ + delete m_libraryLoader; +} + +void XsLibUsb::tryLoadLibrary() +{ + if (m_libraryLoader->isLoaded()) + return; + if (m_libraryLoader->load("libusb-1.0.so")) + return; + if (m_libraryLoader->load("libusb1.0.so")) + return; +} + +void XsLibUsb::initLibrary() +{ + tryLoadLibrary(); + + memset(&m_libUsb, 0, sizeof(m_libUsb)); + + if (m_libraryLoader->isLoaded()) + { + m_libUsb.init = (libUSB_init*)m_libraryLoader->resolve("libusb_init"); + m_libUsb.exit = (libUSB_exit*)m_libraryLoader->resolve("libusb_exit"); + m_libUsb.open = (libUSB_open*)m_libraryLoader->resolve("libusb_open"); + m_libUsb.close = (libUSB_close*)m_libraryLoader->resolve("libusb_close"); + m_libUsb.kernel_driver_active = (libUSB_kernel_driver_active*)m_libraryLoader->resolve("libusb_kernel_driver_active"); + m_libUsb.attach_kernel_driver = (libUSB_attach_kernel_driver*)m_libraryLoader->resolve("libusb_attach_kernel_driver"); + m_libUsb.detach_kernel_driver = (libUSB_detach_kernel_driver*)m_libraryLoader->resolve("libusb_detach_kernel_driver"); + m_libUsb.ref_device = (libUSB_ref_device*)m_libraryLoader->resolve("libusb_ref_device"); + m_libUsb.unref_device = (libUSB_unref_device*)m_libraryLoader->resolve("libusb_unref_device"); + m_libUsb.claim_interface = (libUSB_claim_interface*)m_libraryLoader->resolve("libusb_claim_interface"); + m_libUsb.release_interface = (libUSB_release_interface*)m_libraryLoader->resolve("libusb_release_interface"); + m_libUsb.get_active_config_descriptor = (libUSB_get_active_config_descriptor*)m_libraryLoader->resolve("libusb_get_active_config_descriptor"); + m_libUsb.free_config_descriptor = (libUSB_free_config_descriptor*)m_libraryLoader->resolve("libusb_free_config_descriptor"); + m_libUsb.get_bus_number = (libUSB_get_bus_number*)m_libraryLoader->resolve("libusb_get_bus_number"); + m_libUsb.get_device = (libUSB_get_device*)m_libraryLoader->resolve("libusb_get_device"); + m_libUsb.get_device_address = (libUSB_get_device_address*)m_libraryLoader->resolve("libusb_get_device_address"); + m_libUsb.get_device_descriptor = (libUSB_get_device_descriptor*)m_libraryLoader->resolve("libusb_get_device_descriptor"); + m_libUsb.get_device_list = (libUSB_get_device_list*)m_libraryLoader->resolve("libusb_get_device_list"); + m_libUsb.free_device_list = (libUSB_free_device_list*)m_libraryLoader->resolve("libusb_free_device_list"); + m_libUsb.get_string_descriptor_ascii = (libUSB_get_string_descriptor_ascii*)m_libraryLoader->resolve("libusb_get_string_descriptor_ascii"); + m_libUsb.bulk_transfer = (libUSB_bulk_transfer*)m_libraryLoader->resolve("libusb_bulk_transfer"); + m_libUsb.set_debug = (libUSB_set_debug*)m_libraryLoader->resolve("libusb_set_debug"); + } +} + +/*! \brief Initialize libusb. This function must be called before calling any other libusb function. + + If you do not provide an output location for a context pointer, a default + context will be created. If there was already a default context, it will + be reused (and nothing will be initialized/reinitialized). + + \param context Optional output location for context pointer. + Only valid on return code 0. + \returns 0 on success, or a LIBUSB_ERROR code on failure +*/ +int XsLibUsb::init(libusb_context** ctx) +{ + if (m_libUsb.init) + return m_libUsb.init(ctx); + else + return LIBUSB_ERROR_NOT_SUPPORTED; +} + +/*! \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) +{ + if (m_libUsb.exit) + m_libUsb.exit(ctx); +} + +/*! \brief Open a device and obtain a device handle. A handle allows you to perform I/O on the device in question. + + Internally, this function adds a reference to the device and makes it available to you through libusb_get_device(). + This reference is removed during libusb_close(). + + This is a non-blocking function; no requests are sent over the bus. + + \param dev the device to open + \param handle output location for the returned device handle pointer. Only populated when the return code is 0. + + \returns 0 on success + \returns LIBUSB_ERROR_NO_MEM on memory allocation failure + \returns LIBUSB_ERROR_ACCESS if the user has insufficient permissions + \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) +{ + if (m_libUsb.open) + return m_libUsb.open(dev, handle); + else + return LIBUSB_ERROR_NOT_SUPPORTED; +} + +/*! \brief Close a device handle. Should be called on all open handles before your application exits. + Internally, this function destroys the reference that was added by libusb_open() on the given device. + + 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) +{ + if (m_libUsb.close) + m_libUsb.close(dev_handle); +} + +/*! \brief Determine if a kernel driver is active on an interface. + If a kernel driver is active, you cannot claim the interface, and libusb will be unable to perform I/O. + + This functionality is not available on Windows. + + \param dev a device handle + \param interface_number the interface to check + + \returns 0 if no kernel driver is active + \returns 1 if a kernel driver is active + \returns LIBUSB_ERROR_NO_DEVICE if the device has been disconnected + \returns LIBUSB_ERROR_NOT_SUPPORTED on platforms where the functionality is not available + \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) +{ + if (m_libUsb.kernel_driver_active) + return m_libUsb.kernel_driver_active(dev, interface_number); + else + return LIBUSB_ERROR_NOT_SUPPORTED; +} + +/** \brief Re-attach an interface's kernel driver, which was previously detached using libusb_detach_kernel_driver(). + This call is only effective on Linux and returns LIBUSB_ERROR_NOT_SUPPORTED on all other platforms. + + This functionality is not available on Darwin or Windows. + + \param dev a device handle + \param interface_number the interface to attach the driver from + + \returns 0 on success + \returns LIBUSB_ERROR_NOT_FOUND if no kernel driver was active + \returns LIBUSB_ERROR_INVALID_PARAM if the interface does not exist + \returns LIBUSB_ERROR_NO_DEVICE if the device has been disconnected + \returns LIBUSB_ERROR_NOT_SUPPORTED on platforms where the functionality is not available + \returns LIBUSB_ERROR_BUSY if the driver cannot be attached because the interface is claimed by a program or driver + \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) +{ + if (m_libUsb.attach_kernel_driver) + return m_libUsb.attach_kernel_driver(dev, interface_number); + else + return LIBUSB_ERROR_NOT_SUPPORTED; +} + +/*! \brief Detach a kernel driver from an interface. If successful, you will then be able to claim the interface and perform I/O. + + This functionality is not available on Darwin or Windows. + + \param dev a device handle + \param interface_number the interface to detach the driver from + + \returns 0 on success + \returns LIBUSB_ERROR_NOT_FOUND if no kernel driver was active + \returns LIBUSB_ERROR_INVALID_PARAM if the interface does not exist + \returns LIBUSB_ERROR_NO_DEVICE if the device has been disconnected + \returns LIBUSB_ERROR_NOT_SUPPORTED on platforms where the functionality is not available + \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) +{ + if (m_libUsb.detach_kernel_driver) + return m_libUsb.detach_kernel_driver(dev, interface_number); + else + return LIBUSB_ERROR_NOT_SUPPORTED; +} + +/*! \brief Increment the reference count of a device. + \param dev the device to reference + \returns the same device +*/ +libusb_device* XsLibUsb::ref_device(libusb_device* dev) +{ + if (m_libUsb.ref_device) + return m_libUsb.ref_device(dev); + else + return NULL; +} + +/*! \brief Decrement the reference count of a device. + 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) +{ + if (m_libUsb.unref_device) + m_libUsb.unref_device(dev); +} + +/*! \brief Claim an interface on a given device handle. + You must claim the interface you wish to use before you can perform I/O on any of its endpoints. + + It is legal to attempt to claim an already-claimed interface, in which + case libusb just returns 0 without doing anything. + + Claiming of interfaces is a purely logical operation; it does not cause + any requests to be sent over the bus. Interface claiming is used to + instruct the underlying operating system that your application wishes + to take ownership of the interface. + + This is a non-blocking function. + + \param dev a device handle + \param interface_number the \a bInterfaceNumber of the interface you wish to claim + \returns 0 on success + \returns LIBUSB_ERROR_NOT_FOUND if the requested interface does not exist + \returns LIBUSB_ERROR_BUSY if another program or driver has claimed the interface + \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) +{ + if (m_libUsb.claim_interface) + return m_libUsb.claim_interface(dev, interface_number); + else + return LIBUSB_ERROR_NOT_SUPPORTED; +} + +/*! \brief Release an interface previously claimed with libusb_claim_interface(). + You should release all claimed interfaces before closing a device handle. + + This is a blocking function. A SET_INTERFACE control request will be sent + to the device, resetting interface state to the first alternate setting. + + \param dev a device handle + \param interface_number the \a bInterfaceNumber of the previously-claimed interface + \returns 0 on success + \returns LIBUSB_ERROR_NOT_FOUND if the interface was not claimed + \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) +{ + if (m_libUsb.release_interface) + return m_libUsb.release_interface(dev, interface_number); + else + return LIBUSB_ERROR_NOT_SUPPORTED; +} + +/*! \brief Get the USB configuration descriptor for the currently active configuration. + + This is a non-blocking function which does not involve any requests being + sent to the device. + + \param dev a device + \param config output location for the USB configuration descriptor. + Only valid if 0 was returned. Must be freed with libusb_free_config_descriptor() after use. + \returns 0 on success + \returns LIBUSB_ERROR_NOT_FOUND if the device is in unconfigured state + \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) +{ + if (m_libUsb.get_active_config_descriptor) + return m_libUsb.get_active_config_descriptor(dev, config); + else + return LIBUSB_ERROR_NOT_SUPPORTED; +} + +/*! \brief Free a configuration descriptor obtained from libusb_get_active_config_descriptor() or libusb_get_config_descriptor(). + + It is safe to call this function with a NULL config parameter, in which case the function simply returns. + + \param config the configuration descriptor to free +*/ +void XsLibUsb::free_config_descriptor(struct libusb_config_descriptor* config) +{ + if (m_libUsb.free_config_descriptor) + m_libUsb.free_config_descriptor(config); +} + +/*! \brief Get the number of the bus that a device is connected to. + \param dev a device + \returns the bus number +*/ +uint8_t XsLibUsb::get_bus_number(libusb_device* dev) +{ + if (m_libUsb.get_bus_number) + return m_libUsb.get_bus_number(dev); + else + return 0; +} + +/*! \brief Get the underlying device for a handle. + + This function does not modify the reference count of the returned device, + so do not feel compelled to unreference it when you are done. + \param dev_handle a device handle + \returns the underlying device +*/ +libusb_device* XsLibUsb::get_device(libusb_device_handle* dev_handle) +{ + if (m_libUsb.get_device) + return m_libUsb.get_device(dev_handle); + else + return NULL; +} + +/*! \brief Get the address of the device on the bus it is connected to. + \param dev a device + \returns the device address +*/ +uint8_t XsLibUsb::get_device_address(libusb_device* dev) +{ + if (m_libUsb.get_device_address) + return m_libUsb.get_device_address(dev); + else + return 0; +} + +/*! \brief Get the USB device descriptor for a given device. + + This is a non-blocking function; the device descriptor is cached in memory. + + \param dev the device + \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) +{ + if (m_libUsb.get_device_descriptor) + return m_libUsb.get_device_descriptor(dev, desc); + else + return LIBUSB_ERROR_NOT_SUPPORTED; +} + +/*! \brief Returns a list of USB devices currently attached to the system. + This is your entry point into finding a USB device to operate. + + You are expected to unreference all the devices when you are done with + them, and then free the list with libusb_free_device_list(). Note that + libusb_free_device_list() can unref all the devices for you. Be careful + not to unreference a device you are about to open until after you have + opened it. + + This return value of this function indicates the number of devices in + the resultant list. The list is actually one element larger, as it is + NULL-terminated. + + \param ctx the context to operate on, or NULL for the default context + \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) +{ + if (m_libUsb.get_device_list) + return m_libUsb.get_device_list(ctx, list); + else + return LIBUSB_ERROR_NOT_SUPPORTED; +} + +/*! \brief Frees a list of devices previously discovered using libusb_get_device_list(). + If the unref_devices parameter is set, the reference count of each device in the list is decremented by 1. + \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) +{ + if (m_libUsb.free_device_list) + m_libUsb.free_device_list(list, unref_devices); +} + +/*! \brief Retrieve a string descriptor in C style ASCII. + + Wrapper around libusb_get_string_descriptor(). Uses the first language supported by the device. + + \param dev a device handle + \param desc_index the index of the descriptor to retrieve + \param data output buffer for ASCII string descriptor + \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) +{ + if (m_libUsb.get_string_descriptor_ascii) + return m_libUsb.get_string_descriptor_ascii(dev, desc_index, data, length); + else + return LIBUSB_ERROR_NOT_SUPPORTED; +} + +/*! \brief Perform a USB bulk transfer. The direction of the transfer is inferred from the direction bits of the endpoint address. + + For bulk reads, the \a length field indicates the maximum length of + data you are expecting to receive. If less data arrives than expected, + this function will return that data, so be sure to check the + \a transferred output parameter. + + You should also check the \a transferred parameter for bulk writes. + Not all of the data may have been written. + + Also check \a transferred when dealing with a timeout error code. + libusb may have to split your transfer into a number of chunks to satisfy + underlying O/S requirements, meaning that the timeout may expire after + the first few chunks have completed. libusb is careful not to lose any data + that may have been transferred; do not assume that timeout conditions + indicate a complete lack of I/O. + + \param dev_handle a handle for the device to communicate with + \param endpoint the address of a valid endpoint to communicate with + \param data a suitably-sized data buffer for either input or output (depending on endpoint) + \param length for bulk writes, the number of bytes from data to be sent. for bulk reads, the maximum number of bytes to receive into the data buffer. + \param transferred output location for the number of bytes actually transferred. + \param timeout timeout (in millseconds) that this function should wait before giving up due to no response being received. For an unlimited timeout, use value 0. + + \returns 0 on success (and populates \a transferred) + \returns LIBUSB_ERROR_TIMEOUT if the transfer timed out (and populates \a transferred) + \returns LIBUSB_ERROR_PIPE if the endpoint halted + \returns LIBUSB_ERROR_OVERFLOW if the device offered more data + \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) +{ + if (m_libUsb.bulk_transfer) + return m_libUsb.bulk_transfer(dev_handle, endpoint, data, length, actual_length, timeout); + else + return LIBUSB_ERROR_NOT_SUPPORTED; +} + +/*! \brief Set message verbosity. + - Level 0: no messages ever printed by the library (default) + - Level 1: error messages are printed to stderr + - Level 2: warning and error messages are printed to stderr + - Level 3: informational messages are printed to stdout, warning and error messages are printed to stderr + + The default level is 0, which means no messages are ever printed. If you + choose to increase the message verbosity level, ensure that your + application does not close the stdout/stderr file descriptors. + + You are advised to set level 3. libusb is conservative with its message + logging and most of the time, will only log messages that explain error + conditions and other oddities. This will help you debug your software. + + If the LIBUSB_DEBUG environment variable was set when libusb was + initialized, this function does nothing: the message verbosity is fixed + to the value in the environment variable. + + If libusb was compiled without any message logging, this function does + nothing: you'll never get any messages. + + If libusb was compiled with verbose debug message logging, this function + does nothing: you'll always get messages from all levels. + + \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) +{ + if (m_libUsb.set_debug) + m_libUsb.set_debug(ctx, level); +} + +#else + +/*! \cond NODOXYGEN */ +// this is just here to prevent compiler warnings about this file being empty +bool haveWinUsb = false; +/*! \endcond */ + +#endif // HAVE_LIBUSB diff --git a/extern/xspublic/xscontroller/xslibusb.h b/extern/xspublic/xscontroller/xslibusb.h new file mode 100644 index 0000000..067e0ab --- /dev/null +++ b/extern/xspublic/xscontroller/xslibusb.h @@ -0,0 +1,128 @@ + +// Copyright (c) 2003-2022 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 XSLIBUSB +#define XSLIBUSB + +#ifdef HAVE_LIBUSB + +#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); + +struct XsLibraryLoader; + +class XsLibUsb +{ +public: + XsLibUsb(void); + ~XsLibUsb(void); + + 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; +private: + + 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; + } m_libUsb; + + XsLibraryLoader* m_libraryLoader; + + void tryLoadLibrary(); + void initLibrary(); +}; + +#endif // HAVE_LIBUSB +#endif diff --git a/extern/xspublic/xscontroller/xsoperationalmode.h b/extern/xspublic/xscontroller/xsoperationalmode.h new file mode 100644 index 0000000..f70d2bd --- /dev/null +++ b/extern/xspublic/xscontroller/xsoperationalmode.h @@ -0,0 +1,46 @@ + +// Copyright (c) 2003-2022 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 XSOPERATIONALMODE_H +#define XSOPERATIONALMODE_H + +enum XsOperationalMode +{ + XOP_Unknown, + XOP_LiveStream, + XOP_Obr, + XOP_Robr +}; + +typedef enum XsOperationalMode XsOperationalMode; + +#endif diff --git a/extern/xspublic/xscontroller/xsorientationmode.h b/extern/xspublic/xscontroller/xsorientationmode.h new file mode 100644 index 0000000..bdeec35 --- /dev/null +++ b/extern/xspublic/xscontroller/xsorientationmode.h @@ -0,0 +1,51 @@ + +// Copyright (c) 2003-2022 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 XSORIENTATIONMODE_H +#define XSORIENTATIONMODE_H + +/*! \addtogroup enums Global enumerations + @{ +*/ +/*! \brief Legacy orientation output mode selection +*/ +enum XsOrientationMode +{ + OM_None, + OM_Euler, + OM_Quaternion, + OM_Matrix +}; +/*! @} */ +typedef enum XsOrientationMode XsOrientationMode; + +#endif diff --git a/extern/xspublic/xscontroller/xsprocessingflag.h b/extern/xspublic/xscontroller/xsprocessingflag.h new file mode 100644 index 0000000..30db884 --- /dev/null +++ b/extern/xspublic/xscontroller/xsprocessingflag.h @@ -0,0 +1,45 @@ + +// Copyright (c) 2003-2022 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 XSPROCESSINGFLAG_H +#define XSPROCESSINGFLAG_H + +/*! Used to enable or disable some data processing options + \sa XsDevice::setInitialBiasUpdateEnabled +*/ +enum XsProcessingFlag +{ + XPF_EnableInitialGyroBiasUpdate = 0x0001, + XPF_FixedGravity = 0x0002 +}; + +#endif diff --git a/extern/xspublic/xscontroller/xsprotocoltype.h b/extern/xspublic/xscontroller/xsprotocoltype.h new file mode 100644 index 0000000..3d741ec --- /dev/null +++ b/extern/xspublic/xscontroller/xsprotocoltype.h @@ -0,0 +1,51 @@ + +// Copyright (c) 2003-2022 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 XSPROTOCOLTYPE_H +#define XSPROTOCOLTYPE_H + +/*! \addtogroup enums Global enumerations + @{ +*/ +//! 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 + , XPT_ImarIfogUart //!< An iMAR iFog UART communication protocol +}; +/*! @} */ +typedef enum XsProtocolType XsProtocolType; + +#endif diff --git a/extern/xspublic/xscontroller/xsrejectreason.c b/extern/xspublic/xscontroller/xsrejectreason.c new file mode 100644 index 0000000..e96eeb5 --- /dev/null +++ b/extern/xspublic/xscontroller/xsrejectreason.c @@ -0,0 +1,56 @@ + +// Copyright (c) 2003-2022 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 "xsrejectreason.h" +#include + +/*! \addtogroup cinterface C Interface + @{ +*/ + +/*! \brief Convert the %XsRejectReason to a human readable string */ +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; + } + return "UnknownReason"; +} + +/*! @} */ diff --git a/extern/xspublic/xscontroller/xsrejectreason.h b/extern/xspublic/xscontroller/xsrejectreason.h new file mode 100644 index 0000000..398c835 --- /dev/null +++ b/extern/xspublic/xscontroller/xsrejectreason.h @@ -0,0 +1,70 @@ + +// Copyright (c) 2003-2022 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 XSREJECTREASON_H +#define XSREJECTREASON_H + +#include "xscontrollerconfig.h" + +/*! \addtogroup enums Global enumerations + @{ +*/ +/*! \brief Identifiers for why a device's connection was rejected. */ +enum XsRejectReason +{ + XRR_Unknown = 0, + XRR_VersionMismatch = 1, + XRR_Blacklisted = 2, + XRR_StationIsDisconnecting = 3, + XRR_SystemIsOperational = 4, +}; +/*! @} */ +typedef enum XsRejectReason XsRejectReason; + +#ifdef __cplusplus +extern "C" { +#endif + +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) +{ + return XsRejectReason_toString(r); +} +#endif + + +#endif diff --git a/extern/xspublic/xscontroller/xsscanner.cpp b/extern/xspublic/xscontroller/xsscanner.cpp new file mode 100644 index 0000000..c24eb80 --- /dev/null +++ b/extern/xspublic/xscontroller/xsscanner.cpp @@ -0,0 +1,242 @@ + +// Copyright (c) 2003-2022 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 "xsscanner.h" +#include "scanner.h" +#include "enumerateusbdevices.h" + +/*! \class XsScanner + \brief Provides static functionality for scanning for Xsens devices. +*/ + +/*! \copydoc XsScanner_scanPorts */ +void XsScanner_scanPorts_int(XsPortInfoArray* ports, XsBaudRate baudrate, int singleScanTimeout, int ignoreNonXsensDevices, int detectRs485) +{ + LOGXSSCAN(__FUNCTION__ << " baudrate " << baudrate << " singleScanTimeout " << singleScanTimeout << " ignoreNonXsensDevices " << ignoreNonXsensDevices << " detectRs485 " << detectRs485); + + assert(ports != nullptr); + if (!ports) + return; + + XsPortInfoArray tmp; + Scanner::Accessor accessor; + accessor.scanner().xsScanPorts(tmp, baudrate, (uint32_t) singleScanTimeout, ignoreNonXsensDevices != 0, detectRs485 != 0); + if (tmp.size()) + ports->assign(tmp.size(), &tmp[0]); + else + ports->clear(); +} + +/*! \copydoc XsScanner_scanPort */ +int XsScanner_scanPort_int(XsPortInfo* port, XsBaudRate baudrate, int singleScanTimeout, int detectRs485) +{ + assert(port != nullptr); + if (!port) + return 0; + + Scanner::Accessor accessor; + return accessor.scanner().xsScanPort(*port, baudrate, (uint32_t)singleScanTimeout, detectRs485 != 0) ? 1 : 0; +} + +/*! \relates XsScanner + \brief List all serial ports without scanning + \param[out] ports The list of detected ports. + \param[in] ignoreNonXsensDevices When non-zero (the default), only Xsens ports are returned. +*/ +void XsScanner_enumerateSerialPorts_int(XsPortInfoArray* ports, int ignoreNonXsensDevices) +{ + LOGXSSCAN(__FUNCTION__ << " ignoreNonXsensDevices " << ignoreNonXsensDevices); + + assert(ports != nullptr); + if (!ports) + return; + + XsPortInfoArray tmp; + Scanner::Accessor accessor; + accessor.scanner().xsEnumerateSerialPorts(tmp, ignoreNonXsensDevices != 0); + if (tmp.size()) + ports->assign(tmp.size(), &tmp[0]); + else + ports->clear(); +} + +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); + } + + /*! \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); + } + + /*! \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; + + 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]); + else + ports->clear(); + } + + /*! \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(hub != nullptr && port != nullptr); + if (!hub || !port) + return; + + Scanner::Accessor accessor; + *hub = accessor.scanner().xsScanUsbHub(*port); + } + + void XsScanner_enumerateNetworkDevices(XsPortInfoArray* ports) + { + LOGXSSCAN(__FUNCTION__); + assert(ports != nullptr); + if (!ports) + return; + + XsPortInfoArray tmp; + + Scanner::Accessor accessor; + accessor.scanner().xsEnumerateNetworkDevices(tmp); + ports->swap(tmp); + } + + void XsScanner_enumerateBluetoothDevices(XsPortInfoArray* ports) + { + LOGXSSCAN(__FUNCTION__); + assert(ports != nullptr); + if (!ports) + return; + + XsPortInfoArray 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; + } + +} // extern "C" diff --git a/extern/xspublic/xscontroller/xsscanner.h b/extern/xspublic/xscontroller/xsscanner.h new file mode 100644 index 0000000..12b5086 --- /dev/null +++ b/extern/xspublic/xscontroller/xsscanner.h @@ -0,0 +1,237 @@ + +// Copyright (c) 2003-2022 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 XSSCANNER_H +#define XSSCANNER_H + +#include "xscontrollerconfig.h" +#include + +//AUTO namespace xstypes { +struct XsPortInfoArray; +//AUTO } + +//AUTO namespace xscontroller { +struct XsUsbHubInfo; +//AUTO } + +#ifdef __cplusplus +#include +#include +#include +#include "xsusbhubinfo.h" +#include +#include +extern "C" { +#endif +struct XsPortInfo; + +//! \brief Defines the callback type that can be supplied to XsScanner_setScanLogCallback +typedef void (*XsScanLogCallbackFunc)(struct XsString const*); + +XDA_DLL_API void XsScanner_scanPorts(struct XsPortInfoArray* ports, XsBaudRate baudrate, int singleScanTimeout, int ignoreNonXsensDevices, int detectRs485); +XDA_DLL_API int XsScanner_scanPort(struct XsPortInfo* port, XsBaudRate baudrate, int singleScanTimeout, int detectRs485); +XDA_DLL_API void XsScanner_enumerateSerialPorts(struct XsPortInfoArray* ports, int ignoreNonXsensDevices); +XDA_DLL_API void XsScanner_filterResponsiveDevices(struct XsPortInfoArray* ports, XsBaudRate baudrate, int singleScanTimeout, int detectRs485); +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 +{ +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. + \param[in] singleScanTimeout The timeout of a scan of a single port at a single baud rate in ms. + \param[in] ignoreNonXsensDevices When true (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 + \returns The list of detected ports. + \sa XsScanner_scanPorts + */ + 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); + 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); + } + + /*! \brief Scan a single port for Xsens devices. + \param[in] portName The name of the port to scan. + \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 An XsPortInfo structure with the results of the scan. + \sa XsScanner_scanPort + */ + static inline XsPortInfo scanPort(const XsString& portName, XsBaudRate baudrate = XBR_Invalid, int singleScanTimeout = 100, bool detectRs485 = false) + { + XsPortInfo pi(portName, baudrate); + if (scanPort(pi, baudrate, singleScanTimeout, detectRs485)) + return pi; + + return XsPortInfo(); + } + + /*! \brief Scan a list of Com ports for Xsens devices. + \param[in] portList The list of port names to scan. + \param[in] portLinesOptionsList The list of hardware flow control options for the ports to scan. + \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. + \returns An array of XsPortInfo structures with the results of the scan. + \sa XsScanner_scanCOMPortList + */ + static inline XsPortInfoArray scanComPortList(const XsStringArray& portList, const XsIntArray& portLinesOptionsList = XsIntArray(), XsBaudRate baudrate = XBR_Invalid, int singleScanTimeout = 100) + { + XsPortInfoArray pInfoArray; + + if (!portLinesOptionsList.empty() && (portLinesOptionsList.size() != portList.size())) + return pInfoArray; + + for (XsIntArray::size_type idxPort = 0; idxPort < portList.size(); ++idxPort) + { + const XsPortLinesOptions portLinesOptions = portLinesOptionsList.empty() ? XPLO_All_Ignore : static_cast(portLinesOptionsList[idxPort]); + + XsPortInfo portInfo(portList[idxPort], baudrate, portLinesOptions); + + if (scanPort(portInfo, baudrate, singleScanTimeout, (portLinesOptions == XPLO_All_Clear))) // XPLO_All_Clear == both RTS/DTR to 0 (RS485). + pInfoArray.push_back(portInfo); + } + return pInfoArray; + } + + /*! \brief List all serial ports without scanning + \param ignoreNonXsensDevices When true (the default), only Xsens ports are returned. + \returns The list of detected ports. + */ + static inline XsPortInfoArray enumerateSerialPorts(bool ignoreNonXsensDevices = true) + { + XsPortInfoArray ports; + XsScanner_enumerateSerialPorts(&ports, ignoreNonXsensDevices ? 1 : 0); + return ports; + } + + /*! \brief Scan the supplied ports for Xsens devices. + \details This function does not modify the input list as opposed to XsScanner_filterResponsiveDevices + \param[in] ports The list of ports to scan. + \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 + \returns The list of ports that have responsive devices on them. + \sa XsScanner_filterResponsiveDevices + */ + 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); + return filtered; + } + + /*! \brief List all compatible USB ports without scanning. + \returns The list of detected usb devices. + \sa XsScanner_enumerateUsbDevices + */ + static inline XsPortInfoArray enumerateUsbDevices(void) + { + XsPortInfoArray ports; + XsScanner_enumerateUsbDevices(&ports); + return ports; + } + + /*! \brief Determine the USB hub that \a port is attached to + \param port The port for which to determine the USB hub. + \returns The identifier of the hub that \a port is attached to. + \sa XsScanner_scanUsbHub + */ + static inline XsUsbHubInfo scanUsbHub(const XsPortInfo& port) + { + XsUsbHubInfo hub; + XsScanner_scanUsbHub(&hub, &port); + return hub; + } + + /*! \brief List all compatible network devices without scanning. + \returns The list of detected network services. + \sa XsScanner_enumerateNetworkDevices + */ + static inline XsPortInfoArray enumerateNetworkDevices(void) + { + XsPortInfoArray ports; + XsScanner_enumerateNetworkDevices(&ports); + 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 + */ + static inline void abortScan(void) + { + XsScanner_abortScan(); + } + + /*! \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. + \sa XsScanner_setScanLogCallback + */ + static inline void XSNOCOMEXPORT setScanLogCallback(XsScanLogCallbackFunc cb) + { + XsScanner_setScanLogCallback(cb); + } +}; + +#endif + +#endif diff --git a/extern/xspublic/xscontroller/xsselftestresult.h b/extern/xspublic/xscontroller/xsselftestresult.h new file mode 100644 index 0000000..7fb04af --- /dev/null +++ b/extern/xspublic/xscontroller/xsselftestresult.h @@ -0,0 +1,198 @@ + +// Copyright (c) 2003-2022 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 XSSELFTESTRESULT_H +#define XSSELFTESTRESULT_H + +#include + +/*! \addtogroup enums Global enumerations + @{ +*/ +/*! \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 + , XSTF_Sync = 1 << 14 +}; +/*! @} */ +typedef enum XsSelfTestFlag XsSelfTestFlag; + +/*! \brief Contains the results of a self-test performed by an Xsens device +*/ +struct XsSelfTestResult +{ + uint16_t m_flags; //!< Flags that specify which tests have passed + +#ifdef __cplusplus + //! \brief Create a new %XsSelfTestResult from supplied flags + static inline XsSelfTestResult create(uint16_t resultFlags) + { + XsSelfTestResult tmp = { resultFlags }; + return tmp; + } + + //! \brief Returns whether the accelerometer x-axis passed (true) or failed (false) its self-test + inline bool accX() const + { + return (m_flags & XSTF_AccX) != 0; + } + + //! \brief Returns whether the accelerometer y-axis passed (true) or failed (false) its self-test + inline bool accY() const + { + return (m_flags & XSTF_AccY) != 0; + } + + //! \brief Returns whether the accelerometer z-axis passed (true) or failed (false) its self-test + inline bool accZ() const + { + return (m_flags & XSTF_AccZ) != 0; + } + + //! \brief Returns whether the gyroscope x-axis passed (true) or failed (false) its self-test + inline bool gyrX() const + { + return (m_flags & XSTF_GyrX) != 0; + } + + //! \brief Returns whether the gyroscope y-axis passed (true) or failed (false) its self-test + inline bool gyrY() const + { + return (m_flags & XSTF_GyrY) != 0; + } + + //! \brief Returns whether the gyroscope z-axis passed (true) or failed (false) its self-test + inline bool gyrZ() const + { + return (m_flags & XSTF_GyrZ) != 0; + } + + //! \brief Returns whether the magnetometer x-axis passed (true) or failed (false) its self-test + inline bool magX() const + { + return (m_flags & XSTF_MagX) != 0; + } + + //! \brief Returns whether the magnetometer y-axis passed (true) or failed (false) its self-test + inline bool magY() const + { + return (m_flags & XSTF_MagY) != 0; + } + + //! \brief Returns whether the magnetometer z-axis passed (true) or failed (false) its self-test + inline bool magZ() const + { + return (m_flags & XSTF_MagZ) != 0; + } + + /*! \brief Returns whether the barometer passed (true) or failed (false) its self-test + \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, MTi-670, MTi-680 + \returns True if the gnss selftest has passed + */ + inline bool gnss() const + { + return (m_flags & XSTF_Gnss) != 0; + } + + /*! \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, 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 diff --git a/extern/xspublic/xscontroller/xsubloxgnssplatform.h b/extern/xspublic/xscontroller/xsubloxgnssplatform.h new file mode 100644 index 0000000..92cda1c --- /dev/null +++ b/extern/xspublic/xscontroller/xsubloxgnssplatform.h @@ -0,0 +1,55 @@ + +// Copyright (c) 2003-2022 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 XSUBLOXGNSSPLATFORM_H +#define XSUBLOXGNSSPLATFORM_H + +/*! \brief Used to select u-blox GNSS chip platform + \sa XsDevice::setUbloxGnssPlatform + \details These enum values represent different platform setting for the u-blox GNSS chip +*/ +enum XsUbloxGnssPlatform +{ + XGP_Portable = 0, + XGP_Stationary = 2, + XGP_Pedestrian = 3, + XGP_Automotive = 4, + XGP_AtSea = 5, + XGP_Airborne1g = 6, + XGP_Airborne2g = 7, + XGP_Airborne4g = 8, //!< Airborne with <4g Acceleration + XGP_Wrist = 9 +}; + +typedef enum XsUbloxGnssPlatform XsUbloxGnssPlatform; + +#endif diff --git a/extern/xspublic/xscontroller/xsusbhubinfo.c b/extern/xspublic/xscontroller/xsusbhubinfo.c new file mode 100644 index 0000000..88a02ff --- /dev/null +++ b/extern/xspublic/xscontroller/xsusbhubinfo.c @@ -0,0 +1,143 @@ + +// Copyright (c) 2003-2022 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 "xsusbhubinfo.h" +#include +#include + +/*! \addtogroup cinterface C Interface + @{ +*/ + +/*! \typedef XsHubIdentifier + On Windows: \code + typedef int XsHubIdentifier; + \endcode + + + On non-windows: \code + typedef const char* XsHubIdentifier; + \endcode +*/ + +/*! \brief Initialize the %XsUsbHubInfo with \a hub + + \param[in] hub the hub identifier to initialize with +*/ +void XsUsbHubInfo_construct(XsUsbHubInfo* thisPtr, XsHubIdentifier hub) +{ +#ifdef _WIN32 + thisPtr->m_hub = hub; +#else + if (hub) + thisPtr->m_hub = strdup(hub); + else + thisPtr->m_hub = 0; +#endif +} + +/*! \brief Destroy the %XsUsbHubInfo +*/ +void XsUsbHubInfo_destruct(XsUsbHubInfo* thisPtr) +{ +#ifndef _WIN32 + if (thisPtr->m_hub) + free((void*) thisPtr->m_hub); +#endif + thisPtr->m_hub = 0; +} + +/*! \brief Assign a new hub identifier to the %XsUsbHubInfo + \param[in] hub the hub identifier +*/ +void XsUsbHubInfo_assign(XsUsbHubInfo* thisPtr, XsHubIdentifier hub) +{ + XsUsbHubInfo_destruct(thisPtr); + XsUsbHubInfo_construct(thisPtr, hub); +} + +/*! \brief Copy the contents of the %XsUsbHubInfo to \a copy +*/ +void XsUsbHubInfo_copy(XsUsbHubInfo* copy, XsUsbHubInfo const* src) +{ + if (copy != src) + XsUsbHubInfo_assign(copy, src->m_hub); +} + +/*! \brief Swap the two %XsUsbHubInfo items +*/ +void XsUsbHubInfo_swap(XsUsbHubInfo* thisPtr, XsUsbHubInfo* thatPtr) +{ + XsHubIdentifier tmp; + if (thisPtr == thatPtr) + return; + + tmp = thisPtr->m_hub; + thisPtr->m_hub = thatPtr->m_hub; + thatPtr->m_hub = tmp; +} + +/*! \brief Returns true if the two hub info objects share the same device path + + \param[in] left the left hand side to compare + \param[in] right the right hand side to compare + + \returns non-zero if the two hubs share the same parent +*/ +int XsUsbHubInfo_parentPathMatches(const XsUsbHubInfo* left, const XsUsbHubInfo* right) +{ +#ifdef _WIN32 + return left->m_hub == right->m_hub && left->m_hub != 0; +#else + int dotPos, tmp; + XsHubIdentifier otherLastDot; + XsHubIdentifier thisLastDot; + + if (!right->m_hub || !left->m_hub) + return 0; + if (left == right) + return 1; + + otherLastDot = strrchr(right->m_hub, '.'); + thisLastDot = strrchr(left->m_hub, '.'); + + dotPos = (int)(otherLastDot - right->m_hub); + tmp = (int)(thisLastDot - left->m_hub); + + if (dotPos != tmp) + return 0; + + return !strncmp(right->m_hub, left->m_hub, dotPos); +#endif +} + +/*! @} */ diff --git a/extern/xspublic/xscontroller/xsusbhubinfo.h b/extern/xspublic/xscontroller/xsusbhubinfo.h new file mode 100644 index 0000000..b7f8e73 --- /dev/null +++ b/extern/xspublic/xscontroller/xsusbhubinfo.h @@ -0,0 +1,142 @@ + +// Copyright (c) 2003-2022 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 XSUSBHUBINFO_H +#define XSUSBHUBINFO_H + +#include "xscontrollerconfig.h" + +#ifdef _WIN32 + typedef int XsHubIdentifier; +#else + typedef const char* XsHubIdentifier; +#endif + +struct XsUsbHubInfo; +#ifndef __cplusplus +#define XSUSBHUBINFO_INITIALIZER { 0 } +typedef struct XsUsbHubInfo XsUsbHubInfo; +#else +extern "C" { +#endif + +void XDA_DLL_API XsUsbHubInfo_assign(XsUsbHubInfo* thisPtr, XsHubIdentifier hub); +void XDA_DLL_API XsUsbHubInfo_construct(XsUsbHubInfo* thisPtr, XsHubIdentifier hub); +void XDA_DLL_API XsUsbHubInfo_destruct(XsUsbHubInfo* thisPtr); +void XDA_DLL_API XsUsbHubInfo_copy(XsUsbHubInfo* copy, XsUsbHubInfo const* src); +void XDA_DLL_API XsUsbHubInfo_swap(XsUsbHubInfo* thisPtr, XsUsbHubInfo* thatPtr); +int XDA_DLL_API XsUsbHubInfo_parentPathMatches(const XsUsbHubInfo* thisPtr, const XsUsbHubInfo* other); + +#ifdef __cplusplus +} // extern "C" +#endif + + +/*! \struct XsUsbHubInfo + \brief A structure that wraps USB hub information +*/ +struct XsUsbHubInfo +{ +#ifdef __cplusplus + /*! \brief Default constructor + + \param hubid an optional hub identifier to initialize with + \sa XsUsbHubInfo_construct + */ + explicit XsUsbHubInfo(XsHubIdentifier hubid = 0) + : m_hub(0) + { + if (hubid) + XsUsbHubInfo_construct(this, hubid); + } + + /*! \brief Destructor \sa XsUsbHubInfo_destruct */ + ~XsUsbHubInfo() + { + XsUsbHubInfo_destruct(this); + } + + /*! \brief Copy constructor + \param other the object to copy + \sa XsUsbHubInfo_copy \sa XsUsbHubInfo_construct + */ + XsUsbHubInfo(const XsUsbHubInfo& other) + : m_hub(0) + { + if (other.m_hub) + XsUsbHubInfo_construct(this, other.m_hub); + } + + /*! \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) + { + if (this != &other) + XsUsbHubInfo_copy(this, &other); + return *this; + } + + /*! \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 + { + return 0 != XsUsbHubInfo_parentPathMatches(this, &other); + } + + /*! \brief Returns true if a valid hub is set + */ + bool isValid() const + { + return m_hub != 0; + } + + /*! \brief Return the hub identifier + */ + inline XsHubIdentifier hub() const + { + return m_hub; + } + +private: + //! \protectedsection +#endif + XsHubIdentifier m_hub; //!< The identifier of the USB hub +}; +typedef struct XsUsbHubInfo XsUsbHubInfo; + +#endif diff --git a/extern/xspublic/xscontroller/xswinusb.cpp b/extern/xspublic/xscontroller/xswinusb.cpp new file mode 100644 index 0000000..5ee9339 --- /dev/null +++ b/extern/xspublic/xscontroller/xswinusb.cpp @@ -0,0 +1,538 @@ + +// Copyright (c) 2003-2022 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 "xswinusb.h" + +#ifdef USE_WINUSB +#include + +/*! \class XsWinUsb + \brief Class for dynamic loading of winusb +*/ +XsWinUsb::XsWinUsb(void) +{ + m_libraryLoader = new XsLibraryLoader(); + initLibrary(); +} + +XsWinUsb::~XsWinUsb(void) +{ + delete m_libraryLoader; +} + +void XsWinUsb::initLibrary() +{ + if (!m_libraryLoader->isLoaded()) + m_libraryLoader->load("winusb.dll"); + + m_winUsb.AbortPipe = NULL; + m_winUsb.Initialize = NULL; + m_winUsb.Free = NULL; + m_winUsb.GetAssociatedInterface = NULL; + m_winUsb.GetDescriptor = NULL; + m_winUsb.QueryInterfaceSettings = NULL; + m_winUsb.QueryDeviceInformation = NULL; + m_winUsb.SetCurrentAlternateSetting = NULL; + m_winUsb.GetCurrentAlternateSetting = NULL; + m_winUsb.QueryPipe = NULL; + m_winUsb.SetPipePolicy = NULL; + m_winUsb.GetPipePolicy = NULL; + m_winUsb.ReadPipe = NULL; + m_winUsb.WritePipe = NULL; + m_winUsb.ControlTransfer = NULL; + m_winUsb.ResetPipe = NULL; + m_winUsb.AbortPipe = NULL; + m_winUsb.FlushPipe = NULL; + m_winUsb.SetPowerPolicy = NULL; + m_winUsb.GetPowerPolicy = NULL; + m_winUsb.GetOverlappedResult = NULL; + + if (m_libraryLoader->isLoaded()) + { + m_winUsb.AbortPipe = (WinUSB_AbortPipe*)m_libraryLoader->resolve("WinUsb_AbortPipe"); + m_winUsb.Initialize = (WinUSB_Initialize*)m_libraryLoader->resolve("WinUsb_Initialize"); + m_winUsb.Free = (WinUSB_Free*)m_libraryLoader->resolve("WinUsb_Free"); + m_winUsb.GetAssociatedInterface = (WinUSB_GetAssociatedInterface*)m_libraryLoader->resolve("WinUsb_GetAssociatedInterface"); + m_winUsb.GetDescriptor = (WinUSB_GetDescriptor*)m_libraryLoader->resolve("WinUsb_GetDescriptor"); + m_winUsb.QueryInterfaceSettings = (WinUSB_QueryInterfaceSettings*)m_libraryLoader->resolve("WinUsb_QueryInterfaceSettings"); + m_winUsb.QueryDeviceInformation = (WinUSB_QueryDeviceInformation*)m_libraryLoader->resolve("WinUsb_QueryDeviceInformation"); + m_winUsb.SetCurrentAlternateSetting = (WinUSB_SetCurrentAlternateSetting*)m_libraryLoader->resolve("WinUsb_SetCurrentAlternateSetting"); + m_winUsb.GetCurrentAlternateSetting = (WinUSB_GetCurrentAlternateSetting*)m_libraryLoader->resolve("WinUsb_GetCurrentAlternateSetting"); + m_winUsb.QueryPipe = (WinUSB_QueryPipe*)m_libraryLoader->resolve("WinUsb_QueryPipe"); + m_winUsb.SetPipePolicy = (WinUSB_SetPipePolicy*)m_libraryLoader->resolve("WinUsb_SetPipePolicy"); + m_winUsb.GetPipePolicy = (WinUSB_GetPipePolicy*)m_libraryLoader->resolve("WinUsb_GetPipePolicy"); + m_winUsb.ReadPipe = (WinUSB_ReadPipe*)m_libraryLoader->resolve("WinUsb_ReadPipe"); + m_winUsb.WritePipe = (WinUSB_WritePipe*)m_libraryLoader->resolve("WinUsb_WritePipe"); + m_winUsb.ControlTransfer = (WinUSB_ControlTransfer*)m_libraryLoader->resolve("WinUsb_ControlTransfer"); + m_winUsb.ResetPipe = (WinUSB_ResetPipe*)m_libraryLoader->resolve("WinUsb_ResetPipe"); + m_winUsb.AbortPipe = (WinUSB_AbortPipe*)m_libraryLoader->resolve("WinUsb_AbortPipe"); + m_winUsb.FlushPipe = (WinUSB_FlushPipe*)m_libraryLoader->resolve("WinUsb_FlushPipe"); + m_winUsb.SetPowerPolicy = (WinUSB_SetPowerPolicy*)m_libraryLoader->resolve("WinUsb_SetPowerPolicy"); + m_winUsb.GetPowerPolicy = (WinUSB_GetPowerPolicy*)m_libraryLoader->resolve("WinUsb_GetPowerPolicy"); + m_winUsb.GetOverlappedResult = (WinUSB_GetOverlappedResult*)m_libraryLoader->resolve("WinUsb_GetOverlappedResult"); + } +} + +/*! \brief Creates/opens a WinUsb interface handle from the device list. + + \param[out] InterfaceHandle Receives a handle configured to the first (default) interface on the device. This handle is required by other WinUsb routines that perform operations on the default interface. The handle is opaque. To release this handle, call the \a Free function. + \param[in] DevInfo The device list element to open. + + \returns On success, TRUE. Otherwise FALSE. Use GetLastError() to get extended error information. +*/ +BOOL XsWinUsb::Initialize(HANDLE DeviceHandle, PWINUSB_INTERFACE_HANDLE InterfaceHandle) +{ + if (m_winUsb.Initialize) + return m_winUsb.Initialize(DeviceHandle, InterfaceHandle); + else + return FALSE; +} + +/*! \brief Frees a WinUsb interface handle. + \param[in] InterfaceHandle Handle to an interface on the device. This handle must be created by a previous call to see \a Initialize or \a GetAssociatedInterface. + + \returns TRUE + + \sa GetAssociatedInterface. +*/ +BOOL XsWinUsb::Free(WINUSB_INTERFACE_HANDLE InterfaceHandle) +{ + if (m_winUsb.Free) + return m_winUsb.Free(InterfaceHandle); + else + return FALSE; +} + +/*! \brief Retrieves a handle for an associated interface. + \param[in] InterfaceHandle An initialized usb handle, see \a Initialize. + \param[in] AssociatedInterfaceIndex An index that specifies the associated interface to retrieve. A value of 0 indicates the first associated interface, a value of 1 indicates the second associated interface, and so on. + \param[out] AssociatedInterfaceHandle A handle for the associated interface. Callers must pass this interface handle to WinUsb Functions exposed by WinUsb.dll. To close this handle, call Free. + + \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) +{ + if (m_winUsb.GetAssociatedInterface) + return m_winUsb.GetAssociatedInterface(InterfaceHandle, AssociatedInterfaceIndex, AssociatedInterfaceHandle); + else + return FALSE; +} + +/*! \brief Gets the requested descriptor. This is a synchronous operation. + \param[in] InterfaceHandle An initialized usb handle, see \a Initialize. + \param[in] DescriptorType A value that specifies the type of descriptor to return. This parameter corresponds to the bDescriptorType field of a standard device descriptor, whose values are described in the Universal Serial Bus specification. + \param[in] Index The descriptor index. For an explanation of the descriptor index, see the Universal Serial Bus specification (www.usb.org). + \param[in] LanguageID A value that specifies the language identifier, if the requested descriptor is a string descriptor. + \param[out] Buffer A caller-allocated buffer that receives the requested descriptor. + \param[in] BufferLength The length, in bytes, of Buffer. + \param[out] LengthTransferred The number of bytes that were copied into Buffer. + + \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) +{ + if (m_winUsb.GetDescriptor) + return m_winUsb.GetDescriptor(InterfaceHandle, DescriptorType, Index, LanguageID, Buffer, BufferLength, LengthTransferred); + else + return FALSE; +} + +/*! \brief Retrieves the interface descriptor for the specified alternate interface settings for a particular interface handle. + + The \a QueryInterfaceSettings call searches the current/default interface array for the alternate interface specified by the caller in the \a AltSettingIndex. + If the specified alternate interface is found, the function populates the caller-allocated USB_INTERFACE_DESCRIPTOR structure. + If the specified alternate interface is not found, then the call fails with the ERROR_NO_MORE_ITEMS code. + + \param[in] InterfaceHandle An initialized usb handle, see \a Initialize. + \param[in] AltSettingIndex A value that indicates which alternate setting index to return. A value of 0 indicates the first alternate setting, a value of 1 indicates the second alternate setting, and so on. + \param[out] UsbAltInterfaceDescriptor A pointer to a caller-allocated USB_INTERFACE_DESCRIPTOR structure that contains information about the interface that AltSettingNumber specified. + + \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) +{ + if (m_winUsb.QueryInterfaceSettings) + return m_winUsb.QueryInterfaceSettings(InterfaceHandle, AlternateInterfaceNumber, UsbAltInterfaceDescriptor); + else + return FALSE; +} + +/*! \brief Retrieves information about the physical device that is associated with a WinUSB handle. + \param[in] InterfaceHandle An initialized usb handle, see \a Initialize. + \param[in] InformationType A value that specifies which interface information value to retrieve. On input, InformationType must have the following value: DEVICE_SPEED (0x01). + \param[in,out] BufferLength The maximum number of bytes to read. This number must be less than or equal to the size, in bytes, of Buffer. On output, BufferLength is set to the actual number of bytes that were copied into Buffer. + \param[in,out] Buffer A caller-allocated buffer that receives the requested value. On output, Buffer indicates the device speed: + (0x01) low/full speed device. + (0x03) high speed device. + + \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) +{ + if (m_winUsb.QueryDeviceInformation) + return m_winUsb.QueryDeviceInformation(InterfaceHandle, InformationType, BufferLength, Buffer); + else + return FALSE; +} + +/*! \brief Sets the alternate setting of an interface. + Sets the active \a bAlternateSetting for the current/default interface. + + \param[in] InterfaceHandle An initialized usb handle, see \a Initialize. + \param[in] AltSettingNumber The value that is contained in the \a bAlternateSetting member of the USB_INTERFACE_DESCRIPTOR structure. This structure can be populated by the \a QueryInterfaceSettings routine. + + \returns On success, TRUE. Otherwise FALSE. Use GetLastError() to get extended error information. + + \sa QueryInterfaceSettings +*/ +BOOL XsWinUsb::SetCurrentAlternateSetting(WINUSB_INTERFACE_HANDLE InterfaceHandle, UCHAR SettingNumber) +{ + if (m_winUsb.SetCurrentAlternateSetting) + return m_winUsb.SetCurrentAlternateSetting(InterfaceHandle, SettingNumber); + else + return FALSE; +} + +/*! \brief Gets the current alternate interface setting for an interface. + Gets the active bAlternateSetting for the current/default interface. + + \param[in] InterfaceHandle An initialized usb handle, see \a Initialize. + \param[out] AltSettingNumber A pointer to an unsigned character that receives an integer that indicates the current alternate setting. + + \returns On success, TRUE. Otherwise FALSE. Use GetLastError() to get extended error information. +*/ +BOOL XsWinUsb::GetCurrentAlternateSetting(WINUSB_INTERFACE_HANDLE InterfaceHandle, PUCHAR SettingNumber) +{ + if (m_winUsb.GetCurrentAlternateSetting) + return m_winUsb.GetCurrentAlternateSetting(InterfaceHandle, SettingNumber); + else + return FALSE; +} + +/*! \brief Retrieves information about a pipe that is associated with an interface. + The \a QueryPipe function does not retrieve information about the control pipe. + + Each interface on the USB device can have multiple endpoints. To communicate with each of these endpoints, the bus driver creates pipes for each endpoint on the interface. + The pipe indices are zero-based. Therefore for n number of endpoints, the pipes' indices are set from n-1. + \a QueryPipe parses the configuration descriptor to get the interface specified by the caller. + It searches the interface descriptor for the endpoint descriptor associated with the caller-specified pipe. + If the endpoint is found, the function populates the caller-allocated WINUSB_PIPE_INFORMATION structure with information from the endpoint descriptor. + + \param[in] InterfaceHandle An initialized usb handle, see \a Initialize. + \param[in] AltSettingNumber A value that specifies the alternate interface to return the information for. + \param[in] PipeIndex A value that specifies the pipe to return information about. This value is not the same as the bEndpointAddress field in the endpoint descriptor. + A PipeIndex value of 0 signifies the first endpoint that is associated with the interface, a value of 1 signifies the second endpoint, and so on. + PipeIndex must be less than the value in the bNumEndpoints field of the interface descriptor. + \param[out] PipeInformation A pointer, on output, to a caller-allocated WINUSB_PIPE_INFORMATION structure that contains pipe information. + + \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) +{ + if (m_winUsb.QueryPipe) + return m_winUsb.QueryPipe(InterfaceHandle, AlternateInterfaceNumber, PipeIndex, PipeInformation); + else + return FALSE; +} + +/*! \brief Sets the policy for a specific pipe associated with an endpoint on the device. This is a synchronous operation. + \param[in] InterfaceHandle An initialized usb handle, see \a Initialize. + \param[in] PipeID An 8-bit value that consists of a 7-bit address and a direction bit. This parameter corresponds to the bEndpointAddress field in the endpoint descriptor. + \param[in] PolicyType A UINT variable that specifies the policy parameter to change. The Value parameter contains the new value for the policy parameter. + See the remarks section for information about each of the pipe policies and the resulting behavior. + \param[in] ValueLength The size, in bytes, of the buffer at Value. + \param[in] Value The new value for the policy parameter that PolicyType specifies. The size of this input parameter depends on the policy to change. For information about the size of this parameter, see the description of the PolicyType parameter. + + \remarks + The following list describes symbolic constants for \a PolicyType + + - SHORT_PACKET_TERMINATE (0x01) + - The default value is FALSE. + - To enable SHORT_PACKET_TERMINATE, in Value pass the address of a caller-allocated UCHAR variable set to TRUE (nonzero). + - Enabling SHORT_PACKET_TERMINATE causes the driver to send a zero-length packet at the end of every write request to the host controller. + + - AUTO_CLEAR_STALL (0x02) + - The default value is FALSE. To enable AUTO_CLEAR_STALL, in Value pass the address of a caller-allocated UCHAR variable set to TRUE (nonzero). + - Enabling AUTO_CLEAR_STALL causes winUSB to reset the pipe in order to automatically clear the stall condition. Data continues to flow on the bulk and interrupt IN endpoints again as soon as a new or a queued transfer arrives on the endpoint. This policy parameter does not affect control pipes. + - Disabling AUTO_CLEAR_STALL causes all transfers (that arrive to the endpoint after the stalled transfer) to fail until the caller manually resets the endpoint's pipe by calling ResetPipe. + + - PIPE_TRANSFER_TIMEOUT (0x03) + - The default value is zero. To set a time-out value, in Value pass the address of a caller-allocated UINT variable that contains the time-out interval. + - The PIPE_TRANSFER_TIMEOUT value specifies the time-out interval, in milliseconds. The host controller cancels transfers that do not complete within the specified time-out interval. + - A value of zero (default) indicates that transfers do not time out because the host controller never cancels the transfer. + + - IGNORE_SHORT_PACKETS (0x04) + - The default value is FALSE. To enable IGNORE_SHORT_PACKETS, in Value pass the address of a caller-allocated UCHAR variable set to TRUE (nonzero). + - Enabling IGNORE_SHORT_PACKETS causes the host controller to not complete a read operation after it receives a short packet. Instead, the host controller completes the operation only after the host has read the specified number of bytes. + - Disabling IGNORE_SHORT_PACKETS causes the host controller to complete a read operation when either the host has read the specified number of bytes or the host has received a short packet. + + - ALLOW_PARTIAL_READS (0x05) + - The default value is TRUE (nonzero). To disable ALLOW_PARTIAL_READS, in Value pass the address of a caller-allocated UCHAR variable set to FALSE (zero). + - Disabling ALLOW_PARTIAL_READS causes the read requests to fail whenever the device returns more data (on bulk and interrupt IN endpoints) than the caller requested. + - Enabling ALLOW_PARTIAL_READS causes winUSB to save or discard the extra data when the device returns more data (on bulk and interrupt IN endpoints) than the caller requested. This behavior is defined by setting the AUTO_FLUSH value. + + - AUTO_FLUSH (0x06) + - The default value is FALSE (zero). To enable AUTO_FLUSH, in Value pass the address of a caller-allocated UCHAR variable set to TRUE (nonzero). + - AUTO_FLUSH must be used with ALLOW_PARTIAL_READS enabled. If ALLOW_PARTIAL_READS is TRUE, the value of AUTO_FLUSH determines the action taken by winUSB when the device returns more data than the caller requested. + - Disabling ALLOW_PARTIAL_READS causes winUSB to ignore the AUTO_FLUSH value. + - Disabling AUTO_FLUSH with ALLOW_PARTIAL_READS enabled causes winUSB to save the extra data, add the data to the beginning of the caller's next read request, and send it to the caller in the next read operation. + - Enabling AUTO_FLUSH with ALLOW_PARTIAL_READS enabled causes winUSB to discard the extra data remaining from the read request. + + - RAW_IO (0x07) + - The default value is FALSE (zero). To enable RAW_IO, in Value pass the address of a caller-allocated UCHAR variable set to TRUE (nonzero). + - Enabling RAW_IO causes winUSB to send data directly to the USB driver stack, bypassing winUSB's queuing and error handling mechanism. + - The buffers that are passed to \a ReadPipe must be configured by the caller as follows: + - The buffer length must be a multiple of the maximum endpoint packet size. + - The length must be less than or equal to the value of MAXIMUM_TRANSFER_SIZE retrieved by GetPipePolicy. + - Disabling RAW_IO (FALSE) does not impose any restriction on the buffers that are passed to \a ReadPipe. + + - RESET_PIPE_ON_RESUME (0x09) + - The default value is FALSE (zero). To enable RESET_PIPE_ON_RESUME, in Value pass the address of a caller-allocated UCHAR variable set to TRUE (nonzero). + - TRUE (or a nonzero value) indicates that on resume from suspend, winUSB resets the endpoint before it allows the caller to send new requests to the endpoint. + + + \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) +{ + if (m_winUsb.SetPipePolicy) + return m_winUsb.SetPipePolicy(InterfaceHandle, PipeID, PolicyType, ValueLength, Value); + else + return FALSE; +} + +/*! \brief Gets the policy for a specific pipe (endpoint). + \param[in] InterfaceHandle An initialized usb handle, see \a Initialize. + \param[in] PipeID An 8-bit value that consists of a 7-bit address and a direction bit. This parameter corresponds to the bEndpointAddress field in the endpoint descriptor. + \param[in] PolicyType A UINT variable that specifies the policy parameter to retrieve. The current value for the policy parameter is retrieved the Value parameter. + \param[in,out] ValueLength A pointer to the size, in bytes, of the buffer that Value points to. On output, ValueLength receives the size, in bytes, of the data that was copied into the Value buffer. + \param[out] Value A pointer to a buffer that receives the specified pipe policy value. + + \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) +{ + if (m_winUsb.GetPipePolicy) + return m_winUsb.GetPipePolicy(InterfaceHandle, PipeID, PolicyType, ValueLength, Value); + else + return FALSE; +} + +/*! \brief Reads data from the specified pipe. + \param[in] InterfaceHandle An initialized usb handle, see \a Initialize. + \param[in] PipeID An 8-bit value that consists of a 7-bit address and a direction bit. This parameter corresponds to the bEndpointAddress field in the endpoint descriptor. + \param[out] Buffer A caller-allocated buffer that receives the data that is read. + \param[in] BufferLength The maximum number of bytes to read. This number must be less than or equal to the size, in bytes, of Buffer. + \param[out] LengthTransferred A pointer to a UINT variable that receives the actual number of bytes that were copied into Buffer. For more information, see Remarks. + \param[in] Overlapped An optional pointer to an overlapped structure for asynchronous operations. This can be a KOVL_HANDLE or a pointer to a standard windows OVERLAPPED structure. + If this parameter is specified, \a ReadPipe returns immediately rather than waiting synchronously for the operation to complete before returning. An event is signaled when the operation is complete. + + \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) +{ + if (m_winUsb.ReadPipe) + return m_winUsb.ReadPipe(InterfaceHandle, PipeID, Buffer, BufferLength, LengthTransferred, Overlapped); + else + return FALSE; +} + +/*! \brief Writes data to a pipe. + \param[in] InterfaceHandle An initialized usb handle, see \a Initialize. + \param[in] PipeID An 8-bit value that consists of a 7-bit address and a direction bit. This parameter corresponds to the bEndpointAddress field in the endpoint descriptor. + \param[in] Buffer A caller-allocated buffer the data is written from. + \param[in] BufferLength The maximum number of bytes to write. This number must be less than or equal to the size, in bytes, of Buffer. + \param[out] LengthTransferred A pointer to a UINT variable that receives the actual number of bytes that were transferred from Buffer. + \param[in] Overlapped An optional pointer to an overlapped structure for asynchronous operations. This can be a KOVL_HANDLE or a pointer to a standard windows OVERLAPPED structure. + If this parameter is specified, \a WritePipe returns immediately rather than waiting synchronously for the operation to complete before returning. An event is signaled when the operation is complete. + + \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) +{ + if (m_winUsb.WritePipe) + return m_winUsb.WritePipe(InterfaceHandle, PipeID, Buffer, BufferLength, LengthTransferred, Overlapped); + else + return FALSE; +} + +/*! \brief Transmits control data over a default control endpoint. + A ControlTransfer is never cached. These requests always go directly to the usb device. + + \param[in] InterfaceHandle A valid winUSB interface handle returned by: + - Initialize + - GetAssociatedInterface + \param[in] SetupPacket The 8-byte setup packet of type WINUSB_SETUP_PACKET. + \param[in,out] Buffer A caller-allocated buffer that contains the data to transfer. + \param[in] BufferLength The number of bytes to transfer, not including the setup packet. This number must be less than or equal to the size, in bytes, of Buffer. + \param[out] LengthTransferred A pointer to a UINT variable that receives the actual number of transferred bytes. If the application does not expect any data to be transferred during the data phase (BufferLength is zero), LengthTransferred can be NULL. + \param[in] Overlapped An optional pointer to an OVERLAPPED structure, which is used for asynchronous operations. If this parameter is specified, ControlTransfer immediately returns, and the event is signaled when the operation is complete. If Overlapped is not supplied, the ControlTransfer function transfers data synchronously. + + \returns On success, TRUE. Otherwise FALSE. Use GetLastError() to get extended error information. If an Overlapped member is supplied and the operation succeeds this function returns FALSE and sets last error to ERROR_IO_PENDING. +*/ +BOOL XsWinUsb::ControlTransfer(WINUSB_INTERFACE_HANDLE InterfaceHandle, WINUSB_SETUP_PACKET SetupPacket, PUCHAR Buffer, ULONG BufferLength, PULONG LengthTransferred, LPOVERLAPPED Overlapped) +{ + if (m_winUsb.ControlTransfer) + return m_winUsb.ControlTransfer(InterfaceHandle, SetupPacket, Buffer, BufferLength, LengthTransferred, Overlapped); + else + return FALSE; +} + +/*! \brief Resets the data toggle and clears the stall condition on a pipe. + \param[in] InterfaceHandle An initialized usb handle, see \a Initialize. + \param[in] PipeID An 8-bit value that consists of a 7-bit address and a direction bit. This parameter corresponds to the \a bEndpointAddress field in the endpoint descriptor. + + \returns On success, TRUE. Otherwise FALSE. Use GetLastError() to get extended error information. +*/ +BOOL XsWinUsb::ResetPipe(WINUSB_INTERFACE_HANDLE InterfaceHandle, UCHAR PipeID) +{ + if (m_winUsb.ResetPipe) + return m_winUsb.ResetPipe(InterfaceHandle, PipeID); + else + return FALSE; +} + +/*! \brief Aborts all of the pending transfers for a pipe. + \param[in] InterfaceHandle An initialized usb handle, see \a Initialize. + \param[in] PipeID An 8-bit value that consists of a 7-bit address and a direction bit. This parameter corresponds to the \a bEndpointAddress field in the endpoint descriptor. + + \returns On success, TRUE. Otherwise FALSE. Use GetLastError() to get extended error information. +*/ +BOOL XsWinUsb::AbortPipe(WINUSB_INTERFACE_HANDLE InterfaceHandle, UCHAR PipeID) +{ + if (m_winUsb.AbortPipe) + return m_winUsb.AbortPipe(InterfaceHandle, PipeID); + else + return FALSE; +} + +/*! \brief Discards any data that is cached in a pipe. + \param[in] InterfaceHandle An initialized usb handle, see \a Initialize. + \param[in] PipeID An 8-bit value that consists of a 7-bit address and a direction bit. This parameter corresponds to the \a bEndpointAddress field in the endpoint descriptor. + + \returns On success, TRUE. Otherwise FALSE. Use GetLastError() to get extended error information. +*/ +BOOL XsWinUsb::FlushPipe(WINUSB_INTERFACE_HANDLE InterfaceHandle, UCHAR PipeID) +{ + if (m_winUsb.FlushPipe) + return m_winUsb.FlushPipe(InterfaceHandle, PipeID); + else + return FALSE; +} + +/*! \brief Sets the power policy for a device. + The following list summarizes the effects of changes to power management states: + + - All pipe handles, interface handles, locks, and alternate settings are preserved across power management events. + - Any transfers that are in progress are suspended when a device transfers to a low power state, and they are resumed when the device is restored to a working state. + - The device and system must be in a working state before the client can restore a device-specific configuration. Clients can determine whether the device and system are in a working state from the WM_POWERBROADCAST message. + - The client can indicate that an interface is idle by calling \a SetPowerPolicy. + + \param[in] InterfaceHandle An initialized usb handle, see \a Initialize. + \param[in] PolicyType A value that specifies the power policy to set. The following table describes symbolic constants. + - AUTO_SUSPEND (0x81) + - Specifies the auto-suspend policy type; the power policy parameter must be specified by the caller in the Value parameter. + - For auto-suspend, the Value parameter must point to a UCHAR variable. + - If Value is TRUE (nonzero), the USB stack suspends the device if the device is idle. A device is idle if there are no transfers pending, or if the only pending transfers are IN transfers to interrupt or bulk endpoints. + - The default value is determined by the value set in the DefaultIdleState registry setting. By default, this value is TRUE. + + - SUSPEND_DELAY (0x83) + - Specifies the suspend-delay policy type; the power policy parameter must be specified by the caller in the Value parameter. + - For suspend-delay, Value must point to a UINT variable. + - Value specifies the minimum amount of time, in milliseconds, that the driver must wait post transfer before it can suspend the device. + - The default value is determined by the value set in the DefaultIdleTimeout registry setting. By default, this value is five seconds. + + \param[in] ValueLength The size, in bytes, of the buffer at Value. + \param[in] Value The new value for the power policy parameter. Data type and value for Value depends on the type of power policy passed in PolicyType. For more information, see PolicyType. + + \returns On success, TRUE. Otherwise FALSE. Use GetLastError() to get extended error information. +*/ +BOOL XsWinUsb::SetPowerPolicy(WINUSB_INTERFACE_HANDLE InterfaceHandle, ULONG PolicyType, ULONG ValueLength, PVOID Value) +{ + if (m_winUsb.SetPowerPolicy) + return m_winUsb.SetPowerPolicy(InterfaceHandle, PolicyType, ValueLength, Value); + else + return FALSE; +} + +/*! \brief Gets the power policy for a device. + \param[in] InterfaceHandle An initialized usb handle, see \a Initialize + \param[in] PolicyType A value that specifies the power policy parameter to retrieve in Value. The following table describes symbolic constants that are valid. + - AUTO_SUSPEND (0x81) + - If the caller specifies a power policy of AUTO_SUSPEND, \a GetPowerPolicy returns the value of the auto suspend policy parameter in the Value parameter. + - If Value is TRUE (that is, nonzero), the USB stack suspends the device when no transfers are pending or the only transfers pending are IN transfers on an interrupt or bulk endpoint. + - The value of the DefaultIdleState registry value determines the default value of the auto suspend policy parameter. + - The Value parameter must point to a UCHAR variable. + + - SUSPEND_DELAY (0x83) + - If the caller specifies a power policy of SUSPEND_DELAY, \a GetPowerPolicy returns the value of the suspend delay policy parameter in Value. + - The suspend delay policy parameter specifies the minimum amount of time, in milliseconds, that the driver must wait after any transfer before it can suspend the device. + - Value must point to a UINT variable. + + \param[in,out] ValueLength A pointer to the size of the buffer that Value. On output, ValueLength receives the size of the data that was copied into the Value buffer. + \param[out] Value A buffer that receives the specified power policy parameter. For more information, see PolicyType. + + \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) +{ + if (m_winUsb.GetPowerPolicy) + return m_winUsb.GetPowerPolicy(InterfaceHandle, PolicyType, ValueLength, Value); + else + return FALSE; +} + +/*! \brief Retrieves the results of an overlapped operation on the specified winUSB handle. + + This function is like the Win32 API routine, GetOverlappedResult, with one difference; instead of passing a file handle that is returned from CreateFile, the caller passes an interface handle that is returned from \a Initialize, or \a GetAssociatedInterface. The caller can use either API routine, if the appropriate handle is passed. The \a GetOverlappedResult function extracts the file handle from the interface handle and then calls GetOverlappedResult. + The results that are reported by the GetOverlappedResult function are those from the specified handle's last overlapped operation to which the specified standard windows OVERLAPPED structure was provided, and for which the operation's results were pending. A pending operation is indicated when the function that started the operation returns FALSE, and the GetLastError routine returns ERROR_IO_PENDING. When an I/O operation is pending, the function that started the operation resets the hEvent member of the standard windows OVERLAPPED structure to the nonsignaled state. Then when the pending operation has been completed, the system sets the event object to the signaled state. + The caller can specify that an event object is manually reset in the standard windows OVERLAPPED structure. If an automatic reset event object is used, the event handle must not be specified in any other wait operation in the interval between starting the overlapped operation and the call to \a GetOverlappedResult. For example, the event object is sometimes specified in one of the wait routines to wait for the operation to be completed. When the wait routine returns, the system sets an auto-reset event's state to nonsignaled, and a successive call to \a GetOverlappedResult with the bWait parameter set to TRUE causes the function to be blocked indefinitely. + If the bWait parameter is TRUE, GetOverlappedResult determines whether the pending operation has been completed by waiting for the event object to be in the signaled state. + + If the hEvent member of the standard windows OVERLAPPED structure is NULL, the system uses the state of the file handle to signal when the operation has been completed. Do not use file handles for this purpose. It is better to use an event object because of the confusion that can occur when multiple concurrent overlapped operations are performed on the same file. In this situation, you cannot know which operation caused the state of the object to be signaled. + + \param[in] InterfaceHandle An initialized usb handle, see \a Initialize + \param[in] Overlapped A pointer to a standard windows OVERLAPPED structure that was specified when the overlapped operation was started. + \param[out] lpNumberOfBytesTransferred A pointer to a variable that receives the number of bytes that were actually transferred by a read or write operation. + \param[in] bWait If this parameter is TRUE, the function does not return until the operation has been completed. If this parameter is FALSE and the operation is still pending, the function returns FALSE and the GetLastError function returns ERROR_IO_INCOMPLETE. + + \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) +{ + if (m_winUsb.GetOverlappedResult) + return m_winUsb.GetOverlappedResult(InterfaceHandle, lpOverlapped, lpNumberOfBytesTransferred, bWait); + else + return FALSE; +} + +#endif // USE_WINUSB diff --git a/extern/xspublic/xscontroller/xswinusb.h b/extern/xspublic/xscontroller/xswinusb.h new file mode 100644 index 0000000..8fd91ba --- /dev/null +++ b/extern/xspublic/xscontroller/xswinusb.h @@ -0,0 +1,130 @@ + +// Copyright (c) 2003-2022 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 XSWINUSB +#define XSWINUSB + +#include "xscontrollerconfig.h" + +#ifdef USE_WINUSB + +#include +#include + +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); + +class XsWinUsb +{ +public: + XsWinUsb(void); + ~XsWinUsb(void); + + 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; + +private: + + 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_API; + + WINUSB_API m_winUsb; + XsLibraryLoader* m_libraryLoader; + + void initLibrary(); + + XsWinUsb(XsWinUsb const&) = delete; + XsWinUsb const& operator=(XsWinUsb const&) = delete; +}; + +#endif // USE_WINUSB + +#endif diff --git a/extern/xspublic/xstypes/datapacket_p.cpp b/extern/xspublic/xstypes/datapacket_p.cpp new file mode 100644 index 0000000..aa900bf --- /dev/null +++ b/extern/xspublic/xstypes/datapacket_p.cpp @@ -0,0 +1,168 @@ + +// Copyright (c) 2003-2022 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 "datapacket_p.h" + +/*! \cond XS_INTERNAL */ + +/*! \class DataPacketPrivate + \brief Internal administration for contained data of XsDataPacket class. + \details This is only the part that can be stored in an XsMessage, so no TOA and computed packet IDs +*/ + +volatile std::atomic_int DataPacketPrivate::m_created(0); +volatile std::atomic_int DataPacketPrivate::m_destroyed(0); + +/*! \brief Copy constructor */ +DataPacketPrivate::DataPacketPrivate(DataPacketPrivate const& p) + : MapType() // start with clean map + , m_refCount(1) // this is a new object so the ref count is 1 +{ + ++m_created; + *this = p; // does NOT manipulate the ref count +} + +/*! \brief Destructor */ +DataPacketPrivate::~DataPacketPrivate() +{ + ++m_destroyed; + try + { + clear(); + } + catch (...) + { + } +} + +/*! \brief Assignment operator + \param p The item to copy from + \note This does NOT manipulate the ref count and it shouldn't. + \return A reference to this +*/ +DataPacketPrivate& DataPacketPrivate::operator = (const DataPacketPrivate& p) +{ + if (this != &p) + { + clear(); + for (auto i : p) + insert(i.first, i.second->clone()); + } + return *this; +} + +/*! \brief Clear the contents */ +void DataPacketPrivate::clear() +{ + for (auto it : *this) + delete it.second; + MapType::clear(); +} + +/*! \brief Find the item matching \a id + \details This function will return an iterator to the item that matches \a id or end() if it could not find it. + The function uses a loose comparison to ensure entry ambiguity. As a result, the returned iterator may have a + slightly different id than the supplied one. + \param id The item to search for. + \return An iterator to the requested item or end() +*/ +MapType::const_iterator DataPacketPrivate::find(XsDataIdentifier id) const +{ + return MapType::find(id & XDI_FullTypeMask); +} + +/*! \brief Add or overwrite the item with \a id + \details The function will create a new item or overwite the current item, properly cleaning up existing data + if necessary. + \param id The id to attach to the data. + \param var The data to store. + \return An internal iterator pointing to the inserted or updated item. +*/ +MapType::iterator DataPacketPrivate::insert(XsDataIdentifier id, XsDataPacket_Private::Variant* var) +{ + id = id & XDI_FullTypeMask; + auto it = MapType::lower_bound(id); + if (it != end() && it->first == id) + { + delete it->second; + it->second = var; + return it; + } + else + return MapType::insert(it, std::make_pair(id & XDI_FullTypeMask, var)); +} + +/*! \brief Remove the item with \a id if it exists, cleaning up associated data */ +void DataPacketPrivate::erase(XsDataIdentifier id) +{ + auto it = find(id); + if (it != end()) + erase(it); +} + +/*! \brief Remove the item at \a it, cleaning up associated data */ +void DataPacketPrivate::erase(MapType::const_iterator const& it) +{ + delete it->second; + MapType::erase(it); +} + +/*! \brief Merge \a other into this + \details The function will copy all items from \a other into this. Existing items will only be overwritten if + \a overwrite is set to true. This function does not detect conflicts between closely related but IDs (such as + frame range and packet counter). This is left to the caller. + \param other The other data to merge into this packet + \param overwrite When set to true, the contents of \a other will overwrite existing data. When set to false, only new items will be added. +*/ +void DataPacketPrivate::merge(DataPacketPrivate const& other, bool overwrite) +{ + if (overwrite) + for (auto i : other) + insert(i.first, i.second->clone()); + else + { + for (auto i : other) + { + auto j = find(i.first); + if (j == end()) + insert(i.first, i.second->clone()); + } + } +} + +/*! \brief Returns the difference between created and destroyed DataPacketPrivate objects, for debugging purposes only */ +int DataPacketPrivate::creationDiff() +{ + return m_created.load() - m_destroyed.load(); +} + +/*! \endcond XS_INTERNAL */ diff --git a/extern/xspublic/xstypes/datapacket_p.h b/extern/xspublic/xstypes/datapacket_p.h new file mode 100644 index 0000000..a51d384 --- /dev/null +++ b/extern/xspublic/xstypes/datapacket_p.h @@ -0,0 +1,1034 @@ + +// Copyright (c) 2003-2022 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 DATAPACKET_P_H +#define DATAPACKET_P_H + +#include "xsmessage.h" +#include "xsdeviceid.h" +#include "xstimestamp.h" +#include +#include +#include "xsquaternion.h" +#include "xsushortvector.h" +#include "xsvector3.h" +#include "xsscrdata.h" +#include "xstriggerindicationdata.h" +#include "xseuler.h" +#include "xsmatrix3x3.h" +#include "xsrange.h" +#include "xstimeinfo.h" +#include "xsrawgnsspvtdata.h" +#include "xsrawgnsssatinfo.h" +#include "xsbytearray.h" +#include "xsglovesnapshot.h" +#include "xsglovedata.h" + +#include "xssnapshot.h" + +/*! \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 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; + } + +private: + XsDataIdentifier m_id; +}; + +/*! \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 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; + + /*! \brief Return the size the Variant would have in a message */ + XsSize sizeInMsg() const override + { + return (XsSize)(ptrdiff_t) XsMessage::sizeInMsg(dataId(), C); + } +}; + +/*! \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; +} + +/*! \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 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); + } +}; + +/*! \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 + \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 +{ + /*! \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) {} + + Variant* clone() const override + { + return new XsQuaternionVariant(dataId(), m_data); + } +}; + +/*! \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 + { + return new XsUShortVectorVariant(dataId(), m_data); + } +}; + +/*! \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) + { + assert(val.size() == 3); + } + Variant* clone() const override + { + return new XsVector3Variant(dataId(), m_data); + } +}; + +/*! \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) + { + assert(val.size() == 2); + } + Variant* clone() const override + { + return new XsVector2Variant(dataId(), m_data); + } +}; + +/*! \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); + } + + 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); + } + + XsSize sizeInMsg() const override + { + return 10 * sizeof(uint16_t); + } +}; + +/*! \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); + } + + 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); + } + + XsSize sizeInMsg() const override + { + return 8; + } +}; + +/*! \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 + { + return new XsEulerVariant(dataId(), m_data); + } +}; + +/*! \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); + } + + 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 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); + } + + 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); + } + + XsSize sizeInMsg() const override + { + return 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); + } + + 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); + + // 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); + + 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 const* bareByte = (uint8_t const*) &m_data.m_month; + for (XsSize i = 0; i < 6; ++i) + XsMessage_setDataByte(&msg, bareByte[i], offset + 6 + i); + + // utcOffset is ignored and assumed to be 0, use makeUtc to ensure this if necessary + } + + XsSize sizeInMsg() const override + { + return 12; + } +}; + +/*! \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_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); + XsMessage_setDataShort(&msg, m_data.m_vdop, offset + 86); + 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; + } +}; + +/*! \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 + { + return new XsRawGnssSatInfoVariant(dataId(), m_data); + } + + 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_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; + } + 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) + { + 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; + } + } + + XsSize sizeInMsg() const override + { + return 8 + 4 * m_data.m_numSvs; + } +}; + +/*! \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) {} + + Variant* clone() const override + { + return new XsFullSnapshotVariant(dataId(), m_data); + } + + 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; + } + + 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); + } + + XsSize sizeInMsg() const override + { + return 70; + } +}; + +/*! \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); + } + + 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); + } + + XsSize sizeInMsg() const override + { + return 46; + } +}; + +/*! \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 < 255) + { + m_data.assign(dSize, XsMessage_getDataBuffer(&msg, offset)); + return dSize; + } + 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); + + sOffset = 0; + while (dSize == 255) + { + 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; + } + if (dSize) + { + memcpy(m_data.data() + sOffset, XsMessage_getDataBuffer(&msg, offset), dSize); + total += dSize; + } + return total; + } + } + else + { + m_data.clear(); + return 0; + } + } + void writeToMessage(XsMessage& msg, XsSize offset) const override + { + if (m_data.size()) + { + 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) + { + 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(); + } +}; + +/*! \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 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) + { + 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_flags; + m_data.m_fingers[i].m_flags = 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); + } + + XsSize sizeInMsg() const override + { + return sizeof(XsGloveSnapshot); + } +}; + +/*! \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); + } + + 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(DataPacketPrivate const&); + ~DataPacketPrivate(); + DataPacketPrivate& operator = (const DataPacketPrivate& p); + void erase(XsDataIdentifier id); + void erase(MapType::const_iterator const& it); + MapType::iterator insert(XsDataIdentifier id, XsDataPacket_Private::Variant* var); + + void clear(); + void merge(DataPacketPrivate const& other, bool overwrite); + + MapType::const_iterator find(XsDataIdentifier id) const; + + using MapType::begin; + using MapType::end; + using MapType::size; + using MapType::empty; + mutable volatile std::atomic_int m_refCount; //!< The reference count for this DataPacketPrivate. + static volatile std::atomic_int m_created; //!< The number of DataPacketPrivate objects created so far. \sa creationDiff() + static volatile std::atomic_int m_destroyed; //!< The number of DataPacketPrivate objects destroyed so far. \sa creationDiff() + + static int creationDiff(); +}; + +/*! \endcond */ + +#endif diff --git a/extern/xspublic/xstypes/pstdint.h b/extern/xspublic/xstypes/pstdint.h new file mode 100644 index 0000000..e340848 --- /dev/null +++ b/extern/xspublic/xstypes/pstdint.h @@ -0,0 +1,913 @@ + +// Copyright (c) 2003-2022 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. +// + +/* 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 + +*/ +#ifdef SWIG +#include +#elif !defined(_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_. +*/ + +#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 +#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 + 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) + +#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 + +#endif + +#if defined (__TEST_PSTDINT_FOR_CORRECTNESS) + +/* + Please compile with the maximum warning settings to make sure macros are not + defined more than once. +*/ + +#include +#include +#include + +#define glue3_aux(x,y,z) x ## y ## z +#define glue3(x,y,z) glue3_aux(x,y,z) + +#define DECLU(bits) glue3(uint,bits,_t) glue3(u,bits,=) glue3(UINT,bits,_C) (0); +#define DECLI(bits) glue3(int,bits,_t) glue3(i,bits,=) glue3(INT,bits,_C) (0); + +#define DECL(us,bits) glue3(DECL,us,) (bits) + +#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) +#ifdef INT64_MAX + 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); +#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); +#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); + + TESTUMAX(8); + TESTUMAX(16); + TESTUMAX(32); +#ifdef INT64_MAX + TESTUMAX(64); +#endif + + return EXIT_SUCCESS; +} + +#endif + +#if defined(_STDINT) && defined(_MSC_VER) + #define stdint_int64_defined +#endif + +#endif diff --git a/extern/xspublic/xstypes/resource.h b/extern/xspublic/xstypes/resource.h new file mode 100644 index 0000000..75d79ad --- /dev/null +++ b/extern/xspublic/xstypes/resource.h @@ -0,0 +1,47 @@ + +// Copyright (c) 2003-2022 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. +// + +//{{NO_DEPENDENCIES}} +// Microsoft Visual C++ generated include file. +// Used by C:\dev\trunk_svn\xstypes\xstypes.rc +// + +// 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 +#endif diff --git a/extern/xspublic/xstypes/xsanalogindata.h b/extern/xspublic/xstypes/xsanalogindata.h new file mode 100644 index 0000000..ed75e8c --- /dev/null +++ b/extern/xspublic/xstypes/xsanalogindata.h @@ -0,0 +1,60 @@ + +// Copyright (c) 2003-2022 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 XSANALOGINDATA_H +#define XSANALOGINDATA_H + +#include "pstdint.h" + +/*! \brief Data from analog inputs from sensors. */ +struct XsAnalogInData +{ + uint16_t m_data; /*!< \brief The data */ +#ifdef __cplusplus + /*! \brief Construct a nulled analog data item */ + inline XsAnalogInData() : m_data(0) + {} + + /*! \brief Construct analog-data with value \a data */ + inline XsAnalogInData(uint16_t data) : m_data(data) + {} + + /*! \brief Equality comparison */ + inline bool operator == (XsAnalogInData const& other) const + { + return m_data == other.m_data; + } +#endif +}; +typedef struct XsAnalogInData XsAnalogInData; + +#endif diff --git a/extern/xspublic/xstypes/xsarray.c b/extern/xspublic/xstypes/xsarray.c new file mode 100644 index 0000000..35f90cc --- /dev/null +++ b/extern/xspublic/xstypes/xsarray.c @@ -0,0 +1,724 @@ + +// Copyright (c) 2003-2022 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 "xsarray.h" +#include +#include +#include "xsdebugcounters.h" + +/*! \class XsArray + \brief Provides generic storage for data in an array and manipulation operations on that data +*/ + +/*! \addtogroup cinterface C Interface + @{ +*/ + +/*! \cond NODOXYGEN */ +#define elemSize(thisArray) (thisArray->m_descriptor->itemSize) +#define elemAtX(b, i, thisArray) ((void*)(((char*) (b))+((i)*elemSize(thisArray)))) +#define elemAt(b, i) elemAtX(b, i, thisArray) +/*! \endcond */ + +#ifdef DOXYGEN + /*! \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); +#endif + +/*! \relates XsArray + \brief Initializes the XsArray 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 descriptor The descriptor of the data in the list + \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_construct(void* thisPtr, XsArrayDescriptor const* const descriptor, XsSize count, void const* src) +{ + XsArray* thisArray = (XsArray*) thisPtr; + *((XsArrayDescriptor const**) &thisArray->m_descriptor) = descriptor; + *((XsSize*) &thisArray->m_size) = count; + + if (thisArray->m_size) + { + // init to size + *((void**) &thisArray->m_data) = malloc(thisArray->m_size * elemSize(thisArray)); + if (!thisArray->m_data) + { + // out of memory + *((XsSize*) &thisArray->m_size) = 0; + *((XsSize*) &thisArray->m_reserved) = 0; + *((XsSize*) &thisArray->m_flags) = XSDF_Managed | XSDF_Empty | XSDF_BadAlloc; + return; + } + + 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; 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; i < thisArray->m_size; ++i) + thisArray->m_descriptor->itemConstruct(elemAt(thisArray->m_data, i)); + } + } + else + *((void**) &thisArray->m_data) = 0; + *((XsSize*) &thisArray->m_reserved) = thisArray->m_size; + *((XsSize*) &thisArray->m_flags) = XSDF_Managed; +} + +/*! \relates XsArray + \brief Initializes the XsArray with a copy of \a src + \param src A pointer to the objects to copy. The object may be empty, but src may not be 0 +*/ +void XsArray_copyConstruct(void* thisPtr, void const* src) +{ + XsArray* thisArray = (XsArray*) thisPtr; + XsArray const* srcArray = (XsArray const*) src; + assert(srcArray); + XsArray_construct(thisArray, srcArray->m_descriptor, srcArray->m_size, srcArray->m_data); +} + +/*! \relates XsArray + \brief Clears and frees memory allocated by the XsArray + \note After XsArray_destruct is called, the object is empty but valid, + ie. it can be used as if XsArray_construct has been called on it. +*/ +void XsArray_destruct(void* thisPtr) +{ + XsArray* thisArray = (XsArray*) thisPtr; + if (thisArray->m_data && (thisArray->m_flags & XSDF_Managed)) + { + XsSize i; + // clear contents + if (thisArray->m_descriptor->itemDestruct) + for (i = 0; i < thisArray->m_reserved; ++i) + thisArray->m_descriptor->itemDestruct(elemAt(thisArray->m_data, i)); + free((void*) thisArray->m_data); + XsArray_incFreeCount(); + } + // init to 0 + *((void**) &thisArray->m_data) = 0; + *((XsSize*) &thisArray->m_size) = 0; + *((XsSize*) &thisArray->m_reserved) = 0; + *((XsSize*) &thisArray->m_flags) = (thisArray->m_flags & (XSDF_Managed | XSDF_FixedSize)); +} + +/*! \relates XsArray + \brief Reinitializes the XsArray with space for \a count items and copies them from \a src + \details This function reinitializes the object reserving space for at least \a count items in the + buffer. \a count may be 0. If \a src is not 0, \a count items will be copied from \a src. + Previous data will be cleared automatically, but the reserved space will not be reduced. + \param count the number of items in src + \param src a pointer to an array of items + \sa XsArray_reserve +*/ +void XsArray_assign(void* thisPtr, XsSize count, void const* src) +{ + XsSize i; + XsArray* thisArray = (XsArray*) thisPtr; + + // check if we need to reallocate + if (count > thisArray->m_reserved) + { + assert(thisArray->m_flags & XSDF_Managed); + + if (thisArray->m_data) + XsArray_destruct(thisArray); + XsArray_construct(thisArray, thisArray->m_descriptor, count, src); + return; + } + + // no reallocation necessary, clear excess objects + if (thisArray->m_descriptor->itemDestruct) + 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; i < count; ++i) + thisArray->m_descriptor->itemCopy(elemAt(thisArray->m_data, i), elemAt(src, i)); + } + } + + *((XsSize*) &thisArray->m_size) = count; +} + +/*! \relates XsArray + \brief Resizes the existing list to \a count items + \details This function will keep the data of the remaining items intact. + \param count the number of items the list should have + \sa XsArray_reserve \sa XsArray_assign +*/ +void XsArray_resize(void* thisPtr, XsSize count) +{ + XsArray* thisArray = (XsArray*) thisPtr; + if (thisArray->m_size == count) + return; + if (thisArray->m_size == 0) + { + XsArray_assign(thisArray, count, 0); + return; + } + if (count < thisArray->m_size) + { + XsArray_erase(thisArray, count, thisArray->m_size - count); + return; + } + if (count > thisArray->m_reserved) + XsArray_reserve(thisArray, count); + *((XsSize*) &thisArray->m_size) = count; +} + +/*! \relates XsArray + \brief Reserves space for \a count items + \details This function reserves space for at least \a count items. The function will retain the + current data in the list. + \param count The number of items to reserve space for. When set to 0 or 1, the buffer is resized to the + minimum required size to contain the current contents. Otherwise reserving data will only + expand the buffer, never shrink it. + \sa XsArray_assign +*/ +void XsArray_reserve(void* thisPtr, XsSize count) +{ + XsArray* thisArray = (XsArray*) thisPtr; + XsArray tmp = { 0, thisArray->m_size, 0, XSDF_Managed, thisArray->m_descriptor }; + XsSize i; + + if (count && count <= thisArray->m_reserved) + return; + + if (count < thisArray->m_size) + count = thisArray->m_size; + + if (count == thisArray->m_reserved) + return; + + if (!(thisArray->m_flags & XSDF_Managed)) + { + // attempting this on an unmanaged list is ignored silently + return; + } + + if (!count) + { + XsArray_destruct(thisArray); + return; + } + + *((XsSize*) &tmp.m_reserved) = count; + + // init to size + *((void**) &tmp.m_data) = malloc(tmp.m_reserved * elemSize(thisArray)); + if (!tmp.m_data) + { + // bad alloc clears the array + XsArray_destruct(thisArray); + *((XsSize*) &thisArray->m_flags) = XSDF_Managed | XSDF_Empty | XSDF_BadAlloc; + return; + } + XsArray_incAllocCount(); + + if (thisArray->m_descriptor->itemConstruct) + for (i = 0; i < tmp.m_reserved; ++i) + thisArray->m_descriptor->itemConstruct(elemAt(tmp.m_data, 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); + XsArray_swap(thisArray, &tmp); +} + +/*! \relates XsArray + \brief Copy the contents of \a src to thisArray */ +void XsArray_copy(void* thisPtr, void const* src) +{ + XsArray* thisArray = (XsArray*) thisPtr; + XsArray const* srcArray = (XsArray const*) src; + + if (srcArray == thisArray) + return; + XsArray_assign(thisArray, srcArray->m_size, srcArray->m_data); +} + +/*! \relates XsArray + \brief Appends the \a other list to thisArray list + \param other The list to append to thisArray list. \a other may point to thisArray list +*/ +void XsArray_append(void* thisPtr, void const* other) +{ + XsSize i; + XsArray* thisArray = (XsArray*) thisPtr; + XsArray const* otherArray = (XsArray const*) other; + + if (otherArray->m_size == 0) + return; + + if (otherArray == thisArray) + { + if (thisArray->m_size + thisArray->m_size > thisArray->m_reserved) + 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; 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; + return; + } + + if (thisArray->m_size == 0) + { + XsArray_copy(thisArray, otherArray); + return; + } + + if (thisArray->m_size + otherArray->m_size > thisArray->m_reserved) + { + XsArray_reserve(thisArray, thisArray->m_size + otherArray->m_size); // maybe reserve more here? + if (thisArray->m_flags & XSDF_BadAlloc) + return; + } + + 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; 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; +} + +/*! \relates XsArray + \brief Insert \a count items from \a src at \a index in the array + \param index The index to use for inserting. Anything beyond the end of the array (ie. -1) will + append to the actual end of the array. + \param count The number of items to insert + \param src The items to insert, may not be 0 unless count is 0 +*/ +void XsArray_insert(void* thisPtr, XsSize index, XsSize count, void const* src) +{ + XsSize s; + XsArray* thisArray = (XsArray*) thisPtr; + 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 + if (thisArray->m_flags & XSDF_BadAlloc) + return; + } + + // 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)); + + // 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)); + } + + // update size + *((XsSize*) &thisArray->m_size) = thisArray->m_size + count; +} + +/*! \relates XsArray + \brief Swap the contents of \a a with those of \a b + \details Where possible, the pointers and administrative values are swapped. If for some reason + thisArray is not possible, the lists are swapped one element at a time. + \param a The list to swap with \a b + \param b The list to swap with \a a +*/ +void XsArray_swap(void* a, void* b) +{ + XsArray* aArray = (XsArray*) a; + XsArray* bArray = (XsArray*) b; + + if (!aArray->m_data && !bArray->m_data) + return; + if ((!aArray->m_data || (aArray->m_flags & XSDF_Managed)) && (!bArray->m_data || (bArray->m_flags & XSDF_Managed))) + { + // administrative swap + XsArray tmp; + *((void**) &tmp.m_data) = aArray->m_data; + *((void**) &aArray->m_data) = bArray->m_data; + *((void**) &bArray->m_data) = tmp.m_data; + + *((XsSize*) &tmp.m_size) = aArray->m_size; + *((XsSize*) &aArray->m_size) = bArray->m_size; + *((XsSize*) &bArray->m_size) = tmp.m_size; + + *((XsSize*) &tmp.m_reserved) = aArray->m_reserved; + *((XsSize*) &aArray->m_reserved) = bArray->m_reserved; + *((XsSize*) &bArray->m_reserved) = tmp.m_reserved; + + *((XsSize*) &tmp.m_flags) = aArray->m_flags; + *((XsSize*) &aArray->m_flags) = bArray->m_flags; + *((XsSize*) &bArray->m_flags) = tmp.m_flags; + } + else + { + // elementwise swap + XsSize i; + assert(aArray->m_size == bArray->m_size); + for (i = 0; i < aArray->m_size; ++i) + aArray->m_descriptor->itemSwap(elemAtX(aArray->m_data, i, aArray), elemAtX(bArray->m_data, i, bArray)); + } +} + +/*! \relates XsArray + \brief Removes a \a count items from the list starting at \a index +*/ +void XsArray_erase(void* thisPtr, XsSize index, XsSize count) +{ + XsSize i, newCount; + XsArray* thisArray = (XsArray*) thisPtr; + + if (count == 0 || index >= thisArray->m_size) + return; + + 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)); + + *((XsSize*) &thisArray->m_size) = newCount; +} + +/*! \relates XsArray + \brief Returns non-zero if the lists are different, 0 if they're equal + \details This function compares the two lists in-order + \param a The left hand side of the comparison + \param b The right hand side of the comparison + \return -1 if \a a is smaller in some way than \a b, 1 if it is larger in some way + and 0 if both lists are equal. Please note that not all lists have items that can be accurately + tested for less than or greater than, but can be tested for (in-)equality. So the sign of the + return value should be treated with knowledge of the data type in mind. + \sa XsArray_compareSet \sa XsArray_comparePredicate +*/ +int XsArray_compare(void const* a, void const* b) +{ + XsSize i; + XsArray const* aArray = (XsArray const*) a; + XsArray const* bArray = (XsArray const*) b; + + if (aArray == bArray) + return 0; + + if (aArray->m_size != bArray->m_size) + 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 + for (i = 0; i < aArray->m_size; ++i) // loop over all elements of the lists + { + int r = aArray->m_descriptor->itemCompare(elemAtX(aArray->m_data, i, aArray), elemAtX(bArray->m_data, i, bArray)); + if (r) + return r; + } + return 0; +} + +/*! \relates XsArray + \brief Returns non-zero if the lists are different, 0 if they're equal + \details This function compares the two lists in-order + \param a The left hand side of the comparison + \param b The right hand side of the comparison + \param predicate The custom comparison predicate to use. It should return 0 when the items are considered equal (see return values below). + \return -1 if \a a is smaller in some way than \a b, 1 if it is larger in some way + and 0 if both lists are equal. Please note that not all lists have items that can be accurately + tested for less than or greater than, but can be tested for (in-)equality. So the sign of the + return value should be treated with knowledge of the data type in mind. + \sa XsArray_compareSet \sa XsArray_compare +*/ +int XsArray_comparePredicate(void const* a, void const* b, XsArrayItemCompareFunc predicate) +{ + XsSize i; + XsArray const* aArray = (XsArray const*) a; + XsArray const* bArray = (XsArray const*) b; + + if (aArray == bArray) + return 0; + + if (aArray->m_size != bArray->m_size) + 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 + for (i = 0; i < aArray->m_size; ++i) // loop over all elements of the lists + { + int r = predicate(elemAtX(aArray->m_data, i, aArray), elemAtX(bArray->m_data, i, bArray)); + if (r) + return r; + } + return 0; +} + +/*! \relates XsArray + \brief Returns -1 if \a a is smaller in some way than \a b, 1 if it is larger in some way and 0 if both lists are equal + \details This function compares the two lists out-of-order + \param a The left hand side of the comparison + \param b The right hand side of the comparison + \return -1 if \a a is smaller in some way than \a b, 1 if it is larger in some way + \note It is assumed that both arguments are sets i.e. they contain no duplicates. It is the responsibility of the caller to provide sets as arguments + and 0 if both lists are equal. Please note that not all lists have items that can be accurately + tested for less than or greater than, but can be tested for (in-)equality. So the sign of the + return value should be treated with knowledge of the data type in mind. + \sa XsArray_compare \sa XsArray_comparePredicate +*/ +int XsArray_compareSet(void const* a, void const* b) +{ + XsSize n, m; + XsArray const* aArray = (XsArray const*) a; + XsArray const* bArray = (XsArray const*) b; + + if (aArray == bArray) + return 0; + + if (aArray->m_size != bArray->m_size) + return (aArray->m_size < bArray->m_size) ? -1 : 1; + + for (n = 0; n < aArray->m_size; ++n) // loop over all elements of list aArray + { + int found = 0; + for (m = 0; m < bArray->m_size; ++m) // loop over all elements of list bArray + { + if (aArray->m_descriptor->itemCompare(elemAtX(aArray->m_data, n, aArray), elemAtX(bArray->m_data, m, bArray)) == 0) + { + found = 1; + break; + } + } + if (!found) + return -1; + } + return 0; +} + +/*! \relates XsArray + \brief Returns the index of \a needle in the list or -1 if it wasn't found + \details The search does not assume any kind of ordering of the items so in a worst-case scenario it + will go through the entire list. + \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. +*/ +ptrdiff_t XsArray_find(void const* thisPtr, void const* needle) +{ + XsSize i; + XsArray const* thisArray = (XsArray const*) thisPtr; + + assert(thisArray->m_descriptor->itemCompare); + // 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 (ptrdiff_t) i; + return -1; +} + +/*! \relates XsArray + \brief Returns the index of \a needle in the list or -1 if it wasn't found + \details The search does not assume any kind of ordering of the items so in a worst-case scenario it + will go through the entire list. It will use the supplied predicate to match items against \a needle. + \param needle A pointer to the value to search for + \param predicate A compare function that returns 0 if the values are equal. The arguments are (const void) + 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. +*/ +ptrdiff_t XsArray_findPredicate(void const* thisPtr, void const* needle, XsArrayItemCompareFunc predicate) +{ + XsSize i; + XsArray const* thisArray = (XsArray const*) thisPtr; + + assert(predicate); + // 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 (ptrdiff_t) i; + return -1; +} + +/*! \relates XsArray + \brief Returns a pointer to the item at the supplied \a index or a null pointer if it is out of bounds + \param index The index of the item to return + \returns A pointer to the item or NULL if \a index is out of bounds +*/ +void const* XsArray_at(void const* thisPtr, XsSize index) +{ + XsArray const* thisArray = (XsArray const*) thisPtr; + if (index >= thisArray->m_size) + return 0; + return elemAt(thisArray->m_data, index); +} + +/*! \relates XsArray + \brief Returns a pointer to the item at the supplied \a index or a null pointer if it is out of bounds + \param index The index of the item to return + \returns A pointer to the item or NULL if \a index is out of bounds +*/ +void* XsArray_atIndex(void* thisPtr, XsSize index) +{ + XsArray* thisArray = (XsArray*) thisPtr; + if (index >= thisArray->m_size) + return 0; + return elemAt(thisArray->m_data, index); +} + +/*! \relates XsArray + \brief Removes duplicate entries from the array, keeping only the first instance of each value +*/ +void XsArray_removeDuplicates(void* thisPtr) +{ + XsSize i, j; + XsArray* thisArray = (XsArray*) thisPtr; + if (thisArray->m_size > 1) + { + for (i = 0; i < thisArray->m_size - 1; ++i) + { + 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); + } + } + } +} + +/*! \relates XsArray + \brief Removes duplicate entries from the array, keeping only the first instance of each value + \param predicate A compare function that returns 0 if the values are equal. The arguments are (const void) +*/ +void XsArray_removeDuplicatesPredicate(void* thisPtr, XsArrayItemCompareFunc predicate) +{ + XsSize i, j; + XsArray* thisArray = (XsArray*) thisPtr; + if (thisArray->m_size > 1) + { + for (i = 0; i < thisArray->m_size - 1; ++i) + { + 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); + } + } + } +} + +/*! \relates XsArray + \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. +*/ +int XsArray_empty(void const* thisPtr) +{ + XsArray const* thisArray = (XsArray const*) thisPtr; + return (thisArray->m_size == 0) || (thisArray->m_data == 0) || (thisArray->m_flags & XSDF_Empty); +} + +/*! \relates XsArray + \brief Copies items optimized in a direct way + \details This function copies items directly, using memcpy. This works for simple types only. + \param to Pointer to array to copy to. + \param from Pointer to array to copy from. + \param count The number of items to copy. + \param iSize The size of an individual item +*/ +void XsArray_rawCopy(void* to, void const* from, XsSize count, XsSize iSize) +{ + memcpy(to, from, count * iSize); +} + +/*! \relates XsArray + \brief Sorts the array using the \a itemCompare func in the XsArrayDescriptor + \details This function sorts using qsort +*/ +void XsArray_sort(void* thisPtr) +{ + XsArray* thisArray = (XsArray*) thisPtr; + qsort(thisArray->m_data, thisArray->m_size, thisArray->m_descriptor->itemSize, thisArray->m_descriptor->itemCompare); +} + +/*! \relates XsArray + \brief Reverses the contents of the array by repeatedly using the \a itemSwap func in the XsArrayDescriptor + \details This reverses the contents in-place +*/ +void XsArray_reverse(void* thisPtr) +{ + XsSize i, half; + 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)); +} + +/*! @} */ diff --git a/extern/xspublic/xstypes/xsarray.h b/extern/xspublic/xstypes/xsarray.h new file mode 100644 index 0000000..d962820 --- /dev/null +++ b/extern/xspublic/xstypes/xsarray.h @@ -0,0 +1,1042 @@ + +// Copyright (c) 2003-2022 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 XSARRAY_H +#define XSARRAY_H + +#include "xstypesconfig.h" + +#define XSARRAY_DECL(T)\ + T* const m_data; /*!< Pointer to contained data buffer */\ + const XsSize m_size; /*!< Size of used data buffer in number of items */\ + const XsSize m_reserved; /*!< Size of allocated data buffer in number of items */\ + const XsSize m_flags; /*!< Flags for data management */\ + XsArrayDescriptor const* const m_descriptor; /*!< Describes how to handle the items in the array */ + +#define XSARRAY_STRUCT(S,T) struct S { XSARRAY_DECL(T) } +#define XSARRAY_INITIALIZER(D) { 0, 0, 0, XSDF_Managed, D } + +/*! \cond XS_INTERNAL */ +typedef void (*XsArrayItemSwapFunc)(void*, void*); +typedef void (*XsArrayItemStructFunc)(void*); +typedef void (*XsArrayItemCopyFunc)(void*, void const*); +typedef int (*XsArrayItemCompareFunc)(void const*, void const*); //!< \brief Custom item compare function \return 0 means the items are equal, negative and positive values may be returned for sorting +typedef void (*XsArrayRawCopy)(void*, void const*, XsSize, XsSize); + +#define XSEXPCASTITEMSWAP (XsArrayItemSwapFunc) +#define XSEXPCASTITEMMAKE (XsArrayItemStructFunc) +#define XSEXPCASTITEMCOPY (XsArrayItemCopyFunc) +#define XSEXPCASTITEMCOMP (XsArrayItemCompareFunc) +#define XSEXPCASTRAWCOPY (XsArrayRawCopy) +/*! \endcond */ + +/*! \brief This object describes how to treat the data in an array. + \details Ususally there is one static instance per type of array that will be used by all + XsArrays of that type. +*/ +struct XsArrayDescriptor +{ +#ifndef __cplusplus + const +#else +protected: + template friend struct XsArrayImpl; +#endif + XsSize itemSize; //!< The size of an array item in bytes + void (*itemSwap)(void* a, void* b); //!< The function to use for swapping the data of two array items. \param a Pointer to first item to swap. \param b Pointer to second item to swap. + void (*itemConstruct)(void* e); //!< The function to use for constructing a new array item. May be 0 for simple types. \param e Pointer to item to construct. + void (*itemCopyConstruct)(void* e, void const* s); //!< The function to use for constructing a new array item with a source initializer. This may not be 0. \param e Pointer to item to construct. \param s Pointer to source item to copy from. + void (*itemDestruct)(void* e); //!< The function to use for destructing a array item. May be 0 for simple types. \param e Pointer to item to destruct. + void (*itemCopy)(void* to, void const* from); //!< The function to use for copying the data of \a from to \a to. \param to Pointer to item to copy to. \param from Pointer to item to copy from. + int (*itemCompare)(void const* a, void const* b); //!< The function to use for comparing two items. \param a Left hand side of comparison. \param b Right hand side of comparison. \returns The function will return 0 when the items are equal. When greater/less comparison is possible, the function should return < 0 if a < b and > 0 if a > b. + void (*rawCopy)(void* to, void const* from, XsSize count, XsSize iSize); //!< The function to use for copying the data of an array of \a from to \a to. \param to Pointer to array to copy to. \param from Pointer to array to copy from. \param count The number of items to copy. \param iSize The size of an individual item, should match the \a itemSize descriptor field. \note If this function pointer is 0, itemCopy will be used instead. \note XsArray_rawCopy can be used here for all types that can be safely memcpy'd. This typically means all means non-pointer types and structures containing only types matching these criteria. +}; +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 + +XSTYPES_DLL_API void XsArray_construct(void* thisPtr, XsArrayDescriptor const* const descriptor, XsSize count, void const* src); +XSTYPES_DLL_API void XsArray_copyConstruct(void* thisPtr, void const* src); +XSTYPES_DLL_API void XsArray_destruct(void* thisPtr); +XSTYPES_DLL_API void XsArray_assign(void* thisPtr, XsSize count, void const* src); +XSTYPES_DLL_API void XsArray_resize(void* thisPtr, XsSize count); +XSTYPES_DLL_API void XsArray_reserve(void* thisPtr, XsSize count); +XSTYPES_DLL_API void XsArray_copy(void* thisPtr, void const* src); +XSTYPES_DLL_API void XsArray_append(void* thisPtr, void const* other); +XSTYPES_DLL_API void XsArray_insert(void* thisPtr, XsSize index, XsSize count, void const* src); +XSTYPES_DLL_API void XsArray_erase(void* thisPtr, XsSize index, XsSize count); +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 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); +XSTYPES_DLL_API void XsArray_removeDuplicates(void* thisPtr); +XSTYPES_DLL_API void XsArray_removeDuplicatesPredicate(void* thisPtr, XsArrayItemCompareFunc predicate); +XSTYPES_DLL_API void XsArray_rawCopy(void* to, void const* from, XsSize count, XsSize iSize); +XSTYPES_DLL_API void XsArray_sort(void* thisPtr); +XSTYPES_DLL_API void XsArray_reverse(void* thisPtr); + +struct XsArray +{ + XSARRAY_DECL(void) +#ifdef __cplusplus + //! \copydoc XsArray_construct + inline XsArray(XsArrayDescriptor const* descriptor, XsSize count = 0, void const* src = 0) + : m_data(0) + , m_size(0) + , m_reserved(0) + , m_flags(0) + , m_descriptor(0) + { + XsArray_construct(this, descriptor, count, src); +#ifndef XSENS_NO_EXCEPTIONS + if (count && !m_data) + throw std::bad_alloc(); +#endif + } + + //! \copydoc XsArray_copyConstruct + inline XsArray(const XsArray& src) + : m_data(0) + , m_size(0) + , m_reserved(0) + , m_flags(0) + , m_descriptor(0) + { + XsArray_copyConstruct(this, &src); +#ifndef XSENS_NO_EXCEPTIONS + if (src.m_size && !m_data) + throw std::bad_alloc(); +#endif + } + + //! \brief Creates a array that references the data supplied in \a ref without allocating the data itself + inline explicit XsArray(XsArrayDescriptor const* descriptor, void* ref, XsSize count, XsDataFlags flags) + : m_data(ref) + , m_size(count) + , m_reserved(count) + , m_flags((XsSize)flags) + , m_descriptor(descriptor) + { + } + +#ifndef SWIG + //! \brief Move-construct an array using the supplied \a src + inline XsArray(XsArray&& src) noexcept + : m_data(0) + , m_size(0) + , m_reserved(0) + , m_flags(0) + , m_descriptor(src.m_descriptor) + { + assert(src.m_flags & XSDF_Managed); + XsArray_swap(this, &src); + } +#endif + + //! \brief Destructor + ~XsArray() + { + XsArray_destruct(this); + } + + /*! \brief Assignment operator + \details Copies the values in \a src into \a this + \param src The array to copy from + \return A reference to this + */ + inline XsArray const& operator=(const XsArray& src) + { + if (this != &src) + XsArray_copy(this, &src); +#ifndef XSENS_NO_EXCEPTIONS + if (src.m_size && !m_data) + throw std::bad_alloc(); +#endif + return *this; + } +#endif +}; + +typedef struct XsArray XsArray; + +#ifdef __cplusplus +} // extern "C" + +/*! \brief A C++ wrapper for XsArray, this is a template class for the actual specialized classes + \tparam T The type of the contained values + \tparam D The descriptor to use for this specific array implementation. This must be statically allocated and its lifetime must encompass the lifetime of objects that use it. + \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 +{ + //! \brief The contained type + typedef T value_type; + + //! \brief The type of size fields + typedef XsSize size_type; + + //! \brief A shorthand for the type of this specific implementation + typedef XsArrayImpl ArrayImpl; + + /*! \brief Construct an 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) + { + } + + //! \brief Constructs the XsArray as a copy of \a other + inline XsArrayImpl(ArrayImpl const& other) + : XsArray(other) + { + } + +#ifndef XSENS_NOITERATOR + //! \brief Constructs the XsArray with a copy of the array bound by the supplied iterators \a beginIt and \a endIt + template + inline explicit XsArrayImpl(Iterator const& beginIt, Iterator const& endIt) + : XsArray(&D, 0, 0) + { + ptrdiff_t diff = endIt - beginIt; + if (diff > 0) + { + reserve((XsSize) diff); + for (Iterator it = beginIt; it != endIt; ++it) + push_back(*it); + } + } +#endif + //! \brief Creates the XsArray as a reference to the data supplied in \a ref + inline explicit XsArrayImpl(T* ref, XsSize sz, XsDataFlags flags /* = XSDF_None */) + : XsArray(&D, ref, sz, flags) + { + } + + //! \copydoc XsArray_destruct + inline ~XsArrayImpl() + { + XsArray_destruct(this); + } + + //! \brief Clears the array \sa XsArray_destruct() + inline void clear() + { + XsArray_destruct(this); + } + +#ifndef SWIG + /*! \brief Tests \a other for equality according to \a predicate + \param other the array to compare against + \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); + } + + /*! \brief Tests \a other for equality according to \a predicate + \param other the array to compare against + \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); + } +#endif + + /*! \brief Tests \a other for equality + \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); + } + + /*! \brief Tests \a other for inequality + \param other the array to compare against + \returns true if the two arrays are not equal + \sa XsArray_compareArray + */ + inline bool operator != (ArrayImpl const& other) const + { + return !!XsArray_compare(this, &other); + } + + //! \copydoc XsArray_reserve + inline void reserve(XsSize count) + { + XsArray_reserve(this, count); +#ifndef XSENS_NO_EXCEPTIONS + if (count && !m_data) + throw std::bad_alloc(); +#endif + } + + //! \brief Returns the reserved space in number of items + inline XsSize reserved() const + { + return m_reserved; + } + + //! \brief Returns the XsArrayDescriptor describing the array + inline XsArrayDescriptor const& descriptor() const + { + return *m_descriptor; + } + +protected: +#ifndef XSENS_NOITERATOR + /*! \brief STL-style iterator */ + template + struct IteratorImplBase + { + public: + //! \brief Difference between two items + typedef ptrdiff_t difference_type; + //! \brief The contained type + typedef T value_type; + //! \brief Type of pointer + typedef T* pointer; + //! \brief Type of reference + typedef T& reference; +#ifndef XSENS_NO_STL + //! \brief The category of this type of iterator (random access) + typedef std::random_access_iterator_tag iterator_category; +#endif + //! \brief The type of the inherited class that is actually this class + typedef Derived this_type; + //! \brief The direction of the iterator, +1 = forward, -1 = reverse + static const ptrdiff_t direction = F; + protected: + //! \brief Basic constructor + inline explicit IteratorImplBase(void* p = 0) : m_ptr((T*) p) {} + //! \brief Basic constructor + inline explicit IteratorImplBase(T* p) : m_ptr(p) {} + //! \brief Copy constructor + inline IteratorImplBase(this_type const& i) : m_ptr(i.m_ptr) {} + public: +#ifndef SWIG + /*! \brief indexed data access operator */ + template + inline T const& operator[](J index) const + { +#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 */ + template + inline T& operator[](J 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) + { + m_ptr = (T*) p; + return *(this_type*)this; + } + //! \brief Assignment operator + inline this_type& operator =(T* p) + { + m_ptr = p; + return *(this_type*)this; + } + //! \brief Assignment operator + inline this_type& operator =(this_type const& i) + { + m_ptr = i.m_ptr; + return *(this_type*)this; + } + //! \brief Prefix increment by one operator + inline this_type& operator ++() + { + m_ptr = (T*) ptrAt(m_ptr, F); + return *(this_type*)this; + } + //! \brief Postfix increment by one operator + inline this_type operator ++(int) + { + this_type p(m_ptr); + m_ptr = (T*) ptrAt(m_ptr, F); + return p; + } + //! \brief Prefix decrement by one operator + inline this_type& operator --() + { + m_ptr = (T*) ptrAt(m_ptr, -F); + return *(this_type*)this; + } + //! \brief Postfix decrement by one operator + inline this_type operator --(int) + { + this_type p(m_ptr); + m_ptr = (T*) ptrAt(m_ptr, -F); + return p; + } + //! \brief Increment by \a count operator + inline this_type const& operator +=(ptrdiff_t 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); + 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)); + } + //! \brief Subtraction by \a count operator + inline this_type operator -(ptrdiff_t count) const + { + 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 + difference may be negative if \a other is 'ahead' of this. The function takes the direction + of the iterator into consideration. + \param other The iterator to subtract from this. + \returns The difference between the two iterators. + */ + inline difference_type operator - (const this_type& other) const + { + 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; + } + //! \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); + } + //! \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); + } + //! \brief Iterator comparison + 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); + } + //! \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); + } + //! \brief Dereferencing operator + inline R& operator *() const + { + return *(R*) ptr(); + } + //! \brief Pointer operator + inline R* operator ->() const + { + return (R*) ptr(); + } + //! \brief Access to internal pointer object, use should be avoided + inline T* ptr() const + { + return m_ptr; + } + private: + //! \brief The internal pointer + T* m_ptr; + }; +#endif + +public: +#ifndef XSENS_NOITERATOR + /*! \brief A non-const iterator implementation */ + template + struct IteratorImpl : public IteratorImplBase > + { + private: + //! \brief A shorthand for the base object + typedef IteratorImplBase > ParentType; + public: + //! \brief Basic constructor + inline IteratorImpl(void* p = 0) : ParentType(p) {} + //! \brief Basic constructor + inline IteratorImpl(T* p) : ParentType(p) {} + //! \brief Copy constructor + inline IteratorImpl(const IteratorImpl& i) : ParentType(i) {} + }; + + /*! \brief A const iterator implementation */ + template + struct IteratorImplConst : public IteratorImplBase > + { + private: + //! \brief A shorthand for the base object + typedef IteratorImplBase > ParentType; + public: + //! \brief Basic constructor + inline IteratorImplConst(void* p = 0) : ParentType(p) {} + //! \brief Basic constructor + inline IteratorImplConst(T* p) : ParentType(p) {} + //! \brief Copy constructor + inline IteratorImplConst(IteratorImpl const& i) : ParentType(i.ptr()) {} + //! \brief Copy constructor + inline IteratorImplConst(IteratorImplConst const& i) : ParentType(i) {} + }; + + //! \brief STL-style mutable forward iterator + typedef IteratorImpl<1> iterator; + //! \brief STL-style mutable 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; + + /*! \brief STL-style const_iterator to the first data item in the array */ + 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(); + } + + /*! \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(); + } + /*! \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; + } + + /*! \brief STL-style iterator to the first data item in the array */ + 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(); + } + + /*! \brief STL-style reverse_iterator to the first data item in the reversed array */ + 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; + } +#endif +#ifndef SWIG + /*! \brief indexed data access operator */ + template + inline T const& 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 *ptrAt(m_data, static_cast(index)); + } + /*! \brief indexed data access operator */ + template + inline T& 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 *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 + { +#ifdef XSENS_NO_EXCEPTIONS + assert(index >= m_size); +#else + if (index >= m_size) + throw std::out_of_range("index out of range"); +#endif + return *ptrAt(m_data, index); + } + /*! \brief Returns the first item in the array (by value). */ + inline T first() const + { +#ifdef XSENS_NO_EXCEPTIONS + assert(m_size); +#else + if (!m_size) + throw std::out_of_range("out of range"); +#endif + return *ptrAt(m_data, 0); + } + /*! \brief Returns the last item in the array (by value). */ + inline T last() const + { +#ifdef XSENS_NO_EXCEPTIONS + assert(m_size); +#else + if (!m_size) + throw std::out_of_range("out of range"); +#endif + 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 + { +#ifdef XSENS_NO_EXCEPTIONS + assert(index >= m_size); +#else + if (index >= m_size) + throw std::out_of_range("index out of range"); +#endif + 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) + { +#ifdef XSENS_NO_EXCEPTIONS + assert(index >= m_size); +#else + if (index >= m_size) + throw std::out_of_range("index out of range"); +#endif + return *ptrAt(m_data, index); + } + /*! \brief Insert \a item at \a index + \param item The item to insert + \param index The index to insert the item. When beyond the end of the array, the item is appended. + \sa XsArray_insert + */ + inline void insert(T const& item, XsSize index) + { + insert(&item, index, 1); + } + /*! \brief Insert \a item at \a index + \param items The items to insert + \param index The index to insert the items. When beyond the end of the array, the items are appended. + \param count The number of items to insert + \sa XsArray_insert + */ + inline void insert(T const* items, XsSize index, XsSize count) + { + XsArray_insert(this, index, count, items); +#ifndef XSENS_NO_EXCEPTIONS + if (count && !m_data) + throw std::bad_alloc(); +#endif + } + +#ifndef XSENS_NOITERATOR + /*! \brief Insert \a item before iterator \a it + \param item The item to insert + \param it The iterator before which to insert the item. + \sa XsArray_insert + */ + inline void insert(T const& item, const_iterator it) + { + insert(&item, indexOf(it), 1); + } + /*! \brief Insert \a item before iterator \a it + \param item The item to insert + \param it The iterator before which to insert the item. + \sa XsArray_insert + */ + inline void insert(T const& item, const_reverse_iterator it) + { + insert(&item, indexOf(it), 1); + } + + /*! \brief Insert \a item at \a index + \param items The items to insert + \param it The iterator before which to insert the item. + \param count The number of items to insert + \sa XsArray_insert + */ + inline void insert(T const* items, const_iterator it, XsSize count) + { + insert(items, indexOf(it), count); + } + /*! \brief Insert \a item at \a index + \param items The items to insert + \param it The iterator before which to insert the item. + \param count The number of items to insert + \sa XsArray_insert + */ + inline void insert(T const* items, const_reverse_iterator it, XsSize count) + { + insert(items, indexOf(it), count); + } +#endif + + /*! \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); + } + /*! \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); + else + 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) + { + insert(&item, 0, 1); + } + /*! \brief Removes \a count items from the start of the array \param count The number items to remove \sa XsArray_erase */ + inline void pop_front(XsSize count = 1) + { + erase(0, count); + } + /*! \brief Returns the number of items currently in the array + \returns The number of items currently in the array + \sa reserved \sa setSize \sa resize + */ + inline XsSize size() const noexcept + { + return m_size; + } + /*! \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. */ + inline void erase(XsSize index, XsSize count = 1) + { + XsArray_erase(this, index, count); + } +#ifndef XSENS_NOITERATOR + /*! \brief Removes the item at iterator \a it. \details \param it The item to remove. \returns An iterator pointing to the next item after the erased item. */ + inline iterator erase(iterator it) + { + XsSize idx = indexOf(it); + erase(idx, 1); + return (idx < size()) ? ptrAt(m_data, idx) : end(); + } + /*! \brief Removes the item at iterator \a it. \details \param it The item to remove. \returns An iterator pointing to the next item after the erased item. */ + inline reverse_iterator erase(reverse_iterator it) + { + XsSize idx = indexOf(it); + erase(idx, 1); + return (idx < size()) ? ptrAt(m_data, idx) : rend(); + } +#endif + /*! \copydoc XsArray_assign \sa XsArray_assign */ + inline void assign(XsSize count, T const* src) + { + XsArray_assign(this, count, src); +#ifndef XSENS_NO_EXCEPTIONS + if (count && !m_data) + throw std::bad_alloc(); +#endif + } + /*! \copydoc XsArray_resize \sa XsArray_resize */ + inline void resize(XsSize count) + { + XsArray_resize(this, count); +#ifndef XSENS_NO_EXCEPTIONS + if (count && !m_data) + throw std::bad_alloc(); +#endif + } + /*! \brief Set the size of the array to \a count. + \details This function changes the size of the array to \a count. The contents of the array after this operation are undefined. + \param count The desired new size of the array. + \sa XsArray_assign \sa reserve \sa resize + */ + inline void setSize(XsSize count) + { + if (count != m_size) + { + XsArray_assign(this, count, 0); +#ifndef XSENS_NO_EXCEPTIONS + if (count && !m_data) + throw std::bad_alloc(); +#endif + } + } + /*! \copydoc XsArray_append \sa XsArray_append */ + inline void append(ArrayImpl const& other) + { + XsArray_append(this, &other); +#ifndef XSENS_NO_EXCEPTIONS + if (other.m_size && !m_data) + throw std::bad_alloc(); +#endif + } + /*! \brief Assignment operator, copies \a other into this, overwriting the old contents \param other The array to copy from \returns A reference to this \sa XsArray_copy */ + inline ArrayImpl& operator=(ArrayImpl const& other) + { + if (this != &other) + { + XsArray_copy(this, &other); +#ifndef XSENS_NO_EXCEPTIONS + if (other.m_size && !m_data) + throw std::bad_alloc(); +#endif + } + 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 noexcept + { + return (size() == 0) || (m_data == 0) || ((m_flags & XSDF_Empty) != 0); + } + + /*! \brief Returns whether the array had a bad allocation in its last resize attempt */ + inline bool badAlloc() const noexcept + { + return (m_flags & XSDF_BadAlloc) != 0; + } + +#ifndef XSENS_NOITERATOR + /*! \brief Return the inheriting object */ + inline I const& inherited() const + { + return *static_cast(this); + } + + /*! \brief Return the inheriting object */ + 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*/ + inline void swap(ArrayImpl& other) + { + XsArray_swap(this, &other); + } + + /*! \brief Swap the contents the \a first and \a second array */ + friend void swap(ArrayImpl& first, ArrayImpl& second) + { + first.swap(second); + } + + /*! \brief Swap the item at index \a a with the item at index \a b */ + inline void swap(XsSize a, XsSize b) + { + using std::swap; + if (a >= size() || b >= size()) + return; + swap(at(a), at(b)); + } + + /*! \brief Append \a item in a stream-like manner. + \details This allows for constructing XsArrays easily like this: + XsInt64Array array = XsInt64Array() << 1 << 2 << 3; + \param item The item to append to the array. + \returns A reference to the modified array + \sa push_back + */ +#ifndef XSENS_NOITERATOR + inline I& operator <<(T const& item) + { + push_back(item); + return inherited(); + } +#endif + + /*! \copydoc XsArray_find */ + inline ptrdiff_t find(T const& needle) const + { + return XsArray_find(this, &needle); + } + + /*! \brief Returns true if \a needle is found within this array */ + inline bool contains(T const& needle) const + { + return XsArray_find(this, &needle) >= 0; + } + +#ifndef SWIG + /*! \copydoc XsArray_findPredicate */ + inline ptrdiff_t findPredicate(T const& needle, XsArrayItemCompareFunc predicate) const + { + return XsArray_findPredicate(this, &needle, predicate); + } +#endif +#ifndef XSENS_NOITERATOR + /*! \brief Returns the linear index of \a it in the array + \param it The iterator to analyze + \returns The linear forward index of the item pointed to by \a it. If \a it points to before the + 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 + { + ptrdiff_t d = ((char const*) it.ptr() - (char const*) m_data); + if (d >= 0) + { + XsSize r = d / D.itemSize; + if (r <= size()) + return r; + return size(); + } + return 0; + } +#endif + + /*! \copydoc XsArray_removeDuplicates */ + inline void removeDuplicates() + { + XsArray_removeDuplicates(this); + } + + /*! \copydoc XsArray_removeDuplicatesPredicate */ + inline void removeDuplicatesPredicate(XsArrayItemCompareFunc predicate) + { + XsArray_removeDuplicatesPredicate(this, predicate); + } + + /*! \copydoc XsArray_sort */ + inline void sort() + { + XsArray_sort(this); + } + + /*! \copydoc XsArray_reverse */ + inline void reverse() + { + XsArray_reverse(this); + } + +private: + /*! \internal + \brief Generic pointer movement function + \details This function adds \a count items to \a ptr based on the size specified in \a descriptor + \param ptr The pointer to start from + \param count The number of items to move up or down. count may be negative + \returns The requested pointer. + \note The return value loses its constness, take care when using this function directly. In most cases + it should not be necessary to use this function in user code. + */ + inline static const T* ptrAt(void const* ptr, ptrdiff_t count) + { + return (const T*)(void const*)(((char const*)ptr) + count * (ptrdiff_t)D.itemSize); + } + + /*! \internal + \brief Generic pointer movement function + \details This function adds \a count items to \a ptr based on the size specified in \a descriptor + \param ptr The pointer to start from + \param count The number of items to move up or down. count may be negative + \returns The requested pointer. + \note The return value loses its constness, take care when using this function directly. In most cases + it should not be necessary to use this function in user code. + */ + inline static T* ptrAt(void* ptr, ptrdiff_t count) + { + return (T*)(void*)(((char*)ptr) + count * (ptrdiff_t)D.itemSize); + } +}; +#endif + +#endif diff --git a/extern/xspublic/xstypes/xsbaud.c b/extern/xspublic/xstypes/xsbaud.c new file mode 100644 index 0000000..51b0a45 --- /dev/null +++ b/extern/xspublic/xstypes/xsbaud.c @@ -0,0 +1,232 @@ + +// Copyright (c) 2003-2022 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 "xsbaud.h" + +#ifdef _WIN32 +#define HAVE_ALL_RATES 1 +#else +#define HAVE_ALL_RATES 0 +#endif + +/*! \addtogroup cinterface C Interface + @{ +*/ + +/*! \brief Convert an Xsens baud code to XsBaudRate + + \param baudcode : The code to convert to an XsBaudRate + + \returns An XsBaudrate +*/ +XsBaudRate XsBaud_codeToRate(XsBaudCode 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; + } +} + +/*! \brief Convert a XsBaudRate to an Xsens baud code + + \param baudrate : The code to convert to an XsBaudCode + + \returns An XsBaudCode +*/ +XsBaudCode XsBaud_rateToCode(XsBaudRate baudrate) +{ + switch (baudrate) + { +#if HAVE_ALL_RATES + case XBR_14k4: + return XBC_14k4; + case XBR_28k8: + return XBC_28k8; + case XBR_76k8: + return XBC_76k8; +#endif + case XBR_4800: + return XBC_4k8; + case XBR_9600: + return XBC_9k6; + case XBR_19k2: + return XBC_19k2; + case XBR_38k4: + return XBC_38k4; + case XBR_57k6: + return XBC_57k6; + 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; + } +} + +/*! \brief Convert a XsBaudrate to a numeric baudrate in bps + + \param baudrate : The XsBaudRate to convert to a numeric baudrate + + \returns A baudrate in bps +*/ +int XsBaud_rateToNumeric(XsBaudRate baudrate) +{ + switch (baudrate) + { +#if HAVE_ALL_RATES + case XBR_14k4: + return 14400; + case XBR_28k8: + return 28800; + case XBR_76k8: + return 76800; +#endif + case XBR_4800: + return 4800; + case XBR_9600: + return 9600; + case XBR_19k2: + return 19200; + case XBR_38k4: + return 38400; + case XBR_57k6: + return 57600; + 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; + } +} + +/*! \brief Convert a numeric baudrate in bps to XsBaudrate + + \param numeric : The numeric baudrate to convert to XsBaudRate + + \returns A XsBaudrate +*/ +XsBaudRate XsBaud_numericToRate(int numeric) +{ + switch (numeric) + { +#if HAVE_ALL_RATES + case 14400: + return XBR_14k4; + case 28800: + return XBR_28k8; + case 76800: + return XBR_76k8; +#endif + case 4800: + return XBR_4800; + case 9600: + return XBR_9600; + case 19200: + return XBR_19k2; + case 38400: + return XBR_38k4; + case 57600: + return XBR_57k6; + 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/extern/xspublic/xstypes/xsbaud.h b/extern/xspublic/xstypes/xsbaud.h new file mode 100644 index 0000000..dd5dbc4 --- /dev/null +++ b/extern/xspublic/xstypes/xsbaud.h @@ -0,0 +1,106 @@ + +// Copyright (c) 2003-2022 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 XSBAUD_H +#define XSBAUD_H + +#include "xstypesconfig.h" + + +/*! \addtogroup enums Global enumerations + @{ +*/ + +#include "xsbaudcode.h" +#include "xsbaudrate.h" + +/*! @} */ + +typedef enum XsBaudCode XsBaudCode; +typedef enum XsBaudRate XsBaudRate; + +#ifdef __cplusplus +extern "C" { +#endif + +XSTYPES_DLL_API XsBaudRate XsBaud_codeToRate(XsBaudCode baudcode); +XSTYPES_DLL_API XsBaudCode XsBaud_rateToCode(XsBaudRate baudrate); +XSTYPES_DLL_API int XsBaud_rateToNumeric(XsBaudRate baudrate); +XSTYPES_DLL_API XsBaudRate XsBaud_numericToRate(int numeric); + +#ifdef __cplusplus +} // extern "C" + +/*! \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); +} +} + +#ifndef XSENS_NO_STL +#include + +namespace std +{ +/*! \brief Stream output operator for XsBaudRate */ +template +basic_ostream<_CharT, _Traits>& operator<<(basic_ostream<_CharT, _Traits>& o, XsBaudRate const& xd) +{ + return (o << XsBaud::rateToNumeric(xd)); +} +} +#endif + +#endif + +#endif diff --git a/extern/xspublic/xstypes/xsbaudcode.h b/extern/xspublic/xstypes/xsbaudcode.h new file mode 100644 index 0000000..d9afdcf --- /dev/null +++ b/extern/xspublic/xstypes/xsbaudcode.h @@ -0,0 +1,63 @@ + +// Copyright (c) 2003-2022 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 XSBAUDCODE_H +#define XSBAUDCODE_H + +#include "xstypesconfig.h" + +/*! \brief Internal baud rate configuration codes +*/ + +enum XSNOCOMEXPORT XsBaudCode +{ + // Baudrate codes for SetBaudrate message + XBC_4k8 = 0x0B, //!< 4k8 (4800 bps) + XBC_9k6 = 0x09, //!< 9k6 (9600 bps) + XBC_14k4 = 0x08, //!< 14k4 (14400 bps) + XBC_19k2 = 0x07, //!< 19k2 (19200 bps) + XBC_28k8 = 0x06, //!< 28k8 (28800 bps) + XBC_38k4 = 0x05, //!< 38k4 (38400 bps) + XBC_57k6 = 0x04, //!< 57k6 (57600 bps) + XBC_76k8 = 0x03, //!< 76k8 (76800 bps) + XBC_115k2 = 0x02, //!< 115k2 (115200 bps) + XBC_230k4 = 0x01, //!< 230k4 (230400 bps) + XBC_460k8 = 0x00, //!< 460k8 (460800 bps) + XBC_921k6 = 0x0A, //!< 921k6 (921600 bps). Only usable from MTi/x FW 2.4.6 + XBC_921k6Legacy = 0x80, //!< 921k6 (921600 bps) + XBC_2MegaBaud = 0x0C, //!< 2000k0 (2000000 bps) + XBC_3_5MegaBaud = 0x0E, //!< 3500k0 (3500000 bps) + XBC_4MegaBaud = 0x0D, //!< 4000k0 (4000000 bps) + XBC_Invalid = 0xFF //!< Not a valid baud rate +}; + +#endif diff --git a/extern/xspublic/xstypes/xsbaudrate.h b/extern/xspublic/xstypes/xsbaudrate.h new file mode 100644 index 0000000..60ae5e1 --- /dev/null +++ b/extern/xspublic/xstypes/xsbaudrate.h @@ -0,0 +1,123 @@ + +// Copyright (c) 2003-2022 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 XSBAUDRATE_H +#define XSBAUDRATE_H + +#ifdef _WIN32 + #include +#else + #include + #ifndef B460800 + #undef B230400 + #define B230400 0010003 + #define B460800 0010004 + #define B921600 0010007 + #endif +#endif + +/*! \addtogroup enums Global enumerations + @{ +*/ +#ifdef _WIN32 + +//AUTO namespace xstypes { +/*! \brief Communication speed. */ +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, //!< 14k4 (14400 bps) + XBR_19k2 = CBR_19200, //!< 19k2 (19200 bps) + XBR_28k8 = 28800, //!< 28k8 (28800 bps) + XBR_38k4 = CBR_38400, //!< 38k4 (38400 bps) + XBR_57k6 = CBR_57600, //!< 57k6 (57600 bps) + XBR_76k8 = 76800, //!< 76k8 (76800 bps) + XBR_115k2 = CBR_115200, //!< 115k2 (115200 bps) + XBR_230k4 = 230400, //!< 230k4 (230400 bps) + XBR_460k8 = 460800, //!< 460k8 (460800 bps) + XBR_921k6 = 921600, //!< 921k6 (921600 bps) + XBR_2000k = 2000000, //!< 2000k0 (2000000 bps) + XBR_3500k = 3500000, //!< 3500k0 (3500000 bps) + XBR_4000k = 4000000 //!< 4000k0 (4000000 bps) +}; +//AUTO } + +#else + +/*! \brief Communication speed. */ +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 +#endif + XBR_Invalid = 0, //!< Not a valid baud rate + + XBR_4800 = B4800, //!< 4k8 (4800 bps) + XBR_9600 = B9600, //!< 9k6 (9600 bps) + XBR_14k4 = 0, //!< 14k4 is set to 0 on purpose + XBR_19k2 = B19200, //!< 19k2 (19200 bps) + XBR_28k8 = 0, //!< 28k8 is set to 0 on purpose + XBR_38k4 = B38400, //!< 38k4 (38400 bps) + XBR_57k6 = B57600, //!< 57k6 (57600 bps) + XBR_76k8 = 0, //!< 76k8 is set to 0 on purpose + XBR_115k2 = B115200, //!< 115k2 (115200 bps) + XBR_230k4 = B230400, //!< 230k4 (230400 bps) + XBR_460k8 = B460800, //!< 460k8 (460800 bps) + XBR_921k6 = B921600, //!< 921k6 (921600 bps) +#ifndef SWIG + XBR_2000k = B2000000, //!< 2000k0 (2000000 bps) + XBR_3500k = B3500000, //!< 3500k0 (3500000 bps) + XBR_4000k = B4000000 //!< 4000k0 (4000000 bps) +#else + XBR_2000k = 2000000, //!< 2000k0 (2000000 bps) + XBR_3500k = 3500000, //!< 3500k0 (3500000 bps) + XBR_4000k = 4000000 //!< 4000k0 (4000000 bps) +#endif +}; +/*! \brief Communication speed. */ +typedef enum XsBaudRate XsBaudRate; +#endif +/*! @} */ + +#endif diff --git a/extern/xspublic/xstypes/xsbusid.h b/extern/xspublic/xstypes/xsbusid.h new file mode 100644 index 0000000..5de5898 --- /dev/null +++ b/extern/xspublic/xstypes/xsbusid.h @@ -0,0 +1,59 @@ + +// Copyright (c) 2003-2022 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 XSBUSID_H +#define XSBUSID_H + +/*! \addtogroup cinterface C Interface + @{ +*/ + +/*! \brief The bus identifier of the master device */ +#define XS_BID_MASTER 0xFF + +/*! \brief The bus broadcast bus identifier (all devices) */ +#define XS_BID_BROADCAST 0x00 + +/*! \brief The bus identifier for the first MT on the bus */ +#define XS_BID_MT 0x01 + +/*! \brief An invalid bus identifier + \note This value may change between API releases as it is not strictly part of the Xbus protocol but only used internally +*/ +#define XS_BID_INVALID 0xFD + +/*! \brief A dynamic bus identifier */ +#define XS_BID_DYNAMIC 0xFE + +/*! @} */ + +#endif diff --git a/extern/xspublic/xstypes/xsbytearray.c b/extern/xspublic/xstypes/xsbytearray.c new file mode 100644 index 0000000..d42e34f --- /dev/null +++ b/extern/xspublic/xstypes/xsbytearray.c @@ -0,0 +1,90 @@ + +// Copyright (c) 2003-2022 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 "xsbytearray.h" + +/*! \struct XsByteArray + \brief A list of uint8_t values + \sa XsArray +*/ + +/*! \copydoc XsArrayDescriptor::itemSwap + \note Specialization for uint8_t +*/ +static void swapUint8(uint8_t* a, uint8_t* b) +{ + uint8_t tmp = *a; + *a = *b; + *b = tmp; +} + +/*! \copydoc XsArrayDescriptor::itemCopy + \note Specialization for uint8_t +*/ +static void copyUint8(uint8_t* to, uint8_t const* from) +{ + *to = *from; +} + +/*! \copydoc XsArrayDescriptor::itemCompare + \note Specialization for uint8_t +*/ +static int compareUint8(uint8_t const* a, uint8_t const* b) +{ + if (*a < *b) + return -1; + if (*a > *b) + return 1; + return 0; +} + +//! \brief Descriptor for XsByteArray +XsArrayDescriptor const g_xsByteArrayDescriptor = +{ + sizeof(uint8_t), + XSEXPCASTITEMSWAP swapUint8, // swap + 0, // construct + XSEXPCASTITEMCOPY copyUint8, // copy construct + 0, // destruct + XSEXPCASTITEMCOPY copyUint8, // copy + XSEXPCASTITEMCOMP compareUint8, // compare + XSEXPCASTRAWCOPY XsArray_rawCopy // raw copy +}; + +/*! \copydoc XsArray_constructDerived + \note Specialization for XsByteArray + \relates XsByteArray +*/ +void XsByteArray_construct(XsByteArray* thisPtr, XsSize count, uint8_t const* src) +{ + XsArray_construct(thisPtr, &g_xsByteArrayDescriptor, count, src); +} diff --git a/extern/xspublic/xstypes/xsbytearray.h b/extern/xspublic/xstypes/xsbytearray.h new file mode 100644 index 0000000..964d8ed --- /dev/null +++ b/extern/xspublic/xstypes/xsbytearray.h @@ -0,0 +1,171 @@ + +// Copyright (c) 2003-2022 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 XSBYTEARRAY_H +#define XSBYTEARRAY_H + +#include "xsarray.h" +#include "pstdint.h" + +#ifdef __cplusplus +#include "xsstring.h" +extern "C" { +#endif + +extern XsArrayDescriptor const XSTYPES_DLL_API g_xsByteArrayDescriptor; + +#ifndef __cplusplus +#define XSBYTEARRAY_INITIALIZER XSARRAY_INITIALIZER(&g_xsByteArrayDescriptor) +XSARRAY_STRUCT(XsByteArray, uint8_t); +typedef struct XsByteArray XsByteArray; +XSTYPES_DLL_API void XsByteArray_construct(XsByteArray* thisPtr, XsSize count, uint8_t const* src); + +// obsolete: +#define XsByteArray_ref(thisPtr, sz, src, flags) XsArray_ref(thisPtr, sz, src, flags) +#define XsByteArray_assign(thisPtr, sz, src) XsArray_assign(thisPtr, sz, src) +#define XsByteArray_destruct(thisPtr) XsArray_destruct(thisPtr) +#define XsByteArray_copy(thisPtr, copy) XsArray_copy(copy, thisPtr) +#define XsByteArray_append(thisPtr, other) XsArray_append(thisPtr, other) +#define XsByteArray_popFront(thisPtr, count) XsArray_erase(thisPtr, 0, count) +#define XsByteArray_popBack(thisPtr, count) XsArray_erase(thisPtr, (XsSize)-1, count) +#define XsByteArray_fromString(str, copy) XsArray_assign(copy, str->m_size?str->m_size:1, str->m_size?str->m_data:"\0") +#define XsByteArray_swap(a, b) XsArray_swap(a, b) +#define XsByteArray_erase(thisPtr, index, count) XsArray_erase(thisPtr, index, count) +#endif + +#ifdef __cplusplus +} // extern "C" + +struct XsByteArray : public XsArrayImpl +{ + //! \brief Constructs an XsByteArray + inline explicit XsByteArray(XsSize sz = 0, uint8_t const* src = 0) + : ArrayImpl(sz, src) + { + } + + //! \brief Constructs an XsByteArray as a copy of \a other + inline XsByteArray(XsByteArray const& other) + : ArrayImpl(other) + { + } + + //! \brief Constructs an XsByteArray that references the data supplied in \a ref + inline explicit XsByteArray(uint8_t* 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(XsByteArray& first, XsByteArray& second) + { + first.swap(second); + } +#endif + +#ifndef XSENS_NOITERATOR + //! \brief Constructs an XsByteArray with the array bound by the supplied iterators \a beginIt and \a endIt + template + inline XsByteArray(Iterator beginIt, Iterator endIt) + : ArrayImpl(beginIt, endIt) + { + } +#endif + //! \brief Constructs an XsByteArray as a copy of the supplied XsString, including the terminating 0 + inline XsByteArray(XsString const& src) + : ArrayImpl() + { + 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 ->(); + } + + //! \brief Return a pointer to the internal data buffer + 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 + POD type or structure with a trivial default constructor, since its contents will be completely + overwritten by a memcpy + \param offset The offset of the first byte to convert (in byte units, not in T units) + \returns The converted value + */ + template + inline T getValue(XsSize offset) const + { + assert(offset + sizeof(T) <= size()); + T tmp; + memcpy(&tmp, data() + offset, sizeof(T)); + return tmp; + } + + /*! \brief Append \a value T to the byte array + \param value The value to append to the array. The type T needs to be a POD structure or a primitive type + since it gets copied into the array using memcpy. + */ + template + inline void appendValue(T const& value) + { + XsSize offset = size(); + resize(offset + sizeof(T)); + memcpy(data() + offset, &value, sizeof(T)); + } + + /*! \brief Set \a value T in the byte array at byte position \a offset + \param value The value to append to the array. The type T needs to be a POD structure or a primitive type + since it gets copied into the array using memcpy. + \param offset The position of the first byte to write. The array will be enlarged if necessary + */ + template + inline void setValue(T const& value, XsSize offset) + { + if (size() < offset + sizeof(T)) + resize(offset + sizeof(T)); + memcpy(data() + offset, &value, sizeof(T)); + } +}; +#endif +#endif diff --git a/extern/xspublic/xstypes/xscalibrateddata.c b/extern/xspublic/xstypes/xscalibrateddata.c new file mode 100644 index 0000000..634af5a --- /dev/null +++ b/extern/xspublic/xstypes/xscalibrateddata.c @@ -0,0 +1,64 @@ + +// Copyright (c) 2003-2022 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 "xscalibrateddata.h" + +/*! \struct XsCalibratedData + \brief Container for combined calibrated measurement data from accelerometers, gyroscopes and + magnetometers. +*/ + +/*! \addtogroup cinterface C Interface + @{ +*/ + +/*! \relates XsCalibratedData + \brief Construct an %XsCalibratedData object +*/ +void XsCalibratedData_construct(XsCalibratedData* thisPtr, const XsReal* acc, const XsReal* gyr, const XsReal* mag) +{ + XsVector3_construct(&thisPtr->m_acc, acc); + XsVector3_construct(&thisPtr->m_gyr, gyr); + XsVector3_construct(&thisPtr->m_mag, mag); +} + +/*! \relates XsCalibratedData + \brief Destruct an %XsCalibratedData object +*/ +void XsCalibratedData_destruct(XsCalibratedData* thisPtr) +{ + XsVector3_destruct(&thisPtr->m_acc); + XsVector3_destruct(&thisPtr->m_gyr); + XsVector3_destruct(&thisPtr->m_mag); +} + +/*! @} */ diff --git a/extern/xspublic/xstypes/xscalibrateddata.h b/extern/xspublic/xstypes/xscalibrateddata.h new file mode 100644 index 0000000..e2d2bd1 --- /dev/null +++ b/extern/xspublic/xstypes/xscalibrateddata.h @@ -0,0 +1,97 @@ + +// Copyright (c) 2003-2022 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 XSCALIBRATEDDATA_H +#define XSCALIBRATEDDATA_H + +#include "xstypesconfig.h" +#include "xsvector3.h" + +struct XsCalibratedData; + +#ifdef __cplusplus +extern "C" { +#endif +#ifndef __cplusplus +#define XSCALIBRATEDDATA_INITIALIZER {XSVECTOR3_INITIALIZER, XSVECTOR3_INITIALIZER, XSVECTOR3_INITIALIZER} +#endif + +XSTYPES_DLL_API void XsCalibratedData_construct(struct XsCalibratedData* thisPtr, const XsReal* acc, const XsReal* gyr, const XsReal* mag); +XSTYPES_DLL_API void XsCalibratedData_destruct(struct XsCalibratedData* thisPtr); + +#ifdef __cplusplus +} // extern "C" +#endif + +struct XsCalibratedData +{ + XsVector3 m_acc; //!< Accelerometer data + XsVector3 m_gyr; //!< Gyroscope data + XsVector3 m_mag; //!< Magnetometer data + +#ifdef __cplusplus + //! \brief Constructor \sa XsCalibratedData_construct + inline XsCalibratedData() + : m_acc(0, 0, 0) + , m_gyr(0, 0, 0) + , m_mag(0, 0, 0) + { + } + + //! \brief Copy constructor, copies the values from \a other to this + inline XsCalibratedData(const XsCalibratedData& other) + : m_acc(other.m_acc) + , m_gyr(other.m_gyr) + , m_mag(other.m_mag) + { + } + + //! \brief Destructor + inline ~XsCalibratedData() + {} + + //! \brief Assignment operator, copies the values from \a other to this + inline XsCalibratedData& operator = (const XsCalibratedData& other) + { + if (this != &other) + { + m_acc = other.m_acc; + m_gyr = other.m_gyr; + m_mag = other.m_mag; + } + return *this; + } +#endif +}; +typedef struct XsCalibratedData XsCalibratedData; + +#endif diff --git a/extern/xspublic/xstypes/xscanbaudcode.h b/extern/xspublic/xstypes/xscanbaudcode.h new file mode 100644 index 0000000..cfde9d6 --- /dev/null +++ b/extern/xspublic/xstypes/xscanbaudcode.h @@ -0,0 +1,60 @@ + +// Copyright (c) 2003-2022 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 XSCANBAUDCODE_H +#define XSCANBAUDCODE_H + +#include "xstypesconfig.h" + +/*! \brief Internal baud rate configuration codes +*/ + +enum XSNOCOMEXPORT XsCanBaudCode +{ + // Baudrate codes for SetBaudrate message + XCBC_1M = 0x0C, + XCBC_800k = 0x0B, + XCBC_500k = 0x0A, + XCBC_250k = 0x00, + XCBC_125k = 0x01, + XCBC_100k = 0x02, + XCBC_83k3 = 0x03, + XCBC_62k5 = 0x04, + XCBC_50k = 0x05, + XCBC_33k3 = 0x06, + XCBC_20k = 0x07, + XCBC_10k = 0x08, + XCBC_5k = 0x09, + XCBC_Invalid = 0xFF +}; + +#endif diff --git a/extern/xspublic/xstypes/xscanconfigidentifier.h b/extern/xspublic/xstypes/xscanconfigidentifier.h new file mode 100644 index 0000000..a9f2991 --- /dev/null +++ b/extern/xspublic/xstypes/xscanconfigidentifier.h @@ -0,0 +1,69 @@ + +// Copyright (c) 2003-2022 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 XSCANCONFIGIDENTIFIER_H +#define XSCANCONFIGIDENTIFIER_H + +#include "xscandataidentifier.h" +////////////////////////////////////////////////////////////////////////////////////////// +/*! \addtogroup enums Global enumerations + @{ +*/ + +//AUTO namespace xstypes { +/*! \enum XsCanConfigIdentifier + \brief Defines the config identifiers for CAN messages + + \note Have to be higher than XCDI_HighestIdentifier in xscandataidentifier.h +*/ +enum XsCanConfigIdentifier +{ + XCCI_LowestIdentifier = 0xA0, + + XCCI_DeviceIdReq = 0xAA, + XCCI_DeviceId = 0xAB, + XCCI_GotoConfig = 0xAC, + XCCI_GotoMeasurement = 0xAD, + XCCI_Reset = 0xAE, + + + XCCI_HighestIdentifier, //Keep this entry last. Don't assign IDs with a higher value than this. +}; +/*! @} */ + +#if __cplusplus >= 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/extern/xspublic/xstypes/xscandataidentifier.h b/extern/xspublic/xstypes/xscandataidentifier.h new file mode 100644 index 0000000..f5d22b0 --- /dev/null +++ b/extern/xspublic/xstypes/xscandataidentifier.h @@ -0,0 +1,107 @@ + +// Copyright (c) 2003-2022 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 XSCANDATAIDENTIFIER_H +#define XSCANDATAIDENTIFIER_H + +//! \brief Max frequency values for can interface +#define XCDI_MAX_FREQUENCY_VAL 0x07FF +#define XCDI_MAX_FREQUENCY ((uint16_t) XCDI_MAX_FREQUENCY_VAL) + +////////////////////////////////////////////////////////////////////////////////////////// +/*! \addtogroup enums Global enumerations + @{ +*/ + +//AUTO namespace xstypes { +/*! \enum XsCanDataIdentifier + \brief Defines the data identifiers for CAN messages + +*/ +enum XsCanDataIdentifier +{ + XCDI_Invalid = 0x00, + + /* Group Information & Timestamp Messages */ + XCDI_Error = 0x01, + XCDI_Warning = 0x02, + + XCDI_SampleTime = 0x05, //!< Sample Time in us + XCDI_GroupCounter = 0x06, + XCDI_UtcTime = 0x07, + + /* Group Status Messages */ + XCDI_StatusWord = 0x11, + + /* Group Quaternion Messages */ + XCDI_Quaternion = 0x21, + XCDI_EulerAngles = 0x22, + XCDI_RotationMatrix = 0x23, + + /* Group Inertial Data Messages */ + XCDI_DeltaV = 0x31, //!< DeltaV SDI data output + XCDI_RateOfTurn = 0x32, + XCDI_DeltaQ = 0x33, //!< DeltaQ SDI data + XCDI_Acceleration = 0x34, + XCDI_FreeAcceleration = 0x35, + + /* Group Magnetic Field */ + XCDI_MagneticField = 0x41, //!< Magnetic field data in a.u. + + /* Group Temperature & Pressure Messages */ + XCDI_Temperature = 0x51, //!< Temperature + XCDI_BaroPressure = 0x52, //!< Pressure output recorded from the barometer + + /* Group High-Rate Data Messages */ + XCDI_RateOfTurnHR = 0x61, + XCDI_AccelerationHR = 0x62, + + /* Group Position & Velocity Messages */ + XCDI_LatLong = 0x71, + XCDI_AltitudeEllipsoid = 0x72, + XCDI_PositionEcef_X = 0x73, + XCDI_PositionEcef_Y = 0x74, + XCDI_PositionEcef_Z = 0x75, + XCDI_Velocity = 0x76, + XCDI_Latitude = 0x77, + XCDI_Longitude = 0x78, + 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. +}; +/*! @} */ + +typedef enum XsCanDataIdentifier XsCanDataIdentifier; + +#endif diff --git a/extern/xspublic/xstypes/xscanframeformat.h b/extern/xspublic/xstypes/xscanframeformat.h new file mode 100644 index 0000000..e538275 --- /dev/null +++ b/extern/xspublic/xstypes/xscanframeformat.h @@ -0,0 +1,55 @@ + +// Copyright (c) 2003-2022 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 XSCANFRAMEFORMAT_H +#define XSCANFRAMEFORMAT_H + +////////////////////////////////////////////////////////////////////////////////////////// +/*! \addtogroup enums Global enumerations + @{ +*/ + +//AUTO namespace xstypes { +/*! \enum XsCanFrameFormat + \brief Defines the Frame format for CAN messages + +*/ +enum XsCanFrameFormat +{ + XCFF_11Bit_Identifier = 0, + XCFF_29Bit_Identifier = 1, +}; +/*! @} */ + +typedef enum XsCanFrameFormat XsCanFrameFormat; + +#endif diff --git a/extern/xspublic/xstypes/xscanoutputconfiguration.c b/extern/xspublic/xstypes/xscanoutputconfiguration.c new file mode 100644 index 0000000..b5fe424 --- /dev/null +++ b/extern/xspublic/xstypes/xscanoutputconfiguration.c @@ -0,0 +1,60 @@ + +// Copyright (c) 2003-2022 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 "xscanoutputconfiguration.h" + +void XsCanOutputConfiguration_swap(struct XsCanOutputConfiguration* a, struct XsCanOutputConfiguration* b) +{ + { + XsCanDataIdentifier t = a->m_dataIdentifier; + a->m_dataIdentifier = b->m_dataIdentifier; + b->m_dataIdentifier = t; + } + + { + uint16_t t = a->m_frequency; + a->m_frequency = b->m_frequency; + b->m_frequency = t; + } + + { + uint32_t t = a->m_id; + a->m_id = b->m_id; + b->m_id = t; + } + + { + XsCanFrameFormat t = a->m_frameFormat; + a->m_frameFormat = b->m_frameFormat; + b->m_frameFormat = t; + } +} diff --git a/extern/xspublic/xstypes/xscanoutputconfiguration.h b/extern/xspublic/xstypes/xscanoutputconfiguration.h new file mode 100644 index 0000000..a432b53 --- /dev/null +++ b/extern/xspublic/xstypes/xscanoutputconfiguration.h @@ -0,0 +1,107 @@ + +// Copyright (c) 2003-2022 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 XSCANOUTPUTCONFIGURATION_H +#define XSCANOUTPUTCONFIGURATION_H + +#include "xstypesconfig.h" +#include "pstdint.h" +#include "xscandataidentifier.h" +#include "xscanframeformat.h" + +#define XS_MAX_CANOUTPUTCONFIGURATIONS (16) + +#ifdef __cplusplus +extern "C" { +#endif +#ifndef __cplusplus +#define XSCANOUTPUTCONFIGURATION_INITIALIZER { XCDI_None, 0 } +#endif + +struct XsCanOutputConfiguration; + +XSTYPES_DLL_API void XsCanOutputConfiguration_swap(struct XsCanOutputConfiguration* a, struct XsCanOutputConfiguration* b); + +#ifdef __cplusplus +} // extern "C" +#endif + + +/*! \brief Single data type CAN output configuration + \details This structure contains a single data type and the frequency at which it should be produced. + If m_frequency is 0xFFFF and the %XsCanOutputConfiguration is used for input, the device will configure + 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 +{ + 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 + uint16_t m_frequency; //!< The frequency + +#ifdef __cplusplus + //! Constructor, initializes to an empty object + inline XsCanOutputConfiguration() + : m_frameFormat(XCFF_11Bit_Identifier) + , m_dataIdentifier(XCDI_Invalid) + , m_id(0) + , m_frequency(0) + {} + + //! Constructor, initializes to specified values + inline XsCanOutputConfiguration(XsCanFrameFormat il, XsCanDataIdentifier di, uint32_t id, uint16_t freq) + : m_frameFormat(il) + , m_dataIdentifier(di) + , m_id(id) + , m_frequency(freq) + {} + + //! 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; + +#endif diff --git a/extern/xspublic/xstypes/xscanoutputconfigurationarray.c b/extern/xspublic/xstypes/xscanoutputconfigurationarray.c new file mode 100644 index 0000000..93419e2 --- /dev/null +++ b/extern/xspublic/xstypes/xscanoutputconfigurationarray.c @@ -0,0 +1,99 @@ + +// Copyright (c) 2003-2022 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 "xscanoutputconfigurationarray.h" +#include "xscanoutputconfiguration.h" + +/*! \struct XsCanOutputConfigurationArray + \brief A list of XsCanOutputConfiguration values + \sa XsArray +*/ + +/*! \copydoc XsArrayDescriptor::itemSwap + \note Specialization for XsCanOutputConfiguration*/ +void swapXsCanOutputConfiguration(XsCanOutputConfiguration* a, XsCanOutputConfiguration* b) +{ + XsCanOutputConfiguration tmp = *a; + *a = *b; + *b = tmp; +} + +/*! \copydoc XsArrayDescriptor::itemCopy + \note Specialization for XsCanOutputConfiguration*/ +void copyXsCanOutputConfiguration(XsCanOutputConfiguration* to, XsCanOutputConfiguration const* from) +{ + *to = *from; +} + +/*! \copydoc XsArrayDescriptor::itemCompare + \note Specialization for XsCanOutputConfiguration*/ +int compareXsCanOutputConfiguration(XsCanOutputConfiguration const* a, XsCanOutputConfiguration const* b) +{ + if (a->m_dataIdentifier != b->m_dataIdentifier || a->m_frequency != b->m_frequency || a->m_id != b->m_id || a->m_frameFormat != b->m_frameFormat) + { + if (a->m_dataIdentifier == b->m_dataIdentifier) + { + 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; + } + return (a->m_dataIdentifier < b->m_dataIdentifier) ? -1 : 1; + } + + return 0; +} + + +//! \brief Descriptor for XsCanOutputConfigurationArray +XsArrayDescriptor const g_xsCanOutputConfigurationArrayDescriptor = +{ + sizeof(XsCanOutputConfiguration), + XSEXPCASTITEMSWAP swapXsCanOutputConfiguration, // swap + 0, // construct + XSEXPCASTITEMCOPY copyXsCanOutputConfiguration, // copy construct + 0, // destruct + XSEXPCASTITEMCOPY copyXsCanOutputConfiguration, // copy + XSEXPCASTITEMCOMP compareXsCanOutputConfiguration, // compare + XSEXPCASTRAWCOPY XsArray_rawCopy // raw copy +}; + +/*! \copydoc XsArray_constructDerived + \note Specialization for XsCanOutputConfigurationArray +*/ +void XsCanOutputConfigurationArray_construct(XsCanOutputConfigurationArray* thisPtr, XsSize count, XsCanOutputConfiguration const* src) +{ + XsArray_construct(thisPtr, &g_xsCanOutputConfigurationArrayDescriptor, count, src); +} diff --git a/extern/xspublic/xstypes/xscanoutputconfigurationarray.h b/extern/xspublic/xstypes/xscanoutputconfigurationarray.h new file mode 100644 index 0000000..eec61a6 --- /dev/null +++ b/extern/xspublic/xstypes/xscanoutputconfigurationarray.h @@ -0,0 +1,97 @@ + +// Copyright (c) 2003-2022 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 XSCANOUTPUTCONFIGURATIONARRAY_H +#define XSCANOUTPUTCONFIGURATIONARRAY_H + +#include "xsarray.h" + +#ifdef __cplusplus +#include "xscanoutputconfiguration.h" +extern "C" { +#endif + +extern XsArrayDescriptor const XSTYPES_DLL_API g_xsCanOutputConfigurationArrayDescriptor; + +#ifndef __cplusplus +#define XSCANOUTPUTCONFIGURATIONARRAY_INITIALIZER XSARRAY_INITIALIZER(&g_xsCanOutputConfigurationArrayDescriptor) +struct XsCanOutputConfiguration; + +XSARRAY_STRUCT(XsCanOutputConfigurationArray, struct XsCanOutputConfiguration); +typedef struct XsCanOutputConfigurationArray XsCanOutputConfigurationArray; + +XSTYPES_DLL_API void XsCanOutputConfigurationArray_construct(XsCanOutputConfigurationArray* thisPtr, XsSize count, struct XsCanOutputConfiguration const* src); +#endif + +#ifdef __cplusplus +} // extern "C" + +struct XsCanOutputConfigurationArray : public XsArrayImpl +{ + //! \brief Constructs an XsCanOutputConfigurationArray + inline explicit XsCanOutputConfigurationArray(XsSize sz = 0, XsCanOutputConfiguration const* src = 0) + : ArrayImpl(sz, src) + { + } + + //! \brief Constructs an XsCanOutputConfigurationArray as a copy of \a other + inline XsCanOutputConfigurationArray(XsCanOutputConfigurationArray const& other) + : ArrayImpl(other) + { + } + + //! \brief Constructs an XsCanOutputConfigurationArray that references the data supplied in \a ref + inline explicit XsCanOutputConfigurationArray(XsCanOutputConfiguration* 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(XsCanOutputConfigurationArray& first, XsCanOutputConfigurationArray& second) + { + first.swap(second); + } +#endif + +#ifndef XSENS_NOITERATOR + //! \brief Constructs an XsCanOutputConfigurationArray with the array bound by the supplied iterators \a beginIt and \a endIt + template + inline XsCanOutputConfigurationArray(Iterator beginIt, Iterator endIt) + : ArrayImpl(beginIt, endIt) + { + } +#endif +}; +#endif + +#endif diff --git a/extern/xspublic/xstypes/xscontrolline.h b/extern/xspublic/xstypes/xscontrolline.h new file mode 100644 index 0000000..2bfb126 --- /dev/null +++ b/extern/xspublic/xstypes/xscontrolline.h @@ -0,0 +1,55 @@ + +// Copyright (c) 2003-2022 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 XSCONTROLLINE_H +#define XSCONTROLLINE_H + +/*! \addtogroup enums Global enumerations + @{ +*/ +/*! \brief Serial control lines. */ +enum XsControlLine +{ + XCL_DCD = 0x0001, //!< pin 1: Carrier Detect + XCL_RD = 0x0002, //!< pin 2: Received Data + XCL_TD = 0x0004, //!< pin 3: Transmitted Data + XCL_DTR = 0x0008, //!< pin 4: Data Terminal Ready + XCL_GND = 0x0010, //!< pin 5: Common Ground + XCL_DSR = 0x0020, //!< pin 6: Data Set Ready + XCL_RTS = 0x0040, //!< pin 7: Request To Send + XCL_CTS = 0x0080, //!< pin 8: Clear To Send + XCL_RI = 0x0100 //!< pin 9: Ring Indicator +}; +/*! @} */ +typedef enum XsControlLine XsControlLine; + +#endif diff --git a/extern/xspublic/xstypes/xscopy.h b/extern/xspublic/xstypes/xscopy.h new file mode 100644 index 0000000..ea4c6a6 --- /dev/null +++ b/extern/xspublic/xstypes/xscopy.h @@ -0,0 +1,91 @@ + +// Copyright (c) 2003-2022 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 XSCOPY_H +#define XSCOPY_H + +#define XSLISTCOPY(C) \ + if (copy == thisPtr)\ + {\ + return;\ + }\ + C##_assign(copy, thisPtr->m_size, thisPtr->m_data) + +#define XSLISTSWAP3(C, B, S) \ + if ((!a->m_data || (a->m_flags & XSDF_Managed)) && (!b->m_data || (b->m_flags & XSDF_Managed))) {\ + B tmp;\ + *((C**) &tmp.m_data) = a->m_data;\ + *((XsSize*) &tmp.m_size) = a->m_size;\ + *((XsSize*) &tmp.m_flags) = a->m_flags;\ + *((C**) &a->m_data) = b->m_data;\ + *((XsSize*) &a->m_size) = b->m_size;\ + *((XsSize*) &a->m_flags) = b->m_flags;\ + *((C**) &b->m_data) = tmp.m_data;\ + *((XsSize*) &b->m_size) = tmp.m_size;\ + *((XsSize*) &b->m_flags) = tmp.m_flags;\ + } else { /* elementwise swap */ \ + XsSize i;\ + assert(a->m_size == b->m_size);\ + for (i = 0; i < a->m_size; ++i) S(&a->m_data[i], &b->m_data[i]);\ + } + +#define XSLISTSWAP2(C, B) XSLISTSWAP3(C, B, C##_swap) + +#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/extern/xspublic/xstypes/xsdataidentifier.h b/extern/xspublic/xstypes/xsdataidentifier.h new file mode 100644 index 0000000..e21ea53 --- /dev/null +++ b/extern/xspublic/xstypes/xsdataidentifier.h @@ -0,0 +1,191 @@ + +// Copyright (c) 2003-2022 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 XSDATAIDENTIFIER_H +#define XSDATAIDENTIFIER_H + +////////////////////////////////////////////////////////////////////////////////////////// +/*! \addtogroup enums Global enumerations + @{ +*/ + +//AUTO namespace xstypes { +/*! \enum XsDataIdentifier + \brief Defines the data identifiers + + The list of standard data identifiers is shown below. + The last positions in the data identifier depends on the configuration of the data. + For example 0x4020 is 3D acceleration in single precision float format, + where 0x4022 is 3D acceleration in fixed point 16.32 format. + + Refer to the Low Level Communication Protocol for more details. +*/ +enum XsDataIdentifier +{ + XDI_None = 0x0000, //!< Empty datatype + XDI_TypeMask = 0xFE00, //!< Mask for checking the group which a dataidentifier belongs to, Eg. XDI_TimestampGroup or XDI_OrientationGroup + XDI_FullTypeMask = 0xFFF0, //!< Mask to get the type of data, without the data format + XDI_FullMask = 0xFFFF, //!< Complete mask to get entire data identifier + XDI_FormatMask = 0x01FF, //!< Mask for getting the data id without checking the group + XDI_DataFormatMask = 0x000F, //!< Mask for extracting just the data format /sa XDI_SubFormat + + XDI_SubFormatMask = 0x0003, //!< Determines, float, fp12.20, fp16.32, double output... (where applicable) + XDI_SubFormatFloat = 0x0000, //!< Floating point format + XDI_SubFormatFp1220 = 0x0001, //!< Fixed point 12.20 + XDI_SubFormatFp1632 = 0x0002, //!< Fixed point 16.32 + XDI_SubFormatDouble = 0x0003, //!< Double format + + XDI_SubFormatLeft = 0x0000, //!< Left side data (ie XDI_GloveData for the left hand) + XDI_SubFormatRight = 0x0001, //!< Right side data (ie XDI_GloveData for the right hand) + + XDI_TemperatureGroup = 0x0800, //!< Group for temperature outputs + XDI_Temperature = 0x0810, //!< Temperature + + XDI_TimestampGroup = 0x1000, //!< Group for time stamp related outputs + XDI_UtcTime = 0x1010, //!< Utc time from the GNSS receiver + XDI_PacketCounter = 0x1020, //!< Packet counter, increments every packet + XDI_Itow = 0x1030, //!< Itow. Time Of Week from the GNSS receiver + XDI_GnssAge = 0x1040, //!< Gnss age from the GNSS receiver + XDI_PressureAge = 0x1050, //!< Age of a pressure sample, in packet counts + XDI_SampleTimeFine = 0x1060, //!< Sample Time Fine + XDI_SampleTimeCoarse = 0x1070, //!< Sample Time Coarse + XDI_FrameRange = 0x1080, //!< Reserved \internal add for MTw (if needed) + XDI_PacketCounter8 = 0x1090, //!< 8 bit packet counter, wraps at 256 + XDI_SampleTime64 = 0x10A0, //!< 64 bit sample time + + XDI_OrientationGroup = 0x2000, //!< Group for orientation related outputs + XDI_CoordSysMask = 0x000C, //!< Mask for the coordinate system part of the orientation data identifier + XDI_CoordSysEnu = 0x0000, //!< East North Up orientation output + XDI_CoordSysNed = 0x0004, //!< North East Down orientation output + XDI_CoordSysNwu = 0x0008, //!< North West Up orientation output + XDI_Quaternion = 0x2010, //!< Orientation in quaternion format + XDI_RotationMatrix = 0x2020, //!< Orientation in rotation matrix format + XDI_EulerAngles = 0x2030, //!< Orientation in euler angles format + + XDI_PressureGroup = 0x3000, //!< Group for pressure related outputs + XDI_BaroPressure = 0x3010, //!< Pressure output recorded from the barometer + + XDI_AccelerationGroup = 0x4000, //!< Group for acceleration related outputs + XDI_DeltaV = 0x4010, //!< DeltaV SDI data output + XDI_Acceleration = 0x4020, //!< Acceleration output in m/s2 + XDI_FreeAcceleration = 0x4030, //!< Free acceleration output in m/s2 + XDI_AccelerationHR = 0x4040, //!< AccelerationHR output + + XDI_IndicationGroup = 0x4800, //!< 0100.1000 -> bit reverse = 0001.0010 -> type 18 + XDI_TriggerIn1 = 0x4810, //!< Trigger in 1 indication + XDI_TriggerIn2 = 0x4820, //!< Trigger in 2 indication + XDI_TriggerIn3 = 0x4830, //!< Trigger in 3 indication + + XDI_PositionGroup = 0x5000, //!< Group for position related outputs + XDI_AltitudeMsl = 0x5010, //!< Altitude at Mean Sea Level + XDI_AltitudeEllipsoid = 0x5020, //!< Altitude at ellipsoid + XDI_PositionEcef = 0x5030, //!< Position in earth-centered, earth-fixed format + XDI_LatLon = 0x5040, //!< Position in latitude, longitude + + XDI_GnssGroup = 0x7000, //!< Group for Gnss related outputs + XDI_GnssPvtData = 0x7010, //!< Gnss position, velocity and time data + XDI_GnssSatInfo = 0x7020, //!< Gnss satellite information + XDI_GnssPvtPulse = 0x7030, //!< Time of the PVT timepulse (4Hz upsampled from the 1PPS) + + XDI_AngularVelocityGroup = 0x8000, //!< Group for angular velocity related outputs + XDI_RateOfTurn = 0x8020, //!< Rate of turn data in rad/sec + XDI_DeltaQ = 0x8030, //!< DeltaQ SDI data + XDI_RateOfTurnHR = 0x8040, //!< Rate of turn HR data + + XDI_RawSensorGroup = 0xA000, //!< Group for raw sensor data related outputs + XDI_RawUnsigned = 0x0000, //!< Tracker produces unsigned raw values, usually fixed behavior + XDI_RawSigned = 0x0001, //!< Tracker produces signed raw values, usually fixed behavior + XDI_RawAccGyrMagTemp = 0xA010, //!< Raw acceleration, gyroscope, magnetometer and temperature data + XDI_RawGyroTemp = 0xA020, //!< Raw gyroscope and temperature data + XDI_RawAcc = 0xA030, //!< Raw acceleration data + XDI_RawGyr = 0xA040, //!< Raw gyroscope data + XDI_RawMag = 0xA050, //!< Raw magnetometer data + XDI_RawDeltaQ = 0xA060, //!< Raw deltaQ SDI data + XDI_RawDeltaV = 0xA070, //!< Raw deltaV SDI data + XDI_RawBlob = 0xA080, //!< Raw blob data + + XDI_AnalogInGroup = 0xB000, //!< Group for analog in related outputs + XDI_AnalogIn1 = 0xB010, //!< Data containing adc data from analog in 1 line (if present) + XDI_AnalogIn2 = 0xB020, //!< Data containing adc data from analog in 2 line (if present) + + XDI_MagneticGroup = 0xC000, //!< Group for magnetometer related outputs + XDI_MagneticField = 0xC020, //!< Magnetic field data in a.u. + XDI_MagneticFieldCorrected = 0xC030, //!< Corrected Magnetic field data in a.u. (ICC result) + + XDI_SnapshotGroup = 0xC800, //!< Group for snapshot related outputs + XDI_RetransmissionMask = 0x0001, //!< Mask for the retransmission bit in the snapshot data + 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 + + // 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_VelocityGroup = 0xD000, //!< Group for velocity related outputs + XDI_VelocityXYZ = 0xD010, //!< Velocity in XYZ coordinate frame + + XDI_StatusGroup = 0xE000, //!< Group for status related outputs + XDI_StatusByte = 0xE010, //!< Status byte + XDI_StatusWord = 0xE020, //!< Status word + XDI_Rssi = 0xE040, //!< Rssi information + XDI_DeviceId = 0xE080, //!< DeviceId output + XDI_LocationId = 0xE090, //!< LocationId output +}; +/*! @} */ + +typedef enum XsDataIdentifier XsDataIdentifier; +//AUTO } + +#ifdef __cplusplus +inline XsDataIdentifier operator | (XsDataIdentifier a, XsDataIdentifier b) +{ + return (XsDataIdentifier)((int) a | (int) b); +} + +inline XsDataIdentifier operator & (XsDataIdentifier a, XsDataIdentifier b) +{ + return (XsDataIdentifier)((int) a & (int) b); +} + +inline XsDataIdentifier operator ~(XsDataIdentifier a) +{ + return (XsDataIdentifier) ~((unsigned short)a); +} +#endif + +#include "xsdataidentifiervalue.h" + +#endif diff --git a/extern/xspublic/xstypes/xsdataidentifiervalue.h b/extern/xspublic/xstypes/xsdataidentifiervalue.h new file mode 100644 index 0000000..0e0801d --- /dev/null +++ b/extern/xspublic/xstypes/xsdataidentifiervalue.h @@ -0,0 +1,58 @@ + +// Copyright (c) 2003-2022 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 XSDATAIDENTIFIERVALUE_H +#define XSDATAIDENTIFIERVALUE_H + +#include "pstdint.h" +#define XDI_MAX_FREQUENCY_VAL 0xFFFF +#define XDI_MAX_FREQUENCY ((uint16_t) XDI_MAX_FREQUENCY_VAL) + +////////////////////////////////////////////////////////////////////////////////////////// +/*! \addtogroup enums Global enumerations + @{ +*/ + +/*! \enum XsDataIdentifierValue + \brief Defines some convenience values for use with the data identifiers + + Refer to the Low Level Communication Protocol for more details. +*/ +enum XsDataIdentifierValue +{ + XDIV_MaxFrequency = XDI_MAX_FREQUENCY_VAL, //!< Maximum / no frequency +}; +/*! @} */ + +typedef enum XsDataIdentifierValue XsDataIdentifierValue; + +#endif diff --git a/extern/xspublic/xstypes/xsdatapacket.cpp b/extern/xspublic/xstypes/xsdatapacket.cpp new file mode 100644 index 0000000..a35546e --- /dev/null +++ b/extern/xspublic/xstypes/xsdatapacket.cpp @@ -0,0 +1,2597 @@ + +// Copyright (c) 2003-2022 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 "xsdatapacket.h" +#include "datapacket_p.h" +#include "xsmatrix3x3.h" +#include "xsvector.h" +#include "xsrssi.h" +#include "xsmath.h" +#include + +#define MAP (*thisPtr->d) + +static const uint64_t coarseFactor = 10000ULL; + +using namespace XsDataPacket_Private; + +static void detach(XsDataPacket* thisPtr) +{ + if (thisPtr->d->m_refCount == 1) + return; + DataPacketPrivate* old = thisPtr->d; + thisPtr->d = new DataPacketPrivate(*old); + if (--old->m_refCount == 0) // this can happen in some concurrent situations + delete old; +} + +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; + } +} + +XsUShortVector* rawVector(const XsDataPacket* thisPtr, XsUShortVector* returnVal, XsDataIdentifier id, XsUShortVector XsScrData::* field) +{ + assert(returnVal); + auto it = MAP.find(XDI_RawAccGyrMagTemp); + if (it != MAP.end()) + *returnVal = it->second->toDerived().m_data.*field; + else + { + it = MAP.find(id); + if (it != MAP.end()) + *returnVal = it->second->toDerived().m_data; + } + return returnVal; +} + +void setRawVector(XsDataPacket* thisPtr, const XsUShortVector* vec, XsDataIdentifier id, XsUShortVector XsScrData::* field) +{ + detach(thisPtr); + auto it = MAP.find(XDI_RawAccGyrMagTemp); + if (it != MAP.end()) + it->second->toDerived().m_data.*field = *vec; + else + { + it = MAP.find(id); + if (it != MAP.end()) + it->second->toDerived().m_data = *vec; + else + MAP.insert(id, new XsUShortVectorVariant(id, *vec)); + } +} + +template +T* genericGet(const XsDataPacket* thisPtr, T* returnVal, XsDataIdentifier id, T const& failValue = T()) +{ + assert(returnVal); + auto it = MAP.find(id); + if (it != MAP.end()) + *returnVal = it->second->toDerived().m_data; + else + *returnVal = failValue; + return returnVal; +} + +template +void genericSet(XsDataPacket* thisPtr, T const* val, XsDataIdentifier id) +{ + detach(thisPtr); + assert(val); + auto it = MAP.find(id); + if (it != MAP.end()) + { + it->second->toDerived().m_data = *val; + it->second->setDataId(id); + } + else + MAP.insert(id, new V(id, *val)); +} + +template > +struct GenericSimple +{ + 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)); + } +}; + +inline bool genericContains(const XsDataPacket* thisPtr, XsDataIdentifier id) +{ + return MAP.find(id) != MAP.end(); +} + +/*! \cond XS_INTERNAL */ +/*! \relates XsDataPacket + \brief Check if data item contains quaternion orientation data + \returns true if this packet contains quaternion orientation data +*/ +int XsDataPacket_containsOrientationQuaternion(const XsDataPacket* thisPtr) +{ + return genericContains(thisPtr, XDI_Quaternion); +} + +/*! \relates XsDataPacket + \brief Check if data item contains euler orientation data + \returns true if this packet contains euler orientation data +*/ +int XsDataPacket_containsOrientationEuler(const XsDataPacket* thisPtr) +{ + return genericContains(thisPtr, XDI_EulerAngles); +} + +/*! \relates XsDataPacket + \brief Check if data item contains matrix orientation data + \returns true if this packet contains matrix orientation data +*/ +int XsDataPacket_containsOrientationMatrix(const XsDataPacket* thisPtr) +{ + return genericContains(thisPtr, XDI_RotationMatrix); +} + +/*! \brief Returns quaternion that defines the rotation necessary to rotate from coordinate system + \a actual to \a desired when used as the left side in a quaternion multiplication +*/ +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_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); + + 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; + + case XDI_CoordSysNwu: + return q_nwu2enu; + } + + case XDI_CoordSysNed: + switch (actual & XDI_CoordSysMask) + { + default: + case XDI_CoordSysEnu: + return q_enu2ned; + + case XDI_CoordSysNed: + return q_id; + + case XDI_CoordSysNwu: + return q_nwu2ned; + } + + case XDI_CoordSysNwu: + switch (actual & XDI_CoordSysMask) + { + default: + case XDI_CoordSysEnu: + return q_enu2nwu; + + case XDI_CoordSysNed: + return q_ned2nwu; + + case XDI_CoordSysNwu: + return q_id; + } + } +} + +/*! \endcond */ + + +/*! \class XsDataPacket + \brief Contains an interpreted data message. The class provides easy access to the contained + data through its many functions. + \sa cinterface For the C interface functions. +*/ + +/*! \addtogroup cinterface C Interface + @{ +*/ + +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 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 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) + { + XsDataPacket_destruct(thisPtr); + XsDataPacket_construct(thisPtr); + } + 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) + { + ++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 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 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 + + \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 unsigned_cast(uint16_t v) + { + return v; + } + + /*! \brief The raw accelerometer component of a data item. + + \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); + } + + /*! \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 + + \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); + } + + /*! \brief The raw gyroscope component of a data item. + + \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); + } + + /*! \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 The raw magnetometer component of a data item. + + \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); + } + + /*! \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. + + \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 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(); + } + + /*! \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()) + 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. + + \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; + } + + /*! \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 + { + 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 + { + 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; + } + 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 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 + { + 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 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 The calibrated accelerometer component of a data item. + + \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); + } + + /*! \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 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 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. + + \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); + } + + /*! \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 The calibrated magnetometer component of a data item. + + \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); + } + + /*! \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 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); + + 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) && + 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 The corrected magnetometer component of a data item (ICC result). + + \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); + } + + /*! \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 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. + + \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()) + { + *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)) + { + XsEuler eul; + XsDataPacket_orientationEuler(thisPtr, &eul, coordinateSystem); + returnVal->fromEulerAngles(eul); + } + //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. + + \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()) + { + *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; + } + + /*! \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. + + \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()) + { + *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 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; + } + + /*! \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) || + 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(); + + 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(); + + 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 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 The air pressure component of a data item. + + \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()) + { + 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; + + 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 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); + + 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) && + 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 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) + { + 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 The device id of a data item. + + \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); + } + + /*! \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 The location ID of a data item. + + \returns The requested location ID + */ + uint16_t XsDataPacket_storedLocationId(const XsDataPacket* thisPtr) + { + return GenericSimple::get(thisPtr, 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 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 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 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 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 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 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 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 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 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 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 The position lat lon alt component of a data item. + + \param returnVal : The XsVector to return the requested data in + + \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); + + 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); + + 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 + + \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); + } + + /*! \brief The position latitude longitude component of a data item. + + \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); + } + + /*! \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 + + \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. + + \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 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. + + \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 + + \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 + + \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. + + \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()) + { + *returnVal = it->second->toDerived().m_data; + XsDataIdentifier actualCsys = (it->second->dataId() & XDI_CoordSysMask); + XsDataIdentifier desiredCsys = (coordinateSystem & XDI_CoordSysMask); + if (actualCsys != desiredCsys) + { + XsVector3 vel(*returnVal); + XsVector& rv = *returnVal; + + switch (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; + + case XDI_CoordSysNwu: + { + 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; + + case XDI_CoordSysNed: + { + 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]; + } + } + break; + + default: + { + rv[0] = vel[0]; + rv[1] = vel[1]; + rv[2] = vel[2]; + } + break; + } + } + } + + 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 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)); + } + + /*! \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 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; + + it = MAP.find(XDI_StatusByte); + if (it != MAP.end()) + return it->second->toDerived>().m_data; + + 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 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)) + { + uint32_t word = GenericSimple::get(thisPtr, XDI_StatusWord); + word = ((word & (~0xFF)) | data); + XsDataPacket_setStatus(thisPtr, word); + } + 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); + + 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 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 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 + + \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 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 + + \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 + + \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())); + + 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) || + 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()) + { + auto& var = it->second->toDerived>(); + var.m_data = coarseFactor * (var.m_data / coarseFactor) + (counter % coarseFactor); + } + } + + /*! \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 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 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()) + { + hasSampletimeCoarse = true; + rv += ((uint64_t) it->second->toDerived>().m_data) * coarseFactor; + } + + it = MAP.find(XDI_SampleTimeFine); + if (it != MAP.end()) + { + uint32_t sampletimeFine = it->second->toDerived>().m_data; + if (hasSampletimeCoarse) + sampletimeFine = sampletimeFine % coarseFactor; + rv += sampletimeFine; + } + + 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)); + } + + /*! \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 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 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 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); + } + + /*! \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 + + \returns An XsVector containing the gravity acceleration + */ + XsVector* XsDataPacket_freeAcceleration(const XsDataPacket* thisPtr, XsVector* returnVal) + { + return genericGet(thisPtr, returnVal, XDI_FreeAcceleration); + } + + /*! \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 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 Returns the frame range contained in the datapacket + + \param returnVal : The XsRange object that will get the range from the packet + + \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 Returns whether the datapacket contains a framerange + + \returns Whether the datapacket contains a framerange + */ + int XsDataPacket_containsFrameRange(const XsDataPacket* thisPtr) + { + return genericContains(thisPtr, XDI_FrameRange); + } + + /*! \brief Sets or updates the frame range in the datapacket + + \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 Returns the rssi value contained in the datapacket + + \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 whether the datapacket contains an rssi value + + \returns Whether the datapacket contains an rssi value + */ + int XsDataPacket_containsRssi(const XsDataPacket* thisPtr) + { + return genericContains(thisPtr, XDI_Rssi); + } + + /*! \brief Sets or updates the rssi value in the datapacket + + \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 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 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 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 timestamp of a PVT Pulse + + \returns Returns the timestamp of a PVT Pulse + */ + uint32_t XsDataPacket_gnssPvtPulse(const XsDataPacket* thisPtr) + { + return GenericSimple::get(thisPtr, XDI_GnssPvtPulse); + } + + /*! \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); + } + + /*! \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 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); + } + + /*! \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 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 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 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 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 Merge the data items from \a other into the packet + + \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. + + \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) + { + 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); + + if (over) + { + 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 thisPtr; + } + + /*! \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); + + XsSize offset = 0; + XsSize sz = msg->getDataSize(); + + while (offset + 3 <= sz) // minimum size of an item is 2(ID) + 1(size) + 0(minimum size) + { + 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 + } + 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 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)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); + } + + /*! \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 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 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 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 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) + { + 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 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); + + XsUShortVector tmp; + + auto it = MAP.find(XDI_RawAccGyrMagTemp); + if (it != MAP.end()) + 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; + } + + /*! \brief The raw accelerometer component of a data item, converted to floating point values. + + \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); + } + + /*! \brief The raw gyroscope component of a data item, converted to floating point values. + + \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); + } + + /*! \brief The raw magnetometer component of a data item, converted to floating point values. + + \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); + } + + /*! \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 + + \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 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 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 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 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 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/extern/xspublic/xstypes/xsdatapacket.h b/extern/xspublic/xstypes/xsdatapacket.h new file mode 100644 index 0000000..88fb06c --- /dev/null +++ b/extern/xspublic/xstypes/xsdatapacket.h @@ -0,0 +1,1514 @@ + +// Copyright (c) 2003-2022 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 XSDATAPACKET_H +#define XSDATAPACKET_H + +#include "xstypedefs.h" +#include "pstdint.h" +#include "xsmessage.h" +#include "xstimestamp.h" +#include "xsdataidentifier.h" +#include "xsushortvector.h" +#include "xsscrdata.h" +#include "xscalibrateddata.h" +#include "xspressure.h" +#include "xssdidata.h" +#include "xsvector.h" +#include "xsquaternion.h" +#include "xsmatrix.h" +#include "xseuler.h" +#include "xsanalogindata.h" +#include "xstimeinfo.h" +#include "xsrawgnsspvtdata.h" +#include "xsrawgnsssatinfo.h" +#include "xsdeviceid.h" +#include "xsrange.h" +#include "xstriggerindicationdata.h" +#include "xssnapshot.h" +#include "xsglovesnapshot.h" +#include "xsglovedata.h" +#include "xshandid.h" + +#ifndef XSNOEXPORT + #define XSNOEXPORT +#endif + +//AUTO namespace xstypes { +struct XsDataPacket; +//AUTO } +struct XSNOEXPORT DataPacketPrivate; + +#ifdef __cplusplus +extern "C" { +#endif +#ifndef __cplusplus +typedef struct XsDataPacket XsDataPacket; +//#define XSDATAPACKET_INITIALIZER { 0, 0, XSDEVICEID_INITIALIZER, -1 } //Use XsDataPacket_construct in all cases because of dynamic initialization +#endif + +XSTYPES_DLL_API void XsDataPacket_construct(XsDataPacket* thisPtr); +XSTYPES_DLL_API void XsDataPacket_copyConstruct(XsDataPacket* thisPtr, XsDataPacket const* src); +XSTYPES_DLL_API void XsDataPacket_destruct(XsDataPacket* thisPtr); +XSTYPES_DLL_API void XsDataPacket_clear(XsDataPacket* thisPtr, XsDataIdentifier id); +XSTYPES_DLL_API void XsDataPacket_copy(XsDataPacket* copy, XsDataPacket const* src); +XSTYPES_DLL_API void XsDataPacket_swap(XsDataPacket* thisPtr, XsDataPacket* other); +XSTYPES_DLL_API int XsDataPacket_empty(const XsDataPacket* thisPtr); +XSTYPES_DLL_API int XsDataPacket_itemCount(const XsDataPacket* thisPtr); +XSTYPES_DLL_API void XsDataPacket_setMessage(XsDataPacket* thisPtr, const XsMessage* msg); +XSTYPES_DLL_API XsDataIdentifier XsDataPacket_dataFormat(const XsDataPacket* thisPtr, XsDataIdentifier id); +XSTYPES_DLL_API XsUShortVector* XsDataPacket_rawAcceleration(const XsDataPacket* thisPtr, XsUShortVector* returnVal); +XSTYPES_DLL_API XsVector* XsDataPacket_rawAccelerationConverted(const XsDataPacket* thisPtr, XsVector* returnVal); +XSTYPES_DLL_API int XsDataPacket_containsRawAcceleration(const XsDataPacket* thisPtr); +XSTYPES_DLL_API void XsDataPacket_setRawAcceleration(XsDataPacket* thisPtr, const XsUShortVector* vec); +XSTYPES_DLL_API XsUShortVector* XsDataPacket_rawGyroscopeData(const XsDataPacket* thisPtr, XsUShortVector* returnVal); +XSTYPES_DLL_API XsVector* XsDataPacket_rawGyroscopeDataConverted(const XsDataPacket* thisPtr, XsVector* returnVal); +XSTYPES_DLL_API int XsDataPacket_containsRawGyroscopeData(const XsDataPacket* thisPtr); +XSTYPES_DLL_API void XsDataPacket_setRawGyroscopeData(XsDataPacket* thisPtr, const XsUShortVector* vec); +XSTYPES_DLL_API XsUShortVector* XsDataPacket_rawGyroscopeTemperatureData(const XsDataPacket* thisPtr, XsUShortVector* returnVal); +XSTYPES_DLL_API XsVector* XsDataPacket_rawGyroscopeTemperatureDataConverted(const XsDataPacket* thisPtr, XsVector* returnVal); +XSTYPES_DLL_API int XsDataPacket_containsRawGyroscopeTemperatureData(const XsDataPacket* thisPtr); +XSTYPES_DLL_API void XsDataPacket_setRawGyroscopeTemperatureData(XsDataPacket* thisPtr, const XsUShortVector* vec); +XSTYPES_DLL_API XsUShortVector* XsDataPacket_rawMagneticField(const XsDataPacket* thisPtr, XsUShortVector* returnVal); +XSTYPES_DLL_API XsVector* XsDataPacket_rawMagneticFieldConverted(const XsDataPacket* thisPtr, XsVector* returnVal); +XSTYPES_DLL_API int XsDataPacket_containsRawMagneticField(const XsDataPacket* thisPtr); +XSTYPES_DLL_API void XsDataPacket_setRawMagneticField(XsDataPacket* thisPtr, const XsUShortVector* vec); +XSTYPES_DLL_API uint16_t XsDataPacket_rawTemperature(const XsDataPacket* thisPtr); +XSTYPES_DLL_API int XsDataPacket_containsRawTemperature(const XsDataPacket* thisPtr); +XSTYPES_DLL_API void XsDataPacket_setRawTemperature(XsDataPacket* thisPtr, uint16_t temp); +XSTYPES_DLL_API XsScrData* XsDataPacket_rawData(const XsDataPacket* thisPtr, XsScrData* returnVal); +XSTYPES_DLL_API int XsDataPacket_containsRawData(const XsDataPacket* thisPtr); +XSTYPES_DLL_API void XsDataPacket_setRawData(XsDataPacket* thisPtr, const XsScrData* data); +XSTYPES_DLL_API XsVector* XsDataPacket_velocityIncrement(const XsDataPacket* thisPtr, XsVector* returnVal); +XSTYPES_DLL_API int XsDataPacket_containsVelocityIncrement(const XsDataPacket* thisPtr); +XSTYPES_DLL_API void XsDataPacket_setVelocityIncrement(XsDataPacket* thisPtr, const XsVector* vec); +XSTYPES_DLL_API XsVector* XsDataPacket_calibratedAcceleration(const XsDataPacket* thisPtr, XsVector* returnVal); +XSTYPES_DLL_API int XsDataPacket_containsCalibratedAcceleration(const XsDataPacket* thisPtr); +XSTYPES_DLL_API void XsDataPacket_setCalibratedAcceleration(XsDataPacket* thisPtr, const XsVector* vec); +XSTYPES_DLL_API XsQuaternion* XsDataPacket_orientationIncrement(const XsDataPacket* thisPtr, XsQuaternion* returnVal); +XSTYPES_DLL_API int XsDataPacket_containsOrientationIncrement(const XsDataPacket* thisPtr); +XSTYPES_DLL_API void XsDataPacket_setOrientationIncrement(XsDataPacket* thisPtr, const XsQuaternion* quat); +XSTYPES_DLL_API XsVector* XsDataPacket_calibratedGyroscopeData(const XsDataPacket* thisPtr, XsVector* returnVal); +XSTYPES_DLL_API int XsDataPacket_containsCalibratedGyroscopeData(const XsDataPacket* thisPtr); +XSTYPES_DLL_API void XsDataPacket_setCalibratedGyroscopeData(XsDataPacket* thisPtr, const XsVector* vec); +XSTYPES_DLL_API XsVector* XsDataPacket_calibratedMagneticField(const XsDataPacket* thisPtr, XsVector* returnVal); +XSTYPES_DLL_API int XsDataPacket_containsCalibratedMagneticField(const XsDataPacket* thisPtr); +XSTYPES_DLL_API void XsDataPacket_setCalibratedMagneticField(XsDataPacket* thisPtr, const XsVector* vec); +XSTYPES_DLL_API XsVector* XsDataPacket_correctedMagneticField(const XsDataPacket* thisPtr, XsVector* returnVal); +XSTYPES_DLL_API int XsDataPacket_containsCorrectedMagneticField(const XsDataPacket* thisPtr); +XSTYPES_DLL_API void XsDataPacket_setCorrectedMagneticField(XsDataPacket* thisPtr, const XsVector* vec); +XSTYPES_DLL_API XsCalibratedData* XsDataPacket_calibratedData(const XsDataPacket* thisPtr, XsCalibratedData* returnVal); +XSTYPES_DLL_API int XsDataPacket_containsCalibratedData(const XsDataPacket* thisPtr); +XSTYPES_DLL_API void XsDataPacket_setCalibratedData(XsDataPacket* thisPtr, const XsCalibratedData* data); +XSTYPES_DLL_API XsQuaternion* XsDataPacket_orientationQuaternion(const XsDataPacket* thisPtr, XsQuaternion* returnVal, XsDataIdentifier coordinateSystem); +XSTYPES_DLL_API void XsDataPacket_setOrientationQuaternion(XsDataPacket* thisPtr, const XsQuaternion* data, XsDataIdentifier coordinateSystem); +XSTYPES_DLL_API XsEuler* XsDataPacket_orientationEuler(const XsDataPacket* thisPtr, XsEuler* returnVal, XsDataIdentifier coordinateSystem); +XSTYPES_DLL_API void XsDataPacket_setOrientationEuler(XsDataPacket* thisPtr, const XsEuler* data, XsDataIdentifier coordinateSystem); +XSTYPES_DLL_API XsMatrix* XsDataPacket_orientationMatrix(const XsDataPacket* thisPtr, XsMatrix* returnVal, XsDataIdentifier coordinateSystem); +XSTYPES_DLL_API void XsDataPacket_setOrientationMatrix(XsDataPacket* thisPtr, const XsMatrix* data, XsDataIdentifier coordinateSystem); +XSTYPES_DLL_API int XsDataPacket_containsOrientation(const XsDataPacket* thisPtr); +XSTYPES_DLL_API XsDataIdentifier XsDataPacket_orientationIdentifier(const XsDataPacket* thisPtr); +XSTYPES_DLL_API XsDataIdentifier XsDataPacket_coordinateSystemOrientation(const XsDataPacket* thisPtr); +XSTYPES_DLL_API XsSdiData* XsDataPacket_sdiData(const XsDataPacket* thisPtr, XsSdiData* returnVal); +XSTYPES_DLL_API int XsDataPacket_containsSdiData(const XsDataPacket* thisPtr); +XSTYPES_DLL_API void XsDataPacket_setSdiData(XsDataPacket* thisPtr, const XsSdiData* data); +XSTYPES_DLL_API XsGloveData* XsDataPacket_gloveData(const XsDataPacket* thisPtr, XsGloveData* returnVal, XsHandId hand); +XSTYPES_DLL_API int XsDataPacket_containsGloveData(const XsDataPacket* thisPtr, XsHandId hand); +XSTYPES_DLL_API void XsDataPacket_setGloveData(XsDataPacket* thisPtr, const XsGloveData* data, XsHandId hand); +XSTYPES_DLL_API XsDeviceId* XsDataPacket_storedDeviceId(const XsDataPacket* thisPtr, XsDeviceId* returnVal); +XSTYPES_DLL_API int XsDataPacket_containsStoredDeviceId(const XsDataPacket* thisPtr); +XSTYPES_DLL_API void XsDataPacket_setStoredDeviceId(XsDataPacket* thisPtr, const XsDeviceId* data); +XSTYPES_DLL_API uint16_t XsDataPacket_storedLocationId(const XsDataPacket* thisPtr); +XSTYPES_DLL_API int XsDataPacket_containsStoredLocationId(const XsDataPacket* thisPtr); +XSTYPES_DLL_API void XsDataPacket_setStoredLocationId(XsDataPacket* thisPtr, uint16_t data); +XSTYPES_DLL_API uint32_t XsDataPacket_status(const XsDataPacket* thisPtr); +XSTYPES_DLL_API int XsDataPacket_containsStatus(const XsDataPacket* thisPtr); +XSTYPES_DLL_API int XsDataPacket_containsDetailedStatus(const XsDataPacket* thisPtr); +XSTYPES_DLL_API void XsDataPacket_setStatus(XsDataPacket* thisPtr, uint32_t data); +XSTYPES_DLL_API void XsDataPacket_setStatusByte(XsDataPacket* thisPtr, uint8_t data); +XSTYPES_DLL_API uint8_t XsDataPacket_packetCounter8(const XsDataPacket* thisPtr); +XSTYPES_DLL_API int XsDataPacket_containsPacketCounter8(const XsDataPacket* thisPtr); +XSTYPES_DLL_API void XsDataPacket_setPacketCounter8(XsDataPacket* thisPtr, uint8_t counter); +XSTYPES_DLL_API uint16_t XsDataPacket_packetCounter(const XsDataPacket* thisPtr); +XSTYPES_DLL_API int XsDataPacket_containsPacketCounter(const XsDataPacket* thisPtr); +XSTYPES_DLL_API void XsDataPacket_setPacketCounter(XsDataPacket* thisPtr, uint16_t counter); +XSTYPES_DLL_API uint32_t XsDataPacket_sampleTimeFine(const XsDataPacket* thisPtr); +XSTYPES_DLL_API int XsDataPacket_containsSampleTimeFine(const XsDataPacket* thisPtr); +XSTYPES_DLL_API void XsDataPacket_setSampleTimeFine(XsDataPacket* thisPtr, uint32_t counter); +XSTYPES_DLL_API uint32_t XsDataPacket_sampleTimeCoarse(const XsDataPacket* thisPtr); +XSTYPES_DLL_API int XsDataPacket_containsSampleTimeCoarse(const XsDataPacket* thisPtr); +XSTYPES_DLL_API void XsDataPacket_setSampleTimeCoarse(XsDataPacket* thisPtr, uint32_t counter); +XSTYPES_DLL_API uint64_t XsDataPacket_sampleTime64(const XsDataPacket* thisPtr); +XSTYPES_DLL_API int XsDataPacket_containsSampleTime64(const XsDataPacket* thisPtr); +XSTYPES_DLL_API void XsDataPacket_setSampleTime64(XsDataPacket* thisPtr, uint64_t counter); +XSTYPES_DLL_API XsVector* XsDataPacket_freeAcceleration(const XsDataPacket* thisPtr, XsVector* returnVal); +XSTYPES_DLL_API int XsDataPacket_containsFreeAcceleration(const XsDataPacket* thisPtr); +XSTYPES_DLL_API void XsDataPacket_setFreeAcceleration(XsDataPacket* thisPtr, const XsVector* g); +XSTYPES_DLL_API double XsDataPacket_temperature(const XsDataPacket* thisPtr); +XSTYPES_DLL_API int XsDataPacket_containsTemperature(const XsDataPacket* thisPtr); +XSTYPES_DLL_API void XsDataPacket_setTemperature(XsDataPacket* thisPtr, double temperature); +XSTYPES_DLL_API int XsDataPacket_containsPressure(const XsDataPacket* thisPtr); +XSTYPES_DLL_API XsPressure* XsDataPacket_pressure(const XsDataPacket* thisPtr, XsPressure* returnVal); +XSTYPES_DLL_API int XsDataPacket_containsPressureAge(const XsDataPacket* thisPtr); +XSTYPES_DLL_API void XsDataPacket_setPressure(XsDataPacket* thisPtr, const XsPressure* data); +XSTYPES_DLL_API XsAnalogInData* XsDataPacket_analogIn1Data(const XsDataPacket* thisPtr, XsAnalogInData* returnVal); +XSTYPES_DLL_API int XsDataPacket_containsAnalogIn1Data(const XsDataPacket* thisPtr); +XSTYPES_DLL_API void XsDataPacket_setAnalogIn1Data(XsDataPacket* thisPtr, const XsAnalogInData* data); +XSTYPES_DLL_API XsAnalogInData* XsDataPacket_analogIn2Data(const XsDataPacket* thisPtr, XsAnalogInData* returnVal); +XSTYPES_DLL_API int XsDataPacket_containsAnalogIn2Data(const XsDataPacket* thisPtr); +XSTYPES_DLL_API void XsDataPacket_setAnalogIn2Data(XsDataPacket* thisPtr, const XsAnalogInData* data); +XSTYPES_DLL_API XsVector* XsDataPacket_positionLLA(const XsDataPacket* thisPtr, XsVector* returnVal); +XSTYPES_DLL_API int XsDataPacket_containsPositionLLA(const XsDataPacket* thisPtr); +XSTYPES_DLL_API void XsDataPacket_setPositionLLA(XsDataPacket* thisPtr, const XsVector* data); +XSTYPES_DLL_API XsVector* XsDataPacket_latitudeLongitude(const XsDataPacket* thisPtr, XsVector* returnVal); +XSTYPES_DLL_API int XsDataPacket_containsLatitudeLongitude(const XsDataPacket* thisPtr); +XSTYPES_DLL_API void XsDataPacket_setLatitudeLongitude(XsDataPacket* thisPtr, const XsVector* data); +XSTYPES_DLL_API double XsDataPacket_altitude(const XsDataPacket* thisPtr); +XSTYPES_DLL_API int XsDataPacket_containsAltitude(const XsDataPacket* thisPtr); +XSTYPES_DLL_API void XsDataPacket_setAltitude(XsDataPacket* thisPtr, double data); +XSTYPES_DLL_API double XsDataPacket_altitudeMsl(const XsDataPacket* thisPtr); +XSTYPES_DLL_API int XsDataPacket_containsAltitudeMsl(const XsDataPacket* thisPtr); +XSTYPES_DLL_API void XsDataPacket_setAltitudeMsl(XsDataPacket* thisPtr, double data); +XSTYPES_DLL_API XsVector* XsDataPacket_velocity(const XsDataPacket* thisPtr, XsVector* returnVal, XsDataIdentifier coordinateSystem); +XSTYPES_DLL_API int XsDataPacket_containsVelocity(const XsDataPacket* thisPtr); +XSTYPES_DLL_API void XsDataPacket_setVelocity(XsDataPacket* thisPtr, const XsVector* data, XsDataIdentifier coordinateSystem); +XSTYPES_DLL_API XsDataIdentifier XsDataPacket_velocityIdentifier(const XsDataPacket* thisPtr); +XSTYPES_DLL_API XsDataIdentifier XsDataPacket_coordinateSystemVelocity(const XsDataPacket* thisPtr); +XSTYPES_DLL_API XsTimeInfo* XsDataPacket_utcTime(const XsDataPacket* thisPtr, XsTimeInfo* returnVal); +XSTYPES_DLL_API int XsDataPacket_containsUtcTime(const XsDataPacket* thisPtr); +XSTYPES_DLL_API void XsDataPacket_setUtcTime(XsDataPacket* thisPtr, const XsTimeInfo* data); +XSTYPES_DLL_API XsRange* XsDataPacket_frameRange(const XsDataPacket* thisPtr, XsRange* returnVal); +XSTYPES_DLL_API int XsDataPacket_containsFrameRange(const XsDataPacket* thisPtr); +XSTYPES_DLL_API void XsDataPacket_setFrameRange(XsDataPacket* thisPtr, const XsRange* r); +XSTYPES_DLL_API int XsDataPacket_rssi(const XsDataPacket* thisPtr); +XSTYPES_DLL_API int XsDataPacket_containsRssi(const XsDataPacket* thisPtr); +XSTYPES_DLL_API void XsDataPacket_setRssi(XsDataPacket* thisPtr, int r); + +XSTYPES_DLL_API XsRawGnssPvtData* XsDataPacket_rawGnssPvtData(const XsDataPacket* thisPtr, XsRawGnssPvtData* returnVal); +XSTYPES_DLL_API int XsDataPacket_containsRawGnssPvtData(const XsDataPacket* thisPtr); +XSTYPES_DLL_API void XsDataPacket_setRawGnssPvtData(XsDataPacket* thisPtr, const XsRawGnssPvtData* r); +XSTYPES_DLL_API uint8_t XsDataPacket_gnssAge(const XsDataPacket* thisPtr); +XSTYPES_DLL_API int XsDataPacket_containsGnssAge(const XsDataPacket* thisPtr); +XSTYPES_DLL_API void XsDataPacket_setGnssAge(XsDataPacket* thisPtr, uint8_t age); +XSTYPES_DLL_API XsRawGnssSatInfo* XsDataPacket_rawGnssSatInfo(const XsDataPacket* thisPtr, XsRawGnssSatInfo* returnVal); +XSTYPES_DLL_API int XsDataPacket_containsRawGnssSatInfo(const XsDataPacket* thisPtr); +XSTYPES_DLL_API void XsDataPacket_setRawGnssSatInfo(XsDataPacket* thisPtr, const XsRawGnssSatInfo* r); +XSTYPES_DLL_API uint32_t XsDataPacket_gnssPvtPulse(const XsDataPacket* thisPtr); +XSTYPES_DLL_API int XsDataPacket_containsGnssPvtPulse(const XsDataPacket* thisPtr); +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 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 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 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 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 XsByteArray* XsDataPacket_rawBlob(const XsDataPacket* thisPtr, XsByteArray* returnVal); +XSTYPES_DLL_API int XsDataPacket_containsRawBlob(const XsDataPacket* thisPtr); + +XSTYPES_DLL_API XsVector* XsDataPacket_accelerationHR(const XsDataPacket* thisPtr, XsVector* returnVal); +XSTYPES_DLL_API int XsDataPacket_containsAccelerationHR(const XsDataPacket* thisPtr); +XSTYPES_DLL_API void XsDataPacket_setAccelerationHR(XsDataPacket* thisPtr, const XsVector* vec); + +XSTYPES_DLL_API XsVector* XsDataPacket_rateOfTurnHR(const XsDataPacket* thisPtr, XsVector* returnVal); +XSTYPES_DLL_API int XsDataPacket_containsRateOfTurnHR(const XsDataPacket* thisPtr); +XSTYPES_DLL_API void XsDataPacket_setRateOfTurnHR(XsDataPacket* thisPtr, const XsVector* vec); + +XSTYPES_DLL_API int XsDataPacket_privateCount(); + +#ifdef __cplusplus +} // extern "C" +#ifndef XSENS_NO_STL + #include // only needed for simplifiedContents() +#endif +#endif + +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 + MTData2 data. + */ + inline explicit XsDataPacket(const XsMessage* msg = 0) + { + XsDataPacket_construct(this); + if (msg) + XsDataPacket_setMessage(this, msg); + } + + /*! \brief Copy constructor + \param pack The packet to copy from + */ + inline XsDataPacket(const XsDataPacket& pack) + { + XsDataPacket_copyConstruct(this, &pack); + } + + //! \copydoc XsDataPacket_destruct + inline ~XsDataPacket() + { + XsDataPacket_destruct(this); + } + + /*! \brief Assignment operator + \param other The packet to copy from + \returns A reference to this %XsDataPacket + \sa XsDataPacket_copy + */ + inline XsDataPacket& operator = (const XsDataPacket& other) + { + if (this != &other) + XsDataPacket_copy(this, &other); + return *this; + } + + /*! \copydoc XsDataPacket_swap(XsDataPacket*,XsDataPacket*)*/ + inline void swap(XsDataPacket& other) + { + XsDataPacket_swap(this, &other); + } + +#ifndef SWIG + /*! \brief Swaps \a first with \a second + */ + XSNOEXPORT inline friend void swap(XsDataPacket& first, XsDataPacket& second) + { + first.swap(second); + } +#endif + + /*! \copydoc XsDataPacket_clear(XsDataPacket*,XsDataIdentifier)*/ + inline void clear(XsDataIdentifier id = XDI_None) + { + XsDataPacket_clear(this, id); + } + + /*! \copydoc XsDataPacket_empty(const XsDataPacket*)*/ + inline bool empty(void) const + { + return 0 != XsDataPacket_empty(this); + } + + //! \brief Return the device ID associated with the data packet + inline XsDeviceId deviceId() const + { + return m_deviceId; + } + + /*! \copydoc XsDataPacket_itemCount(const XsDataPacket*)*/ + inline uint16_t itemCount() const + { + return static_cast(static_cast(XsDataPacket_itemCount(this))); + } + + //! \copydoc XsDataPacket_setMessage(XsDataPacket*, const XsMessage*) + inline void setMessage(const XsMessage& msg) + { + XsDataPacket_setMessage(this, &msg); + } + + /*! \brief Returns a const reference to the message that contains the data packet + */ + inline XsMessage toMessage() const + { + XsMessage msg; + XsDataPacket_toMessage(this, &msg); + return msg; + } + + /*! \brief Set the device ID associated with this data packet + \param id The device ID to set + */ + inline void setDeviceId(const XsDeviceId id) + { + m_deviceId = id; + } + + /*! \copydoc XsDataPacket_dataFormat(const XsDataPacket*, XsDataIdentifier) */ + inline XsDataIdentifier dataFormat(XsDataIdentifier id) const + { + return XsDataPacket_dataFormat(this, id); + } + + /*! \brief \copybrief XsDataPacket_rawAcceleration(const XsDataPacket*, XsUShortVector*) */ + inline XsUShortVector rawAcceleration(void) const + { + XsUShortVector returnVal; + return *XsDataPacket_rawAcceleration(this, &returnVal); + } + + /*! \brief \copybrief XsDataPacket_rawAccelerationConverted(const XsDataPacket*, XsVector*) */ + inline XsVector rawAccelerationConverted(void) const + { + XsVector returnVal; + return *XsDataPacket_rawAccelerationConverted(this, &returnVal); + } + + /*! \copydoc XsDataPacket_containsRawAcceleration(const XsDataPacket*) */ + inline bool containsRawAcceleration(void) const + { + return 0 != XsDataPacket_containsRawAcceleration(this); + } + + /*! \copydoc XsDataPacket_setRawAcceleration(XsDataPacket*, const XsUShortVector*) */ + inline void setRawAcceleration(const XsUShortVector& vec) + { + XsDataPacket_setRawAcceleration(this, &vec); + } + + /*! \brief \copybrief XsDataPacket_rawGyroscopeData(const XsDataPacket*, XsUShortVector*) */ + inline XsUShortVector rawGyroscopeData(void) const + { + XsUShortVector returnVal; + return *XsDataPacket_rawGyroscopeData(this, &returnVal); + } + + /*! \brief \copybrief XsDataPacket_rawGyroscopeDataConverted(const XsDataPacket*, XsVector*) */ + inline XsVector rawGyroscopeDataConverted(void) const + { + XsVector returnVal; + return *XsDataPacket_rawGyroscopeDataConverted(this, &returnVal); + } + + /*! \copydoc XsDataPacket_containsRawGyroscopeData(const XsDataPacket*) */ + inline bool containsRawGyroscopeData(void) const + { + return 0 != XsDataPacket_containsRawGyroscopeData(this); + } + + /*! \copydoc XsDataPacket_setRawGyroscopeData(XsDataPacket*, const XsUShortVector*) */ + inline void setRawGyroscopeData(const XsUShortVector& vec) + { + XsDataPacket_setRawGyroscopeData(this, &vec); + } + + /*! \brief \copybrief XsDataPacket_rawGyroscopeTemperatureData(const XsDataPacket*, XsUShortVector*) */ + inline XsUShortVector rawGyroscopeTemperatureData(void) const + { + XsUShortVector returnVal; + return *XsDataPacket_rawGyroscopeTemperatureData(this, &returnVal); + } + + /*! \brief \copybrief XsDataPacket_rawGyroscopeTemperatureDataConverted(const XsDataPacket*, XsVector*) */ + inline XsVector rawGyroscopeTemperatureDataConverted(void) const + { + XsVector returnVal; + return *XsDataPacket_rawGyroscopeTemperatureDataConverted(this, &returnVal); + } + + /*! \copydoc XsDataPacket_containsRawGyroscopeTemperatureData(const XsDataPacket*) */ + inline bool containsRawGyroscopeTemperatureData(void) const + { + return 0 != XsDataPacket_containsRawGyroscopeTemperatureData(this); + } + + /*! \copydoc XsDataPacket_setRawGyroscopeTemperatureData(XsDataPacket*, const XsUShortVector*) */ + inline void setRawGyroscopeTemperatureData(const XsUShortVector& vec) + { + XsDataPacket_setRawGyroscopeTemperatureData(this, &vec); + } + + /*! \brief \copybrief XsDataPacket_rawMagneticField(const XsDataPacket*, XsUShortVector*) */ + inline XsUShortVector rawMagneticField(void) const + { + XsUShortVector returnVal; + return *XsDataPacket_rawMagneticField(this, &returnVal); + } + + /*! \brief \copybrief XsDataPacket_rawMagneticFieldConverted(const XsDataPacket*, XsVector*) */ + inline XsVector rawMagneticFieldConverted(void) const + { + XsVector returnVal; + return *XsDataPacket_rawMagneticFieldConverted(this, &returnVal); + } + + /*! \copydoc XsDataPacket_containsRawMagneticField(const XsDataPacket*) */ + inline bool containsRawMagneticField(void) const + { + return 0 != XsDataPacket_containsRawMagneticField(this); + } + + /*! \copydoc XsDataPacket_setRawMagneticField(XsDataPacket*, const XsUShortVector*) */ + inline void setRawMagneticField(const XsUShortVector& vec) + { + XsDataPacket_setRawMagneticField(this, &vec); + } + + /*! \brief \copybrief XsDataPacket_rawTemperature(const XsDataPacket*) */ + inline uint16_t rawTemperature(void) const + { + return XsDataPacket_rawTemperature(this); + } + + /*! \copydoc XsDataPacket_containsRawTemperature(const XsDataPacket*) */ + inline bool containsRawTemperature(void) const + { + return 0 != XsDataPacket_containsRawTemperature(this); + } + + /*! \copydoc XsDataPacket_setRawTemperature(XsDataPacket*, uint16_t) */ + inline void setRawTemperature(uint16_t temp) + { + XsDataPacket_setRawTemperature(this, temp); + } + + /*! \brief \copybrief XsDataPacket_rawData(const XsDataPacket*, XsScrData*) + \return The raw data component of a data item. + */ + inline XsScrData rawData(void) const + { + XsScrData returnVal; + return *XsDataPacket_rawData(this, &returnVal); + } + + /*! \copydoc XsDataPacket_containsRawData(const XsDataPacket*) */ + inline bool containsRawData(void) const + { + return 0 != XsDataPacket_containsRawData(this); + } + + /*! \copydoc XsDataPacket_setRawData(XsDataPacket*, const XsScrData*) */ + inline void setRawData(const XsScrData& data) + { + XsDataPacket_setRawData(this, &data); + } + + /*! \brief \copybrief XsDataPacket_velocityIncrement(const XsDataPacket*, XsVector*) */ + inline XsVector velocityIncrement(void) const + { + XsVector returnVal; + return *XsDataPacket_velocityIncrement(this, &returnVal); + } + + /*! \copydoc XsDataPacket_containsVelocityIncrement(const XsDataPacket*) */ + inline bool containsVelocityIncrement(void) const + { + return 0 != XsDataPacket_containsVelocityIncrement(this); + } + + /*! \copydoc XsDataPacket_setVelocityIncrement(XsDataPacket*, const XsVector*) */ + inline void setVelocityIncrement(const XsVector& vec) + { + XsDataPacket_setVelocityIncrement(this, &vec); + } + + /*! \brief \copybrief XsDataPacket_calibratedAcceleration(const XsDataPacket*, XsVector*) */ + inline XsVector calibratedAcceleration(void) const + { + XsVector returnVal; + return *XsDataPacket_calibratedAcceleration(this, &returnVal); + } + + /*! \copydoc XsDataPacket_containsCalibratedAcceleration(const XsDataPacket*) */ + inline bool containsCalibratedAcceleration(void) const + { + return 0 != XsDataPacket_containsCalibratedAcceleration(this); + } + + /*! \copydoc XsDataPacket_setCalibratedAcceleration(XsDataPacket*, const XsVector*) */ + inline void setCalibratedAcceleration(const XsVector& vec) + { + XsDataPacket_setCalibratedAcceleration(this, &vec); + } + + /*! \brief \copybrief XsDataPacket_orientationIncrement(const XsDataPacket*, XsQuaternion*) */ + inline XsQuaternion orientationIncrement(void) const + { + XsQuaternion returnVal; + return *XsDataPacket_orientationIncrement(this, &returnVal); + } + + /*! \copydoc XsDataPacket_containsOrientationIncrement(const XsDataPacket*) */ + inline bool containsOrientationIncrement(void) const + { + return 0 != XsDataPacket_containsOrientationIncrement(this); + } + + /*! \copydoc XsDataPacket_setOrientationIncrement(XsDataPacket*, const XsQuaternion*) */ + inline void setOrientationIncrement(const XsQuaternion& quat) + { + XsDataPacket_setOrientationIncrement(this, &quat); + } + + /*! \brief \copybrief XsDataPacket_calibratedGyroscopeData(const XsDataPacket*, XsVector*) */ + inline XsVector calibratedGyroscopeData(void) const + { + XsVector returnVal; + return *XsDataPacket_calibratedGyroscopeData(this, &returnVal); + } + + /*! \copydoc XsDataPacket_containsCalibratedGyroscopeData(const XsDataPacket*) */ + inline bool containsCalibratedGyroscopeData(void) const + { + return 0 != XsDataPacket_containsCalibratedGyroscopeData(this); + } + + /*! \copydoc XsDataPacket_setCalibratedGyroscopeData(XsDataPacket*, const XsVector*) */ + inline void setCalibratedGyroscopeData(const XsVector& vec) + { + XsDataPacket_setCalibratedGyroscopeData(this, &vec); + } + + /*! \brief \copybrief XsDataPacket_calibratedMagneticField(const XsDataPacket*, XsVector*) */ + inline XsVector calibratedMagneticField(void) const + { + XsVector returnVal; + return *XsDataPacket_calibratedMagneticField(this, &returnVal); + } + + /*! \copydoc XsDataPacket_containsCalibratedMagneticField(const XsDataPacket*) */ + inline bool containsCalibratedMagneticField(void) const + { + return 0 != XsDataPacket_containsCalibratedMagneticField(this); + } + + /*! \copydoc XsDataPacket_setCalibratedMagneticField(XsDataPacket*, const XsVector*) */ + inline void setCalibratedMagneticField(const XsVector& vec) + { + XsDataPacket_setCalibratedMagneticField(this, &vec); + } + + /*! \brief \copybrief XsDataPacket_correctedMagneticField(const XsDataPacket*, XsVector*) */ + XSNOEXPORT inline XsVector correctedMagneticField(void) const + { + XsVector returnVal; + return *XsDataPacket_correctedMagneticField(this, &returnVal); + } + + /*! \copydoc XsDataPacket_containsCorrectedMagneticField(const XsDataPacket*) */ + XSNOEXPORT inline bool containsCorrectedMagneticField(void) const + { + return 0 != XsDataPacket_containsCorrectedMagneticField(this); + } + + /*! \copydoc XsDataPacket_setCorrectedMagneticField(XsDataPacket*, const XsVector*) */ + XSNOEXPORT inline void setCorrectedMagneticField(const XsVector& vec) + { + XsDataPacket_setCorrectedMagneticField(this, &vec); + } + + /*! \brief \copybrief XsDataPacket_calibratedData(const XsDataPacket*, XsCalibratedData*) */ + inline XsCalibratedData calibratedData(void) const + { + XsCalibratedData returnVal; + return *XsDataPacket_calibratedData(this, &returnVal); + } + + /*! \copydoc XsDataPacket_containsCalibratedData(const XsDataPacket*) */ + inline bool containsCalibratedData(void) const + { + return 0 != XsDataPacket_containsCalibratedData(this); + } + + /*! \copydoc XsDataPacket_setCalibratedData(XsDataPacket*, const XsCalibratedData*) */ + inline void setCalibratedData(const XsCalibratedData& data) + { + XsDataPacket_setCalibratedData(this, &data); + } + + /*! \brief \copybrief XsDataPacket_orientationQuaternion(const XsDataPacket*, XsQuaternion*, XsDataIdentifier) + \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 The requested data + */ + inline XsQuaternion orientationQuaternion(XsDataIdentifier coordinateSystem) const + { + XsQuaternion returnVal; + return *XsDataPacket_orientationQuaternion(this, &returnVal, coordinateSystem); + } + + /*! \brief Returns the orientation as a quaternion with the current coordinate system + */ + inline XsQuaternion orientationQuaternion() const + { + XsQuaternion returnVal; + return *XsDataPacket_orientationQuaternion(this, &returnVal, coordinateSystemOrientation()); + } + + /*! \copydoc XsDataPacket_setOrientationQuaternion(XsDataPacket*, const XsQuaternion*, XsDataIdentifier) */ + inline void setOrientationQuaternion(const XsQuaternion& data, XsDataIdentifier coordinateSystem) + { + XsDataPacket_setOrientationQuaternion(this, &data, coordinateSystem); + } + + /*! \brief \copybrief XsDataPacket_orientationEuler(const XsDataPacket*, XsEuler*, XsDataIdentifier) + \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 The requested data + */ + inline XsEuler orientationEuler(XsDataIdentifier coordinateSystem) const + { + XsEuler returnVal; + return *XsDataPacket_orientationEuler(this, &returnVal, coordinateSystem); + } + + /*! \brief Returns the orientation as an XsEuler with the current coordinate system*/ + inline XsEuler orientationEuler() const + { + XsEuler returnVal; + return *XsDataPacket_orientationEuler(this, &returnVal, coordinateSystemOrientation()); + } + + /*! \copydoc XsDataPacket_setOrientationEuler(XsDataPacket*, const XsEuler*, XsDataIdentifier) */ + inline void setOrientationEuler(const XsEuler& data, XsDataIdentifier coordinateSystem) + { + XsDataPacket_setOrientationEuler(this, &data, coordinateSystem); + } + + /*! \brief \copybrief XsDataPacket_orientationMatrix(const XsDataPacket*, XsMatrix*, XsDataIdentifier) + \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 The requested data + */ + inline XsMatrix orientationMatrix(XsDataIdentifier coordinateSystem) const + { + XsMatrix returnVal; + return *XsDataPacket_orientationMatrix(this, &returnVal, coordinateSystem); + } + + /*! \brief returns the orientation as an orientation matrix with the current coordinate system*/ + inline XsMatrix orientationMatrix() const + { + XsMatrix returnVal; + return *XsDataPacket_orientationMatrix(this, &returnVal, coordinateSystemOrientation()); + } + + /*! \copydoc XsDataPacket_setOrientationMatrix(XsDataPacket*, const XsMatrix*, XsDataIdentifier) */ + inline void setOrientationMatrix(const XsMatrix& data, XsDataIdentifier coordinateSystem) + { + XsDataPacket_setOrientationMatrix(this, &data, coordinateSystem); + } + + /*! \copydoc XsDataPacket_containsOrientation(const XsDataPacket*) */ + inline bool containsOrientation(void) const + { + return 0 != XsDataPacket_containsOrientation(this); + } + + /*! \copydoc XsDataPacket_orientationIdentifier(const XsDataPacket*) */ + inline XsDataIdentifier orientationIdentifier() const + { + return XsDataPacket_orientationIdentifier(this); + } + + /*! \copydoc XsDataPacket_coordinateSystemOrientation(const XsDataPacket*) */ + inline XsDataIdentifier coordinateSystemOrientation() const + { + return XsDataPacket_coordinateSystemOrientation(this); + } + + /*! \brief \copybrief XsDataPacket_sdiData(const XsDataPacket*, XsSdiData*) */ + inline XsSdiData sdiData(void) const + { + XsSdiData returnVal; + return *XsDataPacket_sdiData(this, &returnVal); + } + + /*! \copydoc XsDataPacket_containsSdiData(const XsDataPacket*) */ + inline bool containsSdiData(void) const + { + return 0 != XsDataPacket_containsSdiData(this); + } + + /*! \copydoc XsDataPacket_setSdiData(XsDataPacket*, const XsSdiData*) */ + inline void setSdiData(const XsSdiData& data) + { + XsDataPacket_setSdiData(this, &data); + } + + /*! \brief \copybrief XsDataPacket_gloveData(const XsDataPacket*, XsGloveData*, XsHandId) */ + XSNOCOMEXPORT inline XsGloveData gloveData(XsHandId hand) const + { + XsGloveData returnVal; + return *XsDataPacket_gloveData(this, &returnVal, hand); + } + + /*! \copydoc XsDataPacket_containsGloveData(const XsDataPacket*, XsHandId) */ + XSNOCOMEXPORT inline bool containsGloveData(XsHandId hand = XHI_Unknown) const + { + return 0 != XsDataPacket_containsGloveData(this, hand); + } + + /*! \copydoc XsDataPacket_setGloveData(XsDataPacket*, const XsGloveData*, XsHandId) */ + XSNOEXPORT inline void setGloveData(const XsGloveData& data, XsHandId hand) + { + XsDataPacket_setGloveData(this, &data, hand); + } + + /*! \brief \copybrief XsDataPacket_storedDeviceId(const XsDataPacket*, XsDeviceId*) + \returns the device ID stored in this packet + */ + inline XsDeviceId storedDeviceId(void) const + { + XsDeviceId returnVal; + return *XsDataPacket_storedDeviceId(this, &returnVal); + } + + /*! \copydoc XsDataPacket_containsStoredDeviceId(const XsDataPacket*) */ + inline bool containsStoredDeviceId(void) const + { + return 0 != XsDataPacket_containsStoredDeviceId(this); + } + + /*! \copydoc XsDataPacket_setStoredDeviceId(XsDataPacket*, const XsDeviceId*) */ + inline void setStoredDeviceId(const XsDeviceId& data) + { + XsDataPacket_setStoredDeviceId(this, &data); + } + + /*! \brief \copybrief XsDataPacket_storedLocationId(const XsDataPacket*) + \returns the location ID stored in this packet + */ + inline uint16_t storedLocationId(void) const + { + return XsDataPacket_storedLocationId(this); + } + + /*! \copydoc XsDataPacket_containsStoredLocationId(const XsDataPacket*) */ + inline bool containsStoredLocationId(void) const + { + return 0 != XsDataPacket_containsStoredLocationId(this); + } + + /*! \copydoc XsDataPacket_setStoredLocationId(XsDataPacket*, uint16_t) */ + inline void setStoredLocationId(uint16_t data) + { + XsDataPacket_setStoredLocationId(this, data); + } + + + /*! \brief \copybrief XsDataPacket_status(const XsDataPacket*) */ + inline uint32_t status(void) const + { + return XsDataPacket_status(this); + } + + /*! \copydoc XsDataPacket_containsStatus(const XsDataPacket*) */ + inline bool containsStatus(void) const + { + return 0 != XsDataPacket_containsStatus(this); + } + + /*! \copydoc XsDataPacket_containsDetailedStatus(const XsDataPacket*) */ + inline bool containsDetailedStatus(void) const + { + return 0 != XsDataPacket_containsDetailedStatus(this); + } + + /*! \copydoc XsDataPacket_setStatus(XsDataPacket*, uint32_t) */ + inline void setStatus(const uint32_t data) + { + XsDataPacket_setStatus(this, data); + } + + /*! \copydoc XsDataPacket_setStatusByte(XsDataPacket*, uint8_t) */ + inline void setStatusByte(const uint8_t data) + { + XsDataPacket_setStatusByte(this, data); + } + + /*! \brief \copybrief XsDataPacket_packetCounter8(const XsDataPacket*) */ + inline uint8_t packetCounter8(void) const + { + return XsDataPacket_packetCounter8(this); + } + + /*! \copydoc XsDataPacket_containsPacketCounter8(const XsDataPacket*) */ + inline bool containsPacketCounter8(void) const + { + return 0 != XsDataPacket_containsPacketCounter8(this); + } + + /*! \copydoc XsDataPacket_setPacketCounter8(XsDataPacket*, uint8_t) */ + inline void setPacketCounter8(uint8_t counter) + { + XsDataPacket_setPacketCounter8(this, counter); + } + + /*! \brief \copybrief XsDataPacket_packetCounter(const XsDataPacket* thisPtr) */ + inline uint16_t packetCounter(void) const + { + return XsDataPacket_packetCounter(this); + } + + /*! \copydoc XsDataPacket_containsPacketCounter(const XsDataPacket*) */ + inline bool containsPacketCounter(void) const + { + return 0 != XsDataPacket_containsPacketCounter(this); + } + + /*! \copydoc XsDataPacket_setPacketCounter(XsDataPacket*, uint16_t) */ + inline void setPacketCounter(uint16_t counter) + { + XsDataPacket_setPacketCounter(this, counter); + } + + /*! \brief \copybrief XsDataPacket_sampleTimeFine(const XsDataPacket*) */ + inline uint32_t sampleTimeFine(void) const + { + return XsDataPacket_sampleTimeFine(this); + } + + /*! \copydoc XsDataPacket_containsSampleTimeFine(const XsDataPacket*) */ + inline bool containsSampleTimeFine(void) const + { + return 0 != XsDataPacket_containsSampleTimeFine(this); + } + + /*! \copydoc XsDataPacket_setSampleTimeFine(XsDataPacket*, uint32_t) */ + inline void setSampleTimeFine(uint32_t counter) + { + XsDataPacket_setSampleTimeFine(this, counter); + } + + /*! \brief \copybrief XsDataPacket_sampleTimeCoarse(const XsDataPacket*) */ + inline uint32_t sampleTimeCoarse(void) const + { + return XsDataPacket_sampleTimeCoarse(this); + } + + /*! \copydoc XsDataPacket_containsSampleTimeCoarse(const XsDataPacket*) */ + inline bool containsSampleTimeCoarse(void) const + { + return 0 != XsDataPacket_containsSampleTimeCoarse(this); + } + + /*! \copydoc XsDataPacket_setSampleTimeCoarse(XsDataPacket*, uint32_t) */ + inline void setSampleTimeCoarse(uint32_t counter) + { + XsDataPacket_setSampleTimeCoarse(this, counter); + } + + /*! \brief \copybrief XsDataPacket_sampleTime64(const XsDataPacket*) */ + inline uint64_t sampleTime64(void) const + { + return XsDataPacket_sampleTime64(this); + } + + /*! \copydoc XsDataPacket_containsSampleTime64(const XsDataPacket*) */ + inline bool containsSampleTime64(void) const + { + return 0 != XsDataPacket_containsSampleTime64(this); + } + + /*! \copydoc XsDataPacket_setSampleTime64(XsDataPacket*, uint64_t) */ + inline void setSampleTime64(uint64_t counter) + { + XsDataPacket_setSampleTime64(this, counter); + } + + /*! \brief \copybrief XsDataPacket_freeAcceleration(const XsDataPacket*, XsVector*) */ + inline XsVector freeAcceleration(void) const + { + XsVector returnVal; + return *XsDataPacket_freeAcceleration(this, &returnVal); + } + + /*! \copydoc XsDataPacket_containsFreeAcceleration(const XsDataPacket*) */ + inline bool containsFreeAcceleration(void) const + { + return 0 != XsDataPacket_containsFreeAcceleration(this); + } + + /*! \copydoc XsDataPacket_setFreeAcceleration(XsDataPacket*, const XsVector*) */ + inline void setFreeAcceleration(const XsVector& g) + { + XsDataPacket_setFreeAcceleration(this, &g); + } + + /*! \brief \copybrief XsDataPacket_temperature(const XsDataPacket*) */ + inline double temperature(void) const + { + return XsDataPacket_temperature(this); + } + + /*! \copydoc XsDataPacket_containsTemperature(const XsDataPacket*) */ + inline bool containsTemperature(void) const + { + return 0 != XsDataPacket_containsTemperature(this); + } + + /*! \copydoc XsDataPacket_setTemperature(XsDataPacket*, double) */ + inline void setTemperature(double temperature) + { + XsDataPacket_setTemperature(this, temperature); + } + + /*! \brief \copybrief XsDataPacket_pressure(const XsDataPacket*, XsPressure*) */ + inline XsPressure pressure(void) const + { + XsPressure returnVal; + return *XsDataPacket_pressure(this, &returnVal); + } + + /*! \copydoc XsDataPacket_containsPressure(const XsDataPacket*) */ + inline bool containsPressure(void) const + { + return 0 != XsDataPacket_containsPressure(this); + } + + /*! \copydoc XsDataPacket_containsPressureAge(const XsDataPacket*) */ + inline bool containsPressureAge(void) const + { + return 0 != XsDataPacket_containsPressureAge(this); + } + + /*! \copydoc XsDataPacket_setPressure(XsDataPacket*, const XsPressure*) */ + inline void setPressure(const XsPressure& data) + { + XsDataPacket_setPressure(this, &data); + } + + /*! \brief \copybrief XsDataPacket_analogIn1Data(const XsDataPacket*, XsAnalogInData*) */ + inline XsAnalogInData analogIn1Data(void) const + { + XsAnalogInData returnVal; + return *XsDataPacket_analogIn1Data(this, &returnVal); + } + + /*! \copydoc XsDataPacket_containsAnalogIn1Data(const XsDataPacket*) */ + inline bool containsAnalogIn1Data(void) const + { + return 0 != XsDataPacket_containsAnalogIn1Data(this); + } + + /*! \copydoc XsDataPacket_setAnalogIn1Data(XsDataPacket*, const XsAnalogInData*) */ + inline void setAnalogIn1Data(const XsAnalogInData& data) + { + XsDataPacket_setAnalogIn1Data(this, &data); + } + + /*! \brief \copybrief XsDataPacket_analogIn2Data(const XsDataPacket*, XsAnalogInData*) */ + inline XsAnalogInData analogIn2Data(void) const + { + XsAnalogInData returnVal; + return *XsDataPacket_analogIn2Data(this, &returnVal); + } + + /*! \copydoc XsDataPacket_containsAnalogIn2Data(const XsDataPacket*) */ + inline bool containsAnalogIn2Data(void) const + { + return 0 != XsDataPacket_containsAnalogIn2Data(this); + } + + /*! \copydoc XsDataPacket_setAnalogIn2Data(XsDataPacket*, const XsAnalogInData*) */ + inline void setAnalogIn2Data(const XsAnalogInData& data) + { + XsDataPacket_setAnalogIn2Data(this, &data); + } + + /*! \brief \copybrief XsDataPacket_positionLLA(const XsDataPacket*, XsVector*) */ + inline XsVector positionLLA(void) const + { + XsVector returnVal; + return *XsDataPacket_positionLLA(this, &returnVal); + } + + /*! \copydoc XsDataPacket_containsPositionLLA(const XsDataPacket*) */ + inline bool containsPositionLLA(void) const + { + return 0 != XsDataPacket_containsPositionLLA(this); + } + + /*! \copydoc XsDataPacket_setPositionLLA(XsDataPacket*, const XsVector*) */ + inline void setPositionLLA(const XsVector& data) + { + XsDataPacket_setPositionLLA(this, &data); + } + + /*! \brief \copybrief XsDataPacket_latitudeLongitude(const XsDataPacket*, XsVector*) */ + inline XsVector latitudeLongitude(void) const + { + XsVector returnVal; + return *XsDataPacket_latitudeLongitude(this, &returnVal); + } + + /*! \copydoc XsDataPacket_containsLatitudeLongitude(const XsDataPacket*) */ + inline bool containsLatitudeLongitude(void) const + { + return 0 != XsDataPacket_containsLatitudeLongitude(this); + } + + /*! \copydoc XsDataPacket_setLatitudeLongitude(XsDataPacket*, const XsVector*) */ + inline void setLatitudeLongitude(const XsVector& data) + { + XsDataPacket_setLatitudeLongitude(this, &data); + } + + /*! \brief \copybrief XsDataPacket_altitude(const XsDataPacket*) */ + inline double altitude(void) const + { + return XsDataPacket_altitude(this); + } + + /*! \copydoc XsDataPacket_containsAltitude(const XsDataPacket*) */ + inline bool containsAltitude(void) const + { + return 0 != XsDataPacket_containsAltitude(this); + } + + /*! \copydoc XsDataPacket_setAltitude(XsDataPacket*, double) */ + inline void setAltitude(double data) + { + XsDataPacket_setAltitude(this, data); + } + + /*! \brief \copybrief XsDataPacket_altitudeMsl(const XsDataPacket*) */ + inline double altitudeMsl(void) const + { + return XsDataPacket_altitudeMsl(this); + } + + /*! \copydoc XsDataPacket_containsAltitudeMsl(const XsDataPacket*) */ + inline bool containsAltitudeMsl(void) const + { + return 0 != XsDataPacket_containsAltitudeMsl(this); + } + + /*! \copydoc XsDataPacket_setAltitudeMsl(XsDataPacket*, double) */ + inline void setAltitudeMsl(double data) + { + XsDataPacket_setAltitudeMsl(this, data); + } + + /*! \brief \copybrief XsDataPacket_velocity(const XsDataPacket*, XsVector*, XsDataIdentifier) + \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 + */ + inline XsVector velocity(XsDataIdentifier coordinateSystem) const + { + XsVector returnVal; + return *XsDataPacket_velocity(this, &returnVal, coordinateSystem); + } + + /*! \brief Returns the velocity with the current coordinate system*/ + inline XsVector velocity(void) const + { + XsVector returnVal; + return *XsDataPacket_velocity(this, &returnVal, coordinateSystemVelocity()); + } + + /*! \copydoc XsDataPacket_containsVelocity(const XsDataPacket*) */ + inline bool containsVelocity(void) const + { + return 0 != XsDataPacket_containsVelocity(this); + } + + /*! \copydoc XsDataPacket_setVelocity(XsDataPacket*, const XsVector*, XsDataIdentifier) */ + inline void setVelocity(const XsVector& data, XsDataIdentifier coordinateSystem) + { + XsDataPacket_setVelocity(this, &data, coordinateSystem); + } + + /*! \brief \copybrief XsDataPacket_velocityIdentifier(const XsDataPacket*) */ + inline XsDataIdentifier velocityIdentifier() const + { + return XsDataPacket_velocityIdentifier(this); + } + + /*! \copydoc XsDataPacket_coordinateSystemVelocity(const XsDataPacket*) */ + inline XsDataIdentifier coordinateSystemVelocity() const + { + return XsDataPacket_coordinateSystemVelocity(this); + } + + /*! \brief \copybrief XsDataPacket_utcTime(const XsDataPacket*, XsTimeInfo*) + \returns An XsTimeInfo containing the utc time value + */ + inline XsTimeInfo utcTime(void) const + { + XsTimeInfo returnVal; + return *XsDataPacket_utcTime(this, &returnVal); + } + + /*! \copydoc XsDataPacket_containsUtcTime(const XsDataPacket*) */ + inline bool containsUtcTime(void) const + { + return 0 != XsDataPacket_containsUtcTime(this); + } + + /*! \copydoc XsDataPacket_setUtcTime(XsDataPacket*, const XsTimeInfo*) */ + inline void setUtcTime(const XsTimeInfo& data) + { + XsDataPacket_setUtcTime(this, &data); + } + + /*! \brief \copybrief XsDataPacket_frameRange(const XsDataPacket*, XsRange*) */ + inline XsRange frameRange() const + { + XsRange returnVal; + return *XsDataPacket_frameRange(this, &returnVal); + } + + /*! \copydoc XsDataPacket_containsFrameRange(const XsDataPacket*) */ + inline bool containsFrameRange() const + { + return 0 != XsDataPacket_containsFrameRange(this); + } + + /*! \copydoc XsDataPacket_setFrameRange(XsDataPacket*, const XsRange*) */ + inline void setFrameRange(const XsRange& r) + { + XsDataPacket_setFrameRange(this, &r); + } + + /*! \brief \copybrief XsDataPacket_rssi(const XsDataPacket*) */ + inline int rssi() const + { + return XsDataPacket_rssi(this); + } + + /*! \copydoc XsDataPacket_containsRssi(const XsDataPacket*) */ + inline bool containsRssi() const + { + return 0 != XsDataPacket_containsRssi(this); + } + + /*! \copydoc XsDataPacket_setRssi(XsDataPacket* thisPtr, int) */ + inline void setRssi(int r) + { + XsDataPacket_setRssi(this, r); + } + + /*! \brief \copybrief XsDataPacket_rawGnssPvtData(const XsDataPacket*, XsRawGnssPvtData*) + \return a struct with RawGnssPvtData + */ + inline XsRawGnssPvtData rawGnssPvtData(void) const + { + XsRawGnssPvtData returnVal; + return *XsDataPacket_rawGnssPvtData(this, &returnVal); + } + + /*! \brief \copybrief XsDataPacket_containsRawGnssPvtData(const XsDataPacket*) */ + inline bool containsRawGnssPvtData(void) const + { + return 0 != XsDataPacket_containsRawGnssPvtData(this); + } + + /*! \brief \copybrief XsDataPacket_setRawGnssPvtData */ + XSNOEXPORT inline void setRawGnssPvtData(XsRawGnssPvtData const& raw) + { + XsDataPacket_setRawGnssPvtData(this, &raw); + } + + /*! \brief \copybrief XsDataPacket_gnssAge(const XsDataPacket*) */ + inline uint8_t gnssAge(void) const + { + return XsDataPacket_gnssAge(this); + } + + /*! \brief \copybrief XsDataPacket_containsGnssAge(const XsDataPacket*) */ + inline bool containsGnssAge(void) const + { + return 0 != XsDataPacket_containsGnssAge(this); + } + + /*! \brief \copybrief XsDataPacket_setGnssAge(XsDataPacket*, uint8_t) */ + XSNOEXPORT inline void setGnssAge(uint8_t age) + { + XsDataPacket_setGnssAge(this, age); + } + + /*! \brief \copybrief XsDataPacket_rawGnssSatInfo(const XsDataPacket*, XsRawGnssSatInfo*) + \return a struct with RawGnssSatInfo + */ + inline XsRawGnssSatInfo rawGnssSatInfo(void) const + { + XsRawGnssSatInfo returnVal; + return *XsDataPacket_rawGnssSatInfo(this, &returnVal); + } + + /*! \brief \copybrief XsDataPacket_containsRawGnssSatInfo(const XsDataPacket*) */ + inline bool containsRawGnssSatInfo(void) const + { + return 0 != XsDataPacket_containsRawGnssSatInfo(this); + } + + /*! \copydoc XsDataPacket_setRawGnssSatInfo(XsDataPacket*, const XsRawGnssSatInfo*) */ + XSNOEXPORT inline void setRawGnssSatInfo(XsRawGnssSatInfo const& data) + { + XsDataPacket_setRawGnssSatInfo(this, &data); + } + + /*! \brief \copybrief XsDataPacket_gnssPvtPulse(const XsDataPacket*) + \return a struct with GnssPvtPulse + */ + inline uint32_t gnssPvtPulse(void) const + { + return XsDataPacket_gnssPvtPulse(this); + } + + /*! \brief \copybrief XsDataPacket_containsGnssPvtPulse(const XsDataPacket*) */ + inline bool containsGnssPvtPulse(void) const + { + return 0 != XsDataPacket_containsGnssPvtPulse(this); + } + + /*! \copydoc XsDataPacket_setGnssPvtPulse(XsDataPacket*, uint32_t) */ + inline void setGnssPvtPulse(uint32_t counter) + { + XsDataPacket_setGnssPvtPulse(this, counter); + } + + /*! \brief \copybrief XsDataPacket_fullSnapshot(const XsDataPacket*, XsSnapshot*) */ + inline XsSnapshot fullSnapshot(void) const + { + XsSnapshot returnVal; + return *XsDataPacket_fullSnapshot(this, &returnVal); + } + + /*! \brief \copybrief XsDataPacket_containsFullSnapshot(const XsDataPacket*) */ + inline bool containsFullSnapshot(void) const + { + return 0 != XsDataPacket_containsFullSnapshot(this); + } + + /*! \copydoc XsDataPacket_setFullSnapshot(XsDataPacket*, XsSnapshot const *, int) */ + XSNOEXPORT inline void setFullSnapshot(XsSnapshot const& data, bool retransmission) + { + XsDataPacket_setFullSnapshot(this, &data, retransmission ? 1 : 0); + } + /*! \brief \copybrief XsDataPacket_awindaSnapshot */ + XSNOEXPORT inline XsSnapshot awindaSnapshot(void) const + { + XsSnapshot returnVal; + return *XsDataPacket_awindaSnapshot(this, &returnVal); + } + + /*! \brief \copybrief XsDataPacket_containsAwindaSnapshot(const XsDataPacket*) */ + XSNOEXPORT inline bool containsAwindaSnapshot(void) const + { + return 0 != XsDataPacket_containsAwindaSnapshot(this); + } + + /*! \brief \copybrief XsDataPacket_setAwindaSnapshot */ + XSNOEXPORT inline void setAwindaSnapshot(XsSnapshot const& raw, bool retransmission) + { + XsDataPacket_setAwindaSnapshot(this, &raw, retransmission ? 1 : 0); + } + + /*! \brief \copybrief XsDataPacket_containsAwindaSnapshot(const XsDataPacket*) */ + inline bool isAwindaSnapshotARetransmission(void) const + { + return 0 != XsDataPacket_isAwindaSnapshotARetransmission(this); + } + + /*! \brief \copybrief XsDataPacket_gloveSnapshot(const XsDataPacket*, XsGloveSnapshot*, XsHandId) */ + XSNOEXPORT inline XsGloveSnapshot gloveSnapshot(XsHandId hand) const + { + XsGloveSnapshot returnVal; + return *XsDataPacket_gloveSnapshot(this, &returnVal, hand); + } + + /*! \brief \copybrief XsDataPacket_containsGloveSnapshot(const XsDataPacket*, XsHandId) */ + XSNOEXPORT inline bool containsGloveSnapshot(XsHandId hand = XHI_Unknown) const + { + return 0 != XsDataPacket_containsGloveSnapshot(this, hand); + } + + /*! \copydoc XsDataPacket_setGloveSnapshot(XsDataPacket*, XsGloveSnapshot const *, int, XsHandId) */ + XSNOEXPORT inline void setGloveSnapshot(XsGloveSnapshot const& data, bool retransmission, XsHandId hand) + { + XsDataPacket_setGloveSnapshot(this, &data, retransmission ? 1 : 0, hand); + } + + /*! \copydoc XsDataPacket_merge(XsDataPacket*, const XsDataPacket*, int) */ + inline XsDataPacket& merge(const XsDataPacket& other, bool overwrite = true) + { + return *XsDataPacket_merge(this, &other, overwrite ? 1 : 0); + } + + /*! \brief Set the time of arrival of the data packet + \param t The time of arrival + */ + inline void setTimeOfArrival(const XsTimeStamp& t) + { + m_toa = t; + } + + /*! \brief Return the time of arrival of the data packet. Only valid for live streams. The behaviour for file streams is undefined and may change in the future. */ + inline XsTimeStamp timeOfArrival() const + { + return m_toa; + } + + /*! \brief Set the estimated time of sampling of the data packet + \param t The estimated time of sampling + */ + inline void setEstimatedTimeOfSampling(const XsTimeStamp& t) + { + m_etos = t; + } + + /*! \brief Return the estimated time of sampling of the data packet. Only valid for live streams. The behaviour for file streams is undefined and may change in the future. */ + inline XsTimeStamp estimatedTimeOfSampling() const + { + return m_etos; + } + + /*! \brief Set the packet ID of the data packet + \param t The packet ID to set + */ + inline void setPacketId(int64_t t) + { + m_packetId = t; + } + + /*! \brief Return the ID of the packet. + \details This ID is based on, depending on availability: (1) packet counter (2) sample time (3) arrival order + \returns The ID of the packet. + */ + inline int64_t packetId() const + { + return m_packetId; + } + + /*! \copydoc XsDataPacket_setTriggerIndication(XsDataPacket*, XsDataIdentifier, const XsTriggerIndicationData *) */ + void setTriggerIndication(XsDataIdentifier triggerId, const XsTriggerIndicationData& triggerIndicationData) + { + XsDataPacket_setTriggerIndication(this, triggerId, &triggerIndicationData); + } + + /*! \copydoc XsDataPacket_containsTriggerIndication(const XsDataPacket*, XsDataIdentifier) */ + inline bool containsTriggerIndication(XsDataIdentifier triggerId) const + { + return 0 != XsDataPacket_containsTriggerIndication(this, triggerId); + } + + /*! \brief Returns the trigger indication data of a packet + \details + 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) + \returns the trigger indication data of a packet + */ + XsTriggerIndicationData triggerIndication(XsDataIdentifier triggerId) const + { + XsTriggerIndicationData returnVal; + return *XsDataPacket_triggerIndication(this, triggerId, &returnVal); + } + + /*! \brief \copybrief XsDataPacket_rawBlob(const XsDataPacket*, XsByteArray*) */ + inline XsByteArray rawBlob(void) const + { + XsByteArray returnVal; + return *XsDataPacket_rawBlob(this, &returnVal); + } + + /*! \brief \copybrief XsDataPacket_containsRawBlob(const XsDataPacket*) */ + inline bool containsRawBlob(void) const + { + return 0 != XsDataPacket_containsRawBlob(this); + } + + /*! \copydoc XsDataPacket_setRawBlob(XsDataPacket*, const XsByteArray *) */ + inline void setRawBlob(const XsByteArray& data) + { + XsDataPacket_setRawBlob(this, &data); + } + + /*! \brief \copybrief XsDataPacket_accelerationHR(const XsDataPacket*, XsVector*) */ + inline XsVector accelerationHR(void) const + { + XsVector returnVal; + return *XsDataPacket_accelerationHR(this, &returnVal); + } + + /*! \copydoc XsDataPacket_containsAccelerationHR(const XsDataPacket*) */ + inline bool containsAccelerationHR(void) const + { + return 0 != XsDataPacket_containsAccelerationHR(this); + } + + /*! \copydoc XsDataPacket_setAccelerationHR(XsDataPacket*, const XsVector*) */ + inline void setAccelerationHR(const XsVector& vec) + { + XsDataPacket_setAccelerationHR(this, &vec); + } + + /*! \brief \copybrief XsDataPacket_rateOfTurnHR(const XsDataPacket*, XsVector*) */ + inline XsVector rateOfTurnHR(void) const + { + XsVector returnVal; + return *XsDataPacket_rateOfTurnHR(this, &returnVal); + } + + /*! \copydoc XsDataPacket_containsRateOfTurnHR(const XsDataPacket*) */ + inline bool containsRateOfTurnHR(void) const + { + return 0 != XsDataPacket_containsRateOfTurnHR(this); + } + + /*! \copydoc XsDataPacket_setRateOfTurnHR(XsDataPacket*, const XsVector*) */ + inline void setRateOfTurnHR(const XsVector& vec) + { + XsDataPacket_setRateOfTurnHR(this, &vec); + } + +#ifndef XSENS_NO_STL + /*! \brief Return the contents of the XsDataPacket in a simplified form + \details This function is meant for debugging purposes only and should not be used in production code. + \return A STL map with the contents of the packet identified by XsDataIdentifier + */ + inline std::map simplifiedContents() const + { + return *((std::map*)(void*)d); + } +#endif + + //protected: + /*! \privatesection */ +#endif // __cplusplus + struct XSNOEXPORT DataPacketPrivate* d; + + XsDeviceId m_deviceId; //!< The device Id to which the message belongs + XsTimeStamp m_toa; //!< Time Of Arrival (live packets only) + int64_t m_packetId; //!< 64 bit packet id, based on, depending on availability: (1) packet counter (2) sample time (3) arrival order + XsTimeStamp m_etos; //!< Estimated Time of Sampling (live packets only) +}; + +#endif diff --git a/extern/xspublic/xstypes/xsdatapacketptr.h b/extern/xspublic/xstypes/xsdatapacketptr.h new file mode 100644 index 0000000..3a8777d --- /dev/null +++ b/extern/xspublic/xstypes/xsdatapacketptr.h @@ -0,0 +1,42 @@ + +// Copyright (c) 2003-2022 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 XSDATAPACKETPTR_H +#define XSDATAPACKETPTR_H + +struct XsDataPacket; + +/*! \brief A pointer to an XsDataPacket object +*/ +typedef struct XsDataPacket* XsDataPacketPtr; + +#endif diff --git a/extern/xspublic/xstypes/xsdatapacketptrarray.c b/extern/xspublic/xstypes/xsdatapacketptrarray.c new file mode 100644 index 0000000..b713e07 --- /dev/null +++ b/extern/xspublic/xstypes/xsdatapacketptrarray.c @@ -0,0 +1,95 @@ + +// Copyright (c) 2003-2022 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 "xsdatapacketptrarray.h" + +/*! \struct XsDataPacketPtrArray + \brief A list of XsDataPacketPtr values + \sa XsArray +*/ + +/*! \copydoc XsArrayDescriptor::itemSwap + \note Specialization for XsDataPacketPtr +*/ +void swapDataPacketPtr(XsDataPacketPtr* a, XsDataPacketPtr* b) +{ + XsDataPacketPtr tmp = *a; + *a = *b; + *b = tmp; +} + +/*! \copydoc XsArrayDescriptor::itemCopy + \note Specialization for XsDataPacketPtr +*/ +void copyDataPacketPtr(XsDataPacketPtr* to, XsDataPacketPtr const* from) +{ + *to = *from; +} + +/*! \copydoc XsArrayDescriptor::itemCompare + \note Specialization for XsDataPacketPtr +*/ +int compareDataPacketPtr(XsDataPacketPtr const* a, XsDataPacketPtr const* b) +{ + if (*a < *b) + return -1; + if (*a > *b) + return 1; + return 0; +} + +//! \brief zero the pointer value +void zeroDataPacketPtr(XsDataPacketPtr* a) +{ + *a = 0; +} + +//! \brief Descriptor for XsDataPacketPtrArray +XsArrayDescriptor const g_xsDataPacketPtrArrayDescriptor = +{ + sizeof(XsDataPacketPtr), + XSEXPCASTITEMSWAP swapDataPacketPtr, // swap + XSEXPCASTITEMMAKE zeroDataPacketPtr, // construct + (XsArrayItemCopyFunc) copyDataPacketPtr, // copy construct + XSEXPCASTITEMMAKE zeroDataPacketPtr, // destruct + (XsArrayItemCopyFunc) copyDataPacketPtr, // copy + (XsArrayItemCompareFunc) compareDataPacketPtr, // compare + XSEXPCASTRAWCOPY XsArray_rawCopy +}; + +/*! \copydoc XsArray_constructDerived + \note Specialization for XsDataPacketPtrArray +*/ +void XsDataPacketPtrArray_construct(XsDataPacketPtrArray* thisPtr, XsSize count, XsDataPacketPtr const* src) +{ + XsArray_construct(thisPtr, &g_xsDataPacketPtrArrayDescriptor, count, src); +} diff --git a/extern/xspublic/xstypes/xsdatapacketptrarray.h b/extern/xspublic/xstypes/xsdatapacketptrarray.h new file mode 100644 index 0000000..c48a8e4 --- /dev/null +++ b/extern/xspublic/xstypes/xsdatapacketptrarray.h @@ -0,0 +1,97 @@ + +// Copyright (c) 2003-2022 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 XSDATAPACKETPTRARRAY_H +#define XSDATAPACKETPTRARRAY_H + +#include "xstypesconfig.h" +#include "xsdatapacket.h" +#include "xsarray.h" +#include "xsdatapacketptr.h" + +#ifdef __cplusplus +extern "C" { +#endif + +extern XsArrayDescriptor const XSTYPES_DLL_API g_xsDataPacketPtrArrayDescriptor; + +#ifndef __cplusplus +#define XSDATAPACKETPTRARRAY_INITIALIZER XSARRAY_INITIALIZER(&g_xsDataPacketPtrArrayDescriptor) + +XSARRAY_STRUCT(XsDataPacketPtrArray, XsDataPacketPtr); +typedef struct XsDataPacketPtrArray XsDataPacketPtrArray; + +XSTYPES_DLL_API void XsDataPacketPtrArray_construct(XsDataPacketPtrArray* thisPtr, XsSize count, XsDataPacketPtr const* src); +#else +} // extern "C" +#endif + +#ifdef __cplusplus + 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 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); + } +#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) + { + } +#endif + }; +#endif +#endif diff --git a/extern/xspublic/xstypes/xsdebugcounters.cpp b/extern/xspublic/xstypes/xsdebugcounters.cpp new file mode 100644 index 0000000..e777c4e --- /dev/null +++ b/extern/xspublic/xstypes/xsdebugcounters.cpp @@ -0,0 +1,125 @@ + +// Copyright (c) 2003-2022 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 "xsdebugcounters.h" +#include + +#ifdef XSENS_USE_DEBUG_COUNTERS +volatile std::atomic_int XsVector_allocCount_value(0); +volatile std::atomic_int XsVector_freeCount_value(0); + +volatile std::atomic_int XsMatrix_allocCount_value(0); +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" + +#endif diff --git a/extern/xspublic/xstypes/xsdebugcounters.h b/extern/xspublic/xstypes/xsdebugcounters.h new file mode 100644 index 0000000..42f0998 --- /dev/null +++ b/extern/xspublic/xstypes/xsdebugcounters.h @@ -0,0 +1,133 @@ + +// Copyright (c) 2003-2022 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 XSDEBUGCOUNTERS_H +#define XSDEBUGCOUNTERS_H + +#include "xstypesconfig.h" + +#ifdef XSENS_USE_DEBUG_COUNTERS +#ifdef __cplusplus +extern "C" { +#endif + +extern XSTYPES_DLL_API int XsVector_resetDebugCounts(void); +extern XSTYPES_DLL_API int XsVector_allocCount(void); +extern XSTYPES_DLL_API int XsVector_freeCount(void); +extern int XsVector_incAllocCount(void); +extern int XsVector_incFreeCount(void); + +extern XSTYPES_DLL_API int XsMatrix_resetDebugCounts(void); +extern XSTYPES_DLL_API int XsMatrix_allocCount(void); +extern XSTYPES_DLL_API int XsMatrix_freeCount(void); +extern int XsMatrix_incAllocCount(void); +extern int XsMatrix_incFreeCount(void); + +extern XSTYPES_DLL_API int XsArray_resetDebugCounts(void); +extern XSTYPES_DLL_API int XsArray_allocCount(void); +extern XSTYPES_DLL_API int XsArray_freeCount(void); +extern int XsArray_incAllocCount(void); +extern int XsArray_incFreeCount(void); + +#ifdef __cplusplus +} // extern "C" +#endif + +#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; +} + +#endif + + +#endif diff --git a/extern/xspublic/xstypes/xsdevicecapabilities.c b/extern/xspublic/xstypes/xsdevicecapabilities.c new file mode 100644 index 0000000..2138a9c --- /dev/null +++ b/extern/xspublic/xstypes/xsdevicecapabilities.c @@ -0,0 +1,131 @@ + +// Copyright (c) 2003-2022 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 "xsdevicecapabilities.h" + +/*! \class XsDeviceCapabilities + \brief Describes what a device is capable of. +*/ + +/*! \addtogroup cinterface C Interface + @{ +*/ + +/*! \brief Test if the given \a XsDeviceCapabilities is valid. + \return True if valid. +*/ +int XsDeviceCapabilities_isValid(const struct XsDeviceCapabilities* thisPtr) +{ + return (thisPtr->m_flags != XDC_Invalid); +} + +/*! \brief Test if the device has an operational accelerometer. + \return True if the Accelerometer flag is set +*/ +int XsDeviceCapabilities_hasAccelerometer(const struct XsDeviceCapabilities* thisPtr) +{ + return (thisPtr->m_flags & XDC_Acc) > 0; +} + +/*! \brief Test if the device has an operational gyroscope. + \return True if the Gyroscope flag is set +*/ +int XsDeviceCapabilities_hasGyroscope(const struct XsDeviceCapabilities* thisPtr) +{ + return (thisPtr->m_flags & XDC_Gyr) > 0; +} + +/*! \brief Test if the device has an operational magnetometer. + \return True if the Magnetometer flag is set +*/ +int XsDeviceCapabilities_hasMagnetometer(const struct XsDeviceCapabilities* thisPtr) +{ + return (thisPtr->m_flags & XDC_Mag) > 0; +} + +/*! \brief Test if the device has an operational barometer. + \return True if the Barometer flag is set +*/ +int XsDeviceCapabilities_hasBarometer(const struct XsDeviceCapabilities* thisPtr) +{ + return (thisPtr->m_flags & XDC_Baro) > 0; +} + +/*! \brief Test if the device has an operational GNSS receiver. + \return True if the GNSS flag is set +*/ +int XsDeviceCapabilities_hasGnss(const struct XsDeviceCapabilities* thisPtr) +{ + return (thisPtr->m_flags & XDC_Gnss) > 0; +} + +/*! \brief Test if the device is an IMU. + \return True if the IMU flag is set +*/ +int XsDeviceCapabilities_isImu(const struct XsDeviceCapabilities* thisPtr) +{ + return (thisPtr->m_flags & XDC_Imu) > 0; +} + +/*! \brief Test if the device has a VRU. + \return True if the VRU flag is set +*/ +int XsDeviceCapabilities_isVru(const struct XsDeviceCapabilities* thisPtr) +{ + return (thisPtr->m_flags & XDC_Vru) > 0; +} + +/*! \brief Test if the device has an AHRS. + \return True if the AHRS flag is set +*/ +int XsDeviceCapabilities_isAhrs(const struct XsDeviceCapabilities* thisPtr) +{ + return (thisPtr->m_flags & XDC_Ahrs) > 0; +} + +/*! \brief Test if the device is a GNSS/INS. + \return True if the GNSS/INS flag is set +*/ +int XsDeviceCapabilities_isGnssIns(const struct XsDeviceCapabilities* thisPtr) +{ + return (thisPtr->m_flags & XDC_GnssIns) > 0; +} + +/*! \brief Test if the device is an RTK device. + \return True if the RTK flag is set +*/ +int XsDeviceCapabilities_isRtk(const struct XsDeviceCapabilities* thisPtr) +{ + return (thisPtr->m_flags & XDC_Rtk) > 0; +} + +/*! @} */ diff --git a/extern/xspublic/xstypes/xsdevicecapabilities.h b/extern/xspublic/xstypes/xsdevicecapabilities.h new file mode 100644 index 0000000..6433ae5 --- /dev/null +++ b/extern/xspublic/xstypes/xsdevicecapabilities.h @@ -0,0 +1,171 @@ + +// Copyright (c) 2003-2022 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 XSDEVICECAPABILITIES_H +#define XSDEVICECAPABILITIES_H + +#include "xstypesconfig.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/*! \addtogroup enums Global enumerations + @{ +*/ +/*! \brief Device capability flags. */ +enum XsDeviceCapability +{ + XDC_Invalid = 0x00000000, //!< Indicates the \a XsDeviceCapabilities structure is invalid. + XDC_Acc = 0x00000001, //!< Device has an operational accelerometer. + XDC_Gyr = 0x00000002, //!< Device has an operational gyroscope. + XDC_Mag = 0x00000004, //!< Device has an operational magnetometer. + XDC_Baro = 0x00000008, //!< Device has an operational barometer + XDC_Gnss = 0x00000010, //!< Device has an operational GNSS receiver. + XDC_Imu = 0x00000020, //!< Device is an IMU. + XDC_Vru = 0x00000040, //!< Device is a VRU. + XDC_Ahrs = 0x00000080, //!< Device is an AHRS. + XDC_GnssIns = 0x00000100, //!< Device is a GNSS/INS. + XDC_Rtk = 0x00000200, //!< Device has RTK features +}; +/*! @} */ + +typedef enum XsDeviceCapability XsDeviceCapability; + +struct XsDeviceCapabilities; + +XSTYPES_DLL_API int XsDeviceCapabilities_isValid(const struct XsDeviceCapabilities* thisPtr); +XSTYPES_DLL_API int XsDeviceCapabilities_hasAccelerometer(const struct XsDeviceCapabilities* thisPtr); +XSTYPES_DLL_API int XsDeviceCapabilities_hasGyroscope(const struct XsDeviceCapabilities* thisPtr); +XSTYPES_DLL_API int XsDeviceCapabilities_hasMagnetometer(const struct XsDeviceCapabilities* thisPtr); +XSTYPES_DLL_API int XsDeviceCapabilities_hasBarometer(const struct XsDeviceCapabilities* thisPtr); +XSTYPES_DLL_API int XsDeviceCapabilities_hasGnss(const struct XsDeviceCapabilities* thisPtr); +XSTYPES_DLL_API int XsDeviceCapabilities_isImu(const struct XsDeviceCapabilities* thisPtr); +XSTYPES_DLL_API int XsDeviceCapabilities_isVru(const struct XsDeviceCapabilities* thisPtr); +XSTYPES_DLL_API int XsDeviceCapabilities_isAhrs(const struct XsDeviceCapabilities* thisPtr); +XSTYPES_DLL_API int XsDeviceCapabilities_isGnssIns(const struct XsDeviceCapabilities* thisPtr); +XSTYPES_DLL_API int XsDeviceCapabilities_isRtk(const struct XsDeviceCapabilities* thisPtr); + +#ifdef __cplusplus +} +#endif + +struct XsDeviceCapabilities +{ +#ifdef __cplusplus +public: + //! \brief Constructor to create an XsDeviceCapabilities instance from a raw flags field. + inline explicit XsDeviceCapabilities(uint32_t flags = XDC_Invalid) + : m_flags(flags) + { + } + + //! \brief Copy constructor. + inline XsDeviceCapabilities(const XsDeviceCapabilities& other) + : m_flags(other.m_flags) + { + } + + //! \brief Copy assignment. + inline const XsDeviceCapabilities& operator=(const XsDeviceCapabilities& other) + { + m_flags = other.m_flags; + return *this; + } + + //! \copydoc XsDeviceCapabilities_hasAccelerometer(const struct XsDeviceCapabilities*) + inline bool hasAccelerometer() const + { + return XsDeviceCapabilities_hasAccelerometer(this) != 0; + } + + //! \copydoc XsDeviceCapabilities_hasGyroscope(const struct XsDeviceCapabilities*) + inline bool hasGyroscope() const + { + return XsDeviceCapabilities_hasGyroscope(this) != 0; + } + + //! \copydoc XsDeviceCapabilities_hasMagnetometer(const struct XsDeviceCapabilities*) + inline bool hasMagnetometer() const + { + return XsDeviceCapabilities_hasMagnetometer(this) != 0; + } + + //! \copydoc XsDeviceCapabilities_hasBarometer(const struct XsDeviceCapabilities*) + inline bool hasBarometer() const + { + return XsDeviceCapabilities_hasBarometer(this) != 0; + } + + //! \copydoc XsDeviceCapabilities_hasGnss(const struct XsDeviceCapabilities*) + inline bool hasGnss() const + { + return XsDeviceCapabilities_hasGnss(this) != 0; + } + + //! \copydoc XsDeviceCapabilities_isImu(const struct XsDeviceCapabilities*) + inline bool isImu() const + { + return XsDeviceCapabilities_isImu(this) != 0; + } + + //! \copydoc XsDeviceCapabilities_isVru(const struct XsDeviceCapabilities*) + inline bool isVru() const + { + return XsDeviceCapabilities_isVru(this) != 0; + } + + //! \copydoc XsDeviceCapabilities_isAhrs(const struct XsDeviceCapabilities*) + inline bool isAhrs() const + { + return XsDeviceCapabilities_isAhrs(this) != 0; + } + + //! \copydoc XsDeviceCapabilities_isGnssIns(const struct XsDeviceCapabilities*) + inline bool isGnssIns() const + { + return XsDeviceCapabilities_isGnssIns(this) != 0; + } + //! \copydoc XsDeviceCapabilities_isRtk(const struct XsDeviceCapabilities*) + inline bool isRtk() const + { + return XsDeviceCapabilities_isRtk(this) != 0; + } +private: +#endif + + uint32_t m_flags; +}; + +typedef struct XsDeviceCapabilities XsDeviceCapabilities; + +#endif diff --git a/extern/xspublic/xstypes/xsdeviceid.c b/extern/xspublic/xstypes/xsdeviceid.c new file mode 100644 index 0000000..5d9d63b --- /dev/null +++ b/extern/xspublic/xstypes/xsdeviceid.c @@ -0,0 +1,1307 @@ + +// Copyright (c) 2003-2022 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 "xsdeviceid.h" +#include "xsstring.h" +#include "xsdid.h" +#include +#include +#include + +/*! \class XsDeviceId + \brief Contains an Xsens device ID and provides operations for determining the type of device +*/ + +/*! \addtogroup cinterface C Interface + @{ +*/ + +/*! \returns The legacy bit used to identify legacy or new XsDeviceId format */ +uint64_t XsDeviceId_legacyBit(const struct XsDeviceId* thisPtr) +{ + (void)thisPtr; + return XS_DID64_BIT; +} + +/*! \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) +{ + return ((thisPtr->m_deviceId & XS_DID64_BIT) == 0); +} + +/*! \brief Test if the device ID represents an Mti 1-series device + \returns true if this XsDeviceId represents an Mti-1 series device +*/ +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) + return 0; + int deviceFamily = atoi(&thisPtr->m_productCode[4]); + return ((deviceFamily != 0) && (deviceFamily < 10)); + } +} + +/*! \brief Test if the device ID represents an Mti 10-series device + \returns true if this XsDeviceId represents an Mti-10 series device +*/ +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) + return 0; + int deviceFamily = atoi(&thisPtr->m_productCode[4]); + return ((deviceFamily != 0) && ((deviceFamily >= 10) && (deviceFamily < 100))); + } +} + +/*! \brief Test if the device ID represents an Mti 100-series device + \returns true if this XsDeviceId represents an Mti-100 series device +*/ +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]); + if ((deviceFamily != 0) && (deviceFamily >= 100)) + return 1; + } + } + return 0; +} + +/*! \brief Test if the device ID represents an Mtig 700 device + \returns true if this XsDeviceId represents an Mtig 700 series device +*/ +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); + } + else + { + if (memcmp(thisPtr->m_productCode, "MTi-G-", 6) != 0) + return 0; + + int deviceFamily = atoi(&thisPtr->m_productCode[6]); + return ((deviceFamily != 0) && (deviceFamily == 700)); + } +} + +/*! \brief Test if the device ID represents an Mtig 710 device + \returns true if this XsDeviceId represents an Mtig 710 series device +*/ +int XsDeviceId_isMtigX10(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); + } + else + { + if (memcmp(thisPtr->m_productCode, "MTi-G-", 6) != 0) + return 0; + + int deviceFamily = atoi(&thisPtr->m_productCode[6]); + return ((deviceFamily != 0) && (deviceFamily == 710)); + } +} + +/*! \brief Test if the device ID represents an MTi-600 series device + \returns true if this XsDeviceId represents an Mti-600 series device +*/ +int XsDeviceId_isMti6X0(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]); + return ((deviceFamily != 0) && ((deviceFamily > 600) && (deviceFamily < 700))); + } +} + +/*! \brief Test if the device ID represents an MTi-800 series device + \returns true if this XsDeviceId represents an Mti-800 series device +*/ +int XsDeviceId_isMti8X0(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]); + return ((deviceFamily != 0) && ((deviceFamily > 800) && (deviceFamily < 900))); + } +} + +/*! \brief Test if the device ID represents an MTi-3X0 device + \returns true if this XsDeviceId represents an MTi-3X0 series device +*/ +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; +} + +/*! \brief Test if the device ID represents a Glove series device + \returns true if this XsDeviceId represents a Glove series device +*/ +int XsDeviceId_isGlove(const struct XsDeviceId* thisPtr) +{ + if (XsDeviceId_isLegacyDeviceId(thisPtr)) + { + if (thisPtr->m_deviceId == XS_DID_GLOVEMASTER_LEFT || thisPtr->m_deviceId == XS_DID_GLOVEMASTER_RIGHT) + return 1; + + return 0; + } + else + { + if (memcmp(thisPtr->m_productCode, "Glove", 5) != 0) + return 0; + + return 1; + } +} + +/*! \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) + return 0; + + return 1; + } +} + +/*! \brief Test if this device ID represents an Rugged Version + \returns true if this XsDeviceId represents an Rugged Sensor +*/ +int XsDeviceId_isRugged(struct XsDeviceId const* thisPtr) +{ + if (XsDeviceId_isMti6X0(thisPtr) || XsDeviceId_isMti8X0(thisPtr)) + { + if (thisPtr->m_productCode[7] == 'R' || thisPtr->m_productCode[7] == 'G') + return 1; + } + return 0; + +} + +/*! \brief Test if this device ID represents a device with internal GNSS receiver + \returns true if this XsDeviceId represents a Sensor with internal GNSS +*/ +int XsDeviceId_hasInternalGnss(struct XsDeviceId const* thisPtr) +{ + if (XsDeviceId_isMti6X0(thisPtr) || XsDeviceId_isMti8X0(thisPtr)) + { + if (thisPtr->m_productCode[7] == 'G') + return 1; + } + if (XsDeviceId_isMtigX10(thisPtr)) + return 1; + return 0; + +} + +/*! \brief Test if this device ID represents an MTw + \returns true if this XsDeviceId represents an MTw +*/ +int XsDeviceId_isMtw(const struct XsDeviceId* thisPtr) +{ + return XsDeviceId_isMtw2(thisPtr); +} + +/*! \brief Test if this device ID represents an MTw2. + \returns true if this XsDeviceId represents an MTw2 +*/ +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 + \returns true if this XsDeviceId represents an MTx +*/ +int XsDeviceId_isMtx(const struct XsDeviceId* thisPtr) +{ + return XsDeviceId_isMtx2(thisPtr); +} + +/*! \brief Test if this device ID represents an MTx2 + \returns true if this XsDeviceId represents an MTx2 +*/ +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. + \returns true if this XsDeviceId represents a bodypack (any version) device +*/ +int XsDeviceId_isBodyPack(const struct XsDeviceId* thisPtr) +{ + if (XsDeviceId_isLegacyDeviceId(thisPtr)) + return XS_DID_BODYPACK(thisPtr->m_deviceId) || (thisPtr->m_deviceId == XS_DID_ABMCLOCKMASTER); + else + return ((memcmp(thisPtr->m_productCode, "BodyPack", 8) == 0) || (memcmp(thisPtr->m_productCode, "BPACK", 5) == 0)); +} + +/*! \brief Test if this device ID represents a bodypack V1 device. + \returns true if this XsDeviceId represents a bodypack V1 device +*/ +int XsDeviceId_isBodyPackV1(const struct XsDeviceId* thisPtr) +{ + if (XsDeviceId_isLegacyDeviceId(thisPtr)) + return XS_DID_BODYPACK(thisPtr->m_deviceId) || (thisPtr->m_deviceId == XS_DID_ABMCLOCKMASTER); + else + return 0; // all BPv1 devices use legacy device IDs +} + +/*! \brief Test if this device ID represents a bodypack V2 device. + \returns true if this XsDeviceId represents a bodypack V2 device +*/ +int XsDeviceId_isBodyPackV2(const struct XsDeviceId* thisPtr) +{ + if (!XsDeviceId_isLegacyDeviceId(thisPtr)) + return memcmp(thisPtr->m_productCode, "BPACK-V2", 8) == 0; + else + return 0; // all BPv2 devices use big device IDs +} + +/*! \brief Test if this device ID represents a Wireless Master device (Awinda Station, Awinda Dongle, Awinda OEM) + \returns true if this XsDeviceId represents a Wireless Master device +*/ +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); + } + else + return (memcmp(thisPtr->m_productCode, "AW-", 3) == 0); +} + +/*! \brief Test if this device ID represents an Awinda device. + \returns true if this XsDeviceId represents an Awinda device +*/ +int XsDeviceId_isAwindaX(const struct XsDeviceId* thisPtr) +{ + return XsDeviceId_isAwinda2(thisPtr); +} + +/*! \brief Test if this device ID represents an Awinda Station. + \returns true if this XsDeviceId represents an Awinds Station +*/ +int XsDeviceId_isAwindaXStation(const struct XsDeviceId* thisPtr) +{ + return XsDeviceId_isAwinda2Station(thisPtr); +} + +/*! \brief Test if this device ID represents an Awinda Dongle. + \returns true if this XsDeviceId represents an Awinda Dongle +*/ +int XsDeviceId_isAwindaXDongle(const struct XsDeviceId* thisPtr) +{ + return XsDeviceId_isAwinda2Dongle(thisPtr); +} + +/*! \brief Test if this device ID represents an Awinda OEM board. + \returns true if this XsDeviceId represents an Awinda OEM board +*/ +int XsDeviceId_isAwindaXOem(const struct XsDeviceId* thisPtr) +{ + return XsDeviceId_isAwinda2Oem(thisPtr); +} + +/*! \brief Test if this device ID represents an Awinda2 device. + \returns true if this XsDeviceId represents an Awinda2 device +*/ +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. + \returns true if this XsDeviceId represents an Awinda2 Station +*/ +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. + \returns true if this XsDeviceId represents an Awinda2 Dongle +*/ +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. + \returns true if this XsDeviceId represents an Awinda2 OEM board +*/ +int XsDeviceId_isAwinda2Oem(const struct XsDeviceId* thisPtr) +{ + return XS_DID_AWINDA2_OEM(thisPtr->m_deviceId); +} + +/*! \brief Test if this device ID represents a SyncStation. + \returns true if this XsDeviceId represents a SyncStation +*/ +int XsDeviceId_isSyncStationX(const struct XsDeviceId* thisPtr) +{ + return XS_DID_SYNCSTATION(thisPtr->m_deviceId); +} + +/*! \brief Test if this device ID represents a SyncStation v2. + \returns true if this XsDeviceId represents a SyncStation v2 +*/ +int XsDeviceId_isSyncStation2(const struct XsDeviceId* thisPtr) +{ + return XS_DID_SYNCSTATION2(thisPtr->m_deviceId); +} + +/*! \brief Test if this device ID represents a Hardware In the Loop test device. + \returns true if this XsDeviceId represents a Hardware In the Loop test device +*/ +int XsDeviceId_isHilDevice(const struct XsDeviceId* thisPtr) +{ + if (XsDeviceId_isLegacyDeviceId(thisPtr)) + return 0; + else + return strcmp(thisPtr->m_productCode, "HILDEVICE") == 0; +} + +/*! \brief Test if this device ID represents an IMU. + \returns true if this XsDeviceId represents an IMU +*/ +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)); + } + else + { + if (memcmp(thisPtr->m_productCode, "MTi-", 4) != 0) + return 0; + + int deviceFamily = thisPtr->m_productCode[4] - '0'; + if (deviceFamily == 0) + return 0; + + if (deviceFamily == 6 || deviceFamily == 8) + deviceFamily = thisPtr->m_productCode[5] - '0'; + + return (deviceFamily == 1); + } +} + +/*! \brief Test if this device ID represents a VRU. + \returns true if this XsDeviceId represents a VRU +*/ +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)); + } + else + { + if (memcmp(thisPtr->m_productCode, "MTi-", 4) != 0) + return 0; + + int deviceFamily = thisPtr->m_productCode[4] - '0'; + if (deviceFamily == 0) + return 0; + + if (deviceFamily == 6 || deviceFamily == 8) + deviceFamily = thisPtr->m_productCode[5] - '0'; + + return (deviceFamily == 2); + } +} + +/*! \brief Test if this device ID represents an AHRS. + \returns true if this XsDeviceId represents an AHRS +*/ +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)); + } + else + { + if (memcmp(thisPtr->m_productCode, "MTi-", 4) != 0) + return 0; + + int deviceFamily = thisPtr->m_productCode[4] - '0'; + if (deviceFamily == 0) + return 0; + + if (deviceFamily == 6 || deviceFamily == 8) + deviceFamily = thisPtr->m_productCode[5] - '0'; + + return (deviceFamily == 3); + } +} + +/*! \brief Test if this device ID represents an GNSS (capable) device. + \returns true if this XsDeviceId represents a GNSS capable device +*/ +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_8_MPU); + } + else + { + if (memcmp(thisPtr->m_productCode, "MTi-", 4) != 0) + return 0; + + 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) + return 0; + + deviceFamily = atoi(&thisPtr->m_productCode[6]); + return (deviceFamily == 700 || deviceFamily == 710); + } + } +} + +/*! \brief Test if this device ID represents an RTK (capable) device. + \returns true if this XsDeviceId represents a RTK capable device +*/ +int XsDeviceId_isRtk(const struct XsDeviceId* thisPtr) +{ + if (XsDeviceId_isLegacyDeviceId(thisPtr)) + return ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_8_MPU); + else + { + if (memcmp(thisPtr->m_productCode, "MTi-", 4) != 0) + return 0; + + int deviceFamily = atoi(&thisPtr->m_productCode[4]); + return ((deviceFamily == 680) || (deviceFamily == 880)); + } +} + +/*! \brief Test if this device ID represents any of the container devices such as Bodypack and Awinda Station + \returns true if this XsDeviceId represents a container device +*/ +int XsDeviceId_isContainerDevice(const struct XsDeviceId* thisPtr) +{ + return XsDeviceId_isBodyPack(thisPtr) || XsDeviceId_isWirelessMaster(thisPtr); +} + +/*! \brief Test if this device ID represents an MT device (any Mti, Mtig, Mtx or Mtw) + \returns true if this XsDeviceId represents an MT device +*/ +int XsDeviceId_isMt(const struct XsDeviceId* thisPtr) +{ + return (XsDeviceId_isMti(thisPtr) || XsDeviceId_isMtig(thisPtr) || XsDeviceId_isMtw(thisPtr) || XsDeviceId_isMtx(thisPtr)); +} + +/*! \brief Test if this device ID represents an MTi device (1, 10 or 100 series, 1 includes MTi-7) + \returns true if this XsDeviceId represents an MTi device +*/ +int XsDeviceId_isMti(const struct XsDeviceId* thisPtr) +{ + return (XsDeviceId_isMtiX(thisPtr) || XsDeviceId_isMtiX0(thisPtr) || XsDeviceId_isMtiX00(thisPtr) || XsDeviceId_isMti3X0(thisPtr) || XsDeviceId_isMti6X0(thisPtr) || XsDeviceId_isMti8X0(thisPtr)); +} + +/*! \brief Test if this device ID represents an MTig device (700 or 710 series) + \returns true if this XsDeviceId represents an MTig device +*/ +int XsDeviceId_isMtig(const struct XsDeviceId* thisPtr) +{ + return (XsDeviceId_isMtigX00(thisPtr) || XsDeviceId_isMtigX10(thisPtr)); +} + +/*! \brief Test if this device ID represents an Mk4 generation MT device + \returns true if this XsDeviceId represents an Mk4 generation device +*/ +int XsDeviceId_isMtMark4(const struct XsDeviceId* thisPtr) +{ + if (XsDeviceId_isLegacyDeviceId(thisPtr)) + return (XsDeviceId_isMtiX0(thisPtr) || XsDeviceId_isMtiX00(thisPtr)) && ((thisPtr->m_deviceId & XS_DID_TYPEL_MK5) != XS_DID_TYPEL_MK5); + else + return (XsDeviceId_isMtiX0(thisPtr) || XsDeviceId_isMtiX00(thisPtr)) && thisPtr->m_hardwareVersion < 0x300; +} + +/*! \brief Test if this device ID represents an Mk5 generation MT device + \returns true if this XsDeviceId represents an Mk5 generation device +*/ +int XsDeviceId_isMtMark5(const struct XsDeviceId* thisPtr) +{ + if (XsDeviceId_isLegacyDeviceId(thisPtr)) + return (XsDeviceId_isMtiX0(thisPtr) || XsDeviceId_isMtiX00(thisPtr)) && ((thisPtr->m_deviceId & XS_DID_TYPEL_MK5) == XS_DID_TYPEL_MK5); + else + return (XsDeviceId_isMtiX0(thisPtr) || XsDeviceId_isMtiX00(thisPtr)) && thisPtr->m_hardwareVersion >= 0x300; +} + +//------------------------------------------------------------------------------------------------------------ +//------------------------------------------------------------------------------------------------------------ +//------------------------------------------------------------------------------------------------------------ +//------------------------------------------------------------------------------------------------------------ + +/*! \brief Get a string with a readable representation of this device ID. + \param str The string to write to +*/ +void XsDeviceId_toString(const XsDeviceId* thisPtr, XsString* str) +{ + if (XsDeviceId_isLegacyDeviceId(thisPtr)) + { + char device[9]; + sprintf(device, "%08" PRINTF_INT32_MODIFIER "X", (uint32_t)thisPtr->m_deviceId); + XsString_assign(str, 8, device); + } + else + { + 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); + } +} + +/*! \brief Read a device ID from the supplied string. + \param str The string to interpret +*/ +void XsDeviceId_fromString(XsDeviceId* thisPtr, const XsString* str) +{ + uint64_t tmp = 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/%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 + \param makeType Boolean whether the deviceid is changed to a type instead of the full deviceid + \param str The string to write to +*/ +void XsDeviceId_toDeviceTypeString(const XsDeviceId* thisPtr, XsString* str, int makeType) +{ + XsDeviceId deviceType = XSDEVICEID_INITIALIZER; + if (makeType) + XsDeviceId_deviceType(thisPtr, 1, &deviceType); + else + deviceType = *thisPtr; + 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); + 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)); + 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) +{ + uint32_t id = 0; + int hwRevH = 0, hwRevL = 0; + uint32_t variant; + char productCode[24]; + if (!thisPtr || !str || !str->m_data) + return; + int result = sscanf(str->m_data, "%24[^_]_%08X.%08X.%d_%d", productCode, &id, &variant, &hwRevH, &hwRevL); + if (result == 5 || result == 3) + { + thisPtr->m_deviceId = id; + thisPtr->m_hardwareVersion = (uint16_t)((uint16_t)hwRevH << 8) + (uint16_t)hwRevL; + thisPtr->m_productVariant = variant; + strcpy(thisPtr->m_productCode, productCode); + } +} + +/*! \brief Test if the device ID is a valid id (not 0). + \returns true if the device ID is vallid +*/ +int XsDeviceId_isValid(const struct XsDeviceId* thisPtr) +{ + if (XsDeviceId_isLegacyDeviceId(thisPtr)) + return thisPtr->m_deviceId != 0; + else + return thisPtr->m_productCode[0] != 0 && thisPtr->m_deviceId != 0; +} + +/*! \brief Swap the contents of \a a with those of \a b + \param a Pointer to first deviceId + \param b Pointer to second deviceId +*/ +void XsDeviceId_swap(XsDeviceId* a, XsDeviceId* b) +{ + XsDeviceId tmp = *a; + *a = *b; + *b = tmp; +} + +/*! \brief Returns true if this is equal to \a other or this is a type-specifier that matches \a other + \param other The deviceid to compare this deviceid to + \return True if the deviceids are equal or the type-specifier matches +*/ +int XsDeviceId_contains(XsDeviceId const* thisPtr, XsDeviceId const* other) +{ + if (thisPtr == other) + return 1; + if (thisPtr->m_deviceId == other->m_deviceId) + return 1; + if (thisPtr->m_deviceId & XS_DID_ID_MASK) // NOTE: This produces incorrect results for device ids ending with 0000. See MVN-3876 + return 0; + XsDeviceId typeThis, typeOther; + XsDeviceId_deviceType(thisPtr, 1, &typeThis); + XsDeviceId_deviceType(other, 1, &typeOther); + if (XsDeviceId_isLegacyDeviceId(thisPtr)) + return typeThis.m_deviceId == typeOther.m_deviceId; + else + { + if (strcmp(typeThis.m_productCode, typeOther.m_productCode) == 0) + return 1; + } + + return 0; +} + +/*! \brief Returns true if the ID is just a device type, not an actual device ID + \returns true if the ID is just a device type +*/ +int XsDeviceId_isType(XsDeviceId const* thisPtr) +{ + // true when we have a valid type (not a broadcast) and a 0 ID + if (XsDeviceId_isLegacyDeviceId(thisPtr)) + return !(thisPtr->m_deviceId & XS_DID_ID_MASK); + else + return !(thisPtr->m_deviceId & ~XS_DID64_BIT); +} + +/*! \brief Returns the name of the type of device identified by this id + \param str String to write to +*/ +void XsDeviceId_typeName(XsDeviceId const* thisPtr, XsString* str) +{ + if (!str) + return; + + if (!thisPtr) + { + XsString_assignCharArray(str, "invalid"); + return; + } + + if (XsDeviceId_isAwinda2Station(thisPtr)) + XsString_assignCharArray(str, "Awinda Station v2"); + else if (XsDeviceId_isAwinda2Dongle(thisPtr)) + XsString_assignCharArray(str, "Awinda Dongle v2"); + else if (XsDeviceId_isAwinda2Oem(thisPtr)) + XsString_assignCharArray(str, "Awinda OEM v2"); + else if (XsDeviceId_isMtw2(thisPtr)) + XsString_assignCharArray(str, "MTw2"); + else if (XsDeviceId_isMtx2(thisPtr)) + XsString_assignCharArray(str, "MTx2"); + else if (XsDeviceId_isBodyPackV2(thisPtr)) + XsString_assignCharArray(str, "BPACK-V2"); + else if (XsDeviceId_isBodyPack(thisPtr)) + XsString_assignCharArray(str, "Bodypack"); + else if (XsDeviceId_isSyncStation2(thisPtr)) + XsString_assignCharArray(str, "Sync Station v2"); + else if (XsDeviceId_isMti6X0(thisPtr)) + { + XsSize length = 7; + if (thisPtr->m_productCode[7] != '-') + length = 8; + XsString_assign(str, length, thisPtr->m_productCode); + } + else if (XsDeviceId_isMti8X0(thisPtr)) + { + XsSize length = 7; + if (thisPtr->m_productCode[7] != '-') + length = 8; + XsString_assign(str, length, thisPtr->m_productCode); + } + else if (XsDeviceId_isMti3X0(thisPtr)) + { + if (XsDeviceId_isImu(thisPtr)) + XsString_assignCharArray(str, "MTi-310"); + else if (XsDeviceId_isVru(thisPtr)) + XsString_assignCharArray(str, "MTi-320"); + else if (XsDeviceId_isAhrs(thisPtr)) + XsString_assignCharArray(str, "MTi-330"); + } + else if (XsDeviceId_isMtiX(thisPtr) && XsDeviceId_isRtk(thisPtr)) + XsString_assignCharArray(str, "MTi-8"); + else if (XsDeviceId_isMtMk4_1(thisPtr)) + XsString_assignCharArray(str, "MTi-1"); + else if (XsDeviceId_isMtMk4_2(thisPtr)) + XsString_assignCharArray(str, "MTi-2"); + else if (XsDeviceId_isMtMk4_3(thisPtr)) + XsString_assignCharArray(str, "MTi-3"); + else if (XsDeviceId_isMtMk4_7(thisPtr)) + XsString_assignCharArray(str, "MTi-7"); + else if (XsDeviceId_isMtMk4_10(thisPtr)) + XsString_assignCharArray(str, "MTi-10"); + else if (XsDeviceId_isMtMk4_20(thisPtr)) + XsString_assignCharArray(str, "MTi-20"); + else if (XsDeviceId_isMtMk4_30(thisPtr)) + XsString_assignCharArray(str, "MTi-30"); + else if (XsDeviceId_isMtMk4_100(thisPtr)) + XsString_assignCharArray(str, "MTi-100"); + else if (XsDeviceId_isMtMk4_200(thisPtr)) + XsString_assignCharArray(str, "MTi-200"); + else if (XsDeviceId_isMtMk4_300(thisPtr)) + XsString_assignCharArray(str, "MTi-300"); + else if (XsDeviceId_isMtMk4_400(thisPtr)) + XsString_assignCharArray(str, "MTi-400"); + else if (XsDeviceId_isMtMk4_500(thisPtr)) + XsString_assignCharArray(str, "MTi-500"); + else if (XsDeviceId_isMtMk4_710(thisPtr)) + XsString_assignCharArray(str, "MTi-G-710"); + else if (XsDeviceId_isMtMk4_700(thisPtr)) + XsString_assignCharArray(str, "MTi-G-700"); + else if (XsDeviceId_isMtMk4_800(thisPtr)) + 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"); +} + +/*! \brief Returns the type of device identified by this id + \param type Type to write to +*/ +void XsDeviceId_type(struct XsDeviceId const* thisPtr, struct XsDeviceId* type) +{ + if (!type) + return; + if (!XsDeviceId_isLegacyDeviceId(thisPtr)) + { + type->m_deviceId = XS_DID64_BIT; + XsString typeName = XsString_INITIALIZER; + XsDeviceId_typeName(thisPtr, &typeName); + strcpy(type->m_productCode, typeName.m_data); + } + else + type->m_deviceId = thisPtr->m_deviceId & XS_DID_FULLTYPE_MASK; +} + +/*! \brief Returns the device type identified by this id (eg 10, 300 and Awinda2 Master) + \param detailed Return detailed type information + \param type Type to write to +*/ +void XsDeviceId_deviceType(struct XsDeviceId const* thisPtr, int detailed, struct XsDeviceId* type) +{ + if (!type) + return; + if (!XsDeviceId_isLegacyDeviceId(thisPtr)) + { + if (XsDeviceId_isMti6X0(thisPtr) || XsDeviceId_isMti8X0(thisPtr)) + { + type->m_deviceId = XS_DID64_BIT; + strncpy(type->m_productCode, thisPtr->m_productCode, 7); + type->m_productCode[7] = 0; + if (detailed) + { + type->m_hardwareVersion = thisPtr->m_hardwareVersion; + type->m_productVariant = thisPtr->m_productVariant; + } + else + { + type->m_hardwareVersion = 0; + type->m_productVariant = 0; + } + } + else + { + type->m_deviceId = XS_DID64_BIT; + strncpy(type->m_productCode, thisPtr->m_productCode, 24); + type->m_productCode[23] = 0; + if (detailed) + { + type->m_hardwareVersion = thisPtr->m_hardwareVersion; + type->m_productVariant = thisPtr->m_productVariant; + } + else + { + type->m_hardwareVersion = 0; + type->m_productVariant = 0; + } + } + } + else + { + XsDeviceId_deviceTypeMask(thisPtr, detailed, type); + type->m_deviceId = thisPtr->m_deviceId & type->m_deviceId; + strcpy(type->m_productCode, thisPtr->m_productCode); + } +} + +/*! \brief Returns the mask which can be used to get the detailed device type (eg 10, 300 and Awinda2 Master) + \param detailed Return detailed type information + \param type Type to write to +*/ +void XsDeviceId_deviceTypeMask(struct XsDeviceId const* thisPtr, int detailed, struct XsDeviceId* type) +{ + if (XsDeviceId_isLegacyDeviceId(thisPtr)) + { + if (XsDeviceId_isMti3X0(thisPtr)) + type->m_deviceId = (XS_DID_TYPEH_MASK | (detailed ? (XS_DID_GPH_MASK | XS_DID_GPL_MASK) : 0)); + else if (XsDeviceId_isMtMk4_X(thisPtr)) + type->m_deviceId = (XS_DID_TYPEH_MASK | (detailed ? (XS_DID_GPH_MASK | XS_DID_GPL_MASK | XS_DID_TYPEL_MASK) : 0)); + 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) : 0)); + else if (XsDeviceId_isSyncStationX(thisPtr)) + 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) + type->m_deviceId = XS_DID_ABMCLOCKMASTER; + else + type->m_deviceId = XS_DID_TYPEH_MASK; + } + else + type->m_deviceId = XS_DID64_BIT; +} + +//============================================================================================================ +//============================================================================================================ +//==== Deprecated methods follow ===== +//============================================================================================================ +//============================================================================================================ + +/*! \brief Test if this device ID represents an MTMk4. + \returns True if it is an MTMk4. + \deprecated If the purpose is to check for a Mk4, Mk5 or 1-series device use: isMti() || isMtig(). If the purpose is to detect actual Mk4 (10,100,7x0) use isMtMark4() +*/ +int XsDeviceId_isMtMk4(const struct XsDeviceId* thisPtr) +{ + return (XsDeviceId_isMtMk4_X(thisPtr) || + XsDeviceId_isMtMk4_X0(thisPtr) || + XsDeviceId_isMtMk4_X00(thisPtr)); +} + +/*! \brief Test if this device ID represents an MTMk4 1 series. + \returns True if it is an MTMk4 1 series. + \deprecated Use isMtiX() +*/ +int XsDeviceId_isMtMk4_X(const struct XsDeviceId* thisPtr) +{ + return XsDeviceId_isMtiX(thisPtr); +} + +/*! \brief Test if this device ID represents an MTMk4 1. + \returns True if it is an MTMk4 1. + \deprecated Use isMtiX() and isImu() +*/ +int XsDeviceId_isMtMk4_1(const struct XsDeviceId* thisPtr) +{ + return XsDeviceId_isMtiX(thisPtr) && XsDeviceId_isImu(thisPtr); +} + +/*! \brief Test if this device ID represents an MTMk4 2. + \returns True if it is an MTMk4 2. + \deprecated Use isMtiX() and isVru() +*/ +int XsDeviceId_isMtMk4_2(const struct XsDeviceId* thisPtr) +{ + return XsDeviceId_isMtiX(thisPtr) && XsDeviceId_isVru(thisPtr); +} + +/*! \brief Test if this device ID represents an MTMk4 3. + \returns True if it is an MTMk4 3. + \deprecated Use isMtiX() and isAhrs() +*/ +int XsDeviceId_isMtMk4_3(const struct XsDeviceId* thisPtr) +{ + return XsDeviceId_isMtiX(thisPtr) && XsDeviceId_isAhrs(thisPtr); +} + +/*! \brief Test if this device ID represents an MTMk4 7. + \returns True if it is an MTMk4 7. + \deprecated Use isMtiX(), isGnss(), and !isRtk() together +*/ +int XsDeviceId_isMtMk4_7(const struct XsDeviceId* thisPtr) +{ + return XsDeviceId_isMtiX(thisPtr) && XsDeviceId_isGnss(thisPtr) && !XsDeviceId_isRtk(thisPtr); +} + +/*! \brief Test if this device ID represents an MTMk4 10 series. + \returns True if it is an MTMk4 10 series. + \deprecated Use isMtiX0() +*/ +int XsDeviceId_isMtMk4_X0(const struct XsDeviceId* thisPtr) +{ + return XsDeviceId_isMtiX0(thisPtr); +} + +/*! \brief Test if this device ID represents an MTMk4 10. + \returns True if it is an MTMk4 10. + \deprecated Use isMtiX0() and isImu() +*/ +int XsDeviceId_isMtMk4_10(const struct XsDeviceId* thisPtr) +{ + return XsDeviceId_isMtiX0(thisPtr) && XsDeviceId_isImu(thisPtr); +} + +/*! \brief Test if this device ID represents an MTMk4 20. + \returns True if it is an MTMk4 20. + \deprecated Use isMtiX0() and isVru() +*/ +int XsDeviceId_isMtMk4_20(const struct XsDeviceId* thisPtr) +{ + return XsDeviceId_isMtiX0(thisPtr) && XsDeviceId_isVru(thisPtr); +} + +/*! \brief Test if this device ID represents an MTMk4 30. + \returns True if it is an MTMk4 30. + \deprecated Use isMtiX0() and isAhrs() +*/ +int XsDeviceId_isMtMk4_30(const struct XsDeviceId* thisPtr) +{ + return XsDeviceId_isMtiX0(thisPtr) && XsDeviceId_isAhrs(thisPtr); +} + +/*! \brief Test if this device ID represents an MTMk4 100 series (including 700 and 710) + \returns True if it is an MTMk4 100 series. + \deprecated Use isMtiX00() || isMtig() to detect all 100's and 7x0's +*/ +int XsDeviceId_isMtMk4_X00(const struct XsDeviceId* thisPtr) +{ + return XsDeviceId_isMtiX00(thisPtr); +} + +/*! \brief Test if this device ID represents an MTMk4 100. + \returns True if it is an MTMk4 100. + \deprecated Use isMtiX00() and isImu() +*/ +int XsDeviceId_isMtMk4_100(const struct XsDeviceId* thisPtr) +{ + return XsDeviceId_isMtiX00(thisPtr) && XsDeviceId_isImu(thisPtr); +} + +/*! \brief Test if this device ID represents an MTMk4 200. + \returns True if it is an MTMk4 200. + \deprecated Use isMtiX00() and isVru() +*/ +int XsDeviceId_isMtMk4_200(const struct XsDeviceId* thisPtr) +{ + return XsDeviceId_isMtiX00(thisPtr) && XsDeviceId_isVru(thisPtr); +} + +/*! \brief Test if this device ID represents an MTMk4 300. + \returns True if it is an MTMk4 300. + \deprecated Use isMtiX00() and isAhrs() +*/ +int XsDeviceId_isMtMk4_300(const struct XsDeviceId* thisPtr) +{ + return XsDeviceId_isMtiX00(thisPtr) && XsDeviceId_isAhrs(thisPtr); +} + +/*! \brief Test if this device ID represents an MTMk4 400. + \returns True if it is an MTMk4 400. + \deprecated Will be removed +*/ +int XsDeviceId_isMtMk4_400(const struct XsDeviceId* thisPtr) +{ + return ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_400); +} + +/*! \brief Test if this device ID represents an MTMk4 500. + \returns True if it is an MTMk4 500. + \deprecated Will be removed +*/ +int XsDeviceId_isMtMk4_500(const struct XsDeviceId* thisPtr) +{ + return ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_500); +} + +/*! \brief Test if this device ID represents an MTMk4 600. + \returns True if it is an MTMk4 600. + \deprecated Will be removed +*/ +int XsDeviceId_isMtMk4_600(const struct XsDeviceId* thisPtr) +{ + return ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_600); +} + +/*! \brief Test if this device ID represents an MTMk4 700. + \returns True for all 700 and 710's (Mk4 and Mk5). + \deprecated use isGnss() to check for Gnss capabilities, + use isMtig() to check for either 700 or 710, + use isMtigX00() or isMtigX10() to discern between 700 and 710, + use isMtMark4() or isMtMark5() to discern between Mark 4 and Mark 5 +*/ +int XsDeviceId_isMtMk4_700(const struct XsDeviceId* thisPtr) +{ + return XsDeviceId_isMtigX00(thisPtr) || XsDeviceId_isMtigX10(thisPtr); +} + +/*! \brief Test if this device ID represents either an MTMk4 710 or Mk5 710. + \returns True if it is either an MTMk4 710 or Mk5 710. + \deprecated use isGnss() to check for Gnss capabilities, use isMtig() to check for either 700 or 710, use isMtigX00() or isMtigX10() to discern between 700 and 710 +*/ +int XsDeviceId_isMtMk4_710(const struct XsDeviceId* thisPtr) +{ + return XsDeviceId_isMtigX10(thisPtr); +} + +/*! \brief Test if this device ID represents an MTMk4 800. + \returns True if it is an MTMk4 800. + \deprecated Will be removed +*/ +int XsDeviceId_isMtMk4_800(const struct XsDeviceId* thisPtr) +{ + return ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_800); +} + +/*! \brief Test if this device ID represents an MTMk4 900. + \returns True if it is an MTMk4 900. + \deprecated Will be removed +*/ +int XsDeviceId_isMtMk4_900(const struct XsDeviceId* thisPtr) +{ + return ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_900); +} + +/*! \brief Test if this device ID represents an MTMk5. + \returns True if it is an MTMk5. + \deprecated Use isMtMark5() +*/ +int XsDeviceId_isMtMk5(const struct XsDeviceId* thisPtr) +{ + return XsDeviceId_isMtMark5(thisPtr); +} + +/*! \brief Test if this device ID represents an MTMk5 10 series. + \returns True if it is an MTMk5 10 series. + \deprecated Use isMtMark5() && isMtiX0() +*/ +int XsDeviceId_isMtMk5_X0(const struct XsDeviceId* thisPtr) +{ + return XsDeviceId_isMtMk5(thisPtr) && XsDeviceId_isMtMk4_X0(thisPtr); +} + +/*! \brief Test if this device ID represents an MTMk5 10. + \returns True if it is an MTMk5 10. + \deprecated Use isMtMark5() && isMtiX0() && isImu() +*/ +int XsDeviceId_isMtMk5_10(const struct XsDeviceId* thisPtr) +{ + return XsDeviceId_isMtMk5(thisPtr) && XsDeviceId_isMtMk4_10(thisPtr); +} + +/*! \brief Test if this device ID represents an MTMk5 20. + \returns True if it is an MTMk5 20. + \deprecated Use isMtMark5() && isMtiX0() && isVru() +*/ +int XsDeviceId_isMtMk5_20(const struct XsDeviceId* thisPtr) +{ + return XsDeviceId_isMtMk5(thisPtr) && XsDeviceId_isMtMk4_20(thisPtr); +} + +/*! \brief Test if this device ID represents an MTMk5 30. + \returns True if it is an MTMk5 30. + \deprecated Use isMtMark5() && isMtiX0() && isAhrs() +*/ +int XsDeviceId_isMtMk5_30(const struct XsDeviceId* thisPtr) +{ + return XsDeviceId_isMtMk5(thisPtr) && XsDeviceId_isMtMk4_30(thisPtr); +} + +/*! \brief Test if this device ID represents an MTMk5 100 series. + \returns True if it is an MTMk5 100 series. + \deprecated Use isMtMark5() && (isMtiX00() || isMtig()) +*/ +int XsDeviceId_isMtMk5_X00(const struct XsDeviceId* thisPtr) +{ + return XsDeviceId_isMtMk5(thisPtr) && XsDeviceId_isMtMk4_X00(thisPtr); +} + +/*! \brief Test if this device ID represents an MTMk5 100. + \returns True if it is an MTMk5 100. + \deprecated Use isMtMark5() && isMtiX00() && isImu() +*/ +int XsDeviceId_isMtMk5_100(const struct XsDeviceId* thisPtr) +{ + return XsDeviceId_isMtMk5(thisPtr) && XsDeviceId_isMtMk4_100(thisPtr); +} + +/*! \brief Test if this device ID represents an MTMk5 200. + \returns True if it is an MTMk5 200. + \deprecated Use isMtMark5() && isMtiX00() && isVru() +*/ +int XsDeviceId_isMtMk5_200(const struct XsDeviceId* thisPtr) +{ + return XsDeviceId_isMtMk5(thisPtr) && XsDeviceId_isMtMk4_200(thisPtr); +} + +/*! \brief Test if this device ID represents an MTMk5 300. + \returns True if it is an MTMk5 300. + \deprecated Use isMtMark5() && isMtiX00() && isAhrs() +*/ +int XsDeviceId_isMtMk5_300(const struct XsDeviceId* thisPtr) +{ + return XsDeviceId_isMtMk5(thisPtr) && XsDeviceId_isMtMk4_300(thisPtr); +} + +/*! \brief Test if this device ID represents an MTMk5 710. + \returns True if it is an MTMk5 710. + \deprecated Use (isMtMark5() && isMtigX10()) +*/ +int XsDeviceId_isMtMk5_710(const struct XsDeviceId* thisPtr) +{ + return XsDeviceId_isMtMk5(thisPtr) && XsDeviceId_isMtMk4_710(thisPtr); +} + +/*! @} */ diff --git a/extern/xspublic/xstypes/xsdeviceid.h b/extern/xspublic/xstypes/xsdeviceid.h new file mode 100644 index 0000000..3d33ff7 --- /dev/null +++ b/extern/xspublic/xstypes/xsdeviceid.h @@ -0,0 +1,925 @@ + +// Copyright (c) 2003-2022 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 XSDEVICEID_H +#define XSDEVICEID_H + +#include "xstypesconfig.h" +#include "pstdint.h" +#include "xsstring.h" +#include "xshandid.h" +#ifdef __cplusplus +#include +extern "C" { +#endif + +#define XSDEVICEID_PRODUCT_CODE_LEN 24 +#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, 0 } +#endif + +struct XsDeviceId; + +XSTYPES_DLL_API int XsDeviceId_isLegacyDeviceId(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API uint64_t XsDeviceId_legacyBit(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMtiX(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMtiX0(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMtiX00(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMtigX00(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMtigX10(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMti3X0(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMti6X0(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMti8X0(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMtw(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMtw2(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMtx(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMtx2(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isBodyPack(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isBodyPackV1(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isBodyPackV2(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isWirelessMaster(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isAwindaX(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isAwindaXStation(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isAwindaXDongle(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isAwindaXOem(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isAwinda2(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isAwinda2Station(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isAwinda2Dongle(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isAwinda2Oem(struct XsDeviceId const* thisPtr); +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); + +XSTYPES_DLL_API int XsDeviceId_isImu(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isVru(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isAhrs(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isGnss(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isRtk(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_hasInternalGnss(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isContainerDevice(struct XsDeviceId const* thisPtr); + +XSTYPES_DLL_API int XsDeviceId_isMt(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMti(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMtig(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMtMark4(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMtMark5(struct XsDeviceId const* thisPtr); + +//------------------------------------------------------------------------------------------------------------ +//------------------------------------------------------------------------------------------------------------ +//------------------------------------------------------------------------------------------------------------ +//------------------------------------------------------------------------------------------------------------ + +XSTYPES_DLL_API void XsDeviceId_toString(struct XsDeviceId const* thisPtr, XsString* str); +XSTYPES_DLL_API void XsDeviceId_fromString(struct XsDeviceId* thisPtr, XsString const* str); +XSTYPES_DLL_API void XsDeviceId_toDeviceTypeString(struct XsDeviceId const* thisPtr, XsString* str, int makeType); +XSTYPES_DLL_API void XsDeviceId_fromDeviceTypeString(struct XsDeviceId* thisPtr, XsString const* str); +XSTYPES_DLL_API int XsDeviceId_isValid(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API void XsDeviceId_swap(struct XsDeviceId* a, struct XsDeviceId* b); +XSTYPES_DLL_API int XsDeviceId_contains(struct XsDeviceId const* a, struct XsDeviceId const* b); +XSTYPES_DLL_API int XsDeviceId_isType(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API void XsDeviceId_typeName(struct XsDeviceId const* thisPtr, XsString* str); +XSTYPES_DLL_API void XsDeviceId_type(struct XsDeviceId const* thisPtr, struct XsDeviceId* type); +XSTYPES_DLL_API void XsDeviceId_deviceType(struct XsDeviceId const* thisPtr, int detailed, struct XsDeviceId* type); +XSTYPES_DLL_API void XsDeviceId_deviceTypeMask(struct XsDeviceId const* thisPtr, int detailed, struct XsDeviceId* type); + +//============================================================================================================ +//==== Deprecated methods follow ===== +//============================================================================================================ +XSTYPES_DLL_API int XsDeviceId_isMtMk4(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMtMk4_X(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMtMk4_1(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMtMk4_2(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMtMk4_3(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMtMk4_7(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMtMk4_X0(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMtMk4_10(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMtMk4_20(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMtMk4_30(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMtMk4_X00(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMtMk4_100(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMtMk4_200(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMtMk4_300(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMtMk4_400(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMtMk4_500(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMtMk4_600(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMtMk4_700(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMtMk4_710(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMtMk4_800(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMtMk4_900(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMtMk5(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMtMk5_X0(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMtMk5_10(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMtMk5_20(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMtMk5_30(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMtMk5_X00(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMtMk5_100(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMtMk5_200(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMtMk5_300(struct XsDeviceId const* thisPtr); +XSTYPES_DLL_API int XsDeviceId_isMtMk5_710(struct XsDeviceId const* thisPtr); +//============================================================================================================ + +#ifdef __cplusplus +} // extern "C" +#endif + +struct XsDeviceId +{ +#ifdef __cplusplus + /*! \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_productVariant(productVariant) + , m_hardwareVersion(hardwareVersion) + , m_subDevice(subDevice) + { + memset(m_productCode, 0, sizeof(m_productCode)); + if (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_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) + { + memcpy(static_cast(this), static_cast(&other), sizeof(XsDeviceId)); + } + /*! \brief Assign the \a other deviceId to this XsDeviceId */ + inline const XsDeviceId& operator=(const XsDeviceId& other) + { + if (this != &other) + memcpy(static_cast(this), static_cast(&other), sizeof(XsDeviceId)); + return *this; + } + + /*! \brief \copybrief XsDeviceId_legacyBit(const struct XsDeviceId*) */ + static inline uint64_t legacyBit() + { + return XsDeviceId_legacyBit(nullptr); + } + /*! \brief \copybrief XsDeviceId_isLegacyDeviceId(const struct XsDeviceId*) */ + inline bool isLegacyDeviceId() const + { + return 0 != XsDeviceId_isLegacyDeviceId(this); + } + /*! \brief Returns the device serial number as an unsigned integer */ + inline uint64_t toInt() const + { + return m_deviceId; + } + /*! \brief Returns the product code */ + inline XsString productCode() const + { + XsString tmp(m_productCode); + return tmp; + } + /*! \brief Returns the product variant */ + inline uint32_t productVariant() const + { + return m_productVariant; + } + /*! \brief Returns the product variant */ + inline uint16_t hardwareVersion() const + { + 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 + { + return static_cast(m_deviceId); + } + /*! \brief \copybrief XsDeviceId_isMtiX(const struct XsDeviceId*) */ + inline bool isMtiX() const + { + return 0 != XsDeviceId_isMtiX(this); + } + /*! \brief \copybrief XsDeviceId_isMtiX0(const struct XsDeviceId*) */ + inline bool isMtiX0() const + { + return 0 != XsDeviceId_isMtiX0(this); + } + /*! \brief \copybrief XsDeviceId_isMtiX00(const struct XsDeviceId*) */ + inline bool isMtiX00() const + { + return 0 != XsDeviceId_isMtiX00(this); + } + /*! \brief \copybrief XsDeviceId_isMtigX00(const struct XsDeviceId*) */ + inline bool isMtigX00() const + { + return 0 != XsDeviceId_isMtigX00(this); + } + /*! \brief \copybrief XsDeviceId_isMtigX10(const struct XsDeviceId*) */ + inline bool isMtigX10() const + { + return 0 != XsDeviceId_isMtigX10(this); + } + /*! \brief \copybrief XsDeviceId_isMti3X0(const struct XsDeviceId*) */ + inline bool isMti3X0() const + { + return 0 != XsDeviceId_isMti3X0(this); + } + /*! \brief \copybrief XsDeviceId_isMti6X0(const struct XsDeviceId*) */ + inline bool isMti6X0() const + { + return 0 != XsDeviceId_isMti6X0(this); + } + /*! \brief \copybrief XsDeviceId_isMti8X0(const struct XsDeviceId*) */ + inline bool isMti8X0() const + { + return 0 != XsDeviceId_isMti8X0(this); + } + /*! \brief \copybrief XsDeviceId_isGlove(const struct XsDeviceId*) */ + 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 + { + return 0 != XsDeviceId_isRugged(this); + } + /*! \brief \copybrief XsDeviceId_isMtw(const struct XsDeviceId*) */ + inline bool isMtw() const + { + return 0 != XsDeviceId_isMtw(this); + } + /*! \brief \copybrief XsDeviceId_isMtw2(const struct XsDeviceId*) */ + inline bool isMtw2() const + { + return 0 != XsDeviceId_isMtw2(this); + } + /*! \brief \copybrief XsDeviceId_isMtx(const struct XsDeviceId*) */ + inline bool isMtx() const + { + return 0 != XsDeviceId_isMtx(this); + } + /*! \brief \copybrief XsDeviceId_isMtx2(const struct XsDeviceId*) */ + inline bool isMtx2() const + { + return 0 != XsDeviceId_isMtx2(this); + } + /*! \brief \copybrief XsDeviceId_isBodyPack(const struct XsDeviceId*) */ + inline bool isBodyPack() const + { + return 0 != XsDeviceId_isBodyPack(this); + } + /*! \brief \copybrief XsDeviceId_isBodyPackV1(const struct XsDeviceId*) */ + inline bool isBodyPackV1() const + { + return 0 != XsDeviceId_isBodyPackV1(this); + } + /*! \brief \copybrief XsDeviceId_isBodyPackV2(const struct XsDeviceId*) */ + inline bool isBodyPackV2() const + { + return 0 != XsDeviceId_isBodyPackV2(this); + } + /*! \brief \copybrief XsDeviceId_isWirelessMaster(const struct XsDeviceId*) */ + inline bool isWirelessMaster() const + { + return 0 != XsDeviceId_isWirelessMaster(this); + } + /*! \brief \copybrief XsDeviceId_isAwindaX(const struct XsDeviceId*) */ + inline bool isAwindaX() const + { + return 0 != XsDeviceId_isAwindaX(this); + } + /*! \brief \copybrief XsDeviceId_isAwindaXStation(const struct XsDeviceId*) */ + inline bool isAwindaXStation() const + { + return 0 != XsDeviceId_isAwindaXStation(this); + } + /*! \brief \copybrief XsDeviceId_isAwindaXDongle(const struct XsDeviceId*) */ + inline bool isAwindaXDongle() const + { + return 0 != XsDeviceId_isAwindaXDongle(this); + } + /*! \brief \copybrief XsDeviceId_isAwindaXOem(const struct XsDeviceId*) */ + inline bool isAwindaXOem() const + { + return 0 != XsDeviceId_isAwindaXOem(this); + } + /*! \brief \copybrief XsDeviceId_isAwinda2(const struct XsDeviceId*) */ + inline bool isAwinda2() const + { + return 0 != XsDeviceId_isAwinda2(this); + } + /*! \brief \copybrief XsDeviceId_isAwinda2Station(const struct XsDeviceId*) */ + inline bool isAwinda2Station() const + { + return 0 != XsDeviceId_isAwinda2Station(this); + } + /*! \brief \copybrief XsDeviceId_isAwinda2Dongle(const struct XsDeviceId*) */ + inline bool isAwinda2Dongle() const + { + return 0 != XsDeviceId_isAwinda2Dongle(this); + } + /*! \brief \copybrief XsDeviceId_isAwinda2Oem(const struct XsDeviceId*) */ + inline bool isAwinda2Oem() const + { + return 0 != XsDeviceId_isAwinda2Oem(this); + } + /*! \brief \copybrief XsDeviceId_isSyncStationX(const struct XsDeviceId*) */ + inline bool isSyncStationX() const + { + return 0 != XsDeviceId_isSyncStationX(this); + } + /*! \brief \copybrief XsDeviceId_isSyncStation2(const struct XsDeviceId*) */ + inline bool isSyncStation2() const + { + return 0 != XsDeviceId_isSyncStation2(this); + } + /*! \brief \copybrief XsDeviceId_isHilDevice(const struct XsDeviceId*) */ + inline bool isHilDevice() const + { + return 0 != XsDeviceId_isHilDevice(this); + } + /*! \brief \copybrief XsDeviceId_isImu(const struct XsDeviceId*) */ + inline bool isImu() const + { + return 0 != XsDeviceId_isImu(this); + } + /*! \brief \copybrief XsDeviceId_isVru(const struct XsDeviceId*) */ + inline bool isVru() const + { + return 0 != XsDeviceId_isVru(this); + } + /*! \brief \copybrief XsDeviceId_isAhrs(const struct XsDeviceId*) */ + inline bool isAhrs() const + { + return 0 != XsDeviceId_isAhrs(this); + } + /*! \brief \copybrief XsDeviceId_isGnss(const struct XsDeviceId*) */ + inline bool isGnss() const + { + return 0 != XsDeviceId_isGnss(this); + } + /*! \brief \copybrief XsDeviceId_isRtk(const struct XsDeviceId*) */ + inline bool isRtk() const + { + return 0 != XsDeviceId_isRtk(this); + } + /*! \brief \copybrief XsDeviceId_hasInternalGnss(const struct XsDeviceId*) */ + inline bool hasInternalGnss() const + { + return 0 != XsDeviceId_hasInternalGnss(this); + } + /*! \brief \copybrief XsDeviceId_isContainerDevice(const struct XsDeviceId*) */ + inline bool isContainerDevice() const + { + return 0 != XsDeviceId_isContainerDevice(this); + } + /*! \brief \copybrief XsDeviceId_isMt(const struct XsDeviceId*) */ + inline bool isMt() const + { + return 0 != XsDeviceId_isMt(this); + } + /*! \brief \copybrief XsDeviceId_isMti(const struct XsDeviceId*) */ + inline bool isMti() const + { + return 0 != XsDeviceId_isMti(this); + } + /*! \brief \copybrief XsDeviceId_isMtig(const struct XsDeviceId*) */ + inline bool isMtig() const + { + return 0 != XsDeviceId_isMtig(this); + } + /*! \brief \copybrief XsDeviceId_isMtMark4(const struct XsDeviceId*) */ + inline bool isMtMark4() const + { + return 0 != XsDeviceId_isMtMark4(this); + } + /*! \brief \copybrief XsDeviceId_isMtMark5(const struct XsDeviceId*) */ + inline bool isMtMark5() const + { + return 0 != XsDeviceId_isMtMark5(this); + } + /*! \brief \copybrief XsDeviceId_toString(const XsDeviceId* thisPtr, XsString* str) */ + inline XsString toString() const + { + XsString tmp; + XsDeviceId_toString(this, &tmp); + return tmp; + } + /*! \copydoc XsDeviceId_fromString(XsDeviceId* thisPtr, const XsString* str) */ + inline void fromString(const XsString& str) + { + XsDeviceId_fromString(this, &str); + } + + /*! \copybrief XsDeviceId_toDeviceTypeString(const XsDeviceId* thisPtr, XsString* str, int makeType) + \param makeType Boolean whether the deviceid is made a devicetype or is printed fully in the output string + \return The string with the device type or device id information in string format + */ + inline XsString toDeviceTypeString(bool makeType = true) const + { + XsString tmp; + XsDeviceId_toDeviceTypeString(this, &tmp, makeType ? 1 : 0); + return tmp; + } + + /*! \copydoc XsDeviceId_fromDeviceTypeString(XsDeviceId* thisPtr, const XsString* str) + */ + inline void fromDeviceTypeString(const XsString& str) + { + XsDeviceId_fromDeviceTypeString(this, &str); + } + + /*! \brief \copybrief XsDeviceId_isValid(const struct XsDeviceId*) */ + inline bool isValid() const + { + return 0 != XsDeviceId_isValid(this); + } + /*! \copydoc XsDeviceId_contains(XsDeviceId const*, XsDeviceId const*) */ + inline bool contains(const XsDeviceId& other) const + { + return 0 != XsDeviceId_contains(this, &other); + } + /*! \brief Returns true if the ID is just a device type, not an actual device ID */ + inline bool isType() const + { + return 0 != XsDeviceId_isType(this); + } + /*! \brief Returns the name of the type of device identified by this id */ + inline XsString typeName() const + { + XsString rv; + XsDeviceId_typeName(this, &rv); + return rv; + } + /*! \brief Returns the type of device identified by this id */ + inline XsDeviceId type() const + { + XsDeviceId xtype; + XsDeviceId_type(this, &xtype); + return xtype; + } + /*! \brief Returns the (detailed) device type of this id + \param detailed Boolean whether detailed information is returned + \return The requested device type + */ + inline XsDeviceId deviceType(bool detailed = true) const + { + XsDeviceId xtype; + XsDeviceId_deviceType(this, detailed ? 1 : 0, &xtype); + return xtype; + } + /*! \brief Returns the detailed device type mask of this id + \param detailed Boolean whether detailed information is returned + \return The requested device type mask + */ + inline XsDeviceId deviceTypeMask(bool detailed = true) const + { + XsDeviceId xtype; + XsDeviceId_deviceTypeMask(this, detailed ? 1 : 0, &xtype); + return xtype; + } + + /*! \brief Returns true if the \a other deviceId matches this deviceId */ + inline bool operator==(const XsDeviceId& other) const + { + if (isLegacyDeviceId() || other.isLegacyDeviceId()) + 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); + } + /*! \brief Returns true if this deviceId is less than the \a other deviceId */ + inline bool operator<(const XsDeviceId& other) const + { + if (isLegacyDeviceId() || other.isLegacyDeviceId()) + return toInt() < other.toInt(); + else + { + int pdiff = strcmp(m_productCode, other.m_productCode); + if (pdiff < 0) + return true; + else if (pdiff > 0) + return false; + else + { + if (m_hardwareVersion < other.m_hardwareVersion) + return true; + else if (m_hardwareVersion > other.m_hardwareVersion) + return false; + else + { + if (m_productVariant < other.m_productVariant) + return true; + else if (m_productVariant > other.m_productVariant) + return false; + else + { + if (toInt() < other.toInt()) + return true; + else if (toInt() > other.toInt()) + return false; + else + return m_subDevice < other.m_subDevice; + } + } + } + } + } + /*! \brief Returns true if this deviceId is less or equal to the \a other deviceId */ + inline bool operator<=(const XsDeviceId& other) const + { + if (isLegacyDeviceId()) + return toInt() <= other.toInt(); + else + { + int pdiff = strcmp(m_productCode, other.m_productCode); + if (pdiff > 0) + return false; + else + { + if (m_hardwareVersion > other.m_hardwareVersion) + return false; + else + { + if (m_productVariant > other.m_productVariant) + return false; + else + { + if (toInt() < other.toInt()) + return true; + else if (toInt() > other.toInt()) + return false; + else + return m_subDevice <= other.m_subDevice; + } + } + } + } + } + /*! \brief Returns true if this deviceId is larger than the \a other deviceId */ + inline bool operator>(const XsDeviceId& other) const + { + if (isLegacyDeviceId()) + return toInt() > other.toInt(); + else + { + int pdiff = strcmp(m_productCode, other.m_productCode); + if (pdiff > 0) + return true; + else if (pdiff < 0) + return false; + else + { + if (m_hardwareVersion > other.m_hardwareVersion) + return true; + else if (m_hardwareVersion < other.m_hardwareVersion) + return false; + else + { + if (m_productVariant > other.m_productVariant) + return true; + else if (m_productVariant < other.m_productVariant) + return false; + else + { + if (toInt() > other.toInt()) + return true; + else if (toInt() < other.toInt()) + return false; + else + return m_subDevice > other.m_subDevice; + } + } + } + } + } + /*! \brief Returns true if this deviceId is larger or equal to the \a other deviceId */ + inline bool operator>=(const XsDeviceId& other) const + { + if (isLegacyDeviceId()) + return toInt() >= other.toInt(); + else + { + int pdiff = strcmp(m_productCode, other.m_productCode); + if (pdiff < 0) + return false; + else + { + if (m_hardwareVersion < other.m_hardwareVersion) + return false; + else + { + if (m_productVariant < other.m_productVariant) + return false; + else + { + if (toInt() > other.toInt()) + return true; + else if (toInt() < other.toInt()) + return false; + else + return m_subDevice >= other.m_subDevice; + } + } + } + } + } + + + //============================================================================================================ + //============================================================================================================ + //==== Deprecated methods follow ===== + //============================================================================================================ + //============================================================================================================ + XSDEPRECATED_START + + /*! \brief \copybrief XsDeviceId_isMtMk4(const struct XsDeviceId*) */ + XSDEPRECATED inline bool isMtMk4() const + { + return 0 != XsDeviceId_isMtMk4(this); + } + /*! \brief \copybrief XsDeviceId_isMtMk4_X(const struct XsDeviceId*) */ + XSDEPRECATED inline bool isMtMk4_X() const + { + return 0 != XsDeviceId_isMtMk4_X(this); + } + /*! \brief \copybrief XsDeviceId_isMtMk4_1(const struct XsDeviceId*) */ + XSDEPRECATED inline bool isMtMk4_1() const + { + return 0 != XsDeviceId_isMtMk4_1(this); + } + /*! \brief \copybrief XsDeviceId_isMtMk4_2(const struct XsDeviceId*) */ + XSDEPRECATED inline bool isMtMk4_2() const + { + return 0 != XsDeviceId_isMtMk4_2(this); + } + /*! \brief \copybrief XsDeviceId_isMtMk4_3(const struct XsDeviceId*) */ + XSDEPRECATED inline bool isMtMk4_3() const + { + return 0 != XsDeviceId_isMtMk4_3(this); + } + /*! \brief \copybrief XsDeviceId_isMtMk4_7(const struct XsDeviceId*) */ + XSDEPRECATED inline bool isMtMk4_7() const + { + return 0 != XsDeviceId_isMtMk4_7(this); + } + /*! \brief \copybrief XsDeviceId_isMtMk4_X0(const struct XsDeviceId*) */ + XSDEPRECATED inline bool isMtMk4_X0() const + { + return 0 != XsDeviceId_isMtMk4_X0(this); + } + /*! \brief \copybrief XsDeviceId_isMtMk4_10(const struct XsDeviceId*) */ + XSDEPRECATED inline bool isMtMk4_10() const + { + return 0 != XsDeviceId_isMtMk4_10(this); + } + /*! \brief \copybrief XsDeviceId_isMtMk4_20(const struct XsDeviceId*) */ + XSDEPRECATED inline bool isMtMk4_20() const + { + return 0 != XsDeviceId_isMtMk4_20(this); + } + /*! \brief \copybrief XsDeviceId_isMtMk4_30(const struct XsDeviceId*) */ + XSDEPRECATED inline bool isMtMk4_30() const + { + return 0 != XsDeviceId_isMtMk4_30(this); + } + /*! \brief \copybrief XsDeviceId_isMtMk4_X00(const struct XsDeviceId*) */ + XSDEPRECATED inline bool isMtMk4_X00() const + { + return 0 != XsDeviceId_isMtMk4_X00(this); + } + /*! \brief \copybrief XsDeviceId_isMtMk4_100(const struct XsDeviceId*) */ + XSDEPRECATED inline bool isMtMk4_100() const + { + return 0 != XsDeviceId_isMtMk4_100(this); + } + /*! \brief \copybrief XsDeviceId_isMtMk4_200(const struct XsDeviceId*) */ + XSDEPRECATED inline bool isMtMk4_200() const + { + return 0 != XsDeviceId_isMtMk4_200(this); + } + /*! \brief \copybrief XsDeviceId_isMtMk4_300(const struct XsDeviceId*) */ + XSDEPRECATED inline bool isMtMk4_300() const + { + return 0 != XsDeviceId_isMtMk4_300(this); + } + /*! \brief \copybrief XsDeviceId_isMtMk4_400(const struct XsDeviceId*) */ + XSDEPRECATED inline bool isMtMk4_400() const + { + return 0 != XsDeviceId_isMtMk4_400(this); + } + /*! \brief \copybrief XsDeviceId_isMtMk4_500(const struct XsDeviceId*) */ + XSDEPRECATED inline bool isMtMk4_500() const + { + return 0 != XsDeviceId_isMtMk4_500(this); + } + /*! \brief \copybrief XsDeviceId_isMtMk4_600(const struct XsDeviceId*) */ + XSDEPRECATED inline bool isMtMk4_600() const + { + return 0 != XsDeviceId_isMtMk4_600(this); + } + /*! \brief \copybrief XsDeviceId_isMtMk4_700(const struct XsDeviceId*) */ + XSDEPRECATED inline bool isMtMk4_700() const + { + return 0 != XsDeviceId_isMtMk4_700(this); + } + /*! \brief \copybrief XsDeviceId_isMtMk4_710(const struct XsDeviceId*) */ + XSDEPRECATED inline bool isMtMk4_710() const + { + return 0 != XsDeviceId_isMtMk4_710(this); + } + /*! \brief \copybrief XsDeviceId_isMtMk4_800(const struct XsDeviceId*) */ + XSDEPRECATED inline bool isMtMk4_800() const + { + return 0 != XsDeviceId_isMtMk4_800(this); + } + /*! \brief \copybrief XsDeviceId_isMtMk4_900(const struct XsDeviceId*) */ + XSDEPRECATED inline bool isMtMk4_900() const + { + return 0 != XsDeviceId_isMtMk4_900(this); + } + /*! \brief \copybrief XsDeviceId_isMtMk5(const struct XsDeviceId*) */ + XSDEPRECATED inline bool isMtMk5() const + { + return 0 != XsDeviceId_isMtMk5(this); + } + /*! \brief \copybrief XsDeviceId_isMtMk5_X0(const struct XsDeviceId*) */ + XSDEPRECATED inline bool isMtMk5_X0() const + { + return 0 != XsDeviceId_isMtMk5_X0(this); + } + /*! \brief \copybrief XsDeviceId_isMtMk5_10(const struct XsDeviceId*) */ + XSDEPRECATED inline bool isMtMk5_10() const + { + return 0 != XsDeviceId_isMtMk5_10(this); + } + /*! \brief \copybrief XsDeviceId_isMtMk5_20(const struct XsDeviceId*) */ + XSDEPRECATED inline bool isMtMk5_20() const + { + return 0 != XsDeviceId_isMtMk5_20(this); + } + /*! \brief \copybrief XsDeviceId_isMtMk5_30(const struct XsDeviceId*) */ + XSDEPRECATED inline bool isMtMk5_30() const + { + return 0 != XsDeviceId_isMtMk5_30(this); + } + /*! \brief \copybrief XsDeviceId_isMtMk5_X00(const struct XsDeviceId*) */ + XSDEPRECATED inline bool isMtMk5_X00() const + { + return 0 != XsDeviceId_isMtMk5_X00(this); + } + /*! \brief \copybrief XsDeviceId_isMtMk5_100(const struct XsDeviceId*) */ + XSDEPRECATED inline bool isMtMk5_100() const + { + return 0 != XsDeviceId_isMtMk5_100(this); + } + /*! \brief \copybrief XsDeviceId_isMtMk5_200(const struct XsDeviceId*) */ + XSDEPRECATED inline bool isMtMk5_200() const + { + return 0 != XsDeviceId_isMtMk5_200(this); + } + /*! \brief \copybrief XsDeviceId_isMtMk5_300(const struct XsDeviceId*) */ + XSDEPRECATED inline bool isMtMk5_300() const + { + return 0 != XsDeviceId_isMtMk5_300(this); + } + /*! \brief \copybrief XsDeviceId_isMtMk5_710(const struct XsDeviceId*) */ + 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[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; + +#if defined(__cplusplus) && !defined(XSENS_NO_STL) +namespace std +{ +/*! \brief Stream output operator for XsDeviceId */ +template +basic_ostream<_CharT, _Traits>& operator<<(basic_ostream<_CharT, _Traits>& o, XsDeviceId const& xd) +{ + return (o << xd.toString()); +} +} + +inline XsString& operator<<(XsString& o, XsDeviceId const& xd) +{ + o.append(xd.toString()); + 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/extern/xspublic/xstypes/xsdeviceidarray.c b/extern/xspublic/xstypes/xsdeviceidarray.c new file mode 100644 index 0000000..e371075 --- /dev/null +++ b/extern/xspublic/xstypes/xsdeviceidarray.c @@ -0,0 +1,82 @@ + +// Copyright (c) 2003-2022 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 "xsdeviceidarray.h" +#include "xsdeviceid.h" + +/*! \struct XsDeviceIdArray + \brief A list of XsDeviceId values + \sa XsArray +*/ + +void initDeviceId(XsDeviceId* did) +{ + static const XsDeviceId cleared = XSDEVICEID_INITIALIZER; + *did = cleared; +} + +void initDeviceIdToValue(XsDeviceId* did, XsDeviceId const* src) +{ + memcpy(did, src, sizeof(XsDeviceId)); +} + +int compareDeviceIds(XsDeviceId const* a, XsDeviceId const* b) +{ + int64_t diff = (int64_t)a->m_deviceId - (int64_t)b->m_deviceId; + if (diff < 0) + return -1; + else if (diff > 0) + return 1; + + return 0; +} + +//! \brief Descriptor for XsDeviceIdArray +XsArrayDescriptor const g_xsDeviceIdArrayDescriptor = +{ + sizeof(XsDeviceId), + XSEXPCASTITEMSWAP XsDeviceId_swap, // swap + XSEXPCASTITEMMAKE initDeviceId, // construct + XSEXPCASTITEMCOPY initDeviceIdToValue, // copy construct + 0, // destruct + XSEXPCASTITEMCOPY initDeviceIdToValue, // copy + XSEXPCASTITEMCOMP compareDeviceIds, // compare + XSEXPCASTRAWCOPY XsArray_rawCopy // raw copy +}; + +/*! \copydoc XsArray_constructDerived + \note Specialization for XsStringArray +*/ +void XsDeviceIdArray_construct(XsDeviceIdArray* thisPtr, XsSize count, XsDeviceId const* src) +{ + XsArray_construct(thisPtr, &g_xsDeviceIdArrayDescriptor, count, src); +} diff --git a/extern/xspublic/xstypes/xsdeviceidarray.h b/extern/xspublic/xstypes/xsdeviceidarray.h new file mode 100644 index 0000000..8323886 --- /dev/null +++ b/extern/xspublic/xstypes/xsdeviceidarray.h @@ -0,0 +1,96 @@ + +// Copyright (c) 2003-2022 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 XSDEVICEIDARRAY_H +#define XSDEVICEIDARRAY_H + +#include "xsarray.h" +#include "xsdeviceid.h" + +#ifdef __cplusplus +extern "C" { +#endif + +extern XsArrayDescriptor const XSTYPES_DLL_API g_xsDeviceIdArrayDescriptor; + +#ifndef __cplusplus +#define XSDEVICEIDARRAY_INITIALIZER XSARRAY_INITIALIZER(&g_xsDeviceIdArrayDescriptor) +XSARRAY_STRUCT(XsDeviceIdArray, XsDeviceId); +typedef struct XsDeviceIdArray XsDeviceIdArray; + +XSTYPES_DLL_API void XsDeviceIdArray_construct(XsDeviceIdArray* thisPtr, XsSize count, XsDeviceId const* src); +#endif + +#ifdef __cplusplus +} // extern "C" + +struct XsDeviceIdArray : public XsArrayImpl +{ + //! \brief Constructs an XsDeviceIdArray + inline explicit XsDeviceIdArray(XsSize sz = 0, XsDeviceId const* src = 0) + : ArrayImpl(sz, src) + { + } + + //! \brief Constructs an XsDeviceIdArray as a copy of \a other + inline XsDeviceIdArray(XsDeviceIdArray const& other) + : ArrayImpl(other) + { + } + + //! \brief Constructs an XsDeviceIdArray that references the data supplied in \a ref + inline explicit XsDeviceIdArray(XsDeviceId* 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(XsDeviceIdArray& first, XsDeviceIdArray& second) + { + first.swap(second); + } +#endif + +#ifndef XSENS_NOITERATOR + //! \brief Constructs an XsDeviceIdArray with the array bound by the supplied iterators \a beginIt and \a endIt + template + inline XsDeviceIdArray(Iterator beginIt, Iterator endIt) + : ArrayImpl(beginIt, endIt) + { + } +#endif +}; +#endif + + +#endif diff --git a/extern/xspublic/xstypes/xsdeviceoptionflag.h b/extern/xspublic/xstypes/xsdeviceoptionflag.h new file mode 100644 index 0000000..77a7b5a --- /dev/null +++ b/extern/xspublic/xstypes/xsdeviceoptionflag.h @@ -0,0 +1,92 @@ + +// Copyright (c) 2003-2022 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 XSDEVICEOPTIONFLAG_H +#define XSDEVICEOPTIONFLAG_H + +/*! \addtogroup enums Global enumerations + @{ +*/ +/*! \brief Used to enable or disable some device options + \sa XsDevice::setDeviceOptionFlags + \note Not all devices support all options. +*/ +enum XsDeviceOptionFlag +{ + XDOF_DisableAutoStore = 0x00000001, //!< When set to 1, automatic writing of configuration will be disabled. + XDOF_DisableAutoMeasurement = 0x00000002, //!< When set to 1, the MT will stay in Config Mode upon start up. + XDOF_EnableBeidou = 0x00000004, //!< When set to 1, enables Beidou, disables GLONASS (MTi-G). + XDOF_DisableGps = 0x00000008, //!< When set to 1, disables GPS (MTi-G). + XDOF_EnableAhs = 0x00000010, //!< When set to 1, the MTi will have Active Heading Stabilization (AHS) enabled. + XDOF_EnableOrientationSmoother = 0x00000020, //!< When set to 1, the MTi will have Orientation Smoother enabled. Only applicable to MTi-G-710, MTi-7, MTi-670 and MTi-680 + XDOF_EnableConfigurableBusId = 0x00000040, //!< When set to 1, allows to configure the BUS ID. + XDOF_EnableInrunCompassCalibration = 0x00000080, //!< When set to 1, the MTi will have In-run Compass Calibration (ICC) enabled. + XDOF_DisableSleepMode = 0x00000100, //!< When set to 1, an MTw will not enter sleep mode after a scan timeout. It will scan indefinitely. + XDOF_EnableConfigMessageAtStartup = 0x00000200, //!< When set to 1, the MT will send the Configuration to the Master at start-up + XDOF_EnableColdFilterResets = 0x00000400, //!< When set to 1, The MT performs a cold filter reset every time it goes to measurement + XDOF_EnablePositionVelocitySmoother = 0x00000800, //!< When set to 1, the MTi will have Position/Velocity Smoother enabled. Only applicable to MTi-680 + XDOF_EnableContinuousZRU = 0x00001000, //!< When set to 1, the MTi filter will perform continuous Zero Rotation Updates for gyroscope bias and noise. + XDOF_EnableRawGnssInputForwarding = 0x00002000, //!< When set to 1, the MTi will forward the raw input coming from the GNSS receiver encapsulated in an Xbus message. + + XDOF_None = 0x00000000, //!< When set to 1, disables all option flags. + XDOF_All = 0x7FFFFFFF //!< When set to 1, enables all option flags. +}; +/*! @} */ +typedef enum XsDeviceOptionFlag XsDeviceOptionFlag; + +#ifdef __cplusplus +//! \brief Logical OR operator for XsDeviceOptionFlag values +inline XsDeviceOptionFlag operator | (XsDeviceOptionFlag a, XsDeviceOptionFlag 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); +} + +//! \brief Logical XOR operator for XsDeviceOptionFlag values +inline XsDeviceOptionFlag operator ^ (XsDeviceOptionFlag a, XsDeviceOptionFlag b) +{ + return (XsDeviceOptionFlag)((int)a ^ (int)b); +} + +//! \brief Logical NEG operator for XsDeviceOptionFlag values +inline XsDeviceOptionFlag operator ~(XsDeviceOptionFlag a) +{ + return (XsDeviceOptionFlag)(~(int)a); +} +#endif + +#endif diff --git a/extern/xspublic/xstypes/xsdid.h b/extern/xspublic/xstypes/xsdid.h new file mode 100644 index 0000000..7154d65 --- /dev/null +++ b/extern/xspublic/xstypes/xsdid.h @@ -0,0 +1,187 @@ + +// Copyright (c) 2003-2022 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 XSDID_H +#define XSDID_H + +// DID Type (high nibble) +#define XS_DID_TYPEL_MASK 0x000F0000 +#define XS_DID_TYPEH_MASK 0x00F00000 +#define XS_DID_GPL_MASK 0x0F000000 +#define XS_DID_GPH_MASK 0xF0000000 +#define XS_DID_TYPE_MASK (XS_DID_TYPEH_MASK | XS_DID_TYPEL_MASK) +#define XS_DID_GP_MASK (XS_DID_GPH_MASK | XS_DID_GPL_MASK) +#define XS_DID_MK4TYPE_MASK (XS_DID_TYPEH_MASK | XS_DID_GPL_MASK) +#define XS_DID_ID_MASK 0x0000FFFF +#define XS_DID_FULLTYPE_MASK 0xFFFF0000 + +#define XS_DID_TYPEL_SHIFT 16 +#define XS_DID_TYPEH_SHIFT 20 +#define XS_DID_GPL_SHIFT 24 +#define XS_DID_GPH_SHIFT 28 + +#define XS_DID_TYPEH_INTERNAL 0x00000000 +#define XS_DID_TYPEH_AWINDAMASTER 0x00200000 +#define XS_DID_TYPEH_MT_X0 0x00600000 +#define XS_DID_TYPEH_MT_X00 0x00700000 +#define XS_DID_TYPEH_MTX2_MTW2 0x00B00000 +#define XS_DID_TYPEH_BODYPACK 0x00A00000 +#define XS_DID_TYPEH_MT_3X0 0x00D00000 +#define XS_DID_TYPEH_MT_X_MPU 0x00800000 + +#define XS_DID_TYPEL_STATION 0x00000000 +#define XS_DID_TYPEL_DONGLE 0x00010000 +#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 +#define XS_DID_TYPEL_RS485XM 0x00020000 +#define XS_DID_TYPEL_RS485 0x00030000 +#define XS_DID_TYPEL_WIRELESS 0x00040000 +#define XS_DID_TYPEL_COMM_MASK 0x00070000 +#define XS_DID_TYPEL_MULTI 0x00080000 +#define XS_DID_TYPEL_MK5 0x00080000 + +#define XS_DID_GPL_1 0x01000000 +#define XS_DID_GPL_2 0x02000000 +#define XS_DID_GPL_3 0x03000000 +#define XS_DID_GPL_7 0x07000000 +#define XS_DID_GPL_8 0x08000000 +#define XS_DID_GPL_10 0x01000000 +#define XS_DID_GPL_20 0x02000000 +#define XS_DID_GPL_30 0x03000000 +#define XS_DID_GPL_100 0x01000000 +#define XS_DID_GPL_200 0x02000000 +#define XS_DID_GPL_300 0x03000000 +#define XS_DID_GPL_400 0x04000000 +#define XS_DID_GPL_500 0x05000000 +#define XS_DID_GPL_600 0x06000000 +#define XS_DID_GPL_700 0x07000000 +#define XS_DID_GPL_800 0x08000000 +#define XS_DID_GPL_900 0x09000000 +#define XS_DID_GPL_IMU 0x01000000 +#define XS_DID_GPL_VRU 0x02000000 +#define XS_DID_GPL_AHRS 0x03000000 +#define XS_DID_GPL_AWINDA2 0x01000000 +#define XS_DID_GPL_AHRSGNSS XS_DID_GPL_400 +#define XS_DID_GPL_AHRSGNSSG XS_DID_GPL_500 +#define XS_DID_GPL_GNSSINS XS_DID_GPL_600 +#define XS_DID_GPL_GNSSINSG XS_DID_GPL_700 +#define XS_DID_GPL_GNSSINSRTK XS_DID_GPL_800 + +#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) + +#define XS_DID_FULLTYPE_AWINDA2_STATION (XS_DID_GPL_AWINDA2 | XS_DID_TYPE_AWINDA_STATION) +#define XS_DID_FULLTYPE_AWINDA2_DONGLE (XS_DID_GPL_AWINDA2 | XS_DID_TYPE_AWINDA_DONGLE) +#define XS_DID_FULLTYPE_AWINDA2_OEM (XS_DID_GPL_AWINDA2 | XS_DID_TYPE_AWINDA_OEM) +#define XS_DID_FULLTYPE_SYNCSTATION2 (XS_DID_GPL_AWINDA2 | XS_DID_TYPE_SYNCSTATION) + +#define XS_DID_TYPE_MTX2 (XS_DID_TYPEH_MTX2_MTW2 | XS_DID_TYPEL_RS485XM) +#define XS_DID_TYPE_MTW2 (XS_DID_TYPEH_MTX2_MTW2 | XS_DID_TYPEL_WIRELESS) + +#define XS_DID_MK4TYPE_MT_1_MPU (XS_DID_TYPEH_MT_X_MPU | XS_DID_GPL_1) +#define XS_DID_MK4TYPE_MT_2_MPU (XS_DID_TYPEH_MT_X_MPU | XS_DID_GPL_2) +#define XS_DID_MK4TYPE_MT_3_MPU (XS_DID_TYPEH_MT_X_MPU | XS_DID_GPL_3) +#define XS_DID_MK4TYPE_MT_7_MPU (XS_DID_TYPEH_MT_X_MPU | XS_DID_GPL_7) +#define XS_DID_MK4TYPE_MT_8_MPU (XS_DID_TYPEH_MT_X_MPU | XS_DID_GPL_8) +#define XS_DID_MK4TYPE_MT_310 (XS_DID_TYPEH_MT_3X0 | XS_DID_GPL_1) +#define XS_DID_MK4TYPE_MT_320 (XS_DID_TYPEH_MT_3X0 | XS_DID_GPL_2) +#define XS_DID_MK4TYPE_MT_330 (XS_DID_TYPEH_MT_3X0 | XS_DID_GPL_3) +#define XS_DID_MK4TYPE_MT_10 (XS_DID_TYPEH_MT_X0 | XS_DID_GPL_10) +#define XS_DID_MK4TYPE_MT_20 (XS_DID_TYPEH_MT_X0 | XS_DID_GPL_20) +#define XS_DID_MK4TYPE_MT_30 (XS_DID_TYPEH_MT_X0 | XS_DID_GPL_30) +#define XS_DID_MK4TYPE_MT_100 (XS_DID_TYPEH_MT_X00 | XS_DID_GPL_100) +#define XS_DID_MK4TYPE_MT_200 (XS_DID_TYPEH_MT_X00 | XS_DID_GPL_200) +#define XS_DID_MK4TYPE_MT_300 (XS_DID_TYPEH_MT_X00 | XS_DID_GPL_300) +#define XS_DID_MK4TYPE_MT_400 (XS_DID_TYPEH_MT_X00 | XS_DID_GPL_400) +#define XS_DID_MK4TYPE_MT_500 (XS_DID_TYPEH_MT_X00 | XS_DID_GPL_500) +#define XS_DID_MK4TYPE_MT_600 (XS_DID_TYPEH_MT_X00 | XS_DID_GPL_600) +#define XS_DID_MK4TYPE_MT_700 (XS_DID_TYPEH_MT_X00 | XS_DID_GPL_700) +#define XS_DID_MK4TYPE_MT_800 (XS_DID_TYPEH_MT_X00 | XS_DID_GPL_800) +#define XS_DID_MK4TYPE_MT_900 (XS_DID_TYPEH_MT_X00 | XS_DID_GPL_900) + +#define XS_DID_MK5TYPE_MT_10 (XS_DID_TYPEH_MT_X0 | XS_DID_GPL_10 | XS_DID_TYPEL_MK5) +#define XS_DID_MK5TYPE_MT_20 (XS_DID_TYPEH_MT_X0 | XS_DID_GPL_20 | XS_DID_TYPEL_MK5) +#define XS_DID_MK5TYPE_MT_30 (XS_DID_TYPEH_MT_X0 | XS_DID_GPL_30 | XS_DID_TYPEL_MK5) +#define XS_DID_MK5TYPE_MT_100 (XS_DID_TYPEH_MT_X00 | XS_DID_GPL_100 | XS_DID_TYPEL_MK5) +#define XS_DID_MK5TYPE_MT_200 (XS_DID_TYPEH_MT_X00 | XS_DID_GPL_200 | XS_DID_TYPEL_MK5) +#define XS_DID_MK5TYPE_MT_300 (XS_DID_TYPEH_MT_X00 | XS_DID_GPL_300 | XS_DID_TYPEL_MK5) +#define XS_DID_MK5TYPE_MT_400 (XS_DID_TYPEH_MT_X00 | XS_DID_GPL_400 | XS_DID_TYPEL_MK5) +#define XS_DID_MK5TYPE_MT_500 (XS_DID_TYPEH_MT_X00 | XS_DID_GPL_500 | XS_DID_TYPEL_MK5) +#define XS_DID_MK5TYPE_MT_600 (XS_DID_TYPEH_MT_X00 | XS_DID_GPL_600 | XS_DID_TYPEL_MK5) +#define XS_DID_MK5TYPE_MT_700 (XS_DID_TYPEH_MT_X00 | XS_DID_GPL_700 | XS_DID_TYPEL_MK5) +#define XS_DID_MK5TYPE_MT_800 (XS_DID_TYPEH_MT_X00 | XS_DID_GPL_800 | XS_DID_TYPEL_MK5) +#define XS_DID_MK5TYPE_MT_900 (XS_DID_TYPEH_MT_X00 | XS_DID_GPL_900 | XS_DID_TYPEL_MK5) + +#define XS_DID_MASTER 0x00000000 +#define XS_DID_ABMCLOCKMASTER 0x00000100 +#define XS_DID_GLOVEMASTER (XS_DID_TYPEH_INTERNAL | 0x00000200) + +#define XS_DID_GLOVETYPE_UNKNOWN 0x00000000 +#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_LEFT ( XS_DID_GLOVEMASTER | XS_DID_GLOVETYPE_LEFT ) +#define XS_DID_GLOVEMASTER_RIGHT ( XS_DID_GLOVEMASTER | XS_DID_GLOVETYPE_RIGHT ) + +#define XS_DID_INTERNAL(did) ((did & XS_DID_TYPEH_MASK) == XS_DID_TYPEH_INTERNAL) +#define XS_DID_WM(did) ((did & XS_DID_TYPEH_MASK) == XS_DID_TYPEH_AWINDAMASTER) +#define XS_DID_NOWM(did) ((did & XS_DID_TYPEH_MASK) != XS_DID_TYPEH_AWINDAMASTER) +#define XS_DID_MTW2(did) ((did & XS_DID_TYPE_MASK) == XS_DID_TYPE_MTW2) +#define XS_DID_MTX2(did) ((did & XS_DID_TYPE_MASK) == XS_DID_TYPE_MTX2) + +#define XS_DID_AWINDA2_STATION(did) ((did & XS_DID_FULLTYPE_MASK) == XS_DID_FULLTYPE_AWINDA2_STATION) +#define XS_DID_AWINDA2_DONGLE(did) ((did & XS_DID_FULLTYPE_MASK) == XS_DID_FULLTYPE_AWINDA2_DONGLE) +#define XS_DID_AWINDA2_OEM(did) ((did & XS_DID_FULLTYPE_MASK) == XS_DID_FULLTYPE_AWINDA2_OEM) + +#define XS_DID_SYNCSTATION2(did) ((did & XS_DID_FULLTYPE_MASK) == XS_DID_FULLTYPE_SYNCSTATION2) +#define XS_DID_SYNCSTATION(did) (XS_DID_SYNCSTATION2(did)) + +#define XS_DID_AWINDA2(did) (XS_DID_AWINDA2_STATION(did) || XS_DID_AWINDA2_DONGLE(did) || XS_DID_AWINDA2_OEM(did)) + +#define XS_DID_BODYPACK(did) ((did & XS_DID_TYPEH_MASK) == XS_DID_TYPEH_BODYPACK) + +#define XS_DID_MK4TYPE_MT_710_RANGE_START (XS_DID_TYPEH_MT_X00 | XS_DID_GPL_700 | 0x1000) +#define XS_DID_MK5TYPE_RANGE_START 0x00002000 + +#define XS_DID64_BIT 0x0000000080000000ULL + +#endif diff --git a/extern/xspublic/xstypes/xsens_compat.h b/extern/xspublic/xstypes/xsens_compat.h new file mode 100644 index 0000000..f3f6f60 --- /dev/null +++ b/extern/xspublic/xstypes/xsens_compat.h @@ -0,0 +1,341 @@ + +// Copyright (c) 2003-2022 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 __XSENS_LINUX_H +#define __XSENS_LINUX_H + +/* #undef HAVE_STRTOK_S */ +/* #undef HAVE_STRTOK_R */ +/* #undef HAVE__WCSICMP */ +/* #undef HAVE_WCSICMP */ +/* #undef HAVE__WCSNICMP */ +/* #undef HAVE_WCSNICMP */ +#define HAVE_WCSNCASECMP 1 +#define HAVE_WCSCASECMP 1 +/* #undef HAVE_SPRINTF_S */ +/* #undef HAVE_STRCPY_S */ +/* #undef HAVE_STRCAT_S */ +/* #undef HAVE_STRNCPY_S */ +/* #undef HAVE_FOPEN_S */ +/* #undef HAVE_STRICMP */ +/* #undef HAVE__STRICMP */ +#define HAVE_STRCASECMP 1 +/* #undef HAVE_STRNICMP */ +/* #undef HAVE__STRNICMP */ +#define HAVE_STRNCASECMP 1 +/* #undef HAVE_WFOPEN */ +/* #undef HAVE__WFOPEN */ +/* #undef HAVE__STRDUP */ +#define HAVE_STRDUP 1 +/* #undef HAVE__WCSDUP */ +#define HAVE_WCSDUP 1 +/* #undef HAVE__FULLPATH */ +/* #undef HAVE_FULLPATH */ +#define HAVE_REALPATH 1 +/* #undef HAVE_WFULLPATH */ +/* #undef HAVE_WFREOPEN */ +/* #undef HAVE__UNLINK */ +#define HAVE_UNLINK 1 +/* #undef HAVE__ISNAN */ +#define HAVE_ISNAN 1 +/* #undef HAVE__FILENO */ +#define HAVE_FILENO 1 +/* #undef HAVE_VSNPRINTF_S */ +/* #undef HAVE_VSPRINTF_S */ +/* #undef HAVE___DEBUGBREAK */ + +#ifdef __GNUC__ + +#include "pstdint.h" +#include +#include +#include +#include +#include +#include +#include +#include + +#include "xstypedefs.h" + +#define LPSTR char * +#define _T(x) (x) + +#define _ASSERT assert +#define _ASSERTE assert +#define HMODULE void * + +#define LINUX_MBS_LENGTH 256 +#define LINUX_MBS_MODELENGTH 5 + +#ifdef __cplusplus +#define XSEXTC extern "C" +#else +#define XSEXTC +#endif + +#define __compat_always_deprecated(msg) __attribute__((deprecated(msg))) +#if 0 +#define __compat_deprecated(msg) __compat_always_deprecated(msg) +#else +#define __compat_deprecated(msg) +#endif +#define __compat_defdeprecated __compat_deprecated("compat should be nicely merged into xstypes") + +#ifndef HAVE_SPRINTF_S +XSEXTC int32_t sprintf_s(char *dest, uint32_t num, const char *fmt, ...) __compat_defdeprecated; +#endif + +#ifndef HAVE_STRCPY_S +XSEXTC int32_t strcpy_s(char *dest, uint32_t size, const char *src) __compat_defdeprecated; + +#ifdef __cplusplus +template +int32_t strcpy_s(char (&dest)[size], const char *src) __compat_defdeprecated; +template +int32_t strcpy_s(char (&dest)[size], const char *src) +{ + if (!src) return EINVAL; + if (size == 0 || size < strlen(src) + 1) return ERANGE; + + strcpy(dest, src); + + return 0; +} +#endif +#endif + +#ifndef HAVE_STRCAT_S +XSEXTC int32_t strcat_s(char *dest, uint32_t size, const char *src) __compat_defdeprecated; + +#ifdef __cplusplus +template +int32_t strncpy_s(char (&dest)[size], const char *src, uint32_t count) __compat_defdeprecated; +template +int32_t strncpy_s(char (&dest)[size], const char *src, uint32_t count) +{ + if (!src) return EINVAL; + size_t itemsToAdd = (count < strlen(src) ? count : strlen(src)); + if (size == 0 || size < itemsToAdd + 1) return ERANGE; + + strncpy(dest, src, count); + + return 0; +} +#endif +#endif + +#ifndef HAVE_STRNCPY_S +XSEXTC int32_t strncpy_s(char *dest, uint32_t size, const char *src, uint32_t count) __compat_defdeprecated; + +#ifdef __cplusplus +template +int32_t strcat_s(char (&dest)[size], const char *src) __compat_defdeprecated; +template +int32_t strcat_s(char (&dest)[size], const char *src) +{ + if (!src) return EINVAL; + if (size == 0 || size < strlen(src) + strlen(dest) + 1) return ERANGE; + + strcat (dest, src); + + return 0; +} +#endif +#endif + +#ifndef HAVE_FOPEN_S +XSEXTC int fopen_s(FILE **file, const char *filename, const char *mode) __compat_deprecated("Use XsFile instead"); +#endif + +#ifndef HAVE__STRICMP +#define _stricmp strcasecmp +#endif +#ifndef HAVE_STRICMP +#define stricmp strcasecmp +#endif +#ifndef HAVE__STRNICMP +#define _strnicmp strncasecmp +#endif +#ifndef HAVE_STRICMP +#define strnicmp strncasecmp +#endif + +/* not available on cygwin */ +#ifdef HAVE_WCSCASECMP +#define _wcsicmp wcscasecmp +#else +#define _wcsicmp(s1, s2) compat_wcsnicmp(s1, s2, 0x7fffffff) +XSEXTC int compat_wcsnicmp(const wchar_t* s1, const wchar_t* s2, size_t length) __compat_defdeprecated; +#endif + +/* not available on cygwin */ +#ifdef HAVE_WCSNCASECMP +#define _wcsnicmp wcsncasecmp +#else +#define _wcsnicmp(s1, s2, n) compat_wcsnicmp(s1, s2, n) +XSEXTC int compat_wcsnicmp(const wchar_t* s1, const wchar_t* s2, size_t length) __compat_defdeprecated; +#endif + +#define _wfopen wfopen +#ifndef HAVE_WFOPEN +#define wfopen compat_wfopen +XSEXTC FILE *compat_wfopen(const wchar_t *filename, const wchar_t *mode) __compat_deprecated("Use XsFile instead"); +#endif + +#define _wfreopen wfreopen +#ifndef HAVE_WFREOPEN +#define wfreopen compat_wfreopen +XSEXTC FILE *compat_wfreopen(const wchar_t *path, const wchar_t *mode, FILE *) __compat_deprecated("Use XsFile instead"); +#endif + +#define _set_printf_count_output(x) + +#ifndef HAVE__STRDUP +#define _strdup strdup +#endif + +#ifndef HAVE__WCSDUP +#ifdef HAVE_WCSDUP +#define _wcsdup wcsdup +#else +#define _wcsdup compat_wcsdup +#endif +#endif +XSEXTC wchar_t *compat_wcsdup(const wchar_t *data) __compat_defdeprecated; + +#ifndef HAVE_FULLPATH +#define fullpath(src,dest,len) realpath(src,dest) +#endif +#ifndef HAVE__FULLPATH +#define _fullpath fullpath +#endif + +#ifndef HAVE__WFULLPATH +#define _wfullpath compat_wfullpath +XSEXTC int32_t compat_wfullpath(wchar_t *resolvedPath, const wchar_t *path, size_t length) __compat_deprecated("Use Xsfile_fullPath instead"); +#endif + +#ifndef HAVE_STRTOK_S +XSEXTC char *strtok_s(char *token, const char *delim, char **context) __compat_defdeprecated; +#endif + +#ifndef _finite +#define _finite compat_finite +XSEXTC int compat_finite(XsReal x) __compat_deprecated("Use XsMath_isFinite instead"); +#endif + +#define LOBYTE(w) ((uint8_t)((w) & 0xff)) +#define HIBYTE(w) ((uint8_t)(((w) >> 8) & 0xff)) + +#define __assume(val) + +#if !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 +#endif + +#ifndef HAVE__UNLINK +#define _unlink unlink +#endif +#ifndef HAVE___DEBUGBREAK +#define __debugbreak() +#endif +#ifndef HAVE__FILENO +#define _fileno fileno +#endif +#ifndef HAVE__STRDUP +#define _strdup strdup +#endif +#ifndef HAVE__ISNAN +#define _isnan isnan +#endif + +#ifndef GetRValue +/* The others will most likely not be defined either */ +#define GetRValue(rgb) ((uint8_t)(rgb)) +#define GetGValue(rgb) ((uint8_t)(((uint32_t)(rgb)) >> 8)) +#define GetBValue(rgb) ((uint8_t)(((uint32_t)(rgb)) >> 16)) +#endif + +typedef pthread_mutex_t CRITICAL_SECTION __compat_deprecated("Use XsMutex instead"); + +static inline void InitializeCriticalSection(CRITICAL_SECTION *cs) __compat_deprecated("Use XsMutex instead"); +static inline void DeleteCriticalSection(CRITICAL_SECTION *cs) __compat_deprecated("Use XsMutex instead"); +static inline void EnterCriticalSection(CRITICAL_SECTION *cs) __compat_deprecated("Use XsMutex instead"); +static inline int TryEnterCriticalSection(CRITICAL_SECTION *cs) __compat_deprecated("Use XsMutex instead"); +static inline void LeaveCriticalSection(CRITICAL_SECTION *cs) __compat_deprecated("Use XsMutex instead"); + +static inline void InitializeCriticalSection(CRITICAL_SECTION *cs) { pthread_mutex_init(cs, 0); } +static inline void DeleteCriticalSection(CRITICAL_SECTION *cs) { pthread_mutex_destroy(cs); } +static inline void EnterCriticalSection(CRITICAL_SECTION *cs) { pthread_mutex_lock(cs); } +static inline int TryEnterCriticalSection(CRITICAL_SECTION *cs) { return (pthread_mutex_trylock(cs) == 0); } +static inline void LeaveCriticalSection(CRITICAL_SECTION *cs) { pthread_mutex_unlock(cs); } + +#ifndef DWORD +# define DWORD uint32_t +#endif + +XSEXTC int IsBadWritePtr(void *p, size_t size); + +#ifndef HAVE_VSNPRINTF_S +#define _TRUNCATE 1 +#define vsnprintf_s(str, size, thingy, fmt, vp) vsnprintf(str, size, fmt, vp) +#endif +#ifndef HAVE_VSPRINTF_S +#define vsprintf_s(str, size, format, args) vsprintf(str, format, args) +#endif + +// MRO (10/24/2013): Linux compiler does not support override keyword yet +// \todo Remove override define when linux compiler supports it +#ifdef __GNUC__ +#if (__GNUC__ >= 4) && (__GNUC_MINOR__ >= 7) + // override keyword supported +#else + #define override +#endif +#endif + +#endif // __GNUC__ +#endif // __XSENS_LINUX_H diff --git a/extern/xspublic/xstypes/xseuler.c b/extern/xspublic/xstypes/xseuler.c new file mode 100644 index 0000000..2a91a10 --- /dev/null +++ b/extern/xspublic/xstypes/xseuler.c @@ -0,0 +1,90 @@ + +// Copyright (c) 2003-2022 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 "xseuler.h" +#include "xsquaternion.h" +#include +#include "xsfloatmath.h" + +/*! \class XsEuler + \brief Contains Euler Angle data and conversion from Quaternion + \details Euler Angles are computed as a rotation around the x-axis, followed by a rotation + around the y-axis, followed by a rotation around the z-axis. + In aerospace terms, x,y and z are also often referred to as roll pitch and yaw, so those names + are also available as a convenience. +*/ + +/*! \addtogroup cinterface C Interface + @{ +*/ + +/*! \relates XsEuler + \brief Clears all angles in the XsEuler object by setting them to 0 +*/ +void XsEuler_destruct(XsEuler* thisPtr) +{ + thisPtr->m_x = XsMath_zero; + thisPtr->m_y = XsMath_zero; + thisPtr->m_z = XsMath_zero; +} + +/*! \relates XsEuler + \brief Returns true if all angles in this object are zero +*/ +int XsEuler_empty(const XsEuler* thisPtr) +{ + return thisPtr->m_x == XsMath_zero && thisPtr->m_y == XsMath_zero && thisPtr->m_z == XsMath_zero; +} + +/*! \relates XsEuler + \brief Get an euler angle representation of the quaternion. +*/ +void XsEuler_fromQuaternion(XsEuler* thisPtr, const XsQuaternion* quat) +{ + XsReal sqw, dphi, dpsi; + + if (XsQuaternion_empty(quat)) + { + XsEuler_destruct(thisPtr); + return; + } + + sqw = quat->m_w * quat->m_w; + 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)); +} + +/*! @} */ diff --git a/extern/xspublic/xstypes/xseuler.h b/extern/xspublic/xstypes/xseuler.h new file mode 100644 index 0000000..72abcca --- /dev/null +++ b/extern/xspublic/xstypes/xseuler.h @@ -0,0 +1,197 @@ + +// Copyright (c) 2003-2022 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 XSEULER_H +#define XSEULER_H + +#include "xsmath.h" +#include + +#ifdef __cplusplus +#include "xsvector.h" +extern "C" { +#endif +#ifndef __cplusplus +#define XSEULER_INITIALIZER { { { XsMath_zero,XsMath_zero,XsMath_zero } } } +struct XsVector; +#endif + +struct XsEuler; +struct XsQuaternion; +struct XsMatrix; + +XSTYPES_DLL_API void XsEuler_destruct(struct XsEuler* thisPtr); +XSTYPES_DLL_API int XsEuler_empty(const struct XsEuler* thisPtr); +XSTYPES_DLL_API void XsEuler_fromQuaternion(struct XsEuler* thisPtr, const struct XsQuaternion* quat); + +#ifdef __cplusplus +} // extern "C" +#endif + +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) {} + //! \brief Constructor that creates an Euler object the specified angles + inline XsEuler(XsReal x_, XsReal y_, XsReal z_) : m_x(x_), m_y(y_), m_z(z_) {} + //! \brief Constructor that creates an Euler object from \a other + inline XsEuler(const XsEuler& other) : m_x(other.m_x), m_y(other.m_y), m_z(other.m_z) {} + //! \brief Constructor that creates an Euler object from \a other + inline explicit XsEuler(const XsVector& other) : m_x(other[0]), m_y(other[1]), m_z(other[2]) {} + + //! \brief \copybrief XsEuler_fromQuaternion + inline explicit XsEuler(const XsQuaternion& q) + { + XsEuler_fromQuaternion(this, &q); + } + + //! \brief Assigns the \a other XsEuler object to this one + inline XsEuler& operator=(const XsEuler& other) + { + m_x = other.m_x; + m_y = other.m_y; + m_z = other.m_z; + return *this; + } + + //! \brief Returns the \a index'th euler angle in the object + inline XsReal operator[](XsSize index) const + { + 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) + { + assert(index <= 2); + return m_data[index]; + } + + //! \brief Returns true if all angles in this object are zero + inline bool empty() const + { + return m_x == XsMath_zero && m_y == XsMath_zero && m_z == XsMath_zero; + } + + //! \brief Return a const pointer to the internal data + inline const XsReal* data() const + { + return m_data; + } + + //! \brief \copybrief XsEuler_fromQuaternion + inline XsEuler& fromQuaternion(const XsQuaternion& quat) + { + XsEuler_fromQuaternion(this, &quat); + return *this; + } + + /*! \brief Returns true if the values in \a other are exactly equal to this + */ + inline bool operator == (const XsEuler& other) const + { + return m_roll == other.m_roll && m_pitch == other.m_pitch && m_yaw == other.m_yaw; + } + + /*! \brief Returns true if the values in \a other are different from this + */ + inline bool operator != (const XsEuler& other) const + { + return m_roll != other.m_roll || m_pitch != other.m_pitch || m_yaw != other.m_yaw; + } + + //! \brief Returns the roll or x value + inline XsReal roll() const + { + return m_roll; + } + //! \brief Returns the pitch or y value + inline XsReal pitch() const + { + return m_pitch; + } + //! \brief Returns the yaw or z value + inline XsReal yaw() const + { + return m_yaw; + } + + //! \brief Returns the x or roll value + inline XsReal x() const + { + return m_x; + } + //! \brief Returns the y or pitch value + inline XsReal y() const + { + return m_y; + } + //! \brief Returns the z or yaw value + 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; + } + +private: +#endif + + 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 + { + 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 + }; + XsReal m_data[3]; //!< Stores the euler triplet in an array of three elements + }; +}; + +typedef struct XsEuler XsEuler; + +#endif diff --git a/extern/xspublic/xstypes/xsexception.h b/extern/xspublic/xstypes/xsexception.h new file mode 100644 index 0000000..618a894 --- /dev/null +++ b/extern/xspublic/xstypes/xsexception.h @@ -0,0 +1,134 @@ + +// Copyright (c) 2003-2022 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. +// + +#ifdef __cplusplus +#ifndef XSEXCEPTION_H +#define XSEXCEPTION_H + +#ifndef XSENS_NO_EXCEPTIONS + +#include +#include "xsresultvalue.h" +#include "xsstring.h" + +/*! \brief Exception class for Xsens public libraries. Inherits from std::exception +*/ +class XsException : public std::exception +{ +public: + //! \brief Copy constructor + XsException(XsException const& e) + : std::exception() + , m_code(e.m_code) + , m_description(e.m_description) + { + } + + /*! \brief Initializing constructor + \details This constructor uses the value in \a err and the supplied \a description to create a full + text for when the user requests what() or text() + \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. + */ + XsException(XsResultValue err, XsString const& description) + : std::exception() + , m_code(err) + , m_description(description) + { + } + + /*! \brief Initializing constructor + \param description A description of the error. + */ + explicit XsException(XsString const& description) + : std::exception() + , m_code(XRV_ERROR) + , m_description(description) + { + } + + //! \brief Destructor + virtual ~XsException() noexcept + { + } + + //! \brief Assignment operator, copies \a e to this + XsException& operator = (XsException const& e) + { + if (this != &e) + { + m_code = e.m_code; + m_description = e.m_description; + } + return *this; + } + + //! \brief Returns the error value supplied during construction + inline XsResultValue code() const noexcept + { + return m_code; + } + + //! \brief Returns a description of the error that occurred as a char const* + inline char const* what() const noexcept + { + return m_description.c_str(); + } + + //! \brief Returns a description of the error that occurred as a XsString + inline XsString const& text() const noexcept + { + return m_description; + } + +private: + XsResultValue m_code; //!< The supplied error code + 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 diff --git a/extern/xspublic/xstypes/xsfile.c b/extern/xspublic/xstypes/xsfile.c new file mode 100644 index 0000000..eeff096 --- /dev/null +++ b/extern/xspublic/xstypes/xsfile.c @@ -0,0 +1,584 @@ + +// Copyright (c) 2003-2022 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 "xsfile.h" +#include "xsstring.h" +#include +#ifndef _WIN32 + #include // close + #include // ioctl + #include // open, O_RDWR + #include // strcpy + #include + #include + #include + #include +#else + #include + #include + #include +#endif + +// helper +static FILE* openFile(const struct XsString* filename, const struct XsString* mode); + +/*! \class XsFile + \brief Encapsulates a file, providing a platform independent interface +*/ +/*! \addtogroup cinterface C Interface + @{ +*/ + + +/*! \relates XsFile + \brief Frees the resources of this object by closing the file if it is open. +*/ +void XsFile_destruct(struct XsFile* thisPtr) +{ + if (thisPtr->m_handle != NULL) + (void)XsFile_close(thisPtr); +} + +/*! \relates XsFile + \brief Creates a new binary file with name \a filename, contents of existing files will be discarded + \param filename name of the file to create + \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) +{ + XsString mode; + XsString_construct(&mode); + XsString_resize(&mode, 16); + + if (thisPtr->m_handle != NULL) + return XRV_ALREADYOPEN; + +#ifdef _WIN32 + wchar_t filenameW[XS_MAX_FILENAME_LENGTH]; + // actively delete the file first to ensure that the file creation time is properly set + XsString_copyToWCharArray(filename, filenameW, XS_MAX_FILENAME_LENGTH); + (void) _wunlink(filenameW); // don't care about return value +#else + unlink(filename->m_data); +#endif + + if (writeOnly) + { + XsString_assign(&mode, 3, "wb"); + thisPtr->m_handle = openFile(filename, &mode); + } + else + { + XsString_assign(&mode, 4, "w+b"); + thisPtr->m_handle = openFile(filename, &mode); + } + + XsString_destruct(&mode); + if (thisPtr->m_handle == NULL) + return XRV_OUTPUTCANNOTBEOPENED; + else + return XRV_OK; +} + +/*! \relates XsFile + \brief Creates a new text file with name \a filename, contents of existing files will be discarded + \param filename name of the file to create + \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) +{ + XsString mode; + XsString_construct(&mode); + XsString_resize(&mode, 16); + + if (thisPtr->m_handle != NULL) + return XRV_ALREADYOPEN; + + if (writeOnly) + { + XsString_assign(&mode, 3, "wt"); + thisPtr->m_handle = openFile(filename, &mode); + } + else + { + XsString_assign(&mode, 4, "w+t"); + thisPtr->m_handle = openFile(filename, &mode); + } + + XsString_destruct(&mode); + if (thisPtr->m_handle == NULL) + return XRV_OUTPUTCANNOTBEOPENED; + else + return XRV_OK; +} + +/*! \relates XsFile + \brief Opens an existing binary file with name \a filename + \param filename name of the file to open + \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) +{ + XsString mode; + XsString_construct(&mode); + XsString_resize(&mode, 16); + + if (thisPtr->m_handle != NULL) + return XRV_ALREADYOPEN; + + if (readOnly) + XsString_assign(&mode, 3, "rb"); + else + XsString_assign(&mode, 4, "r+b"); + thisPtr->m_handle = openFile(filename, &mode); + + XsString_destruct(&mode); + if (thisPtr->m_handle == NULL) + { + if (readOnly) + return XRV_INPUTCANNOTBEOPENED; + else + return XRV_OUTPUTCANNOTBEOPENED; + } + else + return XRV_OK; +} + +/*! \relates XsFile + \brief Opens an existing binary file with name \a filename + \param filename name of the file to open + \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) +{ + XsString mode; + XsString_construct(&mode); + XsString_resize(&mode, 16); + + if (thisPtr->m_handle != NULL) + return XRV_ALREADYOPEN; + + if (readOnly) + { + XsString_assign(&mode, 3, "rt"); + thisPtr->m_handle = openFile(filename, &mode); + } + else + { + XsString_assign(&mode, 4, "r+t"); + thisPtr->m_handle = openFile(filename, &mode); + } + + XsString_destruct(&mode); + if (thisPtr->m_handle == NULL) + return XRV_OUTPUTCANNOTBEOPENED; + else + return XRV_OK; +} + +/*! \brief Helper for file opening */ +static FILE* openFile(const struct XsString* filename, const struct XsString* mode) +{ +#ifdef _WIN32 + wchar_t filenameW[XS_MAX_FILENAME_LENGTH]; + wchar_t modeW[16]; + (void)XsString_copyToWCharArray(filename, filenameW, XS_MAX_FILENAME_LENGTH); + (void)XsString_copyToWCharArray(mode, modeW, 16); + + __try + { + return _wfopen(filenameW, modeW); + } + __except (EXCEPTION_EXECUTE_HANDLER) + { + return NULL; + } +#else + return fopen(filename->m_data, mode->m_data); +#endif +} + +/*! \relates XsFile + \brief Reopens a file + \param filename Name of the file to open after the current one has been closed + \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) +{ +#ifdef _WIN32 + wchar_t filenameW[XS_MAX_FILENAME_LENGTH]; + wchar_t modeW[16]; + + (void)XsString_copyToWCharArray(filename, filenameW, XS_MAX_FILENAME_LENGTH); + (void)XsString_copyToWCharArray(mode, modeW, 16); + + thisPtr->m_handle = _wfreopen(filenameW, modeW, thisPtr->m_handle); +#else + thisPtr->m_handle = freopen(filename->m_data, mode->m_data, thisPtr->m_handle); +#endif + + if (thisPtr->m_handle == NULL) + return XRV_OUTPUTCANNOTBEOPENED; + else + return XRV_OK; +} + +/*! \relates XsFile + \brief Checks if a file is open + \returns 0 if a file is open, another value otherwise +*/ +int XsFile_isOpen(const struct XsFile* thisPtr) +{ + return (thisPtr->m_handle != NULL) ? 0 : 1; +} + +/*! \relates XsFile + \brief Checks if the file exists (can be accessed) + \param filename Name of the file to check for existence + \returns 0 if the file exists, another value otherwise +*/ +int XsFile_exists(const struct XsString* filename) +{ +#ifdef _WIN32 + wchar_t filenameW[XS_MAX_FILENAME_LENGTH]; + struct _stat buffer; + (void)XsString_copyToWCharArray(filename, filenameW, XS_MAX_FILENAME_LENGTH); + return _wstat(filenameW, &buffer); +#else + struct stat buffer; + return stat(filename->m_data, &buffer); +#endif +} + +/*! \relates XsFile + \brief Closes the file + \returns XRV_OK if the file was closed properly, XRV_ENDOFFILE otherwise +*/ +XsResultValue XsFile_close(struct XsFile* thisPtr) +{ + int rv; + + if (thisPtr->m_handle == NULL) + return XRV_NOFILEOPEN; + + fflush(thisPtr->m_handle); // always try to flush first, result is irrelevant + rv = fclose(thisPtr->m_handle); + + // Or keep the handle in case of failure? + thisPtr->m_handle = NULL; + + if (rv == EOF) + return XRV_ENDOFFILE; + else + return XRV_OK; +} + + +/*! \relates XsFile + \brief Writes unwritten data to the file + \returns XRV_OK if the flushing was successful, an XRV_ERROR otherwise + +*/ +XsResultValue XsFile_flush(struct XsFile* thisPtr) +{ + return fflush(thisPtr->m_handle) ? XRV_ERROR : XRV_OK; +} + +/*! \relates XsFile + \brief Reduces the file to a maximum size of \a fileSize bytes + \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) +{ + return XsFile_resize(thisPtr, fileSize); +} + +/*! \relates XsFile + \brief Resizes the file to \a fileSize bytes + \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) +{ +#ifdef _WIN32 + int rv = _chsize_s(_fileno(thisPtr->m_handle), fileSize); +#else + int rv = ftruncate(fileno(thisPtr->m_handle), fileSize); +#endif + if (rv != 0) + { + 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; + } + } + else + return XRV_OK; +} + +/*! \relates XsFile + \brief Deletes a file with name \a filename + \param filename Name of the file to delete + \returns XRV_OK if the file was erased, an error otherwise +*/ +XsResultValue XsFile_erase(const struct XsString* filename) +{ +#ifdef _WIN32 + wchar_t filenameW[XS_MAX_FILENAME_LENGTH]; + (void)XsString_copyToWCharArray(filename, filenameW, XS_MAX_FILENAME_LENGTH); + if (_wunlink(filenameW) != 0) +#else + if (unlink(filename->m_data) != 0) +#endif + { + switch (errno) + { + case EACCES: + return XRV_READONLY; + case ENOENT: + return XRV_NOTFOUND; + default: + return XRV_ERROR; + } + } + else + return XRV_OK; +} + +/*! \relates XsFile + \brief Reads a number of elements from a file + \param destination Buffer to store the read data in + \param size Size of each individual element to read + \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) +{ + return (XsFilePos) fread(destination, (size_t) size, (size_t) count, thisPtr->m_handle); +} + +/*! \relates XsFile + \brief Writes a number of elements to a file + \param source Buffer that contains the elements to be written + \param size Size of each individual element to write + \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) +{ + return (XsFilePos) fwrite(source, (size_t) size, (size_t) count, thisPtr->m_handle); +} + +/*! \relates XsFile + \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) +{ + return fgetc(thisPtr->m_handle); +} + +/*! \relates XsFile + \brief Writes a character to the file + \param character The caharacter to write + \returns XRV_OK on success, XRV_ERROR otehrwise +*/ +XsResultValue XsFile_putc(struct XsFile* thisPtr, int character) +{ + return fputc(character, thisPtr->m_handle) == EOF ? XRV_ERROR : XRV_OK; +} + +/*! \relates XsFile + \brief Reads characters from this file and stores them into str until (num-1) characters have been read or either a newline or the end-of-file is reached, whichever happens first. + \param str The destination for the read string + \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) +{ + return fgets(str, num, thisPtr->m_handle); +} + +/*! \relates XsFile + \brief Writes a null terminated c-string to the file + \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) +{ + return fputs(str, thisPtr->m_handle) != EOF ? XRV_OK : XRV_ERROR; +} + +/*! \relates XsFile + \brief Moves the current file position relative to the start of the file + \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) +{ +#ifdef _WIN32 + return _fseeki64(thisPtr->m_handle, offset, SEEK_SET) ? XRV_ERROR : XRV_OK; +#else + return fseeko(thisPtr->m_handle, offset, SEEK_SET) ? XRV_ERROR : XRV_OK; +#endif +} + +/*! \relates XsFile + \brief Moves the current file position relative to the end of the file + \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) +{ +#ifdef _WIN32 + return _fseeki64(thisPtr->m_handle, offset, SEEK_END) ? XRV_ERROR : XRV_OK; +#else + return fseeko(thisPtr->m_handle, offset, SEEK_END) ? XRV_ERROR : XRV_OK; +#endif +} + +/*! \relates XsFile + \brief Returns the current position in the file + \returns Current position in the current file +*/ +XsFilePos XsFile_tell(struct XsFile const* thisPtr) +{ +#ifdef _WIN32 + return _ftelli64(thisPtr->m_handle); +#else + return ftello(thisPtr->m_handle); +#endif +} + +/*! \relates XsFile + \returns Returns 0 if the current file position is not 'end of file', non 0 if the position is 'end of file' +*/ +int XsFile_eof(struct XsFile const* thisPtr) +{ + return feof(thisPtr->m_handle); +} + +/*! \relates XsFile + \returns Returns XRV_ERROR if the error flag for the file has been set, XRV_OK otherwise +*/ +XsResultValue XsFile_error(struct XsFile const* thisPtr) +{ + return ferror(thisPtr->m_handle) ? XRV_ERROR : XRV_OK; +} + +/*! \relates XsFile + \brief Retrieves the full path for a filename + \param[in] filename The filename to expand + \param[out] fullPath The filename with a fully expanded path is returned in this parameter. This parameter must be allocated by the caller. + \returns XRV_OK if the fullpath could be retrieved, XRV_NULLPTR if fullPath is NULL, XRV_ERROR otherwise +*/ +XsResultValue XsFile_fullPath(const struct XsString* filename, struct XsString* fullPath) +{ + XsResultValue result = XRV_OK; + if (fullPath == NULL) + result = XRV_NULLPTR; + else + { +#ifdef _WIN32 + wchar_t filenamew[XS_MAX_FILENAME_LENGTH]; + wchar_t fullpath[XS_MAX_FILENAME_LENGTH]; + (void)XsString_copyToWCharArray(filename, filenamew, XS_MAX_FILENAME_LENGTH); + + if (_wfullpath(fullpath, filenamew, XS_MAX_FILENAME_LENGTH) == NULL) + result = XRV_ERROR; + else + XsString_assignWCharArray(fullPath, fullpath); +#else + // 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]; + if (realpath(filename->m_data, fullpath) == NULL) + result = XRV_ERROR; + else + XsString_assignCharArray(fullPath, fullpath); +#endif + } + return result; +} + +/*! \relates XsFile + \brief Reads a full line from the file + \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) +{ + int b; + XsResultValue ok = XRV_ENDOFFILE; + + XsString_erase(line, 0, line->m_size); + XsArray_reserve(line, 256); + + b = fgetc(thisPtr->m_handle); + if (b != -1) + ok = XRV_OK; + + while (b != -1) + { + XsString_push_back(line, (char)b); + if (b == '\n') + break; + b = fgetc(thisPtr->m_handle); + } + return ok; +} + +/*! \relates XsFile + \returns The file handle +*/ +FILE* XsFile_handle(struct XsFile* thisPtr) +{ + return thisPtr->m_handle; +} + +/*! @} */ diff --git a/extern/xspublic/xstypes/xsfile.h b/extern/xspublic/xstypes/xsfile.h new file mode 100644 index 0000000..c336904 --- /dev/null +++ b/extern/xspublic/xstypes/xsfile.h @@ -0,0 +1,296 @@ + +// Copyright (c) 2003-2022 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 XSFILE_H +#define XSFILE_H + +#include "xstypesconfig.h" +#include "pstdint.h" +#include +#include "xsresultvalue.h" +#include "xsfilepos.h" +#include "xsstring.h" + +#ifdef _MSC_VER + #define XS_MAX_FILENAME_LENGTH 512 +#else + #define XS_MAX_FILENAME_LENGTH PATH_MAX +#endif + +#ifdef __cplusplus +#include "xsbytearray.h" +extern "C" { +#endif +#ifndef __cplusplus +#define XSFILE_INITIALIZER { 0 } +#endif + +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 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_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 XsResultValue XsFile_seek(struct XsFile* thisPtr, XsFilePos offset); +XSTYPES_DLL_API XsResultValue XsFile_seek_r(struct XsFile* thisPtr, XsFilePos offset); +XSTYPES_DLL_API XsFilePos XsFile_tell(struct XsFile const* thisPtr); + +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 FILE* XsFile_handle(struct XsFile* thisPtr); + +#ifdef __cplusplus +} // extern "C" +#endif + +struct XsFile +{ +#ifdef __cplusplus + /*! \brief Default constructor, creates an empty file object */ + explicit inline XsFile() + : m_handle(NULL) + { + // Silence -Wunused-private-field + // This field is used in the C back-end, but + // some compilers may not notice this + (void)m_handle; + } + + /*! \brief \copybrief XsFile_destruct */ + inline ~XsFile() + { + XsFile_destruct(this); + } + + /*! \brief \copybrief XsFile_create */ + 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) + { + return XsFile_createText(this, &filename, writeOnly ? 1 : 0); + } + + /*! \brief \copybrief XsFile_open */ + 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) + { + return XsFile_openText(this, &fileName, readOnly ? 1 : 0); + } + + /*! \brief \copybrief XsFile_openText */ + inline XsResultValue reopen(const XsString& fileName, const XsString& mode) + { + return XsFile_reopen(this, &fileName, &mode); + } + + /*! \brief \copybrief XsFile_isOpen */ + inline bool isOpen() const + { + return (XsFile_isOpen(this) == 0); + } + + /*! \brief \copybrief XsFile_exists */ + static inline bool exists(const XsString& fileName) + { + return (XsFile_exists(&fileName) == 0); + } + + /*! \brief \copybrief XsFile_close */ + inline XsResultValue close() + { + return XsFile_close(this); + } + + /*! \brief \copybrief XsFile_flush */ + inline XsResultValue flush() + { + return XsFile_flush(this); + } + + /*! \brief \copybrief XsFile_truncate */ + inline XsResultValue truncate(XsFilePos fileSize) + { + return XsFile_truncate(this, fileSize); + } + + /*! \brief \copybrief XsFile_resize */ + inline XsResultValue resize(XsFilePos fileSize) + { + return XsFile_resize(this, fileSize); + } + + /*! \brief \copybrief XsFile_erase */ + static XsResultValue erase(const XsString& filename) + { + return XsFile_erase(&filename); + } + + /*! \brief \copybrief XsFile_read */ + 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) + { + 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() + { + return XsFile_getc(this); + } + + /*! \brief \copybrief XsFile_putc */ + inline XsResultValue putc(int character) + { + return XsFile_putc(this, character); + } + + /*! \brief \copybrief XsFile_gets */ + inline char* gets(char* destination, int maxCount) + { + return XsFile_gets(this, destination, maxCount); + } + + /*! \brief \copybrief XsFile_puts */ + inline XsResultValue puts(const char* source) + { + return XsFile_puts(this, source); + } + + /*! \brief \copybrief XsFile_seek */ + inline XsResultValue seek(XsFilePos offset) + { + return XsFile_seek(this, offset); + } + + /*! \brief \copybrief XsFile_seek_r */ + inline XsResultValue seek_r(XsFilePos offset) + { + return XsFile_seek_r(this, offset); + } + + /*! \brief \copybrief XsFile_tell */ + inline XsFilePos tell() const + { + return XsFile_tell(this); + } + + /*! \brief \copybrief XsFile_eof */ + inline bool eof() const + { + return (0 != XsFile_eof(this)); + } + + /*! \brief \copybrief XsFile_error */ + inline XsResultValue error() const + { + return XsFile_error(this); + } + + /*! \brief \copybrief XsFile_fullPath */ + static XsResultValue fullPath(const XsString& filename, XsString& fullPath) + { + return XsFile_fullPath(&filename, &fullPath); + } + + /*! \brief \copybrief XsFile_getline */ + inline XsResultValue getline(XsString& line) + { + return XsFile_getline(this, &line); + } + + /*! \brief \copybrief XsFile_getline */ + inline XsResultValue getline(std::string& line) + { + XsString tmp; + XsResultValue rv = XsFile_getline(this, &tmp); + if (rv == XRV_OK) + line = tmp.toStdString(); + return rv; + } + + /*! \brief \copybrief XsFile_handle */ + inline FILE* handle() + { + return XsFile_handle(this); + } +private: +#endif + FILE* m_handle; +}; + +typedef struct XsFile XsFile; + +#endif diff --git a/extern/xspublic/xstypes/xsfilepos.h b/extern/xspublic/xstypes/xsfilepos.h new file mode 100644 index 0000000..c3ddfd3 --- /dev/null +++ b/extern/xspublic/xstypes/xsfilepos.h @@ -0,0 +1,76 @@ + +// Copyright (c) 2003-2022 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 XSFILEPOS_H +#define XSFILEPOS_H + +/*! \addtogroup cinterface C Interface + @{ +*/ + +/*! \typedef XsFilePos + \brief The type that is used for positioning inside a file +*/ +/*! \typedef XsIoHandle + \brief The type that is used for low-level identification of an open I/O device +*/ +/*! \typedef XsFileHandle + \brief The type that is used for low-level identification of an open file +*/ + +/*! @} */ + +#include +#ifdef _WIN32 + #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; +#endif +typedef FILE XsFileHandle; + +#endif diff --git a/extern/xspublic/xstypes/xsfilterprofile.c b/extern/xspublic/xstypes/xsfilterprofile.c new file mode 100644 index 0000000..df35952 --- /dev/null +++ b/extern/xspublic/xstypes/xsfilterprofile.c @@ -0,0 +1,74 @@ + +// Copyright (c) 2003-2022 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 "xsfilterprofile.h" +#include "xsstring.h" +#include + +/*! \class XsFilterProfile + \brief Contains information about an available filter profile +*/ +/*! \addtogroup cinterface C Interface + @{ +*/ + +/*! \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) +{ + 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); +} + +/*! \brief Checks if the filter profile is empty + \returns true if the filter profile is empty +*/ +int XsFilterProfile_empty(XsFilterProfile const* thisPtr) +{ + return thisPtr->m_type == 0 && thisPtr->m_label[0] == 0; +} + +/*! \brief Swap the contents of \a a with \a b + \param a Pointer to the first XsFilterProfile + \param b Pointer to the second XsFilterProfile +*/ +void XsFilterProfile_swap(XsFilterProfile* a, XsFilterProfile* b) +{ + XsFilterProfile tmp; + memcpy(&tmp, a, sizeof(XsFilterProfile)); + memcpy(a, b, sizeof(XsFilterProfile)); + memcpy(b, &tmp, sizeof(XsFilterProfile)); +} + +/*! @} */ diff --git a/extern/xspublic/xstypes/xsfilterprofile.h b/extern/xspublic/xstypes/xsfilterprofile.h new file mode 100644 index 0000000..c50450f --- /dev/null +++ b/extern/xspublic/xstypes/xsfilterprofile.h @@ -0,0 +1,270 @@ + +// Copyright (c) 2003-2022 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 XSFILTERPROFILE_H +#define XSFILTERPROFILE_H + +#include "xstypesconfig.h" +#include "xsstring.h" +#include "pstdint.h" +#include "xsfilterprofilekind.h" + +#ifdef __cplusplus +extern "C" { +#else +#define XSFILTERPROFILE_INITIALIZER {0, 0, {0}, 0, 0, 0} +#endif + +struct XsFilterProfile; +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 +} +#endif + +#define XS_MAX_FILTERPROFILES 254 +#define XS_LEN_FILTERPROFILELABEL_TERM (20+1) +#define XS_LEN_FILTERPROFILEKIND_TERM (20+1) +#define XS_MAX_FILTERPROFILES_IN_MT 5 + +struct XsFilterProfile +{ +#ifdef __cplusplus + /*! \brief Construct a filter profile object + \param type_ the profile type + \param version_ the profile version + \param kind_ the kind of profile + \param label_ the profile name + \param filterType_ the filter type this profile is for + \param filterMajor_ the major version of the compatible filter + \param filterMinor_ the minor version of the compatible filter + */ + explicit XsFilterProfile(uint8_t type_ = 0, uint8_t version_ = 0, const char* kind_ = nullptr, const char* label_ = nullptr, char filterType_ = 0, uint8_t filterMajor_ = 0, uint8_t filterMinor_ = 0) + : m_type(type_) + , m_version(version_) + , m_filterType(filterType_) + , m_filterMajor(filterMajor_) + , m_filterMinor(filterMinor_) + { + setKind(kind_); + setLabel(label_); + } + + /*! \brief Copy constructor for a filter profile object + \param other the filter profile object to construct a copy of + */ + XsFilterProfile(const XsFilterProfile& other) + : m_type(other.m_type) + , m_version(other.m_version) + , m_filterType(other.m_filterType) + , m_filterMajor(other.m_filterMajor) + , m_filterMinor(other.m_filterMinor) + { + setKind(other.m_kind); + setLabel(other.m_label); + } + + /*! \brief Destroy the filter profile */ + ~XsFilterProfile() {} + + /*! \brief \copybrief XsFilterProfile_empty + \returns true if the filter profile is empty + \sa XsFilterProfile_empty + */ + inline bool empty() + { + return (0 != XsFilterProfile_empty(this)); + } + + /*! \brief \copybrief XsFilterProfile_toString + \returns a string representation of this filter profile + \sa XsFilterProfile_toString + */ + inline XsString toString() + { + XsString out; + XsFilterProfile_toString(this, &out); + return out; + } + + /*! \brief The filter profile type */ + inline uint8_t type() const + { + return m_type; + } + + /*! \brief The filter profile version */ + inline uint8_t version() const + { + return m_version; + } + + /*! \brief The filter profile name */ + inline const char* label() const + { + return m_label; + } + + /*! \brief The filter profile 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; + } + + /*! \brief The major version of the compatible filter */ + inline uint8_t filterMajor() const + { + return m_filterMajor; + } + + /*! \brief The minor version of the compatible filter */ + 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 + */ + inline void setType(uint8_t type_) + { + m_type = type_; + } + + /*! \brief Set the version of the filter profile to \a version_ + \param version_ the new label of the filter profile + */ + inline void setVersion(uint8_t version_) + { + m_version = version_; + } + + /*! \brief Set the label of the filter profile \a label_ + \param label_ the new label of the filter profile + */ + 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) + { + if (label_[i] == '\0' || label_[i] == ' ') + break; + m_label[i] = label_[i]; + } + m_label[i] = 0; + } + } + + /*! \brief Set the kind of filter profile \a kind_ + \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) + { + if (kind_[i] == '\0' || kind_[i] == ' ') + break; + m_kind[i] = kind_[i]; + } + m_kind[i] = 0; + } + } + + /*! \brief Set the filter type of this filter profile to \a filterType_ + \param filterType_ the new filter type + */ + inline void setFilterType(char filterType_) + { + m_filterType = filterType_; + } + + /*! \brief Set the filter version of this filter profile to \a major_, \a minor_ + \param major_ the major version number + \param minor_ the minor version number + */ + inline void setFilterVersion(uint8_t major_, uint8_t minor_) + { + m_filterMajor = major_; + m_filterMinor = minor_; + } + + /*! \brief Swap the contents with \a other + */ + inline void swap(XsFilterProfile& other) + { + XsFilterProfile_swap(this, &other); + } + + /*! \brief Swap the contents of \a first with \a second + */ + friend void swap(XsFilterProfile& first, XsFilterProfile& second) + { + first.swap(second); + } + + /*! \brief Return true if the filter profile type and version are identical to those of \a other */ + inline bool operator == (const XsFilterProfile& other) const + { + return m_filterMajor == other.m_filterMajor && m_filterMinor == other.m_filterMinor; + } + +protected: +#endif + + uint8_t m_type; //!< The type of the filter profile. When set to 255 in an operation, the 'current' filter profile is used. + uint8_t m_version; //!< The version of the filter profile. + char m_kind[XS_LEN_FILTERPROFILEKIND_TERM]; //!< The kind of filter profile. + char m_label[1 + 2 * XS_LEN_FILTERPROFILELABEL_TERM]; //!< The label of the filter profile (Can be 2 names, including separator) + char m_filterType; //!< The type of the XKF filter this filter profile is intended for '3': XKF-3, '6': XKF-6. \note The value is a character, so XKF-3 is '3', which is hex 0x33 + uint8_t m_filterMajor; //!< The major version of the XKF filter this filter profile is intended for + uint8_t m_filterMinor; //!< The minor version of the XKF filter this filter profile is intended for +}; + +typedef struct XsFilterProfile XsFilterProfile; + +#endif diff --git a/extern/xspublic/xstypes/xsfilterprofilearray.c b/extern/xspublic/xstypes/xsfilterprofilearray.c new file mode 100644 index 0000000..50a550c --- /dev/null +++ b/extern/xspublic/xstypes/xsfilterprofilearray.c @@ -0,0 +1,103 @@ + +// Copyright (c) 2003-2022 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 "xsfilterprofilearray.h" +#include "xsfilterprofile.h" +#include + +/*! \struct XsFilterProfileArray + \brief A list of XsFilterProfile values + \sa XsArray +*/ + +/*! \copydoc XsArrayDescriptor::itemSwap + \note Specialization for XsFilterProfile +*/ +void swapFilterProfile(XsFilterProfile* a, XsFilterProfile* b) +{ + XsFilterProfile tmp = *a; + *a = *b; + *b = tmp; +} + +/*! \copydoc XsArrayDescriptor::itemCopy + \note Specialization for XsFilterProfile +*/ +void copyFilterProfile(XsFilterProfile* to, XsFilterProfile const* from) +{ + *to = *from; +} + +/*! \copydoc XsArrayDescriptor::itemCompare + \note Specialization for XsFilterProfile +*/ +int compareFilterProfile(XsFilterProfile const* a, XsFilterProfile const* b) +{ + if (a->m_filterType != b->m_filterType) + 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; + if (a->m_filterMajor != b->m_filterMajor) + 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; + if (a->m_version != b->m_version) + return (a->m_version < b->m_version) ? -1 : 1; + return 0; +} + +//! \brief zero the pointer value +void zeroFilterProfile(XsFilterProfile* a) +{ + memset(a, 0, sizeof(XsFilterProfile)); +} + +//! \brief Descriptor for XsFilterProfileArray +XsArrayDescriptor const g_xsFilterProfileArrayDescriptor = +{ + sizeof(XsFilterProfile), + XSEXPCASTITEMSWAP swapFilterProfile, // swap + XSEXPCASTITEMMAKE zeroFilterProfile, // construct + XSEXPCASTITEMCOPY copyFilterProfile, // copy construct + XSEXPCASTITEMMAKE zeroFilterProfile, // destruct + XSEXPCASTITEMCOPY copyFilterProfile, // copy + XSEXPCASTITEMCOMP compareFilterProfile, // compare + XSEXPCASTRAWCOPY XsArray_rawCopy +}; + +/*! \copydoc XsArray_constructDerived + \note Specialization for XsFilterProfileArray +*/ +void XsFilterProfileArray_construct(XsFilterProfileArray* thisPtr, XsSize count, XsFilterProfile const* src) +{ + XsArray_construct(thisPtr, &g_xsFilterProfileArrayDescriptor, count, src); +} diff --git a/extern/xspublic/xstypes/xsfilterprofilearray.h b/extern/xspublic/xstypes/xsfilterprofilearray.h new file mode 100644 index 0000000..2ef67f0 --- /dev/null +++ b/extern/xspublic/xstypes/xsfilterprofilearray.h @@ -0,0 +1,98 @@ + +// Copyright (c) 2003-2022 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 XSFILTERPROFILEARRAY_H +#define XSFILTERPROFILEARRAY_H + +#include "xstypesconfig.h" +#include "xsarray.h" + +#ifdef __cplusplus +#include "xsfilterprofile.h" +extern "C" { +#endif + +extern XsArrayDescriptor const XSTYPES_DLL_API g_xsFilterProfileArrayDescriptor; + +#ifndef __cplusplus +#define XSFILTERPROFILEARRAY_INITIALIZER XSARRAY_INITIALIZER(&g_xsFilterProfileArrayDescriptor) + +struct XsFilterProfile; + +XSARRAY_STRUCT(XsFilterProfileArray, struct XsFilterProfile); +typedef struct XsFilterProfileArray XsFilterProfileArray; + +XSTYPES_DLL_API void XsFilterProfileArray_construct(XsFilterProfileArray* thisPtr, XsSize count, struct XsFilterProfile const* src); +#else +} // extern "C" +#endif + +#ifdef __cplusplus + struct XsFilterProfileArray : public XsArrayImpl + { + //! \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); + } +#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) + { + } +#endif + }; +#endif +#endif diff --git a/extern/xspublic/xstypes/xsfilterprofilekind.h b/extern/xspublic/xstypes/xsfilterprofilekind.h new file mode 100644 index 0000000..59ba3ec --- /dev/null +++ b/extern/xspublic/xstypes/xsfilterprofilekind.h @@ -0,0 +1,48 @@ + +// Copyright (c) 2003-2022 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 XSFILTERPROFILEKIND_H +#define XSFILTERPROFILEKIND_H + +/*! \brief Filter Profile Kinds */ +enum XsFilterProfileKind +{ + XFPK_Unknown = 0, //!< Unknown profile kind + XFPK_Base = 195, //!< Indicates a base profile + XFPK_Additional = 196, //!< Indicates an additional profile + XFPK_Heading = 197 //!< Indicates a heading profile +}; + +typedef enum XsFilterProfileKind XsFilterProfileKind; + +#endif + diff --git a/extern/xspublic/xstypes/xsfloatmath.h b/extern/xspublic/xstypes/xsfloatmath.h new file mode 100644 index 0000000..af75b50 --- /dev/null +++ b/extern/xspublic/xstypes/xsfloatmath.h @@ -0,0 +1,42 @@ + +// Copyright (c) 2003-2022 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 XSFLOATMATH_H +/* Rename some functions to their single precision equivalents for non-C++ code */ +#ifdef XSENS_SINGLE_PRECISION +#define atan2 atan2f +#define cos cosf +#define sin sinf +#define sqrt sqrtf +#define asin asinf +#endif +#endif // XSFLOATMATH_H diff --git a/extern/xspublic/xstypes/xsglovedata.c b/extern/xspublic/xstypes/xsglovedata.c new file mode 100644 index 0000000..8320f02 --- /dev/null +++ b/extern/xspublic/xstypes/xsglovedata.c @@ -0,0 +1,149 @@ + +// Copyright (c) 2003-2022 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 "xsglovedata.h" +#include "xsquaternion.h" + +/*! \class XsGloveData + \brief Container for Glove data. +*/ + +#define SWAP(T, a, b) { T tmp = a; a = b; b = tmp; } + +/*! \addtogroup cinterface C Interface + @{ +*/ + +/*! \relates XsGloveData + \brief Initialize an %XsGloveData object +*/ +void XsGloveData_construct(struct XsGloveData* thisPtr) +{ + int i; + memset(thisPtr, 0, sizeof(XsGloveData)); + for (i = 0; i < XSFINGERSEGMENTCOUNT; i++) + XsFingerData_construct(&thisPtr->m_fingerData[i]); +} + +/*! \relates XsGloveData + \brief Destruct an %XsGloveData object +*/ +void XsGloveData_destruct(struct XsGloveData* thisPtr) +{ + // no destruction necessary, no dynamic memory allocated + (void)thisPtr; +} + +/*! \relates XsGloveData + \brief Copy the contents of an %XsGloveData object + \param copy The destination of the copy operation + \param src The object to copy from +*/ +void XsGloveData_copy(struct XsGloveData* copy, struct XsGloveData const* src) +{ + if (copy == src) + return; + int i; + copy->m_frameNumber = src->m_frameNumber; + copy->m_validSampleFlags = src->m_validSampleFlags; + for (i = 0; i < XSFINGERSEGMENTCOUNT; i++) + XsFingerData_copy(©->m_fingerData[i], &src->m_fingerData[i]); +} + +/*! \relates XsGloveData + \brief Swap the contents of two %XsGloveData objects + \param lhs The left hand side object of the swap + \param rhs The right hand side object of the swap +*/ +void XsGloveData_swap(struct XsGloveData* lhs, struct XsGloveData* rhs) +{ + if (lhs == rhs) + return; + int i; + SWAP(uint32_t, lhs->m_frameNumber, rhs->m_frameNumber); + SWAP(uint16_t, lhs->m_validSampleFlags, rhs->m_validSampleFlags); + for (i = 0; i < XSFINGERSEGMENTCOUNT; i++) + XsFingerData_swap(&lhs->m_fingerData[i], &rhs->m_fingerData[i]); +} + +/*! \relates XsFingerData + \brief Initialize an %XsFingerData object +*/ +void XsFingerData_construct(struct XsFingerData* thisPtr) +{ + // XsQuaternion_construct(&thisPtr->m_orientationIncrement); + XsVector3_construct(&thisPtr->m_velocityIncrement, 0); + XsVector3_construct(&thisPtr->m_mag, 0); + thisPtr->m_flags = 0; +} + +/*! \relates XsFingerData + \brief Destruct an %XsFingerData object +*/ +void XsFingerData_destruct(struct XsFingerData* thisPtr) +{ + // no destruction necessary, no dynamic memory allocated + (void)thisPtr; +} + +/*! \relates XsFingerData + \brief Copy the contents of an %XsFingerData object + \param copy The destination of the copy operation + \param src The object to copy from +*/ +void XsFingerData_copy(struct XsFingerData* copy, struct XsFingerData const* src) +{ + if (copy == src) + return; + XsQuaternion_copy(©->m_orientationIncrement, &src->m_orientationIncrement); + XsVector3_copy(©->m_velocityIncrement.m_vector, &src->m_velocityIncrement); + XsVector3_copy(©->m_mag.m_vector, &src->m_mag); + copy->m_flags = src->m_flags; +} + +/*! \relates XsFingerData + \brief Swap the contents of two %XsGloveData objects + \param lhs The left hand side object of the swap + \param rhs The right hand side object of the swap +*/ +void XsFingerData_swap(struct XsFingerData* lhs, struct XsFingerData* rhs) +{ + if (lhs == rhs) + return; + XsQuaternion_swap(&lhs->m_orientationIncrement, &rhs->m_orientationIncrement); + XsVector_swap(&lhs->m_velocityIncrement.m_vector, &rhs->m_velocityIncrement.m_vector); + XsVector_swap(&lhs->m_mag.m_vector, &rhs->m_mag.m_vector); + + SWAP(uint16_t, lhs->m_flags, rhs->m_flags); +} + +/*! @} */ diff --git a/extern/xspublic/xstypes/xsglovedata.h b/extern/xspublic/xstypes/xsglovedata.h new file mode 100644 index 0000000..3d087fd --- /dev/null +++ b/extern/xspublic/xstypes/xsglovedata.h @@ -0,0 +1,285 @@ + +// Copyright (c) 2003-2022 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 XSGLOVEDATA_H +#define XSGLOVEDATA_H + +#include "xstypesconfig.h" +#include "xsvector3.h" +#include "xsquaternion.h" + +struct XsGloveData; +struct XsFingerData; + +#ifdef __cplusplus +extern "C" { +#endif +#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 } +#endif + +XSTYPES_DLL_API void XsFingerData_construct(struct XsFingerData* thisPtr); +XSTYPES_DLL_API void XsFingerData_destruct(struct XsFingerData* thisPtr); +XSTYPES_DLL_API void XsFingerData_copy(struct XsFingerData* copy, struct XsFingerData const* src); +XSTYPES_DLL_API void XsFingerData_swap(struct XsFingerData* lhs, struct XsFingerData* rhs); + +XSTYPES_DLL_API void XsGloveData_construct(struct XsGloveData* thisPtr); +XSTYPES_DLL_API void XsGloveData_destruct(struct XsGloveData* thisPtr); +XSTYPES_DLL_API void XsGloveData_copy(struct XsGloveData* copy, struct XsGloveData const* src); +XSTYPES_DLL_API void XsGloveData_swap(struct XsGloveData* lhs, struct XsGloveData* rhs); + +#ifdef __cplusplus +} // extern "C" +#endif + +#define XSFINGERSEGMENTCOUNT 12 + +/*! \brief A container for Finger data +*/ +struct XsFingerData +{ +#ifdef __cplusplus + //! \brief Construct an empty object + inline XsFingerData() + : m_flags(0) + { + } + + //! \brief Construct an initialized object + inline XsFingerData(const XsQuaternion& dq, const XsVector& dv, const XsVector& mag, const uint16_t flags) + : m_orientationIncrement(dq) + , m_velocityIncrement(dv) + , m_mag(mag) + , m_flags(flags) + { + } + + //! \brief Copy constructor + inline XsFingerData(const XsFingerData& other) + : m_orientationIncrement(other.m_orientationIncrement) + , m_velocityIncrement(other.m_velocityIncrement) + , m_mag(other.m_mag) + , m_flags(other.m_flags) + { + } + + //! \brief Assignment operator + inline const XsFingerData& operator=(const XsFingerData& other) + { + if (this != &other) + { + m_orientationIncrement = other.m_orientationIncrement; + m_velocityIncrement = other.m_velocityIncrement; + m_mag = other.m_mag; + m_flags = other.m_flags; + } + return *this; + } + + //! \brief Clear the object so it contains unity data + inline void clear() + { + m_orientationIncrement = XsQuaternion::identity(); + m_velocityIncrement.setZero(); + m_mag.setZero(); + } + + //! \brief Returns the contained orientation increment + inline const XsQuaternion& orientationIncrement() const + { + return m_orientationIncrement; + } + + //! \brief Returns the contained velocity increment + inline const XsVector3& velocityIncrement() const + { + return m_velocityIncrement; + } + + //! \brief Returns the mag + inline const XsVector3& mag() const + { + return m_mag; + } + + //! \brief Returns the flags + inline uint16_t flags() const + { + return m_flags; + } + + /*! \brief Returns true if all fields of this and \a other are exactly identical */ + inline bool operator == (const XsFingerData& other) const + { + return m_orientationIncrement == other.m_orientationIncrement && + m_velocityIncrement == other.m_velocityIncrement && + m_mag == other.m_mag && + m_flags == other.m_flags; + } + + /*! \brief Returns true if not all fields of this and \a other are exactly identical */ + inline bool operator != (const XsFingerData& other) const + { + return !operator==(other); + } + + /*! \brief Swap the contents of this with \a other */ + inline void swap(XsFingerData& other) + { + XsFingerData_swap(this, &other); + } + + /*! \brief Swap the contents of \a first with \a second */ + friend void swap(XsFingerData& first, XsFingerData& second) + { + first.swap(second); + } + +protected: +#endif + + XsQuaternion m_orientationIncrement; //!< The orientation increment for this segment + XsVector3 m_velocityIncrement; //!< The velocity increment for this segment + XsVector3 m_mag; //!< The magnetic field data for this segment + uint16_t m_flags; //!< Data quality flags (LSB --> MSB: acc x,y,z gyr x,y,z mag x,y,z) +}; +typedef struct XsFingerData XsFingerData; + +/*! \brief A container for glove data +*/ +struct XsGloveData +{ +#ifdef __cplusplus + //! \brief Construct an empty object + inline XsGloveData() + : m_frameNumber(0) + , m_validSampleFlags(0) + { + } + + //! \brief Construct an initialized object + inline XsGloveData(const uint16_t frameNumber, const uint16_t validSampleFlags, const XsFingerData* fingerData) + : m_frameNumber(frameNumber) + , m_validSampleFlags(validSampleFlags) + { + for (int i = 0; i < XSFINGERSEGMENTCOUNT; ++i) + m_fingerData[i] = fingerData[i]; + } + + //! \brief Copy constructor + inline XsGloveData(const XsGloveData& other) + : m_frameNumber(other.m_frameNumber) + , m_validSampleFlags(other.m_validSampleFlags) + { + for (int i = 0; i < XSFINGERSEGMENTCOUNT; ++i) + m_fingerData[i] = other.m_fingerData[i]; + } + + //! \brief Returns the snapshot counter + inline uint32_t frameNumber() const + { + return m_frameNumber; + } + + //! \brief Returns the valid sample flags + inline uint16_t validSampleFlags() const + { + return m_validSampleFlags; + } + + + //! \brief Returns the number of items in fingerData + inline static int fingerSegmentCount() + { + return XSFINGERSEGMENTCOUNT; + } + + //! \brief Returns the finger data + inline XsFingerData const& fingerData(int i) const + { + assert(i >= 0 && i < XSFINGERSEGMENTCOUNT); + return m_fingerData[i]; + } + + /*! \brief Returns true if all fields of this and \a other are exactly identical */ + inline bool operator == (const XsGloveData& other) const + { + if (m_frameNumber != other.m_frameNumber || + m_validSampleFlags != other.m_validSampleFlags) + return false; + + for (int i = 0; i < XSFINGERSEGMENTCOUNT; ++i) + { + if (!(m_fingerData[i] == other.m_fingerData[i])) + return false; + } + return true; + } + + /*! \brief Returns true if not all fields of this and \a other are exactly identical */ + inline bool operator != (const XsGloveData& other) const + { + return !operator==(other); + } + + //! \brief Assignment operator, copies contents of \a other into this + inline XsGloveData const& operator = (XsGloveData const& other) + { + XsGloveData_copy(this, &other); + return *this; + } + + /*! \brief Swap the contents of this with \a other */ + inline void swap(XsGloveData& other) + { + XsGloveData_swap(this, &other); + } + + /*! \brief Swap the contents of \a first with \a second */ + friend void swap(XsGloveData& first, XsGloveData& second) + { + first.swap(second); + } + +protected: +#endif + XsFingerData m_fingerData[XSFINGERSEGMENTCOUNT]; //!< Data for each tracked finger segment + uint32_t m_frameNumber; //!< The sequential frame number for the data + uint16_t m_validSampleFlags; //!< Flags describing the general validity of the data +}; + +typedef struct XsGloveData XsGloveData; + +#endif diff --git a/extern/xspublic/xstypes/xsglovesnapshot.h b/extern/xspublic/xstypes/xsglovesnapshot.h new file mode 100644 index 0000000..90c1465 --- /dev/null +++ b/extern/xspublic/xstypes/xsglovesnapshot.h @@ -0,0 +1,123 @@ + +// Copyright (c) 2003-2022 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 XSGLOVESNAPSHOT_H +#define XSGLOVESNAPSHOT_H + +#include "xstypesconfig.h" +#include "xsdeviceid.h" + +struct XsGloveSnapshot; + +#ifdef __cplusplus +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} +#define XSFINGERSNAPSHOT_INITIALIZER {0,0,0, 0,0,0, 0,0,0, 0, 0, 0} +#endif + +#ifdef __cplusplus +} // extern "C" +#endif + +#include /* pow */ + +XS_PACKED_STRUCT_START +/*! \brief int24 +*/ +struct int24_t +{ + uint8_t m_vals[3]; /*!< \brief The data*/ + +#ifdef __cplusplus + //! \brief convert int24 to double + inline double toDouble() const + { + if (m_vals[0] & 0x80) + return ((double)(int32_t)(((uint32_t)0xff000000) | (((uint32_t)m_vals[0]) << 16) | (((uint32_t)m_vals[1]) << 8) | ((uint32_t)m_vals[2]))); + else + return (double)((((uint32_t)m_vals[0]) << 16) | (((uint32_t)m_vals[1]) << 8) | (uint32_t)m_vals[2]); + } + +#endif +#ifdef SWIG +}; +#else +} XS_PACKED_STRUCT; +#endif +typedef struct int24_t int24_t; + +/*! \brief A container for Finger Snapshot data +*/ +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 */ + uint16_t m_flags; /*!< \brief The flags */ + uint8_t m_accClippingCounter; /*!< \brief The acceleration clipping counter */ + uint8_t m_gyrClippingCounter; /*!< \brief The gyroscope clipping counter */ +#ifdef SWIG +}; +#else +} XS_PACKED_STRUCT; +#endif +typedef struct XsFingerSnapshot XsFingerSnapshot; + +/*! \brief A container for Glove Snapshot data +*/ +struct XsGloveSnapshot +{ + uint32_t m_frameNumber; /*!< \brief The frame number associated with the fingerdata */ + uint16_t m_validSampleFlags; /*!< \brief The valid sample flags */ + uint8_t m_padByte; /*!< \brief A padded byte to align fingerdata*/ + XsFingerSnapshot m_fingers[12]; /*!< \brief The 12 fingers */ +#if 0 // def __cplusplus + /*! \brief Returns true if all fields of this and \a other are exactly identical */ + inline bool operator == (const XsGloveSnapshot& other) const + { + return true; + } +#endif +#ifdef SWIG +}; +#else +} XS_PACKED_STRUCT; +#endif +typedef struct XsGloveSnapshot XsGloveSnapshot; + +XS_PACKED_STRUCT_END + +#endif diff --git a/extern/xspublic/xstypes/xshandid.h b/extern/xspublic/xstypes/xshandid.h new file mode 100644 index 0000000..7663f18 --- /dev/null +++ b/extern/xspublic/xstypes/xshandid.h @@ -0,0 +1,54 @@ + +// Copyright (c) 2003-2022 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 XSHANDID_H +#define XSHANDID_H + +/*! \addtogroup enums + @{ +*/ + +/*! \brief This is an enumerator that contains the left and right hand. + \details These values are to be used when addressing the left or right hand (or either). +*/ +enum XsHandId +{ + XHI_LeftHand = 0, //!< The Left Hand + XHI_RightHand = 1, //!< The Right Hand + XHI_Unknown //!< Used as uninitialized or unknown side +}; + +//! @} + +typedef enum XsHandId XsHandId; + +#endif diff --git a/extern/xspublic/xstypes/xsinforequest.h b/extern/xspublic/xstypes/xsinforequest.h new file mode 100644 index 0000000..9052120 --- /dev/null +++ b/extern/xspublic/xstypes/xsinforequest.h @@ -0,0 +1,54 @@ + +// Copyright (c) 2003-2022 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 XSINFOREQUEST_H +#define XSINFOREQUEST_H + +/*! \addtogroup enums Global enumerations + @{ +*/ + +//AUTO namespace xstypes { +/*! \brief Information request identifiers + \details These values are used by the XsDevice::requestInfo function and + XsCallback::onInfoResponse functions. +*/ +enum XsInfoRequest +{ + XIR_BatteryLevel = 0, //!< Request battery level + XIR_GnssSvInfo, //!< Request Gnss satellite vehicle information +}; +/*! @} */ +typedef enum XsInfoRequest XsInfoRequest; +//AUTO } + +#endif diff --git a/extern/xspublic/xstypes/xsint64array.c b/extern/xspublic/xstypes/xsint64array.c new file mode 100644 index 0000000..225ba37 --- /dev/null +++ b/extern/xspublic/xstypes/xsint64array.c @@ -0,0 +1,86 @@ + +// Copyright (c) 2003-2022 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 "xsint64array.h" + +/*! \struct XsInt64Array + \brief A list of int64_t values + \sa XsArray +*/ + +/*! \copydoc XsArrayDescriptor::itemSwap + \note Specialization for int64_t*/ +void swapInt64(int64_t* a, int64_t* b) +{ + int64_t tmp = *a; + *a = *b; + *b = tmp; +} + +/*! \copydoc XsArrayDescriptor::itemCopy + \note Specialization for int64_t*/ +void copyInt64(int64_t* to, int64_t const* from) +{ + *to = *from; +} + +/*! \copydoc XsArrayDescriptor::itemCompare + \note Specialization for int64_t*/ +int compareInt64(int64_t const* a, int64_t const* b) +{ + if (*a < *b) + return -1; + if (*a > *b) + return 1; + return 0; +} + +//! \brief Descriptor for XsInt64Array +XsArrayDescriptor const g_xsInt64ArrayDescriptor = +{ + sizeof(int64_t), + XSEXPCASTITEMSWAP swapInt64, // swap + 0, // construct + XSEXPCASTITEMCOPY copyInt64, // copy construct + 0, // destruct + XSEXPCASTITEMCOPY copyInt64, // copy + XSEXPCASTITEMCOMP compareInt64, // compare + XSEXPCASTRAWCOPY XsArray_rawCopy // raw copy +}; + +/*! \copydoc XsArray_constructDerived + \note Specialization for XsInt64Array +*/ +void XsInt64Array_construct(XsInt64Array* thisPtr, XsSize count, int64_t const* src) +{ + XsArray_construct(thisPtr, &g_xsInt64ArrayDescriptor, count, src); +} diff --git a/extern/xspublic/xstypes/xsint64array.h b/extern/xspublic/xstypes/xsint64array.h new file mode 100644 index 0000000..51c708b --- /dev/null +++ b/extern/xspublic/xstypes/xsint64array.h @@ -0,0 +1,96 @@ + +// Copyright (c) 2003-2022 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 XSINT64ARRAY_H +#define XSINT64ARRAY_H + +#include "xsarray.h" +#include "pstdint.h" + +#ifdef __cplusplus +extern "C" { +#endif + +extern XsArrayDescriptor const XSTYPES_DLL_API g_xsInt64ArrayDescriptor; + +#ifndef __cplusplus +#define XSINT64ARRAY_INITIALIZER XSARRAY_INITIALIZER(&g_xsInt64ArrayDescriptor) +XSARRAY_STRUCT(XsInt64Array, int64_t); +typedef struct XsInt64Array XsInt64Array; + +XSTYPES_DLL_API void XsInt64Array_construct(XsInt64Array* thisPtr, XsSize count, int64_t const* src); +#endif + +#ifdef __cplusplus +} // extern "C" + +struct XsInt64Array : public XsArrayImpl +{ + //! \brief Constructs an XsInt64Array + inline explicit XsInt64Array(XsSize sz = 0, int64_t const* src = 0) + : ArrayImpl(sz, src) + { + } + + //! \brief Constructs an XsInt64Array as a copy of \a other + inline XsInt64Array(XsInt64Array const& other) + : ArrayImpl(other) + { + } + + //! \brief Constructs an XsInt64Array that references the data supplied in \a ref + inline explicit XsInt64Array(int64_t* 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(XsInt64Array& first, XsInt64Array& second) + { + first.swap(second); + } +#endif + +#ifndef XSENS_NOITERATOR + //! \brief Constructs an XsInt64Array with the array bound by the supplied iterators \a beginIt and \a endIt + template + inline XsInt64Array(Iterator beginIt, Iterator endIt) + : ArrayImpl(beginIt, endIt) + { + } +#endif +}; +#endif + + +#endif diff --git a/extern/xspublic/xstypes/xsintarray.c b/extern/xspublic/xstypes/xsintarray.c new file mode 100644 index 0000000..7db4fdf --- /dev/null +++ b/extern/xspublic/xstypes/xsintarray.c @@ -0,0 +1,87 @@ + +// Copyright (c) 2003-2022 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 "xsintarray.h" + +/*! \struct XsIntArray + \brief A list of XsInt values + \sa XsArray +*/ + +/*! \copydoc XsArrayDescriptor::itemSwap + \note Specialization for int*/ +void swapInt(int* a, int* b) +{ + int tmp = *a; + *a = *b; + *b = tmp; +} + +/*! \copydoc XsArrayDescriptor::itemCopy + \note Specialization for int*/ +void copyInt(int* to, int const* from) +{ + *to = *from; +} + +/*! \copydoc XsArrayDescriptor::itemCompare + \note Specialization for int*/ +int compareInt(int const* a, int const* b) +{ + if (*a < *b) + return -1; + if (*a > *b) + return 1; + return 0; +} + + +//! \brief Descriptor for XsIntArray +XsArrayDescriptor const g_xsIntArrayDescriptor = +{ + sizeof(int), + XSEXPCASTITEMSWAP swapInt, // swap + 0, // construct + XSEXPCASTITEMCOPY copyInt, // copy construct + 0, // destruct + XSEXPCASTITEMCOPY copyInt, // copy + XSEXPCASTITEMCOMP compareInt, // compare + XSEXPCASTRAWCOPY XsArray_rawCopy // raw copy +}; + +/*! \copydoc XsArray_constructDerived + \note Specialization for XsStringArray +*/ +void XsIntArray_construct(XsIntArray* thisPtr, XsSize count, int const* src) +{ + XsArray_construct(thisPtr, &g_xsIntArrayDescriptor, count, src); +} diff --git a/extern/xspublic/xstypes/xsintarray.h b/extern/xspublic/xstypes/xsintarray.h new file mode 100644 index 0000000..e10d880 --- /dev/null +++ b/extern/xspublic/xstypes/xsintarray.h @@ -0,0 +1,95 @@ + +// Copyright (c) 2003-2022 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 XSINTARRAY_H +#define XSINTARRAY_H + +#include "xsarray.h" + +#ifdef __cplusplus +extern "C" { +#endif + +extern XsArrayDescriptor const XSTYPES_DLL_API g_xsIntArrayDescriptor; + +#ifndef __cplusplus +#define XsIntArray_INITIALIZER XSARRAY_INITIALIZER(&g_xsIntArrayDescriptor) + +XSARRAY_STRUCT(XsIntArray, int); +typedef struct XsIntArray XsIntArray; + +XSTYPES_DLL_API void XsIntArray_construct(XsIntArray* thisPtr, XsSize count, int const* src); +#endif + +#ifdef __cplusplus +} // extern "C" + +struct XsIntArray : public XsArrayImpl +{ + //! \brief Constructs an XsIntArray + inline explicit XsIntArray(XsSize sz = 0, int const* src = 0) + : ArrayImpl(sz, src) + { + } + + //! \brief Constructs an XsIntArray as a copy of \a other + inline XsIntArray(XsIntArray const& other) + : ArrayImpl(other) + { + } + + //! \brief Constructs an XsIntArray that references the data supplied in \a ref + inline explicit XsIntArray(int* 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(XsIntArray& first, XsIntArray& second) + { + first.swap(second); + } +#endif + +#ifndef XSENS_NOITERATOR + //! \brief Constructs an XsIntArray with the array bound by the supplied iterators \a beginIt and \a endIt + template + inline XsIntArray(Iterator beginIt, Iterator endIt) + : ArrayImpl(beginIt, endIt) + { + } +#endif +}; +#endif + +#endif diff --git a/extern/xspublic/xstypes/xslibraryloader.c b/extern/xspublic/xstypes/xslibraryloader.c new file mode 100644 index 0000000..5bd9c26 --- /dev/null +++ b/extern/xspublic/xstypes/xslibraryloader.c @@ -0,0 +1,142 @@ + +// Copyright (c) 2003-2022 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 "xslibraryloader.h" +#include "xsstring.h" + +#ifdef __GNUC__ + #include +#elif defined(_MSC_VER) + #include +#endif + +/*! \brief Dynamically load a library + + \param[in,out] thisp the XsLibraryLoader object handle + \param[in] libraryName the name of the library to load. The library + should be present in the current search path, or be specified by a path + \return non-zero if the library could be loaded, zero otherwise +*/ +int XsLibraryLoader_load(XsLibraryLoader* thisp, const XsString* libraryName) +{ +#ifdef __GNUC__ + if (XsLibraryLoader_isLoaded(thisp)) + return 0; + thisp->m_handle = dlopen(libraryName->m_data, RTLD_LAZY); +#elif defined(_MSC_VER) + wchar_t* libraryNameW; + XsSize required; + if (XsLibraryLoader_isLoaded(thisp)) + return 0; + required = XsString_copyToWCharArray(libraryName, NULL, 0); + libraryNameW = (wchar_t*)malloc(required * sizeof(wchar_t)); + + (void)XsString_copyToWCharArray(libraryName, libraryNameW, required); + + thisp->m_handle = LoadLibrary(libraryNameW); + + free(libraryNameW); +#endif + return XsLibraryLoader_isLoaded(thisp); +} + +/*! \brief Resolve a function from the library + + \param[in] thisp the library handle + \param[in] functionName the name of the function + \return a pointer to the resolved function, may be NULL if no function could be resolved +*/ +void* XsLibraryLoader_resolve(const XsLibraryLoader* thisp, const char* functionName) +{ +#ifdef __GNUC__ + return dlsym(thisp->m_handle, functionName); +#elif defined(_MSC_VER) +#pragma warning(push) +#pragma warning(disable: 4152) + return GetProcAddress(thisp->m_handle, functionName); +#pragma warning(pop) +#endif +} + +/*! \brief Unload the loaded library + + \param[in,out] thisp the library handle + \return zero on failure, non-zero otherwise +*/ +int XsLibraryLoader_unload(XsLibraryLoader* thisp) +{ + void* handle = thisp->m_handle; + thisp->m_handle = NULL; + if (handle) + { +#ifdef __GNUC__ + return dlclose(handle) == 0; +#elif defined(_MSC_VER) + return FreeLibrary(handle) != 0; +#endif + } + return 0; +} + +/*! \brief Check if a library is loaded + + \param[in] thisp the library handle + \return zero if nothing is loaded, non-zero otherwise +*/ +int XsLibraryLoader_isLoaded(const XsLibraryLoader* thisp) +{ + return thisp->m_handle != NULL; +} + +/*! \brief Get an error string after a failure occurred + \param[in,out] error the string to fill with the result error +*/ +void XsLibraryLoader_getErrorString(XsString* error) +{ +#ifdef __GNUC__ + XsString_assignCharArray(error, dlerror()); +#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); + + XsString_assignWCharArray(error, errorText); + LocalFree(errorText); +#endif +} diff --git a/extern/xspublic/xstypes/xslibraryloader.h b/extern/xspublic/xstypes/xslibraryloader.h new file mode 100644 index 0000000..e25e346 --- /dev/null +++ b/extern/xspublic/xstypes/xslibraryloader.h @@ -0,0 +1,133 @@ + +// Copyright (c) 2003-2022 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 XSLIBRARYLOADER_H +#define XSLIBRARYLOADER_H + +#include "xstypesconfig.h" +#include "xsstring.h" + +struct XsLibraryLoader; + +#ifdef __cplusplus +extern "C" { +#endif +#ifndef __cplusplus +#define XSLIBRARYLOADER_INITIALIZER { NULL } +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 int XsLibraryLoader_unload(XsLibraryLoader* thisp); +XSTYPES_DLL_API int XsLibraryLoader_isLoaded(const XsLibraryLoader* thisp); +XSTYPES_DLL_API void XsLibraryLoader_getErrorString(XsString* error); + +#ifdef __cplusplus +} +#endif + +/*! \brief The Xsens dynamic library loader base class +*/ +struct XsLibraryLoader +{ +#ifdef __cplusplus +public: + /*! \brief Create a library loader */ + inline XsLibraryLoader() : + m_handle(NULL) + { + // avoid compiler warnings about + // an unused handle. It is used in the c implementations + (void)m_handle; + } + + /*! \brief Destroy a library loader */ + inline ~XsLibraryLoader() + { + unload(); + } + + /*! \brief Load the library + \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) + { + return XsLibraryLoader_load(this, &libraryName) != 0; + } + + /*! \brief Return true if a library has been loaded + + \return true if a library has been loaded, false otherwise + */ + inline bool isLoaded() const + { + return XsLibraryLoader_isLoaded(this) != 0; + } + + /*! \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 + */ + inline void* resolve(const char* functionName) const + { + return XsLibraryLoader_resolve(this, functionName); + } + + /*! \brief Unload the loaded library + */ + inline void unload() noexcept + { + XsLibraryLoader_unload(this); + } + + /*! \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. + + \returns a string describing the error that occurred + */ + inline static XsString errorString() + { + XsString rv; + XsLibraryLoader_getErrorString(&rv); + return rv; + } +private: +#endif + void* m_handle; +}; + +#endif diff --git a/extern/xspublic/xstypes/xsmalloc.c b/extern/xspublic/xstypes/xsmalloc.c new file mode 100644 index 0000000..782bb7b --- /dev/null +++ b/extern/xspublic/xstypes/xsmalloc.c @@ -0,0 +1,182 @@ + +// Copyright (c) 2003-2022 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. +// + +/*! \file + This file contains platform-specific memory allocation routines +*/ + +#include "xsmalloc.h" +#if !(defined __ICCARM__) && !(defined _ADI_COMPILER) && !defined(__APPLE__) && !defined(__CRCC__) && !(defined(__arm__) && defined(__ARMCC_VERSION)) + #include +#endif +#include + +#ifdef XSENS_ASSERT_MALLOC + #include + #undef XSENS_ASSERT_MALLOC + #define XSENS_ASSERT_MALLOC(x) assert(x) +#else + #define XSENS_ASSERT_MALLOC(x) +#endif + +#if !(defined __ICCARM__) && !(defined _ADI_COMPILER) && defined(XSENS_DEBUG) + //#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]; +#endif + +#ifndef _MSC_VER +# ifdef __ANDROID__ +# define _aligned_malloc(size, align) memalign(align, size) +# elif (defined __ICCARM__) || (defined _ADI_COMPILER) || (defined __CRCC__) || (defined IAR_ARM_CM3) || (defined __ARMEL__) || (defined(__arm__) && defined(__ARMCC_VERSION)) +# define _aligned_malloc(a, b) malloc(a) +# else + +void* __cdecl _aligned_malloc(size_t _Size, size_t _Alignment) +{ + void* rv = 0; + int err = posix_memalign(&rv, _Alignment, _Size); + if (err == 0) + return rv; + return NULL; +} + +# endif + +#define _aligned_realloc(p, n, a) realloc(p, n) +#define _aligned_free(_Memory) free(_Memory) + +#endif //!_MSC_VER + + +//! \brief Allocates \a sz bytes of memory, optionally tracking the allocation +void* xsMalloc(size_t sz) +{ +#ifdef TRACK_ALLOCS + void* ptr = malloc(sz); + XSENS_ASSERT_MALLOC(ptr); + lastAllocIdx = (lastAllocIdx + 1) & (TRACK_ALLOCS - 1); + lastAllocs[lastAllocIdx] = ptr; + return ptr; +#else + void* ptr = malloc(sz); + XSENS_ASSERT_MALLOC(ptr); + return ptr; +#endif +} + +//! \brief Reallocates \a sz bytes of memory, optionally tracking the allocation +void* xsRealloc(void* ptr, size_t sz) +{ +#ifdef TRACK_ALLOCS + lastFreeIdx = (lastFreeIdx + 1) & (TRACK_ALLOCS - 1); + lastFrees[lastFreeIdx] = ptr; + + ptr = realloc(ptr, sz); + XSENS_ASSERT_MALLOC(ptr); + lastAllocIdx = (lastAllocIdx + 1) & (TRACK_ALLOCS - 1); + lastAllocs[lastAllocIdx] = ptr; + return ptr; +#else + void* mem = realloc(ptr, sz); + XSENS_ASSERT_MALLOC(mem); + return mem; +#endif +} + +//! \brief Frees the memory pointed to by \a ptr, optionally tracking the allocation +void xsFree(void* ptr) +{ +#ifdef TRACK_ALLOCS + lastFreeIdx = (lastFreeIdx + 1) & (TRACK_ALLOCS - 1); + lastFrees[lastFreeIdx] = ptr; +#endif + free(ptr); +} + +//! \brief Allocates \a sz bytes of memory on a 16 byte boundary, optionally tracking the allocation +void* xsAlignedMalloc(size_t sz) +{ +#ifdef TRACK_ALLOCS + void* ptr = _aligned_malloc(sz, 16); + XSENS_ASSERT_MALLOC(ptr); + lastAlignedAllocIdx = (lastAlignedAllocIdx + 1) & (TRACK_ALLOCS - 1); + lastAlignedAllocs[lastAlignedAllocIdx] = ptr; + return ptr; +#else + void* ptr = _aligned_malloc(sz, 16); + XSENS_ASSERT_MALLOC(ptr); + return ptr; +#endif +} + +//! \brief Reallocates \a sz bytes of memory on a 16 byte boundary, optionally tracking the allocation +void* xsAlignedRealloc(void* ptr, size_t sz) +{ +#ifdef TRACK_ALLOCS + 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); + lastAlignedAllocs[lastAlignedAllocIdx] = ptr; + return ptr; +#else + void* mem = _aligned_realloc(ptr, sz, 16); + XSENS_ASSERT_MALLOC(mem); + return mem; +#endif +} + +//! \brief Frees the (aligned) memory pointed to by \a ptr, optionally tracking the allocation +void xsAlignedFree(void* ptr) +{ +#ifdef TRACK_ALLOCS + lastAlignedFreeIdx = (lastAlignedFreeIdx + 1) & (TRACK_ALLOCS - 1); + lastAlignedFrees[lastAlignedFreeIdx] = ptr; +#endif + _aligned_free(ptr); +} + + + diff --git a/extern/xspublic/xstypes/xsmalloc.h b/extern/xspublic/xstypes/xsmalloc.h new file mode 100644 index 0000000..daf59a8 --- /dev/null +++ b/extern/xspublic/xstypes/xsmalloc.h @@ -0,0 +1,67 @@ + +// Copyright (c) 2003-2022 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 XSMALLOC_H +#define XSMALLOC_H + +#include "xstypesconfig.h" +#include + +#ifdef __cplusplus +extern "C" { +#endif + +XSTYPES_DLL_API void* xsMalloc(size_t sz); +XSTYPES_DLL_API void* xsRealloc(void* ptr, size_t sz); +XSTYPES_DLL_API void xsFree(void* ptr); + +XSTYPES_DLL_API void* xsAlignedMalloc(size_t sz); +XSTYPES_DLL_API void* xsAlignedRealloc(void* ptr, size_t sz); +XSTYPES_DLL_API void xsAlignedFree(void* ptr); + +#ifndef xsMathMalloc +#define xsMathMalloc(n) xsAlignedMalloc(n) +#endif + +#ifndef xsMathRealloc +#define xsMathRealloc(p, n) xsAlignedRealloc(p,n) +#endif + +#ifndef xsMathFree +#define xsMathFree(p) xsAlignedFree(p) +#endif + +#ifdef __cplusplus +} // extern "C" +#endif + +#endif diff --git a/extern/xspublic/xstypes/xsmath.c b/extern/xspublic/xstypes/xsmath.c new file mode 100644 index 0000000..82a7299 --- /dev/null +++ b/extern/xspublic/xstypes/xsmath.c @@ -0,0 +1,43 @@ + +// Copyright (c) 2003-2022 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 "xsmath.h" + +// This file is obsolete and empty, this line is to prevent compiler warnings about empty files +// 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 */ +#endif diff --git a/extern/xspublic/xstypes/xsmath.h b/extern/xspublic/xstypes/xsmath.h new file mode 100644 index 0000000..641fc0c --- /dev/null +++ b/extern/xspublic/xstypes/xsmath.h @@ -0,0 +1,274 @@ + +// Copyright (c) 2003-2022 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 XSMATH_H +#define XSMATH_H + +#include "xstypesconfig.h" +#include "xstypedefs.h" +#include "pstdint.h" +#include +#include +#include "xsfloatmath.h" + +#if defined(SQUISHCOCO) + #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 + #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 + \brief Namespace for mathematical constants and operations. +*/ +/*! \addtogroup cinterface C Interface + @{ +*/ + +//! \brief The value e +XSMATHCONST XsReal XsMath_e = 2.7182818284590452353602874713527; +//! \brief The value pi +XSMATHCONST XsReal XsMath_pi = 3.1415926535897932384626433832795028841971693993751058209749; +//! \brief A really small value +XSMATHCONST XsReal XsMath_tinyValue = 1.0e-16; +//! \brief A convincingly large number +XSMATHCONST XsReal XsMath_hugeValue = 1.0e+16; + +#ifdef XSENS_SINGLE_PRECISION + //! \brief A value related to the precision of floating point arithmetic (1.192092895507813e-07) + XSMATHCONST XsReal XsMath_epsilon = 1.192092895507813e-07; + //! \brief Square root of XsMath_epsilon + XSMATHCONST XsReal XsMath_sqrtEpsilon = 3.452669830012439e-04; + //! \brief Value that represents the subnormal number in floating point wizardry + XSMATHCONST XsReal XsMath_denormalized = 1e-37; + //! \brief Square root of XsMath_denormalized + XSMATHCONST XsReal XsMath_sqrtDenormalized = 3.1622776601683793319988935444327e-19; +#else + //! \brief A value related to the precision of floating point arithmetic (2.2204460492503131e-016) + XSMATHCONST XsReal XsMath_epsilon = 2.2204460492503131e-016; + //! \brief Square root of XsMath_epsilon + XSMATHCONST XsReal XsMath_sqrtEpsilon = 1.4901161193847656e-008; + //! \brief Value that represents the subnormal number in floating point wizardry + XSMATHCONST XsReal XsMath_denormalized = 1e-307; + //! \brief Square root of XsMath_denormalized + XSMATHCONST XsReal XsMath_sqrtDenormalized = 3.1622776601683793319988935444327e-154; +#endif + +//! \brief Value to convert radians to degrees by multiplication +XSMATHCONST XsReal XsMath_rad2degValue = 57.295779513082320876798154814105; // (180.0/pi) +//! \brief Value to convert degrees to radians by multiplication +XSMATHCONST XsReal XsMath_deg2radValue = 0.017453292519943295769236907684886; // (pi/180.0) + +//! \brief 0 +XSMATHCONST XsReal XsMath_zero = 0.0; +//! \brief 0.25 +XSMATHCONST XsReal XsMath_pt25 = 0.25; +//! \brief 0.5 +XSMATHCONST XsReal XsMath_pt5 = 0.5; +//! \brief -0.5 +XSMATHCONST XsReal XsMath_minusPt5 = -0.5; +//! \brief 1.0 +XSMATHCONST XsReal XsMath_one = 1.0; +//! \brief -1.0 +XSMATHCONST XsReal XsMath_minusOne = -1.0; +//! \brief 2 +XSMATHCONST XsReal XsMath_two = 2.0; +//! \brief 4 +XSMATHCONST XsReal XsMath_four = 4.0; +//! \brief -2 +XSMATHCONST XsReal XsMath_minusTwo = -2.0; + +//! \brief -pi/2 +XSMATHCONST XsReal XsMath_minusHalfPi = -1.5707963267948966192313216916397514420985846996875529104874; +//! \brief pi/2 +XSMATHCONST XsReal XsMath_halfPi = 1.5707963267948966192313216916397514420985846996875529104874; +//! \brief 2*pi +XSMATHCONST XsReal XsMath_twoPi = 6.2831853071795864769252867665590057683943387987502116419498; +//! \brief sqrt(2) +XSMATHCONST XsReal XsMath_sqrt2 = 1.4142135623730950488016887242097; +//! \brief sqrt(0.5) +XSMATHCONST XsReal XsMath_sqrtHalf = 0.5 * 1.4142135623730950488016887242097; + +#ifdef XSENS_SINGLE_PRECISION + //! \brief infinity value + XSMATHCONST XsReal XsMath_infinity = FLT_MAX; +#else + //! \brief infinity value + XSMATHCONST XsReal XsMath_infinity = DBL_MAX; +#endif + +/*! \brief Returns asin(\a x) for -1 < x < 1 +*/ +XSMATHINLINE XsReal XsMath_asinClamped(const XsReal x) +{ + return x <= XsMath_minusOne ? XsMath_minusHalfPi : x >= XsMath_one ? XsMath_halfPi : asin(x); +} + +/*! \brief Convert radians to degrees +*/ +XSMATHINLINE XsReal XsMath_rad2deg(XsReal radians) +{ + return XsMath_rad2degValue * radians; +} + +/*! \brief Convert degrees to radians +*/ +XSMATHINLINE XsReal XsMath_deg2rad(XsReal degrees) +{ + return XsMath_deg2radValue * degrees; +} + +/*! \brief Returns \a a to the power of 2 +*/ +XSMATHINLINE XsReal XsMath_pow2(XsReal a) +{ + return a * a; +} + +/*! \brief Returns \a a to the power of 3 +*/ +XSMATHINLINE XsReal XsMath_pow3(XsReal 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); +} + +/*! \brief Returns non-zero if \a x is finite +*/ +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_NN: + case _FPCLASS_ND: + case _FPCLASS_NZ: + case _FPCLASS_PZ: + case _FPCLASS_PD: + case _FPCLASS_PN: + return 1; + + default: + return _finite(x); + } +#elif defined(isfinite) || defined(__APPLE__) + return isfinite(x); +#elif defined(__ANDROID_API__) + return finite(x); +#elif __GNUC__ +#ifdef __cplusplus + return std::isfinite(x); +#else + return isfinite(x); +#endif +#elif defined(_ADI_COMPILER) + return !(isnan(x) || isinf(x)); +#else + return 1; +#endif +} + +/*! \brief Returns \a d integer converted from a single precision floating point value +*/ +XSMATHINLINE2 int32_t XsMath_floatToLong(float d) +{ + 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); +} + +/*! \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); +} + +/*! \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); +} + +#ifdef __cplusplus + #ifndef XSMATH2_H + #include "xsmath2.h" + #endif +#endif + +/*! @} */ + +#endif diff --git a/extern/xspublic/xstypes/xsmath2.h b/extern/xspublic/xstypes/xsmath2.h new file mode 100644 index 0000000..8109f95 --- /dev/null +++ b/extern/xspublic/xstypes/xsmath2.h @@ -0,0 +1,151 @@ + +// Copyright (c) 2003-2022 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 XSMATH2_H +#define XSMATH2_H + +#ifndef XSMATH_H + #include "xsmath.h" +#endif + +#ifdef __cplusplus +namespace XsMath +{ +#ifdef __GNUC__ + #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; +#ifdef __GNUC__ + #pragma GCC diagnostic pop +#endif + +//! \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_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_pow3 +XSMATHINLINE XsReal pow3(XsReal a) +{ + return XsMath_pow3(a); +} + +//! \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); +} +#endif +} // namespace +#endif + +#endif diff --git a/extern/xspublic/xstypes/xsmatrix.c b/extern/xspublic/xstypes/xsmatrix.c new file mode 100644 index 0000000..054eba2 --- /dev/null +++ b/extern/xspublic/xstypes/xsmatrix.c @@ -0,0 +1,375 @@ + +// Copyright (c) 2003-2022 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 "xsmatrix.h" +#include +#include +#include "xsdebugcounters.h" +#include +#include "xsmalloc.h" +#include "xsquaternion.h" +#include + +#ifdef __ICCARM__ + #pragma diag_suppress=Pa039 +#endif + +/*! \class XsMatrix + \brief A class that represents a matrix of real numbers +*/ + +/*! \addtogroup cinterface C Interface + @{ +*/ + +/*! \relates XsMatrix \brief Construct the %XsMatrix as a reference to data in \a buffer */ +void XsMatrix_ref(XsMatrix* thisPtr, XsSize rows, XsSize cols, XsSize stride, XsReal* buffer, XsDataFlags flags) +{ + //assert(!(thisPtr->m_flags & XSDF_FixedSize)); + //XsMatrix_destruct(thisPtr); + + *((XsReal**) &thisPtr->m_data) = buffer; + *((XsSize*) &thisPtr->m_flags) = (XsSize) flags; + *((XsSize*) &thisPtr->m_rows) = rows; + *((XsSize*) &thisPtr->m_cols) = cols; + *((XsSize*) &thisPtr->m_stride) = stride; +} + +/*! \relates XsMatrix \brief Init the %XsMatrix and copy the data from \a src into the matrix if \a src is not null */ +void XsMatrix_construct(XsMatrix* thisPtr, XsSize rows, XsSize cols, XsSize stride, const XsReal* src, XsSize srcStride) +{ + XsSize r; + XsSize size = rows * stride; + + if (stride == 0) + { + stride = cols; + size = rows * stride; + } + if (size) + { + // init to size + XsReal* data = (XsReal*) xsMathMalloc(size * sizeof(XsReal)); + assert(data); + *((XsReal**) &thisPtr->m_data) = data; + XsMatrix_incAllocCount(); + } + else + *((XsReal**) &thisPtr->m_data) = 0; + *((XsSize*) &thisPtr->m_flags) = XSDF_Managed; + *((XsSize*) &thisPtr->m_rows) = rows; + *((XsSize*) &thisPtr->m_cols) = cols; + *((XsSize*) &thisPtr->m_stride) = stride; + + if (src && size) + { + if (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)); +#else + XsSize c; + for (c = 0; c < cols; ++c) + thisPtr->m_data[r * stride + c] = src[r * srcStride + c]; +#endif + } + } + } +} + +/*! \relates XsMatrix \brief Init the %XsMatrix and copy the data from \a src into the matrix if \a src is not null */ +void XsMatrix_assign(XsMatrix* thisPtr, XsSize rows, XsSize cols, XsSize stride, const XsReal* src, XsSize srcStride) +{ + XsSize r; + XsSize size = rows * stride; + + if (thisPtr->m_flags & XSDF_FixedSize) + { + if (rows == 0 && cols == 0) + { + *((XsSize*) &thisPtr->m_flags) |= XSDF_Empty; + return; + } + + assert(thisPtr->m_rows == rows && thisPtr->m_cols == cols); + stride = thisPtr->m_stride; + size = thisPtr->m_rows * stride; + *((XsSize*) &thisPtr->m_flags) &= ~XSDF_Empty; + } + else + { + if (thisPtr->m_rows == rows && thisPtr->m_cols == cols && + (stride == 0 || stride == thisPtr->m_stride)) + { + stride = thisPtr->m_stride; + size = rows * stride; + } + else + { + if (stride == 0) + { + stride = cols; + size = rows * stride; + } + 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)); + assert(data); + *((XsReal**) &thisPtr->m_data) = data; + *((XsSize*) &thisPtr->m_flags) = XSDF_Managed; + XsMatrix_incAllocCount(); + } + } + *((XsSize*) &thisPtr->m_rows) = rows; + *((XsSize*) &thisPtr->m_cols) = cols; + *((XsSize*) &thisPtr->m_stride) = stride; + } + } + if (src && size) + { + 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)); +#else + XsSize c; + for (c = 0; c < cols; ++c) + thisPtr->m_data[r * stride + c] = src[r * srcStride + c]; +#endif + } + } + } +} + +/*! \relates XsMatrix \brief Clear the XsMatrix and release allocated resources */ +void XsMatrix_destruct(XsMatrix* thisPtr) +{ + if (thisPtr->m_data && (thisPtr->m_flags & XSDF_Managed)) + { + // clear contents + xsMathFree((void*) thisPtr->m_data); + XsMatrix_incFreeCount(); + } + // init to 0 + if (!(thisPtr->m_flags & XSDF_FixedSize)) + { + *((XsReal**) &thisPtr->m_data) = 0; + *((XsSize*) &thisPtr->m_rows) = 0; + *((XsSize*) &thisPtr->m_cols) = 0; + *((XsSize*) &thisPtr->m_stride) = 0; + *((XsSize*) &thisPtr->m_flags) = 0; + } + else + *((XsSize*) &thisPtr->m_flags) |= XSDF_Empty; +} + +/*! \relates XsMatrix \brief Copy the contents of \a copy to the %XsMatrix */ +void XsMatrix_copy(XsMatrix* copy, XsMatrix const* src) +{ + if (copy == src) + return; + XsMatrix_assign(copy, src->m_rows, src->m_cols, 0, src->m_data, src->m_stride); +} + +/*! \relates XsMatrix \brief Set all the values in the matrix to zero */ +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); + 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); + } +#else + 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; +#endif +} + +/*! \relates XsMatrix \brief Returns not zero if the matrix contains no values */ +int XsMatrix_empty(const XsMatrix* thisPtr) +{ + return (thisPtr->m_rows == 0) || (thisPtr->m_cols == 0) || (thisPtr->m_flags & XSDF_Empty); +} + +/*! \relates XsMatrix \brief Multiplies all values in this XsMatrix by \a scalar + \param scalar : Value to multiply by + \param dest : The XsMatrix to store the result in +*/ +void XsMatrix_multiplyScalar(const XsMatrix* thisPtr, XsReal scalar, XsMatrix* dest) +{ + 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; +} + +/*! \relates XsMatrix + \brief Returns the offset in the data for accessing the value at \a row and \a column + \param row The row of the value + \param column The column of the value + \returns The offset of the requested item in the internal buffer +*/ +XsSize XsMatrix_offset(const XsMatrix* thisPtr, XsSize row, XsSize column) +{ + return XsMatrix_offsetM(thisPtr, row, column); +} + +//! \relates XsMatrix \brief Returns the data value at \a row and \a column +XsReal XsMatrix_value(const XsMatrix* thisPtr, XsSize row, XsSize column) +{ + return thisPtr->m_data[XsMatrix_offsetM(thisPtr, row, column)]; +} + + +//! \relates XsMatrix \brief Sets the data \a value at \a row and \a column +void XsMatrix_setValue(XsMatrix* thisPtr, XsSize row, XsSize column, XsReal value) +{ + thisPtr->m_data[XsMatrix_offsetM(thisPtr, row, column)] = value; +} + +//! \relates XsMatrix \brief Returns not zero if the dimensions of the %XsMatrix are equal to \a rows and \a columns +int XsMatrix_dimensionsMatch(const XsMatrix* thisPtr, XsSize rows, XsSize columns) +{ + return thisPtr->m_rows == rows && thisPtr->m_cols == columns; +} + +/*! \relates XsMatrix \brief Get an orientation matrix representation of the quaternion. */ +void XsMatrix_fromQuaternion(XsMatrix* thisPtr, const XsQuaternion* quat) +{ + XsReal q00, q11, q22, q33, q01, q02, q03, q12, q13, q23; + + if (XsQuaternion_empty(quat)) + { + XsMatrix_destruct(thisPtr); + 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; + + 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; + + XsMatrix_assign(thisPtr, 3, 3, 3, 0, 0); + + XsMatrix_setValue(thisPtr, 0, 0, (q00 + q11 - q22) - q33); + XsMatrix_setValue(thisPtr, 0, 1, (q12 - q03) * XsMath_two); + XsMatrix_setValue(thisPtr, 0, 2, (q13 + q02) * XsMath_two); + + XsMatrix_setValue(thisPtr, 1, 0, (q12 + q03) * XsMath_two); + XsMatrix_setValue(thisPtr, 1, 1, (q00 - q11) + (q22 - q33)); + XsMatrix_setValue(thisPtr, 1, 2, (q23 - q01) * XsMath_two); + + XsMatrix_setValue(thisPtr, 2, 0, (q13 - q02) * XsMath_two); + XsMatrix_setValue(thisPtr, 2, 1, (q23 + q01) * XsMath_two); + XsMatrix_setValue(thisPtr, 2, 2, ((q00 - q11) - q22) + q33); +} + +/*! \relates XsMatrix \brief Swap the contents of \a a and \a b + \details This function swaps the internal buffers so no actual data is moved around. + This won't work for unmanaged data such as fixed size matrices (XsMatrix3x3) + \param a Object whose contents will be placed in \a b + \param b Object whose contents will be placed in \a a +*/ +void XsMatrix_swap(XsMatrix* a, XsMatrix* b) +{ +#ifdef __ICCARM__ +#pragma diag_suppress=Pe370 +#endif + XsMatrix tmp; + if ((!a->m_data || (a->m_flags & XSDF_Managed)) && (!b->m_data || (b->m_flags & XSDF_Managed))) + { + *((XsReal**) &tmp.m_data) = a->m_data; + *((XsSize*) &tmp.m_rows) = a->m_rows; + *((XsSize*) &tmp.m_cols) = a->m_cols; + *((XsSize*) &tmp.m_stride) = a->m_stride; + *((XsSize*) &tmp.m_flags) = a->m_flags; + + *((XsReal**) &a->m_data) = b->m_data; + *((XsSize*) &a->m_rows) = b->m_rows; + *((XsSize*) &a->m_cols) = b->m_cols; + *((XsSize*) &a->m_stride) = b->m_stride; + *((XsSize*) &a->m_flags) = b->m_flags; + + *((XsReal**) &b->m_data) = tmp.m_data; + *((XsSize*) &b->m_rows) = tmp.m_rows; + *((XsSize*) &b->m_cols) = tmp.m_cols; + *((XsSize*) &b->m_stride) = tmp.m_stride; + *((XsSize*) &b->m_flags) = tmp.m_flags; + } + else + { + XsSize r, c; + XsReal v; + 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; + XsReal* bb = b->m_data + r * b->m_stride; + for (c = 0; c < a->m_cols; ++c, ++aa, ++bb) + { + v = *aa; + *aa = *bb; + *bb = v; + } + } + } +} + +/*! @} */ diff --git a/extern/xspublic/xstypes/xsmatrix.h b/extern/xspublic/xstypes/xsmatrix.h new file mode 100644 index 0000000..197ef1d --- /dev/null +++ b/extern/xspublic/xstypes/xsmatrix.h @@ -0,0 +1,343 @@ + +// Copyright (c) 2003-2022 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 XSMATRIX_H +#define XSMATRIX_H + +#include "xsmath.h" +#include +#include + +struct XsMatrix; +struct XsEuler; +struct XsQuaternion; + +#ifdef __cplusplus +extern "C" { +#endif +#ifndef __cplusplus +#define XSMATRIX_INITIALIZER { NULL, 0, 0, 0, XSDF_Managed } +typedef struct XsMatrix XsMatrix; +#endif + +XSTYPES_DLL_API void XsMatrix_ref(XsMatrix* thisPtr, XsSize rows, XsSize cols, XsSize stride, XsReal* buffer, XsDataFlags flags); +XSTYPES_DLL_API void XsMatrix_construct(XsMatrix* thisPtr, XsSize rows, XsSize cols, XsSize stride, const XsReal* src, XsSize srcStride); +XSTYPES_DLL_API void XsMatrix_assign(XsMatrix* thisPtr, XsSize rows, XsSize cols, XsSize stride, const XsReal* src, XsSize srcStride); +XSTYPES_DLL_API void XsMatrix_destruct(XsMatrix* thisPtr); +XSTYPES_DLL_API void XsMatrix_copy(XsMatrix* copy, XsMatrix const* src); +XSTYPES_DLL_API void XsMatrix_setZero(XsMatrix* thisPtr); +XSTYPES_DLL_API int XsMatrix_empty(const XsMatrix* thisPtr); +XSTYPES_DLL_API void XsMatrix_multiplyScalar(const XsMatrix* thisPtr, XsReal scalar, XsMatrix* dest); +XSTYPES_DLL_API XsSize XsMatrix_offset(const XsMatrix* thisPtr, XsSize row, XsSize column); +XSTYPES_DLL_API XsReal XsMatrix_value(const XsMatrix* thisPtr, XsSize row, XsSize column); +XSTYPES_DLL_API void XsMatrix_setValue(XsMatrix* thisPtr, XsSize row, XsSize column, XsReal value); +XSTYPES_DLL_API int XsMatrix_dimensionsMatch(const XsMatrix* thisPtr, XsSize rows, XsSize columns); +XSTYPES_DLL_API void XsMatrix_fromQuaternion(XsMatrix* thisPtr, const struct XsQuaternion* quat); +XSTYPES_DLL_API void XsMatrix_swap(XsMatrix* a, XsMatrix* b); + +#define XsMatrix_offsetM(thisPtr, row, column) (thisPtr->m_stride*row + column) + +#ifdef __cplusplus +} // extern "C" +#endif +#ifndef XSENS_NO_PACK + #pragma pack(push, 1) +#endif +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 + const XsSize m_stride; //!< Number of items per row in memory (usually equal to cols but not always) + 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; + } +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) + : m_data(0) + , m_rows(0) + , m_cols(0) + , m_stride(0) + , m_flags(0) + { + if (rows && cols) + XsMatrix_construct(this, rows, cols, strde ? strde : cols, dat, 0); + } + + /*! \brief Initialize an XsMatrix object from the \a other XsMatrix */ + inline XsMatrix(const XsMatrix& other) + : m_data(0) + , m_rows(0) + , m_cols(0) + , m_stride(0) + , m_flags(0) + { + XsMatrix_copy(this, &other); + } + +#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) + , m_rows(0) + , m_cols(0) + , m_stride(0) + , m_flags(0) + { + if (!(other.m_flags & XSDF_Managed)) + XsMatrix_copy(this, &other); + else + XsMatrix_swap(this, &other); + } +#endif + + /*! \brief Initialize an XsMatrix object that references the data passed in \a ref. \a rows, \a cols and \a stride can be used to specify the layout of the data */ + inline explicit XsMatrix(XsReal* ref, XsSize rows, XsSize cols, XsSize stride, XsDataFlags flags /* = XSDF_None */) + : m_data(ref) + , m_rows(rows) + , m_cols(cols) + , m_stride(stride) + , m_flags((XsSize) flags) + { + } + + /*! \brief Initialize a copy of \a other in an XsMatrix object that references the data passed in \a ref. \a rows, \a cols and \a stride can be used to specify the layout of the data */ + inline explicit XsMatrix(const XsMatrix& other, XsReal* ref, XsSize rows, XsSize cols, XsSize stride, XsDataFlags flags /* = XSDF_None */) + : m_data(ref) + , m_rows(rows) + , m_cols(cols) + , m_stride(stride) + , m_flags((XsSize) flags) + { + XsMatrix_copy(this, &other); + } + + //! \brief \copybrief XsMatrix_fromQuaternion + inline explicit XsMatrix(const XsQuaternion& quat) + : m_data(0) + , m_rows(0) + , m_cols(0) + , m_stride(0) + , m_flags(0) + { + XsMatrix_fromQuaternion(this, &quat); + } + + //! \copydoc XsMatrix_destruct + inline ~XsMatrix() + { + XsMatrix_destruct(this); + } + + /*! \brief Resize the matrix to the specified number of \a rows and \a cols, destroying its current contents */ + inline void setSize(XsSize rows, XsSize cols, XsSize stride = 0) + { + XsMatrix_assign(this, rows, cols, stride, 0, 0); + } + + /*! \brief \copybrief XsMatrix_copy */ + inline XsMatrix& operator=(const XsMatrix& other) + { + XsMatrix_copy(this, &other); + return *this; + } + + //! \brief \copybrief XsMatrix_empty + inline bool empty() const + { + return 0 != XsMatrix_empty(this); + } + + //! \brief \copybrief XsMatrix_setZero + inline void setZero() + { + XsMatrix_setZero(this); + } + + //! \copydoc XsMatrix_offset */ + inline XsSize offset(XsSize row, XsSize column) const + { + return XsMatrix_offset(this, row, column); + } + + /*! \brief Returns the value at \a row and \a column in the matrix */ + inline XsReal value(XsSize row, XsSize column) const + { + return m_data[XsMatrix_offset(this, row, column)]; + } + + /*! \brief Sets the \a value at \a row and \a column in the matrix */ + inline void setValue(XsSize row, XsSize column, XsReal value) + { + m_data[XsMatrix_offsetM(this, row, column)] = value; + } + + /*! \brief Returns a pointer to the data in \a row */ + inline const XsReal* operator[](XsSize row) const + { + return &m_data[XsMatrix_offsetM(this, row, 0)]; + } + + /*! \brief Returns a reference to the data in \a row */ + inline XsReal* operator[](XsSize row) + { + return &m_data[XsMatrix_offsetM(this, row, 0)]; + } + + /*! \brief \copybrief XsMatrix_multiplyScalar */ + inline XsMatrix operator*(XsReal scalar) const + { + XsMatrix tmp(m_rows, m_cols); + XsMatrix_multiplyScalar(this, scalar, &tmp); + return tmp; + } + + /*! \brief \copybrief XsMatrix_fromQuaternion */ + inline XsMatrix& fromQuaternion(const XsQuaternion& quat) + { + XsMatrix_fromQuaternion(this, &quat); + return *this; + } + + /*! \brief Fill the matrix with zeroes */ + inline void zero() + { + for (XsSize r = 0; r < m_rows; ++r) + for (XsSize c = 0; c < m_cols; ++c) + m_data[XsMatrix_offsetM(this, r, c)] = XsMath_zero; + } + + /*! \brief Return the number of rows in the matrix */ + inline XsSize rows() const + { + return m_rows; + } + + /*! \brief Return the number of columns in the matrix */ + inline XsSize cols() const + { + return m_cols; + } + + /*! \brief Return the stride of the matrix. + \details The stride of a matrix is for internal administration. It defines the number of items + in a row in the data buffer. This is always greater than or equal to the number of columns. + Especially for matrices that reference a part of another matrix this may differ from the + cols() value. + \returns The stride of the matrix. + */ + inline XsSize stride() const + { + return m_stride; + } + + //! \brief Return a const pointer to the internal data + inline const XsReal* data() const + { + return m_data; + } + + //! \brief Returns true if \a other is numerically identical to this + inline bool operator ==(const XsMatrix& other) const + { + if (this == &other) + return true; + if (m_rows != other.m_rows || m_cols != other.m_cols) + return false; + for (XsSize r = 0; r < m_rows; ++r) + for (XsSize c = 0; c < m_cols; ++c) + if (m_data[XsMatrix_offsetM(this, r, c)] != other.m_data[XsMatrix_offsetM((&other), r, c)]) + return false; + return true; + } + + //! \brief Returns true if the values of this and \a other are within \a tolerance of each other + inline bool isEqual(const XsMatrix& other, XsReal tolerance) const + { + if (this == &other) + return true; + if (m_rows != other.m_rows || m_cols != other.m_cols) + return false; + for (XsSize r = 0; r < m_rows; ++r) + for (XsSize c = 0; c < m_cols; ++c) + if (fabs(m_data[XsMatrix_offsetM(this, r, c)] - other.m_data[XsMatrix_offsetM((&other), r, c)]) > tolerance) + return false; + return true; + } + + /*! \brief Swap the contents of \a b with this + \details This function swaps the internal buffers so no actual data is moved around. For unmanaged + data an elementwise swap is done, but only if the matrices are the same size. + \param b Object whose contents will be swapped with this + */ + inline void swap(XsMatrix& b) + { + XsMatrix_swap(this, &b); + } + + /*! \brief swaps the contents \a first with that of \a 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) +#endif + +#ifdef __cplusplus +//! \brief Multiplies all values in the matrix \a m by \a scalar +inline XsMatrix operator *(XsReal scalar, const XsMatrix& m) +{ + return (m * scalar); +} +#endif + +#endif diff --git a/extern/xspublic/xstypes/xsmatrix3x3.c b/extern/xspublic/xstypes/xsmatrix3x3.c new file mode 100644 index 0000000..703b6a4 --- /dev/null +++ b/extern/xspublic/xstypes/xsmatrix3x3.c @@ -0,0 +1,83 @@ + +// Copyright (c) 2003-2022 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 "xsmatrix3x3.h" +#include + +/*! \class XsMatrix3x3 + \brief A class that represents a fixed size (3x3) matrix +*/ + +/*! \addtogroup cinterface C Interface + @{ +*/ + +/*! \relates XsMatrix3x3 \brief Init the %XsMatrix3x3 */ +void XsMatrix3x3_construct(XsMatrix3x3* thisPtr) +{ + XsMatrix_ref(&thisPtr->m_matrix, 3, 3, 3, (XsReal*) thisPtr->m_fixedData, XSDF_FixedSize); +} + +/*! \relates XsMatrix3x3 \brief Init the %XsMatrix3x3 and copy the data from \a src into the matrix if \a src is not null */ +void XsMatrix3x3_assign(XsMatrix3x3* thisPtr, const XsReal* src, XsSize srcStride) +{ + XsSize r, c; + + if (src) + { + if (srcStride == 0 || srcStride == 3) + 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]; + } + } +} + +/*! \relates XsMatrix3x3 \brief Frees the Matrix3x3 */ +void XsMatrix3x3_destruct(XsMatrix3x3* thisPtr) +{ + // don't do anything, no memory needs to be freed, which is what XsMatrix_destruct will figure out + assert(thisPtr->m_matrix.m_flags & XSDF_FixedSize); + (void) thisPtr; + //XsMatrix_destruct(&thisPtr->m_matrix); +} + +/*! \relates XsMatrix3x3 \brief Copy the contents of the %XsMatrix3x3 to \a copy */ +void XsMatrix3x3_copy(XsMatrix* copy, XsMatrix3x3 const* src) +{ + XsMatrix_copy(copy, &src->m_matrix); +} + +/*! @} */ diff --git a/extern/xspublic/xstypes/xsmatrix3x3.h b/extern/xspublic/xstypes/xsmatrix3x3.h new file mode 100644 index 0000000..5f83056 --- /dev/null +++ b/extern/xspublic/xstypes/xsmatrix3x3.h @@ -0,0 +1,109 @@ + +// Copyright (c) 2003-2022 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 XSMATRIX3X3_H +#define XSMATRIX3X3_H + +#include "xsmatrix.h" + +struct XsMatrix3x3; +#ifdef __cplusplus +extern "C" { +#endif +#ifndef __cplusplus +typedef struct XsMatrix3x3 XsMatrix3x3; +#endif + + +XSTYPES_DLL_API void XsMatrix3x3_construct(XsMatrix3x3* thisPtr); +XSTYPES_DLL_API void XsMatrix3x3_assign(XsMatrix3x3* thisPtr, const XsReal* src, XsSize srcStride); +XSTYPES_DLL_API void XsMatrix3x3_destruct(XsMatrix3x3* thisPtr); +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. +*/ +struct XsMatrix3x3 : public XsMatrix +{ + XSCPPPROTECTED +#endif +#ifndef __cplusplus + struct XsMatrix3x3 + { + struct XsMatrix m_matrix; //!< The underlying XsMatrix +#endif + 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) + { + } + + //! \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 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 + +#endif diff --git a/extern/xspublic/xstypes/xsmessage.c b/extern/xspublic/xstypes/xsmessage.c new file mode 100644 index 0000000..775b45f --- /dev/null +++ b/extern/xspublic/xstypes/xsmessage.c @@ -0,0 +1,1265 @@ + +// Copyright (c) 2003-2022 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 "xsmessage.h" +#include +#include // memset +#include "xsbusid.h" +#include + +////////////////////////////////////////////////////////////////////////////////////////// +// 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_DATAEXT0 6 + +#define XS_SELFTEST_OK 0x1FF + +// message data lengths +#define XS_LEN_TRANSPORTMODE 1 +#define XS_LEN_DEVICEID 4 +#define XS_LEN_INITBUSRESULTS 4 +#define XS_LEN_PERIOD 2 +#define XS_LEN_BUSPWR 2 +#define XS_LEN_DATALENGTH 2 +#define XS_LEN_CONFIGURATION 118 +#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_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_SYNCINMODE 2 +#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 +#define XS_LEN_SYNCOUTPULSEWIDTH 4 +#define XS_LEN_ERRORMODE 2 +#define XS_LEN_TRANSMITDELAY 2 +#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_LOCATIONID 2 +#define XS_LEN_EXTOUTPUTMODE 2 +#define XS_LEN_INITTRACKMODE 2 +#define XS_LEN_STOREFILTERSTATE 0 +#define XS_LEN_UTCTIME 12 +#define XS_LEN_FILTERPROFILELABEL 20 +#define XS_LEN_FILTERPROFILEFULL (1+1+XS_LEN_FILTERPROFILELABEL) +#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_GNSSLEVERARM 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_GNSSSTATUS (1+5*16) +#define XS_LEN_CLIENTUSAGE 1 +#define XS_LEN_CLIENTPRIORITY 1 +#define XS_LEN_WIRELESSCONFIG 6 +#define XS_LEN_INFOREQUEST 1 +#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_ORIENT_EULERDATA 12 +#define XS_LEN_ORIENT_MATRIXSTA 36 +#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_ORIENT_QUATDATA_FLT 4 +#define XS_LEN_ORIENT_EULERDATA_FLT 3 +#define XS_LEN_ORIENT_MATRIXSTA_FLT 9 + +#pragma pack(push, 1) +// little endian +union Itypes +{ + int64_t i64; + struct + { + int32_t i1, i0; + } i32; + struct + { + int16_t s3, s2, s1, s0; + } i16; + struct + { + signed char b7, b6, b5, b4, b3, b2, b1, b0; + } i8; + + double d; + struct + { + float f1, f0; + } f32; +}; +#pragma pack(pop) + + +/*! \addtogroup cinterface C Interface + @{ +*/ + +typedef union Itypes Itypes; + +/*! \brief Calculate the sum of the values in the buffer + \details This function calculates the sum of the byte values for the first \a count bytes in the \a buffer + \param buffer An array of (unsigned) bytes + \param count The number of bytes in the buffer + \returns The unsigned sum of the byte values in the buffer modulo 256 +*/ +uint8_t byteSum(const uint8_t* buffer, XsSize count) +{ + register uint8_t sum = 0; + for (; count; --count, sum += *(buffer++)); + return sum; +} + +/*! \brief Get the buffer at offset \a 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; + 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) +{ + 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) +{ + 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) +{ + if (thisPtr->m_autoUpdateChecksum) + { + thisPtr->m_checksum[0] += byteSum(XsMessage_getDataBuffer(thisPtr, offset), sizeofValue); + thisPtr->m_checksum[0] -= byteSum(value, sizeofValue); + } +} + +/*! \brief Swap the endianness based on the data 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); + } +} + +/*! \brief Get data of size \a size at \a offset, and put it byteswapped into \a value + \param value The value to write to + \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) +{ + memcpy(value, (void const*) XsMessage_cdataAtOffset(thisPtr, offset), size); + swapEndian(value, size); +} + +/*! \brief Set value \a value of size \a size byteswapped at \a offset + \param value The value to write to + \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* dest; + XsMessage_ensureDataSize(thisPtr, offset, size); + XsMessage_updateChecksumWithValue(thisPtr, value, size, offset); + dest = XsMessage_dataAtOffset(thisPtr, offset); + memcpy(dest, value, size); + swapEndian(dest, size); +} + +/*! \brief This function initializes the %XsMessage object and reserves \a dataSize bytes for data + + \param[in] dataSize the expected size of the message payload +*/ +void XsMessage_constructSized(XsMessage* thisPtr, XsSize dataSize) +{ + XsSize msgSize; + XsMessageHeader* hdr; + + if (dataSize < 255) + msgSize = dataSize + 5; + else + msgSize = dataSize + 7; + + XsByteArray_construct(&thisPtr->m_message, msgSize, 0); + memset(thisPtr->m_message.m_data, 0, msgSize); + hdr = (XsMessageHeader*)(void*) thisPtr->m_message.m_data; + hdr->m_preamble = XS_PREAMBLE; + hdr->m_messageId = 0; + hdr->m_busId = XS_BID_MASTER; + + if (dataSize < XS_EXTLENCODE) + { + 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; + } + 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_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] -= hdr->m_busId; +} + +/*! \brief This function initializes the %XsMessage object +*/ +void XsMessage_construct(XsMessage* thisPtr) +{ + XsMessage_constructSized(thisPtr, 0); +} + +/*! \brief Construct an XsMessage as a copy of XsMessage \a src + \param src The source message to be copied +*/ +void XsMessage_copyConstruct(XsMessage* thisPtr, XsMessage const* src) +{ + if (!src) + XsMessage_construct(thisPtr); + else + { + XsMessageHeader* hdr; + XsSize dataSize; + XsArray_copyConstruct(&thisPtr->m_message, &src->m_message); + thisPtr->m_autoUpdateChecksum = src->m_autoUpdateChecksum; + 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]; + else + *((uint8_t**) &thisPtr->m_checksum) = &hdr->m_datlen.m_data[dataSize]; + } +} + +/*! \brief This function reinitializes the %XsMessage object and reserves \a dataSize bytes for data + \param[in] dataSize the expected size of the message payload +*/ +void XsMessage_assign(XsMessage* thisPtr, XsSize dataSize) +{ + XsMessage_destruct(thisPtr); + XsMessage_constructSized(thisPtr, dataSize); +} + +/*! \brief This function initializes the %XsMessage object and reserves \a msgSize bytes for data, it then copies in the data from \a src + \param msgSize the size of the data pointed to by src + \param src the data to load the message from + \note This is a constructor! Previous contents will be overwritten without first freeing them! +*/ +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]; +} + +/*! \brief This function clears the data in the message + +*/ +void XsMessage_destruct(XsMessage* thisPtr) +{ + XsArray_destruct(&thisPtr->m_message); + *((uint8_t**) &thisPtr->m_checksum) = 0; +} + +/*! \brief This function copies from \a thisPtr to \a copy + \param copy the object to copy to +*/ +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]; + copy->m_autoUpdateChecksum = thisPtr->m_autoUpdateChecksum; +} + +/*! \brief This function returns the datasize of the message in \a thisptr + \returns the size of the message payload +*/ +XsSize XsMessage_dataSize(XsMessage const* thisPtr) +{ + XsMessageHeader const* hdr; + + if (!thisPtr->m_message.m_data) + return 0; + + 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 + return ((XsSize) hdr->m_length); +} + +/*! \brief This function returns a const pointer to the \a offset in the data of the message in \a thisptr + + \param offset the offset of the data to be returned + + \returns a pointer to the data at offset \a offset +*/ +const uint8_t* XsMessage_constData(XsMessage const* thisPtr, XsSize offset) +{ + if (!thisPtr->m_message.m_data) + return 0; + return XsMessage_cdataAtOffset(thisPtr, offset); +} + +/*! \brief This function returns a const pointer to the header of the message in \a thisptr + + + \returns a pointer to the start of the message +*/ +const uint8_t* XsMessage_getMessageStart(XsMessage const* thisPtr) +{ + return thisPtr->m_message.m_data; +} + +/*! \brief Return the length of the message buffer. + + The function returns the total size of the message, including the checksum. This + is in effect the number of bytes that would be transferred if the message were to + be sent over a communications channel. + + \returns the total message size +*/ +XsSize XsMessage_getTotalMessageSize(XsMessage const* thisPtr) +{ + XsMessageHeader* hdr; + + if (!thisPtr->m_message.m_data) + return 0; + + 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 + return ((XsSize) hdr->m_length) + 5; +} + +/*! \brief Returns the byte value at \a offset in the data of the message + + \param offset the offset in the payload at which to read data + + \returns the byte at offset \a offset in the message payload +*/ +uint8_t XsMessage_getDataByte(XsMessage const* thisPtr, XsSize offset) +{ + return *XsMessage_cdataAtOffset(thisPtr, offset); +} + +/*! \brief Returns the short value at \a offset in the data of the message + + \param offset the offset in the payload at which to read data + + \returns the 16-bit integer value at offset \a offset in the message payload +*/ +uint16_t XsMessage_getDataShort(XsMessage const* thisPtr, XsSize offset) +{ + uint16_t ret; + XsMessage_getEndianCorrectData(thisPtr, &ret, sizeof(ret), offset); + return ret; +} + +/*! \brief Returns the long value at \a offset in the data of the message + + \param offset the offset in the payload at which to read data + + \returns the 32-bit integer value at offset \a offset in the message payload +*/ +uint32_t XsMessage_getDataLong(XsMessage const* thisPtr, XsSize offset) +{ + uint32_t ret; + XsMessage_getEndianCorrectData(thisPtr, &ret, sizeof(ret), offset); + return ret; +} + +/*! \brief Returns the long value at \a offset in the data of the message + + \param offset the offset in the payload at which to read data + + \returns the 64-bit integer value at offset \a offset in the message payload +*/ +uint64_t XsMessage_getDataLongLong(XsMessage const* thisPtr, XsSize offset) +{ + uint64_t ret; + XsMessage_getEndianCorrectData(thisPtr, &ret, sizeof(ret), offset); + return ret; +} + +/*! \brief Returns the float value at \a offset in the data of the message + + \param offset the offset in the payload at which to read data + + \returns the single precision float value at offset \a offset in the message payload +*/ +float XsMessage_getDataFloat(XsMessage const* thisPtr, XsSize offset) +{ + float ret; + XsMessage_getEndianCorrectData(thisPtr, &ret, sizeof(ret), offset); + return ret; +} + +/*! \brief Returns the double at \a offset in the data of the message + \param offset the offset in the payload at which to read data + + \returns the double precision floating point value at offset \a offset in the message payload +*/ +double XsMessage_getDataDouble(XsMessage const* thisPtr, XsSize offset) +{ + double ret; + XsMessage_getEndianCorrectData(thisPtr, &ret, sizeof(ret), offset); + return ret; +} + +/*! \brief Returns the F12.20 value at \a offset in the data of the message + \param offset the offset in the payload at which to read data + + \returns the 12.20 fixed point value at offset \a offset in the message payload +*/ +double XsMessage_getDataF1220(XsMessage const* thisPtr, XsSize offset) +{ + int32_t tmp; + Itypes rv; + tmp = (int32_t) XsMessage_getDataLong(thisPtr, offset); + + rv.d = ((double) tmp) / 1048576.0; + rv.i64 = (rv.i64 & ~1LL) | (tmp & 1); + return rv.d; +} + +/*! \brief Returns the F16.32 value at \a offset in the data of the message + \param offset the offset in the payload at which to read data + + \returns the 16.32 fixed point value at offset \a offset in the message payload +*/ +double XsMessage_getDataFP1632(XsMessage const* thisPtr, XsSize offset) +{ + int16_t fpint; + int32_t fpfrac; + Itypes fp, rv; + + fpfrac = (int32_t) XsMessage_getDataLong(thisPtr, offset); + fpint = (int16_t) XsMessage_getDataShort(thisPtr, offset + 4); + + fp.i32.i0 = fpint; + fp.i32.i1 = fpfrac; + + rv.d = (double) fp.i64 / 4294967296.0; + rv.i64 = (rv.i64 & ~1LL) | (fpfrac & 1); + return rv.d; +} + +/*! \brief Returns a const pointer to the data buffer of the message + + \param offset the offset in the payload at which to read data + + + \returns a const pointer to the data buffer of the message +*/ +const uint8_t* XsMessage_getDataBuffer(XsMessage const* thisPtr, XsSize offset) +{ + return XsMessage_constData(thisPtr, offset); +} + +/*! \brief Set the byte at \a offset in the message to \a value + + \param value the 8-bit value to set + \param offset the offset in the message payload at which to write the data +*/ +void XsMessage_setDataByte(XsMessage* thisPtr, uint8_t value, XsSize offset) +{ + XsMessage_setEndianCorrectData(thisPtr, &value, sizeof(value), offset); +} + +/*! \brief Sets the short at \a offset in the message to \a value + + \param value the 16-bit value to set + \param offset the offset in the message payload at which to write the data +*/ +void XsMessage_setDataShort(XsMessage* thisPtr, uint16_t value, XsSize offset) +{ + XsMessage_setEndianCorrectData(thisPtr, &value, sizeof(value), offset); +} + +/*! \brief Sets the long at \a offset in the message to \a value + + \param value the 32-bit value to set + \param offset the offset in the message payload at which to write the data +*/ +void XsMessage_setDataLong(XsMessage* thisPtr, uint32_t value, XsSize offset) +{ + XsMessage_setEndianCorrectData(thisPtr, &value, sizeof(value), offset); +} + +/*! \brief Sets the long at \a offset in the message to \a value + + \param value the 64-bit value to set + \param offset the offset in the message payload at which to write the data +*/ +void XsMessage_setDataLongLong(XsMessage* thisPtr, uint64_t value, XsSize offset) +{ + XsMessage_setEndianCorrectData(thisPtr, &value, sizeof(value), offset); +} + + +/*! \brief Sets the float at \a offset in the message to \a value + + \param value the single precision floating point value to set + \param offset the offset in the message payload at which to write the data +*/ +void XsMessage_setDataFloat(XsMessage* thisPtr, float value, XsSize offset) +{ + XsMessage_setEndianCorrectData(thisPtr, &value, sizeof(value), offset); +} + +/*! \brief Sets the double at \a offset in the message to \a value + + \param value the double precision floating point value to set + \param offset the offset in the message payload at which to write the data +*/ +void XsMessage_setDataDouble(XsMessage* thisPtr, double value, XsSize offset) +{ + XsMessage_setEndianCorrectData(thisPtr, &value, sizeof(value), offset); +} + +/*! \brief Sets the F12.20 at \a offset in the message to \a value + + \param value the 12.20 fixed point value to set + \param offset the offset in the message payload at which to write the data +*/ +void XsMessage_setDataF1220(XsMessage* thisPtr, double value, XsSize offset) +{ + Itypes fp; + uint32_t val; + + fp.d = value; + val = (uint32_t)(int32_t)(value * 1048576.0); + + XsMessage_setDataLong(thisPtr, (val & ~1UL) | (fp.i64 & 1), offset); +} + +/*! \brief Sets the F16.32 at \a offset in the message to \a value + + \param value the 16.32 fixed point value to set + \param offset the offset in the message payload at which to write the data +*/ +void XsMessage_setDataFP1632(XsMessage* thisPtr, double value, XsSize offset) +{ + Itypes fp; + int16_t fpint; + int32_t fpfrac; + int32_t dexp; + uint32_t b; + + fp.d = value; + b = (uint32_t)(fp.i64 & 1); + dexp = ((fp.i32.i0 & (0x7fffffffL)) >> 20) - 1023; + + if (dexp <= 14) + { + fp.i16.s0 = (fp.i16.s0 & 0x000F) | 0x0010; + if (value < 0) + fp.i64 = -fp.i64; + if (dexp > -32) + fp.i64 = fp.i64 >> (20 - dexp); // 52-32 - exponent + else + fp.i64 = fp.i64 >> 52; // this could be optimized? + fpint = fp.i16.s1; + fpfrac = fp.i32.i1; + } + else + { + if (value < 0) + { + fpint = ((int16_t)(uint16_t)(0x8000)); + fpfrac = 0; + } + else + { + fpint = 0x7fff; + fpfrac = -1; + } + } + + XsMessage_setDataLong(thisPtr, (fpfrac & ~1L) | b, offset); + 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 + \remarks The buffersize of will be increased if \a buffer is too large + + \param buffer the buffer to copy + \param size the size of the buffer + \param offset the offset at which to copy the data +*/ +void XsMessage_setDataBuffer(XsMessage* thisPtr, const uint8_t* buffer, XsSize size, XsSize offset) +{ + XsMessage_ensureDataSize(thisPtr, offset, size); + XsMessage_updateChecksumWithValue(thisPtr, buffer, size, offset); + memcpy(XsMessage_dataAtOffset(thisPtr, offset), buffer, size); +} + +/*! \relates XsMessage + \brief Returns the byte size of \a id if the format is a floating point format + + \param id : The XsDataIdentifier to query + \returns Returns the byte size of XsDataIdentifier \a id +*/ +uint8_t XsMessage_getFPValueSize(XsDataIdentifier id) +{ + switch (id & XDI_SubFormatMask) + { + case XDI_SubFormatFloat: + return 4; + + case XDI_SubFormatDouble: + return 8; + + case XDI_SubFormatFp1632: + return 6; + + case XDI_SubFormatFp1220: + return 4; + + default: + return 0; + } +} + +/*! \returns a double converted from float \a f + + \param[in] f The float to convert +*/ +static double convertFromFloat(float f) +{ + Itypes rv, tmp; + tmp.f32.f0 = f; + rv.d = (double) f; + rv.i64 = (rv.i64 & ~1LL) | (tmp.i32.i0 & 1); + return rv.d; +} + +/*! \returns a float converted from double \a d + + \param[in] d The double to cenvert +*/ +static float convertToFloat(double d) +{ + Itypes rv, tmp; + tmp.d = d; + rv.f32.f0 = (float) d; + rv.i32.i0 = (rv.i32.i0 & ~1L) | (tmp.i64 & 1); + return rv.f32.f0; +} + +/*! \brief Return current data values as double, conversion depends on outputSetting. + + \param dest destination array + \param dataIdentifier Data identifier containing data precision + \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) +{ + XsSize i; + for (i = 0; i < numValues; i++) + { + switch (dataIdentifier & XDI_SubFormatMask) + { + case XDI_SubFormatFloat: + *dest++ = convertFromFloat(XsMessage_getDataFloat(thisPtr, offset)); + offset += 4; + break; + + case XDI_SubFormatDouble: + *dest++ = XsMessage_getDataDouble(thisPtr, offset); + offset += 8; + break; + + case XDI_SubFormatFp1632: + *dest++ = XsMessage_getDataFP1632(thisPtr, offset); + offset += 6; + break; + + case XDI_SubFormatFp1220: + *dest++ = XsMessage_getDataF1220(thisPtr, offset); + offset += 4; + break; + + default: + *dest++ = 0; + break; + } + } +} + +/*! \brief Write a number of floating/fixed point values into to the data buffer, conversion depends on outputSettings + + \param dataIdentifier Data Identifier + \param data The data array to be written to the buffer. + \param offset Offset in the data buffer from where to start writing. + \param numValues number of values to be written +*/ +void XsMessage_setDataFPValuesById(XsMessage* thisPtr, XsDataIdentifier dataIdentifier, double const* data, XsSize offset, XsSize numValues) +{ + XsSize i; + for (i = 0; i < numValues; i++) + { + switch (dataIdentifier & XDI_SubFormatMask) + { + case XDI_SubFormatFloat: + XsMessage_setDataFloat(thisPtr, convertToFloat(data[i]), offset); + offset += 4; + break; + + case XDI_SubFormatDouble: + XsMessage_setDataDouble(thisPtr, data[i], offset); + offset += 8; + break; + + case XDI_SubFormatFp1632: + XsMessage_setDataFP1632(thisPtr, data[i], offset); + offset += 6; + break; + + case XDI_SubFormatFp1220: + XsMessage_setDataF1220(thisPtr, data[i], offset); + offset += 4; + break; + + default: + break; + } + } +} + +/*! \brief Return current data values as XsReal, conversion is done automatically based on data identifier. + + This function behaves exactly like XsMessage_getDataFPValuesById, with the exception that it expects an XsReal array. + + \param dest destination array + \param dataIdentifier Data identifier containing data precision + \param offset offset in the data buffer from where to start reading. + \param numValues number of values to be read +*/ +void XsMessage_getDataRealValuesById(XsMessage const* thisPtr, XsDataIdentifier dataIdentifier, XsReal* dest, XsSize offset, XsSize numValues) +{ + XsSize i; + for (i = 0; i < numValues; i++) + { + switch (dataIdentifier & XDI_SubFormatMask) + { + case XDI_SubFormatFloat: +#ifdef XSENS_SINGLE_PRECISION + *dest++ = XsMessage_getDataFloat(thisPtr, offset); +#else + *dest++ = convertFromFloat(XsMessage_getDataFloat(thisPtr, offset)); +#endif + offset += 4; + break; + + case XDI_SubFormatDouble: +#ifdef XSENS_SINGLE_PRECISION + *dest++ = convertToFloat(XsMessage_getDataDouble(thisPtr, offset)); +#else + *dest++ = XsMessage_getDataDouble(thisPtr, offset); +#endif + offset += 8; + break; + + case XDI_SubFormatFp1632: +#ifdef XSENS_SINGLE_PRECISION + *dest++ = convertToFloat(XsMessage_getDataFP1632(thisPtr, offset)); +#else + *dest++ = XsMessage_getDataFP1632(thisPtr, offset); +#endif + offset += 6; + break; + + case XDI_SubFormatFp1220: +#ifdef XSENS_SINGLE_PRECISION + *dest++ = convertToFloat(XsMessage_getDataF1220(thisPtr, offset)); +#else + *dest++ = XsMessage_getDataF1220(thisPtr, offset); +#endif + offset += 4; + break; + + default: + *dest++ = 0; + break; + } + } +} + +/*! \brief Write a number of floating/fixed point values into to the data buffer, conversion depends on data identifier + + \param dataIdentifier Data Identifier + \param data The data array to be written to the buffer. + \param offset Offset in the data buffer from where to start writing. + \param numValues number of values to be written +*/ +void XsMessage_setDataRealValuesById(XsMessage* thisPtr, XsDataIdentifier dataIdentifier, XsReal const* data, XsSize offset, XsSize numValues) +{ + XsSize i; + for (i = 0; i < numValues; i++) + { + switch (dataIdentifier & XDI_SubFormatMask) + { + case XDI_SubFormatFloat: + XsMessage_setDataFloat(thisPtr, convertToFloat(data[i]), offset); + offset += 4; + break; + + case XDI_SubFormatDouble: + XsMessage_setDataDouble(thisPtr, data[i], offset); + offset += 8; + break; + + case XDI_SubFormatFp1632: + XsMessage_setDataFP1632(thisPtr, data[i], offset); + offset += 6; + break; + + case XDI_SubFormatFp1220: + XsMessage_setDataF1220(thisPtr, data[i], offset); + offset += 4; + break; + + default: + break; + } + } +} + +/*! \brief Computes the checksum for the message + \returns the computed checksum +*/ +uint8_t XsMessage_computeChecksum(XsMessage const* thisPtr) +{ + XsSize i, msgSize; + uint8_t cs = 0; + + msgSize = XsMessage_getTotalMessageSize(thisPtr) - 1; + for (i = 1; i < msgSize; ++i) + cs -= thisPtr->m_message.m_data[i]; + + return cs; +} + +/*! \brief Update the checksum for the message +*/ +void XsMessage_recomputeChecksum(XsMessage* thisPtr) +{ + assert(thisPtr->m_checksum); + thisPtr->m_checksum[0] = XsMessage_computeChecksum(thisPtr); +} + +/*! \brief Returns non-zero if the checksum inside the message is correct for the message, zero otherwise + \returns true (non-zero) if the checksum inside the message is correct, false (zero) otherwise +*/ +int XsMessage_isChecksumOk(XsMessage const* thisPtr) +{ + assert(thisPtr->m_checksum); + return thisPtr->m_checksum[0] == XsMessage_computeChecksum(thisPtr); +} + +/*! \brief Returns a pointer to the message header for this message + \returns a pointer to the header for this XsMessage +*/ +XsMessageHeader* XsMessage_getHeader(XsMessage* thisPtr) +{ + 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; +} + +/*! \brief Test if this message is empty + \returns non-zero if this message is empty, zero otherwise +*/ +int XsMessage_empty(XsMessage const* thisPtr) +{ + XsMessageHeader* hdr = (XsMessageHeader*)(void*) thisPtr->m_message.m_data; + if (!hdr) + return 1; + return (hdr->m_messageId == 0 && hdr->m_busId == XS_BID_MASTER); +} + +/*! \brief Resize the buffer of message to \a newSize bytes + + \param newSize the new size of the message payload buffer +*/ +void XsMessage_resizeData(XsMessage* thisPtr, XsSize newSize) +{ + XsSize index, oldSize; + XsByteArray old = XSBYTEARRAY_INITIALIZER; + XsMessageHeader* oldHdr, *newHdr; + uint8_t* oldData, *newData; + + oldSize = XsMessage_dataSize(thisPtr); + if (oldSize == newSize) + return; + + XsArray_swap(&thisPtr->m_message, &old); + XsMessage_constructSized(thisPtr, newSize); + + newHdr = (XsMessageHeader*) thisPtr->m_message.m_data; + oldHdr = (XsMessageHeader*) old.m_data; + if (!oldHdr) // our original message may have been empty / uninitialized + return; + + if (thisPtr->m_autoUpdateChecksum) + thisPtr->m_checksum[0] += newHdr->m_busId; + + newHdr->m_busId = oldHdr->m_busId; + newHdr->m_preamble = oldHdr->m_preamble; + newHdr->m_messageId = oldHdr->m_messageId; + + if (thisPtr->m_autoUpdateChecksum) + thisPtr->m_checksum[0] -= newHdr->m_busId + newHdr->m_messageId; + + if (oldSize >= XS_EXTLENCODE) + oldData = oldHdr->m_datlen.m_extended.m_data; + else + oldData = oldHdr->m_datlen.m_data; + + if (newSize >= XS_EXTLENCODE) + newData = newHdr->m_datlen.m_extended.m_data; + else + newData = newHdr->m_datlen.m_data; + + if (oldSize > newSize) + oldSize = newSize; // speed up the loop + for (index = 0; index < oldSize; ++index) + newData[index] = oldData[index]; + + if (thisPtr->m_autoUpdateChecksum) + thisPtr->m_checksum[0] -= byteSum(oldData, oldSize); + + XsArray_destruct(&old); +} + +/*! \brief Set the bus id for this message to \a busId + + \param busId the bus identifier + + \sa XS_BID_BROADCAST XS_BID_MASTER XS_BID_MT +*/ +void XsMessage_setBusId(XsMessage* thisPtr, uint8_t busId) +{ + XsMessageHeader* hdr; + if (!thisPtr->m_message.m_data) + XsMessage_construct(thisPtr); + + hdr = (XsMessageHeader*)(void*) thisPtr->m_message.m_data; + if (thisPtr->m_autoUpdateChecksum) + thisPtr->m_checksum[0] += hdr->m_busId; + hdr->m_busId = busId; + if (thisPtr->m_autoUpdateChecksum) + thisPtr->m_checksum[0] -= hdr->m_busId; +} + +/*! \brief Set the message id for this message to \a msgId + + \param msgId the message identifier +*/ +void XsMessage_setMessageId(XsMessage* thisPtr, enum XsXbusMessageId msgId) +{ + XsMessageHeader* hdr; + if (!thisPtr->m_message.m_data) + XsMessage_construct(thisPtr); + + 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; + if (thisPtr->m_autoUpdateChecksum) + thisPtr->m_checksum[0] -= hdr->m_messageId; +} + +/*! \brief Create \a count bytes of empty space at \a offset in this message + + \param count the number of bytes to reserve + \param offset the offset at which to allocate the space +*/ +void XsMessage_insertData(XsMessage* thisPtr, XsSize count, XsSize offset) +{ + XsSize index, oldSize, newSize; + XsByteArray old = XSBYTEARRAY_INITIALIZER; + XsMessageHeader* oldHdr, *newHdr; + uint8_t* oldData, *newData; + + if (!count) + return; + + oldSize = XsMessage_dataSize(thisPtr); + newSize = oldSize + count; + + if (newSize < offset + count) + newSize = offset + count; + + XsArray_swap(&thisPtr->m_message, &old); + XsMessage_constructSized(thisPtr, newSize); + + newHdr = (XsMessageHeader*) thisPtr->m_message.m_data; + oldHdr = (XsMessageHeader*) old.m_data; + if (!oldHdr) // our original message may have been empty / uninitialized + return; + + newHdr->m_busId = oldHdr->m_busId; + newHdr->m_preamble = oldHdr->m_preamble; + newHdr->m_messageId = oldHdr->m_messageId; + + if (oldSize >= XS_EXTLENCODE) + oldData = &oldHdr->m_datlen.m_extended.m_data[0]; + else + oldData = &oldHdr->m_datlen.m_data[0]; + + if (newSize >= XS_EXTLENCODE) + newData = &newHdr->m_datlen.m_extended.m_data[0]; + else + newData = &newHdr->m_datlen.m_data[0]; + + if (offset <= oldSize) + { + for (index = 0; index < offset; ++index) + newData[index] = oldData[index]; + } + else + { + for (index = 0; index < oldSize; ++index) + newData[index] = oldData[index]; + for (; index < offset; ++index) + newData[index] = 0; + } + for (; index < oldSize; ++index) + newData[index + count] = oldData[index]; + + if (thisPtr->m_autoUpdateChecksum) + thisPtr->m_checksum[0] -= byteSum(oldData, oldSize); + + XsArray_destruct(&old); +} + +/*! \brief Remove \a count bytes of data from the message at \a offset + + \param count the number of bytes to remove + \param offset the offset at which to remove the bytes +*/ +void XsMessage_deleteData(XsMessage* thisPtr, XsSize count, XsSize offset) +{ + XsSize index, oldSize, newSize; + XsByteArray old = XSBYTEARRAY_INITIALIZER; + XsMessageHeader* oldHdr, *newHdr; + uint8_t* oldData, *newData; + + oldSize = XsMessage_dataSize(thisPtr); + if (!count || offset >= oldSize) + return; + + if (offset + count >= oldSize) + { + XsMessage_resizeData(thisPtr, offset); + return; + } + newSize = oldSize - count; + + XsArray_swap(&thisPtr->m_message, &old); + XsMessage_constructSized(thisPtr, newSize); + + newHdr = (XsMessageHeader*) thisPtr->m_message.m_data; + oldHdr = (XsMessageHeader*) old.m_data; + if (!oldHdr) // our original message may have been empty / uninitialized + return; + + newHdr->m_busId = oldHdr->m_busId; + newHdr->m_preamble = oldHdr->m_preamble; + newHdr->m_messageId = oldHdr->m_messageId; + + if (oldSize >= XS_EXTLENCODE) + oldData = oldHdr->m_datlen.m_extended.m_data; + else + oldData = oldHdr->m_datlen.m_data; + + if (newSize >= XS_EXTLENCODE) + newData = newHdr->m_datlen.m_extended.m_data; + else + newData = newHdr->m_datlen.m_data; + + for (index = 0; index < offset; ++index) + newData[index] = oldData[index]; + for (; index < newSize; ++index) + newData[index] = oldData[index + count]; + + if (thisPtr->m_autoUpdateChecksum) + thisPtr->m_checksum[0] -= byteSum(newData, newSize); + + XsArray_destruct(&old); +} + +/*! \brief Swap the contents of \a a and \a b + + \details This function swaps the internal buffers so no actual data is moved around. + A result is that it won't work for unmanaged data such as fixed size vectors + + \param a the object to receive \a b's contents + \param b the object to receive \a a's contents +*/ +void XsMessage_swap(XsMessage* a, XsMessage* b) +{ + XsMessage tmp; + + *((uint8_t**)&tmp.m_checksum) = a->m_checksum; + tmp.m_autoUpdateChecksum = a->m_autoUpdateChecksum; + + *((uint8_t**)&a->m_checksum) = b->m_checksum; + a->m_autoUpdateChecksum = b->m_autoUpdateChecksum; + + *((uint8_t**)&b->m_checksum) = tmp.m_checksum; + b->m_autoUpdateChecksum = tmp.m_autoUpdateChecksum; + + XsArray_swap(&a->m_message, &b->m_message); +} + +/*! \brief Compare the contents of the messages \a a and \a b, returning non-0 if they are different + \param a the first XsMessage pointer to compare + \param b the first XsMessage pointer to compare + \returns 0 if the messages are equal +*/ +int XsMessage_compare(XsMessage const* a, XsMessage const* b) +{ + return XsArray_compare(&a->m_message, &b->m_message); +} + +/*! \brief Return a string containing the first \a maxBytes bytes of the message in hex format + \param maxBytes the maximum number of bytes to include in the string, when set to 0, the full message will be used + \param resultValue the resulting string +*/ +void XsMessage_toHexString(XsMessage const* thisPtr, XsSize maxBytes, XsString* resultValue) +{ + char* s; + XsSize i = XsMessage_getTotalMessageSize(thisPtr); + if (maxBytes == 0 || maxBytes > i) + maxBytes = i; + + if (maxBytes) + { + 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]); + } + else + XsString_resize(resultValue, 0); +} + +/*! @} */ diff --git a/extern/xspublic/xstypes/xsmessage.h b/extern/xspublic/xstypes/xsmessage.h new file mode 100644 index 0000000..9daa095 --- /dev/null +++ b/extern/xspublic/xstypes/xsmessage.h @@ -0,0 +1,693 @@ + +// Copyright (c) 2003-2022 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 XSMESSAGE_H +#define XSMESSAGE_H + +#include "xstypesconfig.h" +#include "pstdint.h" +#include "xsbytearray.h" +#include "xsdataidentifier.h" +#include "xsxbusmessageid.h" +#include "xsstring.h" +#include "xsresultvalue.h" +#include "xsbusid.h" + +struct XsMessage; +struct XsMessageHeader; + +#ifdef __cplusplus +extern "C" { +#endif +#ifndef __cplusplus +#define XSMESSAGE_INITIALIZER { XSBYTEARRAY_INITIALIZER, 1, 0 } +typedef struct XsMessage XsMessage; +typedef struct XsMessageHeader XsMessageHeader; +#endif + +XSTYPES_DLL_API void XsMessage_construct(XsMessage* thisPtr); +XSTYPES_DLL_API void XsMessage_constructSized(XsMessage* thisPtr, XsSize dataSize); +XSTYPES_DLL_API void XsMessage_copyConstruct(XsMessage* thisPtr, XsMessage const* src); +XSTYPES_DLL_API void XsMessage_assign(XsMessage* thisPtr, XsSize dataSize); +XSTYPES_DLL_API void XsMessage_load(XsMessage* thisPtr, XsSize msgSize, unsigned char const* src); +XSTYPES_DLL_API void XsMessage_destruct(XsMessage* thisPtr); +XSTYPES_DLL_API void XsMessage_copy(XsMessage* copy, XsMessage const* thisPtr); +XSTYPES_DLL_API void XsMessage_swap(XsMessage* a, XsMessage* b); +XSTYPES_DLL_API XsSize XsMessage_dataSize(XsMessage const* thisPtr); +XSTYPES_DLL_API const uint8_t* XsMessage_constData(XsMessage const* thisPtr, XsSize offset); +XSTYPES_DLL_API const uint8_t* XsMessage_getMessageStart(XsMessage const* thisPtr); +XSTYPES_DLL_API XsSize XsMessage_getTotalMessageSize(XsMessage const* thisPtr); +XSTYPES_DLL_API uint8_t XsMessage_getDataByte(XsMessage const* thisPtr, XsSize offset); +XSTYPES_DLL_API uint16_t XsMessage_getDataShort(XsMessage const* thisPtr, XsSize offset); +XSTYPES_DLL_API uint32_t XsMessage_getDataLong(XsMessage const* thisPtr, XsSize offset); +XSTYPES_DLL_API uint64_t XsMessage_getDataLongLong(XsMessage const* thisPtr, XsSize offset); +XSTYPES_DLL_API float XsMessage_getDataFloat(XsMessage const* thisPtr, XsSize offset); +XSTYPES_DLL_API double XsMessage_getDataDouble(XsMessage const* thisPtr, XsSize offset); +XSTYPES_DLL_API double XsMessage_getDataF1220(XsMessage const* thisPtr, XsSize offset); +XSTYPES_DLL_API double XsMessage_getDataFP1632(XsMessage const* thisPtr, XsSize offset); +XSTYPES_DLL_API const uint8_t* XsMessage_getDataBuffer(XsMessage const* thisPtr, XsSize offset); +XSTYPES_DLL_API void XsMessage_setDataByte(XsMessage* thisPtr, uint8_t value, XsSize offset); +XSTYPES_DLL_API void XsMessage_setDataShort(XsMessage* thisPtr, uint16_t value, XsSize offset); +XSTYPES_DLL_API void XsMessage_setDataLong(XsMessage* thisPtr, uint32_t value, XsSize offset); +XSTYPES_DLL_API void XsMessage_setDataLongLong(XsMessage* thisPtr, uint64_t value, XsSize offset); +XSTYPES_DLL_API void XsMessage_setDataFloat(XsMessage* thisPtr, float value, XsSize offset); +XSTYPES_DLL_API void XsMessage_setDataDouble(XsMessage* thisPtr, double value, XsSize offset); +XSTYPES_DLL_API void XsMessage_setDataF1220(XsMessage* thisPtr, double value, XsSize offset); +XSTYPES_DLL_API void XsMessage_setDataFP1632(XsMessage* thisPtr, double value, XsSize offset); +XSTYPES_DLL_API void XsMessage_setDataBuffer(XsMessage* thisPtr, const uint8_t* buffer, XsSize size, XsSize offset); +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 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); +XSTYPES_DLL_API void XsMessage_setBusId(XsMessage* thisPtr, uint8_t busId); +XSTYPES_DLL_API void XsMessage_setMessageId(XsMessage* thisPtr, XsXbusMessageId msgId); +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 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); + +#ifdef __cplusplus +} // extern "C" +#endif + +#define XS_PREAMBLE 0xFA +#define XS_EXTLENCODE 0xFF + +#define XS_LEN_MSGHEADER 4 +#define XS_LEN_MSGEXTHEADER 6 +#define XS_LEN_MSGHEADERCS 5 +#define XS_LEN_MSGEXTHEADERCS 7 +#define XS_LEN_CHECKSUM 1 +#define XS_LEN_UNSIGSHORT 2 +#define XS_LEN_UNSIGINT 4 +#define XS_LEN_FLOAT 4 + +// Maximum message/data length +#define XS_MAXDATALEN (8192-XS_LEN_MSGEXTHEADERCS) +#define XS_MAXSHORTDATALEN 254 +#define XS_MAXMSGLEN (XS_MAXDATALEN+XS_LEN_MSGEXTHEADERCS) +#define XS_MAXSHORTMSGLEN (XS_MAXSHORTDATALEN+XS_LEN_MSGHEADERCS) +#define XS_MAXGARBAGE (XS_MAXMSGLEN+1) + +////////////////////////////////////////////////////////////////////////////////////////// +// different alignment commands for gcc / MSVS, the structure needs to be 1-byte aligned. +XS_PACKED_STRUCT_START +/*! \brief A message header + \details This structure is used to interpret the header of a message. +*/ +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 + { + /*! Contains extended length information and first byte of data buffer if normal length is 255 */ + struct ExtendedLength + { + /*! The high and low byte of the extended length */ + 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 + uint8_t m_data[1]; //!< \brief The first byte of the data buffer, the data buffer is always at least 1 byte since it has to contain the checksum, but it can be bigger. + } m_extended; //!< \brief The extended length, only valid if normal length is 255 + uint8_t m_data[1]; //!< \brief The first byte of the data buffer if length < 255, the data buffer is always at least 1 byte since it has to contain the checksum, but it can be bigger. + } m_datlen; //!< \brief Data or length and data +#ifdef SWIG +}; +#else +} XS_PACKED_STRUCT; +#endif + +XS_PACKED_STRUCT_END + +////////////////////////////////////////////////////////////////////////////////////////// +//! \brief Structure for storing a single message. +struct XsMessage +{ +#ifdef __cplusplus + /*! \brief Create a XsMessage object with the given data length and message Id. + + The function allocates enough memory to hold an entire message with the given + data length. + \param msgId The message Id that will be assigned to the m_messageId field. + \param dataLength The length of the data in the message. + */ + inline explicit XsMessage(XsXbusMessageId msgId = XMID_InvalidMessage, XsSize dataLength = 0) + : m_autoUpdateChecksum(1) + , m_checksum(0) + { + XsMessage_constructSized(this, dataLength); + XsMessage_setMessageId(this, msgId); + } + + /*! \brief Create a message from the given source byte array + + This is done through a simple memory copy. The number of bytes copied is taken + from the data in the message (so the message is interpreted first). + Note that this does NOT recompute the checksum, nor is it checked. + + \param source The source byte array containing message data + \param size The size of the source string + */ + inline XsMessage(const uint8_t* source, XsSize size) + : m_autoUpdateChecksum(1) + , m_checksum(0) + { + XsMessage_load(this, size, source); + } + + /*! \brief Create a message from the given source string + + The string is interpreted as hex-encoded bytes. Example to request device ID: XsMessage("FAFF0000", true). + + \param source The source string containing the full message. + \param computeChecksum When set to true (default), the checksum will be automatically computed and added or updated. When set to false, a valid checksum is assumed to be included in \a source. + */ + inline XsMessage(const XsString& source, bool computeChecksum = true) + : m_autoUpdateChecksum(1) + , m_checksum(0) + { + 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'); + if (a >= 'a' && a <= 'f') + return (uint8_t)(10 + a - 'a'); + if (a >= 'A' && a <= 'F') + 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 + XsMessage_load(this, tmp.size(), tmp.data()); + if (computeChecksum) + XsMessage_recomputeChecksum(this); + } + + //! \brief Copy constructor + inline XsMessage(const XsMessage& src) + : m_message(src.m_message) + , m_autoUpdateChecksum(src.m_autoUpdateChecksum) + , m_checksum(0) + { + updateChecksumPtr(); + } + + //! Destroy the message + inline ~XsMessage() + { + XsMessage_destruct(this); + } + + //! \brief Clear all data in the message + inline void clear(void) + { + XsMessage_destruct(this); + } + + /*! \brief Test if this message is empty + + \returns true if this message is empty, false otherwise + */ + inline bool empty(void) const + { + return 0 != XsMessage_empty(this); + } + + //! Return the busId header field. + inline uint8_t getBusId(void) const + { + const XsMessageHeader* hdr = XsMessage_getConstHeader(this); + if (!hdr) + return 0; + return hdr->m_busId; + } + + /*! \copydoc XsMessage_constData + */ + inline const uint8_t* getDataBuffer(XsSize offset = 0) const + { + return XsMessage_constData(this, offset); + } + + /*! \copydoc XsMessage_getDataByte + */ + inline uint8_t getDataByte(XsSize offset = 0) const + { + return XsMessage_getDataByte(this, offset); + } + + /*! \copydoc XsMessage_getDataDouble + */ + inline double getDataDouble(XsSize offset = 0) const + { + return XsMessage_getDataDouble(this, offset); + } + + /*! \copydoc XsMessage_getDataFloat + */ + inline float getDataFloat(XsSize offset = 0) const + { + return XsMessage_getDataFloat(this, offset); + } + + /*! \copydoc XsMessage_getDataF1220 + */ + inline double getDataF1220(XsSize offset = 0) const + { + return XsMessage_getDataF1220(this, offset); + } + + /*! \copydoc XsMessage_getDataFP1632 + */ + inline double getDataFP1632(XsSize offset = 0) const + { + return XsMessage_getDataFP1632(this, offset); + } + + /*! \copydoc XsMessage_getDataLong + */ + inline uint32_t getDataLong(XsSize offset = 0) const + { + return XsMessage_getDataLong(this, offset); + } + + /*! \copydoc XsMessage_getDataLongLong + */ + inline uint64_t getDataLongLong(XsSize offset = 0) const + { + return XsMessage_getDataLongLong(this, offset); + } + + /*! \copydoc XsMessage_getDataShort + */ + inline uint16_t getDataShort(XsSize offset = 0) const + { + return XsMessage_getDataShort(this, offset); + } + + /*! \copydoc XsMessage_dataSize + */ + inline XsSize getDataSize(void) const + { + return XsMessage_dataSize(this); + } + + //! Return the current value of the m_messageId field. + inline XsXbusMessageId getMessageId(void) const + { + const XsMessageHeader* hdr = XsMessage_getConstHeader(this); + if (!hdr) + return XMID_InvalidMessage; + return (XsXbusMessageId) hdr->m_messageId; + } + + //! Returns XRV_OK if this is a normal message or an error code if the message was an XMID_Error message or XRV_NULLPTR if the message is invalid / empty + inline XsResultValue toResultValue(void) const + { + const XsMessageHeader* hdr = XsMessage_getConstHeader(this); + if (!hdr) + return XRV_NULLPTR; + if (hdr->m_messageId == 0 && hdr->m_busId == XS_BID_MASTER) + return XRV_TIMEOUTNODATA; // we assume that an empty message indicates a timeout + if ((XsXbusMessageId) hdr->m_messageId != XMID_Error) + return XRV_OK; + return (XsResultValue) getDataByte(); + } + + /*! \copydoc XsMessage_getMessageStart + */ + inline const uint8_t* getMessageStart(void) const + { + return XsMessage_getMessageStart(this); + } + + /*! \copydoc XsMessage_getTotalMessageSize + */ + inline XsSize getTotalMessageSize(void) const + { + return XsMessage_getTotalMessageSize(this); + } + + /*! \copydoc XsMessage_isChecksumOk + */ + inline bool isChecksumOk(void) const + { + return 0 != XsMessage_isChecksumOk(this); + } + + /*! \brief Initialize the %XsMessage with the data from \a src + + \param msgSize the size of the data pointed to by src + \param src the data to load the message from + + \returns true if the checksum of the loaded message is OK. + */ + inline bool loadFromString(const uint8_t* src, XsSize msgSize) + { + XsArray_destruct(&m_message); + XsMessage_load(this, msgSize, src); + return isChecksumOk(); + } + + /*! \copydoc XsMessage_recomputeChecksum + */ + inline void recomputeChecksum(void) + { + XsMessage_recomputeChecksum(this); + } + + /*! \copydoc XsMessage_resizeData + */ + inline void resizeData(XsSize newSize) + { + XsMessage_resizeData(this, newSize); + } + + /*! \copydoc XsMessage_setBusId + */ + inline void setBusId(uint8_t busId) + { + XsMessage_setBusId(this, busId); + } + + /*! \copydoc XsMessage_setBusId + */ + inline void setBusId(int busId) + { + XsMessage_setBusId(this, (uint8_t) busId); + } + + /*! \copydoc XsMessage_setDataBuffer + */ + inline void setDataBuffer(const uint8_t* buffer, XsSize size, XsSize offset = 0) + { + XsMessage_setDataBuffer(this, buffer, size, offset); + } + + /*! \copydoc XsMessage_setDataByte + */ + inline void setDataByte(const uint8_t value, XsSize offset = 0) + { + XsMessage_setDataByte(this, value, offset); + } + + /*! \copydoc XsMessage_setDataDouble + */ + inline void setDataDouble(const double value, XsSize offset = 0) + { + XsMessage_setDataDouble(this, value, offset); + } + + /*! \copydoc XsMessage_setDataFloat + */ + inline void setDataFloat(const float value, XsSize offset = 0) + { + XsMessage_setDataFloat(this, value, offset); + } + + /*! \copydoc XsMessage_setDataF1220 + */ + inline void setDataF1220(const double value, XsSize offset = 0) + { + XsMessage_setDataF1220(this, value, offset); + } + + /*! \copydoc XsMessage_setDataFP1632 + */ + inline void setDataFP1632(const double value, XsSize offset = 0) + { + XsMessage_setDataFP1632(this, value, offset); + } + + /*! \copydoc XsMessage_setDataLong + */ + inline void setDataLong(const uint32_t value, XsSize offset = 0) + { + XsMessage_setDataLong(this, value, offset); + } + + /*! \copydoc XsMessage_setDataLongLong + */ + inline void setDataLongLong(const uint64_t value, XsSize offset = 0) + { + XsMessage_setDataLongLong(this, value, offset); + } + + /*! \copydoc XsMessage_setDataShort + */ + inline void setDataShort(const uint16_t value, XsSize offset = 0) + { + XsMessage_setDataShort(this, value, offset); + } + + /*! \copydoc XsMessage_setMessageId + */ + inline void setMessageId(const XsXbusMessageId msgId) + { + XsMessage_setMessageId(this, msgId); + } + + //! Copy message src into this + inline XsMessage& operator = (const XsMessage& src) + { + if (this != &src) + XsMessage_copy(this, &src); + return *this; + } + + /*! \copydoc XsMessage_deleteData */ + inline void deleteData(XsSize count, XsSize offset = 0) + { + XsMessage_deleteData(this, count, offset); + } + + /*! \copydoc XsMessage_insertData */ + inline void insertData(XsSize count, XsSize offset = 0) + { + XsMessage_insertData(this, count, offset); + } + + /*! \copydoc XsMessage_getFPValueSize */ + inline static uint8_t getFPValueSize(XsDataIdentifier id) + { + return XsMessage_getFPValueSize(id); + } + + /*! \copydoc XsMessage_getDataFPValuesById */ + inline void getDataFPValue(XsDataIdentifier dataIdentifier, double* dest, XsSize offset = 0, XsSize numValues = 1) const + { + XsMessage_getDataFPValuesById(this, dataIdentifier, dest, offset, numValues); + } + + /*! \brief Return current data values as double, conversion depends on outputSetting + + \param dataIdentifier Data identifier containing data precision + \param offset offset in the data buffer from where to start reading. + + \returns the current data value as double + */ + inline double getDataFPValue(XsDataIdentifier dataIdentifier, XsSize offset = 0) const + { + double tmp; + XsMessage_getDataFPValuesById(this, dataIdentifier, &tmp, offset, 1); + return tmp; + } + + /*! \copydoc XsMessage_setDataFPValuesById */ + inline void setDataFPValue(XsDataIdentifier dataIdentifier, const double* data, XsSize offset = 0, XsSize numValues = 1) + { + XsMessage_setDataFPValuesById(this, dataIdentifier, data, offset, numValues); + } + + /*! \brief Write a floating/fixed point value into to the data buffer, conversion depends on outputSettings + + \param dataIdentifier Data Identifier + \param data The data array to be written to the buffer. + \param offset Offset in the data buffer from where to start writing. + */ + inline void setDataFPValue(XsDataIdentifier dataIdentifier, double data, XsSize offset = 0) + { + XsMessage_setDataFPValuesById(this, dataIdentifier, &data, offset, 1); + } + + /*! \brief Return true if \a other is identical to this */ + inline bool operator == (const XsMessage& other) const + { + if (this == &other) + return true; + return m_message == other.m_message; + } + + /*! \brief Return the message in its raw form */ + inline XsByteArray const& rawMessage() const + { + return m_message; + }; + + /*! \brief Return a string containing the first \a maxBytes bytes of the message in hex format + \param maxBytes the maximum number of bytes to include in the string, when set to 0, the full message will be used + \return A string containing the first \a maxBytes bytes of the message in hex format + */ + inline XsString toHexString(XsSize maxBytes = 0) const + { + XsString rv; + XsMessage_toHexString(this, maxBytes, &rv); + return rv; + } + + /*! \brief Get data of type T from the message + \details This function can be used to get data of (simple) type T from the message. + \param data A pointer to a buffer with space for at least \a numValues of T + \param id Ignored for this function, but added here for generalization purposes + \param offset The offset of the first byte of the data in the message + \param numValues The number of consecutive values to read + */ + template + inline void getData(T* data, XsDataIdentifier id, XsSize offset = 0, int numValues = 1) const + { + (void) id; + for (int i = 0; i < numValues; ++i) + XsMessage_getEndianCorrectData(this, &data[i], sizeof(T), offset + ((unsigned int)i)*sizeof(T)); + } + + /*! \brief Write data of type T to the message + \details This function can be used to write data of (simple) type T to the message. + \param data A pointer to a buffer with space for at least \a numValues of T + \param id Ignored for this function, but added here for generalization purposes + \param offset The offset of the first byte of the data in the message + \param numValues The number of consecutive values to write + */ + template + inline void setData(T const* data, XsDataIdentifier id, XsSize offset = 0, int numValues = 1) + { + (void) id; + for (int i = 0; i < numValues; ++i) + 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 + \param id Ignored for this function, but added here for generalization purposes + \param numValues The desired number of items, used as a simple multiplier + \return The number of bytes that \a numValues items of type T will require in a message + */ + template + inline static int sizeInMsg(XsDataIdentifier id, int numValues = 1) + { + (void) id; + return numValues * (int) sizeof(T); + } + +private: + /*! \brief Update the checksum pointer after changing the size of the message */ + inline void updateChecksumPtr() + { + XsSize sz = XsMessage_getTotalMessageSize(this); + if (sz) + *const_cast(&m_checksum) = &m_message[sz - 1]; + else + *const_cast(&m_checksum) = 0; + } + +#endif + + XsByteArray m_message; + int m_autoUpdateChecksum; + uint8_t* const m_checksum; //!< Points to the checksum to speed up automatic checksum updates +}; + +#ifdef __cplusplus +/*! \brief Get 'real' data from the message + \details This function can be used to get 'real' data from the message, possibly converting from an + underlying fixed or floating point type if necessary. + \param data A pointer to a buffer with space for at least \a numValues doubles + \param id The dataidentifer that will be used to determine the underlying data type + \param offset The offset of the first byte of the data in the message + \param numValues The number of consecutive values to read +*/ +template <> +inline void XsMessage::getData(double* data, XsDataIdentifier id, XsSize offset, int numValues) const +{ + getDataFPValue(id, data, offset, (XsSize)(ptrdiff_t)numValues); +} + +/*! \brief Write 'real' data from the message + \details This function can be used to write 'real' data to the message, possibly converting to an + underlying fixed or floating point type if necessary. + \param data A pointer to a buffer with space for at least \a numValues doubles + \param id The dataidentifer that will be used to determine the underlying data type + \param offset The offset of the first byte of the data in the message + \param numValues The number of consecutive values to write +*/ +template <> +inline void XsMessage::setData(double const* data, XsDataIdentifier id, XsSize offset, int numValues) +{ + setDataFPValue(id, data, offset, (XsSize)(ptrdiff_t)numValues); +} + +/*! \brief Return the number of bytes that \a numValues 'real' items will require in a message + \param id Specifies the desired underlying type to use + \param numValues The desired number of items, used as a simple multiplier + \return The number of bytes that \a numValues 'real' items will require in a message +*/ +template <> +inline int XsMessage::sizeInMsg(XsDataIdentifier id, int numValues) +{ + return XsMessage_getFPValueSize(id) * numValues; +} +#endif + +// some macros to help when constructing/parsing messages +#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/extern/xspublic/xstypes/xsmessagearray.c b/extern/xspublic/xstypes/xsmessagearray.c new file mode 100644 index 0000000..4269d9c --- /dev/null +++ b/extern/xspublic/xstypes/xsmessagearray.c @@ -0,0 +1,60 @@ + +// Copyright (c) 2003-2022 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 "xsmessagearray.h" +#include "xsmessage.h" + +/*! \struct XsMessageArray + \brief A list of XsMessage values + \sa XsArray +*/ + +//! \brief Descriptor for XsMessageArray +XsArrayDescriptor const g_xsMessageArrayDescriptor = +{ + sizeof(XsMessage), + XSEXPCASTITEMSWAP XsMessage_swap, + XSEXPCASTITEMMAKE XsMessage_construct, + XSEXPCASTITEMCOPY XsMessage_copyConstruct, + XSEXPCASTITEMMAKE XsMessage_destruct, + XSEXPCASTITEMCOPY XsMessage_copy, + XSEXPCASTITEMCOMP XsMessage_compare, + 0 +}; + +/*! \copydoc XsArray_constructDerived + \note Specialization for XsStringArray +*/ +void XsMessageArray_construct(XsMessageArray* thisPtr, XsSize count, XsMessage const* src) +{ + XsArray_construct(thisPtr, &g_xsMessageArrayDescriptor, count, src); +} diff --git a/extern/xspublic/xstypes/xsmessagearray.h b/extern/xspublic/xstypes/xsmessagearray.h new file mode 100644 index 0000000..0b2b251 --- /dev/null +++ b/extern/xspublic/xstypes/xsmessagearray.h @@ -0,0 +1,98 @@ + +// Copyright (c) 2003-2022 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 XSMESSAGEARRAY_H +#define XSMESSAGEARRAY_H + +#include "xsarray.h" +#include "pstdint.h" + +#ifdef __cplusplus +#include "xsmessage.h" +extern "C" { +#endif + +extern XsArrayDescriptor const XSTYPES_DLL_API g_xsMessageArrayDescriptor; + +#ifndef __cplusplus +#define XSMESSAGEARRAY_INITIALIZER XSARRAY_INITIALIZER(&g_xsMessageArrayDescriptor) + +struct XsMessage; +XSARRAY_STRUCT(XsMessageArray, struct XsMessage); +typedef struct XsMessageArray XsMessageArray; + +XSTYPES_DLL_API void XsMessageArray_construct(XsMessageArray* thisPtr, XsSize count, struct XsMessage const* src); +#endif + +#ifdef __cplusplus +} // extern "C" + +struct XsMessageArray : public XsArrayImpl +{ + //! \brief Constructs an XsMessageArray + inline explicit XsMessageArray(XsSize sz = 0, XsMessage const* src = 0) + : ArrayImpl(sz, src) + { + } + + //! \brief Constructs an XsMessageArray as a copy of \a other + inline XsMessageArray(XsMessageArray const& other) + : ArrayImpl(other) + { + } + + //! \brief Constructs an XsMessageArray that references the data supplied in \a ref + inline explicit XsMessageArray(XsMessage* 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(XsMessageArray& first, XsMessageArray& second) + { + first.swap(second); + } +#endif + +#ifndef XSENS_NOITERATOR + //! \brief Constructs an XsMessageArray with the array bound by the supplied iterators \a beginIt and \a endIt + template + inline XsMessageArray(Iterator beginIt, Iterator endIt) + : ArrayImpl(beginIt, endIt) + { + } +#endif +}; +#endif + +#endif diff --git a/extern/xspublic/xstypes/xsmfmresultvalue.h b/extern/xspublic/xstypes/xsmfmresultvalue.h new file mode 100644 index 0000000..9efb785 --- /dev/null +++ b/extern/xspublic/xstypes/xsmfmresultvalue.h @@ -0,0 +1,48 @@ + +// Copyright (c) 2003-2022 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 XSMFMRESULTVALUE_H +#define XSMFMRESULTVALUE_H + +enum XsMfmResultValue +{ + MRV_Failed, + MRV_Bad, + MRV_Acceptable, + MRV_Good, + MRV_Processing, + MRV_Skipped, + + MRV_Unknown // initial value +}; + +#endif diff --git a/extern/xspublic/xstypes/xsoption.h b/extern/xspublic/xstypes/xsoption.h new file mode 100644 index 0000000..5b48ba3 --- /dev/null +++ b/extern/xspublic/xstypes/xsoption.h @@ -0,0 +1,101 @@ + +// Copyright (c) 2003-2022 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 XSOPTION_H +#define XSOPTION_H + +/*! \addtogroup enums Global enumerations + @{ +*/ +/*! \brief Xda options, used to control the kind of data processing done by XDA + \details These options are used to specify whether XDA should compute certain kinds of data from + available other data. XsOptions can be logically ORed together +*/ +//AUTO namespace xstypes { +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. + + , XSO_KeepLastLiveData = 0x0004 //!< Keep the last available live data in a cache so XsDevice::lastAvailableLiveData will work + , XSO_RetainLiveData = 0x0008 //!< Keep the live data in a cache so it can be accessed through XsDevice::getDataPacketByIndex or XsDevice::takeFirstDataPacketInQueue. This option is mutually exclusive with XSO_RetainBufferedData. If both are set, XSO_RetainBufferedData will be used. + , XSO_RetainBufferedData = 0x0010 //!< Keep the buffered data in a cache so it can be accessed through XsDevice::getDataPacketByIndex or XsDevice::takeFirstDataPacketInQueue. This option is mutually exclusive with XSO_RetainLiveData. If both are set, XSO_RetainBufferedData will be used. + , XSO_OrientationInLiveStream = 0x0020 //!< Compute orientation in the live stream. This is no longer (since version 4.9.2) mutually exclusive with XSO_OrientationInBufferedStream, but unless they're both explicitly set, the system will choose only one stream to use. Please note that this option does not do anything unless XSO_Orientation is also enabled. + , XSO_OrientationInBufferedStream = 0x0040 //!< Compute orientation in the buffered stream. This is no longer (since version 4.9.2) mutually exclusive with XSO_OrientationInLiveStream, but unless they're both explicitly set, the system will choose only one stream to use. Please note that this option does not do anything unless XSO_Orientation is also enabled. + + , XSO_ApplyOrientationResetToCalData = 0x0080 //!< Apply orientation reset to calibrated acc, gyr and mag (object reset only) and heading reset to free acc. Default is enabled for the MTw family, disabled for others + + , XSO_InterpolateMissingData = 0x1000 //!< When set, any gaps in the data streams of child devices will be filled with interpolated data. This is only for applications that require data for each sample counter. Not recommended when XSO_SkipDataBundling is disabled. + , 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 +}; +#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; + +//AUTO } + +#ifdef __cplusplus +//! \brief Logical OR operator for XsOption values +inline XsOption operator | (XsOption a, XsOption 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); +} +//! \brief Logical XOR operator for XsOption values +inline XsOption operator ^ (XsOption a, XsOption b) +{ + return (XsOption)((int)a ^ (int)b); +} +//! \brief Logical NEG operator for XsOption values +inline XsOption operator ~(XsOption a) +{ + return (XsOption)(~(int)a); +} +//! \brief Return the option with mutually exclusive values 'fixed' +inline XsOption XsOption_purify(XsOption a) +{ + if ((a & XSO_RetainLiveData) && (a & XSO_RetainBufferedData)) + a = a & ~XSO_RetainLiveData; + + return a; +} +#endif + +#endif diff --git a/extern/xspublic/xstypes/xsoutputconfiguration.c b/extern/xspublic/xstypes/xsoutputconfiguration.c new file mode 100644 index 0000000..2220747 --- /dev/null +++ b/extern/xspublic/xstypes/xsoutputconfiguration.c @@ -0,0 +1,48 @@ + +// Copyright (c) 2003-2022 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 "xsoutputconfiguration.h" + +void XsOutputConfiguration_swap(struct XsOutputConfiguration* a, struct XsOutputConfiguration* b) +{ + { + XsDataIdentifier t = a->m_dataIdentifier; + a->m_dataIdentifier = b->m_dataIdentifier; + b->m_dataIdentifier = t; + } + + { + uint16_t t = a->m_frequency; + a->m_frequency = b->m_frequency; + b->m_frequency = t; + } +} diff --git a/extern/xspublic/xstypes/xsoutputconfiguration.h b/extern/xspublic/xstypes/xsoutputconfiguration.h new file mode 100644 index 0000000..1e09158 --- /dev/null +++ b/extern/xspublic/xstypes/xsoutputconfiguration.h @@ -0,0 +1,88 @@ + +// Copyright (c) 2003-2022 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 XSOUTPUTCONFIGURATION_H +#define XSOUTPUTCONFIGURATION_H + +#include "xstypesconfig.h" +#include "pstdint.h" +#include "xsdataidentifier.h" + +#define XS_MAX_OUTPUTCONFIGURATIONS (32) + +#ifdef __cplusplus +extern "C" { +#endif +#ifndef __cplusplus +#define XSOUTPUTCONFIGURATION_INITIALIZER { XDI_None, 0 } +#endif + +struct XsOutputConfiguration; + +XSTYPES_DLL_API void XsOutputConfiguration_swap(struct XsOutputConfiguration* a, struct XsOutputConfiguration* b); + +#ifdef __cplusplus +} // extern "C" +#endif + + +/*! \brief Single data type output configuration + \details This structure contains a single data type and the frequency at which it should be produced. + If m_frequency is 0xFFFF and the %XsOutputConfiguration is used for input, the device will configure + 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 +{ + XsDataIdentifier m_dataIdentifier; //!< The data identifier + uint16_t m_frequency; //!< The frequency + +#ifdef __cplusplus + //! Constructor, initializes to an empty object + XsOutputConfiguration() + : m_dataIdentifier(XDI_None), m_frequency(0) {} + + //! Constructor, initializes to specified values + XsOutputConfiguration(XsDataIdentifier di, uint16_t freq) + : m_dataIdentifier(di), m_frequency(freq) + {} + + //! Comparison operator + bool operator == (const XsOutputConfiguration& other) const + { + return m_dataIdentifier == other.m_dataIdentifier && m_frequency == other.m_frequency; + } +#endif +}; +typedef struct XsOutputConfiguration XsOutputConfiguration; + +#endif diff --git a/extern/xspublic/xstypes/xsoutputconfigurationarray.c b/extern/xspublic/xstypes/xsoutputconfigurationarray.c new file mode 100644 index 0000000..8821062 --- /dev/null +++ b/extern/xspublic/xstypes/xsoutputconfigurationarray.c @@ -0,0 +1,91 @@ + +// Copyright (c) 2003-2022 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 "xsoutputconfigurationarray.h" +#include "xsoutputconfiguration.h" + +/*! \struct XsOutputConfigurationArray + \brief A list of XsOutputConfiguration values + \sa XsArray +*/ + +/*! \copydoc XsArrayDescriptor::itemSwap + \note Specialization for XsOutputConfiguration*/ +void swapXsOutputConfiguration(XsOutputConfiguration* a, XsOutputConfiguration* b) +{ + XsOutputConfiguration tmp = *a; + *a = *b; + *b = tmp; +} + +/*! \copydoc XsArrayDescriptor::itemCopy + \note Specialization for XsOutputConfiguration*/ +void copyXsOutputConfiguration(XsOutputConfiguration* to, XsOutputConfiguration const* from) +{ + *to = *from; +} + +/*! \copydoc XsArrayDescriptor::itemCompare + \note Specialization for XsOutputConfiguration*/ +int compareXsOutputConfiguration(XsOutputConfiguration const* a, XsOutputConfiguration const* b) +{ + if (a->m_dataIdentifier != b->m_dataIdentifier || a->m_frequency != b->m_frequency) + { + if (a->m_dataIdentifier == b->m_dataIdentifier) + return (a->m_frequency < b->m_frequency) ? -1 : 1; + return (a->m_dataIdentifier < b->m_dataIdentifier) ? -1 : 1; + } + + return 0; +} + + +//! \brief Descriptor for XsOutputConfigurationArray +XsArrayDescriptor const g_xsOutputConfigurationArrayDescriptor = +{ + sizeof(XsOutputConfiguration), + XSEXPCASTITEMSWAP swapXsOutputConfiguration, // swap + 0, // construct + XSEXPCASTITEMCOPY copyXsOutputConfiguration, // copy construct + 0, // destruct + XSEXPCASTITEMCOPY copyXsOutputConfiguration, // copy + XSEXPCASTITEMCOMP compareXsOutputConfiguration, // compare + XSEXPCASTRAWCOPY XsArray_rawCopy // raw copy +}; + +/*! \copydoc XsArray_constructDerived + \note Specialization for XsOutputConfigurationArray +*/ +void XsOutputConfigurationArray_construct(XsOutputConfigurationArray* thisPtr, XsSize count, XsOutputConfiguration const* src) +{ + XsArray_construct(thisPtr, &g_xsOutputConfigurationArrayDescriptor, count, src); +} diff --git a/extern/xspublic/xstypes/xsoutputconfigurationarray.h b/extern/xspublic/xstypes/xsoutputconfigurationarray.h new file mode 100644 index 0000000..0306752 --- /dev/null +++ b/extern/xspublic/xstypes/xsoutputconfigurationarray.h @@ -0,0 +1,97 @@ + +// Copyright (c) 2003-2022 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 XSOUTPUTCONFIGURATIONARRAY_H +#define XSOUTPUTCONFIGURATIONARRAY_H + +#include "xsarray.h" + +#ifdef __cplusplus +#include "xsoutputconfiguration.h" +extern "C" { +#endif + +extern XsArrayDescriptor const XSTYPES_DLL_API g_xsOutputConfigurationArrayDescriptor; + +#ifndef __cplusplus +#define XSOUTPUTCONFIGURATIONARRAY_INITIALIZER XSARRAY_INITIALIZER(&g_xsOutputConfigurationArrayDescriptor) +struct XsOutputConfiguration; + +XSARRAY_STRUCT(XsOutputConfigurationArray, struct XsOutputConfiguration); +typedef struct XsOutputConfigurationArray XsOutputConfigurationArray; + +XSTYPES_DLL_API void XsOutputConfigurationArray_construct(XsOutputConfigurationArray* thisPtr, XsSize count, struct XsOutputConfiguration const* src); +#endif + +#ifdef __cplusplus +} // extern "C" + +struct XsOutputConfigurationArray : public XsArrayImpl +{ + //! \brief Constructs an XsOutputConfigurationArray + inline explicit XsOutputConfigurationArray(XsSize sz = 0, XsOutputConfiguration const* src = 0) + : ArrayImpl(sz, src) + { + } + + //! \brief Constructs an XsOutputConfigurationArray as a copy of \a other + inline XsOutputConfigurationArray(XsOutputConfigurationArray const& other) + : ArrayImpl(other) + { + } + + //! \brief Constructs an XsOutputConfigurationArray that references the data supplied in \a ref + inline explicit XsOutputConfigurationArray(XsOutputConfiguration* 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(XsOutputConfigurationArray& first, XsOutputConfigurationArray& second) + { + first.swap(second); + } +#endif + +#ifndef XSENS_NOITERATOR + //! \brief Constructs an XsOutputConfigurationArray with the array bound by the supplied iterators \a beginIt and \a endIt + template + inline XsOutputConfigurationArray(Iterator beginIt, Iterator endIt) + : ArrayImpl(beginIt, endIt) + { + } +#endif +}; +#endif + +#endif diff --git a/extern/xspublic/xstypes/xsplatform.h b/extern/xspublic/xstypes/xsplatform.h new file mode 100644 index 0000000..b51c14f --- /dev/null +++ b/extern/xspublic/xstypes/xsplatform.h @@ -0,0 +1,52 @@ + +// Copyright (c) 2003-2022 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 XSPLATFORM_H +#define XSPLATFORM_H + +#include "xstypesconfig.h" + +#ifdef _WIN32 + /// microsoft / windows + #include + + #define XsIoHandle HANDLE + +#else + /// gcc / linux + #include + + typedef int32_t XsIoHandle; + +#endif + +#endif diff --git a/extern/xspublic/xstypes/xsportinfo.c b/extern/xspublic/xstypes/xsportinfo.c new file mode 100644 index 0000000..ffcffa0 --- /dev/null +++ b/extern/xspublic/xstypes/xsportinfo.c @@ -0,0 +1,202 @@ + +// Copyright (c) 2003-2022 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 "xsportinfo.h" +#include +#include // strlen +#include // atoi + +/*! \class XsPortInfo + \brief Contains a descriptor for opening a communication port to an Xsens device. +*/ +/*! \addtogroup cinterface C Interface + @{ +*/ + +/*! \relates XsPortInfo + \brief Initializes the object to the empty state +*/ +void XsPortInfo_clear(XsPortInfo* thisPtr) +{ + thisPtr->m_baudrate = XBR_Invalid; + thisPtr->m_deviceId.m_deviceId = 0; + thisPtr->m_deviceId.m_productCode[0] = '\0'; + thisPtr->m_portName[0] = '\0'; + thisPtr->m_linesOptions = XPLO_All_Ignore; +} + +/*! \relates XsPortInfo + \brief Returns true if the XsPortInfo object is empty +*/ +int XsPortInfo_empty(const struct XsPortInfo* thisPtr) +{ + return (thisPtr->m_portName[0] == '\0'); +} + +/*! \relates XsPortInfo + \brief The port number + \returns Returns the port number + \note Available on Windows only +*/ +int XsPortInfo_portNumber(const struct XsPortInfo* thisPtr) +{ + size_t i; + + if (XsPortInfo_empty(thisPtr)) + return 0; + + 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 + return strncmp("\\\\?\\usb", thisPtr->m_portName, 7) == 0; +#else + return strncmp("USB", thisPtr->m_portName, 3) == 0; // libusb devices start with USB +#endif +} + +/*! + \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 +*/ +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 + \note Available on Linux only +*/ +int XsPortInfo_usbBus(const struct XsPortInfo* thisPtr) +{ +#ifndef XSENS_WINDOWS + if (XsPortInfo_isUsb(thisPtr)) + return atoi(&thisPtr->m_portName[3]); +#else + (void) thisPtr; +#endif + return 0; +} + +/*! \relates XsPortInfo + \brief The usb address + \returns Returns the usb address + \note Available on Linux only +*/ +int XsPortInfo_usbAddress(const struct XsPortInfo* thisPtr) +{ +#ifndef XSENS_WINDOWS + if (XsPortInfo_isUsb(thisPtr)) + return atoi(&thisPtr->m_portName[7]); +#else + (void) thisPtr; +#endif + return 0; +} + +/*! \brief Swap the contents of \a a with those of \a b + \param a swap this with \a b + \param b swap this with \a a +*/ +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; + b->m_baudrate = t; + + XsDeviceId_swap(&a->m_deviceId, &b->m_deviceId); + + for (i = 0; i < 256; ++i) + { + c = a->m_portName[i]; + a->m_portName[i] = b->m_portName[i]; + b->m_portName[i] = c; + } + + 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/extern/xspublic/xstypes/xsportinfo.h b/extern/xspublic/xstypes/xsportinfo.h new file mode 100644 index 0000000..8f60018 --- /dev/null +++ b/extern/xspublic/xstypes/xsportinfo.h @@ -0,0 +1,345 @@ + +// Copyright (c) 2003-2022 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 XSPORTINFO_H +#define XSPORTINFO_H + +#include "xstypesconfig.h" +#include "pstdint.h" +#include "xsdeviceid.h" +#include "xsbaud.h" +#include "xsstring.h" +#include + +#define XSENS_VENDOR_ID 0x2639 +#define FTDI_VENDOR_ID 0x0403 // needed for Xsens USB-serial converters + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef __cplusplus +typedef struct XsPortInfo XsPortInfo; +#else +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); + +#ifdef __cplusplus +} +#endif + +#if defined(_MSC_VER) + #pragma warning(push) + #pragma warning(disable : 4996) +#endif + +typedef enum XsPortLinesOptions +{ + XPLO_Invalid = 0, + + XPLO_RTS_Set = 1, + XPLO_RTS_Clear = (1 << 1), + XPLO_RTS_Ignore = (1 << 2), + + XPLO_DTR_Set = (1 << 3), + XPLO_DTR_Clear = (1 << 4), + XPLO_DTR_Ignore = (1 << 5), + + XPLO_All_Set = XPLO_RTS_Set | XPLO_DTR_Set, + XPLO_All_Clear = XPLO_RTS_Clear | XPLO_DTR_Clear, + XPLO_All_Ignore = XPLO_RTS_Ignore | XPLO_DTR_Ignore, + + XPLO_RtsCtsFlowControl = (1 << 6) +} XsPortLinesOptions; + +struct XsPortInfo +{ +#ifdef __cplusplus + /*! \brief Default constructor, creates an empty port info object */ + XsPortInfo() + : m_deviceId(0) + , m_baudrate(XBR_Invalid) + , m_linesOptions(XPLO_All_Ignore) + , m_vid(0) + , m_pid(0) + { + memset(m_portName, '\0', 256); + } + + /*! \brief Named constructor, initializes the object to the supplied \a portname and optional \a baudRate + \param portname The name of the port, maximum 255 characters + \param baudRate The baud rate to configure for the port, for scanning XBR_Invalid may be used to scan all known baud rates + \param linesOptions The options for the hardware flow control lines + */ + explicit XsPortInfo(const XsString& portname, XsBaudRate baudRate = XBR_Invalid, XsPortLinesOptions linesOptions = XPLO_All_Ignore) + : m_deviceId(0) + , m_baudrate(baudRate) + , m_linesOptions(linesOptions) + , m_vid(0) + , m_pid(0) + { + memset(m_portName, '\0', 256); + if (portname.size() < 255) + strcpy(m_portName, portname.c_str()); + } + +#ifndef XSENS_NO_PORT_NUMBERS + /*! \brief Port number constructor, initializes the port to have COM as its name and the optional \a baudRate for a baud rate + \param portNr The number of the COM port + \param baudRate The baud rate to configure for the port, for scanning XBR_Invalid may be used to scan all known baud rates + \param linesOptions The options for the hardware flow control lines + \note Numbered COM ports are only available on Windows platforms. + */ + explicit XsPortInfo(int portNr, XsBaudRate baudRate = XBR_Invalid, XsPortLinesOptions linesOptions = XPLO_All_Ignore) + : m_deviceId(0) + , m_baudrate(baudRate) + , m_linesOptions(linesOptions) + , m_vid(0) + , m_pid(0) + { + memset(m_portName, 0, 256); + sprintf(m_portName, "COM%d", portNr); + } +#endif + + /*! \brief \copybrief XsPortInfo_clear */ + inline void clear() + { + XsPortInfo_clear(this); + } + + /*! \brief \copybrief XsPortInfo_empty */ + inline bool empty() const + { + return XsPortInfo_empty(this) != 0; + } + + /*! \brief greater than operator, used for sorting the list. */ + 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; + } + + /*! \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; + } + + /*! \brief equality operator, used for finding items in a list. */ + inline bool operator == (const char* port) const + { + return strcmp(m_portName, port) == 0; + } + + /*! \copydoc XsPortInfo_portNumber */ + inline int portNumber() const + { + return XsPortInfo_portNumber(this); + } + + /*! \brief The port name + */ + inline XsString portName() const + { + return XsString(m_portName); + } + + /*! \brief The port name + */ + inline char const* portName_c_str() const + { + return m_portName; + } + + /*! \brief Set the port name + */ + inline void setPortName(const XsString& portName_) + { + strncpy(m_portName, portName_.c_str(), 256); + } + + /*! \brief \copybrief XsPortInfo_isUsb */ + inline bool isUsb() const + { + 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 + { + return XsPortInfo_isNetwork(this) != 0; + } + + /*! \brief \copybrief XsPortInfo_networkServiceName */ + inline XsString networkServiceName() const + { + return XsString(XsPortInfo_networkServiceName(this)); + } + + /*! \copydoc XsPortInfo_bluetoothAddress */ + inline XsString bluetoothAddress() const + { + return XsString(XsPortInfo_bluetoothAddress(this)); + } + + /*! \copydoc XsPortInfo_usbBus */ + inline int usbBus() const + { + return XsPortInfo_usbBus(this); + } + + /*! \copydoc XsPortInfo_usbAddress */ + inline int usbAddress() const + { + return XsPortInfo_usbAddress(this); + } + + /*! \brief The baudrate + */ + inline XsBaudRate baudrate() const + { + return m_baudrate; + } + + /*! \brief Set the baudrate + */ + inline void setBaudrate(XsBaudRate baudrate_) + { + m_baudrate = baudrate_; + } + + /*! \brief The device ID + */ + inline XsDeviceId deviceId() const + { + return m_deviceId; + } + + /*! \brief Set the device ID + */ + inline void setDeviceId(XsDeviceId deviceId_) + { + m_deviceId = deviceId_; + } + + /*! \brief The options for the hw flow control lines + */ + inline XsPortLinesOptions linesOptions() const + { + return m_linesOptions; + } + + /*! \brief Set the options for the hw flow control lines + */ + inline void setLinesOptions(XsPortLinesOptions linesOptions) + { + m_linesOptions = linesOptions; + } + + /*! \brief Get the vid and pid of this portinfo + */ + inline void getVidPid(uint16_t& vid, uint16_t& pid) const + { + vid = m_vid; + pid = m_pid; + } + + /*! \brief Set the vid and pid + */ + inline void setVidPid(uint16_t vid, uint16_t pid) + { + m_vid = vid; + m_pid = pid; + } + +private: +#endif + + XsDeviceId m_deviceId; //!< The device Id of main Xsens device detected on the port + char m_portName[256]; //!< The port name + XsBaudRate m_baudrate; //!< The baudrate at which an Xsens device was detected, may be XBR_Invalid for pure USB ports + XsPortLinesOptions m_linesOptions; //!< The hardware flow control lines options for the port + uint16_t m_vid, m_pid; //!< The USB Vendor Id and Hardware Id of this connection, when available +}; + +#if defined(_MSC_VER) + #pragma warning(pop) +#endif + +#if defined(__cplusplus) && !defined(XSENS_NO_STL) +#include +namespace std +{ +/*! \brief Stream output operator for XsPortInfo */ +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 + + +#endif diff --git a/extern/xspublic/xstypes/xsportinfoarray.c b/extern/xspublic/xstypes/xsportinfoarray.c new file mode 100644 index 0000000..6530a59 --- /dev/null +++ b/extern/xspublic/xstypes/xsportinfoarray.c @@ -0,0 +1,77 @@ + +// Copyright (c) 2003-2022 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 "xsportinfoarray.h" + +/*! \struct XsPortInfoArray + \brief A list of XsPortInfo values + \sa XsArray +*/ + +void copyPortInfo(XsPortInfo* dest, XsPortInfo const* src) +{ + memcpy(dest, src, sizeof(XsPortInfo)); +} + +int comparePortInfo(XsPortInfo const* a, XsPortInfo const* b) +{ + 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 = +{ + sizeof(XsPortInfo), + XSEXPCASTITEMSWAP XsPortInfo_swap, // swap + XSEXPCASTITEMMAKE XsPortInfo_clear, // construct + XSEXPCASTITEMCOPY copyPortInfo, // copy construct + 0, // destruct + XSEXPCASTITEMCOPY copyPortInfo, // copy + XSEXPCASTITEMCOMP comparePortInfo, // compare + XSEXPCASTRAWCOPY XsArray_rawCopy // raw copy +}; + +/*! \copydoc XsArray_constructDerived + \note Specialization for XsPortInfoArray +*/ +void XsPortInfoArray_construct(XsPortInfoArray* thisPtr, XsSize count, XsPortInfo const* src) +{ + XsArray_construct(thisPtr, &g_xsPortInfoArrayDescriptor, count, src); +} diff --git a/extern/xspublic/xstypes/xsportinfoarray.h b/extern/xspublic/xstypes/xsportinfoarray.h new file mode 100644 index 0000000..c41d550 --- /dev/null +++ b/extern/xspublic/xstypes/xsportinfoarray.h @@ -0,0 +1,97 @@ + +// Copyright (c) 2003-2022 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 XSPORTINFOARRAY_H +#define XSPORTINFOARRAY_H + +#include "xsarray.h" +#include "xsportinfo.h" + +#ifdef __cplusplus +extern "C" { +#endif + +extern XsArrayDescriptor const XSTYPES_DLL_API g_xsPortInfoArrayDescriptor; + +#ifndef __cplusplus +#define XSPORTINFOARRAY_INITIALIZER XSARRAY_INITIALIZER(&g_xsPortInfoArrayDescriptor) +struct XsPortInfo; + +XSARRAY_STRUCT(XsPortInfoArray, struct XsPortInfo); +typedef struct XsPortInfoArray XsPortInfoArray; + +XSTYPES_DLL_API void XsPortInfoArray_construct(XsPortInfoArray* thisPtr, XsSize count, struct XsPortInfo const* src); +#endif + +#ifdef __cplusplus +} // extern "C" + +struct XsPortInfoArray : public XsArrayImpl +{ + //! \brief Constructs an XsPortInfoArray + inline explicit XsPortInfoArray(XsSize sz = 0, XsPortInfo const* src = 0) + : ArrayImpl(sz, src) + { + } + + //! \brief Constructs an XsPortInfoArray as a copy of \a other + inline XsPortInfoArray(XsPortInfoArray const& other) + : ArrayImpl(other) + { + } + + //! \brief Constructs an XsPortInfoArray that references the data supplied in \a ref + inline explicit XsPortInfoArray(XsPortInfo* 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(XsPortInfoArray& first, XsPortInfoArray& second) + { + first.swap(second); + } +#endif + +#ifndef XSENS_NOITERATOR + //! \brief Constructs an XsPortInfoArray with the array bound by the supplied iterators \a beginIt and \a endIt + template + inline XsPortInfoArray(Iterator beginIt, Iterator endIt) + : ArrayImpl(beginIt, endIt) + { + } +#endif +}; +#endif + +#endif diff --git a/extern/xspublic/xstypes/xspressure.h b/extern/xspublic/xstypes/xspressure.h new file mode 100644 index 0000000..7a2ab43 --- /dev/null +++ b/extern/xspublic/xstypes/xspressure.h @@ -0,0 +1,106 @@ + +// Copyright (c) 2003-2022 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 XSPRESSURE_H +#define XSPRESSURE_H + +#include "pstdint.h" + +/*! \brief Pressure data. + \details Contains the pressure data and the pressure age +*/ +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, uint8_t age = 0) + : m_pressure(pressure) + , m_pressureAge(age) + { + } + + /*! \brief Create a new XsPressure as copy from \a other + \param other the pressure carrier to copy from + */ + inline XsPressure(XsPressure const& other) : + m_pressure(other.m_pressure), + m_pressureAge(other.m_pressureAge) + { + } + + /*! \brief Copy the data from \a other + \param other the pressure carrier to copy from + \return this + */ + inline XsPressure const& operator=(XsPressure const& other) + { + m_pressure = other.m_pressure; + m_pressureAge = other.m_pressureAge; + return *this; + } + + /*! \brief Return true if this is equal to \a other + \param other the pressure carrier to compare against + \return true if both XsPressures are equal + */ + inline bool operator==(XsPressure const& other) const + { + 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 +}; +typedef struct XsPressure XsPressure; + +#endif diff --git a/extern/xspublic/xstypes/xsprotocol.h b/extern/xspublic/xstypes/xsprotocol.h new file mode 100644 index 0000000..3c8cbaa --- /dev/null +++ b/extern/xspublic/xstypes/xsprotocol.h @@ -0,0 +1,50 @@ + +// Copyright (c) 2003-2022 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 XSPROTOCOL_H +#define XSPROTOCOL_H + +/*! \addtogroup enums Global enumerations + @{ +*/ +//! Protocol types, used for MTi6x0 devices. +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; + +#endif diff --git a/extern/xspublic/xstypes/xsquaternion.c b/extern/xspublic/xstypes/xsquaternion.c new file mode 100644 index 0000000..65aca04 --- /dev/null +++ b/extern/xspublic/xstypes/xsquaternion.c @@ -0,0 +1,319 @@ + +// Copyright (c) 2003-2022 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 "xsquaternion.h" +#include "xseuler.h" +#include "xsmatrix.h" +#include "xsvector.h" +#include +#include "xsfloatmath.h" + + +/*! \class XsQuaternion + \brief A class that implements a quaternion +*/ + +/*! \addtogroup cinterface C Interface + @{ +*/ + +/*! \relates XsQuaternion + \brief Sets the contents to 0, which is an invalid XsQuaternion + \details The normal destructor does not call this function, it is intended for explicit signalling of errors +*/ +void XsQuaternion_destruct(XsQuaternion* thisPtr) +{ + thisPtr->m_w = XsMath_zero; + thisPtr->m_x = XsMath_zero; + thisPtr->m_y = XsMath_zero; + thisPtr->m_z = XsMath_zero; +} + +/*! \relates XsQuaternion \brief Test if this is a null object. */ +int XsQuaternion_empty(const XsQuaternion* thisPtr) +{ + return thisPtr->m_w == XsMath_zero && thisPtr->m_x == XsMath_zero && thisPtr->m_y == XsMath_zero && thisPtr->m_z == XsMath_zero; +} + +/*! \relates XsQuaternion + \brief Invert this quaternion + \details Where \a q = a + bi + cj + dk, this function will replace it with a - bi - cj - dk. +*/ +void XsQuaternion_invert(XsQuaternion* thisPtr) +{ + XsQuaternion_inverse(thisPtr, thisPtr); +} + +/*! \relates XsQuaternion + \brief Compute the inverse/conjugate of this quaternion + \details Where \a q = a + bi + cj + dk, this function will return a - bi - cj - dk in \a dest. + \param dest The object to write to +*/ +void XsQuaternion_inverse(const XsQuaternion* thisPtr, XsQuaternion* dest) +{ + dest->m_w = thisPtr->m_w; + dest->m_x = -thisPtr->m_x; + dest->m_y = -thisPtr->m_y; + dest->m_z = -thisPtr->m_z; +} + +/*! \relates XsQuaternion \brief Create a normalized version of this quaternion +*/ +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; + if (thisPtr->m_w < 0) + divisor = -divisor; + + dest->m_w = thisPtr->m_w * divisor; + dest->m_x = thisPtr->m_x * divisor; + dest->m_y = thisPtr->m_y * divisor; + dest->m_z = thisPtr->m_z * divisor; + + return length; +} + +/*! \relates XsQuaternion \brief Normalize this quaternion +*/ +XsReal XsQuaternion_normalize(XsQuaternion* thisPtr) +{ + return XsQuaternion_normalized(thisPtr, thisPtr); +} + +/*! \relates XsQuaternion \brief Create a quaternion representation from \a euler angles */ +void XsQuaternion_fromEulerAngles(XsQuaternion* thisPtr, const XsEuler* src) +{ + XsReal cosX, sinX, cosY, sinY, cosZ, sinZ; + + if (XsEuler_empty(src)) + { + *thisPtr = *XsQuaternion_identity(); + return; + } + + cosX = cos(XsMath_pt5 * XsMath_deg2rad(src->m_x)); + sinX = sin(XsMath_pt5 * XsMath_deg2rad(src->m_x)); + cosY = cos(XsMath_pt5 * XsMath_deg2rad(src->m_y)); + sinY = sin(XsMath_pt5 * XsMath_deg2rad(src->m_y)); + cosZ = cos(XsMath_pt5 * XsMath_deg2rad(src->m_z)); + sinZ = sin(XsMath_pt5 * XsMath_deg2rad(src->m_z)); + + thisPtr->m_w = cosX * cosY * cosZ + sinX * sinY * sinZ; + thisPtr->m_x = sinX * cosY * cosZ - cosX * sinY * sinZ; + thisPtr->m_y = cosX * sinY * cosZ + sinX * cosY * sinZ; + thisPtr->m_z = cosX * cosY * sinZ - sinX * sinY * cosZ; +} + +/*! \relates XsQuaternion + \brief Create a quaternion representation of orientation matrix \a ori + \details The matrix \a ori is interpreted as an orthonormal orientation matrix, which is + translated into a quaternion representation. If \a ori is not a 3x3 matrix, a null-quaternion is + returned. + \param ori The source orientation matrix +*/ +void XsQuaternion_fromRotationMatrix(XsQuaternion* thisPtr, const XsMatrix* ori) +{ + XsReal trace; // Trace of matrix + XsReal s; + + if (!XsMatrix_dimensionsMatch(ori, 3, 3)) + { + XsQuaternion_destruct(thisPtr); + return; + } + + // XsQuaternion result; + + // Calculate trace of matrix + // T = 4 - 4x^2 - 4y^2 - 4z^2 + // = 4 ( 1 - x^2 - y^2 - z^2) + // = 1 + fRgs[0][0] + fRgs[1][1] + fRgs[2][2] (= 4q0^2) + + trace = XsMatrix_value(ori, 0, 0) + XsMatrix_value(ori, 1, 1) + XsMatrix_value(ori, 2, 2) + XsMath_one; + + // If the trace of the matrix is greater than zero, + // then perform an "instant" calculation. + // Important note wrt. rounding errors: + // 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) + { + s = XsMath_two * sqrt(trace); + thisPtr->m_w = XsMath_pt25 * 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; + } + else if ((XsMatrix_value(ori, 0, 0) > XsMatrix_value(ori, 1, 1)) && (XsMatrix_value(ori, 0, 0) > XsMatrix_value(ori, 2, 2))) + { + trace = XsMath_one + XsMatrix_value(ori, 0, 0) - XsMatrix_value(ori, 1, 1) - XsMatrix_value(ori, 2, 2); + s = XsMath_two * sqrt(trace); + thisPtr->m_x = XsMath_pt25 * 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; + } + else if (XsMatrix_value(ori, 1, 1) > XsMatrix_value(ori, 2, 2)) + { + trace = XsMath_one + XsMatrix_value(ori, 1, 1) - XsMatrix_value(ori, 0, 0) - XsMatrix_value(ori, 2, 2); + s = XsMath_two * sqrt(trace); + thisPtr->m_y = XsMath_pt25 * 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; + } + else + { + trace = XsMath_one + XsMatrix_value(ori, 2, 2) - XsMatrix_value(ori, 0, 0) - XsMatrix_value(ori, 1, 1); + s = XsMath_two * sqrt(trace); + thisPtr->m_z = XsMath_pt25 * 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; + } + + XsQuaternion_inverse(thisPtr, thisPtr); +} + +/*! \relates XsQuaternion \brief Returns an XsQuaternion that represents the identity quaternion */ +const XsQuaternion* XsQuaternion_identity(void) +{ + static const XsQuaternion sIdentity = { { { 1, 0, 0, 0 } } }; + return &sIdentity; +} + +/*! \relates XsQuaternion \brief Multiply \a left quaternion with \a right quaternion and put the result in \a dest. The parameters may point to the same XsQuaternion(s). */ +void XsQuaternion_multiply(const XsQuaternion* left, const XsQuaternion* right, XsQuaternion* dest) +{ + XsReal qa0 = left->m_w; + XsReal qa1 = left->m_x; + XsReal qa2 = left->m_y; + XsReal qa3 = left->m_z; + + XsReal qb0 = right->m_w; + XsReal qb1 = right->m_x; + XsReal qb2 = right->m_y; + XsReal qb3 = right->m_z; + + dest->m_w = qa0 * qb0 - qa1 * qb1 - qa2 * qb2 - qa3 * qb3; + dest->m_x = qa1 * qb0 + qa0 * qb1 - qa3 * qb2 + qa2 * qb3; + dest->m_y = qa2 * qb0 + qa3 * qb1 + qa0 * qb2 - qa1 * qb3; + dest->m_z = qa3 * qb0 - qa2 * qb1 + qa1 * qb2 + qa0 * qb3; +} + +/*! \relates XsQuaternion \brief Swap the contents of \a a and \a b +*/ +void XsQuaternion_swap(XsQuaternion* a, XsQuaternion* b) +{ + XsReal t; + int i; + for (i = 0; i < 4; ++i) + { + t = a->m_data[i]; + a->m_data[i] = b->m_data[i]; + b->m_data[i] = t; + } +} + +/*! \relates XsQuaternion \brief Copies the contents of \a thisPtr into \a copy +*/ +void XsQuaternion_copy(XsQuaternion* copy, XsQuaternion const* src) +{ + copy->m_w = src->m_w; + copy->m_x = src->m_x; + copy->m_y = src->m_y; + copy->m_z = src->m_z; +} + +/*! \relates XsQuaternion \brief returns non-zero if \a a and \a b are numerically equal +*/ +int XsQuaternion_equal(XsQuaternion const* a, XsQuaternion const* b) +{ + return (a->m_w == b->m_w && + a->m_x == b->m_x && + a->m_y == b->m_y && + a->m_z == b->m_z) ? 1 : 0; +} + +/*! \brief Checks whether \a a and \a b are equal with tolerance \a tolerance + \param[in] a double a + \param[in] b double b + \param[in] tolerance The tolerance + \returns a positive value when \a a and \a b are considered equal +*/ +static int fuzzyIsEqual(double a, double b, double tolerance) +{ + return fabs(a - b) <= tolerance; +} + +/*! \relates XsQuaternion + \brief Returns zero if the values at \a thisPtr and \a other are within \a tolerance of each other +*/ +int XsQuaternion_compare(XsQuaternion const* thisPtr, XsQuaternion const* other, XsReal tolerance) +{ + if (thisPtr == other) + return 0; + + if (fuzzyIsEqual(thisPtr->m_data[0], other->m_data[0], tolerance) && + fuzzyIsEqual(thisPtr->m_data[1], other->m_data[1], tolerance) && + fuzzyIsEqual(thisPtr->m_data[2], other->m_data[2], tolerance) && + fuzzyIsEqual(thisPtr->m_data[3], other->m_data[3], tolerance)) + return 0; + // add extra check for q == -q (negative-definite vs positive-definite comparison) + return (fuzzyIsEqual(thisPtr->m_data[0], -other->m_data[0], tolerance) && + fuzzyIsEqual(thisPtr->m_data[1], -other->m_data[1], tolerance) && + fuzzyIsEqual(thisPtr->m_data[2], -other->m_data[2], tolerance) && + fuzzyIsEqual(thisPtr->m_data[3], -other->m_data[3], tolerance)) ? 0 : 1; +} + +/*! \relates XsQuaternion + \brief Returns the dot product of the \a thisPtr with \a 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); +} + +/*! @} */ diff --git a/extern/xspublic/xstypes/xsquaternion.h b/extern/xspublic/xstypes/xsquaternion.h new file mode 100644 index 0000000..e48f2bb --- /dev/null +++ b/extern/xspublic/xstypes/xsquaternion.h @@ -0,0 +1,364 @@ + +// Copyright (c) 2003-2022 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 XSQUATERNION_H +#define XSQUATERNION_H + +#include "xsmath.h" + +struct XsEuler; +struct XsMatrix; +struct XsVector; +struct XsQuaternion; + +#ifdef __cplusplus +extern "C" { +#endif +#ifndef __cplusplus +#define XSQUATERNION_INITIALIZER { { { XsMath_zero, XsMath_zero, XsMath_zero, XsMath_zero } } } +typedef struct XsQuaternion XsQuaternion; +#endif + +XSTYPES_DLL_API void XsQuaternion_destruct(XsQuaternion* thisPtr); +XSTYPES_DLL_API int XsQuaternion_empty(const XsQuaternion* thisPtr); +XSTYPES_DLL_API void XsQuaternion_inverse(const XsQuaternion* thisPtr, XsQuaternion* dest); +XSTYPES_DLL_API XsReal XsQuaternion_normalized(const XsQuaternion* thisPtr, XsQuaternion* dest); +XSTYPES_DLL_API XsReal XsQuaternion_normalize(XsQuaternion* thisPtr); +XSTYPES_DLL_API void XsQuaternion_fromEulerAngles(XsQuaternion* thisPtr, const struct XsEuler* src); +XSTYPES_DLL_API void XsQuaternion_fromRotationMatrix(XsQuaternion* thisPtr, const struct XsMatrix* ori); +XSTYPES_DLL_API const XsQuaternion* XsQuaternion_identity(void); +XSTYPES_DLL_API void XsQuaternion_multiply(const XsQuaternion* left, const XsQuaternion* right, XsQuaternion* dest); +XSTYPES_DLL_API void XsQuaternion_swap(XsQuaternion* a, XsQuaternion* b); +XSTYPES_DLL_API void XsQuaternion_copy(XsQuaternion* copy, XsQuaternion const* src); +XSTYPES_DLL_API int XsQuaternion_equal(XsQuaternion const* a, XsQuaternion const* b); +XSTYPES_DLL_API int XsQuaternion_compare(XsQuaternion const* thisPtr, XsQuaternion const* other, XsReal tolerance); +XSTYPES_DLL_API XsReal XsQuaternion_dotProduct(XsQuaternion const* thisPtr, XsQuaternion const* other); + +#ifdef __cplusplus +} // extern "C" +#endif + +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 + XsReal m_z; //!< Stores the z component of the quaternion + }; + XsReal m_data[4]; //!< Stores the quaternion in an array of four elements + }; +#ifdef __cplusplus +public: + //! \brief Construct a quaternion with the supplied values, or zero's by default + inline explicit XsQuaternion(XsReal w_ = XsMath_zero, XsReal x_ = XsMath_zero, XsReal y_ = XsMath_zero, XsReal z_ = XsMath_zero) + : m_w(w_) + , m_x(x_) + , m_y(y_) + , m_z(z_) + { + } + + //! \brief Construct a quaternion with the supplied values, or zero's by default and optionally normalize + inline explicit XsQuaternion(XsReal w_, XsReal x_, XsReal y_, XsReal z_, bool normalize_) + : m_w(w_) + , m_x(x_) + , m_y(y_) + , m_z(z_) + { + if (normalize_) + normalize(); + } + + //! \brief Construct a quaternion with the supplied values from the \a other Quaternion + inline XsQuaternion(const XsQuaternion& other) + : m_w(other.m_w) + , m_x(other.m_x) + , m_y(other.m_y) + , m_z(other.m_z) + {} + +#if !defined(SWIG) && !defined(__ADSP21000__) + //! \brief Move-construct a quaternion with the supplied values from the \a other Quaternion + inline XsQuaternion(XsQuaternion&& other) + : m_w(other.m_w) + , m_x(other.m_x) + , m_y(other.m_y) + , m_z(other.m_z) + {} +#endif + + //! \brief Construct a quaternion by converting from an XsEuler object + inline explicit XsQuaternion(const XsEuler& euler) + { + XsQuaternion_fromEulerAngles(this, &euler); + } + + //! \brief Construct a quaternion by converting from an XsMatrix rotation matrix object + inline explicit XsQuaternion(const XsMatrix& ori) + { + XsQuaternion_fromRotationMatrix(this, &ori); + } + + //! \brief Destructor, intentionally empty + inline ~XsQuaternion() + { + } + + //! \brief Assigns the \a other quaternion to this quaternion + inline XsQuaternion& operator=(const XsQuaternion& other) + { + if (this != &other) + { + m_w = other.m_w; + m_x = other.m_x; + m_y = other.m_y; + m_z = other.m_z; + } + return *this; + } + + /*! \brief Set the Quaternion to these specific values + */ + inline void assign(XsReal w_, XsReal x_, XsReal y_, XsReal z_) + { + m_w = w_; + m_x = x_; + m_y = y_; + m_z = z_; + } + + /*! \brief Set the Quaternion to the specific values in the supplied array. + \param values An array that contains at least 4 items. The first four will be interpreted as w,x,y,z + */ + inline void assign(const XsReal* values) + { + for (int i = 0; i < 4; ++i) + m_data[i] = values[i]; + } + + //! \brief Returns a reference to the \a index'th component of the quaternion + inline XsReal& operator[](XsSize index) + { + assert(index < 4); + return m_data[index]; + } + + //! \brief Returns the \a index'th component of the quaternion + inline XsReal operator[](XsSize index) const + { + assert(index < 4); + return m_data[index]; + } + + //! \brief Return a const pointer to the internal data + inline const XsReal* data() const + { + return m_data; + } + + //! \brief \copybrief XsQuaternion_inverse \returns The inverse/conjugate of the quaternion + inline XsQuaternion inverse() const + { + XsQuaternion tmp; + XsQuaternion_inverse(this, &tmp); + return tmp; + } + + //! \brief \copybrief XsQuaternion_inverse \returns The inverse/conjugate of the quaternion + inline XsQuaternion conjugate() const + { + XsQuaternion tmp; + XsQuaternion_inverse(this, &tmp); + return tmp; + } + + //! \brief Return a normalized version of the quaternion \sa XsQuaternion_normalized \returns The normalized quaternion + XsQuaternion normalized() const + { + XsQuaternion tmp; + XsQuaternion_normalized(this, &tmp); + return tmp; + } + + //! \brief Normalize the quaternion \sa XsQuaternion_normalized \returns The cartesian length of the quaternion before normalization + inline XsReal normalize() + { + return XsQuaternion_normalized(this, this); + } + + //! \brief \copybrief XsQuaternion_empty + inline bool empty() const + { + return 0 != XsQuaternion_empty(this); + } + + //! \brief \copybrief XsQuaternion_fromEulerAngles + inline XsQuaternion& fromEulerAngles(const XsEuler& src) + { + XsQuaternion_fromEulerAngles(this, &src); + return *this; + } + + //! \brief \copybrief XsQuaternion_fromRotationMatrix + inline XsQuaternion& fromRotationMatrix(const XsMatrix& ori) + { + XsQuaternion_fromRotationMatrix(this, &ori); + return *this; + } + + //! \brief \copybrief XsQuaternion_identity + inline static const XsQuaternion& identity() + { + return *XsQuaternion_identity(); + } + + /*! \brief In-place multiplication of this quaternion with \a other quaternion + \param other The other quaternion to multiply with (right side of multiplication) + \sa XsQuaternion_multiply() + */ + inline void operator*=(const XsQuaternion& other) + { + 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; + } + //! \brief Return the x component of the quaternion + inline XsReal x() const + { + return m_x; + } + //! \brief Return the y component of the quaternion + inline XsReal y() const + { + return m_y; + } + //! \brief Return the z component of the quaternion + inline XsReal z() const + { + return m_z; + } + //! \brief Return a reference to the w component of the quaternion + inline XsReal& w() + { + return m_w; + } + //! \brief Return a reference to the x component of the quaternion + inline XsReal& x() + { + return m_x; + } + //! \brief Return a reference to the y component of the quaternion + inline XsReal& y() + { + return m_y; + } + //! \brief Return a reference to the z component of the quaternion + 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) + { + first.swap(second); + } + + //! \brief Returns true if \a other is numerically identical to this + 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; + } + + /*! \brief Returns true if the fields of this and \a other are within \a tolerance of each other + \note This considers positive- and negative-definite quaternions to be identical! + */ + inline bool isEqual(const XsQuaternion& other, XsReal tolerance) const + { + return !XsQuaternion_compare(this, &other, tolerance); + } + + //! \brief Returns the dot product of this with \a other + inline XsReal dotProduct(const XsQuaternion& other) const + { + return XsQuaternion_dotProduct(this, &other); + } +#endif +}; + +#ifdef __cplusplus +//! \brief Return the negated version of the Quaternion \a q (w,-x,-y,-z) +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) +{ + XsQuaternion tmp(lhs); + tmp *= rhs; + return tmp; +} +#endif + +#endif diff --git a/extern/xspublic/xstypes/xsquaternionarray.c b/extern/xspublic/xstypes/xsquaternionarray.c new file mode 100644 index 0000000..56dcefa --- /dev/null +++ b/extern/xspublic/xstypes/xsquaternionarray.c @@ -0,0 +1,69 @@ + +// Copyright (c) 2003-2022 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 "xsquaternionarray.h" +#include "xsquaternion.h" + +/*! \struct XsQuaternionArray + \brief A list of XsQuaternion values + \sa XsArray + \details new elements are not initialized +*/ + + +/*! \copydoc XsArrayDescriptor::itemCompare + \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 = +{ + sizeof(XsQuaternion), + XSEXPCASTITEMSWAP XsQuaternion_swap, + 0, // item construct + XSEXPCASTITEMCOPY XsQuaternion_copy, + 0, // item destruct + XSEXPCASTITEMCOPY XsQuaternion_copy, + XSEXPCASTITEMCOMP compareQuat, + XSEXPCASTRAWCOPY XsArray_rawCopy +}; + +/*! \copydoc XsArray_constructDerived + \note Specialization for XsQuaternionArray +*/ +void XsQuaternionArray_construct(XsQuaternionArray* thisPtr, XsSize count, XsQuaternion const* src) +{ + XsArray_construct(thisPtr, &g_xsQuaternionArrayDescriptor, count, src); +} diff --git a/extern/xspublic/xstypes/xsquaternionarray.h b/extern/xspublic/xstypes/xsquaternionarray.h new file mode 100644 index 0000000..5c4b67b --- /dev/null +++ b/extern/xspublic/xstypes/xsquaternionarray.h @@ -0,0 +1,101 @@ + +// Copyright (c) 2003-2022 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 XSQUATERNIONARRAY_H +#define XSQUATERNIONARRAY_H + +#include "xsarray.h" + +#ifdef __cplusplus +#include "xsquaternion.h" +extern "C" { +#endif + +extern XsArrayDescriptor const XSTYPES_DLL_API g_xsQuaternionArrayDescriptor; + +#ifndef __cplusplus +#define XSQUATERNIONARRAY_INITIALIZER XSARRAY_INITIALIZER(&g_xsQuaternionArrayDescriptor) +struct XsQuaternion; + +XSARRAY_STRUCT(XsQuaternionArray, struct XsQuaternion); +typedef struct XsQuaternionArray XsQuaternionArray; + +XSTYPES_DLL_API void XsQuaternionArray_construct(XsQuaternionArray* thisPtr, XsSize count, struct XsQuaternion const* src); +#define XsQuaternionArray_destruct(thisPtr) XsArray_destruct(thisPtr) +#endif + +#ifdef __cplusplus +} // extern "C" +#endif + +#ifdef __cplusplus +struct XsQuaternionArray : public XsArrayImpl +{ + //! \brief Constructs an XsQuaternionArray + inline explicit XsQuaternionArray(XsSize sz = 0, XsQuaternion const* src = 0) + : ArrayImpl(sz, src) + { + } + + //! \brief Constructs an XsQuaternionArray as a copy of \a other + inline XsQuaternionArray(XsQuaternionArray const& other) + : ArrayImpl(other) + { + } + + //! \brief Constructs an XsQuaternionArray that references the data supplied in \a ref + inline explicit XsQuaternionArray(XsQuaternion* 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(XsQuaternionArray& first, XsQuaternionArray& second) + { + first.swap(second); + } +#endif + +#ifndef XSENS_NOITERATOR + //! \brief Constructs an XsQuaternionArray with the array bound by the supplied iterators \a beginIt and \a endIt + template + inline XsQuaternionArray(Iterator beginIt, Iterator endIt) + : ArrayImpl(beginIt, endIt) + { + } +#endif + +}; +#endif + +#endif diff --git a/extern/xspublic/xstypes/xsrange.c b/extern/xspublic/xstypes/xsrange.c new file mode 100644 index 0000000..bfd3c22 --- /dev/null +++ b/extern/xspublic/xstypes/xsrange.c @@ -0,0 +1,88 @@ + +// Copyright (c) 2003-2022 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 "xsrange.h" + +/*! \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. +*/ + +/*! \addtogroup cinterface C Interface + @{ +*/ + +/*! \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 + \returns The number of values in the range (inclusive) +*/ +int XsRange_count(const XsRange* thisPtr) +{ + if (thisPtr->m_last < thisPtr->m_first) + return 0; + return 1 + thisPtr->m_last - thisPtr->m_first; +} + +/*! \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 + \returns The number of values in the range (exclusive) +*/ +int XsRange_interval(const XsRange* thisPtr) +{ + if (thisPtr->m_last <= thisPtr->m_first) + return 0; + return thisPtr->m_last - thisPtr->m_first; +} + +/*! \relates XsRange \brief Test if the range contains the given value \a i. */ +int XsRange_contains(const XsRange* thisPtr, int i) +{ + return (i >= thisPtr->m_first && i <= thisPtr->m_last); +} + +/*! \relates XsRange \brief Set a new range. */ +void XsRange_setRange(XsRange* thisPtr, int f, int l) +{ + thisPtr->m_first = f; + thisPtr->m_last = l; +} + +/*! \relates XsRange \brief Test if the range is empty. + \details An empty range has a last element that is lower than its first element. + \returns true if the range is empty, false otherwise +*/ +int XsRange_empty(const XsRange* thisPtr) +{ + return thisPtr->m_last < thisPtr->m_first; +} + +/*! @} */ diff --git a/extern/xspublic/xstypes/xsrange.h b/extern/xspublic/xstypes/xsrange.h new file mode 100644 index 0000000..d0b326e --- /dev/null +++ b/extern/xspublic/xstypes/xsrange.h @@ -0,0 +1,155 @@ + +// Copyright (c) 2003-2022 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 XSRANGE_H +#define XSRANGE_H + +#include "xstypesconfig.h" + +#ifdef __cplusplus +extern "C" { +#endif +#ifndef __cplusplus +#define XSRANGE_INITIALIZER { 0, -1 } +#endif + +struct XsRange; + +XSTYPES_DLL_API int XsRange_count(const struct XsRange* thisPtr); +XSTYPES_DLL_API int XsRange_contains(const struct XsRange* thisPtr, int i); +XSTYPES_DLL_API int XsRange_interval(const struct XsRange* thisPtr); +XSTYPES_DLL_API void XsRange_setRange(struct XsRange* thisPtr, int f, int l); +XSTYPES_DLL_API int XsRange_empty(const struct XsRange* thisPtr); + +#ifdef __cplusplus +} // extern "C" +#endif + +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) + : m_first(f) + , m_last(l) + {} + + //! \brief Constructs a range based upon \a other + inline XsRange(const XsRange& other) + : m_first(other.m_first) + , m_last(other.m_last) + {} + + //! \brief Assigns the range \a other to this + inline XsRange& operator = (const XsRange& other) + { + m_first = other.m_first; + m_last = other.m_last; + return *this; + } + + //! \brief \copybrief XsRange_count + inline int count() const + { + return XsRange_count(this); + } + + //! \brief \copybrief XsRange_interval + inline int interval() const + { + return XsRange_interval(this); + } + + //! \brief \copybrief XsRange_contains + inline bool contains(int i) const + { + return 0 != XsRange_contains(this, i); + } + + //! \brief Set the range to \a f - \a l + inline void setRange(int f, int l) + { + m_first = f; + m_last = l; + } + + //! \brief \copybrief XsRange_empty + inline bool empty() const + { + return 0 != XsRange_empty(this); + } + + //! \brief Return the \e first value of the range + inline int first() const + { + return m_first; + } + + //! \brief Return the \e last value of the range + inline int last() const + { + return m_last; + } + + //! \brief Return true if this is equal to other + bool operator == (const XsRange& other) const + { + return m_first == other.m_first && m_last == other.m_last; + } + + //! \brief Return true if start of this is less than start of other + bool operator < (const XsRange& other) const + { + return m_first < other.m_first; + } + +private: +#endif + + int m_first; //!< Storage for the lower end of the range + int m_last; //!< Storage for the upper end of the range +}; + +typedef struct XsRange XsRange; + +#if defined(__cplusplus) && !defined(XSENS_NO_STL) +#include + +/*! \brief Stream output operator for XsRange */ +template +std::basic_ostream<_CharT, _Traits>& operator<<(std::basic_ostream<_CharT, _Traits>& o, XsRange const& xs) +{ + return (o << "(" << xs.first() << ", " << xs.last() << "]"); +} +#endif + +#endif diff --git a/extern/xspublic/xstypes/xsrawgnsspvtdata.h b/extern/xspublic/xstypes/xsrawgnsspvtdata.h new file mode 100644 index 0000000..05da144 --- /dev/null +++ b/extern/xspublic/xstypes/xsrawgnsspvtdata.h @@ -0,0 +1,186 @@ + +// Copyright (c) 2003-2022 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 XSRAWGNSSPVTDATA_H +#define XSRAWGNSSPVTDATA_H + +#include "pstdint.h" +#include "xstypedefs.h" + +#ifdef _MSC_VER + #pragma pack(push, 1) +#elif defined (__ICCARM__) + _Pragma("pack(push, 1)") +#endif + +#define XSRAWGNSSPVTDATA_INITIALIZER { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 } +#ifdef __cplusplus + #include +#endif + +// 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 +{ + uint32_t m_itow; //!< GNSS time of week (ms) + uint16_t m_year; //!< Year (UTC) + uint8_t m_month; //!< Month (UTC) + uint8_t m_day; //!< Day of Month (UTC) + uint8_t m_hour; //!< Hour of day 0..23 (UTC) + uint8_t m_min; //!< Minute of hour 0..59 (UTC) + uint8_t m_sec; //!< Seconds of minute 0..60 (UTC) + uint8_t m_valid; /*!< Validity Flags + bit(0) = Set if UTC Date is valid + bit(1) = Set if UTC Time of Day if valid + bit(2) = Set if UTC Time of Day has been fully resolved (no seconds uncertainty) +*/ + uint32_t m_tAcc; //!< Time accuracy estimate (ns) (UTC) + int32_t m_nano; //!< Fraction of second (ns) -1e9..1e9 (UTC) + uint8_t m_fixType; /*!< GNSSfix Type, range 0..5 + 0x00 = No Fix + 0x01 = Dead Reckoning only + 0x02 = 2D-Fix + 0x03 = 3D-Fix + 0x04 = GNSS + dead reckoning combined + 0x05 = Time only fix + 0x06..0xff: reserved +*/ + 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(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 + uint8_t m_res1; //!< Reserved for future use (1) + int32_t m_lon; //!< Longitude (deg) (scaling 1e-7) + int32_t m_lat; //!< Latitude (deg) (scaling 1e-7) + int32_t m_height; //!< Height above ellipsoid (mm) + int32_t m_hMsl; //!< Height above mean sea level (mm) + + uint32_t m_hAcc; //!< Horizontal accuracy estimate (mm) + uint32_t m_vAcc; //!< Vertical accuracy estimate (mm) + + int32_t m_velN; //!< NED north velocity (mm/s) + int32_t m_velE; //!< NED east velocity (mm/s) + int32_t m_velD; //!< NED down velocity (mm/s) + int32_t m_gSpeed; //!< 2D Ground Speed (mm/s) + int32_t m_headMot; //!< 2D Heading of motion (deg) (scaling 1e-5) + + uint32_t m_sAcc; //!< Speed accuracy estimate (mm/s) + uint32_t m_headAcc; //!< Heading accuracy estimate (both motion and vehicle) (deg) (scaling 1-e5) + int32_t m_headVeh; //!< 2D Heading of vehicle (deg) (scaling 1e-5) + + uint16_t m_gdop; //!< Geometric DOP (scaling 0.01) + uint16_t m_pdop; //!< Position DOP (scaling 0.01) + uint16_t m_tdop; //!< Time DOP (scaling 0.01) + uint16_t m_vdop; //!< Vertical DOP (scaling 0.01) + uint16_t m_hdop; //!< Horizontal DOP (scaling 0.01) + uint16_t m_ndop; //!< Northing DOP (scaling 0.01) + uint16_t m_edop; //!< Easting DOP (scaling 0.01) + +#ifdef __cplusplus + /*! \brief Returns true if all fields of this and \a other are exactly identical */ + inline bool operator == (const XsRawGnssPvtData& other) const + { + // direct memory comparison is allowed because all fields line up properly + return std::memcmp(this, &other, sizeof(XsRawGnssPvtData)) == 0; + } +#endif + +#ifdef SWIG +}; +#else +} XS_PACKED_STRUCT; +#endif +typedef struct XsRawGnssPvtData XsRawGnssPvtData; + +#ifdef _MSC_VER + #pragma pack(pop) +#elif defined (__ICCARM__) + _Pragma("pack(pop)") +#endif + +#endif diff --git a/extern/xspublic/xstypes/xsrawgnsssatinfo.h b/extern/xspublic/xstypes/xsrawgnsssatinfo.h new file mode 100644 index 0000000..8b0884a --- /dev/null +++ b/extern/xspublic/xstypes/xsrawgnsssatinfo.h @@ -0,0 +1,144 @@ + +// Copyright (c) 2003-2022 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 XSRAWGNSSSATINFO_H +#define XSRAWGNSSSATINFO_H + +#include "pstdint.h" + +#ifdef _MSC_VER + #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 } +#endif + +/*! \addtogroup enums Global enumerations + @{ +*/ +/*! \brief Xsens Satellite Info Flags +*/ +enum XsSatInfoFlags +{ + XSIF_SignalQualityIndicator_Mask = 0x7, + XSIF_SignalQualityIndicator_NoSignal = 0x0, + XSIF_SignalQualityIndicator_Searching = 0x1, + XSIF_SignalQualityIndicator_Acquired = 0x2, + XSIF_SignalQualityIndicator_Unusable = 0x3, + XSIF_SignalQualityIndicator_CodeTimeOk = 0x4, + XSIF_SignalQualityIndicator_CodeCarrierTimeOk1 = 0x5, + XSIF_SignalQualityIndicator_CodeCarrierTimeOk2 = 0x6, + XSIF_SignalQualityIndicator_CodeCarrierTimeOk3 = 0x7, + XSIF_UsedForNavigation_Mask = 0x8, + XSIF_UsedForNavigation_Used = 0x8, + XSIF_HealthFlag_Mask = 0x30, + XSIF_HealthFlag_Unknown = 0x00, + XSIF_HealthFlag_Healthy = 0x10, + XSIF_HealthFlag_Unhealthy = 0x20, + XSIF_Differential_Mask = 0x40, + XSIF_Differential_Available = 0x40 +}; +/*! @} */ +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; /*!< + bits[0..2] : Signal quality indicator + 0 = No signal + 1 = Searching signal + 2 = Signal acquired + 3 = Signal detected but unusable + 4 = Code locked and time synchronized + 5, 6, 7 = Code and carrier locked and time synchronized + bits[3] : Is set to 1 when the SV is being used for navigation + bits[4..5] : SV health flag + 0 = unknown + 1 = healthy + 2 = unhealthy + bits[6] : Is set to 1 when differential correction data is available + bits[7] : reserved +*/ +}; +typedef struct XsSatInfo XsSatInfo; + +/*! \brief A container for GNSS Satellite Information +*/ +struct XsRawGnssSatInfo +{ + uint32_t m_itow; //!< GNSS time of week (ms) + uint8_t m_numSvs; //!< Number of satellites + uint8_t m_res1; //!< Reserved for future use (1) + uint8_t m_res2; //!< Reserved for future use (2) + uint8_t m_res3; //!< Reserved for future use (3) + + XsSatInfo m_satInfos[60]; //!< The information of all satellites, maximum 60 + +#ifdef __cplusplus + /*! \brief Returns true if all fields of this and \a other are exactly identical */ + inline bool operator == (const XsRawGnssSatInfo& b) const + { + if (m_itow != b.m_itow || + m_numSvs != b.m_numSvs || + m_res1 != b.m_res1 || + m_res2 != b.m_res2 || + m_res3 != b.m_res3) + return false; + for (uint8_t i = 0; i < m_numSvs; ++i) + if (m_satInfos[i].m_gnssId != b.m_satInfos[i].m_gnssId || + m_satInfos[i].m_svId != b.m_satInfos[i].m_svId || + m_satInfos[i].m_cno != b.m_satInfos[i].m_cno || + m_satInfos[i].m_flags != b.m_satInfos[i].m_flags) + return false; + return true; + } +#endif +}; +typedef struct XsRawGnssSatInfo XsRawGnssSatInfo; + +#ifdef _MSC_VER + #pragma pack(pop) +#endif + +#endif diff --git a/extern/xspublic/xstypes/xsresetmethod.h b/extern/xspublic/xstypes/xsresetmethod.h new file mode 100644 index 0000000..1ce98ef --- /dev/null +++ b/extern/xspublic/xstypes/xsresetmethod.h @@ -0,0 +1,59 @@ + +// Copyright (c) 2003-2022 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 XSRESETMETHOD_H +#define XSRESETMETHOD_H + +/*! \addtogroup enums Global enumerations + @{ +*/ + +//AUTO namespace xstypes { +/*! \brief Orientation reset type. */ +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 + XRM_Inclination = XRM_Object, //!< \brief Reset the inclination (roll, pitch), same as XRM_Object + XRM_Alignment = 4, //!< \brief Reset heading and attitude \details This effectively combines the XRM_Heading and XRM_Object + XRM_Global = XRM_Alignment, //!< \brief Reset the full orientation of the device \details Obsolete. Use XRM_Alignment instead. + XRM_DefaultHeading = 5, //!< \brief Revert to default behaviour for heading, undoes XRM_Heading + XRM_DefaultInclination = 6, //!< \brief Revert to default behaviour for inclination, undoes XRM_Inclination + XRM_DefaultAlignment = 7, //!< \brief Revert to default behaviour for heading and inclination, undoes any reset + XRM_None //!< \brief No reset planned +}; +/*! @} */ +typedef enum XsResetMethod XsResetMethod; +//AUTO } + +#endif diff --git a/extern/xspublic/xstypes/xsresultvalue.c b/extern/xspublic/xstypes/xsresultvalue.c new file mode 100644 index 0000000..de5b2ed --- /dev/null +++ b/extern/xspublic/xstypes/xsresultvalue.c @@ -0,0 +1,289 @@ + +// Copyright (c) 2003-2022 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 "xsresultvalue.h" + +/*! \addtogroup cinterface C Interface + @{ +*/ + +////////////////////////////////////////////////////////////////////////////////////////// +/*! \brief Retrieve a character string corresponding to the given result code. + \param result The result code to convert + \return A static string describing the result code + \note In the Python interface this function is called XsResultValueToString + \note In C/C++ do NOT delete the returned char* as it points to static memory. +*/ +const char* XsResultValue_toString(XsResultValue result) +{ + switch (result) + { + 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_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"; + case XRV_GNSSCONNECTIONLOST: + return "Connection lost with the GNSS module"; + case XRV_GNSSLOWINPUTRATE: + return "GNSS input rate is too low"; + case XRV_GNSSINCOMPLETEDATASET: + return "Incomplete dataset for the GNSS module"; + default: + return "!!Undefined Result Value!!"; + } +} + +/*! @} */ diff --git a/extern/xspublic/xstypes/xsresultvalue.h b/extern/xspublic/xstypes/xsresultvalue.h new file mode 100644 index 0000000..9f9e474 --- /dev/null +++ b/extern/xspublic/xstypes/xsresultvalue.h @@ -0,0 +1,213 @@ + +// Copyright (c) 2003-2022 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 XSRESULTVALUE_H +#define XSRESULTVALUE_H + +#include "xstypesconfig.h" + +#ifdef __cplusplus +extern "C" { +#endif + +////////////////////////////////////////////////////////////////////////////////////////// +/*! \addtogroup enums Global enumerations + @{ +*/ +/*! \brief Xsens result values + \details These values are used to signal success or specific failures of functions + \sa XsResultValue_toString +*/ +enum XsResultValue +{ + // general OK + XRV_OK = 0, //!< 0: Operation was performed successfully + + // communication protocol + XRV_NOBUS = 1, //!< 1: No bus communication possible + XRV_BUSNOTREADY = 2, //!< 2: InitBus and/or SetBID are not issued + XRV_INVALIDPERIOD = 3, //!< 3: Period sent is invalid + XRV_INVALIDMSG = 4, //!< 4: The message is invalid or not implemented + XRV_INITBUSFAIL1 = 16, //!< 16: A slave did not respond to WaitForSetBID + XRV_INITBUSFAIL2 = 17, //!< 17: An incorrect answer received after WaitForSetBID + XRV_INITBUSFAIL3 = 18, //!< 18: After four bus-scans still undetected Motion Trackers + XRV_SETBIDFAIL1 = 20, //!< 20: No reply to SetBID message during SetBID procedure + XRV_SETBIDFAIL2 = 21, //!< 21: Other than SetBIDAck received + XRV_MEASUREMENTFAIL1 = 24, //!< 24: Timer overflow - period too short to collect all data from Motion Trackers + XRV_MEASUREMENTFAIL2 = 25, //!< 25: Motion Tracker responds with other than SlaveData message + XRV_MEASUREMENTFAIL3 = 26, //!< 26: Total bytes of data of Motion Trackers including sample counter exceeds 255 bytes + XRV_MEASUREMENTFAIL4 = 27, //!< 27: Timer overflows during measurement + XRV_MEASUREMENTFAIL5 = 28, //!< 28: Timer overflows during measurement + XRV_MEASUREMENTFAIL6 = 29, //!< 29: No correct response from Motion Tracker during measurement + XRV_TIMEROVERFLOW = 30, //!< 30: Timer overflows during measurement + XRV_BAUDRATEINVALID = 32, //!< 32: Baud rate does not comply with valid range + XRV_INVALIDPARAM = 33, //!< 33: An invalid parameter is supplied + XRV_MEASUREMENTFAIL7 = 35, //!< 35: TX PC Buffer is full + XRV_MEASUREMENTFAIL8 = 36, //!< 36: TX PC Buffer overflow, cannot fit full message + XRV_WIRELESSFAIL = 37, //!< 37: Wireless subsystem failed + XRV_DEVICEERROR = 40, //!< 40: The device generated an error, try updating the firmware + XRV_DATAOVERFLOW = 41, //!< 41: The device generates more data than the bus communication can handle (baud rate may be too low) + XRV_BUFFEROVERFLOW = 42, //!< 42: The sample buffer of the device was full during a communication outage + XRV_EXTTRIGGERERROR = 43, //!< 43: The external trigger is not behaving as configured + XRV_SAMPLESTREAMERROR = 44, //!< 44: The sample stream detected an error in the ordering of sample data + XRV_POWER_DIP = 45, //!< 45: A dip in the power supply was detected and recovered from + XRV_POWER_OVERCURRENT = 46, //!< 46: A current limiter has been activated, shutting down the device + XRV_OVERHEATING = 47, //!< 47: Device temperature is not within operational limits + XRV_BATTERYLOW = 48, //!< 48: Battery level reached lower limit + XRV_INVALIDFILTERPROFILE = 49, //!< 49: Specified filter profile ID is not available on the device or the user is trying to duplicate an existing filter profile type + XRV_INVALIDSTOREDSETTINGS = 50, //!< 50: The settings stored in the device's non volatile memory are invalid + 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 + XRV_NOTIMPLEMENTED = 257, //!< 257: Operation not implemented in this version (yet) + XRV_TIMEOUT = 258, //!< 258: A timeout occurred + XRV_TIMEOUTNODATA = 259, //!< 259: Operation aborted because of no data read + XRV_CHECKSUMFAULT = 260, //!< 260: Checksum fault occurred + XRV_OUTOFMEMORY = 261, //!< 261: No internal memory available + XRV_NOTFOUND = 262, //!< 262: The requested item was not found + XRV_UNEXPECTEDMSG = 263, //!< 263: Unexpected message received (e.g. no acknowledge message received) + XRV_INVALIDID = 264, //!< 264: Invalid id supplied + XRV_INVALIDOPERATION = 265, //!< 265: Operation is invalid at this point + XRV_INSUFFICIENTSPACE = 266, //!< 266: Insufficient buffer space available + XRV_INPUTCANNOTBEOPENED = 267, //!< 267: The specified i/o device can not be opened + XRV_OUTPUTCANNOTBEOPENED = 268, //!< 268: The specified i/o device can not be opened + XRV_ALREADYOPEN = 269, //!< 269: An I/O device is already opened with this object + XRV_ENDOFFILE = 270, //!< 270: End of file is reached + XRV_COULDNOTREADSETTINGS = 271, //!< 271: A required settings file could not be opened or is missing some data + XRV_NODATA = 272, //!< 272: No data is available + XRV_READONLY = 273, //!< 273: Tried to change a read-only value + XRV_NULLPTR = 274, //!< 274: Tried to supply a NULL value where it is not allowed + XRV_INSUFFICIENTDATA = 275, //!< 275: Insufficient data was supplied to a function + XRV_BUSY = 276, //!< 276: Busy processing, try again later + XRV_INVALIDINSTANCE = 277, //!< 277: Invalid instance called, because of an invalid or missing license + XRV_DATACORRUPT = 278, //!< 278: A trusted data stream proves to contain corrupted data + + XRV_READINITFAILED = 279, //!< 279: Failure during read of settings + XRV_NOXMFOUND = 280, //!< 280: Could not find any MVN-compatible hardware + XRV_DEVICECOUNTZERO = 282, //!< 282: No xsens devices found + XRV_MTLOCATIONINVALID = 283, //!< 283: One or more sensors are not where they were expected + XRV_INSUFFICIENTMTS = 284, //!< 284: Not enough sensors were found + XRV_INITFUSIONFAILED = 285, //!< 285: Failure during initialization of Fusion Engine + XRV_OTHER = 286, //!< 286: Something else was received than was requested + + XRV_NOFILEOPEN = 287, //!< 287: No file opened for reading/writing + XRV_NOPORTOPEN = 288, //!< 288: No serial port opened for reading/writing + XRV_NOFILEORPORTOPEN = 289, //!< 289: No file or serial port opened for reading/writing + XRV_PORTNOTFOUND = 290, //!< 290: A required port could not be found + XRV_INITPORTFAILED = 291, //!< 291: The low-level port handler failed to initialize + XRV_CALIBRATIONFAILED = 292, //!< 292: A calibration routine failed + + XRV_CONFIGCHECKFAIL = 293, //!< 293: A configuration-time check of the device failed + XRV_ALREADYDONE = 294, //!< 294: The operation is once only and has already been performed + + XRV_SYNC_SINGLE_SLAVE = 295, //!< 295: The single connected device is configured as a slave + XRV_SYNC_SECOND_MASTER = 296, //!< 296: More than one master was detected + XRV_SYNC_NO_SYNC = 297, //!< 297: A device was detected that was neither master nor slave + XRV_SYNC_NO_MASTER = 298, //!< 298: No master detected + XRV_SYNC_DATA_MISSING = 299, //!< 299: A device is not sending enough data + + XRV_VERSION_TOO_LOW = 300, //!< 300: The version of the object is too low for the requested operation + XRV_VERSION_PROBLEM = 301, //!< 301: The object has an unrecognised version, so it's not safe to perform the operation + + XRV_ABORTED = 302, //!< 302: The process was aborted by an external event, usually a user action or process termination + XRV_UNSUPPORTED = 303, //!< 303: The requested functionality is not supported by the device + + XRV_PACKETCOUNTERMISSED = 304, //!< 304: A packet counter value was missed + + XRV_MEASUREMENTFAILED = 305, //!< 305: An error occurred while trying to put the device in measurement mode + XRV_STARTRECORDINGFAILED = 306, //!< 306: A device could not start recording + XRV_STOPRECORDINGFAILED = 307, //!< 307: A device could not stop recording + + XRV_RADIO_CHANNEL_IN_USE = 311, //!< 311: Radio channel is in use by another system + XRV_UNEXPECTED_DISCONNECT = 312, //!< 312: Motion tracker disconnected unexpectedly + XRV_TOO_MANY_CONNECTED_TRACKERS = 313, //!< 313: Too many motion trackers connected + XRV_GOTOCONFIGFAILED = 314, //!< 314: A device could not be put in config mode + XRV_OUTOFRANGE = 315, //!< 315: Device has gone out of range + XRV_BACKINRANGE = 316, //!< 316: Device is back in range, resuming normal operation + XRV_EXPECTED_DISCONNECT = 317, //!< 317: The device was disconnected + + XRV_RESTORE_COMMUNICATION_FAILED = 318, //!< 318: Restore communication failed + XRV_RESTORE_COMMUNICATION_STOPPED = 319, //!< 319: Restore communication was stopped + + XRV_EXPECTED_CONNECT = 320, //!< 320: The device was connected + XRV_IN_USE = 321, //!< 321: The requested device/port/address is already in use. Most likely returned by XsSocket::bind + + XRV_PERFORMANCE_WARNING = 322, //!< 322: The system running the application can't fully keep up with the incoming data. This may lead to degraded performance or lag. + XRV_PERFORMANCE_OK = 323, //!< 323: The system running the application has recovered from a previously reported XRV_PERFORMANCE_WARNING. + + // notifications + XRV_SHUTTINGDOWN = 400, //!< 400: The device is shutting down + XRV_GNSSCONFIGURATIONERROR = 401, //!< 401: A configuration item was refused by the GNSS module + XRV_GNSSCOMMTIMEOUT = 402, //!< 402: The communication with the GNSS module timed out + XRV_GNSSERROR = 403, //!< 403: Communication between the device and the GNSS module failed + XRV_DEVICE_NOT_CALIBRATED = 404, //!< 404: The EMTS of the device does not contain calibration data + XRV_GNSSCONNECTIONLOST = 405, //!< 405: Connection lost with the GNSS module + XRV_GNSSLOWINPUTRATE = 406, //!< 406: GNSS input rate is too low + XRV_GNSSINCOMPLETEDATASET = 407 //!< 407: Incomplete dataset for the GNSS module +}; +/*! @} */ +typedef enum XsResultValue XsResultValue; + +//! These enum values can be used to specify a device error (XRV_DEVICEERROR 0x28), i.e. a xbus message like [FA FF 42 05 28 XXXXXXXX CS] +enum XsDeviceErrorType +{ + XERR_Unknown = 0, + XERR_ImcuTimeout = 1, + XERR_ImcuSettingsInvalid = 2, + XERR_ImcuSettingsFailure = 3, + XERR_ImcuEmtsWriteFailure = 4, + XERR_ImcuEmtsReadFailure = 5, + XERR_DspCrashed = 6, + XERR_DspBootingTimeout = 7, + XERR_DspSettingsInvalid = 8 +}; +typedef enum XsDeviceErrorType XsDeviceErrorType; + +////////////////////////////////////////////////////////////////////////////////////////// + +/*! \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 + "!!Undefined Result Value!!" string is returned. +*/ +XSTYPES_DLL_API const char* XsResultValue_toString(XsResultValue result); + +#ifdef __cplusplus +} // extern "C" +#endif + +#endif diff --git a/extern/xspublic/xstypes/xsrssi.c b/extern/xspublic/xstypes/xsrssi.c new file mode 100644 index 0000000..4f0fa11 --- /dev/null +++ b/extern/xspublic/xstypes/xsrssi.c @@ -0,0 +1,70 @@ + +// Copyright (c) 2003-2022 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 "xsrssi.h" + +/*! \namespace XsRssi + \brief Contains Received Signal Strength Indication (RSSI) constants +*/ +/*! \addtogroup cinterface C Interface + @{ +*/ + +/*! \brief The maximum RSSI value. + \returns the maximum RSSI value +*/ +XSTYPES_DLL_API int XsRssi_max(void) +{ + return XS_RSSI_MAX; +} + +/*! \brief The RSSI value that was reserved for when the RSSI is unknown. + \returns he RSSI 'unknown' value +*/ +XSTYPES_DLL_API int XsRssi_unknown(void) +{ + return XS_RSSI_UNKNOWN; +} + +/*! \brief Returns the raw RSSI value transformed into a usable (unbiased) number. + \details Actual rssi is calculated by \a raw + XS_RSSI_MAX, where \a raw is a negative number. + \param raw The RSSI value as reported by the device. + \returns The unbiased RSSI value. +*/ +XSTYPES_DLL_API int XsRssi_unbiased(int raw) +{ + if (raw < 0) + return raw + XS_RSSI_MAX; + return XS_RSSI_MAX; +} + +/*! @} */ diff --git a/extern/xspublic/xstypes/xsrssi.h b/extern/xspublic/xstypes/xsrssi.h new file mode 100644 index 0000000..2545c14 --- /dev/null +++ b/extern/xspublic/xstypes/xsrssi.h @@ -0,0 +1,74 @@ + +// Copyright (c) 2003-2022 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 XSRSSI_H +#define XSRSSI_H + +#include "xstypesconfig.h" + +#define XS_RSSI_MAX (128) +#define XS_RSSI_UNKNOWN (-XS_RSSI_MAX) + +#ifdef __cplusplus +extern "C" { +#endif + +XSTYPES_DLL_API int XsRssi_max(void); +XSTYPES_DLL_API int XsRssi_unknown(void); +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; + +/*! \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; + +/*! \copydoc XsRssi_unbiased */ +inline int unbiased(int raw) +{ + return XsRssi_unbiased(raw); +} +} +#endif + +#endif diff --git a/extern/xspublic/xstypes/xsscrdata.h b/extern/xspublic/xstypes/xsscrdata.h new file mode 100644 index 0000000..17c266f --- /dev/null +++ b/extern/xspublic/xstypes/xsscrdata.h @@ -0,0 +1,56 @@ + +// Copyright (c) 2003-2022 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 XSSCRDATA_H +#define XSSCRDATA_H + +#include "pstdint.h" +#include "xsushortvector.h" + +#ifndef __cplusplus + #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 +{ + XsUShortVector m_acc; //!< The raw accelerometer data + XsUShortVector m_gyr; //!< The raw gyroscope data + XsUShortVector m_mag; //!< The raw magnetometer data + uint16_t m_temp; //!< The temperature data +}; +typedef struct XsScrData XsScrData; + +#endif diff --git a/extern/xspublic/xstypes/xssdidata.c b/extern/xspublic/xstypes/xssdidata.c new file mode 100644 index 0000000..d289f9a --- /dev/null +++ b/extern/xspublic/xstypes/xssdidata.c @@ -0,0 +1,77 @@ + +// Copyright (c) 2003-2022 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 "xssdidata.h" + +/*! \class XsSdiData + \brief Contains StrapDown Integration (SDI) data. + \details SDI data consists of a rotation and an acceleration, expressed as an orientation increment + (also known as deltaQ) and a velocity increment (also known as deltaV). +*/ + +/*! \addtogroup cinterface C Interface + @{ +*/ + +/*! \relates XsSdiData + \brief Initialize an %XsSdiData object with the optional arguments. + \param orientationIncrement The orientation increment to initialize the object with, may be 0 + \param velocityIncrement The velocity increment to initialize the object with, may be 0 +*/ +void XsSdiData_construct(XsSdiData* thisPtr, const XsReal* orientationIncrement, const XsReal* velocityIncrement) +{ + if (orientationIncrement) + { + thisPtr->m_orientationIncrement.m_data[0] = orientationIncrement[0]; + thisPtr->m_orientationIncrement.m_data[1] = orientationIncrement[1]; + thisPtr->m_orientationIncrement.m_data[2] = orientationIncrement[2]; + thisPtr->m_orientationIncrement.m_data[3] = orientationIncrement[3]; + } + else + XsQuaternion_destruct(&thisPtr->m_orientationIncrement); + + if (velocityIncrement) + XsVector3_assign(&thisPtr->m_velocityIncrement, velocityIncrement); + else + XsVector3_destruct(&thisPtr->m_velocityIncrement); +} + +/*! \relates XsSdiData + \brief Destruct the object, makes the fields invalid +*/ +void XsSdiData_destruct(XsSdiData* thisPtr) +{ + XsQuaternion_destruct(&thisPtr->m_orientationIncrement); + XsVector3_destruct(&thisPtr->m_velocityIncrement); +} + +/*! @} */ diff --git a/extern/xspublic/xstypes/xssdidata.h b/extern/xspublic/xstypes/xssdidata.h new file mode 100644 index 0000000..fed078f --- /dev/null +++ b/extern/xspublic/xstypes/xssdidata.h @@ -0,0 +1,136 @@ + +// Copyright (c) 2003-2022 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 XSSDIDATA_H +#define XSSDIDATA_H + +#include "xstypesconfig.h" +#include "xsvector3.h" +#include "xsquaternion.h" + +struct XsSdiData; + +#ifdef __cplusplus +extern "C" { +#endif +#ifndef __cplusplus +#define XSSDIDATA_INITIALIZER {XSQUATERNION_INITIALIZER, XSVECTOR3_INITIALIZER} +#endif + +XSTYPES_DLL_API void XsSdiData_construct(struct XsSdiData* thisPtr, const XsReal* orientationIncrement, const XsReal* velocityIncrement); +XSTYPES_DLL_API void XsSdiData_destruct(struct XsSdiData* thisPtr); + +#ifdef __cplusplus +} // extern "C" +#endif + +struct XsSdiData +{ +#ifdef __cplusplus + //! \brief Construct an empty object + inline XsSdiData() + { + } + + //! \brief Construct an initialized object + inline XsSdiData(const XsQuaternion& dq, const XsVector& dv) + : m_orientationIncrement(dq) + , m_velocityIncrement(dv) + { + } + + //! \brief Copy constructor + inline XsSdiData(const XsSdiData& other) + : m_orientationIncrement(other.m_orientationIncrement) + , m_velocityIncrement(other.m_velocityIncrement) + { + } + + //! \brief Assignment operator + inline const XsSdiData& operator=(const XsSdiData& other) + { + if (this != &other) + { + m_orientationIncrement = other.m_orientationIncrement; + m_velocityIncrement = other.m_velocityIncrement; + } + return *this; + } + + //! \brief Clear the object so it contains unity data + inline void zero() + { + m_orientationIncrement = XsQuaternion::identity(); + m_velocityIncrement.zero(); + } + + //! \brief Returns the contained orientation increment + inline const XsQuaternion& orientationIncrement() const + { + return m_orientationIncrement; + } + + //! \brief Update the contained orientation increment + inline void setOrientationIncrement(const XsQuaternion& dq) + { + m_orientationIncrement = dq; + } + + //! \brief Returns the contained velocity increment + inline const XsVector3& velocityIncrement() const + { + return m_velocityIncrement; + } + + //! \brief Update the contained velocity increment + inline void setVelocityIncrement(const XsVector& dv) + { + m_velocityIncrement = dv; + } + + /*! \brief Returns true if all fields of this and \a other are exactly identical */ + inline bool operator == (const XsSdiData& other) const + { + return m_orientationIncrement == other.m_orientationIncrement && + m_velocityIncrement == other.m_velocityIncrement; + } + +private: +#endif + + XsQuaternion m_orientationIncrement; //!< The orientation increment + XsVector3 m_velocityIncrement; //!< The velocity increment +}; + +typedef struct XsSdiData XsSdiData; + +#endif diff --git a/extern/xspublic/xstypes/xssensorranges.cpp b/extern/xspublic/xstypes/xssensorranges.cpp new file mode 100644 index 0000000..c69d05a --- /dev/null +++ b/extern/xspublic/xstypes/xssensorranges.cpp @@ -0,0 +1,247 @@ + +// Copyright (c) 2003-2022 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 "xssensorranges.h" +#include + +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; + } + + /*! \brief Return the hardware manufacturer from \a productCode + */ + HardwareManufacturerType findHardwareManufacturerC(const XsString* productCode) + { + if (strstr(productCode->c_str(), "MT") != nullptr) + return HMT_MT; + + return HMT_None; + } + + /*! \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; + } + + /* Return the accelerometer range field */ + static char accelerometerRangeField(const char* productCode) + { + 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)) + { + 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; + } + } + + /*! \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)) + { + 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; + } + } + + /*! \brief The gyroscope range from product code \a productCode + */ + double gyroscopeRangeC(const XsString* productCode) + { + switch (findHardwareManufacturerC(productCode)) + { + 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; + } + } + + /*! \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)) + { + 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; + } + } + +} diff --git a/extern/xspublic/xstypes/xssensorranges.h b/extern/xspublic/xstypes/xssensorranges.h new file mode 100644 index 0000000..596701f --- /dev/null +++ b/extern/xspublic/xstypes/xssensorranges.h @@ -0,0 +1,90 @@ + +// Copyright (c) 2003-2022 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 XSSENSORRANGES_H +#define XSSENSORRANGES_H + +#include "xstypesconfig.h" +#include "xsstring.h" + +#ifdef __cplusplus +extern "C" { +#endif + +enum HardwareManufacturerType +{ + HMT_MT = 0, + HMT_None +}; +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 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); + +#ifdef __cplusplus +} + +inline static XsString findHardwareType(const XsString& productCode) +{ + XsString rv; + findHardwareTypeC(&productCode, &rv); + return rv; +} +inline static HardwareManufacturerType findHardwareManufacturer(const XsString& productCode) +{ + return findHardwareManufacturerC(&productCode); +} +inline static double accelerometerRange(const XsString& productCode, int32_t hwVersionMajor) +{ + return accelerometerRangeC(&productCode, hwVersionMajor); +} +inline static double gyroscopeRange(const XsString& productCode) +{ + return gyroscopeRangeC(&productCode); +} +inline static double actualAccelerometerRange(const XsString& productCode, int32_t hwVersionMajor) +{ + return actualAccelerometerRangeC(&productCode, hwVersionMajor); +} +inline static double actualGyroscopeRange(const XsString& productCode) +{ + return actualGyroscopeRangeC(&productCode); +} +#endif + +#endif diff --git a/extern/xspublic/xstypes/xssimpleversion.c b/extern/xspublic/xstypes/xssimpleversion.c new file mode 100644 index 0000000..46fd1f3 --- /dev/null +++ b/extern/xspublic/xstypes/xssimpleversion.c @@ -0,0 +1,128 @@ + +// Copyright (c) 2003-2022 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 "xssimpleversion.h" + +#ifdef _WIN32 + #include +#else + #ifdef __GNUC__ + #include + #endif + #include + #include + #include +#endif + +/*! \class XsSimpleVersion + \brief A class to store version information +*/ + +/*! \addtogroup cinterface C Interface + @{ +*/ + +/*! \relates XsSimpleVersion \brief Test if this is a null-version. */ +int XsSimpleVersion_empty(const XsSimpleVersion* thisPtr) +{ +#ifdef __cplusplus + return thisPtr->major() == 0 && thisPtr->minor() == 0 && thisPtr->revision() == 0; +#else + return thisPtr->m_major == 0 && thisPtr->m_minor == 0 && thisPtr->m_revision == 0; +#endif +} + +/*! \brief Swap the contents of \a a with those of \a b +*/ +void XsSimpleVersion_swap(struct XsSimpleVersion* a, struct XsSimpleVersion* b) +{ + XsSimpleVersion tmp = *a; + *a = *b; + *b = tmp; +} + +/*! \brief Compare two XsSimpleVersion objects. + \param a The left hand side of the comparison + \param b The right hand side of the comparison + \return 0 when they're equal +*/ +int XsSimpleVersion_compare(XsSimpleVersion const* a, XsSimpleVersion const* b) +{ +#ifdef __cplusplus + return a->major() != b->major() || a->minor() != b->minor() || a->revision() != b->revision(); +#else + return a->m_major != b->m_major || a->m_minor != b->m_minor || a->m_revision != b->m_revision; +#endif +} + +/*! \brief Platdorm independent request of the OS version. Results are cached and of course platform dependent. */ +void XsSimpleVersion_osVersion(XsSimpleVersion* thisPtr) +{ + static XsSimpleVersion* fetched = 0; + if (!fetched) + { +#ifdef _WIN32 + NTSTATUS(WINAPI * RtlGetVersion)(LPOSVERSIONINFOEXW); + OSVERSIONINFOEXW osInfo; + *(FARPROC*)&RtlGetVersion = GetProcAddress(GetModuleHandleA("ntdll"), "RtlGetVersion"); + if (RtlGetVersion) + { + osInfo.dwOSVersionInfoSize = sizeof(osInfo); + RtlGetVersion(&osInfo); + fetched = (XsSimpleVersion*)malloc(sizeof(XsSimpleVersion)); + fetched->m_major = (uint8_t) osInfo.dwMajorVersion; + fetched->m_minor = (uint8_t) osInfo.dwMinorVersion; + fetched->m_revision = (uint8_t) osInfo.dwBuildNumber; + } +#elif defined(__GNUC__) + struct utsname utsn; + if (0 == uname(&utsn)) + { + fetched = (XsSimpleVersion*)malloc(sizeof(XsSimpleVersion)); + unsigned int major = 0, minor = 0, revis = 0; + sscanf(utsn.release, "%u.%u.%u", &major, &minor, &revis); + fetched->m_major = major; + fetched->m_minor = minor; + fetched->m_revision = revis; + } +#else + fetched = (XsSimpleVersion*)malloc(sizeof(XsSimpleVersion)); + fetched->m_major = 0; + fetched->m_minor = 0; + fetched->m_revision = 0; +#endif + } + if (fetched && thisPtr) + memcpy(thisPtr, fetched, sizeof(XsSimpleVersion)); +} + +/*! @} */ diff --git a/extern/xspublic/xstypes/xssimpleversion.h b/extern/xspublic/xstypes/xssimpleversion.h new file mode 100644 index 0000000..0ad68e8 --- /dev/null +++ b/extern/xspublic/xstypes/xssimpleversion.h @@ -0,0 +1,192 @@ + +// Copyright (c) 2003-2022 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 XSSIMPLEVERSION_H +#define XSSIMPLEVERSION_H + +#include "xstypesconfig.h" + +#undef minor +#undef major + +#ifdef __cplusplus +struct XsSimpleVersion; +extern "C" { +#endif +#ifndef __cplusplus +#define XSSIMPLEVERSION_INITIALIZER { 0, 0, 0 } +typedef struct XsSimpleVersion XsSimpleVersion; +#endif + +XSTYPES_DLL_API int XsSimpleVersion_empty(const XsSimpleVersion* thisPtr); +XSTYPES_DLL_API void XsSimpleVersion_swap(XsSimpleVersion* a, XsSimpleVersion* b); +XSTYPES_DLL_API int XsSimpleVersion_compare(XsSimpleVersion const* a, XsSimpleVersion const* b); +XSTYPES_DLL_API void XsSimpleVersion_osVersion(XsSimpleVersion* thisPtr); + +#ifdef __cplusplus +} // extern "C" +#endif + +struct XsSimpleVersion +{ +#ifdef __cplusplus + //! \brief Constructs a simple-version object using the supplied parameters or an empty version object if no parameters are given. + explicit XsSimpleVersion(int vmaj = 0, int vmin = 0, int vrev = 0) + : m_major((uint8_t) vmaj) + , m_minor((uint8_t) vmin) + , m_revision((uint8_t) vrev) + {} + + //! \brief Constructs a simple-version object based upon the \a other object + XsSimpleVersion(const XsSimpleVersion& other) + : m_major(other.m_major) + , m_minor(other.m_minor) + , m_revision(other.m_revision) + {} + + //! \brief Assign the simple-version from the \a other object + XsSimpleVersion& operator = (const XsSimpleVersion& other) + { + m_major = other.m_major; + m_minor = other.m_minor; + m_revision = other.m_revision; + return *this; + } + + /*! \brief Test if the \a other simple-version is equal to this. */ + inline bool operator == (const XsSimpleVersion& other) const + { + return !XsSimpleVersion_compare(this, &other); + } + + /*! \brief Test if the \a other simple-version is different to this. */ + inline bool operator != (const XsSimpleVersion& other) const + { + if (m_major != other.m_major || m_minor != other.m_minor || m_revision != other.m_revision) + return true; + + return false; + } + + /*! \brief Test if the \a other version is lower than this. The comparison involves only the version numbers (major, minor and revision). */ + inline bool operator < (const XsSimpleVersion& other) const + { + 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 false; + } + + /*! \brief Test if the \a other version is lower or equal than this. */ + inline bool operator <= (const XsSimpleVersion& other) const + { + 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. */ + inline bool operator > (const XsSimpleVersion& other) const + { + return !(*this <= other); + } + + /*! \brief Test if the \a other version is higher or equal than this. */ + inline bool operator >= (const XsSimpleVersion& other) const + { + return !(*this < other); + } + + //! \brief \copybrief XsSimpleVersion_empty + inline bool empty() const + { + return 0 != XsSimpleVersion_empty(this); + } + + //! \brief Return the \e major part of the version + 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; + } + //! \brief Return the \e revision part of the version + 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; + } + (); + return rv; + } + +private: +#endif + uint8_t m_major; //!< The major part of the version number + uint8_t m_minor; //!< The minor part of the version number + uint8_t m_revision; //!< The revision number of the version +}; + +#endif diff --git a/extern/xspublic/xstypes/xssnapshot.c b/extern/xspublic/xstypes/xssnapshot.c new file mode 100644 index 0000000..5d00f7a --- /dev/null +++ b/extern/xspublic/xstypes/xssnapshot.c @@ -0,0 +1,59 @@ + +// Copyright (c) 2003-2022 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 "xssnapshot.h" + +/*! \struct XsSnapshot + \brief Container for Snapshot data. +*/ + +/*! \addtogroup cinterface C Interface + @{ +*/ + +/*! \relates XsSnapshot + \brief Construct an %XsSnapshot object +*/ +void XsSnapshot_construct(XsSnapshot* thisPtr) +{ + memset(thisPtr, 0, sizeof(XsSnapshot)); +} + +/*! \relates XsSnapshot + \brief Destruct an %XsSnapshot object +*/ +void XsSnapshot_destruct(XsSnapshot* thisPtr) +{ + (void) thisPtr; +} + +/*! @} */ diff --git a/extern/xspublic/xstypes/xssnapshot.h b/extern/xspublic/xstypes/xssnapshot.h new file mode 100644 index 0000000..256a4d0 --- /dev/null +++ b/extern/xspublic/xstypes/xssnapshot.h @@ -0,0 +1,134 @@ + +// Copyright (c) 2003-2022 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 XSSNAPSHOT_H +#define XSSNAPSHOT_H + +#include "xstypesconfig.h" +#include "xsdeviceid.h" + +struct XsSnapshot; + +#ifdef __cplusplus +extern "C" { +#endif +#ifndef __cplusplus +#define XSSNAPSHOT_INITIALIZER { XSDEVICEID_INITIALIZER, 0, 0, 0,0,0,0, 0,0,0, 0,0,0, 0, 0, 0, 0 } +#endif + +XSTYPES_DLL_API void XsSnapshot_construct(struct XsSnapshot* thisPtr); +XSTYPES_DLL_API void XsSnapshot_destruct(struct XsSnapshot* thisPtr); + +#ifdef __cplusplus +} // extern "C" +#endif + +enum SnapshotType +{ + ST_Awinda = 0, // + ST_Full +}; +typedef enum SnapshotType SnapshotType; +/*! \brief A container for Snapshot data +*/ +struct XsSnapshot +{ + XsDeviceId m_deviceId; /*!< \brief The ID of the device that created the data */ + uint32_t m_frameNumber; /*!< \brief The frame */ + uint64_t m_timestamp; /*!< \brief The timestamp */ + int32_t m_iQ[4]; /*!< \brief The integrated orientation */ + int64_t m_iV[3]; /*!< \brief The integrated velocity */ + int32_t m_mag[3]; /*!< \brief The magnetic field */ + int32_t m_baro; /*!< \brief The barometric pressure */ + uint16_t m_status; /*!< \brief The clipping flags of the latest interval */ + uint8_t m_accClippingCounter; /*!< \brief The clipping event counter for the Acc */ + uint8_t m_gyrClippingCounter; /*!< \brief The clipping event counter for the Gyr */ + SnapshotType m_type; /*!< \brief The type of the snapshot (Awinda, Full) */ +#ifdef __cplusplus + /*! \brief Returns true if all fields of this and \a other are exactly identical */ + inline bool operator == (const XsSnapshot& other) const + { + if (m_frameNumber != other.m_frameNumber || + m_baro != other.m_baro || + m_status != other.m_status || + m_accClippingCounter != other.m_accClippingCounter || + m_gyrClippingCounter != other.m_gyrClippingCounter || + m_type != other.m_type) + return false; + + for (int i = 0; i < 3; ++i) + { + if (m_iQ[i] != other.m_iQ[i] || + m_iV[i] != other.m_iV[i] || + m_mag[i] != other.m_mag[i]) + return false; + } + if (m_type == ST_Full) + { + if (m_iQ[3] != other.m_iQ[3] || m_timestamp != other.m_timestamp) + return false; + } + return true; + } +#endif +}; +typedef struct XsSnapshot XsSnapshot; + +/*! \brief Status flag definitions for XsSnapshot status field */ +enum SnapshotStatusFlag +{ + FSFL_ClipAccX = 0x0001, + FSFL_ClipAccY = 0x0002, + FSFL_ClipAccZ = 0x0004, + FSFL_ClipAccMask = 0x0007, + FSFL_ClipGyrX = 0x0008, + FSFL_ClipGyrY = 0x0010, + FSFL_ClipGyrZ = 0x0020, + FSFL_ClipGyrMask = 0x0038, + FSFL_ClipMagX = 0x0040, + FSFL_ClipMagY = 0x0080, + FSFL_ClipMagZ = 0x0100, + FSFL_ClipMagMask = 0x01C0, + FSFL_MagIsNew = 0x0200, + FSFL_BaroIsNew = 0x0400, + FSFL_RotationMask = 0x1800 +}; +typedef enum SnapshotStatusFlag SnapshotStatusFlag; + + + +#define FSFL_ClipAccShift 0 +#define FSFL_ClipGyrShift 04 +#define FSFL_ClipMagshift 6 +#define FSFL_RotationShift 11 + +#endif diff --git a/extern/xspublic/xstypes/xssocket.c b/extern/xspublic/xstypes/xssocket.c new file mode 100644 index 0000000..58cfc70 --- /dev/null +++ b/extern/xspublic/xstypes/xssocket.c @@ -0,0 +1,1295 @@ + +// Copyright (c) 2003-2022 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. +// + +#ifdef _WIN32 +#include +#include +#include +#include +#include + +#pragma comment(lib, "ws2_32.lib") +#else +#include +#include +#include +#include +#include +#include +#include +#include +#include +#define SOCKET int32_t +#define SOCKET_ERROR -1 +#define INVALID_SOCKET ((int32_t)-1) + +/* capture the fact that windows has a specific closesocket function */ +static int closesocket(SOCKET s) +{ + close(s); + return 0; +} +#endif +#if defined(__FreeBSD__) || defined(BSD) || defined(__APPLE__) || defined(__linux__) + #define USE_GETIFADDRS 1 + #include +#endif +#include + +// MSG_NOSIGNAL is linux stuff +#ifndef MSG_NOSIGNAL + #define MSG_NOSIGNAL 0 +#endif + +#include "xssocket.h" + +#ifndef PEEKBUFSIZE + #define PEEKBUFSIZE 32768 +#endif + +/* the socket data */ +struct XsSocketPrivate +{ + SOCKET m_sd; + + enum NetworkLayerProtocol m_ipVersion; + enum IpProtocol m_ipProtocol; + + struct sockaddr_storage m_remoteAddr; + socklen_t m_remoteAddrLen; + + XsResultValue m_lastResult; + int m_lastSystemError; + + XsDataFlags m_flags; + char m_peekBuf[PEEKBUFSIZE]; + +#ifdef _WIN32 + WSADATA m_sockData; +#endif +}; + +/* Return non-zero if the native socket s is usable */ +static int socketIsUsable(SOCKET s) +{ + return (s != INVALID_SOCKET && s > 0); +} + + +/*! + \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; + socklen_t length; + + switch (remote->ss_family) + { + case AF_INET: + default: + src = &(((struct sockaddr_in*)remote)->sin_addr); + length = INET_ADDRSTRLEN; + break; + case AF_INET6: + src = &(((struct sockaddr_in6*)remote)->sin6_addr); + length = INET6_ADDRSTRLEN; + break; + } + XsString_resize(address, (XsSize)(ptrdiff_t) length); + if (inet_ntop(remote->ss_family, src, address->m_data, (size_t)(ptrdiff_t) length) == NULL) + XsString_erase(address, 0, address->m_size); +} + +/* Update the last result of thisPtr to retval, if thisPtr is non-null + + Returns retval for easy use in return statements. +*/ +static XsResultValue setLastResult(XsSocket* thisPtr, XsResultValue retval, int systemError) +{ + if (thisPtr && thisPtr->d) + { + thisPtr->d->m_lastResult = retval; + thisPtr->d->m_lastSystemError = systemError; + } + return retval; +} + +/* Translate a platform error into an XsResultValue + + If thisPtr is non-null, the socket's last result will be updated. +*/ +static XsResultValue translateAndReturnSocketError(XsSocket* thisPtr, int functionResult) +{ + XsResultValue result; + int err; + if (functionResult == 0) + return setLastResult(thisPtr, XRV_OK, 0); +#ifdef _WIN32 + 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; + } +#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; + } +#endif + return setLastResult(thisPtr, result, err); +} + +static void translateSocketError(XsSocket* thisPtr, int functionResult) +{ + (void)translateAndReturnSocketError(thisPtr, functionResult); +} + +/* Initialize the socket + + This function performs some basic initialization on the socket +*/ +static void XsSocket_initialize(XsSocket* thisPtr, XsDataFlags flags) +{ + thisPtr->d = (struct XsSocketPrivate*)malloc(sizeof(struct XsSocketPrivate)); + memset(thisPtr->d, 0, sizeof(*thisPtr->d)); + thisPtr->d->m_sd = INVALID_SOCKET; + thisPtr->d->m_flags = flags; +#ifdef _WIN32 + (void)WSAStartup(MAKEWORD(2, 0), &thisPtr->d->m_sockData); +#endif +} + +/*! \brief Create socket \a thisPtr for IP version \a ip with protocol \a protocol + + \param[in] ip the IP version + \param[in] protocol the protocol. Currently supported are TCP and UDP + \relates XsSocket +*/ +void XsSocket_create(XsSocket* thisPtr, enum NetworkLayerProtocol ip, enum IpProtocol protocol) +{ + XsSocket_initialize(thisPtr, XSDF_Managed); + + thisPtr->d->m_sd = socket( + (ip == NLP_IPV4) ? PF_INET : PF_INET6, + (protocol == IP_UDP) ? SOCK_DGRAM : SOCK_STREAM, + (protocol == IP_UDP) ? IPPROTO_UDP : IPPROTO_TCP); + + thisPtr->d->m_ipVersion = ip; + thisPtr->d->m_ipProtocol = protocol; +} + +/* 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) +{ + XsSocket_initialize(thisPtr, flags); + thisPtr->d->m_sd = nativeSocket; + + if (theirInfo) + { + memcpy(&thisPtr->d->m_remoteAddr, theirInfo, (size_t)(ptrdiff_t) infolen); + thisPtr->d->m_remoteAddrLen = infolen; + } + else + { + int rv; + thisPtr->d->m_remoteAddrLen = sizeof(thisPtr->d->m_remoteAddr); + rv = getpeername(thisPtr->d->m_sd, (struct sockaddr*)&thisPtr->d->m_remoteAddr, &thisPtr->d->m_remoteAddrLen); + translateSocketError(thisPtr, rv); + } + + 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; + } +} + +/*! + \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. +*/ +XSOCKET XsSocket_nativeDescriptor(XsSocket const* thisPtr) +{ + return thisPtr->d->m_sd; +} + +/*! \brief Close a socket + \details This function closes the socket. The XsSocket object is reusable after this, but this is + not recommended. + \return XRV_OK if the socket was already closed or is now closed, XRV_ERROR if some error occurred while closing the socket + \relates XsSocket +*/ +XsResultValue XsSocket_close(XsSocket* thisPtr) +{ + if (thisPtr->d->m_sd == INVALID_SOCKET) + return XRV_OK; + + if (closesocket(thisPtr->d->m_sd) == SOCKET_ERROR) + return XRV_ERROR; + + thisPtr->d->m_sd = INVALID_SOCKET; + return XRV_OK; +} + +/*! \brief Destroy the given socket + + After calling this function, the XsSocket will no longer be usable for socket communications. + + It is safe to call this function twice on the same XsSocket. + + If the socket was created from a file descriptor using + XsSocket_createFromFileDescriptor and had the XSDF_Managed flag set then + the underlying socket will be closed. + + \relates XsSocket +*/ +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 + // decrease ref counter and possibly clean up socket dll + // http://msdn.microsoft.com/en-us/library/windows/desktop/ms741549%28v=vs.85%29.aspx + (void)WSACleanup(); +#endif + } +} + +/*! \brief Wait for read or write for mstimeout milliseconds on socket thisPtr + \param[in] mstimeout timeout in milliseconds, set to a negative value to do a blocking call + \param[in,out] canRead if not null will be set to non-zero if the socket has data to read + \param[in,out] canWrite if not null will be set to non-zero if the socket can be written to + \returns -1 on error, 0 when no data is available, a positive number otherwise + \relates XsSocket + + Use canRead and canWrite to determine whether you can read from or write to the socket. + They will be set to non-zero if you can write, zero if you can't. + 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) +{ + fd_set readfd; + fd_set writefd; + fd_set errorfd; + struct timeval timeout; + int rv; + FD_ZERO(&readfd); + FD_ZERO(&writefd); + FD_ZERO(&errorfd); + FD_SET(thisPtr->d->m_sd, &readfd); + FD_SET(thisPtr->d->m_sd, &writefd); + FD_SET(thisPtr->d->m_sd, &errorfd); + + if (canRead) + *canRead = 0; + if (canWrite) + *canWrite = 0; + + 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); + + 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; + break; + } + + if (canRead) + *canRead = FD_ISSET(thisPtr->d->m_sd, &readfd); + if (canWrite) + *canWrite = FD_ISSET(thisPtr->d->m_sd, &writefd); + break; + } + return rv; +} + +/*! \brief Read \a size data into \a dest + + \param[in,out] dest a pointer to a data buffer + \param[in] size the size of the buffer \a dest points to + \param[in] timeout the amount of time in milliseconds to wait for data + + \returns the size of the data read from the socket, -1 on error. + \relates XsSocket +*/ +int XsSocket_read(XsSocket* thisPtr, void* dest, XsSize size, int timeout) +{ + return XsSocket_readFrom(thisPtr, dest, size, NULL, NULL, timeout); +} + +/* peek at the size of the incoming data */ +static int peekPendingDataSize(XsSocket* thisPtr) +{ + int rv = recvfrom(thisPtr->d->m_sd, thisPtr->d->m_peekBuf, PEEKBUFSIZE, MSG_PEEK, NULL, NULL); + if (rv < 0) + { + translateSocketError(thisPtr, rv); + if (thisPtr->d->m_lastResult == XRV_BUFFEROVERFLOW) + return PEEKBUFSIZE; + } + return rv; +} + +/*! \brief Read \a size data into \a dest + + \param[in,out] dest a pointer to a data buffer, may be NULL, in which case only the size of the pending data is reported. + \param[in] size the size of the buffer \a dest points to + \param[in,out] hostname a pointer to a string that contains the hostname of the sender after returning from this function. May be NULL. + \param[in,out] port a pointer that will contain the port of the sender after returning from this function. May be NULL. + \param[in] timeout the amount of time in milliseconds to wait for data + + \returns the size of the data read from the socket, -1 on error. + \relates XsSocket +*/ +int XsSocket_readFrom(XsSocket* thisPtr, void* dest, XsSize size, XsString* hostname, uint16_t* port, int timeout) +{ + int rv; + int canRead; + struct sockaddr_storage sender; + socklen_t l = sizeof(sender); + + rv = XsSocket_select(thisPtr, timeout, &canRead, NULL); + (void) canRead; + if (rv <= 0) + return rv; + + if (!dest) + return peekPendingDataSize(thisPtr); + + rv = recvfrom(thisPtr->d->m_sd, dest, (int)size, 0, (struct sockaddr*)&sender, &l); + + if (hostname) + getRemoteHostAddress(&sender, hostname); + + if (port) + *port = ntohs(((struct sockaddr_in*)&sender)->sin_port); + + translateSocketError(thisPtr, rv); + return rv; +} + +/*! \brief Read \a size data into \a dest + + \param[in,out] dest a pointer to a data buffer XsByteArray, may be NULL, in which case only the size of the pending data is reported. + \param[in] timeout the amount of time in milliseconds to wait for data + + \returns the size of the data read from the socket, -1 on error. + \relates XsSocket +*/ +int XsSocket_read2ByteArray(XsSocket* thisPtr, XsByteArray* dest, int timeout) +{ + return XsSocket_readFrom2ByteArray(thisPtr, dest, NULL, NULL, timeout); +} + +/*! \brief Read \a size data into \a dest + + \param[in,out] dest a pointer to a data buffer XsByteArray, may be NULL, in which case only the size of the pending data is reported. + \param[in,out] hostname a pointer to a string that contains the hostname of the sender after returning from this function. May be NULL. + \param[in,out] port a pointer that will contain the port of the sender after returning from this function. May be NULL. + \param[in] timeout the amount of time in milliseconds to wait for data + + \returns the size of the data read from the socket, -1 on error. + \relates XsSocket +*/ +int XsSocket_readFrom2ByteArray(XsSocket* thisPtr, XsByteArray* dest, XsString* hostname, uint16_t* port, int timeout) +{ + int rv; + int canRead; + struct sockaddr_storage sender; + socklen_t l = sizeof(sender); + + rv = XsSocket_select(thisPtr, timeout, &canRead, NULL); + (void) canRead; + if (rv <= 0) + return rv; + + if (!dest) + return peekPendingDataSize(thisPtr); + + rv = recvfrom(thisPtr->d->m_sd, thisPtr->d->m_peekBuf, PEEKBUFSIZE, 0, (struct sockaddr*)&sender, &l); + if (rv <= 0) + { + translateSocketError(thisPtr, rv); + return -1; + } + XsByteArray_assign(dest, (XsSize)(ptrdiff_t) rv, thisPtr->d->m_peekBuf); + + if (hostname) + getRemoteHostAddress(&sender, hostname); + + if (port) + *port = ntohs(((struct sockaddr_in*)&sender)->sin_port); + + translateSocketError(thisPtr, 0); + return rv; +} + +/*! \brief Write data to the socket + + \param[in] data a pointer to the data to write + \param[in] size the size of the data to write + + \returns the number of bytes written, -1 or 0 on error + \relates XsSocket +*/ +int XsSocket_write(XsSocket* thisPtr, const void* data, XsSize size) +{ + int canWrite; + int rv = XsSocket_select(thisPtr, 0, NULL, &canWrite); + (void) canWrite; + if (rv <= 0) + return rv; + + return send(thisPtr->d->m_sd, data, (int)(ptrdiff_t)size, MSG_NOSIGNAL); +} + +/* Return non-zero if the hostname is actually an IPv4 address */ +int isIPv4Address(XsString const* hostname) +{ + char* c; + int expectNum = 1; + int expectDot = 0; + int numbersFound = 0; + + if (!hostname || !hostname->m_data) + return 0; + + for (c = hostname->m_data; *c != '\0'; c++) + { + if (*c == '.') + { + if (!expectDot) + return 0; + expectNum = 1; + numbersFound = 0; + expectDot = 0; + } + else if (isdigit(*c)) + { + if (!expectNum) + return 0; + expectDot = 1; + ++numbersFound; + if (numbersFound == 3) + expectNum = 0; + } + else + return 0; + } + return 1; +} + +/* Prefix the hostname with ::ffff: if we're on ipv6 and hostname looks like a ipv4 address */ +void XsSocket_fixupHostname(XsSocket const* thisPtr, XsString* hostname) +{ + if (!hostname || !hostname->m_data) + return; + + if (thisPtr->d->m_ipVersion == NLP_IPV4) + return; + + if (isIPv4Address(hostname)) + { + const char prefix[] = "::ffff:"; + XsArray_insert(hostname, 0, (XsSize)strlen(prefix), prefix); + } +} + +typedef int (*lookupTestFunction)(XsSocket* thisPtr, SOCKET currentSocket, struct addrinfo const* info); + +/* Do a lookup of the given hostname and port + + This is an internal function that centralizes the lookup code. + + thisPtr - the socket to use information from + hostname - the hostname to look up + port - the port to check the connection on + hints_flags - the flags we should put into the lookup hints, e.g. AI_PASSIVE + tester - a function that performs some sanity checks on the passed info + info - a pointer to a sockaddr buffer. Should be at least as big as sockaddr, preferably be sockaddr_storage + addrlen - the size of info. This function will change addrlen based on the actual size of info if necessary + + 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) +{ + struct addrinfo* lookupInfo, *p; + SOCKET s; + char gaport[7]; + struct addrinfo hints; + int ret; + memset(&hints, 0, sizeof(hints)); + + 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; + } + hints.ai_socktype = (thisPtr->d->m_ipProtocol == IP_UDP) ? SOCK_DGRAM : SOCK_STREAM; + hints.ai_flags = hints_flags; + + sprintf(gaport, "%u", (unsigned int) port); + if (hostname) + { + XsString host; + + XsString_construct(&host); + XsArray_copy(&host, hostname); + + XsSocket_fixupHostname(thisPtr, &host); + + ret = getaddrinfo(host.m_data, gaport, &hints, &lookupInfo); + + 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); + } + } + + setLastResult(thisPtr, XRV_NOTFOUND, -1); + for (p = lookupInfo; p != NULL; p = p->ai_next) + { + s = socket(p->ai_family, p->ai_socktype, p->ai_protocol); + if (!socketIsUsable(s)) + continue; + + ret = tester(thisPtr, s, p); + if (ret == 0) + { + (void)closesocket(s); + if (info && addrlen) + { + if ((socklen_t)p->ai_addrlen < *addrlen) + *addrlen = (socklen_t)p->ai_addrlen; + memcpy(info, p->ai_addr, (size_t)(ptrdiff_t) *addrlen); + } + break; + } + (void)closesocket(s); + } + freeaddrinfo(lookupInfo); + return setLastResult(thisPtr, (ret == 0) ? XRV_OK : thisPtr->d->m_lastResult, ret); +} + +/* try and see if we can connect on currentSocket to the remote host */ +static int defaultLookupTest(XsSocket* thisPtr, SOCKET currentSocket, struct addrinfo const* info) +{ + (void)thisPtr; + return connect(currentSocket, info->ai_addr, (int) info->ai_addrlen); +} + +/*! \brief Perform a lookup */ +static XsResultValue XsSocket_lookup(XsSocket* thisPtr, const XsString* hostname, uint16_t port, struct sockaddr* info, socklen_t* addrlen) +{ + return XsSocket_internalLookup(thisPtr, hostname, port, 0, defaultLookupTest, info, addrlen); +} + +/*! \brief Write data to the socket to the host indicated by \a hostname, \a port + + \param[in] data the data to write + \param[in] size the size of the data + \param[in] hostname the name of the host to send data to + \param[in] port the port to send data to + + \returns the number of bytes written, -1 on error. + \relates XsSocket +*/ +int XsSocket_writeTo(XsSocket* thisPtr, const void* data, XsSize size, const XsString* hostname, uint16_t port) +{ + struct sockaddr_storage storage; + struct sockaddr* addr = NULL; + socklen_t addrlen = 0; + int sent; + int canWrite; + int rv; + + rv = XsSocket_select(thisPtr, 0, NULL, &canWrite); + (void) canWrite; + if (rv <= 0) + return rv; + + if (thisPtr->d->m_ipProtocol == IP_UDP) + { + addr = (struct sockaddr*)&storage; + addrlen = sizeof(storage); + + if (XsSocket_lookup(thisPtr, hostname, port, addr, &addrlen) != XRV_OK) + return -1; + } + + sent = sendto(thisPtr->d->m_sd, data, (int)size, MSG_NOSIGNAL, addr, addrlen); + translateSocketError(thisPtr, sent); + return sent; +} + +/*! \brief Enable sending and receiving broadcasts on this socket + \details By default sockets do not receive broadcasts and can't send them. This function can enable this option. + \param enable Set to 1 to enable broadcasts, 0 to disable them again + \return 1 if successful, 0 if a failure occurred + \relates XsSocket +*/ +int XsSocket_enableBroadcasts(XsSocket* thisPtr, int enable) +{ + int broadcast = enable ? 1 : 0; + int res = setsockopt(thisPtr->d->m_sd, SOL_SOCKET, SO_BROADCAST, (char*)&broadcast, sizeof(broadcast)); + if (res != 0) + { + thisPtr->d->m_lastResult = translateAndReturnSocketError(thisPtr, res); + return 0; + } + thisPtr->d->m_lastResult = XRV_OK; + return 1; +} + +#ifdef USE_GETIFADDRS +static uint32_t SockAddrToUint32(struct sockaddr* a) +{ + return ((a) && (a->sa_family == AF_INET)) ? ntohl(((struct sockaddr_in*)a)->sin_addr.s_addr) : 0; +} +#endif + +/*! \brief Broadcast data over the socket to the port indicated by \a port + + \param[in] data the data to write + \param[in] size the size of the data + \param[in] port the port to send data to + + \returns the number of bytes written, -1 on error, depending on whether the majority of sub broadcasts succeeded or failed. + \relates XsSocket +*/ +int XsSocket_broadcast(XsSocket* thisPtr, const void* data, XsSize size, uint16_t port) +{ + struct sockaddr_in addr; + int sent = 0; + int failed = 0; + int ok = 0; + int canWrite; + int rv; + + rv = XsSocket_select(thisPtr, 0, NULL, &canWrite); + (void) canWrite; + if (rv <= 0) + return rv; + + if (thisPtr->d->m_ipProtocol == IP_UDP) + { + addr.sin_family = AF_INET; + addr.sin_port = htons(port); + addr.sin_addr.s_addr = INADDR_BROADCAST; // this is equiv to 255.255.255.255 and won't work! + +#ifdef _WIN32 + do + { + // Windows XP style implementation + MIB_IPADDRTABLE* ipTable = NULL; + { + ULONG bufLen = 0; + 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); + } + else if (ipRet == NO_ERROR) + break; + else + { + free(ipTable); + ipTable = NULL; + break; + } + } + } + + if (ipTable) + { + IP_ADAPTER_INFO* pAdapterInfo = NULL; + { + ULONG bufLen = 0; + 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); + } + else if (apRet == ERROR_SUCCESS) + break; + else + { + free(pAdapterInfo); + pAdapterInfo = NULL; + break; + } + } + } + + 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); + 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) + { + ++failed; + translateSocketError(thisPtr, sent); + } + else if (sent > 0) + ++ok; + } + + free(pAdapterInfo); + free(ipTable); + } + } while (0); +#elif defined(USE_GETIFADDRS) + // BSD-style implementation + struct ifaddrs* ifap; + if (getifaddrs(&ifap) == 0) + { + 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); + 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) + { + ++failed; + translateSocketError(thisPtr, sent); + } + else if (sent > 0) + ++ok; + } + p = p->ifa_next; + } + freeifaddrs(ifap); + } +#else + // just use direct local broadcast, this may or may not work depending on the system configuration + sent = sendto(thisPtr->d->m_sd, data, (int)size, MSG_NOSIGNAL, (struct sockaddr const*) &addr, sizeof(addr)); + if (sent < 0) + ++failed; + else + ++ok; +#endif + } + if (failed >= ok) + return -1; + translateSocketError(thisPtr, sent); + return sent; +} + +/*! \brief Flush the incoming data + + Flush all data from the incoming buffer. + + \relates XsSocket +*/ +void XsSocket_flush(XsSocket* thisPtr) +{ + char buf[512]; + + while (XsSocket_read(thisPtr, buf, sizeof(buf), 0) > 0) + { + /* nop */ + } +} + +/*! \brief Accept an incoming connection + + This function requires that XsSocket_listen() has already been called. + + The returned pointer should be freed using XsSocket_freeAcceptedSocket(). + + \param[in] mstimeout the timeout in milliseconds + + \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; + struct sockaddr_storage theirInfo; + socklen_t infoLength = sizeof(struct sockaddr_storage); + SOCKET sd; + + if (mstimeout >= 0) + { + int read; + int rv = XsSocket_select(thisPtr, mstimeout, &read, NULL); + (void) read; + if (rv == 0) + { + (void)setLastResult(thisPtr, XRV_TIMEOUTNODATA, 0); + return NULL; + } + else if (rv == -1) + { + translateSocketError(thisPtr, rv); + return NULL; + } + } + + sd = accept(thisPtr->d->m_sd, (struct sockaddr*)&theirInfo, &infoLength); + if (!socketIsUsable(sd)) + return NULL; + + ns = XsSocket_allocate(); + XsSocket_createFromNativeSocket(ns, sd, (struct sockaddr*)&theirInfo, infoLength, XSDF_Managed); + return ns; +} + +/*! \brief Dynamically allocate an XsSocket + + After calling this function, it is still required to call + XsSocket_create or the internal function XsSocket_createFromNativeSocket + + Use XsSocket_free to safely free the returned XsSocket. + + \returns a pointer to a newly allocated XsSocket, or NULL on error +*/ +XsSocket* XsSocket_allocate() +{ + return (XsSocket*)malloc(sizeof(XsSocket)); +} + +/*! \brief Free a socket returned from XsSocket_accept() or XsSocket_allocate() + + Use this function only on sockets returned from XsSocket_accept(). + + This function calls XsSocket_destroy() before freeing the memory. + + \see XsSocket_destroy + \relates XsSocket +*/ +void XsSocket_free(XsSocket* thisPtr) +{ + XsSocket_destroy(thisPtr); + free(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) +{ + int res; + int nativeOption; +#ifdef _WIN32 + const char* valPtr = (const char*)valuePtr; +#else + const void* valPtr = valuePtr; +#endif + + switch (option) + { + case XSO_ReuseAddress: + nativeOption = SO_REUSEADDR; + break; + +#if !defined(_WIN32) && !defined(__ANDROID__) + case XSO_ReusePort: + nativeOption = SO_REUSEPORT; + break; +#endif + + default: + return XRV_OK; + } + + res = setsockopt(thisPtr->d->m_sd, SOL_SOCKET, nativeOption, valPtr, valueSize); + return translateAndReturnSocketError(thisPtr, res); +} + + +/* test if we can bind to info + + keeps the bind alive after leaving the function +*/ +static int binder(XsSocket* thisPtr, SOCKET currentSocket, struct addrinfo const* info) +{ + int res; + int yesval = 1; +#ifdef _WIN32 + const char* yes = (const char*)&yesval; +#else + const void* yes = &yesval; +#endif + + (void)currentSocket; + + res = setsockopt(thisPtr->d->m_sd, SOL_SOCKET, SO_REUSEADDR, yes, sizeof(yesval)); + if (res) + { + translateSocketError(thisPtr, res); + return res; + } + + res = bind(thisPtr->d->m_sd, info->ai_addr, (int)info->ai_addrlen); + translateSocketError(thisPtr, res); + return res; +} + +/*! \brief Bind to the \a hostname and \a port combination + + \param[in] hostname the hostname to bind to, may be NULL, in which we will bind to any address + \param[in] port the port to bind to. Must be a value above 1023, unless we have elevated privileges + + \return an XsResultValue indicating the result of the operation, possibly pointing towards a cause + \relates XsSocket +*/ +XsResultValue XsSocket_bind(XsSocket* thisPtr, const XsString* hostname, uint16_t port) +{ + int rv; + struct sockaddr_storage s; + socklen_t addrlen; + + if (hostname) + return XsSocket_internalLookup(thisPtr, hostname, port, AI_PASSIVE, binder, NULL, NULL); + + memset(&s, 0, sizeof(s)); + + if (thisPtr->d->m_ipVersion == NLP_IPV4) + { + 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; + addrlen = sizeof(*sin); + } + else + { + struct sockaddr_in6* sin6 = (struct sockaddr_in6*)&s; + sin6->sin6_family = AF_INET6; + sin6->sin6_port = htons(port); + sin6->sin6_addr = in6addr_any; + addrlen = sizeof(*sin6); + } + + rv = bind(thisPtr->d->m_sd, (struct sockaddr*)&s, addrlen); + return translateAndReturnSocketError(thisPtr, rv); +} + +/*! \brief Start listening for incoming connections on this socket + + \param[in] maxPending the maximum amount of pending connections + + \return an XsResultValue indicating the result of the operation + \relates XsSocket +*/ +XsResultValue XsSocket_listen(XsSocket* thisPtr, int maxPending) +{ + int r = listen(thisPtr->d->m_sd, maxPending); + return translateAndReturnSocketError(thisPtr, r); +} + +/* Connect to info + + Keep the connection alive +*/ +static int connector(XsSocket* thisPtr, SOCKET currentSocket, struct addrinfo const* info) +{ + int ret; + (void)currentSocket; + + ret = connect(thisPtr->d->m_sd, info->ai_addr, (int)info->ai_addrlen); + + if (ret == 0) + { + thisPtr->d->m_ipVersion = (info->ai_family == AF_INET ? NLP_IPV4 : NLP_IPV6); + memcpy(&thisPtr->d->m_remoteAddr, info->ai_addr, info->ai_addrlen); + thisPtr->d->m_remoteAddrLen = (socklen_t)(info->ai_addrlen); + } + else + translateSocketError(thisPtr, ret); + + return ret; +} + +/*! \brief Connect to \a hostame on \a port + + \param[in] host the hostname to connect to + \param[in] port the port to connect to + + \returns an XsResultValue indicating the result of the operation + \relates XsSocket +*/ +XsResultValue XsSocket_connect(XsSocket* thisPtr, const XsString* host, uint16_t port) +{ + return XsSocket_internalLookup(thisPtr, host, port, 0, connector, NULL, NULL); +} + +/*! \brief Return non-zero if the socket is usable for reading and writing data + + \returns non-zero if the socket is usable for reading and writing data, zero otherwise + \relates XsSocket +*/ +int XsSocket_isUsable(const XsSocket* thisPtr) +{ + return socketIsUsable(thisPtr->d->m_sd); +} + +/*! + \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 +*/ +int XsSocket_getLastSystemError(const XsSocket* thisPtr) +{ + return thisPtr->d->m_lastSystemError; +} diff --git a/extern/xspublic/xstypes/xssocket.h b/extern/xspublic/xstypes/xssocket.h new file mode 100644 index 0000000..f6e6e0a --- /dev/null +++ b/extern/xspublic/xstypes/xssocket.h @@ -0,0 +1,476 @@ + +// Copyright (c) 2003-2022 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 XSSOCKET_H +#define XSSOCKET_H + +#include "xsstring.h" +#include "xsbytearray.h" +#include "xsresultvalue.h" +#include "xstypedefs.h" + +struct XsSocket; +struct XsSocketInterface; +typedef struct XsSocket XsSocket; + +/*! \brief the protocol on top of IP */ +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 +{ + NLP_IPV4, /*!< \brief IPv4 address family */ + NLP_IPV6, /*!< \brief IPv6 address family */ + NLP_IPVX /*!< \brief any IP address family */ +}; + +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 +#else + typedef int XSOCKET; +#endif + +#ifdef __cplusplus +extern "C" { +#endif +#ifndef __cplusplus +#define XSSOCKET_INITIALIZER { NULL } +#endif + +#define XSSOCKET_INFINITE_TIMEOUT -1 + +XSTYPES_DLL_API void XsSocket_create(XsSocket* thisPtr, enum NetworkLayerProtocol ip, enum IpProtocol protocol); +XSTYPES_DLL_API void XsSocket_createFromFileDescriptor(XsSocket* thisPtr, int sockfd, XsDataFlags flags); +XSTYPES_DLL_API void XsSocket_destroy(XsSocket* thisPtr); +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_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); +XSTYPES_DLL_API int XsSocket_readFrom2ByteArray(XsSocket* thisPtr, XsByteArray* dest, XsString* hostname, uint16_t* port, int timeout); + +XSTYPES_DLL_API int XsSocket_write(XsSocket* thisPtr, const void* buffer, XsSize size); +XSTYPES_DLL_API int XsSocket_writeTo(XsSocket* thisPtr, const void* buffer, XsSize size, const XsString* hostname, uint16_t port); + +XSTYPES_DLL_API void XsSocket_flush(XsSocket* thisPtr); + +XSTYPES_DLL_API XsSocket* XsSocket_accept(XsSocket* thisPtr, int mstimeout); + +XSTYPES_DLL_API XsSocket* XsSocket_allocate(); +XSTYPES_DLL_API void XsSocket_free(XsSocket* thisPtr); + +XSTYPES_DLL_API XsResultValue XsSocket_setSocketOption(XsSocket* thisPtr, enum XsSocketOption option, void* valuePtr, int valueSize); +XSTYPES_DLL_API XsResultValue XsSocket_bind(XsSocket* thisPtr, const XsString* hostname, uint16_t port); +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 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); + +#ifdef __cplusplus +} +#endif + +/*! \brief A platform independent socket implementation + + The internals are hidden to remove any inconvenience that + may occur because of the order of inclusion of system headers. +*/ +struct XsSocket +{ +#ifdef __cplusplus + /*! \brief \copybrief XsSocket_create + + \param[in] protocol the protocol on top of IP to use + \param[in] ipVersion the ip address family to use + */ + inline explicit XsSocket(IpProtocol protocol, NetworkLayerProtocol ipVersion = NLP_IPVX) + : d(NULL) + { + (void)d; + XsSocket_create(this, ipVersion, protocol); + } + + /*! \brief \copybrief XsSocket_createFromFileDescriptor + + \param[in] sockfd the file descriptor of the socket to wrap + \param[in] flags if set to XSDF_Managed the wrapped socket will be closed when object is destroyed + */ + inline explicit XsSocket(int sockfd, XsDataFlags flags = XSDF_None) + : d(NULL) + { + (void)d; + XsSocket_createFromFileDescriptor(this, sockfd, flags); + } + + /*! \brief \copybrief XsSocket_destroy + */ + inline ~XsSocket() + { + XsSocket_destroy(this); + } + + /*! \brief \copybrief XsSocket_close + */ + inline XsResultValue close() + { + return XsSocket_close(this); + } + + /*! \brief Return the native file descriptor */ + inline XSOCKET nativeDescriptor() const + { + return XsSocket_nativeDescriptor(this); + } + + /*! \brief read data from the socket into \a buffer + + \param[in,out] buffer the buffer to fill, contents are automatically adjusted to the data size available + \param[in] timeout the maximum time to wait before data is in in milliseconds + + \returns the number of bytes read from the socket + \see read(void*,XsSize,int) + \see XsSocket_read + */ + inline int read(XsByteArray& buffer, int timeout = 0) + { + return XsSocket_read2ByteArray(this, &buffer, timeout); + } + + /*! \brief \copybrief XsSocket_read + + \param[in,out] dest the destination buffer for the incoming data + \param[in] size the size of the destination buffer + \param[in] timeout the maximum time to wait before data is in in milliseconds + + \returns the number of bytes read from the socket + \see read(XsByteArray&,int) + \see XsSocket_read + */ + inline int read(void* dest, XsSize size, int timeout = 0) + { + return XsSocket_read(this, dest, size, timeout); + } + + /*! \brief read data from the socket into \a buffer + + \param[in,out] buffer the buffer to write into + \param[in,out] hostname a pointer to a string that contains the hostname of the sender after returning from this function. May be NULL + \param[in,out] port a pointer that will contain the port of the sender after returning from this function. May be NULL + \param[in] timeout the read timeout in milliseconds + + \returns the size of the data read from the socket, -1 on error + \see readFrom(void*,XsSize,XsString*,uint16_t*,int) + \see XsSocket_readFrom + */ + inline int readFrom(XsByteArray& buffer, XsString* hostname = NULL, uint16_t* port = NULL, int timeout = 0) + { + return XsSocket_readFrom2ByteArray(this, &buffer, hostname, port, timeout); + } + + /*! \brief \copybrief XsSocket_readFrom + + \param[in,out] dest the buffer to write into + \param[in] size the size of the destination buffer + \param[in,out] hostname a pointer to a string that contains the hostname of the sender after returning from this function. May be NULL + \param[in,out] port a pointer that will contain the port of the sender after returning from this function. May be NULL + \param[in] timeout the read timeout in milliseconds + + \returns the size of the data read from the socket, -1 on error + \see readFrom(XsByteArray,XsString*,uint16_t,int) + \see XsSocket_readFrom + */ + inline int readFrom(void* dest, XsSize size, XsString* hostname = NULL, uint16_t* port = NULL, int timeout = 0) + { + return XsSocket_readFrom(this, dest, size, hostname, port, timeout); + } + + /*! \brief Write the data in \a buffer to the socket + + \param[in] buffer the data to write + + \returns the number of bytes written + \see write(const void*,XsSize) + \see XsSocket_write + */ + int write(const XsByteArray& buffer) + { + return write(buffer.data(), buffer.size()); + } + + /*! \brief \copybrief XsSocket_write + + \param[in] data the data to write + \param[in] size the size of the data to write + + \returns the number of bytes written to the socket + \see write(const XsByteArray&) + \see XsSocket_write + */ + int write(const void* data, XsSize size) + { + return XsSocket_write(this, data, size); + } + + /*! \brief Write the data in \a buffer to the socket + + \param[in] buffer the data to write + \param[in] hostname the host to write data to + \param[in] port the port to write data to + + \returns the number of bytes written + \see writeTo(const void*,XsSize,XsString,uint16_t) + \see XsSocket_writeTo + */ + int writeTo(const XsByteArray& buffer, const XsString& hostname, uint16_t port) + { + return writeTo(buffer.data(), buffer.size(), hostname, port); + } + + /*! \brief \copybrief XsSocket_writeTo + + \param[in] data the data to write + \param[in] size the size of the data to write + \param[in] hostname the host to write data to + \param[in] port the port to write data to + + \returns the number of bytes written to the socket + \see writeTo(XsByteArray,XsString,uint16_t) + \see XsSocket_writeTo + */ + int writeTo(const void* data, XsSize size, const XsString& hostname, uint16_t port) + { + return XsSocket_writeTo(this, data, size, &hostname, port); + } + + /*! \brief \copybrief XsSocket_flush + */ + void flush() + { + XsSocket_flush(this); + } + + /*! \brief \copybrief XsSocket_select + \param[in] mstimeout timeout to wait for socket to be readable/writeable + \param[in,out] canRead if not null then will be set to non-zero to indicate that the socket has data to read. + \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) + { + return XsSocket_select(this, mstimeout, canRead, canWrite); + } + + /*! \brief \copybrief XsSocket_accept + + \param[in] mstimeout the timeout in milliseconds. Negative timeouts count as infinite; XSSOCKET_INFINITE_TIMEOUT (-1) by default + \returns a pointer to a newly created XsSocket, NULL on error. The pointer can be deleted. + + \see listen + \see XsSocket_accept + */ + XsSocket* accept(int mstimeout = XSSOCKET_INFINITE_TIMEOUT) + { + return XsSocket_accept(this, mstimeout); + } + + /*! \brief Allocate memory for a socket */ + inline void* operator new (size_t) + { + XsSocket* s = XsSocket_allocate(); +#ifndef XSENS_NO_EXCEPTIONS + if (!s) + throw std::bad_alloc(); +#endif + return s; + } + + /*! \brief De-allocate the socket */ + inline void operator delete (void* p) noexcept + { + XsSocket_free(reinterpret_cast(p)); + } + + /*! \brief \copybrief XsSocket_setSocketOption + \param[in] option The socket option to change + \param[in] valuePtr Points to the new value of the socket option + \param[in] valueSize The size of the value \a valuePtr points to + \returns XRV_OK on success, another XsResultValue otherwise + \see XsSocket_setSocketOption + */ + XsResultValue setSocketOption(XsSocketOption option, void* valuePtr, int valueSize) + { + return XsSocket_setSocketOption(this, option, valuePtr, valueSize); + } + + /*! \brief Sets an socket option of int type + \param[in] option The socket option to change + \param[in] value The new value of the socket option + \returns XRV_OK on success, another XsResultValue otherwise + \see XsSocket_setSocketOption + */ + XsResultValue setSocketOption(XsSocketOption option, int value) + { + return XsSocket_setSocketOption(this, option, (void*)&value, sizeof(int)); + } + + /*! \brief Bind to any interface on \a port + + \param[in] port the port to bind to + + \returns XRV_OK on success, another XsResultValue otherwise + \see bind(XsString, port) + \see XsSocket_bind + */ + XsResultValue bind(uint16_t port) + { + return XsSocket_bind(this, NULL, port); + } + + /*! \brief \copybrief XsSocket_bind + + \param[in] hostname the hostname to bind to + \param[in] port the port to bind to + + \returns XRV_OK on success, another XsResultValue otherwise + \see bind(uint16_t) + \see XsSocket_bind + */ + XsResultValue bind(const XsString& hostname, uint16_t port) + { + return XsSocket_bind(this, &hostname, port); + } + + /*! \brief \copybrief XsSocket_listen + + \param[in] maxPending the maximum pending connections + + \returns XRV_OK on success, another XsResultValue when an error occurs + \see XsSocket_listen + \see XsSocket_accept + */ + XsResultValue listen(int maxPending = 20) + { + return XsSocket_listen(this, maxPending); + } + + /*! \brief \copybrief XsSocket_connect + + \param[in] host the host to connect to + \param[in] port the port to connect to + + \returns XRV_OK on success, another XsResultValue when an error occurs + \see XsSocket_connect + */ + XsResultValue connect(const XsString& host, uint16_t port) + { + return XsSocket_connect(this, &host, port); + } + + + /*! \brief Return true if the socket is usable + + \returns true if the socket can be used for communication, false otherwise + \see XsSocket_isUsable + */ + bool isUsable() const + { + return XsSocket_isUsable(this) != 0; + } + + /*! \brief Get the IP address of the remote host this socket is connected to. + + Only works for connected TCP sockets + \returns the remote host IP address + */ + XsString getRemoteAddress() const + { + XsString address; + XsSocket_getRemoteAddress(this, &address); + return address; + } + + /*! \brief Return the system error reported by the last operation + */ + int getLastSystemError() const + { + return XsSocket_getLastSystemError(this); + } + + /*! \brief Broadcast data over the socket to the port indicated by \a port + + \param[in] data the data to write + \param[in] size the size of the data + \param[in] port the port to send data to + + \returns the number of bytes written, -1 on error, depending on whether the majority of sub broadcasts succeeded or failed. + */ + int broadcast(const void* data, XsSize size, uint16_t port) + { + return XsSocket_broadcast(this, data, size, port); + } + + /*! \brief Enable sending and receiving broadcasts on this socket + \details By default sockets do not receive broadcasts and can't send them. This function can enable this option. + \param enable Set to 1 to enable broadcasts, 0 to disable them again + \return 1 if successful, 0 if a failure occurred + */ + bool enableBroadcasts(bool enable) + { + return 0 != XsSocket_enableBroadcasts(this, enable ? 1 : 0); + } +private: +#endif + struct XsSocketPrivate* d; +}; + +#endif diff --git a/extern/xspublic/xstypes/xsstatusflag.h b/extern/xspublic/xstypes/xsstatusflag.h new file mode 100644 index 0000000..74a45aa --- /dev/null +++ b/extern/xspublic/xstypes/xsstatusflag.h @@ -0,0 +1,290 @@ + +// Copyright (c) 2003-2022 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 XSSTATUSFLAG_H +#define XSSTATUSFLAG_H + +#include "pstdint.h" + +/*! \addtogroup enums Global enumerations + @{ +*/ +/*! \brief Status flags + \details These flags define the function of specific bits in the status returned by + XsDataPacket::status() + \sa XsDataPacket::status() +*/ + +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 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 + \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 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 +}; + +/*! @} */ +typedef enum XsStatusFlag XsStatusFlag; +typedef enum XsStatusFlagOffset XsStatusFlagOffset; + +//! Return if any acc channel clipped +inline static bool anyAccClipped(int status) +{ + return 0 != (status & (XSF_ClipAccX | XSF_ClipAccY | XSF_ClipAccZ)); +} + +//! Return if any gyr channel clipped +inline static bool anyGyrClipped(int status) +{ + return 0 != (status & (XSF_ClipGyrX | XSF_ClipGyrY | XSF_ClipGyrZ)); +} + +//! Return if any mag channel clipped +inline static bool anyMagClipped(int status) +{ + return 0 != (status & (XSF_ClipMagX | XSF_ClipMagY | XSF_ClipMagZ)); +} + + +//! \brief Status object. +class XsStatus +{ +public: + /*! \brief Status object constructor + \param s XsStatus object to copy from + */ + inline XsStatus(const XsStatus& s) : m_status(s.m_status) + { + } + + /*! \brief Status object constructor + \param status status flags to set in the object + */ + inline XsStatus(int status) : m_status((uint32_t)status) + { + } + + //! \brief Status object constructor, clears all flags + inline XsStatus() : m_status(0) + { + } + + //! \brief Return statusflag of status object + inline XsStatusFlag get() const + { + return (XsStatusFlag) m_status; + } + + /*! \brief Set statusflag of status object + \param a The Status object to copy from + */ + inline void set(XsStatus const& a) + { + m_status = a.m_status; + } + + /*! \brief Set statusflag of status object + \param a The full status flags to set + */ + inline void set(uint32_t a) + { + m_status = a; + } + + /*! \brief Set statusflag of status object + \param a The full status flags to set + */ + inline void set(int a) + { + m_status = (uint32_t)(a & 0x1FFFF); + } + + /*! \brief Assignment operator + \param a The Status object to copy from + \return A reference to this object + */ + inline XsStatus const& operator = (XsStatus const& a) + { + m_status = a.m_status; + return *this; + } + + /*! \brief Assignment operator + \param a The full status flags to set + \return A reference to this object + */ + inline XsStatus const& operator = (int a) + { + m_status = (uint32_t)(a & 0x1FFFF); + return *this; + } + + /*! \brief Binary OR operator + \param a The status flags to set, leaving already set flags as they were + \return A reference to this object + */ + inline XsStatus const& operator |= (XsStatus const& a) + { + m_status |= a.m_status; + return *this; + } + + /*! \brief Binary OR operator + \param a The status flags to set, leaving already set flags as they were + \return A reference to this object + */ + inline XsStatus const& operator |= (int a) + { + m_status |= (uint32_t)(a & 0x1FFFF); + return *this; + } + + /*! \brief Binary AND operator + \param a The mask of flags to keep, clearing other flags + \return A reference to this object + */ + inline XsStatus const& operator &= (XsStatus const& a) + { + m_status &= a.m_status; + return *this; + } + + /*! \brief Binary AND operator + \param a The mask of flags to keep, clearing other flags + \return A reference to this object + */ + inline XsStatus const& operator &= (int a) + { + m_status &= (uint32_t)(a & 0x1FFFF); + return *this; + } + + //! Return if any acc channel clipped + inline bool anyAccClipped() const + { + return ::anyAccClipped((int)m_status); + } + + //! Return if any gyr channel clipped + inline bool anyGyrClipped() const + { + return ::anyGyrClipped((int)m_status); + } + + //! Return if any mag channel clipped + inline bool anyMagClipped() const + { + return ::anyMagClipped((int)m_status); + } + + //! Clear status object + inline void clear() + { + m_status = 0; + } + +protected: + uint32_t m_status; //!< Statusflag +}; + +#endif diff --git a/extern/xspublic/xstypes/xsstring.c b/extern/xspublic/xstypes/xsstring.c new file mode 100644 index 0000000..b389d7d --- /dev/null +++ b/extern/xspublic/xstypes/xsstring.c @@ -0,0 +1,651 @@ + +// Copyright (c) 2003-2022 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 "xsstring.h" +#include +#include // memcpy +#include + +#if defined(WIN32) + #include +#endif + +/*! \struct XsString + \brief A 0-terminated managed string of characters + \details This structure uses XsArray to manage its internal data. + The C++ interface reports the size of the string excluding the terminating 0, but since the C version + uses XsArray directly, its m_size member includes the terminating 0. + When using the C version, make sure that the 0 character is always preserved when manipulating the data. + \sa XsArray +*/ + +/*! \copydoc XsArrayDescriptor::itemSwap + \note Specialization for char*/ +void swapChar(char* a, char* b) +{ + char tmp = *a; + *a = *b; + *b = tmp; +} + +/*! \copydoc XsArrayDescriptor::itemCopy + \note Specialization for char*/ +void copyChar(char* to, char const* from) +{ + *to = *from; +} + +/*! \copydoc XsArrayDescriptor::itemCompare + \note Specialization for char*/ +int compareChar(void const* a, void const* b) +{ + if (*(char*)a < * (char*)b) + return -1; + if (*(char*)a > *(char*)b) + return 1; + return 0; +} + +//! \brief Descriptor for XsInt64Array +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); + XSEXPCASTITEMCOPY copyChar, // void (*itemCopyConstruct)(void* e, void const* s); + 0, // void (*itemDestruct)(void* e); + XSEXPCASTITEMCOPY copyChar, // void (*itemCopy)(void const* from, void* to); + XSEXPCASTITEMCOMP compareChar, // int (*itemCompare)(void const* a, void const* b); + XSEXPCASTRAWCOPY XsArray_rawCopy // void (*rawCopy)(void* to, void const* from, XsSize count, XsSize iSize) +}; + +/*! \brief Initializes the XsString object as an empty string + \details This function initializes the object as an empty string. +*/ +void XsString_construct(XsString* thisPtr) +{ + XsArray_construct(thisPtr, &g_xsStringDescriptor, 0, 0); +} + +/*! \copydoc XsArray_destruct + \note Specialization for XsString +*/ +void XsString_destruct(XsString* thisPtr) +{ + XsArray_destruct(thisPtr); +} + +/*! \copydoc XsArray_assign + \note Specialization for XsString +*/ +void XsString_assign(XsString* thisPtr, XsSize count, const char* src) +{ + if (!count && src) + count = (XsSize)strlen(src) + 1; + + if (src) + { + if (src[count - 1]) + { + XsArray_assign(thisPtr, count + 1, 0); + memcpy(thisPtr->m_data, src, count); + thisPtr->m_data[count] = 0; + } + else + XsArray_assign(thisPtr, count, src); + } + else + { + if (count) + { + XsArray_assign(thisPtr, count + 1, 0); + memset(thisPtr->m_data, ' ', count); + thisPtr->m_data[count] = 0; + } + else + XsArray_assign(thisPtr, 0, 0); + } +} + +/*! \brief This function determines the size of \a src and copies the contents to the object */ +void XsString_assignCharArray(XsString* thisPtr, const char* src) +{ + XsString_assign(thisPtr, 0, src); +} + +#ifndef XSENS_NO_WCHAR +/*! \brief This function determines the size of \a src and copies the contents to the object after + converting it from a unicode string to a multibyte character string. +*/ +void XsString_assignWCharArray(XsString* thisPtr, const wchar_t* src) +{ + if (src) + { +#ifdef WIN32 + 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; + if (reqv > thisPtr->m_reserved) + XsArray_reserve(thisPtr, reqv); + WideCharToMultiByte(CP_UTF8, 0, src, unicodeLength, thisPtr->m_data, required + 1, NULL, NULL); + thisPtr->m_data[required] = '\0'; + *((XsSize*) &thisPtr->m_size) = reqv; + return; + } +#else + 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; + return; + } +#endif + } + XsArray_assign(thisPtr, 0, 0); +} + +/*! \brief This function copies the contents of the object to a unicode wchar_t array +*/ +XsSize XsString_copyToWCharArray(const XsString* thisPtr, wchar_t* dest, XsSize size) +{ +#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); +#endif +} + +/*! \brief Append unicode character \a c to the string + \param c The character to append +*/ +void XsString_push_backWChar(XsString* thisPtr, wchar_t c) +{ + wchar_t buf[2] = { c, 0 }; + XsString tmp; + + XsString_construct(&tmp); + XsString_assignWCharArray(&tmp, buf); + XsString_append(thisPtr, &tmp); + XsString_destruct(&tmp); +} +#endif // XSENS_NO_WCHAR + +/*! \brief This function resizes the contained string to the desired size, while retaining its contents + \param count The desired size of the string. This excludes the terminating 0 character. + \sa XsArray_resize +*/ +void XsString_resize(XsString* thisPtr, XsSize count) +{ + XsSize sz = thisPtr->m_size; + XsArray_resize(thisPtr, count ? count + 1 : 0); + if (count) + { + for (; sz < count; ++sz) + thisPtr->m_data[sz] = ' '; + thisPtr->m_data[count] = 0; + } +} + +/*! \brief This function concatenates the \a other to this +*/ +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_append(thisPtr, other); + if (thisPtr == other) + { + // add terminating null again + static const char nullChar = 0; + XsArray_insert(thisPtr, (XsSize) - 1, 1, &nullChar); + } + } +} + +/*! \copydoc XsArray_erase + \note The function maintains the terminating 0 character +*/ +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); + else + XsArray_erase(thisPtr, 0, thisPtr->m_size); + } + else + XsArray_erase(thisPtr, index, count); +} + +/*! \brief Append character \a c to the string + \param c The character to append +*/ +void XsString_push_back(XsString* thisPtr, char c) +{ + XsSize sz = thisPtr->m_size; + if (!sz) + sz = 1; + XsString_resize(thisPtr, sz); + thisPtr->m_data[sz - 1] = c; +} + +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 + p += 4; + else + p += 3; + else + p += 2; + return p; +} + +/*! \brief Returns the number of characters in a UTF-8 encoded string + \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 count = 0; + uint8_t const* p = (uint8_t const*) thisPtr->m_data; + + if (!thisPtr || !thisPtr->m_data) + return 0; + + while (*p != 0) + { + ++count; + p = advanceUtf8(p); + } + return count; +} + +#ifndef XSENS_NO_WCHAR +int32_t shiftUtf8(int32_t t, uint8_t const* p, int bytes) +{ + int i; + for (i = 0; i < bytes; ++i) + t = (t << 6) | (p[i] & 0x3F); + return t; +} + +/*! \brief The decoded UTF-8 character at index \a index in the UTF-8 encoded string + \details http://en.wikipedia.org/wiki/Utf-8#Description + \param index The index of the character to return. + \returns the decoded UTF-8 character at index \a index in the UTF-8 encoded string +*/ +wchar_t XsString_utf8At(XsString const* thisPtr, XsSize index) +{ + int32_t t = 0; + uint8_t const* p = (uint8_t const*) thisPtr->m_data; + + if (!thisPtr || !thisPtr->m_data) + return 0; + + while (*p != 0 && index) + { + --index; + p = advanceUtf8(p); + } + + if (*p == 0) + return 0; + + // translate! + + 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 + t = shiftUtf8(p[0] & 0x07, p + 1, 3); + else + t = shiftUtf8(p[0] & 0x0F, p + 1, 2); + else + t = shiftUtf8(p[0] & 0x1F, p + 1, 1); + return (wchar_t) t; +} +#endif + +/*! \brief Returns whether this string ends with \a other + \param other The string to match with the end of this string + \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) +{ + const char* left; + const char* right; + + // we can never find a bigger string than our own string + if (thisPtr->m_size < other->m_size) + return 0; + + // we always match an empty string + if (other->m_size <= 1) + return 1; + + left = thisPtr->m_data + thisPtr->m_size - other->m_size; + right = other->m_data; + + if (caseSensitive) + for (; *left == *right && *right; ++left, ++right); + else + for (; tolower((unsigned char)*left) == tolower((unsigned char)*right) && *right; ++left, ++right); + + if (!*right) + return 1; + + return 0; +} + +/*! \brief Returns whether this string starts with \a other + \param other The string to match with the start of this string + \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) +{ + const char* left = thisPtr->m_data; + const char* right = other->m_data; + + // we can never find a bigger string than our own string + if (thisPtr->m_size < other->m_size) + return 0; + + // we always match an empty string + if (other->m_size <= 1) + return 1; + + if (caseSensitive) + for (; *left == *right && *right; ++left, ++right); + else + for (; tolower((unsigned char)*left) == tolower((unsigned char)*right) && *right; ++left, ++right); + + if (!*right) + return 1; + + return 0; +} + +/*! \brief Returns whether this string contains \a other + \param other The string to match with this string + \param caseSensitive Whether to compare case sensitive or not (case insensitive is the default) + \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) +{ + XsSize offsetI = 0; + if (!offset) + offset = &offsetI; + *offset = 0; + + // we always match an empty string + if (other->m_size <= 1) + return 1; + + // we can never find a bigger string than our own string + while (thisPtr->m_size - *offset >= other->m_size) + { + const char* left = thisPtr->m_data + *offset; + const char* right = other->m_data; + if (caseSensitive) + for (; *left == *right && *right; ++left, ++right); + else + for (; tolower((unsigned char)*left) == tolower((unsigned char)*right) && *right; ++left, ++right); + + if (!*right) + return 1; + + ++*offset; + } + *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) +{ + if (!thisPtr) + return 1; + if (!thisPtr->m_size || (thisPtr->m_flags & XSDF_Empty)) + return 1; + return !(thisPtr->m_size - 1); +} + +/*! \brief Sorts the string + \details This function sorts using qsort +*/ +void XsString_sort(XsString* thisPtr) +{ + if (thisPtr->m_size > 2) + qsort(thisPtr->m_data, thisPtr->m_size - 1, sizeof(char), compareChar); +} + +/*! \brief Reverses the contents of the string + \details This reverses the contents in-place + \note This does not take into account utf-8 encoded characters +*/ +void XsString_reverse(XsString* thisPtr) +{ + int i, half; + char* data, tmp, *right; + if (thisPtr->m_size > 2) + { + half = (int)((thisPtr->m_size - 1) >> 1); + data = (char*) thisPtr->m_data; + right = data + thisPtr->m_size - 2; + for (i = 0; i < half; ++i) + { + tmp = data[i]; + data[i] = right[-i]; + right[-i] = tmp; + } + } +} + +/*! \brief Find the first occurrence of \a needle in the string + \param needle The string to find + \return The offset of \a needle or -1 if it was not found +*/ +ptrdiff_t XsString_findSubStr(XsString const* thisPtr, XsString const* needle) +{ + XsSize offset, i, end, endN; + if (!thisPtr) // no string to search in + return -1; + if (!needle || needle->m_size <= 1) // empty string matches start of string, even if searchee is empty + return 0; + if (thisPtr->m_size <= 1 || thisPtr->m_size < needle->m_size) + return -1; + + end = thisPtr->m_size - needle->m_size; + endN = needle->m_size - 1; + + for (offset = 0; offset <= end; ++offset) + { + for (i = 0; i < endN; ++i) + if (thisPtr->m_data[offset + i] != needle->m_data[i]) + break; + if (i == endN) + return (ptrdiff_t) offset; // found! + } + // not found + return -1; +} + +/*! \brief Copy a substring of the \a source string + \details The function copies up to \a count characters from \a source to the string, starting at offset \a start + \param source The source to copy from + \param start The offset of the first character to copy + \param count The maximum number of characters to copy +*/ +void XsString_mid(XsString* thisPtr, XsString const* source, XsSize start, XsSize count) +{ + if (!thisPtr || !source) + return; + if (start >= source->m_size) + { + XsString_assign(thisPtr, 0, 0); + return; + } + if (start + count >= source->m_size) + count = (source->m_size - 1) - start; + XsString_assign(thisPtr, count, count ? source->m_data + start : 0); +} + +/*! \brief Replace all occurrences of \a src with \a dst, modifying thisPtr + \param src Substring to search for + \param dst Substring to use as replacement +*/ +void XsString_replaceAll(XsString* thisPtr, XsString const* src, XsString const* dst) +{ + XsSize offset, i, end, endN, start; + XsString rv = XSSTRING_INITIALIZER; + XsString sub = XSSTRING_INITIALIZER; + if (XsString_empty(thisPtr) || XsString_empty(src)) + return; + XsString_construct(&rv); + XsString_construct(&sub); + + end = thisPtr->m_size - src->m_size; + endN = src->m_size - 1; + start = 0; + + for (offset = 0; offset <= end;) + { + for (i = 0; i < endN; ++i) + if (thisPtr->m_data[offset + i] != src->m_data[i]) + break; + if (i == endN) + { + if (start < offset) + { + XsString_assign(&sub, offset - start, &thisPtr->m_data[start]); + XsString_append(&rv, &sub); + } + if (dst) + XsString_append(&rv, dst); + offset += endN; + start = offset; + } + else + ++offset; + } + if (start < thisPtr->m_size) + { + XsString_assign(&sub, thisPtr->m_size - start, &thisPtr->m_data[start]); + XsString_append(&rv, &sub); + } + XsString_swap(thisPtr, &rv); + 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 (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); + } +} + +/*! \brief Make the string all lower-case +*/ +void XsString_toLower(XsString* thisPtr) +{ + XsSize i; + if (XsString_empty(thisPtr)) + return; + + for (i = 0; i < thisPtr->m_size-1; ++i) + thisPtr->m_data[i] = (char) tolower(thisPtr->m_data[i]); +} + +/*! \brief Make the string all upper-case +*/ +void XsString_toUpper(XsString* thisPtr) +{ + XsSize i; + if (XsString_empty(thisPtr)) + return; + + for (i = 0; i < thisPtr->m_size - 1; ++i) + thisPtr->m_data[i] = (char)toupper(thisPtr->m_data[i]); +} diff --git a/extern/xspublic/xstypes/xsstring.h b/extern/xspublic/xstypes/xsstring.h new file mode 100644 index 0000000..c5d04ff --- /dev/null +++ b/extern/xspublic/xstypes/xsstring.h @@ -0,0 +1,658 @@ + +// Copyright (c) 2003-2022 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 XSSTRING_H +#define XSSTRING_H + +#include "xstypesconfig.h" +#include "xsarray.h" +#ifndef XSENS_NO_WCHAR + #include +#endif // XSENS_NO_WCHAR +#include + +#ifdef __cplusplus +#include +#include +#include +extern "C" { +#endif + +extern XsArrayDescriptor const XSTYPES_DLL_API g_xsStringDescriptor; + +#ifndef __cplusplus +#define XsString_INITIALIZER XSARRAY_INITIALIZER(&g_xsStringDescriptor) +XSARRAY_STRUCT(XsString, char); +typedef struct XsString XsString; +// obsolete: +#define XSSTRING_INITIALIZER XsString_INITIALIZER +#else +struct XsString; +#endif + +XSTYPES_DLL_API void XsString_construct(XsString* thisPtr); +XSTYPES_DLL_API void XsString_destruct(XsString* thisPtr); +XSTYPES_DLL_API void XsString_assign(XsString* thisPtr, XsSize count, const char* src); +XSTYPES_DLL_API void XsString_assignCharArray(XsString* thisPtr, const char* src); +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 void XsString_sort(XsString* thisPtr); +XSTYPES_DLL_API void XsString_reverse(XsString* thisPtr); +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); +XSTYPES_DLL_API void XsString_toLower(XsString* thisPtr); +XSTYPES_DLL_API void XsString_toUpper(XsString* thisPtr); + +#ifndef XSENS_NO_WCHAR +XSTYPES_DLL_API XsSize XsString_copyToWCharArray(const XsString* thisPtr, wchar_t* dest, XsSize size); +XSTYPES_DLL_API wchar_t XsString_utf8At(const XsString* thisPtr, XsSize index); +XSTYPES_DLL_API void XsString_assignWCharArray(XsString* thisPtr, const wchar_t* src); +XSTYPES_DLL_API void XsString_push_backWChar(XsString* thisPtr, wchar_t c); +#endif // XSENS_NO_WCHAR + +#ifndef __cplusplus +// obsolete: +#define XsString_copy(thisPtr, copy) XsArray_copy(copy, thisPtr) +#define XsString_swap(a, b) XsArray_swap(a, b) +#endif +#ifdef __cplusplus +} // extern "C" +#endif + +#ifdef __cplusplus +/* 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; + +/*! \brief Returns the number of items currently in the array, excluding the terminating 0 + \details This function returns the number of bytes in the underlying char array. This doesn't mean it returns the number of characters as XsString assumes the char* is a utf-8 encoded string. + \returns The number of items currently in the array + \sa reserved \sa setSize \sa resize \sa utf8Len +*/ +template<> inline XsSize XsStringType::size() const noexcept +{ + return m_size ? m_size - 1 : 0; +} + +//! \copydoc XsArray_reserve +template<> inline void XsStringType::reserve(XsSize count) +{ + if (count) + XsArray_reserve(this, count + 1); + else + XsArray_reserve(this, 0); +} + +//! \brief Returns the reserved space in number of items +template<> inline XsSize XsStringType::reserved() const +{ + 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. */ +template<> inline void XsStringType::erase(XsSize index, XsSize count) +{ + XsString_erase((XsString*) this, index, count); +} + +/*! \brief Insert \a count \a items at \a index in the string + \param items The items to insert, may not be 0 unless count is 0 + \param index The index to use for inserting. Anything beyond the end of the string (ie. -1) will + append to the actual end of the string. + \param count The number of items to insert +*/ +template<> inline void XsArrayImpl::insert(char const* items, XsSize index, XsSize count) +{ + if (size()) + { + if (index >= size()) + index = size(); + XsArray_insert(this, index, count, items); + } + else + XsString_assign((XsString*) this, count, items); +} + +/*! \brief Assign the characters in \a src to the string + \details This function is resizes the string to fit \a count characters and copies those from \a src. + \param count The number of characters in src. If this value is 0 and \a scr is not 0, the value is + determined automatically by looking through src until a terminating 0 is found. + \param src The source string to copy from. If this is 0 and \a count is not 0, the string will be + filled with spaces instead. + \sa XsString_assign +*/ +template<> inline void XsArrayImpl::assign(XsSize count, char const* src) +{ + XsString_assign((XsString*) this, count, src); +} + +/*! \brief This function resizes the contained string to the desired size, while retaining its contents + \param count The desired size of the string. This excludes the terminating 0 character. + \sa XsString_resize +*/ +template<> inline void XsArrayImpl::resize(XsSize count) +{ + XsString_resize((XsString*) this, count); +} + +/*! \brief Set the size of the array to \a count. + \details The contents of the array after this operation are undefined. + \param count The desired new size fo the array. + \sa XsArray_assign \sa reserve \sa resize +*/ +template<> inline void XsArrayImpl::setSize(XsSize count) +{ + if (count != size()) + XsString_assign((XsString*) this, count, 0); +} + +/*! \copydoc XsArray_append \sa XsArray_append */ +template<> inline void XsArrayImpl::append(const XsStringType& other) +{ + XsString_append((XsString*) this, (XsString const*) &other); +} + +struct XsString : public XsStringType +{ + //! \brief Constructs an XsString + inline explicit XsString(XsSize sz = 0, char const* src = 0) + : XsStringType() + { + if (sz || src) + XsString_assign(this, sz, src); + } + +#ifndef SWIG + //! \brief Constructs an XsString as a copy of \a other + inline XsString(const XsStringType& other) + : XsStringType(other) + { + } +#else + inline XsString(const XsString& other) + : XsStringType(other) + { + } +#endif + + //! \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) + { + } +#ifndef XSENS_NOITERATOR + //! \brief Constructs an XsString with the list bound by the supplied iterators \a beginIt and \a endIt + template + inline explicit XsString(Iterator const& beginIt, Iterator const& endIt) + : XsStringType(beginIt, endIt) + { + } +#endif + + //! \brief Construct an XsString from a 0-terminated character array + inline XsString(char const* src) + : XsStringType() + { + if (src && src[0]) + XsString_assignCharArray(this, src); + } + +#ifndef XSENS_NO_WCHAR + //! \brief Construct an XsString from a 0-terminated character array + inline XsString(wchar_t const* src) + : XsStringType() + { + if (src && src[0]) + XsString_assignWCharArray(this, src); + } +#endif + +#ifndef XSENS_NO_STL + //! \brief Construct an XsString from a std::string + inline XsString(std::string const& src) + : XsStringType() + { + if (!src.empty()) + XsString_assign(this, (XsSize)src.size() + 1, src.c_str()); + } + + //! \brief Construct an XsString from a std::wstring + inline XsString(std::wstring const& src) + : XsStringType() + { + if (!src.empty()) + XsString_assignWCharArray(this, src.c_str()); + } +#endif // XSENS_NO_STL + + //! \brief Return the internal 0-terminated C-style string + inline char* c_str() + { + static const char nullChar = 0; + if (empty()) + return const_cast(&nullChar); +#ifdef XSENS_NO_EXCEPTION + try + { + return begin().operator ->(); + } + catch (...) + { + return const_cast(&nullChar); + } +#else + return begin().operator->(); +#endif + } + + //! \brief Return the internal 0-terminated C-style string + inline char const* c_str() const + { + static const char nullChar = 0; + if (empty()) + return &nullChar; +#ifdef XSENS_NO_EXCEPTION + try + { + return begin().operator ->(); + } + catch (...) + { + return &nullChar; + } +#else + return begin().operator->(); +#endif + } + +#ifndef XSENS_NO_STL + //! \brief Return the string as a std::string + inline std::string toStdString() const + { + if (empty()) + return std::string(); + return std::string((char const*)begin().operator ->()); + } +#endif // XSENS_NO_STL + + //! \brief Return \a other appended to this, without modifying this + XsString operator + (XsString const& other) const + { + XsString tmp; + tmp.reserve(size() + other.size()); + tmp.append(*this); + tmp.append(other); + return tmp; + } + +#ifndef XSENS_NO_STL + //! \brief Return the string as a std::wstring + std::wstring toStdWString() const + { + if (empty()) + return std::wstring(); + size_t s = XsString_copyToWCharArray(this, NULL, 0); + std::wstring w; + w.resize(s - 1); + (void) XsString_copyToWCharArray(this, &w[0], (XsSize)s); + return w; + } +#endif // XSENS_NO_STL + + /*! \cond NODOXYGEN */ + using XsStringType::operator ==; + using XsStringType::operator !=; +#ifndef XSENS_NOITERATOR + using XsStringType::operator <<; +#endif + /*! \endcond */ + + //! \brief Return true if the contents of \a str are identical to this string + inline bool operator == (char const* str) const + { + 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) + { + if (str && str[0]) + { + XsString const ref(const_cast(str), (XsSize)strlen(str), XSDF_None); + append(ref); + } + return *this; + } + + //! \brief Append an integer to the string in a stream-like way + inline XsString& operator << (int i) + { + char buffer[32]; + XSENS_MSC_WARNING_SUPPRESS(4996) + append(XsString(buffer, (XsSize)(ptrdiff_t) std::sprintf(buffer, "%d", i), XSDF_None)); + return *this; + } + + //! \brief Append another string to the string in a stream-like way + inline XsString& operator << (XsString const& s) + { + append(s); + return *this; + } + + //! \brief Return true if the contents if \a str are greater then this string + inline bool operator < (const XsString& str) const + { +#ifdef XSENS_NO_STL + return (strcmp(c_str(), str.c_str()) < 0); +#else + return (this->toStdString() < str.toStdString()); +#endif + } + + //! \brief Return true if the contents if \a str are less then this string + inline bool operator > (const XsString& str) const + { +#ifdef XSENS_NO_STL + return (strcmp(c_str(), str.c_str()) > 0); +#else + return (this->toStdString() > str.toStdString()); +#endif + } + +#ifndef SWIG + /*! \brief Swap the contents the \a first and \a second array */ + friend inline void swap(XsString& first, XsString& second) + { + first.swap(second); + } +#endif + + //! \brief Append a character to the string + inline XsString& push_back(char c) + { + XsString_push_back(this, c); + return *this; + } + + //! \brief Return true when the string is empty + inline bool empty() const + { + return !!XsString_empty(this); + } + +#ifndef SWIG + using XsStringType::append; +#else + void append(XsString const& other) + { + XsStringType::append(other); + } +#endif + + //! \brief Append a char array to the string + inline void append(char const* other) + { + if (other) + { + XsString tmp(other); + XsString_append(this, &tmp); + } + } + +#ifndef XSENS_NO_WCHAR + //! \brief Append a wchar array to the string + inline void append(wchar_t const* other) + { + if (other) + { + XsString tmp(other); + XsString_append(this, &tmp); + } + } +#endif + + //! \returns the number of characters in a utf-8 encoded string + inline XsSize utf8Len() const + { + return XsString_utf8Len(this); + } + + /*! \brief Returns whether this string ends with \a other (case-insensitive!) + \param other The string to match with the end of this string + \param caseSensitive Whether to compare case sensitive or not (case insensitive is the default) + \return true when the string ends with the given string + */ + inline bool endsWith(XsString const& other, bool caseSensitive = false) const + { + return 0 != XsString_endsWith(this, &other, caseSensitive ? 1 : 0); + } + + /*! \brief Returns whether this string starts with \a other (case-insensitive!) + \param other The string to match with the start of this string + \param caseSensitive Whether to compare case sensitive or not (case insensitive is the default) + \return true when the string starts with the given string + */ + inline bool startsWith(XsString const& other, bool caseSensitive = false) const + { + return 0 != XsString_startsWith(this, &other, caseSensitive ? 1 : 0); + } + + /*! \brief Returns whether this string contains \a other (case-insensitive!) + \param other The string to match with this string + \param caseSensitive Whether to compare case sensitive or not (case insensitive is the default) + \param offset When not null, this will be filled with the first index where \a other was found + \return true when the string contains the given string + */ + inline bool contains(XsString const& other, bool caseSensitive = false, XsSize* offset = 0) const + { + return 0 != XsString_contains(this, &other, caseSensitive ? 1 : 0, offset); + } + +#ifndef XSENS_NO_WCHAR + /*! \brief The decoded UTF-8 character at index \a index in the UTF-8 encoded string + \details http://en.wikipedia.org/wiki/Utf-8#Description + \param index The index of the character to return. + \returns the decoded UTF-8 character at index \a index in the UTF-8 encoded string + */ + inline wchar_t utf8At(XsSize index) const + { + return XsString_utf8At(this, index); + } + + //! \brief Append a unicode character to the string + inline XsString& push_back(wchar_t c) + { + XsString_push_backWChar(this, c); + return *this; + } +#endif + + //! \brief Sort the characters in the string + inline void sort() + { + XsString_sort(this); + } + + /*! \brief Reverse the characters in the string + \note This does not take into account utf-8 encoded characters + */ + inline void reverse() + { + XsString_reverse(this); + } + + /*! \brief Find the first occurrence of \a needle in the string + \param needle The string to find + \return The offset of \a needle or -1 if it was not found + */ + inline ptrdiff_t findSubStr(XsString const& needle) const + { + return XsString_findSubStr(this, &needle); + } + + /*! \brief Return a substring of the string + \details The function returns a copy of up to \a count characters from the string, starting at offset \a start + \param start The offset of the first character to copy + \param count The maximum number of characters to copy + \return The requested substring + */ + inline XsString mid(XsSize start, XsSize count) const + { + XsString rv; + XsString_mid(&rv, this, start, count); + return rv; + } + + /*! \brief Replace all occurrences of \a src with \a dst, modifying this + \param src Substring to search for + \param dst Substring to use as replacement + */ + inline void replaceAll(XsString const& src, XsString const& dst) + { + XsString_replaceAll(this, &src, &dst); + } + + /*! \brief Replace all occurrences of \a src with \a dst, returning the modified result without modifying this + \param src Substring to search for + \param dst Substring to use as replacement + */ + 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; + } + + /*! \brief Make the string all lower-case, modifying this + */ + inline void toLower() + { + XsString_toLower(this); + } + + /*! \brief Make the string all upper-case, modifying this + */ + inline void toUpper() + { + XsString_toUpper(this); + } + + /*! \brief Return an all lower-case version of this, does not modify this + */ + inline XsString lowered() const + { + XsString tmp = *this; + XsString_toLower(&tmp); + return tmp; + } + + /*! \brief Return an all upper-case version of this, does not modify this + */ + inline XsString uppered() const + { + XsString tmp = *this; + XsString_toUpper(&tmp); + return tmp; + } +}; + +#ifndef XSENS_NO_STL +namespace std +{ +/*! \brief Stream output operator for XsString */ +template +basic_ostream<_CharT, _Traits>& operator<<(basic_ostream<_CharT, _Traits>& o, XsString const& xs) +{ + return (o << xs.toStdString()); +} +} +#endif + +#endif +#endif diff --git a/extern/xspublic/xstypes/xsstringarray.c b/extern/xspublic/xstypes/xsstringarray.c new file mode 100644 index 0000000..049dfaf --- /dev/null +++ b/extern/xspublic/xstypes/xsstringarray.c @@ -0,0 +1,139 @@ + +// Copyright (c) 2003-2022 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 "xsstringarray.h" +#include "xsstring.h" + +/*! \struct XsStringArray + \brief A list of XsString values + \sa XsArray +*/ + +//! \brief Descriptor for XsStringArray +XsArrayDescriptor const g_xsStringArrayDescriptor = +{ + sizeof(XsString), + XSEXPCASTITEMSWAP XsArray_swap, + XSEXPCASTITEMMAKE XsString_construct, + XSEXPCASTITEMCOPY XsArray_copyConstruct, + XSEXPCASTITEMMAKE XsArray_destruct, + XSEXPCASTITEMCOPY XsArray_copy, + XSEXPCASTITEMCOMP XsArray_compare, + 0 +}; + +/*! \copydoc XsArray_constructDerived + \note Specialization for XsStringArray +*/ +void XsStringArray_construct(XsStringArray* thisPtr, XsSize count, XsString const* src) +{ + XsArray_construct(thisPtr, &g_xsStringArrayDescriptor, count, src); +} + +/*! \relates XsStringArray + \brief Splice the supplied string and put the resulting substrings in the string array + \details The source string will be searched for instances of characters in \a separators and spliced whereever + one was found. The spliced list will not include characters in \a separators and empty substrings will be + discarded. + \param src The source string to splice + \param separators A list of separator characters that will be used to splice \a src. +*/ +void XsStringArray_fromSplicedString(struct XsStringArray* thisPtr, struct XsString const* src, struct XsString const* separators) +{ + XsString s; + XsString_construct(&s); + XsArray_destruct(thisPtr); + if (src->m_size > 0) + { + // check against 1 because an empty string can either be size 0 or size 1 (just the null-terminator) + if (separators->m_size <= 1) + { + // no separator + XsArray_insert(thisPtr, 0, 1, src); + } + else + { + char const* sep = (char const*) separators->m_data; + char const* idx = (char const*) src->m_data; + char const* newIdx = strpbrk(idx, sep); + while (newIdx && *idx) + { + if (newIdx != idx) + { + XsString_assign(&s, (XsSize)(newIdx - idx), idx); + XsArray_insert(thisPtr, thisPtr->m_size, 1, &s); + } + idx = newIdx + 1; + newIdx = strpbrk(idx, sep); + } + if (*idx) + { + XsString_assignCharArray(&s, idx); + XsArray_insert(thisPtr, thisPtr->m_size, 1, &s); + } + } + } + XsString_destruct(&s); +} + +/*! \relates XsStringArray + \brief Join the string array into a single string, inserting \a separator between substrings. + \param result The result of the join + \param separator The separator to insert between successive items +*/ +void XsStringArray_join(struct XsStringArray const* thisPtr, struct XsString* result, struct XsString const* separator) +{ + // determine required buffer size + XsSize i; + 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); + } + + XsArray_destruct(result); + if (chars) + { + XsArray_reserve(result, chars + 1); + for (i = 0; i < thisPtr->m_size; ++i) + { + const XsString* s = (const XsString*)XsArray_at(thisPtr, i); + if (s->m_size > 1) + { + if (result->m_size > 1) + XsString_append(result, separator); + XsString_append(result, s); + } + } + } +} diff --git a/extern/xspublic/xstypes/xsstringarray.h b/extern/xspublic/xstypes/xsstringarray.h new file mode 100644 index 0000000..f92745c --- /dev/null +++ b/extern/xspublic/xstypes/xsstringarray.h @@ -0,0 +1,185 @@ + +// Copyright (c) 2003-2022 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 XSSTRINGARRAY_H +#define XSSTRINGARRAY_H + +#include "xsarray.h" + +#ifdef __cplusplus +#include "xsstring.h" +extern "C" { +#endif + +extern XsArrayDescriptor const XSTYPES_DLL_API g_xsStringArrayDescriptor; + +#ifndef __cplusplus +#define XSSTRINGARRAY_INITIALIZER XSARRAY_INITIALIZER(&g_xsStringArrayDescriptor) +struct XsString; + +XSARRAY_STRUCT(XsStringArray, struct XsString); +typedef struct XsStringArray XsStringArray; + +XSTYPES_DLL_API void XsStringArray_construct(XsStringArray* thisPtr, XsSize count, struct XsString const* src); +#define XsStringArray_destruct(thisPtr) XsArray_destruct(thisPtr) +#endif +XSTYPES_DLL_API void XsStringArray_fromSplicedString(struct XsStringArray* thisPtr, struct XsString const* src, struct XsString const* separators); +XSTYPES_DLL_API void XsStringArray_join(struct XsStringArray const* thisPtr, struct XsString* result, struct XsString const* separator); + +#ifdef __cplusplus +} // extern "C" + +struct XsStringArray : public XsArrayImpl +{ + //! \brief Constructs an XsStringArray + inline explicit XsStringArray(XsSize sz = 0, XsString const* src = 0) + : ArrayImpl(sz, src) + { + } + + //! \brief Constructs an XsStringArray as a copy of \a other + inline XsStringArray(XsStringArray const& other) + : ArrayImpl(other) + { + } + + //! \brief Constructs an XsStringArray that references the data supplied in \a ref + inline explicit XsStringArray(XsString* 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(XsStringArray& first, XsStringArray& second) + { + first.swap(second); + } +#endif + +#ifndef XSENS_NOITERATOR + //! \brief Constructs an XsStringArray with the array bound by the supplied iterators \a beginIt and \a endIt + template + inline XsStringArray(Iterator beginIt, Iterator endIt) + : ArrayImpl(beginIt, endIt) + { + } +#endif + + /*! \brief Join the non-empty strings contained in the XsStringArray into one XsString, separating each item with \a separator + \param separator An optional separator string to put between substrings + \return The joined string + */ + XsString join(XsString const& separator) const + { + XsString tmp; + XsStringArray_join(this, &tmp, &separator); + return tmp; + } + + /*! \copydoc XsStringArray_fromSplicedString */ + void fromSplicedString(XsString const& src, XsString const& separators) + { + XsStringArray_fromSplicedString(this, &src, &separators); + } + + //! \brief Constructs an XsStringArray from an XsString \a src that is split up at each \a separator + inline explicit XsStringArray(XsString const& src, XsString const& separators) + : ArrayImpl() + { + XsStringArray_fromSplicedString(this, &src, &separators); + } + + /*! \brief find same string in array which can be case insensitive or not + \param needle String to be found + \param isCaseSensitive Optional, default is true + \return place of needle in the array or -1 if the string was not found + */ + ptrdiff_t find(XsString const& needle, bool isCaseSensitive = true) const + { + if (isCaseSensitive) + return XsArrayImpl::find(needle); + + if (size() == 0) + return -1; + + XsString needleLower(needle); + for (XsString::iterator c = needleLower.begin(); c != needleLower.end(); ++c) + *c = tolower(*c); + + for (XsSize i = 0; i < size(); ++i) // loop over all elements of the lists + { + XsString const& a = at(i); + if (a.size() != needleLower.size()) + continue; + + bool found = true; + for (XsSize j = 0; j < a.size(); ++j) + { + if (tolower(a[j]) != needleLower[j]) + { + found = false; + break; + } + } + + if (found) + return (ptrdiff_t)i; + } + return -1; + } + + /*! \brief find the first item where startsWith(needle) returns true will be returned + \param needle String to be found + \param isCaseSensitive Optional, default is true + \return place of needle in the array or -1 if the string was not found + */ + ptrdiff_t findPrefix(XsString const& needle, bool isCaseSensitive = true) const + { + if (size() == 0) + return -1; + + for (XsSize i = 0; i < size(); ++i) // loop over all elements of the lists + { + XsString const& a = at(i); + if (a.size() < needle.size()) + continue; + + if (a.startsWith(needle, isCaseSensitive)) + return (ptrdiff_t)i; + } + return -1; + } +}; +#endif + +#endif diff --git a/extern/xspublic/xstypes/xsstringoutputtype.h b/extern/xspublic/xstypes/xsstringoutputtype.h new file mode 100644 index 0000000..a44d9e7 --- /dev/null +++ b/extern/xspublic/xstypes/xsstringoutputtype.h @@ -0,0 +1,62 @@ + +// Copyright (c) 2003-2022 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 XSSTRINGOUTPUTTYPE_H +#define XSSTRINGOUTPUTTYPE_H + +/*! \addtogroup enums Global enumerations + @{ +*/ +//! 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 +}; +/*! @} */ +typedef enum XsStringOutputType XsStringOutputType; + +#endif diff --git a/extern/xspublic/xstypes/xsstringoutputtypearray.c b/extern/xspublic/xstypes/xsstringoutputtypearray.c new file mode 100644 index 0000000..bfb5bdb --- /dev/null +++ b/extern/xspublic/xstypes/xsstringoutputtypearray.c @@ -0,0 +1,91 @@ + +// Copyright (c) 2003-2022 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 "xsstringoutputtypearray.h" +#include +#include + +/*! \struct XsStringOutputTypeArray + \brief A list of XsStringOutputType values + \sa XsArray +*/ + +/*! \copydoc XsArrayDescriptor::itemSwap + \note Specialization for XsStringOutputType +*/ +void swapStringOutputType(XsStringOutputType* a, XsStringOutputType* b) +{ + XsStringOutputType tmp = *a; + *a = *b; + *b = tmp; +} + +/*! \copydoc XsArrayDescriptor::itemCopy + \note Specialization for XsStringOutputType +*/ +void copyStringOutputType(XsStringOutputType* to, XsStringOutputType const* from) +{ + *to = *from; +} + +/*! \copydoc XsArrayDescriptor::itemCompare + \note Specialization for XsStringOutputType +*/ +int compareStringOutputType(XsStringOutputType const* a, XsStringOutputType const* b) +{ + if (*a < *b) + return -1; + if (*a > *b) + return 1; + return 0; +} + +//! \brief Descriptor for XsStringOutputTypeArray +XsArrayDescriptor const g_xsStringOutputTypeArrayDescriptor = +{ + sizeof(XsStringOutputType), + XSEXPCASTITEMSWAP swapStringOutputType, // swap + 0, // construct + (XsArrayItemCopyFunc)copyStringOutputType, // copy construct + 0, // destruct + (XsArrayItemCopyFunc)copyStringOutputType, // copy + (XsArrayItemCompareFunc)compareStringOutputType,// compare + XSEXPCASTRAWCOPY XsArray_rawCopy +}; + +/*! \copydoc XsArray_constructDerived + \note Specialization for XsStringOutputTypeArray +*/ +void XsStringOutputTypeArray_construct(XsStringOutputTypeArray* thisPtr, XsSize count, XsStringOutputType const* src) +{ + XsArray_construct(thisPtr, &g_xsStringOutputTypeArrayDescriptor, count, src); +} diff --git a/extern/xspublic/xstypes/xsstringoutputtypearray.h b/extern/xspublic/xstypes/xsstringoutputtypearray.h new file mode 100644 index 0000000..4590b2f --- /dev/null +++ b/extern/xspublic/xstypes/xsstringoutputtypearray.h @@ -0,0 +1,95 @@ + +// Copyright (c) 2003-2022 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 XSSTRINGOUTPUTTYPEARRAY_H +#define XSSTRINGOUTPUTTYPEARRAY_H + +#include "xsstringoutputtype.h" +#include "xsarray.h" + +#ifdef __cplusplus +extern "C" { +#endif + +extern XsArrayDescriptor const XSTYPES_DLL_API g_xsStringOutputTypeArrayDescriptor; + +#ifndef __cplusplus +#define XSSTRINGOUTPUTTYPEARRAY_INITIALIZER XSARRAY_INITIALIZER(&g_xsStringOutputTypeArrayDescriptor) + +XSARRAY_STRUCT(XsStringOutputTypeArray, XsStringOutputType); +typedef struct XsStringOutputTypeArray XsStringOutputTypeArray; + +XSTYPES_DLL_API void XsStringOutputTypeArray_construct(XsStringOutputTypeArray* thisPtr, XsSize count, XsStringOutputType const* src); +#else +} // extern "C" +#endif + +#ifdef __cplusplus + 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 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); + } +#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) + { + } +#endif + }; +#endif +#endif diff --git a/extern/xspublic/xstypes/xsstringstreaming.h b/extern/xspublic/xstypes/xsstringstreaming.h new file mode 100644 index 0000000..f5c553c --- /dev/null +++ b/extern/xspublic/xstypes/xsstringstreaming.h @@ -0,0 +1,144 @@ + +// Copyright (c) 2003-2022 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 XSSTRINGSTREAMING_H +#define XSSTRINGSTREAMING_H + +#ifdef __cplusplus +#include "xsstring.h" +#include "xsvector.h" +#include "xsmatrix.h" +#include "xsquaternion.h" + +#ifndef XSENS_NO_STL +#include +namespace std +{ +/*! \brief Stream output operator for XsVector */ +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] << ")"); +} + +/*! \brief Stream output operator for XsMatrix */ +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) + { + 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; +} + +/*! \brief Stream output operator for XsQuaternion */ +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 + +/*! \brief To XsString stream output operator for XsSize */ +inline XsString& operator<<(XsString& o, XsSize const& v) +{ + char buffer[32]; // 2e64 = 1.8e19 so this should be enough + sprintf(buffer, "%" PRINTF_SIZET_MODIFIER "u", v); + o << XsString(buffer); + return o; +} + +/*! \brief To XsString stream output operator for XsReal */ +inline XsString& operator<<(XsString& o, XsReal const& v) +{ + char buffer[64]; + sprintf(buffer, "%g", v); + o << XsString(buffer); + return o; +} + +/*! \brief To XsString stream output operator for XsVector */ +inline XsString& operator<<(XsString& 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] << ")"); +} + +/*! \brief To XsString stream output operator for XsMatrix */ +inline XsString& operator<<(XsString& o, XsMatrix const& xm) +{ + o << "M<" << xm.rows() << "," << xm.cols() << ">("; + for (XsSize r = 0; r < xm.rows(); ++r) + { + if (xm.cols()) + { + if (r > 0) + o << "\t"; + 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; +} + +/*! \brief To XsString stream output operator for XsQuaternion */ +inline XsString& operator<<(XsString& o, XsQuaternion const& xq) +{ + o << "Q("; + for (int i = 0; i < 3; i++) + o << xq[i] << ", "; + return (o << xq[3] << ")"); +} + +#endif + +#endif diff --git a/extern/xspublic/xstypes/xssyncfunction.h b/extern/xspublic/xstypes/xssyncfunction.h new file mode 100644 index 0000000..29d1cd2 --- /dev/null +++ b/extern/xspublic/xstypes/xssyncfunction.h @@ -0,0 +1,75 @@ + +// Copyright (c) 2003-2022 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 XSSYNCFUNCTION_H +#define XSSYNCFUNCTION_H + +/*! \addtogroup enums Global enumerations + @{ +*/ + +//AUTO namespace xstypes { +/*! \brief Actions to be taken on input triggers */ +enum XsSyncFunction +{ + XSF_StartRecordingIn, /*!< \brief Start recording on trigger or emit trigger when first recording frame is started (In). \remark Applies to Awinda Station. */ + XSF_StopRecordingIn, /*!< \brief Stop recording on trigger or emit trigger when recording is stopped (In). \remark Applies to Awinda Station. */ + XSF_ResetTimer, /*!< \brief On input trigger, the outgoing timer of the station will be set to 0. \remark Applies to Awinda Station. */ + XSF_TriggerIndication, /*!< \brief An indication is sent to the driver when trigger is detected. \remark Applies to Awinda Station. */ + + XSF_IntervalTransitionMeasurement, /*!< \brief Emit trigger on an interval transition during measurement and recording. */ + XSF_IntervalTransitionRecording, /*!< \brief Emit trigger on an interval transition during recording. */ + XSF_GotoOperational, /*!< \brief Emit trigger when going to Operational mode \remark Applies to Awinda Station. */ + + XSF_SampleAndSend, /*!< \brief Sample a sample and send the MT Data message. \remark Applies to Mt. */ + XSF_SendLatest, /*!< \brief Send the latest sample. \remark Applies to Mt. */ + XSF_ClockBiasEstimation, /*!< \brief Do a clock bias estimation on trigger. \remark Applies to Mti-G. */ + + XSF_PulseWithModulation, /*!< \brief Do interval transition measurement with pulse width modulation. */ + + XSF_StartSampling, /*!< \brief Start sampling. Data will only be transmitted after this trigger has been received. \remark Applies only to Mk4. */ + + XSF_StartRecordingOut, /*!< \brief Start recording on trigger or emit trigger when first recording frame is started (Out). \remark Applies to Awinda Station. */ + XSF_StopRecordingOut, /*!< \brief Stop recording on trigger or emit trigger when recording is stopped (Out). \remark Applies to Awinda Station. */ + + XSF_Gnss1Pps, /*!< \brief Emit trigger on the start of each second, generated by the GNSS receiver. */ + + XSF_BusSync, /*!< \brief Supply periodic trigger to synchronize multiple MTi devices on a single bus. */ + + XSF_Invalid, /*!< \brief Invalid action \details This indicates the trigger action is not usable. */ + XSF_Count = XSF_Invalid /*!< \brief Amount of trigger actions */ +}; +/*! @} */ +typedef enum XsSyncFunction XsSyncFunction; +//AUTO } + +#endif diff --git a/extern/xspublic/xstypes/xssyncline.h b/extern/xspublic/xstypes/xssyncline.h new file mode 100644 index 0000000..12efa0a --- /dev/null +++ b/extern/xspublic/xstypes/xssyncline.h @@ -0,0 +1,68 @@ + +// Copyright (c) 2003-2022 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 XSSYNCLINE_H +#define XSSYNCLINE_H + +/*! \addtogroup enums Global enumerations + @{ +*/ + +//AUTO namespace xstypes { +/*! \brief Synchronization line identifiers */ +enum XsSyncLine +{ + XSL_Inputs, /*!< \brief Value for checking if a line is an input, any item equal to or greater than XSL_Inputs and less than XSL_Outputs is an input */ + XSL_In1 = XSL_Inputs, /*!< \brief Sync In 1 \remark Applies to Awinda Station and Mt */ + XSL_In2, /*!< \brief Sync In 2 \remark Applies to Awinda Station */ + XSL_In3, /*!< \brief Sync In 3 \remark Applies to MTi-680 */ + XSL_Bi1In, /*!< \brief Bidirectional Sync 1 In */ + XSL_ClockIn, /*!< \brief Clock synchronisation input \remark Applies to Mk4 */ + XSL_CtsIn, /*!< \brief RS232 CTS sync in */ + XSL_GnssClockIn, /*!< \brief Clock synchronisation input line attached to internal GNSS unit \remark Applies to Mk4 */ + XSL_ExtTimepulseIn, /*!< \brief External time pulse input (e.g. for external GNSS unit) \remark Applies to Mk4 with external device */ + XSL_ReqData, /*!< \brief Serial data sync option, use \a XMID_ReqData message id for this \remark Applies to Mk4*/ + XSL_Gnss1Pps, /*!< \brief GNSS 1PPS pulse sync line \remark Applies to MTi-7*/ + + XSL_Outputs, /*!< \brief Value for checking if a line is output. Values equal to or greater than this are outputs */ + XSL_Out1 = XSL_Outputs, /*!< \brief Sync Out 1 \remark Applies to Awinda Station and Mt */ + XSL_Out2, /*!< \brief Sync Out 2 \remark Applies to Awinda Station */ + XSL_Bi1Out, /*!< \brief Bidirectional Sync 1 Out */ + XSL_RtsOut, /*!< \brief RS232 RTS sync out */ + + XSL_Invalid /*!< \brief Invalid sync setting. Used if no sync line is set */ +}; +/*! @} */ +typedef enum XsSyncLine XsSyncLine; +//AUTO } + +#endif diff --git a/extern/xspublic/xstypes/xssyncpolarity.h b/extern/xspublic/xstypes/xssyncpolarity.h new file mode 100644 index 0000000..80b479c --- /dev/null +++ b/extern/xspublic/xstypes/xssyncpolarity.h @@ -0,0 +1,55 @@ + +// Copyright (c) 2003-2022 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 XSSYNCPOLARITY_H +#define XSSYNCPOLARITY_H + +/*! \addtogroup enums Global enumerations + @{ +*/ + +//AUTO namespace xstypes { +/*! \brief Signal polarity */ +enum XsSyncPolarity +{ + XSP_None = 0, /*!< \brief Don't generate or react to trigger level changes */ + XSP_RisingEdge = 1, /*!< \brief React to a rising edge on input */ + XSP_PositivePulse = XSP_RisingEdge, /*!< \brief Generate a positive pulse on output */ + XSP_FallingEdge = 2, /*!< \brief React to a falling edge on input */ + XSP_NegativePulse = XSP_FallingEdge, /*!< \brief Generate a falling edge on output */ + XSP_Both /*!< \brief Toggle output or react on all toggles on input */ +}; +/*! @} */ +typedef enum XsSyncPolarity XsSyncPolarity; +//AUTO } + +#endif diff --git a/extern/xspublic/xstypes/xssyncrole.h b/extern/xspublic/xstypes/xssyncrole.h new file mode 100644 index 0000000..0e635a3 --- /dev/null +++ b/extern/xspublic/xstypes/xssyncrole.h @@ -0,0 +1,51 @@ + +// Copyright (c) 2003-2022 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 XSSYNCROLE_H +#define XSSYNCROLE_H + +/*! \addtogroup enums Global enumerations + @{ +*/ +/*! \brief Synchronization roles +*/ +enum XsSyncRole +{ + XSR_Slave, + XSR_None, + XSR_MasterSlave, + XSR_Master +}; +/*! @} */ +typedef enum XsSyncRole XsSyncRole; + +#endif diff --git a/extern/xspublic/xstypes/xssyncsetting.c b/extern/xspublic/xstypes/xssyncsetting.c new file mode 100644 index 0000000..29c433b --- /dev/null +++ b/extern/xspublic/xstypes/xssyncsetting.c @@ -0,0 +1,141 @@ + +// Copyright (c) 2003-2022 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 "xssyncsetting.h" +#include + +/*! \addtogroup cinterface C Interface + @{ +*/ + +/*! \brief Returns whether the selected line is configured as an input line +*/ +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; + + default: + return 0; + } +} + +/*! \brief Returns whether the selected line is configured as an output line +*/ +int XsSyncSetting_isOutput(const XsSyncSetting* thisPtr) +{ + switch (thisPtr->m_line) + { + case XSL_Out1: + case XSL_Out2: + case XSL_Bi1Out: + case XSL_RtsOut: + return 1; + + default: + return 0; + } +} + +/*! \brief Swap the contents of \a a with \a b +*/ +void XsSyncSetting_swap(XsSyncSetting* a, XsSyncSetting* b) +{ + XsSyncSetting tmp; + memcpy(&tmp, a, sizeof(XsSyncSetting)); + memcpy(a, b, sizeof(XsSyncSetting)); + memcpy(b, &tmp, sizeof(XsSyncSetting)); +} + +/*! \brief Compares \a a with \a b + \returns 0 if \a a equals \a b, negative value if \a a < \a b, positive value if \a a > \a b + \param[in] a Sync setting a + \param[in] b Sync setting b +*/ +int XsSyncSetting_compare(const struct XsSyncSetting* a, const struct XsSyncSetting* b) +{ + 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 (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_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_triggerOnce < b->m_triggerOnce) + return -1; + if (b->m_triggerOnce < a->m_triggerOnce) + return 1; + + return 0; +} + +/*! @} */ diff --git a/extern/xspublic/xstypes/xssyncsetting.h b/extern/xspublic/xstypes/xssyncsetting.h new file mode 100644 index 0000000..c0878ff --- /dev/null +++ b/extern/xspublic/xstypes/xssyncsetting.h @@ -0,0 +1,164 @@ + +// Copyright (c) 2003-2022 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 XSSYNCSETTINGS_H +#define XSSYNCSETTINGS_H + +#include + +#include "pstdint.h" +#include "xstypesconfig.h" +#include "xssyncline.h" +#include "xssyncfunction.h" +#include "xssyncpolarity.h" + +struct XsSyncSetting; + +#ifdef __cplusplus +extern "C" { +#endif +#ifndef __cplusplus +#define XSSYNCSETTINGS_INITIALIZER { XSL_Invalid, XSF_Invalid, XSP_None, 1, 0, 0, 0, 0, 0, 0 } +#endif + +XSTYPES_DLL_API int XsSyncSetting_isInput(const struct XsSyncSetting* thisPtr); +XSTYPES_DLL_API int XsSyncSetting_isOutput(const struct XsSyncSetting* thisPtr); +XSTYPES_DLL_API void XsSyncSetting_swap(struct XsSyncSetting* a, struct XsSyncSetting* b); +XSTYPES_DLL_API int XsSyncSetting_compare(const struct XsSyncSetting* a, const struct XsSyncSetting* b); + +#ifdef __cplusplus +} // extern "C" +#endif + +/*! \brief A structure for storing all xsens sync settings */ +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. */ + uint32_t m_pulseWidth; /*!< The time to keep the line polarity before toggling back, in microseconds. */ + int32_t m_offset; /*!< The time between reception of a line change and the execution of the sync action, in microseconds. */ + uint16_t m_skipFirst; /*!< The number of frames to skip before executing the action. */ + uint16_t m_skipFactor; /*!< The number of frames to skip between 2 actions. */ + uint16_t m_clockPeriod; /*!< The frequency of the external clock in milliseconds, only valid when function is XSF_ClockBiasEstimation. */ + uint8_t m_triggerOnce; /*!< Whether the action is repeated for each frame. */ + uint8_t m_padding; /*!< Padding to get at a 4 byte boundary so memcpy/memcmp works with sizeof(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) + : m_line(line) + , m_function(function) + , m_polarity(polarity) + , m_pulseWidth(pulseWidth) + , m_offset(offset) + , m_skipFirst(skipFirst) + , m_skipFactor(skipFactor) + , m_clockPeriod(clockPeriod) + , m_triggerOnce(triggerOnce) + , m_padding(0) + { + } + + //! \brief Construct a XsSyncSetting as a copy of \a other. + XsSyncSetting(const XsSyncSetting& other) + { + memcpy(this, &other, sizeof(XsSyncSetting)); + } + + //! \brief Copy values of \a other into this. + const XsSyncSetting& operator =(const XsSyncSetting& other) + { + if (this != &other) + memcpy(this, &other, sizeof(XsSyncSetting)); + return *this; + } + + //! \brief \copybrief XsSyncSetting_isInput + inline bool isInput() const + { + return 0 != XsSyncSetting_isInput(this); + } + + //! \brief \copybrief XsSyncSetting_isOutput + inline bool isOutput() const + { + return 0 != XsSyncSetting_isOutput(this); + } + + /*! \brief Swap the contents with \a other + */ + inline void swap(XsSyncSetting& other) + { + XsSyncSetting_swap(this, &other); + } + + /*! \brief Swap the contents with \a fist with that of \a second + */ + friend void swap(XsSyncSetting& first, XsSyncSetting& second) + { + first.swap(second); + } + + /*! \brief Return true if \a other is identical to this + */ + inline bool operator == (const XsSyncSetting& other) const + { + return (this == &other) || XsSyncSetting_compare(this, &other) == 0; + } + + /*! \brief Return true if \a other is not identical to this + */ + inline bool operator != (const XsSyncSetting& other) const + { + return (this != &other) && XsSyncSetting_compare(this, &other) != 0; + } + + /*! \brief Return true if \a other is less than this + */ + inline bool operator < (const XsSyncSetting& other) const + { + return (this != &other) && XsSyncSetting_compare(this, &other) < 0; + } +#endif +}; + +typedef struct XsSyncSetting XsSyncSetting; + +#endif diff --git a/extern/xspublic/xstypes/xssyncsettingarray.c b/extern/xspublic/xstypes/xssyncsettingarray.c new file mode 100644 index 0000000..3a6300a --- /dev/null +++ b/extern/xspublic/xstypes/xssyncsettingarray.c @@ -0,0 +1,83 @@ + +// Copyright (c) 2003-2022 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 "xssyncsettingarray.h" +#include "xssyncsetting.h" +#include +#include + +/*! \struct XsSyncSettingArray + \brief A list of XsSyncSetting values + \sa XsArray +*/ + +/*! \copydoc XsArrayDescriptor::itemCopy + \note Specialization for XsSyncSetting*/ +void copySyncSetting(XsSyncSetting* to, XsSyncSetting const* from) +{ + *to = *from; +} + +/*! \copydoc XsArrayDescriptor::itemCompare + \note Specialization for XsSyncSetting*/ +int compareSyncSetting(XsSyncSetting const* a, XsSyncSetting const* b) +{ + return XsSyncSetting_compare(a, b); +} + +//! \brief zero the pointer value +void zeroSyncSetting(XsSyncSetting* a) +{ + memset(a, 0, sizeof(XsSyncSetting)); + a->m_line = XSL_Invalid; +} + +//! \brief Descriptor for XsSyncSettingArray +XsArrayDescriptor const g_xsSyncSettingArrayDescriptor = +{ + sizeof(XsSyncSetting), + XSEXPCASTITEMSWAP XsSyncSetting_swap, + XSEXPCASTITEMMAKE zeroSyncSetting, // construct + XSEXPCASTITEMCOPY copySyncSetting, // copy construct + 0, // destruct + XSEXPCASTITEMCOPY copySyncSetting, + XSEXPCASTITEMCOMP compareSyncSetting, + XSEXPCASTRAWCOPY XsArray_rawCopy // raw copy +}; + +/*! \copydoc XsArray_constructDerived + \note Specialization for XsSyncSettingArray +*/ +void XsSyncSettingArray_construct(XsSyncSettingArray* thisPtr, XsSize count, XsSyncSetting const* src) +{ + XsArray_construct(thisPtr, &g_xsSyncSettingArrayDescriptor, count, src); +} diff --git a/extern/xspublic/xstypes/xssyncsettingarray.h b/extern/xspublic/xstypes/xssyncsettingarray.h new file mode 100644 index 0000000..00262bf --- /dev/null +++ b/extern/xspublic/xstypes/xssyncsettingarray.h @@ -0,0 +1,96 @@ + +// Copyright (c) 2003-2022 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 XSSYNCSETTINGARRAY_H +#define XSSYNCSETTINGARRAY_H + +#include "xsarray.h" + +#ifdef __cplusplus +#include "xssyncsetting.h" +extern "C" { +#endif + +extern XsArrayDescriptor const XSTYPES_DLL_API g_xsSyncSettingArrayDescriptor; + +#ifndef __cplusplus +#define XSSYNCSETTINGSARRAY_INITIALIZER XSARRAY_INITIALIZER(&g_xsSyncSettingArrayDescriptor) + +struct XsSyncSetting; +XSARRAY_STRUCT(XsSyncSettingArray, struct XsSyncSetting); +typedef struct XsSyncSettingArray XsSyncSettingArray; + +XSTYPES_DLL_API void XsSyncSettingArray_construct(XsSyncSettingArray* thisPtr, XsSize count, struct XsSyncSetting const* src); +#endif + +#ifdef __cplusplus +} // extern "C" + +struct XsSyncSettingArray : public XsArrayImpl +{ + //! \brief Constructs an XsSyncSettingArray + inline explicit XsSyncSettingArray(XsSize sz = 0, XsSyncSetting const* src = 0) + : ArrayImpl(sz, src) + { + } + + //! \brief Constructs an XsSyncSettingArray as a copy of \a other + inline XsSyncSettingArray(XsSyncSettingArray const& other) + : ArrayImpl(other) + { + } + + //! \brief Constructs an XsSyncSettingArray that references the data supplied in \a ref + inline explicit XsSyncSettingArray(XsSyncSetting* 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(XsSyncSettingArray& first, XsSyncSettingArray& second) + { + first.swap(second); + } +#endif + +#ifndef XSENS_NOITERATOR + //! \brief Constructs an XsSyncSettingArray with the array bound by the supplied iterators \a beginIt and \a endIt + template + inline XsSyncSettingArray(Iterator beginIt, Iterator endIt) + : ArrayImpl(beginIt, endIt) + { + } +#endif +}; +#endif +#endif diff --git a/extern/xspublic/xstypes/xsthread.c b/extern/xspublic/xstypes/xsthread.c new file mode 100644 index 0000000..1b85c7c --- /dev/null +++ b/extern/xspublic/xstypes/xsthread.c @@ -0,0 +1,133 @@ + +// Copyright (c) 2003-2022 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 "xsthread.h" + +#ifdef _WIN32 +#define MS_VC_EXCEPTION 0x406D1388 + +#pragma pack(push,8) +// Copied from windows API +struct THREADNAME_INFO +{ + DWORD dwType; // Must be 0x1000. + LPCSTR szName; // Pointer to name (in user addr space). + DWORD dwThreadID; // XsThread ID (-1=caller thread). + DWORD dwFlags; // Reserved for future use, must be zero. +}; +#pragma pack(pop) +typedef struct THREADNAME_INFO THREADNAME_INFO; + +/*! \addtogroup cinterface C Interface + @{ +*/ + +/*! \brief Set the name of the current thread to \a threadName + \details Set the name of the current thread + \param threadName Pointer to the name to assign to the thread +*/ +void XSTYPES_DLL_API xsNameThisThread(const char* threadName) +{ + DWORD dwThreadID = GetCurrentThreadId(); + + //Sleep(10); + THREADNAME_INFO info; + info.dwType = 0x1000; + info.szName = threadName; + info.dwThreadID = dwThreadID; + info.dwFlags = 0; + + __try + { + RaiseException(MS_VC_EXCEPTION, 0, sizeof(info) / sizeof(ULONG_PTR), (ULONG_PTR*)&info); + } + __except (EXCEPTION_EXECUTE_HANDLER) + { + } +} + +/*! @} */ + +#else +#include +#ifdef __APPLE__ +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); + +inline static int pthread_setname_np2(pthread_t __target_thread, const char* __name) +{ + return pthread_setname_np(__target_thread, __name); +} + +#endif + +/*! \addtogroup cinterface C Interface + @{ +*/ + +/*! \brief Set the name of the current thread to \a threadName + \details Set the name of the current thread + \param threadName The new name of the thread, ignored if empty or null + \note If threadName is longer than 15 characters, it will be compressed +*/ +void XSTYPES_DLL_API xsNameThisThread(const char* threadName) +{ + if (!threadName || !threadName[0]) + return; + if (pthread_setname_np2(xsGetCurrentThreadId(), threadName) == ERANGE) + { + char dup[16]; + strncpy(dup, threadName, 11); + strncpy(dup + 11, threadName + strlen(threadName) - 4, 4); + dup[15] = 0; + pthread_setname_np2(xsGetCurrentThreadId(), dup); + } +} + +/*! @} */ + +pthread_t XSTYPES_DLL_API xsStartThread(void* (func)(void*), void* param, void* pid) +{ + (void)pid; + pthread_t thread; + if (pthread_create(&thread, NULL, func, param)) + return XSENS_INVALID_THREAD; + return thread; +} + +#endif diff --git a/extern/xspublic/xstypes/xsthread.h b/extern/xspublic/xstypes/xsthread.h new file mode 100644 index 0000000..956fdd7 --- /dev/null +++ b/extern/xspublic/xstypes/xsthread.h @@ -0,0 +1,184 @@ + +// Copyright (c) 2003-2022 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 XSTHREAD_H +#define XSTHREAD_H + +#include "xstime.h" +#if defined(XSENS_DEBUG) && defined(_MSC_VER) + #pragma warning (disable: 4985) + #include +#endif + +#ifndef __GNUC__ + #pragma warning(disable: 4127) +#endif + +#ifdef __cplusplus +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; +#ifdef __cplusplus +typedef ::DWORD XsThreadId; +#else +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) + +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) + +#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) + +#endif // _WIN32 + +#if !defined(XSENS_USE_POSIX_LOCKING) +#if defined(XSENS_WINDOWS) +#define XSENS_USE_POSIX_LOCKING 0 +#else +#define XSENS_USE_POSIX_LOCKING 1 +#endif +#endif + +#ifndef __GNUC__ +#pragma warning(default: 4127) +#endif + +#ifdef __cplusplus +} // extern "C" +#endif + +typedef enum XsThreadPriority XsThreadPriority; + +#endif diff --git a/extern/xspublic/xstypes/xstime.c b/extern/xspublic/xstypes/xstime.c new file mode 100644 index 0000000..9f47432 --- /dev/null +++ b/extern/xspublic/xstypes/xstime.c @@ -0,0 +1,441 @@ + +// Copyright (c) 2003-2022 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. +// + +// Note, this function requires compiler option "-lrt" to be set when compiling with gcc +#include "xstime.h" +#include "xstimestamp.h" +#include +#include + +#ifdef _WIN32 + #include + #include "xssimpleversion.h" +#else + #include + #include + #include + #include + #include +#endif +#include + +/*! \namespace XsTime + \brief Namespace for time and date constants and operations +*/ + +//! The number of seconds in a normal day +const XsTimeStamp XsTime_secPerDay = { 60 * 60 * 24LL }; + +//! The number of milliseconds in a normal day +const XsTimeStamp XsTime_milliSecPerDay = { 60 * 60 * 24 * 1000LL }; + +//! The maximum positive value of an XsTimeStamp value +const XsTimeStamp XsTime_timeStampMax = { 0x7FFFFFFFFFFFFFFFLL }; + +////////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////// Other functions //////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////// +#if 0 // def __APPLE__, it is in fact available on apple, so... +/*! \brief An implementation of linux' clock_gettime() + + clock_gettime() is not available on Apple/Darwin platforms. This function helps + maintaining compatibility with Linux code. +*/ +#ifndef CLOCK_REALTIME + #define CLOCK_REALTIME 0 +#endif +static int clock_gettime(int clk_id, struct timespec* tp) +{ + (void)clk_id; + struct timeval now; + + int rv = gettimeofday(&now, NULL); + if (rv != 0) + return rv; + + tp->tv_sec = now.tv_sec; + tp->tv_nsec = now.tv_usec * 1000; + + return 0; +} +#endif + +/*! \addtogroup cinterface C Interface + @{ +*/ + +////////////////////////////////////////////////////////////////////////////////////////// +/*! \brief The function returns the current time of day in ms since midnight + \param date_ When not 0, the corresponding current date is put in here. + \param secs_ When not 0, the full timestamp in seconds since the epoch is put in here. + \returns The number of milliseconds that have passed since midnight. +*/ +uint32_t XsTime_getTimeOfDay(struct tm* date_, time_t* secs_) +{ +#ifdef _WIN32 + typedef void(WINAPI * GetSystemTimeType)(LPFILETIME); + static GetSystemTimeType getSystemTime = 0; + static int havePreciseFileTime = 0; + if (getSystemTime == 0) + { + XsSimpleVersion ver; + XsSimpleVersion_osVersion(&ver); + if (ver.m_major >= 8) + { + getSystemTime = (GetSystemTimeType)GetProcAddress(GetModuleHandleA("Kernel32"), "GetSystemTimePreciseAsFileTime"); + if (getSystemTime) + havePreciseFileTime = 1; + else + getSystemTime = GetSystemTimeAsFileTime; + } + else + getSystemTime = GetSystemTimeAsFileTime; + } + + if (havePreciseFileTime) + { + int64_t t; + FILETIME now; + __time64_t tin; + getSystemTime(&now); + t = (int64_t)(((((uint64_t) now.dwHighDateTime << 32) | now.dwLowDateTime) / 10000) - 11644473600000); + tin = t / 1000; + if (date_ != NULL) + _localtime64_s(date_, &tin); + if (secs_ != NULL) + *secs_ = (time_t) tin; + return (uint32_t)(t % (XsTime_secPerDay.m_msTime * 1000)); + } + else + { + // CORRECTION_DELTA_MS is the maximum allowed difference between the system time and the performance counter time. This depends on the granularity of the clocks. + static const int64_t CORRECTION_DELTA_MS = 32; + static int64_t startTimePerfCount; + static int64_t startTimeSysTime; + static int64_t perfCountFreq = 0; + int64_t t; + LARGE_INTEGER pc; + FILETIME now; + __time64_t tin; + + GetSystemTimeAsFileTime(&now); + t = (int64_t)(((((uint64_t) now.dwHighDateTime << 32) | now.dwLowDateTime) / 10000) - 11644473600000); + + if (QueryPerformanceCounter(&pc)) + { + int64_t tNow; + if (!perfCountFreq) + { + LARGE_INTEGER tmp; + QueryPerformanceFrequency(&tmp); + perfCountFreq = tmp.QuadPart; + startTimePerfCount = pc.QuadPart; + startTimeSysTime = t; + } + + tNow = startTimeSysTime + (1000 * (pc.QuadPart - startTimePerfCount)) / perfCountFreq; //lint !e414 + + if (t > tNow || (tNow - t) > CORRECTION_DELTA_MS) + { + startTimePerfCount = pc.QuadPart; + startTimeSysTime = t; + } + else + t = tNow; + } + + tin = t / 1000; + if (date_ != NULL) + _localtime64_s(date_, &tin); + if (secs_ != NULL) + *secs_ = (time_t) tin; + 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_); + + 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)); +#endif +} + +/*! \brief Retrieves the date and time (platform-independent) + \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) +{ +#ifdef _WIN32 + __time64_t t; + _time64(&t); + if (date != 0) + _localtime64_s(date, &t); + return (int64_t)t; +#else + time_t t; + time(&t); + if (date != 0) + { + struct tm* result = localtime(&t); + memcpy(date, result, sizeof(struct tm)); + } + return (int64_t)t; +#endif +} + +/*! \brief Retrieves the date as string representation + The format is YYYYMMDD + 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; + unsigned int year, month; + + if (date != 0) + dt = *date; + else + XsTime_getDateTime(&dt); + + year = (unsigned int) (dt.tm_year + 1900); + month = (unsigned int) (dt.tm_mon + 1); + + char tmpDest[9]; +#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); +} + +/*! \brief Retrieves the time as binary + The format is HHMMSShh (where H is hour and 'h' is hundredths) + so 14:25:01.23 is stored as an array dest[8] = { '1', '4', '2', '5', '0', '1', '2', '3'} + \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) + \note (for now hundreths are set to 0) +*/ +void XsTime_getTimeAsString(char* dest, const struct tm* date) +{ + struct tm dt; + + if (date != 0) + dt = *date; + else + XsTime_getDateTime(&dt); + + char tmpDest[9]; + snprintf(tmpDest, 9, "%02d%02d%02d%02d", dt.tm_hour, dt.tm_min, dt.tm_sec, 0); + memcpy(dest, tmpDest, 8); +} + +/*! \brief Retrieves the date as wstring representation + The format is YYYYMMDD + so 25 dec 2010 is stored as an array dest[8] = {'2', '0', '1', '0', '1', '2', '2', '5' } + \param dest : A pointer to an array of at least (!) 8 wchars + \param date : If date is non-zero this date is converted, otherwise the current date is retrieved and used) +*/ +void XsTime_getDateAsWString(wchar_t* dest, const struct tm* date) +{ + struct tm dt; + int year, month; + + if (date != 0) + dt = *date; + else + XsTime_getDateTime(&dt); + + year = dt.tm_year + 1900; + month = dt.tm_mon + 1; + swprintf(dest, 9, L"%04d%02d%02d", year, month, dt.tm_mday); +} + +/*! \brief Retrieves the time as binary + The format is HHMMSShh (where H is hour and 'h' is hundredths) + so 14:25:01.23 is stored as an array dest[8] = { '1', '4', '2', '5', '0', '1', '2', '3'} + \param dest : A pointer to an array of at least (!) 8 wchars + \param date : If date is non-zero this date is converted, otherwise the current date is retrieved and used) + \note (for now hundreths are set to 0) +*/ +void XsTime_getTimeAsWString(wchar_t* dest, const struct tm* date) +{ + struct tm dt; + + if (date != 0) + dt = *date; + else + XsTime_getDateTime(&dt); + + swprintf(dest, 9, L"%02d%02d%02d%02d", dt.tm_hour, dt.tm_min, dt.tm_sec, 0); +} + +/*! \brief Make the current thread sleep for at least \a ms milliseconds + \details A platform independent sleep routine to sleep for at least \a ms milliseconds. + + \param ms The number of milliseconds to sleep + + \sa XsTime_udelay +*/ +void XsTime_msleep(uint32_t ms) +{ +#ifdef _WIN32 + Sleep(ms); +#else + XsTime_udelay(ms * 1000); +#endif +} + +/*! \brief Delays the current thread for at least \a us microseconds + \details A platform independent delay routine to sleep for at least \a us microseconds. + + \param us The number of microseconds to delay + + \sa XsTime_msleep +*/ +void XsTime_udelay(uint64_t us) +{ +#ifdef _WIN32 + LARGE_INTEGER freq, start, stop; + double countPerMicroSecond ; + QueryPerformanceCounter(&start); + QueryPerformanceFrequency(&freq); + countPerMicroSecond = freq.QuadPart / 1.0e6; + do + { + QueryPerformanceCounter(&stop); + } while (start.QuadPart + ((double)(us) * countPerMicroSecond) > stop.QuadPart); +#else + struct timespec ts; + int ret = -1; + + ts.tv_sec = us / 1000000; + ts.tv_nsec = ((long)us - (ts.tv_sec * 1000000)) * 1000; + + while (ret) + { + ret = nanosleep(&ts, &ts); + if (ret) + assert(errno == EINTR); + } +#endif +} + +/*! \brief Returns the current time in ms since the epoch (Jan 1st 1970) + \param now Pointer to %XsTimeStamp container for the returned value, may be 0 + \returns The current time in ms since the epoch (Jan 1st 1970) as a 64-bit integer +*/ +int64_t XsTime_timeStampNow(XsTimeStamp* now) +{ + XsTimeStamp tmp; + time_t s; + + if (now == 0) + now = &tmp; + + now->m_msTime = (long long) XsTime_getTimeOfDay(NULL, &s); + now->m_msTime = (now->m_msTime % 1000) + (((long long)s) * 1000); + + return now->m_msTime; +} + +/*! \cond XS_INTERNAL */ +int64_t XsTime_utcToLocalValue = 0; //!< Internal storage for UTC to local time correction (ms) +int64_t XsTime_localToUtcValue = 0; //!< Internal storage for local time to UTC correction (ms) +/*! \endcond XS_INTERNAL */ + +/*! \brief Stabilize the clock + \details Repeatedly calls XsTime_timeStampNow for 16-32 ms to stabilize the clock. + Initializes the local to UTC conversion parameters. +*/ +void XsTime_initializeTime() +{ + int64_t tutc, tloc; +#ifdef _WIN32 + FILETIME now, nowloc; + + 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); + +#else + struct timespec tp; + struct tm now, nowloc; + + clock_gettime(CLOCK_REALTIME, &tp); // compile with -lrt + gmtime_r(&tp.tv_sec, &now); + localtime_r(&tp.tv_sec, &nowloc); + + tutc = (int64_t) now.tm_min * 60000; + tloc = (int64_t) nowloc.tm_min * 60000; +#endif + + XsTime_utcToLocalValue = tloc - tutc; + XsTime_localToUtcValue = tutc - tloc; + + long long start = XsTime_timeStampNow(0); + while (XsTime_timeStampNow(0) - start < 32) {} +} + +/*! \brief Returns the conversion value from UTC time to local time in ms + \returns The conversion value from UTC time to local time in ms + \note Make sure to call XsTime_initializeTime() before calling this function as well as when changing time zones. +*/ +int64_t XsTime_utcToLocal() +{ + return XsTime_utcToLocalValue; +} + +/*! \brief Returns the conversion value from local time to UTC time in ms + \returns The conversion value from local time to UTC time in ms + \note Make sure to call XsTime_initializeTime() before calling this function as well as when changing time zones. +*/ +int64_t XsTime_localToUtc() +{ + return XsTime_localToUtcValue; +} + +/*! @} */ diff --git a/extern/xspublic/xstypes/xstime.h b/extern/xspublic/xstypes/xstime.h new file mode 100644 index 0000000..a021665 --- /dev/null +++ b/extern/xspublic/xstypes/xstime.h @@ -0,0 +1,162 @@ + +// Copyright (c) 2003-2022 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 XSTIME_H +#define XSTIME_H + +#include "xstypesconfig.h" +#ifdef _WIN32 + #include +#endif + +#include +#include "pstdint.h" +#include "xstimestamp.h" + +#ifdef __cplusplus +#include "xsstring.h" +extern "C" { +#endif + +XSTYPES_DLL_API extern const XsTimeStamp XsTime_secPerDay; +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 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); +XSTYPES_DLL_API void XsTime_getTimeAsWString(wchar_t* dest, struct tm const* time); +XSTYPES_DLL_API void XsTime_msleep(uint32_t ms); +XSTYPES_DLL_API void XsTime_udelay(uint64_t us); +XSTYPES_DLL_API int64_t XsTime_timeStampNow(XsTimeStamp* now); +XSTYPES_DLL_API void XsTime_initializeTime(void); +XSTYPES_DLL_API int64_t XsTime_utcToLocal(); +XSTYPES_DLL_API int64_t XsTime_localToUtc(); + +#ifdef __cplusplus +} // extern "C" + +namespace XsTime +{ +#ifdef __GNUC__ + #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; +#ifdef __GNUC__ + #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); +} +} +#endif + +#endif diff --git a/extern/xspublic/xstypes/xstimeinfo.c b/extern/xspublic/xstypes/xstimeinfo.c new file mode 100644 index 0000000..d7cc1ec --- /dev/null +++ b/extern/xspublic/xstypes/xstimeinfo.c @@ -0,0 +1,86 @@ + +// Copyright (c) 2003-2022 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 "xstimestamp.h" +#include "xstimeinfo.h" + +/*! \class XsTimeInfo + \brief This class can contain UTC time information +*/ + +/*! \addtogroup cinterface C Interface + @{ +*/ + +/*! \relates XsTimeInfo + \brief Returns the current UTC time +*/ +void XsTimeInfo_currentTime(struct XsTimeInfo* thisPtr) +{ + XsTimeStamp timeStamp; + + if (thisPtr == 0) + return; + + (void)XsTimeStamp_now(&timeStamp); + XsTimeStamp_toTimeInfo(&timeStamp, thisPtr); +} + +/*! \relates XsTimeInfo + \brief Returns the current local time +*/ +void XsTimeInfo_currentLocalTime(struct XsTimeInfo* thisPtr) +{ + XsTimeStamp timeStamp; + + if (thisPtr == 0) + return; + + (void)XsTimeStamp_now(&timeStamp); + XsTimeStamp_utcToLocalTime(&timeStamp, &timeStamp); + XsTimeStamp_toTimeInfo(&timeStamp, thisPtr); +} + +/*! \relates XsTimeInfo + \brief Removes the local time information, making the object pure UTC +*/ +void XsTimeInfo_makeUtc(struct XsTimeInfo* thisPtr) +{ + if (thisPtr->m_utcOffset) + { + XsTimeStamp timeStamp; + XsTimeStamp_fromTimeInfo(&timeStamp, thisPtr); + XsTimeStamp_toTimeInfo(&timeStamp, thisPtr); + } +} + +/*! @} */ diff --git a/extern/xspublic/xstypes/xstimeinfo.h b/extern/xspublic/xstypes/xstimeinfo.h new file mode 100644 index 0000000..c16b739 --- /dev/null +++ b/extern/xspublic/xstypes/xstimeinfo.h @@ -0,0 +1,118 @@ + +// Copyright (c) 2003-2022 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 XSTIMEINFO_H +#define XSTIMEINFO_H + +#include "xstypesconfig.h" +#include "pstdint.h" + +#ifdef __cplusplus +#include +extern "C" { +#endif +#define XSTIMEINFO_INITIALIZER { 0, 0 ,0, 0, 0, 0, 0, 0, 0} +struct XsTimeInfo; + +XSTYPES_DLL_API void XsTimeInfo_currentTime(struct XsTimeInfo* thisPtr); +XSTYPES_DLL_API void XsTimeInfo_currentLocalTime(struct XsTimeInfo* thisPtr); +XSTYPES_DLL_API void XsTimeInfo_makeUtc(struct XsTimeInfo* thisPtr); + +#ifdef __cplusplus +} // extern "C" +#endif + +/*! \brief A structure for storing Time values. */ +struct XsTimeInfo +{ + uint32_t m_nano; //!< \brief Nanosecond part of the time + uint16_t m_year; //!< \brief The year (if date is valid) + uint8_t m_month; //!< \brief The month (if date is valid) + uint8_t m_day; //!< \brief The day of the month (if date is valid) + uint8_t m_hour; //!< \brief The hour (if time is valid) + uint8_t m_minute; //!< \brief The minute (if time is valid) + uint8_t m_second; //!< \brief The second (if time is valid) + uint8_t m_valid; //!< \brief Validity indicator \details When received: bit (0) - UTC Date is valid; bit (1) - UTC Time of Day is valid; bit (2) - UTC Time of Day has been fully resolved (i.e. no seconds uncertainty). + int16_t m_utcOffset; //!< \brief Offset to UTC time in minutes. This value can be added to the stored time to get UTC time. + +#ifdef __cplusplus + /*! \brief Sets all values to 0 */ + void clear() + { + memset(this, 0, sizeof(XsTimeInfo)); + } + + /*! \copydoc XsTimeInfo_currentTime + \return The current UTC Time + */ + inline static XsTimeInfo currentTime() + { + XsTimeInfo tmp; + XsTimeInfo_currentTime(&tmp); + return tmp; + } + + /*! \copydoc XsTimeInfo_currentLocalTime + \return The current local Time + */ + inline static XsTimeInfo currentLocalTime() + { + XsTimeInfo tmp; + XsTimeInfo_currentLocalTime(&tmp); + return tmp; + } + + /*! \brief Returns true if all fields of this and \a other are exactly identical */ + 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; + } + + /*! \brief Removes the local time information, making the object pure UTC */ + void makeUtc() + { + if (m_utcOffset) + XsTimeInfo_makeUtc(this); + } +#endif +}; +typedef struct XsTimeInfo XsTimeInfo; + +#endif diff --git a/extern/xspublic/xstypes/xstimestamp.c b/extern/xspublic/xstypes/xstimestamp.c new file mode 100644 index 0000000..a820d8f --- /dev/null +++ b/extern/xspublic/xstypes/xstimestamp.c @@ -0,0 +1,291 @@ + +// Copyright (c) 2003-2022 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 "xstimestamp.h" +#include "xstime.h" +#include "xstimeinfo.h" +#ifndef XSENS_NO_STL + #include "xsstring.h" +#endif +#include +#include +#include + +/*! \class XsTimeStamp + \brief This class contains method to set, retrieve and compare timestamps +*/ + +/*! \addtogroup cinterface C Interface + @{ +*/ + +/*! \relates XsTimeStamp \brief Set the stored time to \a miliseconds. */ +void XsTimeStamp_setMilliSecondTime(XsTimeStamp* thisPtr, int64_t t) +{ + thisPtr->m_msTime = 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; +} + +/*! \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; +} + +/*! \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); +} + +/*! \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); +} + +/*! \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); +} + +/*! \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); +} + +/*! \relates XsTimeStamp + \brief Returns the current time in ms since the epoch (Jan 1st 1970) + \see XsTime_timeStampNow + \param dest The object to write the time to, may be 0 in which case only the return value is generated. + \returns The current time in ms since the epoch (Jan 1st 1970) +*/ +int64_t XsTimeStamp_now(XsTimeStamp* dest) +{ + return (int64_t) XsTime_timeStampNow(dest); +} + +/*! \relates XsTimeStamp + \brief Returns the maximum value of an %XsTimeStamp +*/ +int64_t XsTimeStamp_maxValue(void) +{ + return 9223372036854775807LL; //INT64_MAX; +} + +/*! \relates XsTimeStamp + \brief Creates a (UTC) XsTimeStamp from an XsTimeInfo object + \param info The time info object to convert + \returns The converted UTC time in ms since the epoch (Jan 1st 1970) +*/ +int64_t XsTimeStamp_fromTimeInfo(struct XsTimeStamp* thisPtr, const struct XsTimeInfo* info) +{ + int64_t rv = -1; + int64_t epoch = 0; + if (info && info->m_valid) + { + struct tm utctm; + utctm.tm_year = info->m_year - 1900; + utctm.tm_mon = info->m_month - 1; + utctm.tm_mday = info->m_day; + + utctm.tm_hour = info->m_hour; + utctm.tm_min = info->m_minute; + utctm.tm_sec = info->m_second; + + // unused + utctm.tm_wday = 0; + utctm.tm_yday = 0; + utctm.tm_isdst = 0; + +#ifdef _WIN32 + epoch = (int64_t)_mkgmtime(&utctm); +#else + epoch = (int64_t)timegm(&utctm); +#endif + + rv = (epoch * 1000LL) + (info->m_nano / 1000000) + ((int64_t) info->m_utcOffset * 60000); + } + if (thisPtr) + thisPtr->m_msTime = rv; + return rv; +} + +/*! \relates XsTimeStamp + \brief Converts the timestamp into an XsTimeInfo object + \param info The XsTimeInfo time object to write the conversion result to +*/ +void XsTimeStamp_toTimeInfo(struct XsTimeStamp const* thisPtr, struct XsTimeInfo* info) +{ + struct tm tmUtc; +#ifdef _WIN32 + __time64_t t; + t = (__time64_t)(thisPtr->m_msTime / 1000); + if (_gmtime64_s(&tmUtc, &t)) + { + //in case of an error the result is an invalid XsTimeInfo + info->m_valid = 0; + return; + } +#elif (defined(__arm__) && defined(__ARMCC_VERSION)) +#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); + if (tmUtcPtr == 0) + { + info->m_valid = 0; + return; + } + memcpy(&tmUtc, tmUtcPtr, sizeof(tmUtc)); +#else + time_t t; + t = (time_t)(thisPtr->m_msTime / 1000); + if (gmtime_r(&t, &tmUtc) == 0) + { + //in case of an error the result is an invalid XsTimeInfo + info->m_valid = 0; + return; + } +#endif + + info->m_day = (uint8_t) tmUtc.tm_mday; + info->m_hour = (uint8_t) tmUtc.tm_hour; + info->m_minute = (uint8_t) tmUtc.tm_min; + 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_valid = 1; + info->m_utcOffset = 0; +} + +#ifndef XSENS_NO_STL +/*! \relates XsTimeStamp + \brief Converts the timestamp into an XsString object in format YYYY/MM/DD hh:mm:ss.nnn + \param result The XsString object to write the conversion result to +*/ +void XsTimeStamp_toString(struct XsTimeStamp const* thisPtr, struct XsString* result) +{ + XsTimeInfo info; + char buffer[32]; + XsTimeStamp_toTimeInfo(thisPtr, &info); + + if (info.m_valid) + { + sprintf(buffer, "%04d/%02d/%02d %02d:%02d:%02d.%03d" + , (int) info.m_year + , (int) info.m_month + , (int) info.m_day + , (int) info.m_hour + , (int) info.m_minute + , (int) info.m_second + , (int)(thisPtr->m_msTime % 1000)); + XsString_assign(result, 23, buffer); + } + else + XsString_assign(result, 23, "0000/00/00 00:00:00.000"); +} +#endif + +/*! \cond XS_INTERNAL */ +extern int64_t XsTime_utcToLocalValue; //!< Internal storage for UTC to local time correction (ms) +extern int64_t XsTime_localToUtcValue; //!< Internal storage for local time to UTC correction (ms) +/*! \endcond */ + +/*! \brief Convert the supplied time from (assumed) UTC to local time, using the system's local time zone knowledge + \param local The returned, converted time + \note Make sure to call XsTime_initializeTime() before calling this function as well as when changing time zones. +*/ +void XsTimeStamp_utcToLocalTime(struct XsTimeStamp const* thisPtr, struct XsTimeStamp* local) +{ + local->m_msTime = thisPtr->m_msTime + (thisPtr->m_msTime ? XsTime_utcToLocalValue : 0LL); +} + +/*! \brief Convert the supplied time from (assumed) local time to UTC, using the system's local time zone knowledge + \param utc The returned, converted time + \note Make sure to call XsTime_initializeTime() before calling this function as well as when changing time zones. +*/ +void XsTimeStamp_localToUtcTime(struct XsTimeStamp const* thisPtr, struct XsTimeStamp* utc) +{ + utc->m_msTime = thisPtr->m_msTime + (thisPtr->m_msTime ? XsTime_localToUtcValue : 0LL); +} + +/*! \brief Convert the supplied time from (assumed) UTC to local time, using the offset in \a info + \param local The returned, converted time + \param info The XsTimeInfo object containing the time offset to apply +*/ +void XsTimeStamp_utcToLocalTime2(struct XsTimeStamp const* thisPtr, struct XsTimeStamp* local, const struct XsTimeInfo* info) +{ + local->m_msTime = thisPtr->m_msTime - (thisPtr->m_msTime ? (int64_t) info->m_utcOffset * 60000 : 0LL); +} + +/*! \brief Convert the supplied time from (assumed) local time to UTC, using the offset in \a info + \param utc The returned, converted time + \param info The XsTimeInfo object containing the time offset to apply +*/ +void XsTimeStamp_localToUtcTime2(struct XsTimeStamp const* thisPtr, struct XsTimeStamp* utc, const struct XsTimeInfo* info) +{ + utc->m_msTime = thisPtr->m_msTime + (thisPtr->m_msTime ? (int64_t) info->m_utcOffset * 60000 : 0LL); +} + +/*! \brief Convert the supplied time from (assumed) UTC to local time, using the offset in \a utcOffset + \param local The returned, converted time + \param utcOffset The time offset to apply as defined in XsTimeInfo::m_utcOffset but in ms instead of minutes +*/ +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); +} + +/*! \brief Convert the supplied time from (assumed) local time to UTC, using the offset in \a utcOffset + \param utc The returned, converted time + \param utcOffset The time offset to apply as defined in XsTimeInfo::m_utcOffset but in ms instead of minutes +*/ +void XsTimeStamp_localToUtcTime_ms(struct XsTimeStamp const* thisPtr, struct XsTimeStamp* utc, int64_t utcOffset) +{ + utc->m_msTime = thisPtr->m_msTime + (thisPtr->m_msTime ? utcOffset : 0LL); +} + +/*! @} */ diff --git a/extern/xspublic/xstypes/xstimestamp.h b/extern/xspublic/xstypes/xstimestamp.h new file mode 100644 index 0000000..fa67ecb --- /dev/null +++ b/extern/xspublic/xstypes/xstimestamp.h @@ -0,0 +1,428 @@ + +// Copyright (c) 2003-2022 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 XSTIMESTAMP_H +#define XSTIMESTAMP_H + +#include "xstypesconfig.h" +#include "pstdint.h" + +#ifdef __cplusplus +#ifndef XSENS_NO_STL + #include "xsstring.h" +#endif +#include "xstimeinfo.h" +extern "C" { +#endif +#ifndef __cplusplus +#define XSTIMESTAMP_INITIALIZER { 0 } +#endif + +struct XsTimeStamp; +struct XsTimeInfo; +struct XsString; + +XSTYPES_DLL_API void XsTimeStamp_setMilliSecondTime(struct XsTimeStamp* thisPtr, int64_t t); +XSTYPES_DLL_API double XsTimeStamp_timeOfDay(const struct XsTimeStamp* thisPtr); +XSTYPES_DLL_API int64_t XsTimeStamp_secondTime(const struct XsTimeStamp* thisPtr); +XSTYPES_DLL_API int32_t XsTimeStamp_milliSecondPart(const struct XsTimeStamp* thisPtr); +XSTYPES_DLL_API int32_t XsTimeStamp_secondPart(const struct XsTimeStamp* thisPtr); +XSTYPES_DLL_API int32_t XsTimeStamp_minutePart(const struct XsTimeStamp* thisPtr); +XSTYPES_DLL_API int32_t XsTimeStamp_hourPart(const struct XsTimeStamp* thisPtr); +XSTYPES_DLL_API int64_t XsTimeStamp_now(struct XsTimeStamp* thisPtr); +XSTYPES_DLL_API int64_t XsTimeStamp_maxValue(void); + +XSTYPES_DLL_API int64_t XsTimeStamp_fromTimeInfo(struct XsTimeStamp* thisPtr, const struct XsTimeInfo* info); +XSTYPES_DLL_API void XsTimeStamp_toTimeInfo(struct XsTimeStamp const* thisPtr, struct XsTimeInfo* info); +#ifndef XSENS_NO_STL +XSTYPES_DLL_API void XsTimeStamp_toString(struct XsTimeStamp const* thisPtr, struct XsString* result); +#endif +XSTYPES_DLL_API void XsTimeStamp_utcToLocalTime(struct XsTimeStamp const* thisPtr, struct XsTimeStamp* local); +XSTYPES_DLL_API void XsTimeStamp_localToUtcTime(struct XsTimeStamp const* thisPtr, struct XsTimeStamp* utc); +XSTYPES_DLL_API void XsTimeStamp_utcToLocalTime2(struct XsTimeStamp const* thisPtr, struct XsTimeStamp* local, const struct XsTimeInfo* info); +XSTYPES_DLL_API void XsTimeStamp_localToUtcTime2(struct XsTimeStamp const* thisPtr, struct XsTimeStamp* utc, const struct XsTimeInfo* info); +XSTYPES_DLL_API void XsTimeStamp_utcToLocalTime_ms(struct XsTimeStamp const* thisPtr, struct XsTimeStamp* local, int64_t utcOffset); +XSTYPES_DLL_API void XsTimeStamp_localToUtcTime_ms(struct XsTimeStamp const* thisPtr, struct XsTimeStamp* utc, int64_t utcOffset); + +#ifdef __cplusplus +} // extern "C" +#endif + +/*! \struct XsTimeStamp + \brief Class for managing timestamps in a unified way. +*/ +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) {} + + /*! \brief Construct a timestamp with \a t as the time in milliseconds. */ + 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)) {} + + /*! \brief Construct a copy of \a other. */ + inline XsTimeStamp(const XsTimeStamp& other) : m_msTime(other.m_msTime) {} + + /*! \brief Construct from \a utc */ + inline XsTimeStamp(const XsTimeInfo& info) + { + XsTimeStamp_fromTimeInfo(this, &info); + } + + /*! \brief Convert this timestamp to UTC time */ + inline XsTimeInfo toTimeInfo() const + { + XsTimeInfo info; + XsTimeStamp_toTimeInfo(this, &info); + return info; + } + + /*! \brief Convert this timestamp to a XsTimeInfo object \a info */ + inline void toTimeInfo(XsTimeInfo& info) const + { + XsTimeStamp_toTimeInfo(this, &info); + } + +#ifndef XSENS_NO_STL + /*! \brief Convert this timestamp to a string in format YYYY/MM/DD hh:mm:ss.nnn */ + inline XsString toString() const + { + XsString result; + XsTimeStamp_toString(this, &result); + return result; + } +#endif + + /*! \brief Assign the contents of the \a other timestamp to this timestamp. */ + inline XsTimeStamp& operator = (const XsTimeStamp& other) + { + m_msTime = other.m_msTime; + return *this; + } + + /*! \brief Get the stored time as milliseconds. */ + inline int64_t msTime(void) const + { + return m_msTime; + } + + /*! \brief Set the stored time to \a t milliseconds. */ + inline void setMsTime(int64_t 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); + } + + /*! \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); + } + + /*! \brief Return the time as seconds */ + inline double secTime() const + { + return ((double)m_msTime) * 0.001; + } + + /*! \brief Set the time as seconds */ + inline void setSecTime(double t) + { + 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); + } + + /*! \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); + } + + /*! \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; + } + + /*! \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; + } + + /*! \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; + } + + /*! \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; + } + + /*! \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; + } + + /*! \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; + } + + /*! \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; + } + + /*! \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; + } + + /*! \brief Test if the given \a other is smaller than the current timestamp. */ + 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; + } + + /*! \brief Test if the given \a other is equal to the current timestamp. */ + inline bool operator == (int other) const + { + 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; + } + + /*! \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; + } + + /*! \brief Test if the given \a other is not equal to the current timestamp. */ + inline bool operator != (int other) const + { + 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; + } + + /*! \brief Returns the millisecond part of the time (in the range 0-999) */ + inline int32_t milliSecondPart() const + { + 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); + } + + /*! \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); + } + + /*! \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); + } + + /*! \brief Returns the current time in ms since the epoch (Jan 1st 1970) */ + inline static XsTimeStamp now() + { + XsTimeStamp tmp; + XsTimeStamp_now(&tmp); + return tmp; + } + + /*! \brief Returns the current time in ms since the epoch (Jan 1st 1970) */ + inline static int64_t nowMs() + { + XsTimeStamp tmp; + XsTimeStamp_now(&tmp); + return tmp.msTime(); + } + + /*! \brief Returns the difference between now() and the stored time value */ + inline int64_t elapsedToNow() const + { + return XsTimeStamp_now(0) - m_msTime; + } + + /*! \brief Returns the maximum possible value of an %XsTimeStamp */ + inline static XsTimeStamp maxValue() + { + return XsTimeStamp(int64_t(9223372036854775807LL)); //INT64_MAX + } + + /*! \brief Increment the timestamp by one ms, prefix */ + XsTimeStamp& operator++() + { + ++m_msTime; + return *this; + } + + /*! \brief Increment the timestamp by one ms, postfix */ + XsTimeStamp operator++(int) + { + return XsTimeStamp(m_msTime++); + } + + /*! \brief Decrement the timestamp by one ms, prefix */ + XsTimeStamp& operator--() + { + --m_msTime; + return *this; + } + + /*! \brief Decrement the timestamp by one ms, postfix */ + XsTimeStamp operator--(int) + { + 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 + \note Make sure to call XsTime_initializeTime() before calling this function as well as when changing time zones. + */ + XsTimeStamp utcToLocalTime() const + { + XsTimeStamp local; + XsTimeStamp_utcToLocalTime(this, &local); + return local; + } + + /*! \brief Convert the supplied time from (assumed) local time to UTC, using the system's local time zone knowledge + \returns The converted time + \note Make sure to call XsTime_initializeTime() before calling this function as well as when changing time zones. + */ + XsTimeStamp localToUtcTime() const + { + XsTimeStamp utc; + XsTimeStamp_localToUtcTime(this, &utc); + return utc; + } + + /*! \brief Convert the supplied time from (assumed) UTC to local time, using the offset in \a info + \param info The XsTimeInfo object containing the time offset to apply + \returns The converted time + */ + XsTimeStamp utcToLocalTime(XsTimeInfo const& info) const + { + XsTimeStamp local; + XsTimeStamp_utcToLocalTime2(this, &local, &info); + return local; + } + + /*! \brief Convert the supplied time from (assumed) local time to UTC, using the offset in \a info + \param info The XsTimeInfo object containing the time offset to apply + \returns The converted time + */ + XsTimeStamp localToUtcTime(XsTimeInfo const& info) const + { + XsTimeStamp utc; + XsTimeStamp_localToUtcTime2(this, &utc, &info); + return utc; + } + + /*! \brief Convert the supplied time from (assumed) UTC to local time, using the offset in \a info + \param utcOffset The time offset to apply + \returns The converted time + */ + XsTimeStamp utcToLocalTime(int64_t utcOffset) const + { + XsTimeStamp local; + XsTimeStamp_utcToLocalTime_ms(this, &local, utcOffset); + return local; + } + + /*! \brief Convert the supplied time from (assumed) local time to UTC, using the offset in \a info + \param utcOffset The time offset to apply + \returns The converted time + */ + XsTimeStamp localToUtcTime(int64_t utcOffset) const + { + XsTimeStamp utc; + XsTimeStamp_localToUtcTime_ms(this, &utc, utcOffset); + return utc; + } +private: +#endif + + int64_t m_msTime; //!< The timestamp value +}; + +typedef struct XsTimeStamp XsTimeStamp; + +#endif diff --git a/extern/xspublic/xstypes/xstriggerindicationdata.c b/extern/xspublic/xstypes/xstriggerindicationdata.c new file mode 100644 index 0000000..f18630e --- /dev/null +++ b/extern/xspublic/xstypes/xstriggerindicationdata.c @@ -0,0 +1,52 @@ + +// Copyright (c) 2003-2022 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 "xstriggerindicationdata.h" +#include + +/*! \addtogroup cinterface C Interface + @{ +*/ + +/*! \brief Destroy the %XsTriggerIndicationData object */ +void XsTriggerIndicationData_destruct(XsTriggerIndicationData* thisPtr) +{ + memset(thisPtr, 0, sizeof(XsTriggerIndicationData)); +} + +/*! \brief Returns true if the object is valid (line and polarity may not be 0) */ +int XsTriggerIndicationData_valid(const XsTriggerIndicationData* thisPtr) +{ + return thisPtr->m_line != 0 && thisPtr->m_polarity != 0; +} + +/*! @} */ diff --git a/extern/xspublic/xstypes/xstriggerindicationdata.h b/extern/xspublic/xstypes/xstriggerindicationdata.h new file mode 100644 index 0000000..bc9bd14 --- /dev/null +++ b/extern/xspublic/xstypes/xstriggerindicationdata.h @@ -0,0 +1,100 @@ + +// Copyright (c) 2003-2022 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 XSTRIGGERINDICATIONDATA_H +#define XSTRIGGERINDICATIONDATA_H + +#include "xstypesconfig.h" +#include "pstdint.h" + +#ifdef __cplusplus +extern "C" { +#endif +#ifndef __cplusplus +#define XSTRIGGERINDICATIONDATA_INITIALIZER { 0, 0, 0, 0 } +#endif + +struct XsTriggerIndicationData; + +XSTYPES_DLL_API void XsTriggerIndicationData_destruct(struct XsTriggerIndicationData* thisPtr); +XSTYPES_DLL_API int XsTriggerIndicationData_valid(const struct XsTriggerIndicationData* thisPtr); + +#ifdef __cplusplus +} // extern "C" +#endif + + +/*! \brief Data for a trigger indication message */ +struct XsTriggerIndicationData +{ + uint8_t m_line; //!< The line number + uint8_t m_polarity; //!< The polarity + uint32_t m_timestamp; //!< The timestamp + uint16_t m_frameNumber; //!< The frame number + +#ifdef __cplusplus + /*! Constructor + \param[in] line Line + \param[in] polarity Polarity + \param[in] timestamp Timestamp + \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) + {} + + /*! \brief \copybrief XsTriggerIndicationData_destruct */ + inline void clear() + { + XsTriggerIndicationData_destruct(this); + } + + /*! \brief \copybrief XsTriggerIndicationData_valid */ + inline bool valid() const + { + return 0 != XsTriggerIndicationData_valid(this); + } + + /*! \brief Returns true if all fields of this and \a other are exactly identical */ + 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; + } +#endif +}; + +typedef struct XsTriggerIndicationData XsTriggerIndicationData; + +#endif diff --git a/extern/xspublic/xstypes/xstypedefs.c b/extern/xspublic/xstypes/xstypedefs.c new file mode 100644 index 0000000..a938226 --- /dev/null +++ b/extern/xspublic/xstypes/xstypedefs.c @@ -0,0 +1,70 @@ + +// Copyright (c) 2003-2022 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 "xstypedefs.h" +#include + +/*! \addtogroup cinterface C Interface + @{ +*/ + +/*! \brief Convert the %XsDataFlags to a human readable string + \param f The flags to translate + \returns A pointer to a statically allocated memory buffer. Do not free this buffer. + \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) +{ + static char rv[4 * 20]; + if (f == XSDF_None) + return "XSDF_None"; + + rv[0] = 0; + if (f & XSDF_Managed) + strcpy(rv, "XSDF_Managed"); + if (f & XSDF_FixedSize) + { + if (rv[0]) + strcat(rv, " | "); + strcat(rv, "XSDF_FixedSize"); + } + if (f & XSDF_Empty) + { + if (rv[0]) + strcat(rv, " | "); + strcat(rv, "XSDF_Empty"); + } + return rv; +} + +/*! @} */ diff --git a/extern/xspublic/xstypes/xstypedefs.h b/extern/xspublic/xstypes/xstypedefs.h new file mode 100644 index 0000000..447902d --- /dev/null +++ b/extern/xspublic/xstypes/xstypedefs.h @@ -0,0 +1,193 @@ + +// Copyright (c) 2003-2022 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 XSTYPEDEFS_H +#define XSTYPEDEFS_H + +#include "xstypesconfig.h" +#include "pstdint.h" + +#ifndef XSENS_SINGLE_PRECISION + #include + typedef double XsReal; //!< Defines the floating point type used by the Xsens libraries + typedef size_t XsSize; //!< XsSize must be unsigned number! + typedef ptrdiff_t XsSizeSigned; //!< signed variant of XsSize + #define XSREAL_ALLOWS_MEMCPY 1 + #ifndef PRINTF_SIZET_MODIFIER + #if defined(XSENS_64BIT) + #if defined(__APPLE__) + #define PRINTF_SIZET_MODIFIER "l" + #else + #define PRINTF_SIZET_MODIFIER PRINTF_INT64_MODIFIER + #endif + #else + #if defined(__APPLE__) + #define PRINTF_SIZET_MODIFIER "l" + #else + #define PRINTF_SIZET_MODIFIER PRINTF_INT32_MODIFIER + #endif + #endif + #endif // PRINTF_SIZET_MODIFIER +#else + typedef float XsReal; //!< Defines the floating point type used by the Xsens libraries + typedef unsigned int XsSize; //!< XsSize must be unsigned number! + typedef int XsSizeSigned; //!< signed variant of XsSize + #define XSREAL_ALLOWS_MEMCPY 1 + #define PRINTF_SIZET_MODIFIER "" +#endif // XSENS_SINGLE_PRECISION + + +/*! \addtogroup enums Global enumerations + @{ +*/ +/*! \brief These flags define the behaviour of data contained by Xsens data structures + \details Normally, the user should never need to use these directly. +*/ +enum XsDataFlags +{ + XSDF_None = 0, //!< No flag set + XSDF_Managed = 1, //!< The contained data should be managed (freed) by the object, when false, the object assumes the memory is freed by some other process after its destruction + XSDF_FixedSize = 2, //!< The contained data points to a fixed-size buffer, this allows creation of dynamic objects on the stack without malloc/free overhead. + XSDF_Empty = 4, //!< The object contains undefined data / should be considered empty. Usually only relevant when XSDF_FixedSize is also set, as otherwise the data pointer will be NULL and empty-ness is implicit. + XSDF_BadAlloc = 8, //!< The last memory allocation in the object failed, the contents are now erased +}; +/*! @} */ +typedef enum XsDataFlags XsDataFlags; + +#ifdef __cplusplus +extern "C" { +#endif + +XSTYPES_DLL_API const char* XsDataFlags_toString(XsDataFlags f); + +#ifdef __cplusplus +} // extern "C" + +/*! \brief boolean xor function, since C++ does not provide this and operator ^ is not guaranteed to work */ +inline bool xorBool(bool a, bool b) +{ + return (a && !b) || (b && !a); +} + +/*! \brief \copybrief XsDataFlags_toString \sa XsDataFlags_toString */ +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 + typedef int BOOL; + #endif + + #ifndef TRUE + #define TRUE (1) + #endif + + #ifndef FALSE + #define FALSE (0) + #endif + #define XSFALLTHROUGH /* fallthrough */ + #define XSNORETURN /* noreturn */ + #define XSDEPRECATED /* deprecated */ +#else + #if __cplusplus >= 201703L + #define XSFALLTHROUGH [[fallthrough]] /* FALLTHRU fallthrough */ + #define XSNORETURN [[noreturn]] /* noreturn */ + #define XSDEPRECATED [[deprecated]] /* deprecated */ + #else + #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/extern/xspublic/xstypes/xstypesconfig.h b/extern/xspublic/xstypes/xstypesconfig.h new file mode 100644 index 0000000..adc25fd --- /dev/null +++ b/extern/xspublic/xstypes/xstypesconfig.h @@ -0,0 +1,305 @@ + +// Copyright (c) 2003-2022 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. +// + +#define XSTYPES_DLL_API +#define XSNOEXPORT +#define XSENS_NO_AUTOLIB +// include this file in Visual Studio using C/C++->Advanced->Force Includes (the /FI option) +#ifndef XSTYPES_CONFIG_H +#define XSTYPES_CONFIG_H + +////////////////////////////////////////////////// +// generic preprocessor defines + +//http://support.microsoft.com/kb/155196 +#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 + // 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 +#endif +#if !defined(_WIN32) && defined(_MSC_VER) && !defined(_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 +#endif +#ifndef _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 +#else + #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 +#else + #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 +#else + #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 +#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) + //// Debug or RelWithDeb build + #if defined(XSENS_RELEASE) + //// RelWithDeb build + #else + //// Debug build + #endif + #else + //// Release build + #endif +*/ + +#if defined(XSENS_DEBUG) + //// Debug or RelWithDeb build + #ifndef XSENS_NO_STL + #define XSENS_USE_DEBUG_COUNTERS + #endif + + #if !defined(XSENS_RELEASE) + //// Debug build + #if !defined(QT_DEBUG) && !defined(QT_NO_DEBUG) + #define QT_DEBUG 1 + #endif + #ifndef XSENS_PFCONF + #ifdef __GNUC__ + #define XSENS_PFCONF "" + #else + #define XSENS_PFCONF "d" + #endif + #endif + #else + //// RelWithDeb build + #if !defined(QT_DEBUG) && !defined(QT_NO_DEBUG) + #define QT_NO_DEBUG 1 + #endif + #ifndef XSENS_PFCONF + #ifdef __GNUC__ + #define XSENS_PFCONF "" + #else + #define XSENS_PFCONF "rd" + #endif + #endif + #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 + #ifndef NDEBUG + #ifndef KEEP_ASSERTS + #define NDEBUG // make sure assertions and other debug options are compiled away by MSVC + #endif + #endif + #ifndef XSENS_PFCONF + #define XSENS_PFCONF "" + #endif +#endif + +#ifndef XSENS_PFFULL + #define XSENS_PFFULL XSENS_PFBITS XSENS_PFCONF +#endif + +////////////////////////////////////////////////// +// more generic preprocessor defines +// required for gnu c++ compiler versions due to difference in attribute declarations +#if defined(__AVR32__) + #define __cdecl + #define __stdcall +#elif defined(_ADI_COMPILER) + #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 +#elif defined(IAR_ARM_CM3_NAMESPACE) + #define __cdecl + #define __stdcall + #define XSENS_32BIT +#endif + +////////////////////////////////////////////////// +// generic preprocessor defines + +// use XSENS_32BIT and XSENS_64BIT to check for 32/64 bit builds in your application +// 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 + #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 + +#include + +// since this is (almost) always required and it does not conflict with pstdint, include the file here +//#define XSENS_SINGLE_PRECISION // enable for single precision +#include "xstypedefs.h" + +#ifndef XSNOCOMEXPORT + #define XSNOCOMEXPORT +#endif + +#ifdef XSENS_DEBUG + #define XSDEBUGLINE(a) a + #define XSNODEBUGLINE(...) +#else + #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 +#endif + +#endif diff --git a/extern/xspublic/xstypes/xstypesdynlib.c b/extern/xspublic/xstypes/xstypesdynlib.c new file mode 100644 index 0000000..b7d9d1c --- /dev/null +++ b/extern/xspublic/xstypes/xstypesdynlib.c @@ -0,0 +1,136 @@ + +// Copyright (c) 2003-2022 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. +// + +#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 _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_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" +#include "xstime.h" +#include "xsstring.h" + +#ifdef _MANAGED + #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))) +#endif +so_init(void) +{ + XsTime_initializeTime(); +} + +void +#ifdef __GNUC__ + __attribute__((destructor)) +#endif +so_fini(void) +{ +} + +#ifdef _WIN32 +HMODULE g_hModule = 0; +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; + } + return TRUE; +} +#endif + +/*! \brief Return the path that the xsensdeviceapi.dll file is currently running from + \param path Storage for the path + \note This is a windows-only function +*/ +XSTYPES_DLL_API void xstypesPath(XsString* path) +{ +#ifdef XSENS_WINDOWS + wchar_t filename[1024] = L""; + if (!path) + return; + + if (!GetModuleFileNameW(g_hModule, filename, 1024)) + XsString_destruct(path); + else + XsString_assignWCharArray(path, filename); +#else + if (!path) + return; + XsString_destruct(path); +#endif +} +#ifdef _MANAGED + #pragma managed(pop) +#endif diff --git a/extern/xspublic/xstypes/xstypesdynlib.h b/extern/xspublic/xstypes/xstypesdynlib.h new file mode 100644 index 0000000..e077d5b --- /dev/null +++ b/extern/xspublic/xstypes/xstypesdynlib.h @@ -0,0 +1,50 @@ + +// Copyright (c) 2003-2022 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 XSTYPESDYNLIB_H +#define XSTYPESDYNLIB_H + +#include "xstypesconfig.h" + +struct XsString; + +#ifdef __cplusplus +extern "C" { +#endif + +XSTYPES_DLL_API void xstypesPath(struct XsString* path); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/extern/xspublic/xstypes/xstypesinfo.h b/extern/xspublic/xstypes/xstypesinfo.h new file mode 100644 index 0000000..20a909c --- /dev/null +++ b/extern/xspublic/xstypes/xstypesinfo.h @@ -0,0 +1,57 @@ + +// Copyright (c) 2003-2022 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 XSTYPESINFO_H +#define XSTYPESINFO_H + +#include "xstypesconfig.h" + +struct XsVersion; + +#ifdef __cplusplus + #include "xsversion.h" + extern "C" +#endif +XSTYPES_DLL_API void xsTypesVersion(struct XsVersion* version); +#define XsTypesInfoGetVersion(a) xsTypesVersion(a) + +#ifdef __cplusplus +/*! \brief Return the version information of the XsTypes library */ +inline static XsVersion xsTypesVersion() +{ + XsVersion v; + xsTypesVersion(&v); + return v; +} +#endif + +#endif diff --git a/extern/xspublic/xstypes/xsushortvector.h b/extern/xspublic/xstypes/xsushortvector.h new file mode 100644 index 0000000..45377ad --- /dev/null +++ b/extern/xspublic/xstypes/xsushortvector.h @@ -0,0 +1,139 @@ + +// Copyright (c) 2003-2022 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 XSUSHORTVECTOR_H +#define XSUSHORTVECTOR_H + +#ifdef __cplusplus + #include "xstypedefs.h" +#endif +#ifndef __cplusplus + #define XSUSHORTVECTOR_INITIALIZER {{0,0,0}} +#endif + + +/*! \brief A vector containing 3 short values. */ +struct XsUShortVector +{ +#ifdef __cplusplus + //! \brief Constructor that creates the vector with all components set to 0 + inline XsUShortVector() + { + m_data[0] = 0; + m_data[1] = 0; + m_data[2] = 0; + } + + //! \brief Constructor that creates the vector with all components set to given values \a v1 \a v2 and \a v3 + inline XsUShortVector(unsigned short v1, unsigned short v2, unsigned short v3) + { + m_data[0] = v1; + m_data[1] = v2; + m_data[2] = v3; + } + + //! \brief Constructor that creates the vector with all components set to values in array \a a + inline explicit XsUShortVector(const unsigned short* a) + { + m_data[0] = a[0]; + m_data[1] = a[1]; + m_data[2] = a[2]; + } + + //! \brief Constructor that creates the vector and initializes it with data from the \a other vector + inline XsUShortVector(const XsUShortVector& other) + { + m_data[0] = other.m_data[0]; + m_data[1] = other.m_data[1]; + m_data[2] = other.m_data[2]; + } + + //! \brief Assignment operator copies the data from the \a other vector to this vector + inline XsUShortVector& operator = (const XsUShortVector& other) + { + if (this != &other) + { + m_data[0] = other.m_data[0]; + m_data[1] = other.m_data[1]; + m_data[2] = other.m_data[2]; + } + return *this; + } + + //! \brief Comparison operator, returns true if the contents of the \a other vector match those of this vector + 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]; + } + + //! \brief Return the size of the vector (always 3) + inline XsSize size() const + { + return 3; + } + + //! \brief Return a value from the vector (needed to allow generated C# access to these elements) + inline unsigned short at(int index) const + { + return m_data[index]; + } + + //! \brief Returns the \a index'th item in the vector + inline unsigned short operator[](XsSize index) const + { + assert(index < 3); + return m_data[index]; + } + + //! \brief Returns a reference the \a index'th item in the vector + inline unsigned short& operator[](XsSize index) + { + assert(index < 3); + return m_data[index]; + } + + //! \brief Returns the start of the internal data buffer + inline unsigned short const* data() const + { + return m_data; + } +private: +#endif + + unsigned short m_data[3]; //!< vector component storage +}; + +typedef struct XsUShortVector XsUShortVector; + +#endif diff --git a/extern/xspublic/xstypes/xsutctime.c b/extern/xspublic/xstypes/xsutctime.c new file mode 100644 index 0000000..3f18d23 --- /dev/null +++ b/extern/xspublic/xstypes/xsutctime.c @@ -0,0 +1,47 @@ + +// Copyright (c) 2003-2022 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 "xsutctime.h" + +/*! \addtogroup cinterface C Interface + @{ +*/ + +/*! \relates XsTimeInfo + \brief Returns the current time in UTC date time format +*/ +void XsUtcTime_currentTime(struct XsTimeInfo* thisPtr) +{ + XsTimeInfo_currentTime(thisPtr); +} + +/*! @} */ diff --git a/extern/xspublic/xstypes/xsutctime.h b/extern/xspublic/xstypes/xsutctime.h new file mode 100644 index 0000000..89799b7 --- /dev/null +++ b/extern/xspublic/xstypes/xsutctime.h @@ -0,0 +1,53 @@ + +// Copyright (c) 2003-2022 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 XSUTCTIME_H +#define XSUTCTIME_H + +#include "xstimeinfo.h" + +#ifdef __cplusplus +extern "C" { +#endif +#ifndef __cplusplus +#define XSUTCTIME_INITIALIZER { 0, 0 ,0, 0, 0, 0, 0, 0, 0} +#endif + +// for backwards compatibility only, use XsTimeInfo instead +#define XsUtcTime XsTimeInfo +XSTYPES_DLL_API void XsUtcTime_currentTime(struct XsUtcTime* now); + +#ifdef __cplusplus +} // extern "C" +#endif + +#endif diff --git a/extern/xspublic/xstypes/xsvector.c b/extern/xspublic/xstypes/xsvector.c new file mode 100644 index 0000000..53f2640 --- /dev/null +++ b/extern/xspublic/xstypes/xsvector.c @@ -0,0 +1,312 @@ + +// Copyright (c) 2003-2022 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 "xsvector.h" +#include +#include +#include "xsdebugcounters.h" +#include +#include "xsmalloc.h" +#include "xsquaternion.h" +#include "xscopy.h" +#include +#include "xsfloatmath.h" + +#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 +*/ + +/*! \addtogroup cinterface C Interface + @{ +*/ + +//! \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); + *((XsReal**) &thisPtr->m_data) = buffer; + *((XsSize*) &thisPtr->m_size) = sz; + *((XsSize*) &thisPtr->m_flags) = flags; +} + +//! \relates XsVector \brief Initialize the %XsVector using \a sz number of items from \a src +void XsVector_construct(XsVector* thisPtr, XsSize sz, const XsReal* src) +{ + if (sz) + { + // init to size + XsReal* data = (XsReal*) xsMathMalloc(sz * sizeof(XsReal)); + assert(data); + *((XsReal**) &thisPtr->m_data) = data; + XsVector_incAllocCount(); + } + else + *((XsReal**) &thisPtr->m_data) = 0; + *((XsSize*) &thisPtr->m_flags) = XSDF_Managed; + *((XsSize*) &thisPtr->m_size) = sz; + if (src && sz) + memcpy(thisPtr->m_data, src, sz * sizeof(XsReal)); +} + +/*! \relates XsVector + \brief Initialize the XsVector using \a sz number of items from \a src + \param sz The desired size of the vector + \param src 0 or a pointer to a buffer containing \a sz items to copy into the %XsVector +*/ +void XsVector_assign(XsVector* thisPtr, XsSize sz, const XsReal* src) +{ + if (thisPtr->m_flags & XSDF_FixedSize) + { + if (sz == 0) + { + *((XsSize*) &thisPtr->m_flags) |= XSDF_Empty; + return; + } + assert(sz == thisPtr->m_size); + *((XsSize*) &thisPtr->m_flags) &= ~XSDF_Empty; + } + + if (sz > thisPtr->m_size || sz == 0) + { + XsVector_destruct(thisPtr); + if (sz) + { + // init to size + XsReal* data = (XsReal*) xsMathMalloc(sz * sizeof(XsReal)); + assert(data); + *((XsReal**) &thisPtr->m_data) = data; + *((XsSize*) &thisPtr->m_flags) = XSDF_Managed; + XsVector_incAllocCount(); + } + } + *((XsSize*) &thisPtr->m_size) = sz; + if (src && sz) + memcpy(thisPtr->m_data, src, sz * sizeof(XsReal)); +} + +//! \relates XsVector \brief Release and clear the contents of the vector +void XsVector_destruct(XsVector* thisPtr) +{ + if (thisPtr->m_data && (thisPtr->m_flags & XSDF_Managed)) + { + // clear contents + xsMathFree((void*) thisPtr->m_data); + XsVector_incFreeCount(); + } + // init to 0 + if (!(thisPtr->m_flags & XSDF_FixedSize)) + { + *((XsReal**) &thisPtr->m_data) = 0; + *((XsSize*) &thisPtr->m_size) = 0; + *((XsSize*) &thisPtr->m_flags) = 0; + } + else + *((XsSize*) &thisPtr->m_flags) |= XSDF_Empty; +} + +//! \relates XsVector \brief Copy the contents of the %XsVector to \a copy +void XsVector_copy(XsVector* copy, XsVector const* src) +{ + if (copy == src) + return; + + if (src->m_flags & XSDF_Empty) + XsVector_destruct(copy); + else + XsVector_assign(copy, src->m_size, src->m_data); +} + +/*! \relates XsVector + \brief Compute and return the dot product of XsVectors \a a and \a b +*/ +XsReal XsVector_dotProduct(const XsVector* a, const XsVector* b) +{ + XsSize i; + 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]; + return r; +} + +/*! \relates XsVector + \brief Compute and return the cartesian length + \returns The cartesian length (square root of the dot product) of the vector +*/ +XsReal XsVector_cartesianLength(const XsVector* thisPtr) +{ + return sqrt(XsVector_dotProduct(thisPtr, thisPtr)); +} + +/*! \relates XsVector + \brief Normalize the vector +*/ +void XsVector_normalize(XsVector* thisPtr) +{ + XsReal length = XsVector_cartesianLength(thisPtr); + + if (length < XsMath_tinyValue) + { + XsVector_setZero(thisPtr); + return; + } + + XsVector_multiplyScalar(thisPtr, XsMath_one / length, thisPtr); +} + +//! \relates XsVector \brief Sets all elements of the %XsVector to 0 +void XsVector_setZero(XsVector* thisPtr) +{ +#if XSREAL_ALLOWS_MEMCPY + memset(thisPtr->m_data, 0, sizeof(XsReal)*thisPtr->m_size); +#else + XsSize i; + for (i = 0; i < thisPtr->m_size; ++i) + thisPtr->m_data[i] = XsMath_zero; +#endif +} + +//! \relates XsVector \brief Sets all elements of the %XsVector to \a value +void XsVector_fill(struct XsVector* thisPtr, XsReal value) +{ + XsSize i; + for (i = 0; i < thisPtr->m_size; ++i) + thisPtr->m_data[i] = value; +} + +//! \relates XsVector \brief Returns a non-zero value if the XsVector does not contain any values +int XsVector_empty(const XsVector* thisPtr) +{ + return (thisPtr->m_size == 0) || (thisPtr->m_flags & XSDF_Empty); +} + +//! \relates XsVector \brief Multiplies all values in this XsVector by \a scalar and puts the result in XsVector \a dest +void XsVector_multiplyScalar(const XsVector* thisPtr, XsReal scalar, XsVector* dest) +{ + XsSize i; + XsVector_assign(dest, thisPtr->m_size, 0); + for (i = 0; i < thisPtr->m_size; ++i) + dest->m_data[i] = thisPtr->m_data[i] * scalar; +} + +/*! \relates XsVector + \brief Get an effective angular velocity from the quaternion, which must represent a delta angle. + \param deltaT The length of the time interval over which \a quat was integrated in seconds + \param quat The orientation increment to convert to an angular velocity + \returns A vector containing the effective angular velocity in radians around each axis. +*/ +void XsVector_angularVelocityFromQuaternion(XsVector* thisPtr, XsReal deltaT, const XsQuaternion* quat) +{ + XsReal a; + if (XsQuaternion_empty(quat)) + { + XsVector_destruct(thisPtr); + return; + } + + 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); +} + +/*! \relates XsVector \brief Swap the contents of \a a and \a b + \details This function swaps the internal buffers so no actual data is moved around. For unmanaged + data an elementwise swap is done, but only if the vectors are the same size. + \param a Object whose contents will be placed in \a b + \param b Object whose contents will be placed in \a a +*/ +void XsVector_swap(XsVector* a, XsVector* b) +{ +#ifdef __ICCARM__ +#pragma diag_suppress=Pe370 +#endif + XSLISTSWAP3(XsReal, XsVector, realSwap) +} + +/*! \relates XsVector + \brief Returns non-zero when the two vectors are identical + \param a Vector to compare against \a b + \param b Vector to compare against \a a + \returns non-zero when the vectors are identical +*/ +int XsVector_equal(const struct XsVector* a, const struct XsVector* b) +{ + if (a == b) + return 1; + if (!a || !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 +} + +/*! \relates XsVector + \brief Returns non-zero if the two vectors are equal within \a epsilon + + \param thatPtr the vector to compare with + \param epsilon the maximum difference between values + \returns non-zero when the vectors are equal within \a epsilon +*/ +int XsVector_compare(const struct XsVector* thisPtr, const struct XsVector* thatPtr, XsReal epsilon) +{ + XsSize i; + + if (thisPtr == thatPtr) + return 1; + if (!thisPtr || !thatPtr) + 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; + + for (i = 0; i < thisPtr->m_size; ++i) + if (fabs(thisPtr->m_data[i] - thatPtr->m_data[i]) > epsilon) + return 0; + + return 1; +} + +/*! @} */ diff --git a/extern/xspublic/xstypes/xsvector.h b/extern/xspublic/xstypes/xsvector.h new file mode 100644 index 0000000..4722fde --- /dev/null +++ b/extern/xspublic/xstypes/xsvector.h @@ -0,0 +1,465 @@ + +// Copyright (c) 2003-2022 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 XSVECTOR_H +#define XSVECTOR_H + +#include "xsmath.h" +#include +#include // memcpy + +struct XsVector; +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 +#define XSVECTOR_INITIALIZER { NULL, 0, 0 } +typedef struct XsVector XsVector; +#endif + +XSTYPES_DLL_API void XsVector_ref(XsVector* thisPtr, XsSize sz, XsReal* buffer, XsDataFlags flags); +XSTYPES_DLL_API void XsVector_construct(XsVector* thisPtr, XsSize sz, const XsReal* src); +XSTYPES_DLL_API void XsVector_assign(XsVector* thisPtr, XsSize sz, const XsReal* src); +XSTYPES_DLL_API void XsVector_destruct(XsVector* thisPtr); +XSTYPES_DLL_API void XsVector_copy(XsVector* copy, XsVector const* src); +XSTYPES_DLL_API XsReal XsVector_dotProduct(const XsVector* a, const XsVector* b); +XSTYPES_DLL_API XsReal XsVector_cartesianLength(const XsVector* thisPtr); +XSTYPES_DLL_API void XsVector_normalize(XsVector* thisPtr); +XSTYPES_DLL_API void XsVector_setZero(XsVector* thisPtr); +XSTYPES_DLL_API int XsVector_empty(const XsVector* thisPtr); +XSTYPES_DLL_API void XsVector_multiplyScalar(const XsVector* thisPtr, XsReal scalar, XsVector* dest); +XSTYPES_DLL_API void XsVector_angularVelocityFromQuaternion(XsVector* thisPtr, XsReal deltaT, const struct XsQuaternion* quat); +XSTYPES_DLL_API void XsVector_swap(XsVector* a, XsVector* b); +XSTYPES_DLL_API void XsVector_fill(XsVector* thisPtr, XsReal value); +XSTYPES_DLL_API int XsVector_equal(const XsVector* thisPtr, const XsVector* thatPtr); +XSTYPES_DLL_API int XsVector_compare(const XsVector* thisPtr, const XsVector* thatPtr, XsReal epsilon); + +#ifdef __cplusplus +} // extern "C" +#endif +#ifndef XSENS_NO_PACK + #pragma pack(push,1) +#endif +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 + +#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) + : m_data(0) + , m_size(0) + , m_flags(0) + { + if (sz) + XsVector_construct(this, sz, src); + } + + //! \brief Initialize a vector using the supplied \a other vector + inline XsVector(const XsVector& other) + : m_data(0) + , m_size(0) + , m_flags(0) + { + XsVector_copy(this, &other); + } + + //! \brief Initialize a vector that references the supplied data + inline explicit XsVector(XsReal* ref, XsSize sz, XsDataFlags flags_ = XSDF_None) + : m_data(ref) + , m_size(sz) + , m_flags((XsSize) flags_) + { + } + + //! \brief Initialize a vector that references the supplied data + inline explicit XsVector(const XsVector& other, XsReal* ref, XsSize sz, XsDataFlags flags_ = XSDF_None) + : m_data(ref) + , m_size(sz) + , m_flags((XsSize) flags_) + { + XsVector_copy(this, &other); + } + + //! \copydoc XsVector_angularVelocityFromQuaternion + inline explicit XsVector(const XsQuaternion& quat, XsReal deltaT) + : m_data(0) + , m_size(0) + , m_flags(0) + { + XsVector_angularVelocityFromQuaternion(this, deltaT, &quat); + } + +#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) + , m_size(0) + , m_flags(0) + { + if (!(other.m_flags & XSDF_Managed)) + XsVector_copy(this, &other); + else + XsVector_swap(this, &other); + } +#endif + + //! \brief Assignment operator. Copies from \a other into this + inline XsVector& operator=(const XsVector& other) + { + XsVector_copy(this, &other); + return *this; + } + + //! \copydoc XsVector_destruct + inline ~XsVector() + { + XsVector_destruct(this); + } + + //! \copydoc XsVector_assign + inline void assign(XsSize sz, const XsReal* src) + { + XsVector_assign(this, sz, src); + } + + /*! \brief Sets the size of the XsVector to \a sz items + \param sz The desired size of the vector + \sa XsVector_assign + */ + inline void setSize(XsSize sz) + { + XsVector_assign(this, sz, 0); + } + + //! \brief Returns the number of elements in the vector + inline XsSize size() const + { + return m_size; + } + + //! \brief Return a const pointer to the data + inline const XsReal* data() const + { + return m_data; + } + + //! \brief Multiply the vector by \a scalar and return the result + inline XsVector operator * (XsReal scalar) const + { + XsVector v(m_size); + for (XsSize i = 0; i < m_size; ++i) + v.m_data[i] = m_data[i] * scalar; + return v; + } + + //! \brief Multiply the vector by \a scalar and store the result in this vector + inline void operator *=(XsReal scalar) + { + for (XsSize i = 0; i < m_size; ++i) + m_data[i] *= scalar; + } + + //! \brief Returns a reference to the \a index'th item in the vector + inline XsReal& at(XsSize index) + { + assert(index < m_size); + return m_data[index]; + } + + //! \brief Returns a const reference to the \a index'th item in the vector + inline const XsReal& at(XsSize index) const + { + assert(index < m_size); + return m_data[index]; + } + + //! \brief Returns the \a index'th item in the vector + inline XsReal value(XsSize index) const + { + assert(index < m_size); + return m_data[index]; + } + + //! \brief Sets the \a index'th item in the vector + inline void setValue(XsSize index, XsReal val) + { + assert(index < m_size); + 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 + { + assert(index < m_size); + return m_data[index]; + } + + //! \brief Returns a reference the \a index'th item in the vector + inline XsReal& operator[](XsSize index) + { + assert(index < m_size); + return m_data[index]; + } +#endif + + //! \brief \copybrief XsVector_dotProduct + inline XsReal dotProduct(const XsVector& v) const + { + return XsVector_dotProduct(this, &v); + } + + //! \copydoc XsVector_cartesianLength + inline XsReal cartesianLength() const + { + return XsVector_cartesianLength(this); + } + + //! \copydoc XsVector_normalize + inline void normalize() + { + XsVector_normalize(this); + } + + //! \brief \copybrief XsVector_setZero + inline void setZero() + { + return XsVector_setZero(this); + } + + //! \brief \copybrief XsVector_empty + inline bool empty() const + { + return 0 != XsVector_empty(this); + } + + //! \copydoc XsVector_angularVelocityFromQuaternion + inline XsVector& angularVelocityFromQuaternion(const XsQuaternion& quat, XsReal deltaT) + { + XsVector_angularVelocityFromQuaternion(this, deltaT, &quat); + return *this; + } + + //! \brief Return \e this - \a sub + XsVector operator-(const XsVector& sub) const + { + assert(m_size == sub.m_size); + XsVector tmp(m_size); + for (XsSize i = 0; i < m_size; ++i) + tmp[i] = m_data[i] - sub.m_data[i]; + return tmp; + } + + //! \brief Return \e this + \a sub + XsVector operator+(const XsVector& sub) const + { + assert(m_size == sub.m_size); + XsVector tmp(m_size); + for (XsSize i = 0; i < m_size; ++i) + tmp[i] = m_data[i] + sub.m_data[i]; + return tmp; + } + + //! \brief Return \e this - \a sub after updating this with the new values + XsVector operator-=(const XsVector& sub) + { + assert(m_size == sub.m_size); + for (XsSize i = 0; i < m_size; ++i) + m_data[i] -= sub.m_data[i]; + return *this; + } + + //! \brief Return \e this + \a sub after updating this with the new values + XsVector operator+=(const XsVector& sub) + { + assert(m_size == sub.m_size); + for (XsSize i = 0; i < m_size; ++i) + m_data[i] += sub.m_data[i]; + return *this; + } + + //! \brief Return true when the values in this vector are exactly (to the last bit) equal to \a other + bool operator==(const XsVector& other) const + { + return 0 != XsVector_equal(this, &other); + } + + /*! \brief Return true when the values in this vector are equal within \a epsilon + \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 + { + return 0 != XsVector_compare(this, &other, epsilon); + } + +#ifndef XSENS_NO_STL + //! \brief Returns the XsVector as a std::vector of XsReal + inline std::vector toVector() const + { + std::vector tmp(m_size); + if (m_size) + memcpy(&tmp[0], m_data, m_size * sizeof(XsReal)); + return tmp; + } +#endif + + /*! \brief Fill the vector with zeroes */ + inline void zero() + { + for (XsSize i = 0; i < m_size; ++i) + m_data[i] = XsMath_zero; + } + + /*! \brief Fill the vector with \a val */ + inline void fill(XsReal val) + { + for (XsSize i = 0; i < m_size; ++i) + m_data[i] = val; + } + + /*! \brief Swap the contents of \a b with this + \details This function swaps the internal buffers so no actual data is moved around. For unmanaged + data an elementwise swap is done, but only if the vectors are the same size. + \param b Object whose contents will be swapped with this + */ + inline void swap(XsVector& b) + { + XsVector_swap(this, &b); + } + + /*! \brief swap the contents of \a first and \a second */ + friend void swap(XsVector& first, XsVector& second) + { + first.swap(second); + } + + /*! \brief Append \a other to this + \details Append the vector in \a other to the end of this vector + \param other The vector to append + */ + inline void append(XsVector const& other) + { + XsVector tmp(size() + other.size()); + for (XsSize i = 0; i < size(); ++i) + tmp[i] = (*this)[i]; + for (XsSize i = 0; i < other.size(); ++i) + tmp[i + size()] = other[i]; + swap(tmp); + } + + /*! \brief Clear the vector, making it size 0 */ + inline void clear() + { + setSize(0); + } + +#ifndef XSENS_NO_STL + /*! \brief Reverse the values in the vector */ + inline void reverse() + { + using std::swap; + + XsSize sz = size(); + XsSize half = sz >> 1; + --sz; + for (XsSize i = 0; i < half; ++i) + swap(operator[](i), operator[](sz - i)); + } +#endif + +#ifdef __ICCARM__ +#pragma diag_default=Pa039 +#endif + +#endif +}; +#ifndef XSENS_NO_PACK + #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) +{ + return v * scalar; +} +#endif + +#endif diff --git a/extern/xspublic/xstypes/xsvector3.c b/extern/xspublic/xstypes/xsvector3.c new file mode 100644 index 0000000..85ac027 --- /dev/null +++ b/extern/xspublic/xstypes/xsvector3.c @@ -0,0 +1,72 @@ + +// Copyright (c) 2003-2022 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 "xsvector3.h" +#include + +/*! \class XsVector3 + \brief A class that represents a fixed size (3) vector +*/ + +/*! \addtogroup cinterface C Interface + @{ +*/ + +/*! \relates XsVector3 \brief Init the %XsVector3 and copy the data from \a src into the vector if \a src is not null */ +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)); +} + +/*! \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) +{ + XsVector_assign(&thisPtr->m_vector, 3, src); +} + +/*! \relates XsVector3 \brief Frees the XsVector3 */ +void XsVector3_destruct(XsVector3* thisPtr) +{ + // don't do anything, no memory needs to be freed + assert(thisPtr->m_vector.m_flags & XSDF_FixedSize); + (void)thisPtr; +} + +/*! \relates XsVector3 \brief Copy the contents of the %XsVector3 to \a copy */ +void XsVector3_copy(XsVector* copy, XsVector3 const* src) +{ + XsVector_copy(copy, &src->m_vector); +} + +/*! @} */ diff --git a/extern/xspublic/xstypes/xsvector3.h b/extern/xspublic/xstypes/xsvector3.h new file mode 100644 index 0000000..5456726 --- /dev/null +++ b/extern/xspublic/xstypes/xsvector3.h @@ -0,0 +1,123 @@ + +// Copyright (c) 2003-2022 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 XSVECTOR3_H +#define XSVECTOR3_H + +#include "xsvector.h" + +struct XsVector3; +#ifdef __cplusplus +extern "C" { +#endif +#ifndef __cplusplus +typedef struct XsVector3 XsVector3; +#endif + +XSTYPES_DLL_API void XsVector3_construct(XsVector3* thisPtr, const XsReal* src); +XSTYPES_DLL_API void XsVector3_assign(XsVector3* thisPtr, const XsReal* src); +XSTYPES_DLL_API void XsVector3_destruct(XsVector3* thisPtr); +XSTYPES_DLL_API void XsVector3_copy(XsVector* copy, XsVector3 const* src); + +#ifdef __cplusplus +} // extern "C" +#endif + +#ifdef __cplusplus +/* 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 +#else +struct XsVector3 +{ + XsVector m_vector; //!< The underlying vector +#endif + XsReal m_fixedData[3]; //!< Fixed size storage for the components in the vector + +#ifdef __cplusplus +public: + //! \brief Constructs an empty vector3 + XsVector3() : XsVector(m_fixedData, 3, XSDF_FixedSize) + { + //XsVector3_construct(this, 0); + } + + //! \brief Constructs a vector3 from an \a other XsVector + XsVector3(XsVector3 const& other) : XsVector(other, m_fixedData, 3, XSDF_FixedSize) + { + } + + //! \brief Constructs a vector3 from an \a other XsVector + XsVector3(XsVector const& other) : XsVector(other, m_fixedData, 3, XSDF_FixedSize) + { + } + + //! \brief Constructs a vector3 using the values \a x, \a y, \a z + XsVector3(XsReal x, XsReal y, XsReal z) : XsVector(m_fixedData, 3, XSDF_FixedSize) + { + m_data[0] = x; + m_data[1] = y; + m_data[2] = z; + } + + //! \brief Return a 3-element zero vector + static XsVector3 const& zero3() + { + static const XsVector3 rv(XsMath_zero, XsMath_zero, XsMath_zero); + return rv; + } + + using XsVector::operator=; + //! \brief Assignment operator. Copies from \a other into this + inline XsVector3& operator=(const XsVector3& other) + { + XsVector_copy(this, &other); + return *this; + } + + using XsVector::assign; + //! \brief Assignment, directly assigns values. + inline void assign(XsReal x, XsReal y, XsReal z) + { + m_data[0] = x; + m_data[1] = y; + m_data[2] = z; + const_cast(m_flags) &= ~XSDF_Empty; + } + // using XsVector::operator[]; +#endif +}; + +#endif diff --git a/extern/xspublic/xstypes/xsversion.c b/extern/xspublic/xstypes/xsversion.c new file mode 100644 index 0000000..545926c --- /dev/null +++ b/extern/xspublic/xstypes/xsversion.c @@ -0,0 +1,148 @@ + +// Copyright (c) 2003-2022 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 "xsversion.h" +#include "xsstring.h" +#include + +/*! \class XsVersion + \brief A class to store version information +*/ + +/*! \addtogroup cinterface C Interface + @{ +*/ + +/*! \relates XsVersion \brief Test if this is a null-version. */ +int XsVersion_empty(const XsVersion* thisPtr) +{ + return thisPtr->m_major == 0 && thisPtr->m_minor == 0 && thisPtr->m_revision == 0; +} + +/*! \relates XsVersion \brief Get a string with the version expressed in a readable format. */ +void XsVersion_toString(const XsVersion* thisPtr, XsString* version) +{ + char buffer[256]; + int chars; + + if (thisPtr->m_build != 0 && thisPtr->m_reposVersion != 0) + chars = sprintf(buffer, "%d.%d.%d build %d rev %d", thisPtr->m_major, thisPtr->m_minor, thisPtr->m_revision, thisPtr->m_build, thisPtr->m_reposVersion); + else + chars = sprintf(buffer, "%d.%d.%d", thisPtr->m_major, thisPtr->m_minor, thisPtr->m_revision); + + XsString_assign(version, (XsSize)(ptrdiff_t)chars, buffer); + if (thisPtr->m_extra.m_size != 0) + { + const char 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. +*/ +void XsVersion_toSimpleString(const XsVersion* thisPtr, XsString* version) +{ + char buffer[256]; + int chars = sprintf(buffer, "%d.%d.%d", thisPtr->m_major, thisPtr->m_minor, thisPtr->m_revision); + XsString_assign(version, (XsSize)(ptrdiff_t)chars, buffer); +} + +/*! + \relates XsVersion + \brief Set the version to the values in the string +*/ +void XsVersion_fromString(XsVersion* thisPtr, const XsString* version) +{ + int major = 0; + int minor = 0; + int revision = 0; + int build = 0; + int reposVersion = 0; + int result; + size_t count = 0; + + assert(thisPtr); + thisPtr->m_major = 0; + thisPtr->m_minor = 0; + thisPtr->m_revision = 0; + thisPtr->m_build = 0; + thisPtr->m_reposVersion = 0; + XsString_resize(&thisPtr->m_extra, 0); + if (!version || XsString_empty(version)) + return; + + result = sscanf(version->m_data, "%d.%d.%d build %d rev %d%zn", &major, &minor, &revision, &build, &reposVersion, &count); + + if (result > 0) + { + thisPtr->m_major = major; + thisPtr->m_minor = minor; + thisPtr->m_revision = revision; + } + + if (result > 3) + { + thisPtr->m_build = build; + thisPtr->m_reposVersion = reposVersion; + } + + if ((result == 5) && ((count + 1) < version->m_size)) + XsString_assignCharArray(&thisPtr->m_extra, &version->m_data[count + 1]); +} + +/*! \relates XsVersion + \brief Create a XsVersion a XsSimpleVersion, \a simpleVersion. +*/ +void XsVersion_fromSimpleVersion(XsVersion* thisPtr, const XsSimpleVersion* simpleVersion) +{ + thisPtr->m_major = simpleVersion->m_major; + thisPtr->m_minor = simpleVersion->m_minor; + thisPtr->m_revision = simpleVersion->m_revision; + thisPtr->m_build = 0; + thisPtr->m_reposVersion = 0; + XsString_resize(&thisPtr->m_extra, 0); +} + +/*! \relates 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; + simpleVersion->m_minor = (uint8_t)(int8_t)thisPtr->m_minor; + simpleVersion->m_revision = (uint8_t)(int8_t)thisPtr->m_revision; +} + +/*! @} */ diff --git a/extern/xspublic/xstypes/xsversion.h b/extern/xspublic/xstypes/xsversion.h new file mode 100644 index 0000000..c92ff71 --- /dev/null +++ b/extern/xspublic/xstypes/xsversion.h @@ -0,0 +1,291 @@ + +// Copyright (c) 2003-2022 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 XSVERSION_H +#define XSVERSION_H + +#include "xstypesconfig.h" +#include "xssimpleversion.h" +#include "xsstring.h" + +#undef minor +#undef major + +#ifdef __cplusplus +struct XsVersion; +extern "C" { +#endif +#ifndef __cplusplus +typedef struct XsVersion XsVersion; +#define XSVERSION_INITIALIZER { 0, 0, 0, 0, 0, XsString_INITIALIZER } +#endif + +XSTYPES_DLL_API int XsVersion_empty(const XsVersion* thisPtr); +XSTYPES_DLL_API void XsVersion_toString(const XsVersion* thisPtr, XsString* version); +XSTYPES_DLL_API void XsVersion_fromString(XsVersion* thisPtr, const XsString* version); +XSTYPES_DLL_API void XsVersion_fromSimpleVersion(XsVersion* thisPtr, const XsSimpleVersion* simpleVersion); +XSTYPES_DLL_API void XsVersion_toSimpleVersion(const XsVersion* thisPtr, XsSimpleVersion* simpleVersion); +XSTYPES_DLL_API void XsVersion_toSimpleString(const XsVersion* thisPtr, XsString* version); + +#ifdef __cplusplus +} // extern "C" +#endif + +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) + : m_major(maj) + , m_minor(min) + , m_revision(rev) + , m_build(build) + , m_reposVersion(0) + , m_extra(extra) + {} + + //! \brief Constructs a version object using the supplied parameters or an empty version object if no parameters are given. + explicit XsVersion(int maj = 0, int min = 0, int rev = 0, int build = 0, int reposVersion = 0, const XsString& extra = XsString()) + : m_major(maj) + , m_minor(min) + , m_revision(rev) + , m_build(build) + , m_reposVersion(reposVersion) + , m_extra(extra) + {} + + //! \brief Constructs a version object based upon the version contained by \a vString. + explicit XsVersion(const XsString& vString) + { + XsVersion_fromString(this, &vString); + } + + //! \brief Constructs a version object based upon the version contained by \a simpleVersion. + XsVersion(const XsSimpleVersion& simpleVersion) + { + XsVersion_fromSimpleVersion(this, &simpleVersion); + } + + //! \brief Constructs a version object based upon the \a other object + XsVersion(const XsVersion& other) + : m_major(other.m_major) + , m_minor(other.m_minor) + , m_revision(other.m_revision) + , m_build(other.m_build) + , m_reposVersion(other.m_reposVersion) + , m_extra(other.m_extra) + {} + + //! \brief Assign the version from the \a other object + XsVersion& operator = (const XsVersion& other) + { + m_major = other.m_major; + m_minor = other.m_minor; + m_revision = other.m_revision; + m_build = other.m_build; + m_reposVersion = other.m_reposVersion; + m_extra = other.m_extra; + return *this; + } + + /*! \brief Test if the \a other version is equal to this. The comparison involves the entire object.*/ + 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); + } + + /*! \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 + { + 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 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). */ + inline bool operator < (const XsVersion& other) const + { + 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 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 + { + 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). */ + inline bool operator > (const XsVersion& other) const + { + return !(*this <= other); + } + + /*! \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); + } + + //! \brief \copybrief XsVersion_empty + inline bool empty() const + { + return 0 != XsVersion_empty(this); + } + + //! \brief \copybrief XsVersion_toString + inline XsString toString() const + { + XsString tmp; + XsVersion_toString(this, &tmp); + return tmp; + } + + //! \brief \copybrief XsVersion_toSimpleString + inline XsString toSimpleString() const + { + XsString tmp; + XsVersion_toSimpleString(this, &tmp); + return tmp; + } + + //! \brief \copybrief XsVersion_toSimpleVersion + inline XsSimpleVersion toSimpleVersion() const + { + XsSimpleVersion result; + XsVersion_toSimpleVersion(this, &result); + return result; + } + + //! \brief Return the \e major part of the version + inline int major() const + { + return m_major; + } + //! \brief Return the \e minor part of the version + inline int minor() const + { + return m_minor; + } + //! \brief Return the \e revision part of the version + inline int revision() const + { + return m_revision; + } + //! \brief Return the \e build number used for this 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; + } + //! \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; + } + + //! \brief Set the \e major part of the version + 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; + } + //! \brief Set the \e revision part of the version + 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; + } + //! \brief Set the \e reposVersion part of the version + 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; + } + +private: +#endif + + int m_major; //!< The major part of the version number + int m_minor; //!< The minor part of the version number + int m_revision; //!< The revision number of the version + int m_build; //!< The build number for this build + int m_reposVersion; //!< The source revision used for this build + XsString m_extra; //!< Storage for some extra information about the version +}; + +#endif diff --git a/extern/xspublic/xstypes/xsxbusmessageid.h b/extern/xspublic/xstypes/xsxbusmessageid.h new file mode 100644 index 0000000..5d4c516 --- /dev/null +++ b/extern/xspublic/xstypes/xsxbusmessageid.h @@ -0,0 +1,492 @@ + +// Copyright (c) 2003-2022 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 XSXBUSMESSAGEID_H +#define XSXBUSMESSAGEID_H + +/*! \addtogroup enums Global enumerations + @{ +*/ +/*! \brief Xsens Xbus Message Identifiers +*/ +enum XsXbusMessageId +{ + XMID_InvalidMessage = 0x00, + + // Config state messages + XMID_ReqDid = 0x00, + XMID_DeviceId = 0x01, + XMID_Initbus = 0x02, + XMID_InitBusResults = 0x03, + XMID_ReqPeriod = 0x04, + XMID_ReqPeriodAck = 0x05, + XMID_SetPeriod = 0x04, + XMID_SetPeriodAck = 0x05, + + XMID_SetBid = 0x06, + XMID_SetBidAck = 0x07, + XMID_AutoStart = 0x06, + XMID_AutoStartAck = 0x07, + XMID_BusPower = 0x08, + XMID_BusPowerAck = 0x09, + + XMID_ReqDataLength = 0x0A, + XMID_DataLength = 0x0B, + XMID_ReqConfiguration = 0x0C, + XMID_Configuration = 0x0D, + XMID_RestoreFactoryDef = 0x0E, + XMID_RestoreFactoryDefAck = 0x0F, + + XMID_GotoMeasurement = 0x10, + XMID_GotoMeasurementAck = 0x11, + XMID_ReqFirmwareRevision = 0x12, + XMID_FirmwareRevision = 0x13, + XMID_ReqUniqueId = 0x14, + XMID_UniqueId = 0x15, + XMID_ReqBodypackMode = 0x16, + XMID_ReqBodypackAck = 0x17, + XMID_SetBodypackMode = 0x16, + XMID_SetBodypackModeAck = 0x17, + + XMID_ReqBaudrate = 0x18, + XMID_ReqBaudrateAck = 0x19, + XMID_SetBaudrate = 0x18, + XMID_SetBaudrateAck = 0x19, + XMID_ReqProductVariant = 0x1A, + XMID_ProductVariant = 0x1B, + XMID_ReqProductCode = 0x1C, + XMID_ProductCode = 0x1D, + + XMID_ReqHardwareVersion = 0x1E, + XMID_HardwareVersion = 0x1F, + + XMID_ReqProcessingFlags = 0x20, + XMID_ReqProcessingFlagsAck = 0x21, + XMID_SetProcessingFlags = 0x20, + XMID_SetProcessingFlagsAck = 0x21, + + XMID_SetNoRotation = 0x22, + XMID_SetNoRotationAck = 0x23, + + XMID_RunSelfTest = 0x24, + XMID_SelfTestResults = 0x25, + + XMID_ReqInputTrigger = 0x26, + XMID_ReqInputTriggerAck = 0x27, + XMID_SetInputTrigger = 0x26, + XMID_SetInputTriggerAck = 0x27, + + XMID_ReqOutputTrigger = 0x28, + XMID_ReqOutputTriggerAck = 0x29, + 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_SetSyncConfiguration = 0x2C, + XMID_SetSyncConfigurationAck = 0x2D, + XMID_ReqSyncConfiguration = 0x2C, + XMID_SyncConfiguration = 0x2D, + + XMID_DriverDisconnect = 0x2E, + XMID_DriverDisconnectAck = 0x2F, + + XMID_GotoConfig = 0x30, + XMID_GotoConfigAck = 0x31, + XMID_MtData = 0x32, + XMID_BusData = 0x32, + + // Manual + XMID_PrepareData = 0x32, + XMID_ReqData = 0x34, + XMID_ReqDataAck = 0x35, + + XMID_MtData2 = 0x36, + XMID_MtData2Ack = 0x37, + + XMID_RequestControl = 0x38, + XMID_RequestControlAck = 0x39, + + XMID_SetDataPort = 0x3A, + XMID_SetDataPortAck = 0x3B, + + XMID_ReqRetransmission = 0x3C, + XMID_ReqRetransmissionAck = 0x3D, + + // Wakeup state messages + XMID_Wakeup = 0x3E, + XMID_WakeupAck = 0x3F, + + // Valid in all states + XMID_Reset = 0x40, + XMID_ResetAck = 0x41, + XMID_Error = 0x42, + XMID_Warning = 0x43, + // end Valid in all states + + XMID_XmPowerOff = 0x44, + + // Wireless + XMID_MasterIndication = 0x46, + XMID_XsbData = 0x47, // optional meta-data + + XMID_ReqOptionFlags = 0x48, + XMID_ReqOptionFlagsAck = 0x49, + XMID_SetOptionFlags = 0x48, + XMID_SetOptionFlagsAck = 0x49, + XMID_ReqStealthMode = 0x4A, + XMID_StealthMode = 0x4B, + XMID_SetStealthMode = 0x4A, + XMID_SetStealthModeAck = 0x4B, + + XMID_UserInterface = 0x4C, + XMID_UserInterfaceAck = 0x4D, + + XMID_EndOfRecording = 0x4E, + XMID_EndOfRecordingAck = 0x4F, + + XMID_GotoTransparentMode = 0x50, + XMID_GotoTransparentModeAck = 0x51, + + XMID_ReqDeviceCapabilities = 0x52, + XMID_DeviceCapabilities = 0x53, + + XMID_DiscardRetransmissions = 0x54, + XMID_DiscardRetransmissionsAck = 0x55, + + XMID_RunFactoryTest = 0x56, + XMID_FactoryTestResults = 0x57, + XMID_FactoryTestConnect = 0x58, + XMID_FactoryTestConnectAck = 0x59, + + XMID_SetDataOutputDelay = 0x5A, + XMID_SetDataOutputDelayAck = 0x5B, + + XMID_SetBodypackConfigFile = 0x5C, + XMID_SetBodypackConfigFileAck = 0x5D, + + XMID_ReqObrStatus = 0x5E, + XMID_ObrStatus = 0x5F, + + XMID_SetUtcTime = 0x60, + XMID_ReqUtcTime = 0x60, + XMID_SetUtcTimeAck = 0x61, + XMID_UtcTime = 0x61, + + XMID_FactoryTestSensorTiming = 0x60, + XMID_FactoryTestSensorTimingResults = 0x61, + + 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_ReqGravityMagnitude = 0x66, + XMID_ReqGravityMagnitudeAck = 0x67, + XMID_SetGravityMagnitude = 0x66, + XMID_SetGravityMagnitudeAck = 0x67, + + XMID_ReqGnssLeverArm = 0x68, + XMID_ReqGnssLeverArmAck = 0x69, + XMID_SetGnssLeverArm = 0x68, + XMID_SetGnssLeverArmAck = 0x69, + + XMID_ReqMagneticField = 0x6A, + XMID_ReqMagneticFieldAck = 0x6B, + XMID_SetMagneticField = 0x6A, + XMID_SetMagneticFieldAck = 0x6B, + + XMID_ReqReplayMode = 0x6C, + XMID_ReqReplayModeAck = 0x6D, + XMID_SetReplayMode = 0x6C, + XMID_SetReplayModeAck = 0x6D, + + XMID_ReqLatLonAlt = 0x6E, + XMID_ReqLatLonAltAck = 0x6F, + XMID_SetLatLonAlt = 0x6E, + XMID_SetLatLonAltAck = 0x6F, + + XMID_KeepAlive = 0x70, + XMID_KeepAliveAck = 0x71, + + XMID_CloseConnection = 0x72, + XMID_CloseConnectionAck = 0x73, + + XMID_IccCommand = 0x74, + XMID_IccCommandAck = 0x75, + + XMID_ReqGnssPlatform = 0x76, + XMID_ReqGnssPlatformAck = 0x77, + XMID_SetGnssPlatform = 0x76, + XMID_SetGnssPlatformAck = 0x77, + + XMID_ReqConnectionSettings = 0x78, + XMID_ReqConnectionSettingsAck = 0x79, + XMID_SetConnectionSettings = 0x78, + XMID_SetConnectionSettingsAck = 0x79, + + XMID_BodyPackBundle = 0x7A, + XMID_BodyPackBundleAck = 0x7B, + + XMID_ReqStationOptions = 0x7C, + XMID_ReqStationOptionsAck = 0x7D, + + XMID_ReqErrorReport = 0x7E, + XMID_ErrorReport = 0x7F, + + XMID_ReqXmErrorMode = 0x82, + XMID_ReqXmErrorModeAck = 0x83, + XMID_SetXmErrorMode = 0x82, + XMID_SetXmErrorModeAck = 0x83, + + XMID_ReqBufferSize = 0x84, + XMID_ReqBufferSizeAck = 0x85, + XMID_SetBufferSize = 0x84, + XMID_SetBufferSizeAck = 0x85, + + XMID_ReqHeading = 0x82, + XMID_ReqHeadingAck = 0x83, + XMID_SetHeading = 0x82, + XMID_SetHeadingAck = 0x83, + + XMID_ReqLocationId = 0x84, + XMID_ReqLocationIdAck = 0x85, + XMID_SetLocationId = 0x84, + XMID_SetLocationIdAck = 0x85, + + XMID_ReqExtOutputMode = 0x86, + XMID_ReqExtOutputModeAck = 0x87, + XMID_SetExtOutputMode = 0x86, + XMID_SetExtOutputModeAck = 0x87, + + XMID_ReqBatteryLevel = 0x88, + XMID_Batterylevel = 0x89, + + XMID_ReqInitTrackMode = 0x88, + XMID_ReqInitTrackModeAck = 0x89, + XMID_SetInitTrackMode = 0x88, + XMID_SetInitTrackModeAck = 0x89, + + XMID_ReqMasterSettings = 0x8A, + XMID_MasterSettings = 0x8B, + + XMID_StoreFilterState = 0x8A, + XMID_StoreFilterStateAck = 0x8B, + + 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_ReqEmts = 0x90, + XMID_EmtsData = 0x91, + XMID_UpdateFilterProfile = 0x92, + XMID_UpdateFilterProfileAck = 0x93, + + XMID_RestoreEmts = 0x94, + XMID_RestoreEmtsAck = 0x95, + XMID_StoreEmts = 0x96, + XMID_StoreEmtsAck = 0x97, + + XMID_UnlockMaintenanceMode = 0x98, + XMID_UnlockMaintenanceModeAck = 0x99, + + XMID_ClockSyncCommand = 0x9A, + XMID_ClockSyncCommandAck = 0x9B, + + XMID_ReqActiveClockCorrection = 0x9C, + XMID_ActiveClockCorrection = 0x9D, + XMID_StoreActiveClockCorrection = 0x9E, + XMID_StoreActiveClockCorrectionAck = 0x9F, + + XMID_ReqFilterSettings = 0xA0, + XMID_ReqFilterSettingsAck = 0xA1, + XMID_SetFilterSettings = 0xA0, + XMID_SetFilterSettingsAck = 0xA1, + XMID_ReqAmd = 0xA2, + XMID_ReqAmdAck = 0xA3, + XMID_SetAmd = 0xA2, + XMID_SetAmdAck = 0xA3, + XMID_ResetOrientation = 0xA4, + XMID_ResetOrientationAck = 0xA5, + + XMID_ReqGnssStatus = 0xA6, + XMID_GnssStatus = 0xA7, + + XMID_AdjustUtcTime = 0xA8, + XMID_AdjustUtcTimeAck = 0xA9, + + 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_ReqAccessControlList = 0xAE, + XMID_AccessControlList = 0xAF, + XMID_SetAccessControlList = 0xAE, + XMID_SetAccessControlListAck = 0xAF, + + // Wireless + XMID_ScanChannels = 0xB0, + XMID_ScanChannelsAck = 0xB1, + XMID_EnableMaster = 0xB2, + XMID_EnableMasterAck = 0xB3, + XMID_DisableMaster = 0xB4, + XMID_DisableMasterAck = 0xB5, + XMID_ReqRadioChannel = 0xB6, + XMID_ReqRadioChannelAck = 0xB7, + XMID_SetClientPriority = 0xB8, + XMID_SetClientPriorityAck = 0xB9, + XMID_ReqClientPriority = 0xB8, + XMID_ReqClientPriorityAck = 0xB9, + XMID_SetWirelessConfig = 0xBA, + XMID_SetWirelessConfigAck = 0xBB, + XMID_ReqWirelessConfig = 0xBA, + XMID_ReqWirelessConfigAck = 0xBB, + XMID_UpdateBias = 0xBC, + XMID_UpdateBiasAck = 0xBD, + XMID_ToggleIoPins = 0xBE, + XMID_ToggleIoPinsAck = 0xBF, + + XMID_GotoOperational = 0xC0, + XMID_GotoOperationalAck = 0xC1, + + XMID_SetTransportMode = 0xC2, + XMID_SetTransportModeAck = 0xC3, + XMID_ReqTransportMode = 0xC2, + XMID_ReqTransportModeAck = 0xC3, + + XMID_AcceptMtw = 0xC4, + XMID_AcceptMtwAck = 0xC5, + XMID_RejectMtw = 0xC6, + XMID_RejectMtwAck = 0xC7, + XMID_InfoRequest = 0xC8, + XMID_InfoRequestAck = 0xC9, + + XMID_ReqFrameRates = 0xCA, + XMID_ReqFrameRatesAck = 0xCB, + + XMID_StartRecording = 0xCC, + XMID_StartRecordingAck = 0xCD, + XMID_StopRecording = 0xCE, + XMID_StopRecordingAck = 0xCF, + // End Wireless + + XMID_ReqOutputConfiguration = 0xC0, + XMID_ReqOutputConfigurationAck = 0xC1, + XMID_SetOutputConfiguration = 0xC0, + XMID_SetOutputConfigurationAck = 0xC1, + + XMID_ReqOutputMode = 0xD0, + XMID_ReqOutputModeAck = 0xD1, + XMID_SetOutputMode = 0xD0, + XMID_SetOutputModeAck = 0xD1, + + XMID_ReqOutputSettings = 0xD2, + XMID_ReqOutputSettingsAck = 0xD3, + XMID_SetOutputSettings = 0xD2, + XMID_SetOutputSettingsAck = 0xD3, + + XMID_ReqOutputSkipFactor = 0xD4, + XMID_ReqOutputSkipFactorAck = 0xD5, + XMID_SetOutputSkipFactor = 0xD4, + XMID_SetOutputSkipFactorAck = 0xD5, + + XMID_SetInitialHeading = 0xD6, + XMID_SetInitialHeadingAck = 0xD7, + + XMID_ReqErrorMode = 0xDA, + XMID_ReqErrorModeAck = 0xDB, + XMID_SetErrorMode = 0xDA, + XMID_SetErrorModeAck = 0xDB, + + XMID_ReqTransmitDelay = 0xDC, + XMID_ReqTransmitDelayAck = 0xDD, + XMID_SetTransmitDelay = 0xDC, + XMID_SetTransmitDelayAck = 0xDD, + + XMID_SetMfmResults = 0xDE, + XMID_SetMfmResultsAck = 0xDF, + + XMID_ReqObjectAlignment = 0xE0, + XMID_ReqObjectAlignmentAck = 0xE1, + 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_ReqAlignmentRotation = 0xEC, + XMID_ReqAlignmentRotationAck = 0xED, + XMID_SetAlignmentRotation = 0xEC, + XMID_SetAlignmentRotationAck = 0xED, + + XMID_ExtensionReserved1 = 0xEE, + XMID_ExtensionReserved2 = 0xEF, + + XMID_SetDeviceIdContext = 0xFE, + XMID_SetDeviceIdContextAck = 0xFF +}; +/*! @} */ +typedef enum XsXbusMessageId XsXbusMessageId; + +#endif diff --git a/extern/xstypes/Makefile b/extern/xstypes/Makefile deleted file mode 100644 index f27451f..0000000 --- a/extern/xstypes/Makefile +++ /dev/null @@ -1,41 +0,0 @@ -TARGET:=libxstypes.a -EXCLUDE:=xstypesdynlib.c -C_FILES := $(filter-out $(EXCLUDE), $(wildcard *.c)) -CPP_FILES := $(wildcard *.cpp) -OBJECTS := $(C_FILES:.c=.c.o) $(CPP_FILES:.cpp=.cpp.o) -CFLAGS+= -I$(shell dirname $$(pwd))/include/xsens -CXXFLAGS+= -std=c++11 -I$(shell dirname $$(pwd))/include/xsens - -.PHONY: all xstypes -all: xstypes -xstypes: $(TARGET) - --include $(FILES:.c=.d) - -%.c.o: %.c - $(CC) -c $(CFLAGS) $< -o $@ - @$(CC) -MM $(CFLAGS) $< > $*.d - @mv -f $*.d $*.d.tmp - @sed -e 's|.*:|$*.c.o:|' < $*.d.tmp > $*.d - @sed -e 's/.*://' -e 's/\\$$//' < $*.d.tmp | fmt -1 | \ - sed -e 's/^ *//' -e 's/$$/:/' >> $*.d - @rm -f $*.d.tmp - --include $(FILES:.cpp=.dpp) - -%.cpp.o: %.cpp - $(CXX) -c $(CXXFLAGS) $< -o $@ - @$(CXX) -MM $(CXXFLAGS) $< > $*.dpp - @mv -f $*.dpp $*.dpp.tmp - @sed -e 's|.*:|$*.c.o:|' < $*.dpp.tmp > $*.dpp - @sed -e 's/.*://' -e 's/\\$$//' < $*.dpp.tmp | fmt -1 | \ - sed -e 's/^ *//' -e 's/$$/:/' >> $*.dpp - @rm -f $*.dpp.tmp - -$(TARGET): $(OBJECTS) - ar rcs $@ $^ - -clean: - -$(RM) buildversion.h - -$(RM) *.o *.d *.dpp - -$(RM) $(TARGET) diff --git a/extern/xstypes/datapacket_p.cpp b/extern/xstypes/datapacket_p.cpp deleted file mode 100644 index 63a67c6..0000000 --- a/extern/xstypes/datapacket_p.cpp +++ /dev/null @@ -1,127 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -//lint -e1512 -e1509 -e1511 -e534 -e578 -e1060 -e613 -#include "datapacket_p.h" - -/*! \cond XS_INTERNAL */ - -/*! \class DataPacketPrivate - \brief Internal administration for contained data of XsDataPacket class. - \details This is only the part that can be stored in an XsMessage, so no TOA and computed packet IDs -*/ - -/*! \brief Copy constructor */ -DataPacketPrivate::DataPacketPrivate(DataPacketPrivate const& p) - : MapType() // start with clean map -{ - *this = p; -} - -/*! \brief Destructor */ -DataPacketPrivate::~DataPacketPrivate() -{ - try { - clear(); - } catch(...) - { - } -} - -/*! \brief Assignment operator */ -const DataPacketPrivate& DataPacketPrivate::operator = (const DataPacketPrivate& p) -{ - if (this != &p) - { - clear(); - for (auto i : p) - insert(i.first, i.second->clone()); - } - return *this; -} - -/*! \brief Clear the contents */ -void DataPacketPrivate::clear() -{ - for (auto it : *this) - delete it.second; - MapType::clear(); -} - -/*! \brief Find the item matching \a id - \details This function will return an iterator to the item that matches \a id or end() if it could not find it. - The function uses a loose comparison to ensure entry ambiguity. As a result, the returned iterator may have a - slightly different id than the supplied one. - \return An iterator to the requested item or end() -*/ -MapType::const_iterator DataPacketPrivate::find(XsDataIdentifier id) const -{ - return MapType::find(id & XDI_FullTypeMask); -} - -/*! \brief Add or overwrite the item with \a id - \details The function will create a new item or overwite the current item, properly cleaning up existing data - if necessary. -*/ -MapType::iterator DataPacketPrivate::insert(XsDataIdentifier id, XsDataPacket_Private::Variant* var) -{ - id = id & XDI_FullTypeMask; - auto it = MapType::lower_bound(id); - if (it != end() && it->first == id) - { - delete it->second; - it->second = var; - return it; - } - else - return MapType::insert(it, std::make_pair(id & XDI_FullTypeMask, var)); -} - -/*! \brief Remove the item with \a id if it exists, cleaning up associated data */ -void DataPacketPrivate::erase(XsDataIdentifier id) -{ - auto it = find(id); - if (it != end()) - erase(it); -} - -/*! \brief Remove the item at \a it, cleaning up associated data */ -void DataPacketPrivate::erase(MapType::const_iterator it) -{ - delete it->second; - MapType::erase(it); -} - -/*! \brief Merge \a other into this - \details The function will copy all items from \a other into this. Existing items will only be overwritten if - \a overwrite is set to true. This function does not detect conflicts between closely related but IDs (such as - frame range and packet counter). This is left to the caller. -*/ -void DataPacketPrivate::merge(DataPacketPrivate const& other, bool overwrite) -{ - if (overwrite) - for (auto i : other) - insert(i.first, i.second->clone()); - else - { - for (auto i : other) - { - auto j = find(i.first); - if (j == end()) - insert(i.first, i.second->clone()); - } - } -} - -/*! \endcond XS_INTERNAL */ diff --git a/extern/xstypes/datapacket_p.h b/extern/xstypes/datapacket_p.h deleted file mode 100644 index 9b9646d..0000000 --- a/extern/xstypes/datapacket_p.h +++ /dev/null @@ -1,895 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifndef DATAPACKET_P_H -#define DATAPACKET_P_H - -#include "xsmessage.h" -#include "xsdeviceid.h" -#include "xstimestamp.h" -#include -#include "xsquaternion.h" -#include "xsushortvector.h" -#include "xsvector3.h" -#include "xsscrdata.h" -#include "xstriggerindicationdata.h" -#include "xseuler.h" -#include "xsmatrix3x3.h" -#include "xsrange.h" -#include "xsutctime.h" -#include "xsrawgnsspvtdata.h" -#include "xsrawgnsssatinfo.h" -#include "xsrawgpsdop.h" -#include "xsrawgpssol.h" -#include "xsrawgpssvinfo.h" -#include "xsrawgpstimeutc.h" -#include "xsbytearray.h" - -#include "xssnapshot.h" - -/*! \cond XS_INTERNAL */ -namespace XsDataPacket_Private { - class Variant { - public: - Variant(XsDataIdentifier id) : m_id(id) {} - virtual ~Variant() {} - virtual void readFromMessage(XsMessage const& msg, int offset, int dSize) = 0; - virtual void writeToMessage(XsMessage& msg, int offset) const = 0; - virtual int sizeInMsg() const = 0; - virtual Variant* clone() const = 0; - - void setDataId(XsDataIdentifier id) - { - assert((m_id & XDI_FullTypeMask) == (id & XDI_FullTypeMask)); - m_id = id; - } - XsDataIdentifier dataId() const { return m_id; } - - template - U& toDerived() - { - U* ptr = dynamic_cast(this); - assert(ptr); - return *ptr; - } - - template - U const& toDerived() const - { - U const* ptr = dynamic_cast(this); - assert(ptr); - return *ptr; - } - - private: - XsDataIdentifier m_id; - }; - - template - class GenericVariant : public Variant { - public: - GenericVariant(XsDataIdentifier id) : Variant(id) {} - - void readFromMessage(XsMessage const& msg, int offset, int /*ignored*/) override - { - msg.getData(data(), dataId(), offset, C); - } - void writeToMessage(XsMessage& msg, int offset) const override - { - msg.setData(constData(), dataId(), offset, C); - } - virtual T* data() = 0; - virtual T const* constData() const = 0; - - int sizeInMsg() const override - { - return XsMessage::sizeInMsg(dataId(), C); - } - }; - - template<> - inline void GenericVariant::readFromMessage(XsMessage const& msg, int offset, int /*ignored*/) - { - *data() = 0; - *data() = ((uint64_t)XsMessage_getDataLong(&msg, offset)) << 32; - *data() += ((uint64_t)XsMessage_getDataLong(&msg, offset + 4)); - } - - template<> - inline void GenericVariant::writeToMessage(XsMessage& msg, int offset) const - { - XsMessage_setDataLong(&msg, (uint32_t) ((*constData()) >> 32), offset); - XsMessage_setDataLong(&msg, (uint32_t) ((*constData()) & 0xFFFFFFFF), offset); - } - - template - struct SimpleVariant : public GenericVariant { - using GenericVariant::dataId; - - SimpleVariant(XsDataIdentifier id) : GenericVariant(id), m_data() {} - SimpleVariant(XsDataIdentifier id, T const& val) : GenericVariant(id), m_data(val) {} - - T m_data; - T* data() override - { - return &m_data; - } - T const* constData() const override - { - return &m_data; - } - - Variant* clone() const override - { - return new SimpleVariant(dataId(), m_data); - } - - int sizeInMsg() const override - { - return XsMessage::sizeInMsg(dataId(), 1); - } - }; - - template - struct ComplexVariant : public GenericVariant - { - using GenericVariant::dataId; - ComplexVariant(XsDataIdentifier id) : GenericVariant(id) {} - ComplexVariant(XsDataIdentifier id, U const& val) : GenericVariant(id), m_data(val) {} - - U m_data; - T* data() override - { - return const_cast(m_data.data()); - } - T const* constData() const override - { - return m_data.data(); - } - - //! \note Override in derived classes! - Variant* clone() const override - { - return new ComplexVariant(dataId(), m_data); - } - }; - - struct XsQuaternionVariant : public ComplexVariant - { - XsQuaternionVariant(XsDataIdentifier id) : ComplexVariant(id) {} - XsQuaternionVariant(XsDataIdentifier id, XsQuaternion const& val) : ComplexVariant(id, val) {} - - Variant* clone() const override - { - return new XsQuaternionVariant(dataId(), m_data); - } - }; - - struct XsUShortVectorVariant : public ComplexVariant - { - XsUShortVectorVariant(XsDataIdentifier id) : ComplexVariant(id) {} - XsUShortVectorVariant(XsDataIdentifier id, XsUShortVector const& val) : ComplexVariant(id, val) {} - Variant* clone() const override - { - return new XsUShortVectorVariant(dataId(), m_data); - } - }; - - struct XsVector3Variant : public ComplexVariant - { - XsVector3Variant(XsDataIdentifier id) : ComplexVariant(id) {} - XsVector3Variant(XsDataIdentifier id, XsVector const& val) : ComplexVariant(id, val) - { - assert(val.size() == 3); - } - Variant* clone() const override - { - return new XsVector3Variant(dataId(), m_data); - } - }; - - struct XsVector2Variant : public ComplexVariant - { - XsVector2Variant(XsDataIdentifier id) : ComplexVariant(id, XsVector(2,0)) {} - XsVector2Variant(XsDataIdentifier id, XsVector const& val) : ComplexVariant(id, val) - { - assert(val.size() == 2); - } - Variant* clone() const override - { - return new XsVector2Variant(dataId(), m_data); - } - }; - - struct XsScrDataVariant : public Variant - { - XsScrDataVariant(XsDataIdentifier id) : Variant(id) {} - XsScrDataVariant(XsDataIdentifier id, XsScrData const& val) : Variant(id), m_data(val) {} - Variant* clone() const override - { - return new XsScrDataVariant(dataId(), m_data); - } - - XsScrData m_data; - void readFromMessage(XsMessage const& msg, int offset, int /*ignored*/) override - { - for (int i = 0; i < 3; ++i, offset +=2) - m_data.m_acc[i] = msg.getDataShort(offset); - for (int i = 0; i < 3; ++i, offset +=2) - m_data.m_gyr[i] = msg.getDataShort(offset); - for (int i = 0; i < 3; ++i, offset +=2) - m_data.m_mag[i] = msg.getDataShort(offset); - m_data.m_temp = msg.getDataShort(offset); - } - void writeToMessage(XsMessage& msg, int offset) const override - { - for (int i = 0; i < 3; ++i, offset +=2) - msg.setDataShort(m_data.m_acc[i], offset); - for (int i = 0; i < 3; ++i, offset +=2) - msg.setDataShort(m_data.m_gyr[i], offset); - for (int i = 0; i < 3; ++i, offset +=2) - msg.setDataShort(m_data.m_mag[i], offset); - msg.setDataShort(m_data.m_temp, offset); - } - - int sizeInMsg() const override - { - return 10*sizeof(uint16_t); - } - }; - - struct XsTriggerIndicationDataVariant : public Variant - { - XsTriggerIndicationDataVariant(XsDataIdentifier id) : Variant(id) {} - XsTriggerIndicationDataVariant(XsDataIdentifier id, XsTriggerIndicationData const& val) : Variant(id), m_data(val) {} - Variant* clone() const override - { - return new XsTriggerIndicationDataVariant(dataId(), m_data); - } - - XsTriggerIndicationData m_data; - void readFromMessage(XsMessage const& msg, int offset, int /*ignored*/) 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); - } - void writeToMessage(XsMessage& msg, int 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); - } - - int sizeInMsg() const override - { - return 8; - } - }; - - struct XsEulerVariant : public ComplexVariant - { - XsEulerVariant(XsDataIdentifier id) : ComplexVariant(id) {} - XsEulerVariant(XsDataIdentifier id, XsEuler const& val) : ComplexVariant(id, val) {} - Variant* clone() const override - { - return new XsEulerVariant(dataId(), m_data); - } - }; - - struct XsMatrixVariant : public ComplexVariant - { - XsMatrixVariant(XsDataIdentifier id) : ComplexVariant(id) {} - XsMatrixVariant(XsDataIdentifier id, XsMatrix const& val) : ComplexVariant(id, val) {} - Variant* clone() const override - { - return new XsMatrixVariant(dataId(), m_data); - } - - void readFromMessage(XsMessage const& msg, int offset, int /*ignored*/) override - { - int ds = XsMessage_getFPValueSize(dataId()); - int k = 0; - for (int i=0 ; i<3 ; ++i) - for (int j=0 ; j<3 ; ++j, k+=ds) - XsMessage_getDataFPValuesById(&msg, dataId(), &m_data[j][i], offset+k, 1); - } - void writeToMessage(XsMessage& msg, int offset) const override - { - int ds = XsMessage_getFPValueSize(dataId()); - int k = 0; - for (int i=0 ; i<3 ; ++i) - for (int j=0 ; j<3 ; ++j, k+=ds) - XsMessage_setDataFPValuesById(&msg, dataId(), &m_data[j][i], offset+k, 1); - } - }; - - struct XsRangeVariant : public Variant - { - XsRangeVariant(XsDataIdentifier id) : Variant(id) {} - XsRangeVariant(XsDataIdentifier id, XsRange const& val) : Variant(id), m_data(val) {} - Variant* clone() const override - { - return new XsRangeVariant(dataId(), m_data); - } - - XsRange m_data; - void readFromMessage(XsMessage const& msg, int offset, int /*ignored*/) 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); - } - void writeToMessage(XsMessage& msg, int offset) const override - { - XsMessage_setDataShort(&msg, (uint16_t) m_data.first(), offset + 0); - XsMessage_setDataShort(&msg, (uint16_t) m_data.last(), offset + 2); - } - - int sizeInMsg() const override - { - return 4; - } - }; - - struct XsUtcTimeVariant : public Variant - { - XsUtcTimeVariant(XsDataIdentifier id) : Variant(id), m_data() {} - XsUtcTimeVariant(XsDataIdentifier id, XsUtcTime const& val) : Variant(id), m_data(val) {} - Variant* clone() const override - { - return new XsUtcTimeVariant(dataId(), m_data); - } - - XsUtcTime m_data; - void readFromMessage(XsMessage const& msg, int offset, int /*ignored*/) override - { - m_data.m_nano = XsMessage_getDataLong(&msg, offset); - m_data.m_year = XsMessage_getDataShort(&msg, offset+4); - - //lint --e{662, 661} these fields are named first-of-array fields - // month, day, hour, minute, second and valid - uint8_t* bareByte = (uint8_t*) &m_data.m_month; - for (int i=0; i < 6; ++i) - bareByte[i] = XsMessage_getDataByte(&msg, offset + 6 + i); - } - void writeToMessage(XsMessage& msg, int 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 (int i=0; i<6;++i) - XsMessage_setDataByte(&msg, bareByte[i], offset + 6 + i); //lint !e661 !e662 - } - - int sizeInMsg() const override - { - return 12; - } - }; - - struct XsRawGnssPvtDataVariant : public Variant - { - XsRawGnssPvtDataVariant(XsDataIdentifier id) : Variant(id), m_data() {} - XsRawGnssPvtDataVariant(XsDataIdentifier id, XsRawGnssPvtData const& val) : Variant(id), m_data(val) {} - Variant* clone() const override - { - return new XsRawGnssPvtDataVariant(dataId(), m_data); - } - - XsRawGnssPvtData m_data; - void readFromMessage(XsMessage const& msg, int offset, int /*ignored*/) 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); - } - void writeToMessage(XsMessage& msg, int 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_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); - XsMessage_setDataShort(&msg, m_data.m_vdop , offset + 86); - 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); - } - - int sizeInMsg() const override - { - return 94; - } - }; - - struct XsRawGnssSatInfoVariant : public Variant - { - XsRawGnssSatInfoVariant(XsDataIdentifier id) : Variant(id), m_data() {} - XsRawGnssSatInfoVariant(XsDataIdentifier id, XsRawGnssSatInfo const& val) : Variant(id), m_data(val) {} - Variant* clone() const override - { - return new XsRawGnssSatInfoVariant(dataId(), m_data); - } - - XsRawGnssSatInfo m_data; - void readFromMessage(XsMessage const& msg, int offset, int /*ignored*/) 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_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, int 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) - { - 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; - } - } - - int sizeInMsg() const override - { - return 8 + 4*m_data.m_numSvs; - } - }; - - struct XsRawGpsDopVariant : public Variant - { - XsRawGpsDopVariant(XsDataIdentifier id) : Variant(id), m_data() {} - XsRawGpsDopVariant(XsDataIdentifier id, XsRawGpsDop const& val) : Variant(id), m_data(val) {} - Variant* clone() const override - { - return new XsRawGpsDopVariant(dataId(), m_data); - } - - XsRawGpsDop m_data; - void readFromMessage(XsMessage const& msg, int offset, int /*ignored*/) override - { - m_data.m_itow = XsMessage_getDataLong (&msg, offset); - m_data.m_gdop = XsMessage_getDataShort(&msg, offset+4); - m_data.m_pdop = XsMessage_getDataShort(&msg, offset+6); - m_data.m_tdop = XsMessage_getDataShort(&msg, offset+8); - m_data.m_vdop = XsMessage_getDataShort(&msg, offset+10); - m_data.m_hdop = XsMessage_getDataShort(&msg, offset+12); - m_data.m_ndop = XsMessage_getDataShort(&msg, offset+14); - m_data.m_edop = XsMessage_getDataShort(&msg, offset+16); - } - void writeToMessage(XsMessage& msg, int offset) const override - { - XsMessage_setDataLong (&msg, m_data.m_itow, offset); - XsMessage_setDataShort(&msg, m_data.m_gdop, offset+4); - XsMessage_setDataShort(&msg, m_data.m_pdop, offset+6); - XsMessage_setDataShort(&msg, m_data.m_tdop, offset+8); - XsMessage_setDataShort(&msg, m_data.m_vdop, offset+10); - XsMessage_setDataShort(&msg, m_data.m_hdop, offset+12); - XsMessage_setDataShort(&msg, m_data.m_ndop, offset+14); - XsMessage_setDataShort(&msg, m_data.m_edop, offset+16); - } - - int sizeInMsg() const override - { - return 18; - } - }; - - struct XsRawGpsSolVariant : public Variant - { - XsRawGpsSolVariant(XsDataIdentifier id) : Variant(id), m_data() {} - XsRawGpsSolVariant(XsDataIdentifier id, XsRawGpsSol const& val) : Variant(id), m_data(val) {} - Variant* clone() const override - { - return new XsRawGpsSolVariant(dataId(), m_data); - } - - XsRawGpsSol m_data; - void readFromMessage(XsMessage const& msg, int offset, int /*ignored*/) override - { - m_data.m_itow = XsMessage_getDataLong (&msg, offset); - m_data.m_frac = XsMessage_getDataLong (&msg, offset+4); - m_data.m_week = XsMessage_getDataShort(&msg, offset+8); - m_data.m_gpsfix = XsMessage_getDataByte (&msg, offset+10); - m_data.m_flags = XsMessage_getDataByte (&msg, offset+11); - m_data.m_ecef_x = XsMessage_getDataLong (&msg, offset+12); - m_data.m_ecef_y = XsMessage_getDataLong (&msg, offset+16); - m_data.m_ecef_z = XsMessage_getDataLong (&msg, offset+20); - m_data.m_pacc = XsMessage_getDataLong (&msg, offset+24); - m_data.m_ecef_vx= XsMessage_getDataLong (&msg, offset+28); - m_data.m_ecef_vy= XsMessage_getDataLong (&msg, offset+32); - m_data.m_ecef_vz= XsMessage_getDataLong (&msg, offset+36); - m_data.m_sacc = XsMessage_getDataLong (&msg, offset+40); - m_data.m_pdop = XsMessage_getDataShort(&msg, offset+44); - m_data.m_res1 = XsMessage_getDataByte (&msg, offset+46); - m_data.m_numsv = XsMessage_getDataByte (&msg, offset+47); - m_data.m_res2 = XsMessage_getDataLong (&msg, offset+48); - } - void writeToMessage(XsMessage& msg, int offset) const override - { - XsMessage_setDataLong (&msg, m_data.m_itow , offset); - XsMessage_setDataLong (&msg, m_data.m_frac , offset+4); - XsMessage_setDataShort(&msg, m_data.m_week , offset+8); - XsMessage_setDataByte (&msg, m_data.m_gpsfix , offset+10); - XsMessage_setDataByte (&msg, m_data.m_flags , offset+11); - XsMessage_setDataLong (&msg, m_data.m_ecef_x , offset+12); - XsMessage_setDataLong (&msg, m_data.m_ecef_y , offset+16); - XsMessage_setDataLong (&msg, m_data.m_ecef_z , offset+20); - XsMessage_setDataLong (&msg, m_data.m_pacc , offset+24); - XsMessage_setDataLong (&msg, m_data.m_ecef_vx , offset+28); - XsMessage_setDataLong (&msg, m_data.m_ecef_vy , offset+32); - XsMessage_setDataLong (&msg, m_data.m_ecef_vz , offset+36); - XsMessage_setDataLong (&msg, m_data.m_sacc , offset+40); - XsMessage_setDataShort(&msg, m_data.m_pdop , offset+44); - XsMessage_setDataByte (&msg, m_data.m_res1 , offset+46); - XsMessage_setDataByte (&msg, m_data.m_numsv , offset+47); - XsMessage_setDataLong (&msg, m_data.m_res2 , offset+48); - } - - int sizeInMsg() const override - { - return 52; - } - }; - - struct XsRawGpsTimeUtcVariant : public Variant - { - XsRawGpsTimeUtcVariant(XsDataIdentifier id) : Variant(id), m_data() {} - XsRawGpsTimeUtcVariant(XsDataIdentifier id, XsRawGpsTimeUtc const& val) : Variant(id), m_data(val) {} - Variant* clone() const override - { - return new XsRawGpsTimeUtcVariant(dataId(), m_data); - } - - XsRawGpsTimeUtc m_data; - void readFromMessage(XsMessage const& msg, int offset, int /*ignored*/) override - { - m_data.m_itow = XsMessage_getDataLong (&msg, offset); - m_data.m_tacc = XsMessage_getDataLong (&msg, offset+4); - m_data.m_nano = XsMessage_getDataLong (&msg, offset+8); - m_data.m_year = XsMessage_getDataShort(&msg, offset+12); - m_data.m_month = XsMessage_getDataByte (&msg, offset+14); - m_data.m_day = XsMessage_getDataByte (&msg, offset+15); - m_data.m_hour = XsMessage_getDataByte (&msg, offset+16); - m_data.m_min = XsMessage_getDataByte (&msg, offset+17); - m_data.m_sec = XsMessage_getDataByte (&msg, offset+18); - m_data.m_valid = XsMessage_getDataByte (&msg, offset+19); - } - void writeToMessage(XsMessage& msg, int offset) const override - { - XsMessage_setDataLong (&msg, m_data.m_itow , offset); - XsMessage_setDataLong (&msg, m_data.m_tacc , offset+4); - XsMessage_setDataLong (&msg, m_data.m_nano , offset+8); - XsMessage_setDataShort(&msg, m_data.m_year , offset+12); - XsMessage_setDataByte (&msg, m_data.m_month, offset+14); - XsMessage_setDataByte (&msg, m_data.m_day , offset+15); - XsMessage_setDataByte (&msg, m_data.m_hour , offset+16); - XsMessage_setDataByte (&msg, m_data.m_min , offset+17); - XsMessage_setDataByte (&msg, m_data.m_sec , offset+18); - XsMessage_setDataByte (&msg, m_data.m_valid, offset+19); - } - - int sizeInMsg() const override - { - return 20; - } - }; - - struct XsRawGpsSvInfoVariant : public Variant - { - XsRawGpsSvInfoVariant(XsDataIdentifier id) : Variant(id), m_data() {} - XsRawGpsSvInfoVariant(XsDataIdentifier id, XsRawGpsSvInfo const& val) : Variant(id), m_data(val) {} - Variant* clone() const override - { - return new XsRawGpsSvInfoVariant(dataId(), m_data); - } - - XsRawGpsSvInfo m_data; - void readFromMessage(XsMessage const& msg, int offset, int /*ignored*/) override - { - m_data.m_itow = XsMessage_getDataLong(&msg, offset); - m_data.m_nch = XsMessage_getDataByte( &msg, offset+4); - m_data.m_res1 = XsMessage_getDataByte(&msg, offset+5); - m_data.m_res2= XsMessage_getDataShort(&msg, offset+6); - - offset += 8; - for (int i = 0; i < m_data.m_nch; ++i) - { - m_data.m_svInfos[i].m_chn = XsMessage_getDataByte( &msg, offset+0); - m_data.m_svInfos[i].m_svid = XsMessage_getDataByte( &msg, offset+1); - m_data.m_svInfos[i].m_flags = XsMessage_getDataByte( &msg, offset+2); - m_data.m_svInfos[i].m_qi = XsMessage_getDataByte( &msg, offset+3); - m_data.m_svInfos[i].m_cno = XsMessage_getDataByte( &msg, offset+4); - m_data.m_svInfos[i].m_elev = XsMessage_getDataByte( &msg, offset+5); - m_data.m_svInfos[i].m_azim = XsMessage_getDataShort(&msg, offset+6); - m_data.m_svInfos[i].m_prres = XsMessage_getDataLong( &msg, offset+8); - offset += 12; - } - } - void writeToMessage(XsMessage& msg, int offset) const override - { - XsMessage_setDataLong(&msg, m_data.m_itow, offset); - XsMessage_setDataByte( &msg, m_data.m_nch , offset+4); - XsMessage_setDataByte(&msg, m_data.m_res1, offset+5); - XsMessage_setDataShort(&msg, m_data.m_res2, offset+6); - - offset += 8; - for (int i = 0; i < m_data.m_nch; ++i) - { - XsMessage_setDataByte( &msg, m_data.m_svInfos[i].m_chn , offset+0); - XsMessage_setDataByte( &msg, m_data.m_svInfos[i].m_svid , offset+1); - XsMessage_setDataByte( &msg, m_data.m_svInfos[i].m_flags, offset+2); - XsMessage_setDataByte( &msg, m_data.m_svInfos[i].m_qi , offset+3); - XsMessage_setDataByte( &msg, m_data.m_svInfos[i].m_cno , offset+4); - XsMessage_setDataByte( &msg, m_data.m_svInfos[i].m_elev , offset+5); - XsMessage_setDataShort(&msg, m_data.m_svInfos[i].m_azim , offset+6); - XsMessage_setDataLong( &msg, m_data.m_svInfos[i].m_prres, offset+8); - offset += 12; - } - } - - int sizeInMsg() const override - { - return 8 + 12*m_data.m_nch; - } - }; - - struct XsFullSnapshotVariant : public Variant - { - XsFullSnapshotVariant(XsDataIdentifier id) : Variant(id) {} - - XsFullSnapshotVariant(XsDataIdentifier id, XsSnapshot const& val) : Variant(id), m_data(val) {} - - Variant* clone() const override - { - return new XsFullSnapshotVariant(dataId(), m_data); - } - - XsSnapshot m_data; - void readFromMessage(XsMessage const& msg, int offset, int /*ignored*/) 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; - } - - void writeToMessage(XsMessage& msg, int offset) const override - { - assert(m_data.m_type == ST_Full); - XsMessage_setDataShort(&msg, 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); - } - - int sizeInMsg() const override - { - return 70; - } - }; - - struct XsAwindaSnapshotVariant : public Variant - { - XsAwindaSnapshotVariant(XsDataIdentifier id) : Variant(id) - { - m_data.m_type = ST_Awinda; - } - 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); - } - - XsSnapshot m_data; - void readFromMessage(XsMessage const& msg, int offset, int /*ignored*/) 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; - } - void writeToMessage(XsMessage& msg, int offset) const override - { - assert(m_data.m_type == ST_Awinda); - XsMessage_setDataLong(&msg, m_data.m_deviceId.toInt(), 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, 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); - } - - int sizeInMsg() const override - { - return 46; - } - }; - - struct XsByteArrayVariant : public Variant - { - XsByteArrayVariant(XsDataIdentifier id) : Variant(id) - { - } - XsByteArrayVariant(XsDataIdentifier id, XsByteArray const& val) : Variant(id), m_data(val) - { - } - Variant* clone() const override - { - return new XsByteArrayVariant(dataId(), m_data); - } - - XsByteArray m_data; - void readFromMessage(XsMessage const& msg, int offset, int dSize) override - { - m_data.assign(dSize, XsMessage_getDataBuffer(&msg, offset)); - } - void writeToMessage(XsMessage& msg, int offset) const override - { - XsMessage_setDataBuffer(&msg, m_data.data(), m_data.size(), offset); - } - - int sizeInMsg() const override - { - return (int) m_data.size(); - } - }; -} - -typedef std::map MapType; - -struct DataPacketPrivate : private MapType { - DataPacketPrivate() {} - DataPacketPrivate(DataPacketPrivate const& p); - ~DataPacketPrivate(); - const DataPacketPrivate& operator = (const DataPacketPrivate& p); - void erase(XsDataIdentifier id); - void erase(MapType::const_iterator it); - MapType::iterator insert(XsDataIdentifier id, XsDataPacket_Private::Variant* var); - - void clear(); - void merge(DataPacketPrivate const& other, bool overwrite); - - MapType::const_iterator find(XsDataIdentifier id) const; - - using MapType::begin; - using MapType::end; - using MapType::size; - using MapType::empty; -}; - -/*! \endcond */ - -#endif diff --git a/extern/xstypes/resource.h b/extern/xstypes/resource.h deleted file mode 100644 index 32d047a..0000000 --- a/extern/xstypes/resource.h +++ /dev/null @@ -1,29 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -//{{NO_DEPENDENCIES}} -// Microsoft Visual C++ generated include file. -// Used by C:\dev\trunk_svn\xstypes\xstypes.rc -// - -// 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 -#endif diff --git a/extern/xstypes/xsarray.c b/extern/xstypes/xsarray.c deleted file mode 100644 index 8349e2e..0000000 --- a/extern/xstypes/xsarray.c +++ /dev/null @@ -1,696 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#include "xsarray.h" -#include "xsatomicint.h" -#include -#include - -#ifndef XSENS_NO_ALLOC_TRACKING -XsAtomicInt XSTYPES_DLL_API XsArray_allocCount = XSATOMICINT_INITIALIZER; //!< The number of times XsArray_ functions have allocated memory -XsAtomicInt XSTYPES_DLL_API XsArray_freeCount = XSATOMICINT_INITIALIZER; //!< The number of times XsArray_ functions have freed memory -#define INC_ALLOC() (void)XsAtomicInt_preIncrement(&XsArray_allocCount) -#define INC_FREE() (void)XsAtomicInt_preIncrement(&XsArray_freeCount) -#else -#define INC_ALLOC() ((void)0) -#define INC_FREE() ((void)0) -#endif - -/*! \class XsArray - \brief Provides generic storage for data in an array and manipulation operations on that data -*/ - -/*! \addtogroup cinterface C Interface - @{ -*/ - -/*! \cond NODOXYGEN */ -#define elemSize(thisArray) (thisArray->m_descriptor->itemSize) -#define elemAtX(b, i, thisArray) ((void*)(((char*) (b))+((i)*elemSize(thisArray)))) -#define elemAt(b, i) elemAtX(b, i, thisArray) -/*! \endcond */ - -/*! \relates XsArray - \brief Initializes the XsArray 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 descriptor The descriptor of the data in the list - \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_construct(void* thisPtr, XsArrayDescriptor const* const descriptor, XsSize count, void const* src) -{ - XsArray* thisArray = (XsArray*) thisPtr; - *((XsArrayDescriptor const**) &thisArray->m_descriptor) = descriptor; - *((XsSize*) &thisArray->m_size) = count; - - if (thisArray->m_size) - { - // init to size - *((void**) &thisArray->m_data) = malloc(thisArray->m_size*elemSize(thisArray)); - INC_ALLOC(); - - // 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) - 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) - thisArray->m_descriptor->itemConstruct(elemAt(thisArray->m_data, i)); - } - } - else - *((void**) &thisArray->m_data) = 0; - *((XsSize*) &thisArray->m_reserved) = thisArray->m_size; - *((int*) &thisArray->m_flags) = XSDF_Managed; -} - -/*! \relates XsArray - \brief Initializes the XsArray with a copy of \a src - \param src A pointer to the objects to copy. The object may be empty, but src may not be 0 -*/ -void XsArray_copyConstruct(void* thisPtr, void const* src) -{ - XsArray* thisArray = (XsArray*) thisPtr; - XsArray const* srcArray = (XsArray const*) src; - assert(srcArray); - XsArray_construct(thisArray, srcArray->m_descriptor, srcArray->m_size, srcArray->m_data); -} - -/*! \relates XsArray - \brief Clears and frees memory allocated by the XsArray - \note After XsArray_destruct is called, the object is empty but valid, - ie. it can be used as if XsArray_construct has been called on it. -*/ -void XsArray_destruct(void* thisPtr) -{ - XsArray* thisArray = (XsArray*) thisPtr; - if (thisArray->m_data && (thisArray->m_flags & XSDF_Managed)) - { - XsSize i; - // clear contents - if (thisArray->m_descriptor->itemDestruct) - for (i=0; im_reserved; ++i) - thisArray->m_descriptor->itemDestruct(elemAt(thisArray->m_data, i)); - free((void*) thisArray->m_data); - INC_FREE(); - } - // init to 0 - *((void**) &thisArray->m_data) = 0; - *((XsSize*) &thisArray->m_size) = 0; - *((XsSize*) &thisArray->m_reserved) = 0; - *((int*) &thisArray->m_flags) = (thisArray->m_flags & (XSDF_Managed | XSDF_FixedSize)); -} - -/*! \relates XsArray - \brief Reinitializes the XsArray with space for \a count items and copies them from \a src - \details This function reinitializes the object reserving space for at least \a count items in the - buffer. \a count may be 0. If \a src is not 0, \a count items will be copied from \a src. - Previous data will be cleared automatically, but the reserved space will not be reduced. - \param count the number of items in src - \param src a pointer to an array of items - \sa XsArray_reserve -*/ -void XsArray_assign(void* thisPtr, XsSize count, void const* src) -{ - XsSize i; - XsArray* thisArray = (XsArray*) thisPtr; - - // check if we need to reallocate - if (count > thisArray->m_reserved) - { - assert(thisArray->m_flags & XSDF_Managed); - - if (thisArray->m_data) - XsArray_destruct(thisArray); - XsArray_construct(thisArray, thisArray->m_descriptor, count, src); - return; - } - - // no reallocation necessary, clear excess objects - if (thisArray->m_descriptor->itemDestruct) - for (i=count; im_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)); - } - } - - *((XsSize*) &thisArray->m_size) = count; -} - -/*! \relates XsArray - \brief Resizes the existing list to \a count items - \details This function will keep the data of the remaining items intact. - \param count the number of items the list should have - \sa XsArray_reserve \sa XsArray_assign -*/ -void XsArray_resize(void* thisPtr, XsSize count) -{ - XsArray* thisArray = (XsArray*) thisPtr; - if (thisArray->m_size == count) - return; - if (thisArray->m_size == 0) - { - XsArray_assign(thisArray, count, 0); - return; - } - if (count < thisArray->m_size) - { - XsArray_erase(thisArray, count, thisArray->m_size - count); - return; - } - if (count > thisArray->m_reserved) - XsArray_reserve(thisArray, count); - *((XsSize*) &thisArray->m_size) = count; -} - -/*! \relates XsArray - \brief Reserves space for \a count items - \details This function reserves space for at least \a count items. The function will retain the - current data in the list. - \param count The number of items to reserve space for. When set to 0 or 1, the buffer is resized to the - minimum required size to contain the current contents. Otherwise reserving data will only - expand the buffer, never shrink it. - \sa XsArray_assign -*/ -void XsArray_reserve(void* thisPtr, XsSize count) -{ - XsArray* thisArray = (XsArray*) thisPtr; - XsArray tmp = { 0, thisArray->m_size, 0, XSDF_Managed, thisArray->m_descriptor }; - XsSize i; - - if (count && count <= thisArray->m_reserved) - return; - - if (count < thisArray->m_size) - count = thisArray->m_size; - - if (count == thisArray->m_reserved) - return; - - if (!(thisArray->m_flags & XSDF_Managed)) - { - // attempting this on an unmanaged list is ignored silently - return; - } - - if (!count) - { - XsArray_destruct(thisArray); - return; - } - - *((XsSize*) &tmp.m_reserved) = count; - - // init to size - *((void**) &tmp.m_data) = malloc(tmp.m_reserved*elemSize(thisArray)); - INC_ALLOC(); - - if (thisArray->m_descriptor->itemConstruct) - for (i=0; im_descriptor->itemConstruct(elemAt(tmp.m_data, i)); - - for (i=0; im_size; ++i) - thisArray->m_descriptor->itemSwap(elemAt(thisArray->m_data, i), elemAt(tmp.m_data, i)); - - XsArray_destruct(thisArray); - XsArray_swap(thisArray, &tmp); -} - -/*! \relates XsArray - \brief Copy the contents of \a src to thisArray */ -void XsArray_copy(void* thisPtr, void const* src) -{ - XsArray* thisArray = (XsArray*) thisPtr; - XsArray const* srcArray = (XsArray const*) src; - - if (srcArray == thisArray) - { - return; - } - XsArray_assign(thisArray, srcArray->m_size, srcArray->m_data); -} - -/*! \relates XsArray - \brief Appends the \a other list to thisArray list - \param other The list to append to thisArray list. \a other may point to thisArray list -*/ -void XsArray_append(void* thisPtr, void const* other) -{ - XsSize i; - XsArray* thisArray = (XsArray*) thisPtr; - XsArray const* otherArray = (XsArray const*) other; - - if (otherArray->m_size == 0) - return; - - if (otherArray == thisArray) - { - if (thisArray->m_size + thisArray->m_size > thisArray->m_reserved) - 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)); - } - - *((XsSize*) &thisArray->m_size) = thisArray->m_size + thisArray->m_size; - return; - } - - if (thisArray->m_size == 0) - { - XsArray_copy(thisArray, otherArray); - return; - } - - if (thisArray->m_size + otherArray->m_size > thisArray->m_reserved) - 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)); - } - - *((XsSize*) &thisArray->m_size) = thisArray->m_size + otherArray->m_size; -} - -/*! \relates XsArray - \brief Insert \a count items from \a src at \a index in the array - \param index The index to use for inserting. Anything beyond the end of the array (ie. -1) will - append to the actual end of the array. - \param count The number of items to insert - \param src The items to insert, may not be 0 unless count is 0 -*/ -void XsArray_insert(void* thisPtr, XsSize index, XsSize count, void const* src) -{ - XsSize s; - XsArray* thisArray = (XsArray*) thisPtr; - int i,d = (int) 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 - - // 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 = ((int)thisArray->m_size)-1; i >= (int) index; --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)); - } - - // update size - *((XsSize*) &thisArray->m_size) = thisArray->m_size + count; -} - -/*! \relates XsArray - \brief Swap the contents of \a a with those of \a b - \details Where possible, the pointers and administrative values are swapped. If for some reason - thisArray is not possible, the lists are swapped one element at a time. - \param a The list to swap with \a b - \param b The list to swap with \a a -*/ -void XsArray_swap(void* a, void* b) -{ - XsArray* aArray = (XsArray*) a; - XsArray* bArray = (XsArray*) b; - - if (!aArray->m_data && !bArray->m_data) - return; - if ((!aArray->m_data || (aArray->m_flags & XSDF_Managed)) && (!bArray->m_data || (bArray->m_flags & XSDF_Managed))) - { - // administrative swap - XsArray tmp; - *((void**) &tmp.m_data) = aArray->m_data; - *((void**) &aArray->m_data) = bArray->m_data; - *((void**) &bArray->m_data) = tmp.m_data; - - *((XsSize*) &tmp.m_size) = aArray->m_size; - *((XsSize*) &aArray->m_size) = bArray->m_size; - *((XsSize*) &bArray->m_size) = tmp.m_size; - - *((XsSize*) &tmp.m_reserved) = aArray->m_reserved; - *((XsSize*) &aArray->m_reserved) = bArray->m_reserved; - *((XsSize*) &bArray->m_reserved) = tmp.m_reserved; - - *((int*) &tmp.m_flags) = aArray->m_flags; - *((int*) &aArray->m_flags) = bArray->m_flags; - *((int*) &bArray->m_flags) = tmp.m_flags; - } else { - // elementwise swap - XsSize i; - assert(aArray->m_size == bArray->m_size); - for (i = 0; i < aArray->m_size; ++i) - aArray->m_descriptor->itemSwap(elemAtX(aArray->m_data, i, aArray), elemAtX(bArray->m_data, i, bArray)); - } -} - -/*! \relates XsArray - \brief Removes a \a count items from the list starting at \a index -*/ -void XsArray_erase(void* thisPtr, XsSize index, XsSize count) -{ - XsSize i, newCount; - XsArray* thisArray = (XsArray*) thisPtr; - - if (count == 0 || index >= thisArray->m_size) - return; - - 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)); - - *((XsSize*) &thisArray->m_size) = newCount; -} - -/*! \relates XsArray - \brief Returns non-zero if the lists are different, 0 if they're equal - \details This function compares the two lists in-order - \param a The left hand side of the comparison - \param b The right hand side of the comparison - \return -1 if \a a is smaller in some way than \a b, 1 if it is larger in some way - and 0 if both lists are equal. Please note that not all lists have items that can be accurately - tested for less than or greater than, but can be tested for (in-)equality. So the sign of the - return value should be treated with knowledge of the data type in mind. - \sa XsArray_compareSet \sa XsArray_comparePredicate -*/ -int XsArray_compare(void const* a, void const* b) -{ - XsSize i; - XsArray const* aArray = (XsArray const*) a; - XsArray const* bArray = (XsArray const*) b; - - if (aArray == bArray) - return 0; - - if (aArray->m_size != bArray->m_size) - 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 - for (i = 0; i < aArray->m_size; ++i) // loop over all elements of the lists - { - int r = aArray->m_descriptor->itemCompare(elemAtX(aArray->m_data, i, aArray), elemAtX(bArray->m_data, i, bArray)); - if (r) - return r; - } - return 0; -} - -/*! \relates XsArray - \brief Returns non-zero if the lists are different, 0 if they're equal - \details This function compares the two lists in-order - \param a The left hand side of the comparison - \param b The right hand side of the comparison - \param predicate The custom comparison predicate to use. It should return 0 when the items are considered equal (see return values below). - \return -1 if \a a is smaller in some way than \a b, 1 if it is larger in some way - and 0 if both lists are equal. Please note that not all lists have items that can be accurately - tested for less than or greater than, but can be tested for (in-)equality. So the sign of the - return value should be treated with knowledge of the data type in mind. - \sa XsArray_compareSet \sa XsArray_compare -*/ -int XsArray_comparePredicate(void const* a, void const* b, XsArrayItemCompareFunc predicate) -{ - XsSize i; - XsArray const* aArray = (XsArray const*) a; - XsArray const* bArray = (XsArray const*) b; - - if (aArray == bArray) - return 0; - - if (aArray->m_size != bArray->m_size) - 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 - for (i = 0; i < aArray->m_size; ++i) // loop over all elements of the lists - { - int r = predicate(elemAtX(aArray->m_data, i, aArray), elemAtX(bArray->m_data, i, bArray)); - if (r) - return r; - } - return 0; -} - -/*! \relates XsArray - \brief Returns -1 if \a a is smaller in some way than \a b, 1 if it is larger in some way and 0 if both lists are equal - \details This function compares the two lists out-of-order - \param a The left hand side of the comparison - \param b The right hand side of the comparison - \return -1 if \a a is smaller in some way than \a b, 1 if it is larger in some way - \note It is assumed that both arguments are sets i.e. they contain no duplicates. It is the responsibility of the caller to provide sets as arguments - and 0 if both lists are equal. Please note that not all lists have items that can be accurately - tested for less than or greater than, but can be tested for (in-)equality. So the sign of the - return value should be treated with knowledge of the data type in mind. - \sa XsArray_compare \sa XsArray_comparePredicate -*/ -int XsArray_compareSet(void const* a, void const* b) -{ - XsSize n, m; - XsArray const* aArray = (XsArray const*) a; - XsArray const* bArray = (XsArray const*) b; - - if (aArray == bArray) - return 0; - - if (aArray->m_size != bArray->m_size) - return (aArray->m_size < bArray->m_size)?-1:1; - - for (n = 0; n < aArray->m_size; ++n) // loop over all elements of list aArray - { - int found = 0; - for (m = 0; m < bArray->m_size; ++m) // loop over all elements of list bArray - { - if (aArray->m_descriptor->itemCompare(elemAtX(aArray->m_data, n, aArray), elemAtX(bArray->m_data, m, bArray)) == 0) - { - found = 1; - break; - } - } - if (!found) - return -1; - } - return 0; -} - -/*! \relates XsArray - \brief Returns the index of \a needle in the list or -1 if it wasn't found - \details The search does not assume any kind of ordering of the items so in a worst-case scenario it - will go through the entire list. - \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) -{ - XsSize i; - XsArray const* thisArray = (XsArray const*) thisPtr; - - assert(thisArray->m_descriptor->itemCompare); - // 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 -1; -} - -/*! \relates XsArray - \brief Returns the index of \a needle in the list or -1 if it wasn't found - \details The search does not assume any kind of ordering of the items so in a worst-case scenario it - will go through the entire list. It will use the supplied predicate to match items against \a needle. - \param needle A pointer to the value to search for - \param predicate A compare function that returns 0 if the values are equal. The arguments are (const void) - 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) -{ - XsSize i; - XsArray const* thisArray = (XsArray const*) thisPtr; - - assert(predicate); - // 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 -1; -} - -/*! \relates XsArray - \brief Returns a pointer to the item at the supplied \a index or a null pointer if it is out of bounds - \param index The index of the item to return - \returns A pointer to the item or NULL if \a index is out of bounds -*/ -void const* XsArray_at(void const* thisPtr, XsSize index) -{ - XsArray const* thisArray = (XsArray const*) thisPtr; - if (index >= thisArray->m_size) - return 0; - return elemAt(thisArray->m_data, index); -} - -/*! \relates XsArray - \brief Returns a pointer to the item at the supplied \a index or a null pointer if it is out of bounds - \param index The index of the item to return - \returns A pointer to the item or NULL if \a index is out of bounds -*/ -void* XsArray_atIndex(void* thisPtr, XsSize index) -{ - XsArray* thisArray = (XsArray*) thisPtr; - if (index >= thisArray->m_size) - return 0; - return elemAt(thisArray->m_data, index); -} - -/*! \relates XsArray - \brief Removes duplicate entries from the array, keeping only the first instance of each value -*/ -void XsArray_removeDuplicates(void* thisPtr) -{ - XsSize i,j; - XsArray* thisArray = (XsArray*) thisPtr; - if (thisArray->m_size > 1) - { - for (i = 0; i < thisArray->m_size-1; ++i) - { - 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); - } - } - } - } -} - -/*! \relates XsArray - \brief Removes duplicate entries from the array, keeping only the first instance of each value - \param predicate A compare function that returns 0 if the values are equal. The arguments are (const void) -*/ -void XsArray_removeDuplicatesPredicate(void* thisPtr, XsArrayItemCompareFunc predicate) -{ - XsSize i,j; - XsArray* thisArray = (XsArray*) thisPtr; - if (thisArray->m_size > 1) - { - for (i = 0; i < thisArray->m_size-1; ++i) - { - 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); - } - } - } - } -} - -/*! \relates XsArray - \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. -*/ -int XsArray_empty(void const* thisPtr) -{ - XsArray const* thisArray = (XsArray const*) thisPtr; - return (thisArray->m_size == 0) || (thisArray->m_data == 0) || (thisArray->m_flags & XSDF_Empty); -} - -/*! \relates XsArray - \brief Copies items optimized in a direct way - \details This function copies items directly, using memcpy. This works for simple types only. - \param to Pointer to array to copy to. - \param from Pointer to array to copy from. - \param count The number of items to copy. - \param iSize The size of an individual item -*/ -void XsArray_rawCopy(void* to, void const* from, XsSize count, XsSize iSize) -{ - memcpy(to, from, count*iSize); -} - -/*! \relates XsArray - \brief Sorts the array using the \a itemCompare func in the XsArrayDescriptor - \details This function sorts using qsort -*/ -void XsArray_sort(void* thisPtr) -{ - XsArray* thisArray = (XsArray*) thisPtr; - qsort(thisArray->m_data, thisArray->m_size, thisArray->m_descriptor->itemSize, thisArray->m_descriptor->itemCompare); -} - -/*! \relates XsArray - \brief Reverses the contents of the array by repeatedly using the \a itemSwap func in the XsArrayDescriptor - \details This reverses the contents in-place -*/ -void XsArray_reverse(void* thisPtr) -{ - XsSize i, half; - 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)); -} - -/*! @} */ diff --git a/extern/xstypes/xsbaud.c b/extern/xstypes/xsbaud.c deleted file mode 100644 index ebe36f6..0000000 --- a/extern/xstypes/xsbaud.c +++ /dev/null @@ -1,138 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#include "xsbaud.h" - -/*! \addtogroup cinterface C Interface - @{ -*/ - -/*! \brief Convert an Xsens baud code to XsBaudRate - - \param baudcode : The code to convert to an XsBaudRate - - \returns An XsBaudrate -*/ -XsBaudRate XsBaud_codeToRate(XsBaudCode 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; - } -} - -/*! \brief Convert a XsBaudRate to an Xsens baud code - - \param baudrate : The code to convert to an XsBaudCode - - \returns An XsBaudCode -*/ -XsBaudCode XsBaud_rateToCode(XsBaudRate 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; - } -} - -/*! \brief Convert a XsBaudrate to a numeric baudrate in bps - - \param baudrate : The XsBaudRate to convert to a numeric baudrate - - \returns A baudrate in bps -*/ -int XsBaud_rateToNumeric(XsBaudRate 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; - } -} - -/*! \brief Convert a numeric baudrate in bps to XsBaudrate - - \param numeric : The numeric baudrate to convert to XsBaudRate - - \returns A XsBaudrate -*/ -XsBaudRate XsBaud_numericToRate(int 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; - } -} - - /*! @} */ diff --git a/extern/xstypes/xsbytearray.c b/extern/xstypes/xsbytearray.c deleted file mode 100644 index c222ce1..0000000 --- a/extern/xstypes/xsbytearray.c +++ /dev/null @@ -1,68 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#include "xsbytearray.h" - -/*! \struct XsByteArray - \brief A list of uint8_t values - \sa XsArray -*/ - -/*! \copydoc XsArrayDescriptor::itemSwap - \note Specialization for uint8_t*/ -void swapUint8(uint8_t* a, uint8_t* b) -{ - uint8_t tmp = *a; - *a = *b; - *b = tmp; -} - -/*! \copydoc XsArrayDescriptor::itemCopy - \note Specialization for uint8_t*/ -void copyUint8(uint8_t* to, uint8_t const* from) -{ - *to = *from; -} - -/*! \copydoc XsArrayDescriptor::itemCompare - \note Specialization for uint8_t*/ -int compareUint8(uint8_t const* a, uint8_t const* b) -{ - if (*a < *b) - return -1; - if (*a > *b) - return 1; - return 0; -} - -//! \brief Descriptor for XsByteArray -XsArrayDescriptor const g_xsByteArrayDescriptor = { - //lint --e{64} ignore exact type mismatches here - sizeof(uint8_t), - XSEXPCASTITEMSWAP swapUint8, // swap - 0, // construct - XSEXPCASTITEMCOPY copyUint8, // copy construct - 0, // destruct - XSEXPCASTITEMCOPY copyUint8, // copy - XSEXPCASTITEMCOMP compareUint8, // compare - XSEXPCASTRAWCOPY XsArray_rawCopy // raw copy -}; - -/*! \copydoc XsArray_construct - \note Specialization for XsByteArray -*/ -void XsByteArray_construct(XsByteArray* thisPtr, XsSize count, uint8_t const* src) -{ - XsArray_construct(thisPtr, &g_xsByteArrayDescriptor, count, src); -} diff --git a/extern/xstypes/xscalibrateddata.c b/extern/xstypes/xscalibrateddata.c deleted file mode 100644 index 2a964e1..0000000 --- a/extern/xstypes/xscalibrateddata.c +++ /dev/null @@ -1,46 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#include "xscalibrateddata.h" - -/*! \struct XsCalibratedData - \brief Container for combined calibrated measurement data from accelerometers, gyroscopes and - magnetometers. -*/ - -/*! \addtogroup cinterface C Interface - @{ -*/ - -/*! \relates XsCalibratedData - \brief Construct an %XsCalibratedData object -*/ -void XsCalibratedData_construct(XsCalibratedData* thisPtr, const XsReal* acc, const XsReal* gyr, const XsReal* mag) -{ - XsVector3_construct(&thisPtr->m_acc, acc); - XsVector3_construct(&thisPtr->m_gyr, gyr); - XsVector3_construct(&thisPtr->m_mag, mag); -} - -/*! \relates XsCalibratedData - \brief Destruct an %XsCalibratedData object -*/ -void XsCalibratedData_destruct(XsCalibratedData* thisPtr) -{ - XsVector3_destruct(&thisPtr->m_acc); - XsVector3_destruct(&thisPtr->m_gyr); - XsVector3_destruct(&thisPtr->m_mag); -} - -/*! @} */ diff --git a/extern/xstypes/xsdatapacket.cpp b/extern/xstypes/xsdatapacket.cpp deleted file mode 100644 index 134df1a..0000000 --- a/extern/xstypes/xsdatapacket.cpp +++ /dev/null @@ -1,2445 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -//lint -e1512 -e1509 -e1511 -#include "xsdatapacket.h" -#include "datapacket_p.h" -#include "xsmatrix3x3.h" -#include "xsvector.h" -#include "xsrssi.h" -#include "xsmath.h" -#include - -#define MAP (*thisPtr->d) - -static const uint64_t coarseFactor = 10000ULL; - -using namespace XsDataPacket_Private; - -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 XsUtcTimeVariant(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_AngularVelocityGroup :// 0x8000, - case XDI_RateOfTurn :// 0x8020, - case XDI_RateOfTurnHR :// 0x8040, - return new XsVector3Variant(id); - case XDI_DeltaQ :// 0x8030, - return new XsQuaternionVariant(id); - - //case XDI_GpsGroup :// 0x8800, //!< \deprecated - case XDI_GpsDop :// 0x8830, //!< \deprecated - return new XsRawGpsDopVariant(id); - case XDI_GpsSol :// 0x8840, //!< \deprecated - return new XsRawGpsSolVariant(id); - case XDI_GpsTimeUtc :// 0x8880, //!< \deprecated - return new XsRawGpsTimeUtcVariant(id); - case XDI_GpsSvInfo :// 0x88A0, //!< \deprecated - return new XsRawGpsSvInfoVariant(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_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_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); - - default: - //JLERRORG("Unknown id: " << id); - assert(0); - return nullptr; - } -} - -XsUShortVector* rawVector(const XsDataPacket* thisPtr, XsUShortVector* returnVal, XsDataIdentifier id, XsUShortVector XsScrData::* field) -{ - assert(returnVal); - auto it = MAP.find(XDI_RawAccGyrMagTemp); - if (it != MAP.end()) - *returnVal = it->second->toDerived().m_data.*field; - else - { - it = MAP.find(id); - if (it != MAP.end()) - *returnVal = it->second->toDerived().m_data; - } - return returnVal; -} - -void setRawVector(XsDataPacket* thisPtr, const XsUShortVector* vec, XsDataIdentifier id, XsUShortVector XsScrData::* field) -{ - auto it = MAP.find(XDI_RawAccGyrMagTemp); - if (it != MAP.end()) - it->second->toDerived().m_data.*field = *vec; - else - { - it = MAP.find(id); - if (it != MAP.end()) - it->second->toDerived().m_data = *vec; - else - MAP.insert(id, new XsUShortVectorVariant(id, *vec)); - } -} - -template -T* genericGet(const XsDataPacket* thisPtr, T* returnVal, XsDataIdentifier id, T const& failValue = T()) -{ - assert(returnVal); - auto it = MAP.find(id); - if (it != MAP.end()) - *returnVal = it->second->toDerived().m_data; - else - *returnVal = failValue; - return returnVal; -} - -template -void genericSet(XsDataPacket* thisPtr, T const* val, XsDataIdentifier id) -{ - assert(val); - auto it = MAP.find(id); - if (it != MAP.end()) - { - it->second->toDerived().m_data = *val; - it->second->setDataId(id); - } - else - MAP.insert(id, new V(id, *val)); -} - -template > -struct GenericSimple { -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) -{ - 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) -{ - return MAP.find(id) != MAP.end(); -} - -/*! \cond XS_INTERNAL */ -/*! \relates XsDataPacket - \brief Check if data item contains quaternion orientation data - \returns true if this packet contains quaternion orientation data -*/ -int XsDataPacket_containsOrientationQuaternion(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_Quaternion); -} - -/*! \relates XsDataPacket - \brief Check if data item contains euler orientation data - \returns true if this packet contains euler orientation data -*/ -int XsDataPacket_containsOrientationEuler(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_EulerAngles); -} - -/*! \relates XsDataPacket - \brief Check if data item contains matrix orientation data - \returns true if this packet contains matrix orientation data -*/ -int XsDataPacket_containsOrientationMatrix(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_RotationMatrix); -} - -/*! \brief Returns quaternion that defines the rotation necessary to rotate from coordinate system - \a actual to \a desired when used as the left side in a quaternion multiplication -*/ -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_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); - - 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; - - case XDI_CoordSysNwu: - return q_nwu2enu; - } - - case XDI_CoordSysNed: - switch (actual & XDI_CoordSysMask) - { - default: - case XDI_CoordSysEnu: - return q_enu2ned; - - case XDI_CoordSysNed: - return q_id; - - case XDI_CoordSysNwu: - return q_nwu2ned; - } - - case XDI_CoordSysNwu: - switch (actual & XDI_CoordSysMask) - { - default: - case XDI_CoordSysEnu: - return q_enu2nwu; - - case XDI_CoordSysNed: - return q_ned2nwu; - - case XDI_CoordSysNwu: - return q_id; - } - } -} - -/*! \endcond */ - - -/*! \class XsDataPacket - \brief Contains an interpreted data message. The class provides easy access to the contained - data through its many functions. - \sa cinterface For the C interface functions. -*/ - -/*! \addtogroup cinterface C Interface - @{ -*/ - -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 Clears and frees data in an XsDataPacket -*/ -void XsDataPacket_destruct(XsDataPacket* thisPtr) -{ - if (thisPtr->d) - { - 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) -{ - if (id == XDI_None) - { - XsDataPacket_destruct(thisPtr); - XsDataPacket_construct(thisPtr); - } - 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) -{ - *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) -{ - std::swap(thisPtr->d, other->d); - std::swap(thisPtr->m_deviceId, other->m_deviceId); - std::swap(thisPtr->m_toa, other->m_toa); - std::swap(thisPtr->m_packetId, other->m_packetId); - std::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 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 - - \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; -} - -/*! \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 The raw accelerometer component of a data item. - - \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); -} - -/*! \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 - - \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); -} - -/*! \brief The raw gyroscope component of a data item. - - \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); -} - -/*! \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 The raw magnetometer component of a data item. - - \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); -} - -/*! \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. - - \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 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(); -} - -/*! \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 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. - - \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; -} - -/*! \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) -{ - 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; - 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 Add/update raw data for the item - \param data The new data to set -*/ -void XsDataPacket_setRawData(XsDataPacket* thisPtr, const XsScrData* data) -{ - 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 calibrated accelerometer component of a data item. - - \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); -} - -/*! \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 The calibrated gyroscope component of a data item. - - \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); -} - -/*! \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 The calibrated magnetometer component of a data item. - - \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); -} - -/*! \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 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); - - 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) && - 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 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. - - \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()) - { - *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)) - { - XsEuler eul; - XsDataPacket_orientationEuler(thisPtr, &eul, coordinateSystem); - returnVal->fromEulerAngles(eul); - } - //else - // memset(returnVal->m_data, 0, 4*sizeof(XsReal)); - - return returnVal; -} - -/*! \brief Removes all orientations from the datapacket */ -static void removeAllOrientations(XsDataPacket* 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. - - \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()) - { - *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; -} - -/*! \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. - - \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()) - { - *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 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; -} - -/*! \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) || - 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(); - - 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(); - - 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 The gps PVT data component of a data item. - - \param returnVal : An XsGpsPvtData object to put the requested in - - \deprecated - - \returns An XsGpsPvtData containing the gps PVT data -*/ -XsGpsPvtData* XsDataPacket_gpsPvtData(const XsDataPacket* thisPtr, XsGpsPvtData* returnVal) -{ - assert (returnVal); - XsPressure pres; - XsDataPacket_pressure(thisPtr, &pres); - returnVal->m_pressure = (uint16_t)XsMath::doubleToLong(pres.m_pressure); - returnVal->m_pressureAge = pres.m_pressureAge; - - returnVal->m_itow = GenericSimple::get(thisPtr, XDI_Itow, 0); - returnVal->m_gpsAge = GenericSimple::get(thisPtr, XDI_GpsAge, 0); - - XsRawGpsSol sol; - XsDataPacket_rawGpsSol(thisPtr, &sol); - - returnVal->m_latitude = sol.m_ecef_x; - returnVal->m_longitude = sol.m_ecef_y; - returnVal->m_height = sol.m_ecef_z; - - returnVal->m_veln = sol.m_ecef_vx; - returnVal->m_vele = sol.m_ecef_vy; - returnVal->m_veld = sol.m_ecef_vz; - - returnVal->m_hacc = sol.m_pacc; - returnVal->m_vacc = sol.m_pacc; - returnVal->m_sacc = sol.m_pacc; - - return returnVal; -} - -/*! \brief Check if data item contains gps PVT data - \returns true if this packet contains gps PVT data - \deprecated -*/ -int XsDataPacket_containsGpsPvtData(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_GpsAge) && - genericContains(thisPtr, XDI_PressureAge) && - genericContains(thisPtr, XDI_BaroPressure) && - genericContains(thisPtr, XDI_Itow) && - genericContains(thisPtr, XDI_GpsSol); -} - -/*! \brief Add/update gps PVT data for the item - \param data : The data to update the XsDataPacket with - \deprecated -*/ -void XsDataPacket_setGpsPvtData(XsDataPacket* thisPtr, const XsGpsPvtData* data) -{ - XsPressure pres(data->m_pressure, data->m_pressureAge); - XsDataPacket_setPressure(thisPtr, &pres); - - GenericSimple::set(thisPtr, data->m_itow, XDI_Itow); - GenericSimple::set(thisPtr, data->m_gpsAge, XDI_GpsAge); - - XsRawGpsSol sol; - - sol.m_ecef_x = data->m_latitude; - sol.m_ecef_y = data->m_longitude; - sol.m_ecef_z = data->m_height; - sol.m_ecef_vx = data->m_veln; - sol.m_ecef_vy = data->m_vele; - sol.m_ecef_vz = data->m_veld; - sol.m_pacc = data->m_hacc; - - XsDataPacket_setRawGpsSol(thisPtr, &sol); -} - -/*! \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 The air pressure component of a data item. - - \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()) - { - 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; - - 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, 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); - - it = MAP.find(XDI_DeltaV); - if (it != MAP.end()) - returnVal->setVelocityIncrement(it->second->toDerived().m_data); - - 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) && - 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 The device id of a data item. - - \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); -} - -/*! \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->toInt(), XDI_DeviceId); -} - -/*! \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 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 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 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 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 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 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 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 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 The position lat lon alt component of a data item. - - \param returnVal : The XsVector to return the requested data in - - \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); - - 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; - } - (*returnVal)[2] = GenericSimple::get(thisPtr, XDI_AltitudeEllipsoid); - - 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 - - \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); -} - -/*! \brief The position latitude longitude component of a data item. - - \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); -} - -/*! \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 - - \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. - - \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 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. - - \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 - - \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 - - \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. - - \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()) - { - *returnVal = it->second->toDerived().m_data; - XsDataIdentifier actualCsys = (it->second->dataId() & XDI_CoordSysMask); - XsDataIdentifier desiredCsys = (coordinateSystem & XDI_CoordSysMask); - if (actualCsys != desiredCsys) - { - XsVector3 vel(*returnVal); - XsVector& rv = *returnVal; - - switch (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; - - case XDI_CoordSysNwu: - { - 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; - - case XDI_CoordSysNed: - { - 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]; - } - } break; - - default: - { - rv[0] = vel[0]; - rv[1] = vel[1]; - rv[2] = vel[2]; - } break; - } - } - } - - 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 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) -{ - auto it = MAP.find(XDI_VelocityXYZ); - if (it != MAP.end()) - MAP.erase(it); - - 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 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. - - \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; - - 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 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) -{ - 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); -} - -/*! \brief Add/update status data for the item - \param data The new data to set -*/ -void XsDataPacket_setStatus(XsDataPacket* thisPtr, uint32_t data) -{ - auto it = MAP.find(XDI_StatusByte); - if (it != MAP.end()) - MAP.erase(it); - - 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 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 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 - - \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 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 - - \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 - - \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())); - - 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) || - 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()) - { - auto& var = it->second->toDerived>(); - var.m_data = coarseFactor*(var.m_data/coarseFactor) + (counter%coarseFactor); - } -} - -/*! \brief 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 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 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. -*/ -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()) - { - hasSampletimeCoarse = true; - rv += ((uint64_t) it->second->toDerived>().m_data) * coarseFactor; - } - - it = MAP.find(XDI_SampleTimeFine); - if (it != MAP.end()) - { - uint32_t sampletimeFine = it->second->toDerived>().m_data; - if (hasSampletimeCoarse) - sampletimeFine = sampletimeFine % coarseFactor; - rv += sampletimeFine; - } - - 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)); -} - -/*! \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 The utc time component of a data item. - \param returnVal : The XsUtcTime to return the requested data in - \returns An XsUtcTime containing the utc time value -*/ -XsUtcTime* XsDataPacket_utcTime(const XsDataPacket* thisPtr, XsUtcTime* returnVal) -{ - return genericGet(thisPtr, returnVal, XDI_UtcTime); -} - -/*! \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 Add/update utc time data for the item - \param data The new data to set -*/ -void XsDataPacket_setUtcTime(XsDataPacket* thisPtr, const XsUtcTime* data) -{ - genericSet(thisPtr, data, XDI_UtcTime); -} - -/*! \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 - - \returns An XsVector containing the gravity acceleration -*/ -XsVector* XsDataPacket_freeAcceleration(const XsDataPacket* thisPtr, XsVector* returnVal) -{ - return genericGet(thisPtr, returnVal, XDI_FreeAcceleration); -} - -/*! \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 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 Returns the frame range contained in the datapacket - - \param returnVal : The XsRange object that will get the range from the packet - - \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 Returns whether the datapacket contains a framerange - - \returns Whether the datapacket contains a framerange -*/ -int XsDataPacket_containsFrameRange(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_FrameRange); -} - -/*! \brief Sets or updates the frame range in the datapacket - - \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 Returns the rssi value contained in the datapacket - - \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 whether the datapacket contains an rssi value - - \returns Whether the datapacket contains an rssi value -*/ -int XsDataPacket_containsRssi(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_Rssi); -} - -/*! \brief Sets or updates the rssi value in the datapacket - - \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 a struct with XsRawGpsDop data - \param returnVal Buffer for storage of requested value - \return the supplied \a returnVal - \deprecated Use the appropriate GNSS function instead -*/ -XsRawGpsDop* XsDataPacket_rawGpsDop(const XsDataPacket* thisPtr, XsRawGpsDop* returnVal) -{ - return genericGet(thisPtr, returnVal, XDI_GpsDop); -} - -/*! \brief Returns true if data item contains RawGpsDop, 0 otherwise - \return non-zero if data item contains RawGpsDop, 0 otherwise - \deprecated Use the appropriate GNSS function instead -*/ -int XsDataPacket_containsRawGpsDop(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_GpsDop); -} - -/*! \brief Sets or updates the RawGpsDop value in the datapacket - \param data The new value that should be added to the packet -*/ -void XsDataPacket_setRawGpsDop(XsDataPacket* thisPtr, const XsRawGpsDop* data) -{ - genericSet(thisPtr, data, XDI_GpsDop); -} - -/*! \brief Returns a struct with RawGpsSol data - \param returnVal Buffer for storage of requested value - \return the supplied \a returnVal - \deprecated Use the appropriate GNSS function instead -*/ -XsRawGpsSol* XsDataPacket_rawGpsSol(const XsDataPacket* thisPtr, XsRawGpsSol* returnVal) -{ - return genericGet(thisPtr, returnVal, XDI_GpsSol); -} - -/*! \brief Returns 1 if data item contains RawGpsSol, 0 otherwise - \return non-zero if data item contains RawGpsSol, 0 otherwise - \deprecated Use the appropriate GNSS function instead -*/ -int XsDataPacket_containsRawGpsSol(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_GpsSol); -} - -/*! \brief Sets or updates the RawGpsSol value in the datapacket - \param data The new value that should be added to the packet -*/ -void XsDataPacket_setRawGpsSol(XsDataPacket* thisPtr, const XsRawGpsSol* data) -{ - genericSet(thisPtr, data, XDI_GpsSol); -} - -/*! \brief Returns a struct with RawGpsTimeUtc data - \param returnVal Buffer for storage of requested value - \return the supplied \a returnVal - \deprecated Use the appropriate GNSS function instead -*/ -XsRawGpsTimeUtc* XsDataPacket_rawGpsTimeUtc(const XsDataPacket* thisPtr, XsRawGpsTimeUtc* returnVal) -{ - return genericGet(thisPtr, returnVal, XDI_GpsTimeUtc); -} - -/*! \brief Returns 1 if data item contains RawGpsTimeUtc, 0 otherwise - \return non-zero if data item contains RawGpsTimeUtc, 0 otherwise - \deprecated Use the appropriate GNSS function instead -*/ -int XsDataPacket_containsRawGpsTimeUtc(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_GpsTimeUtc); -} - -/*! \brief Sets or updates the RawGpsTimeUtc value in the datapacket - \param data The new value that should be added to the packet -*/ -void XsDataPacket_setRawGpsTimeUtc(XsDataPacket* thisPtr, const XsRawGpsTimeUtc* data) -{ - genericSet(thisPtr, data, XDI_GpsTimeUtc); -} - -/*! \brief Returns a struct with RawGpsSvInfo data - \param returnVal Buffer for storage of requested value - \return the supplied \a returnVal - \deprecated Use the appropriate GNSS function instead -*/ -XsRawGpsSvInfo* XsDataPacket_rawGpsSvInfo(const XsDataPacket* thisPtr, XsRawGpsSvInfo* returnVal) -{ - return genericGet(thisPtr, returnVal, XDI_GpsSvInfo); -} - -/*! \brief Returns 1 if data item contains RawGpsSvInfo, 0 otherwise - \return 1 if data item contains RawGpsSvInfo, 0 otherwise - \deprecated Use the appropriate GNSS function instead -*/ -int XsDataPacket_containsRawGpsSvInfo(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_GpsSvInfo); -} - -/*! \brief Sets or updates the RawGpsSvInfo value in the datapacket - \param r The new value that should be added to the packet -*/ -void XsDataPacket_setRawGpsSvInfo(XsDataPacket* thisPtr, const XsRawGpsSvInfo* r) -{ - genericSet(thisPtr, r, XDI_GpsSvInfo); -} - -/*! \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 Returns 1 if data item contains RawGnssPvtData, 0 otherwise -*/ -int XsDataPacket_containsRawGnssPvtData(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_GnssPvtData); -} - -/*! \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 age of the GNSS data (in samples) -*/ -uint8_t XsDataPacket_gnssAge(const XsDataPacket* thisPtr) -{ - return GenericSimple::get(thisPtr, XDI_GnssAge, 255); -} - -/*! \brief Returns 1 if data item contains GnssAge, 0 otherwise -*/ -int XsDataPacket_containsGnssAge(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_GnssAge); -} - -/*! \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 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) -{ - if (XsDataPacket_containsRawGnssSatInfo(thisPtr)) - return genericGet(thisPtr, returnVal, XDI_GnssSatInfo); - else if (XsDataPacket_containsRawGpsSvInfo(thisPtr)) - { - XsSize i; - XsRawGpsSvInfo svInfo; - (void)XsDataPacket_rawGpsSvInfo(thisPtr, &svInfo); - returnVal->m_itow = svInfo.m_itow; - returnVal->m_numSvs = svInfo.m_nch; - for (i = 0; i < returnVal->m_numSvs; i++) - { - returnVal->m_satInfos[i].m_gnssId = 0; // Gps - returnVal->m_satInfos[i].m_svId = svInfo.m_svInfos[i].m_svid; - returnVal->m_satInfos[i].m_cno = svInfo.m_svInfos[i].m_cno; - returnVal->m_satInfos[i].m_flags = svInfo.m_svInfos[i].m_qi; - returnVal->m_satInfos[i].m_flags |= (svInfo.m_svInfos[i].m_flags & 0x1) << 3; - if ((svInfo.m_svInfos[i].m_flags & 0x10) == 0) - returnVal->m_satInfos[i].m_flags |= 0x10; // healthy - else - returnVal->m_satInfos[i].m_flags |= 0x20; // SV is unhealthy / shall not be used - returnVal->m_satInfos[i].m_flags |= (svInfo.m_svInfos[i].m_flags & 0x2) << 5; - } - } - else - *returnVal = XsRawGnssSatInfo(); - return returnVal; -} - -/*! \brief Returns 1 if data item contains RawGnssPvtData, 0 otherwise -*/ -int XsDataPacket_containsRawGnssSatInfo(const XsDataPacket* thisPtr) -{ - return genericContains(thisPtr, XDI_GnssSatInfo); -} - -/*! \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 Merge the data items from \a other into the packet - - \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. - - \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) -{ - 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)) - { - if (genericContains(other, id1) ^ over) - 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); - - if (over) - { - 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 thisPtr; -} - -/*! \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); - - int offset = 0; - int sz = (int) msg->getDataSize(); - - while (offset+4 <= sz) // minimum size of an item is 2(ID) + 1(size) + 1(minimum size) - { - XsDataIdentifier id = static_cast(XsMessage_getDataShort(msg, offset)); - uint8_t itemSize = XsMessage_getDataByte(msg, offset+2); - if (offset + itemSize + 3 > sz || itemSize == 0) - break; // the item is corrupt - - Variant* var = createVariant(id); - if (var) - { - var->readFromMessage(*msg, offset+3, itemSize); - MAP.insert(id, var); - assert(var->sizeInMsg() == itemSize); // this is not strictly necessary, but useful during initial development - } - offset += 3 + itemSize; // never use var->sizeInMsg() here, since it _may_ differ - } - 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); - - int 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) - { - msg->setDataShort((uint16_t) i.second->dataId(), offset); - int sz = i.second->sizeInMsg(); - msg->setDataByte((uint8_t)(int8_t)sz, offset+2); - i.second->writeToMessage(*msg, offset+3); - offset += 3+sz; - } - 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) -{ - return genericGet(thisPtr, returnVal, 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 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 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) -{ - auto it = MAP.find(XDI_AwindaSnapshot); - if (it == MAP.end()) - return false; - return (it->second->dataId() & XDI_RetransmissionMask) == XDI_RetransmissionFlag; -} - -/*! \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 (int 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); - - XsUShortVector tmp; - - auto it = MAP.find(XDI_RawAccGyrMagTemp); - if (it != MAP.end()) - 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; -} - -/*! \brief The raw accelerometer component of a data item, converted to floating point values. - - \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); -} - -/*! \brief The raw gyroscope component of a data item, converted to floating point values. - - \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); -} - -/*! \brief The raw magnetometer component of a data item, converted to floating point values. - - \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); -} - -/*! \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 - - \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 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 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 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 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); -} - - -} // extern "C" - -/*! @} */ diff --git a/extern/xstypes/xsdeviceid.c b/extern/xstypes/xsdeviceid.c deleted file mode 100644 index 60de6a3..0000000 --- a/extern/xstypes/xsdeviceid.c +++ /dev/null @@ -1,683 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#include "xsdeviceid.h" -#include "xsstring.h" -#include "xsdid.h" -#include - -/*! \class XsDeviceId - \brief Contains an Xsens device ID and provides operations for determining the type of device -*/ - -/*! \addtogroup cinterface C Interface - @{ -*/ - -/*! \brief Returns the value of a broadcast deviceID. -*/ -uint32_t XsDeviceId_broadcast(void) -{ - return XS_DID_BROADCAST; -} - -/*! \brief Test if the device ID is a valid id (not 0). -*/ -int XsDeviceId_isValid(const struct XsDeviceId* thisPtr) -{ - return thisPtr->m_deviceId != 0; -} - -/*! \brief Test if this device ID represents an Xbus Master. */ -int XsDeviceId_isXbusMaster(const struct XsDeviceId* thisPtr) -{ - return ((thisPtr->m_deviceId & XS_DID_TYPEH_MASK) == XS_DID_TYPEH_XM); -} - -/*! \brief Test if this device ID represents an awinda1 device. */ -int XsDeviceId_isAwinda1(const struct XsDeviceId* thisPtr) -{ - return XS_DID_AWINDA1(thisPtr->m_deviceId); -} - -/*! \brief Test if this device ID represents an awinda2 device. */ -int XsDeviceId_isAwinda2(const struct XsDeviceId* thisPtr) -{ - return XS_DID_AWINDA2(thisPtr->m_deviceId); -} - -/*! \brief Test if this device ID represents an awinda device. */ -int XsDeviceId_isAwinda(const struct XsDeviceId* thisPtr) -{ - return XsDeviceId_isAwinda1(thisPtr) || XsDeviceId_isAwinda2(thisPtr); -} - -/*! \brief Test if this device ID represents a bodypack device. */ -int XsDeviceId_isBodyPack(const struct XsDeviceId* thisPtr) -{ - return XS_DID_BODYPACK(thisPtr->m_deviceId) || (thisPtr->m_deviceId == XS_DID_ABMCLOCKMASTER); -} - -/*! \brief Test if this device ID represents a Wireless Master device (Awinda Station, Awinda Dongle, Awinda OEM). */ -int XsDeviceId_isWirelessMaster(const struct XsDeviceId* thisPtr) -{ - return ((thisPtr->m_deviceId & XS_DID_TYPEH_MASK) == XS_DID_TYPEH_AWINDAMASTER) && - !XsDeviceId_isBodyPack(thisPtr) && - !XsDeviceId_isSyncStation(thisPtr); -} - -/*! \brief Test if this device ID represents an Awinda1 Station. -*/ -int XsDeviceId_isAwinda1Station(const struct XsDeviceId* thisPtr) -{ - return XS_DID_AWINDA1_STATION(thisPtr->m_deviceId); -} - -/*! \brief Test if this device ID represents an Awinda1 Dongle. */ -int XsDeviceId_isAwinda1Dongle(const struct XsDeviceId* thisPtr) -{ - return XS_DID_AWINDA1_DONGLE(thisPtr->m_deviceId); -} - -/*! \brief Test if this device ID represents an Awinda1 OEM board. */ -int XsDeviceId_isAwinda1Oem(const struct XsDeviceId* thisPtr) -{ - return XS_DID_AWINDA1_OEM(thisPtr->m_deviceId); -} - -/*! \brief Test if this device ID represents an Awinda2 Station. -*/ -int XsDeviceId_isAwinda2Station(const struct XsDeviceId* thisPtr) -{ - return XS_DID_AWINDA2_STATION(thisPtr->m_deviceId); -} - -/*! \brief Test if this device ID represents an Awinda2 Dongle. */ -int XsDeviceId_isAwinda2Dongle(const struct XsDeviceId* thisPtr) -{ - return XS_DID_AWINDA2_DONGLE(thisPtr->m_deviceId); -} - -/*! \brief Test if this device ID represents an Awinda2 OEM board. */ -int XsDeviceId_isAwinda2Oem(const struct XsDeviceId* thisPtr) -{ - return XS_DID_AWINDA2_OEM(thisPtr->m_deviceId); -} - -/*! \brief Test if this device ID represents an Awinda Station. -*/ -int XsDeviceId_isAwindaStation(const struct XsDeviceId* thisPtr) -{ - return XsDeviceId_isAwinda1Station(thisPtr) || XsDeviceId_isAwinda2Station(thisPtr); -} - -/*! \brief Test if this device ID represents an Awinda Dongle. */ -int XsDeviceId_isAwindaDongle(const struct XsDeviceId* thisPtr) -{ - return XsDeviceId_isAwinda1Dongle(thisPtr) || XsDeviceId_isAwinda2Dongle(thisPtr); -} - -/*! \brief Test if this device ID represents an Awinda OEM board. */ -int XsDeviceId_isAwindaOem(const struct XsDeviceId* thisPtr) -{ - return XsDeviceId_isAwinda1Oem(thisPtr) || XsDeviceId_isAwinda2Oem(thisPtr); -} - -/*! \brief Test if this device ID represents a SyncStation. */ -int XsDeviceId_isSyncStation(const struct XsDeviceId* thisPtr) -{ - return XS_DID_SYNCSTATION(thisPtr->m_deviceId); -} - -/*! \brief Test if this device ID represents a SyncStation v1. */ -int XsDeviceId_isSyncStation1(const struct XsDeviceId* thisPtr) -{ - return XS_DID_SYNCSTATION1(thisPtr->m_deviceId); -} - -/*! \brief Test if this device ID represents a SyncStation v2. */ -int XsDeviceId_isSyncStation2(const struct XsDeviceId* thisPtr) -{ - return XS_DID_SYNCSTATION2(thisPtr->m_deviceId); -} - -/*! \brief Test if this device ID represents an MTw1. */ -int XsDeviceId_isMtw1(const struct XsDeviceId* thisPtr) -{ - return XS_DID_WMMT(thisPtr->m_deviceId); -} - -/*! \brief Test if this device ID represents an MTw2. */ -int XsDeviceId_isMtw2(const struct XsDeviceId* thisPtr) -{ - return XS_DID_MTW2(thisPtr->m_deviceId); -} - -/*! \brief Test if this device ID represents an MTw */ -int XsDeviceId_isMtw(const struct XsDeviceId* thisPtr) -{ - return XsDeviceId_isMtw1(thisPtr) || XsDeviceId_isMtw2(thisPtr); -} - -/*! \brief Test if this device ID represents an Xbus Master Motion Tracker. */ -int XsDeviceId_isXbusMasterMotionTracker(const struct XsDeviceId* thisPtr) -{ - return ((thisPtr->m_deviceId & XS_DID_TYPE_MASK) == XS_DID_TYPE_MTX_XBUS); -} - -/*! \brief Test if this device ID represents an MT9c. */ -int XsDeviceId_isMt9c(const struct XsDeviceId* thisPtr) -{ - return (((thisPtr->m_deviceId & XS_DID_TYPEH_MASK) == XS_DID_TYPEH_MTI_MTX) && !XsDeviceId_isMtw1(thisPtr)); -} - -/*! \brief Test if this device ID represents a legacy MTi-G. */ -int XsDeviceId_isLegacyMtig(const struct XsDeviceId* thisPtr) -{ - return ((thisPtr->m_deviceId & XS_DID_TYPEH_MASK) == XS_DID_TYPEH_MTIG); -} - -/*! \brief Test if this device ID represents an MTi-G. */ -int XsDeviceId_isMtig(const struct XsDeviceId* thisPtr) -{ - return (((thisPtr->m_deviceId & XS_DID_TYPEH_MASK) == XS_DID_TYPEH_MTIG) || ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_700)); -} - -/*! \brief Test if this device ID represents an MTMk4. */ -int XsDeviceId_isMtMk4(const struct XsDeviceId* thisPtr) -{ - return ( XsDeviceId_isMtMk4_X(thisPtr) || - XsDeviceId_isMtMk4_X0(thisPtr) || - XsDeviceId_isMtMk4_X00 (thisPtr)); -} - -/*! \brief Test if this device ID represents an MTMk4 1 series. */ -int XsDeviceId_isMtMk4_X(const struct XsDeviceId* thisPtr) -{ - return ((thisPtr->m_deviceId & XS_DID_TYPEH_MASK) == XS_DID_TYPEH_MT_X_MPU); -} - -/*! \brief Test if this device ID represents an MTMk4 1. */ -int XsDeviceId_isMtMk4_1(const struct XsDeviceId* thisPtr) -{ - return ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_1_MPU); -} - -/*! \brief Test if this device ID represents an MTMk4 2. */ -int XsDeviceId_isMtMk4_2(const struct XsDeviceId* thisPtr) -{ - return ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_2_MPU); -} - -/*! \brief Test if this device ID represents an MTMk4 3. */ -int XsDeviceId_isMtMk4_3(const struct XsDeviceId* thisPtr) -{ - return ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_3_MPU); -} - -/*! \brief Test if this device ID represents an MTMk4 10 series. */ -int XsDeviceId_isMtMk4_X0(const struct XsDeviceId* thisPtr) -{ - return ((thisPtr->m_deviceId & XS_DID_TYPEH_MASK) == XS_DID_TYPEH_MT_X0); -} - -/*! \brief Test if this device ID represents an MTMk4 10. */ -int XsDeviceId_isMtMk4_10(const struct XsDeviceId* thisPtr) -{ - return ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_10); -} - -/*! \brief Test if this device ID represents an MTMk4 20. */ -int XsDeviceId_isMtMk4_20(const struct XsDeviceId* thisPtr) -{ - return ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_20); -} - -/*! \brief Test if this device ID represents an MTMk4 30. */ -int XsDeviceId_isMtMk4_30(const struct XsDeviceId* thisPtr) -{ - return ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_30); -} - -/*! \brief Test if this device ID represents an MTMk4 100 series. */ -int XsDeviceId_isMtMk4_X00(const struct XsDeviceId* thisPtr) -{ - return ((thisPtr->m_deviceId & XS_DID_TYPEH_MASK) == XS_DID_TYPEH_MT_X00); -} - -/*! \brief Test if this device ID represents an MTMk4 100. */ -int XsDeviceId_isMtMk4_100(const struct XsDeviceId* thisPtr) -{ - return ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_100); -} - -/*! \brief Test if this device ID represents an MTMk4 200. */ -int XsDeviceId_isMtMk4_200(const struct XsDeviceId* thisPtr) -{ - return ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_200); -} - -/*! \brief Test if this device ID represents an MTMk4 300. */ -int XsDeviceId_isMtMk4_300(const struct XsDeviceId* thisPtr) -{ - return ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_300); -} - -/*! \brief Test if this device ID represents an MTMk4 400. */ -int XsDeviceId_isMtMk4_400(const struct XsDeviceId* thisPtr) -{ - return ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_400); -} - -/*! \brief Test if this device ID represents an MTMk4 500. */ -int XsDeviceId_isMtMk4_500(const struct XsDeviceId* thisPtr) -{ - return ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_500); -} - -/*! \brief Test if this device ID represents an MTMk4 600. */ -int XsDeviceId_isMtMk4_600(const struct XsDeviceId* thisPtr) -{ - return ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_600); -} - -/*! \brief Test if this device ID represents an MTMk4 700. - \return True if the device ID represents an MTMk4 700. -*/ -int XsDeviceId_isMtMk4_700(const struct XsDeviceId* thisPtr) -{ - return ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_700); -} - -/*! \brief Test if this device ID represents an MTMk4 710. */ -int XsDeviceId_isMtMk4_710(const struct XsDeviceId* thisPtr) -{ - return ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_700) && thisPtr->m_deviceId >= XS_DID_MK4TYPE_MT_710_RANGE_START; -} - -/*! \brief Test if this device ID represents an MTMk4 800. */ -int XsDeviceId_isMtMk4_800(const struct XsDeviceId* thisPtr) -{ - return ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_800); -} - -/*! \brief Test if this device ID represents an MTMk4 900. */ -int XsDeviceId_isMtMk4_900(const struct XsDeviceId* thisPtr) -{ - return ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_900); -} - -/*! \brief Test if this device ID represents an MTMk5. */ -int XsDeviceId_isMtMk5(const struct XsDeviceId* thisPtr) -{ - return ((XsDeviceId_isMtMk4_X0(thisPtr) || XsDeviceId_isMtMk4_X00(thisPtr)) - && ((thisPtr->m_deviceId & XS_DID_TYPEL_MK5) != 0)); -} - -/*! \brief Test if this device ID represents an MTMk5 10 series. */ -int XsDeviceId_isMtMk5_X0(const struct XsDeviceId* thisPtr) -{ - return XsDeviceId_isMtMk5(thisPtr) && XsDeviceId_isMtMk4_X0(thisPtr); -} - -/*! \brief Test if this device ID represents an MTMk5 10. */ -int XsDeviceId_isMtMk5_10(const struct XsDeviceId* thisPtr) -{ - return XsDeviceId_isMtMk5(thisPtr) && XsDeviceId_isMtMk4_10(thisPtr); -} - -/*! \brief Test if this device ID represents an MTMk5 20. */ -int XsDeviceId_isMtMk5_20(const struct XsDeviceId* thisPtr) -{ - return XsDeviceId_isMtMk5(thisPtr) && XsDeviceId_isMtMk4_20(thisPtr); -} - -/*! \brief Test if this device ID represents an MTMk5 30. */ -int XsDeviceId_isMtMk5_30(const struct XsDeviceId* thisPtr) -{ - return XsDeviceId_isMtMk5(thisPtr) && XsDeviceId_isMtMk4_30(thisPtr); -} - -/*! \brief Test if this device ID represents an MTMk5 100 series. */ -int XsDeviceId_isMtMk5_X00(const struct XsDeviceId* thisPtr) -{ - return XsDeviceId_isMtMk5(thisPtr) && XsDeviceId_isMtMk4_X00(thisPtr); -} - -/*! \brief Test if this device ID represents an MTMk5 100. */ -int XsDeviceId_isMtMk5_100(const struct XsDeviceId* thisPtr) -{ - return XsDeviceId_isMtMk5(thisPtr) && XsDeviceId_isMtMk4_100(thisPtr); -} - -/*! \brief Test if this device ID represents an MTMk5 200. */ -int XsDeviceId_isMtMk5_200(const struct XsDeviceId* thisPtr) -{ - return XsDeviceId_isMtMk5(thisPtr) && XsDeviceId_isMtMk4_200(thisPtr); -} - -/*! \brief Test if this device ID represents an MTMk5 300. */ -int XsDeviceId_isMtMk5_300(const struct XsDeviceId* thisPtr) -{ - return XsDeviceId_isMtMk5(thisPtr) && XsDeviceId_isMtMk4_300(thisPtr); -} - -/*! \brief Test if this device ID represents an MTMk5 710. */ -int XsDeviceId_isMtMk5_710(const struct XsDeviceId* thisPtr) -{ - return XsDeviceId_isMtMk5(thisPtr) && XsDeviceId_isMtMk4_710(thisPtr); -} - -/*! \brief Test if this device ID represents an MTx2 */ -int XsDeviceId_isMtx2(const struct XsDeviceId* thisPtr) -{ - return XS_DID_MTX2(thisPtr->m_deviceId); -} - -/*! \brief Test if this device ID represents an MTx */ -int XsDeviceId_isMtx(const struct XsDeviceId* thisPtr) -{ - return XsDeviceId_isMtx2(thisPtr); -} - -/*! \brief Test if this device ID represents a Fairchild FIS1100 SDK -*/ -int XsDeviceId_isFis1100EvalKit(const struct XsDeviceId* thisPtr) -{ - return XS_DID_FIS1100EVK(thisPtr->m_deviceId); -} - -/*! \brief Test if this device ID represents a Fairchild FIS2100 SDK -*/ -int XsDeviceId_isFis2100EvalKit(const struct XsDeviceId* thisPtr) -{ - return XS_DID_FIS2100EVK(thisPtr->m_deviceId); -} - -/*! \brief Test if this device ID represents a Fairchild FIS SDK -*/ -int XsDeviceId_isFisEvalKit(const struct XsDeviceId* thisPtr) -{ - return XsDeviceId_isFis1100EvalKit(thisPtr) || XsDeviceId_isFis2100EvalKit(thisPtr); -} - -/*! \brief Test if this device ID represents an Fmt x000 series. */ -int XsDeviceId_isFmt_X000(const struct XsDeviceId* thisPtr) -{ - return XsDeviceId_isFmt1000(thisPtr) ; -} - -/*! \brief Test if this device ID represents an FMT1000 series. */ -int XsDeviceId_isFmt1000(const struct XsDeviceId* thisPtr) -{ - return ((thisPtr->m_deviceId & XS_DID_TYPEH_MASK) == XS_DID_TYPEH_MT_X_FIS); -} - -/*! \brief Test if this device ID represents an FMT1010. */ -int XsDeviceId_isFmt1010(const struct XsDeviceId* thisPtr) -{ - return ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_1_FIS); -} - -/*! \brief Test if this device ID represents an FMT1020. */ -int XsDeviceId_isFmt1020(const struct XsDeviceId* thisPtr) -{ - return ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_2_FIS); -} - -/*! \brief Test if this device ID represents an FMT1030. */ -int XsDeviceId_isFmt1030(const struct XsDeviceId* thisPtr) -{ - return ((thisPtr->m_deviceId & XS_DID_MK4TYPE_MASK) == XS_DID_MK4TYPE_MT_3_FIS); -} - -/*! \brief Test if this device ID represents an IMU. */ -int XsDeviceId_isImu(const struct XsDeviceId* 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_1_FIS)); -} - -/*! \brief Test if this device ID represents a VRU. */ -int XsDeviceId_isVru(const struct XsDeviceId* 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_2_FIS)); -} - -/*! \brief Test if this device ID represents an AHRS. */ -int XsDeviceId_isAhrs(const struct XsDeviceId* 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_3_FIS)); -} - -/*! \brief Test if this device ID represents any of the container devices such as Xbus Master and Awinda Station -*/ -int XsDeviceId_isContainerDevice(const struct XsDeviceId* thisPtr) -{ - return XsDeviceId_isBodyPack(thisPtr) || - XsDeviceId_isWirelessMaster(thisPtr) || - XsDeviceId_isXbusMaster(thisPtr); -} - -/*! \brief Test if the device ID has the broadcast bit set - \return True if the broadcast bit is set -*/ -int XsDeviceId_containsBroadcast(const XsDeviceId* thisPtr) -{ - return ((thisPtr->m_deviceId & XS_DID_BROADCAST) != 0); -} - -/*! \brief Test if this device ID \e is \e the broadcast id. - \return TRue if the deviceid is a broadcast -*/ -int XsDeviceId_isBroadcast(const XsDeviceId* thisPtr) -{ - return (thisPtr->m_deviceId == XS_DID_BROADCAST); -} - -/*! \brief Get a string with a readable representation of this device ID. */ -void XsDeviceId_toString(const XsDeviceId* thisPtr, XsString* str) -{ - char device[9]; - sprintf(device, "%08" PRINTF_INT32_MODIFIER "X", thisPtr->m_deviceId); //lint !e534 - XsString_assign(str, 8, device); -} - -/*! \brief Read a device ID from the supplied string. */ -void XsDeviceId_fromString(XsDeviceId* thisPtr, const XsString *str) -{ - int tmp = 0; - int count = 0; - if (!thisPtr || !str || !str->m_data) - return; - if (sscanf(str->m_data, "%x%n", &tmp, &count) == 1 && count >= 6) - thisPtr->m_deviceId = (uint32_t) tmp; -} - -/*! \brief Swap the contents of \a a with those of \a b -*/ -void XsDeviceId_swap(XsDeviceId* a, XsDeviceId* b) -{ - uint32_t tmp = a->m_deviceId; - a->m_deviceId = b->m_deviceId; - b->m_deviceId = tmp; -} - -/*! \brief Returns true if this is equal to \a other or this is a type-specifier that matches \a other - \param other The deviceid to compare this deviceid to - \return True if the deviceids are equal or the type-specifier matches -*/ -int XsDeviceId_contains(XsDeviceId const* thisPtr, XsDeviceId const* other) -{ - if (thisPtr == other) - return 1; - if (thisPtr->m_deviceId == other->m_deviceId) - return 1; - if (thisPtr->m_deviceId & XS_DID_ID_MASK) // NOTE: This produces incorrect results for device ids ending with 0000. See MVN-3876 - return 0; - if ((thisPtr->m_deviceId & XS_DID_FULLTYPE_MASK) == (other->m_deviceId & XS_DID_FULLTYPE_MASK)) - return 1; - return 0; -} - -/*! \brief Returns true if the ID is just a device type, not an actual device ID -*/ -int XsDeviceId_isType(XsDeviceId const* thisPtr) -{ - // true when we have a valid type (not a broadcast) and a 0 ID - return (thisPtr->m_deviceId & (XS_DID_FULLTYPE_MASK & ~XS_DID_BROADCAST)) - && !(thisPtr->m_deviceId & XS_DID_ID_MASK); -} - -/*! \brief Returns the name of the type of device identified by this id -*/ -void XsDeviceId_typeName(XsDeviceId const* thisPtr, XsString* str) -{ - if (!str) - return; - - if (!thisPtr || thisPtr->m_deviceId == 0) - { - XsString_assignCharArray(str, "invalid"); - return; - } - - if (XsDeviceId_isXbusMaster(thisPtr)) - XsString_assignCharArray(str, "Xbus Master"); - else if(XsDeviceId_isAwinda1Station(thisPtr)) - XsString_assignCharArray(str, "Awinda Station v1"); - else if(XsDeviceId_isAwinda1Dongle(thisPtr)) - XsString_assignCharArray(str, "Awinda Dongle v1"); - else if(XsDeviceId_isAwinda1Oem(thisPtr)) - XsString_assignCharArray(str, "Awinda OEM v1"); - else if (XsDeviceId_isAwinda2Station(thisPtr)) - XsString_assignCharArray(str, "Awinda Station v2"); - else if(XsDeviceId_isAwinda2Dongle(thisPtr)) - XsString_assignCharArray(str, "Awinda Dongle v2"); - else if(XsDeviceId_isAwinda2Oem(thisPtr)) - XsString_assignCharArray(str, "Awinda OEM v2"); - else if (XsDeviceId_isMtw1(thisPtr)) - XsString_assignCharArray(str, "MTw"); - else if (XsDeviceId_isMtw2(thisPtr)) - XsString_assignCharArray(str, "MTw2"); - else if (XsDeviceId_isMtx(thisPtr)) - XsString_assignCharArray(str, "MTx Xbus"); - else if (XsDeviceId_isMtx2(thisPtr)) - XsString_assignCharArray(str, "MTx2"); - else if (XsDeviceId_isBodyPack(thisPtr)) - XsString_assignCharArray(str, "Bodypack"); - else if (XsDeviceId_isFis1100EvalKit(thisPtr)) - XsString_assignCharArray(str, "FIS1100 EVK"); - else if (XsDeviceId_isFis2100EvalKit(thisPtr)) - XsString_assignCharArray(str, "FIS2100 EVK"); - else if (XsDeviceId_isFmt1010(thisPtr)) - XsString_assignCharArray(str, "FMT1010"); - else if (XsDeviceId_isFmt1020(thisPtr)) - XsString_assignCharArray(str, "FMT1020"); - else if (XsDeviceId_isFmt1030(thisPtr)) - XsString_assignCharArray(str, "FMT1030"); - else if (XsDeviceId_isSyncStation1(thisPtr)) - XsString_assignCharArray(str, "Sync Station v1"); - else if (XsDeviceId_isSyncStation2(thisPtr)) - XsString_assignCharArray(str, "Sync Station v2"); - else if (XsDeviceId_isMtMk4_1(thisPtr)) - XsString_assignCharArray(str, "MTi-1"); - else if (XsDeviceId_isMtMk4_2(thisPtr)) - XsString_assignCharArray(str, "MTi-2"); - else if (XsDeviceId_isMtMk4_3(thisPtr)) - XsString_assignCharArray(str, "MTi-3"); - else if (XsDeviceId_isMtMk4_10(thisPtr)) - XsString_assignCharArray(str, "MTi-10"); - else if (XsDeviceId_isMtMk4_20(thisPtr)) - XsString_assignCharArray(str, "MTi-20"); - else if (XsDeviceId_isMtMk4_30(thisPtr)) - XsString_assignCharArray(str, "MTi-30"); - else if (XsDeviceId_isMtMk4_100(thisPtr)) - XsString_assignCharArray(str, "MTi-100"); - else if (XsDeviceId_isMtMk4_200(thisPtr)) - XsString_assignCharArray(str, "MTi-200"); - else if (XsDeviceId_isMtMk4_300(thisPtr)) - XsString_assignCharArray(str, "MTi-300"); - else if ( XsDeviceId_isMtMk4_400(thisPtr)) - XsString_assignCharArray(str, "MTi-400"); - else if (XsDeviceId_isMtMk4_500(thisPtr)) - XsString_assignCharArray(str, "MTi-500"); - else if (XsDeviceId_isMtMk4_600(thisPtr)) - XsString_assignCharArray(str, "MTi-600"); - else if (XsDeviceId_isMtMk4_700(thisPtr)) - XsString_assignCharArray(str, "MTi-G-7XX"); - else if (XsDeviceId_isMtMk4_800(thisPtr)) - XsString_assignCharArray(str, "MTi-G-800"); - else if (XsDeviceId_isMtMk4_900(thisPtr)) - XsString_assignCharArray(str, "MTi-G-900"); - else - XsString_assignCharArray(str, "Unknown"); - - if (thisPtr->m_deviceId & XS_DID_BROADCAST) - { - XsString bc; - XsString_construct(&bc); - XsString_assignCharArray(&bc, " broadcast"); - XsString_append(str, &bc); - } -} - -/*! \brief Returns the type of device identified by this id -*/ -uint32_t XsDeviceId_type(struct XsDeviceId const* thisPtr) -{ - return thisPtr->m_deviceId & XS_DID_FULLTYPE_MASK; -} - -/*! \brief Returns the device type identified by this id (eg 10, 300 and Awinda2 Master) -*/ -uint32_t XsDeviceId_deviceType(struct XsDeviceId const* thisPtr, int detailed) -{ - uint32_t deviceMask = XsDeviceId_deviceTypeMask(thisPtr, detailed); - return thisPtr->m_deviceId & deviceMask; -} - -/*! \brief Returns the mask which can be used to get the detailed device type (eg 10, 300 and Awinda2 Master) -*/ -uint32_t XsDeviceId_deviceTypeMask(struct XsDeviceId const* thisPtr, int detailed) -{ - uint32_t deviceMask; - - if (XsDeviceId_isMtMk4_X(thisPtr) || XsDeviceId_isFmt_X000(thisPtr)) - deviceMask = (XS_DID_TYPEH_MASK | (detailed ? (XS_DID_GPH_MASK | XS_DID_GPL_MASK | XS_DID_TYPEL_MASK) : 0)); - else if (XsDeviceId_isMtMk4(thisPtr)) - deviceMask = (XS_DID_TYPEH_MASK | (detailed ? (XS_DID_GPH_MASK | XS_DID_GPL_MASK | XS_DID_TYPEL_MK5) : 0)); - else if (XsDeviceId_isAwinda(thisPtr)) - deviceMask = (XS_DID_TYPEH_MASK | (detailed ? (XS_DID_GPH_MASK | XS_DID_GPL_MASK) : 0)); - else if (XsDeviceId_isSyncStation(thisPtr)) - deviceMask = (XS_DID_TYPEH_MASK | (detailed ? (XS_DID_GPH_MASK | XS_DID_GPL_MASK) : 0)); - else if (XsDeviceId_isMtw(thisPtr) || XsDeviceId_isMtx(thisPtr)) - deviceMask = (XS_DID_TYPE_MASK | (detailed ? (XS_DID_GPH_MASK | XS_DID_GPL_MASK) : 0)); - else if (thisPtr->m_deviceId == XS_DID_ABMCLOCKMASTER) - deviceMask = XS_DID_ABMCLOCKMASTER; - else - deviceMask = XS_DID_TYPEH_MASK; - - return deviceMask; -} - -/*! @} */ diff --git a/extern/xstypes/xsdeviceidarray.c b/extern/xstypes/xsdeviceidarray.c deleted file mode 100644 index 865bd0e..0000000 --- a/extern/xstypes/xsdeviceidarray.c +++ /dev/null @@ -1,57 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#include "xsdeviceidarray.h" -#include "xsdeviceid.h" - -/*! \struct XsDeviceIdArray - \brief A list of XsDeviceId values - \sa XsArray -*/ - -void initDeviceId(XsDeviceId* did) -{ - did->m_deviceId = 0; -} - -void initDeviceIdToValue(XsDeviceId* did, XsDeviceId const* src) -{ - did->m_deviceId = src->m_deviceId; -} - -int compareDeviceIds(XsDeviceId const* a, XsDeviceId const* b) -{ - return ((int) a->m_deviceId) - ((int) b->m_deviceId); -} - -//! \brief Descriptor for XsDeviceIdArray -XsArrayDescriptor const g_xsDeviceIdArrayDescriptor = { - //lint --e{64} ignore exact type mismatches here - sizeof(XsDeviceId), - XSEXPCASTITEMSWAP XsDeviceId_swap, // swap - XSEXPCASTITEMMAKE initDeviceId, // construct - XSEXPCASTITEMCOPY initDeviceIdToValue, // copy construct - 0, // destruct - XSEXPCASTITEMCOPY initDeviceIdToValue, // copy - XSEXPCASTITEMCOMP compareDeviceIds, // compare - XSEXPCASTRAWCOPY XsArray_rawCopy // raw copy -}; - -/*! \copydoc XsArray_construct - \note Specialization for XsStringArray -*/ -void XsDeviceIdArray_construct(XsDeviceIdArray* thisPtr, XsSize count, XsDeviceId const* src) -{ - XsArray_construct(thisPtr, &g_xsDeviceIdArrayDescriptor, count, src); -} diff --git a/extern/xstypes/xseuler.c b/extern/xstypes/xseuler.c deleted file mode 100644 index 3c514ed..0000000 --- a/extern/xstypes/xseuler.c +++ /dev/null @@ -1,71 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#include "xseuler.h" -#include "xsquaternion.h" -#include - -/*! \class XsEuler - \brief Contains Euler Angle data and conversion from Quaternion - \details Euler Angles are computed as a rotation around the x-axis, followed by a rotation - around the y-axis, followed by a rotation around the z-axis. - In aerospace terms, x,y and z are also often referred to as roll pitch and yaw, so those names - are also available as a convenience. -*/ - -/*! \addtogroup cinterface C Interface - @{ -*/ - -/*! \relates XsEuler - \brief Clears all angles in the XsEuler object by setting them to 0 -*/ -void XsEuler_destruct(XsEuler* thisPtr) -{ - thisPtr->m_x = XsMath_zero; - thisPtr->m_y = XsMath_zero; - thisPtr->m_z = XsMath_zero; -} - -/*! \relates XsEuler - \brief Returns true if all angles in this object are zero -*/ -int XsEuler_empty(const XsEuler* thisPtr) -{ - return thisPtr->m_x == XsMath_zero && thisPtr->m_y == XsMath_zero && thisPtr->m_z == XsMath_zero; -} - -/*! \relates XsEuler - \brief Get an euler angle representation of the quaternion. -*/ -void XsEuler_fromQuaternion(XsEuler* thisPtr, const XsQuaternion* quat) -{ - XsReal sqw, dphi, dpsi; - - if (XsQuaternion_empty(quat)) - { - XsEuler_destruct(thisPtr); - return; - } - - sqw = quat->m_w * quat->m_w; - 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)); -} - -/*! @} */ diff --git a/extern/xstypes/xsfile.c b/extern/xstypes/xsfile.c deleted file mode 100644 index 5cce8c2..0000000 --- a/extern/xstypes/xsfile.c +++ /dev/null @@ -1,561 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#include "xsfile.h" -#include "xsstring.h" -#include -#ifndef _WIN32 -# include // close -# include // ioctl -# include // open, O_RDWR -# include // strcpy -# include -# include -# include -# include -#else -# include -# include -# include -#endif - -// helper -static FILE* openFile(const struct XsString* filename, const struct XsString* mode); - -/*! \class XsFile - \brief Encapsulates a file, providing a platform independent interface -*/ -/*! \addtogroup cinterface C Interface - @{ -*/ - - -/*! \relates XsFile - \brief Frees the resources of this object by closing the file if it is open. -*/ -void XsFile_destruct(struct XsFile* thisPtr) -{ - if (thisPtr->m_handle != NULL) - { - (void)XsFile_close(thisPtr); - } -} - -/*! \relates XsFile - \brief Creates a new binary file with name \a filename, contents of existing files will be discarded - \param filename name of the file to create - \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) -{ - XsString mode; - XsString_construct(&mode); - XsString_resize(&mode, 16); - - if (thisPtr->m_handle != NULL) - return XRV_ALREADYOPEN; - - if (writeOnly) - { - XsString_assign(&mode, 3, "wb"); - thisPtr->m_handle = openFile(filename, &mode); - } - else - { - XsString_assign(&mode, 4, "w+b"); - thisPtr->m_handle = openFile(filename, &mode); - } - - XsString_destruct(&mode); - if (thisPtr->m_handle == NULL) - return XRV_OUTPUTCANNOTBEOPENED; - else - return XRV_OK; -} - -/*! \relates XsFile - \brief Creates a new text file with name \a filename, contents of existing files will be discarded - \param filename name of the file to create - \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) -{ - XsString mode; - XsString_construct(&mode); - XsString_resize(&mode, 16); - - if (thisPtr->m_handle != NULL) - return XRV_ALREADYOPEN; - - if (writeOnly) - { - XsString_assign(&mode, 3, "wt"); - thisPtr->m_handle = openFile(filename, &mode); - } - else - { - XsString_assign(&mode, 4, "w+t"); - thisPtr->m_handle = openFile(filename, &mode); - } - - XsString_destruct(&mode); - if (thisPtr->m_handle == NULL) - return XRV_OUTPUTCANNOTBEOPENED; - else - return XRV_OK; -} - -/*! \relates XsFile - \brief Opens an existing binary file with name \a filename - \param filename name of the file to open - \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) -{ - XsString mode; - XsString_construct(&mode); - XsString_resize(&mode, 16); - - if (thisPtr->m_handle != NULL) - return XRV_ALREADYOPEN; - - if (readOnly) - { - XsString_assign(&mode, 3, "rb"); - thisPtr->m_handle = openFile(filename, &mode); - } - else - { - XsString_assign(&mode, 4, "r+b"); - thisPtr->m_handle = openFile(filename, &mode); - } - - XsString_destruct(&mode); - if (thisPtr->m_handle == NULL) - return XRV_OUTPUTCANNOTBEOPENED; - else - return XRV_OK; -} - -/*! \relates XsFile - \brief Opens an existing binary file with name \a filename - \param filename name of the file to open - \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) -{ - XsString mode; - XsString_construct(&mode); - XsString_resize(&mode, 16); - - if (thisPtr->m_handle != NULL) - return XRV_ALREADYOPEN; - - if (readOnly) - { - XsString_assign(&mode, 3, "rt"); - thisPtr->m_handle = openFile(filename, &mode); - } - else - { - XsString_assign(&mode, 4, "r+t"); - thisPtr->m_handle = openFile(filename, &mode); - } - - XsString_destruct(&mode); - if (thisPtr->m_handle == NULL) - return XRV_OUTPUTCANNOTBEOPENED; - else - return XRV_OK; -} - -/*! \brief Helper for file opening */ -FILE* openFile(const struct XsString* filename, const struct XsString* mode) -{ -#ifdef _WIN32 - wchar_t filenameW[XS_MAX_FILENAME_LENGTH]; - wchar_t modeW[16]; - (void)XsString_copyToWCharArray(filename, filenameW, XS_MAX_FILENAME_LENGTH); - (void)XsString_copyToWCharArray(mode, modeW, 16); - - __try - { - return _wfopen(filenameW, modeW); - } - __except(EXCEPTION_EXECUTE_HANDLER) - { - return NULL; - } -#else - return fopen(filename->m_data, mode->m_data); -#endif -} - -/*! \relates XsFile - \brief Reopens a file - \param filename Name of the file to open after the current one has been closed - \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) -{ -#ifdef _WIN32 - wchar_t filenameW[XS_MAX_FILENAME_LENGTH]; - wchar_t modeW[16]; - - (void)XsString_copyToWCharArray(filename, filenameW, XS_MAX_FILENAME_LENGTH); - (void)XsString_copyToWCharArray(mode, modeW, 16); - - thisPtr->m_handle = _wfreopen(filenameW, modeW, thisPtr->m_handle); -#else - thisPtr->m_handle = freopen(filename->m_data, mode->m_data, thisPtr->m_handle); -#endif - - if (thisPtr->m_handle == NULL) - return XRV_OUTPUTCANNOTBEOPENED; - else - return XRV_OK; -} - -/*! \relates XsFile - \brief Checks if a file is open - \returns 0 if a file is open, another value otherwise -*/ -int XsFile_isOpen(const struct XsFile *thisPtr) -{ - return (thisPtr->m_handle != NULL) ? 0 : 1; -} - -/*! \relates XsFile - \brief Checks if the file exists (can be accessed) - \param filename Name of the file to check for existence - \returns 0 if the file exists, another value otherwise -*/ -int XsFile_exists(const struct XsString* filename) -{ -#ifdef _WIN32 - wchar_t filenameW[XS_MAX_FILENAME_LENGTH]; - struct _stat buffer; - (void)XsString_copyToWCharArray(filename, filenameW, XS_MAX_FILENAME_LENGTH); - return _wstat(filenameW, &buffer); -#else - struct stat buffer; - return stat(filename->m_data, &buffer); -#endif -} - -/*! \relates XsFile - \brief Closes the file - \returns XRV_OK if the file was closed properly, XRV_ENDOFFILE otherwise -*/ -XsResultValue XsFile_close(struct XsFile *thisPtr) -{ - int rv; - - if (thisPtr->m_handle == NULL) - return XRV_NOFILEOPEN; - - rv = fclose(thisPtr->m_handle); - - // Or keep the handle in case of failure? - thisPtr->m_handle = NULL; - - if (rv == EOF) - return XRV_ENDOFFILE; - else - return XRV_OK; -} - - -/*! \relates XsFile - \brief Writes unwritten data to the file - \returns XRV_OK if the flushing was succesful, an XRV_ERROR otherwise - -*/ -XsResultValue XsFile_flush(struct XsFile *thisPtr) -{ - return fflush(thisPtr->m_handle) ? XRV_ERROR : XRV_OK; -} - -/*! \relates XsFile - \brief Reduces the file to a maximum size of \a fileSize bytes - \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, XsSize fileSize) -{ - return XsFile_resize(thisPtr, fileSize); -} - -/*! \relates XsFile - \brief Resizes the file to \a fileSize bytes - \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, XsSize fileSize) -{ -#ifdef _WIN32 - int32_t rv = _chsize_s(_fileno(thisPtr->m_handle), fileSize); -#else - int32_t rv = ftruncate(fileno(thisPtr->m_handle), fileSize); -#endif - if (rv != 0) - { - 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; - } - } - else - return XRV_OK; -} - -/*! \relates XsFile - \brief Deletes a file with name \a filename - \param filename Name of the file to delete - \returns XRV_OK if the file was erased, an error otherwise -*/ -XsResultValue XsFile_erase(const struct XsString* filename) -{ -#ifdef _WIN32 - wchar_t filenameW[XS_MAX_FILENAME_LENGTH]; - (void)XsString_copyToWCharArray(filename, filenameW, XS_MAX_FILENAME_LENGTH); - if (_wunlink(filenameW) != 0) -#else - if (unlink(filename->m_data) != 0) -#endif - { - switch (errno) - { - case EACCES: - return XRV_READONLY; - case ENOENT: - return XRV_NOTFOUND; - default: - return XRV_ERROR; - } - } - else - return XRV_OK; -} - -/*! \relates XsFile - \brief Reads a number of elements from a file - \param destination Buffer to store the read data in - \param size Size of each individual element to read - \param count Number of elements to read - \returns Total number of elements successfully read -*/ -XsSize XsFile_read(struct XsFile *thisPtr, void *destination, XsSize size, XsSize count) -{ - return fread(destination, size, count, thisPtr->m_handle); -} - -/*! \relates XsFile - \brief Writes a number of elements to a file - \param source Buffer that contains the elements to be written - \param size Size of each individual element to read - \param count Number of elements to write - \returns Total number of elements successfully written -*/ -XsSize XsFile_write(struct XsFile *thisPtr, const void *source, XsSize size, XsSize count) -{ - return fwrite(source, size, count, thisPtr->m_handle); -} - -/*! \relates XsFile - \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) -{ - return fgetc(thisPtr->m_handle); -} - -/*! \relates XsFile - \brief Writes a character to the file - \param character The caharacter to write - \returns XRV_OK on success, XRV_ERROR otehrwise -*/ -XsResultValue XsFile_putc(struct XsFile *thisPtr, int character) -{ - return fputc(character, thisPtr->m_handle) == EOF ? XRV_ERROR : XRV_OK; -} - -/*! \relates XsFile - \brief Reads characters from this file and stores them into str until (num-1) characters have been read or either a newline or the end-of-file is reached, whichever happens first. - \param str The destination for the read string - \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) -{ - return fgets(str, num, thisPtr->m_handle); -} - -/*! \relates XsFile - \brief Writes a null terminated c-string to the file - \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) -{ - return fputs(str, thisPtr->m_handle) != EOF ? XRV_OK : XRV_ERROR; -} - -/*! \relates XsFile - \brief Moves the current file position relative to the start of the file - \param offset Position in the file to move to, relative to the start of the file - \returns XRV_OK if the seek was succesful -*/ -XsResultValue XsFile_seek(struct XsFile *thisPtr, XsFilePos offset) -{ -#ifdef _WIN32 - return _fseeki64(thisPtr->m_handle, offset, SEEK_SET) ? XRV_ERROR : XRV_OK; -#else - return fseeko(thisPtr->m_handle, offset, SEEK_SET) ? XRV_ERROR : XRV_OK; -#endif -} - -/*! \relates XsFile - \brief Moves the current file position relative to the end of the file - \param offset Position in the file to move to, relative to the end of the file - \returns XRV_OK if the seek was succesful -*/ -XsResultValue XsFile_seek_r(struct XsFile *thisPtr, XsFilePos offset) -{ -#ifdef _WIN32 - return _fseeki64(thisPtr->m_handle, offset, SEEK_END) ? XRV_ERROR : XRV_OK; -#else - return fseeko(thisPtr->m_handle, offset, SEEK_END) ? XRV_ERROR : XRV_OK; -#endif -} - -/*! \relates XsFile - \brief Returns the current position in the file - \returns Current position in the current file -*/ -XsFilePos XsFile_tell(struct XsFile *thisPtr) -{ -#ifdef _WIN32 - return _ftelli64(thisPtr->m_handle); -#else - return ftello(thisPtr->m_handle); -#endif -} - -/*! \relates XsFile - \returns Returns 0 if the current file position is not 'end of file', non 0 if the position is 'end of file' -*/ -int XsFile_eof(struct XsFile *thisPtr) -{ - return feof(thisPtr->m_handle); -} - -/*! \relates XsFile - \returns Returns XRV_ERROR if the error flag for the file has been set, XRV_OK otherwise -*/ -XsResultValue XsFile_error(struct XsFile *thisPtr) -{ - return ferror(thisPtr->m_handle) ? XRV_ERROR : XRV_OK; -} - -/*! \relates XsFile - \brief Retrieves the full path for a filename - \param[in] filename The filename to expand - \param[out] fullPath The filename with a fully expanded path is returned in this parameter. This parameter must be allocated by the caller. - \returns XRV_OK if the fullpath could be retrieved, XRV_NULLPTR if fullPath is NULL, XRV_ERROR otherwise -*/ -XsResultValue XsFile_fullPath(const struct XsString* filename, struct XsString* fullPath) -{ - XsResultValue result = XRV_OK; - if (fullPath == NULL) - { - result = XRV_NULLPTR; - } - else - { -#ifdef _WIN32 - wchar_t filenamew[XS_MAX_FILENAME_LENGTH]; - wchar_t fullpath[XS_MAX_FILENAME_LENGTH]; - (void)XsString_copyToWCharArray(filename, filenamew, XS_MAX_FILENAME_LENGTH); - - if (_wfullpath(fullpath, filenamew, XS_MAX_FILENAME_LENGTH) == NULL) - result = XRV_ERROR; - else - XsString_assignWCharArray(fullPath, fullpath); -#else - // 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]; - if (realpath(filename->m_data, fullpath) == NULL) - result = XRV_ERROR; - else - XsString_assignCharArray(fullPath, fullpath); -#endif - } - return result; -} - -/*! \relates XsFile - \brief Reads a full line from the file - \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) -{ - int b; - XsResultValue ok = XRV_ENDOFFILE; - - XsString_erase(line, 0, line->m_size); - XsArray_reserve(line, 256); - - b = fgetc(thisPtr->m_handle); - if (b != -1) - ok = XRV_OK; - - while (b != -1) - { - XsString_push_back(line, (char)b); - if (b == '\n') - break; - b = fgetc(thisPtr->m_handle); - } - return ok; -} - -/*! \relates XsFile - \returns The file handle -*/ -FILE* XsFile_handle(struct XsFile *thisPtr) -{ - return thisPtr->m_handle; -} - - -/*! @} */ diff --git a/extern/xstypes/xsgpspvtdata.c b/extern/xstypes/xsgpspvtdata.c deleted file mode 100644 index d522bd3..0000000 --- a/extern/xstypes/xsgpspvtdata.c +++ /dev/null @@ -1,41 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#include "xsgpspvtdata.h" -#include - -/*! \addtogroup cinterface C Interface - @{ -*/ - -/*! \brief Destroy the %XsGpsPvtData object - \deprecated -*/ -void XsGpsPvtData_destruct(XsGpsPvtData* thisPtr) -{ - memset(thisPtr, 0, sizeof(XsGpsPvtData)); - thisPtr->m_pressureAge = 255; - thisPtr->m_gpsAge = 255; -} - -/*! \brief Returns true if the object is empty (when it contains no valid data) - \return non-zero (true) if the object is considered empty - \deprecated -*/ -int XsGpsPvtData_empty(const XsGpsPvtData* thisPtr) -{ - return thisPtr->m_pressureAge == 255 && thisPtr->m_gpsAge == 255; -} - -/*! @} */ diff --git a/extern/xstypes/xsint64array.c b/extern/xstypes/xsint64array.c deleted file mode 100644 index 113053c..0000000 --- a/extern/xstypes/xsint64array.c +++ /dev/null @@ -1,68 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#include "xsint64array.h" - -/*! \struct XsInt64Array - \brief A list of int64_t values - \sa XsArray -*/ - -/*! \copydoc XsArrayDescriptor::itemSwap - \note Specialization for int64_t*/ -void swapInt64(int64_t* a, int64_t* b) -{ - int64_t tmp = *a; - *a = *b; - *b = tmp; -} - -/*! \copydoc XsArrayDescriptor::itemCopy - \note Specialization for int64_t*/ -void copyInt64(int64_t* to, int64_t const* from) -{ - *to = *from; -} - -/*! \copydoc XsArrayDescriptor::itemCompare - \note Specialization for int64_t*/ -int compareInt64(int64_t const* a, int64_t const* b) -{ - if (*a < *b) - return -1; - if (*a > *b) - return 1; - return 0; -} - -//! \brief Descriptor for XsInt64Array -XsArrayDescriptor const g_xsInt64ArrayDescriptor = { - //lint --e{64} ignore exact type mismatches here - sizeof(int64_t), - XSEXPCASTITEMSWAP swapInt64, // swap - 0, // construct - XSEXPCASTITEMCOPY copyInt64, // copy construct - 0, // destruct - XSEXPCASTITEMCOPY copyInt64, // copy - XSEXPCASTITEMCOMP compareInt64, // compare - XSEXPCASTRAWCOPY XsArray_rawCopy // raw copy -}; - -/*! \copydoc XsArray_construct - \note Specialization for XsInt64Array -*/ -void XsInt64Array_construct(XsInt64Array* thisPtr, XsSize count, int64_t const* src) -{ - XsArray_construct(thisPtr, &g_xsInt64ArrayDescriptor, count, src); -} diff --git a/extern/xstypes/xsintarray.c b/extern/xstypes/xsintarray.c deleted file mode 100644 index 1975f8f..0000000 --- a/extern/xstypes/xsintarray.c +++ /dev/null @@ -1,69 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#include "xsintarray.h" - -/*! \struct XsIntArray - \brief A list of XsInt values - \sa XsArray -*/ - -/*! \copydoc XsArrayDescriptor::itemSwap - \note Specialization for int*/ -void swapInt(int* a, int* b) -{ - int tmp = *a; - *a = *b; - *b = tmp; -} - -/*! \copydoc XsArrayDescriptor::itemCopy - \note Specialization for int*/ -void copyInt(int* to, int const* from) -{ - *to = *from; -} - -/*! \copydoc XsArrayDescriptor::itemCompare - \note Specialization for int*/ -int compareInt(int const* a, int const* b) -{ - if (*a < *b) - return -1; - if (*a > *b) - return 1; - return 0; -} - - -//! \brief Descriptor for XsIntArray -XsArrayDescriptor const g_xsIntArrayDescriptor = { - //lint --e{64} ignore exact type mismatches here - sizeof(int), - XSEXPCASTITEMSWAP swapInt, // swap - 0, // construct - XSEXPCASTITEMCOPY copyInt, // copy construct - 0, // destruct - XSEXPCASTITEMCOPY copyInt, // copy - XSEXPCASTITEMCOMP compareInt, // compare - XSEXPCASTRAWCOPY XsArray_rawCopy // raw copy -}; - -/*! \copydoc XsArray_construct - \note Specialization for XsStringArray -*/ -void XsIntArray_construct(XsIntArray* thisPtr, XsSize count, int const* src) -{ - XsArray_construct(thisPtr, &g_xsIntArrayDescriptor, count, src); -} diff --git a/extern/xstypes/xslibraryloader.c b/extern/xstypes/xslibraryloader.c deleted file mode 100644 index f12be0d..0000000 --- a/extern/xstypes/xslibraryloader.c +++ /dev/null @@ -1,124 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#include "xslibraryloader.h" -#include "xsstring.h" - -#ifdef __GNUC__ -#include -#elif defined(_MSC_VER) -#include -#endif - -/*! \brief Dynamically load a library - - \param[in,out] thisp the XsLibraryLoader object handle - \param[in] libraryName the name of the library to load. The library - should be present in the current search path, or be specified by a path - \return non-zero if the library could be loaded, zero otherwise -*/ -int XsLibraryLoader_load(XsLibraryLoader* thisp, const XsString* libraryName) -{ -#ifdef __GNUC__ - if (XsLibraryLoader_isLoaded(thisp)) - return 0; - thisp->m_handle = dlopen(libraryName->m_data, RTLD_LAZY); -#elif defined(_MSC_VER) - wchar_t *libraryNameW; - XsSize required; - if (XsLibraryLoader_isLoaded(thisp)) - return 0; - required = XsString_copyToWCharArray(libraryName, NULL, 0); - libraryNameW = (wchar_t*)malloc(required * sizeof(wchar_t)); - - (void)XsString_copyToWCharArray(libraryName, libraryNameW, required); - - thisp->m_handle = LoadLibrary(libraryNameW); - - free(libraryNameW); -#endif - return XsLibraryLoader_isLoaded(thisp); -} - -/*! \brief Resolve a function from the library - - \param[in] thisp the library handle - \param[in] functionName the name of the function - \return a pointer to the resolved function, may be NULL if no function could be resolved -*/ -void* XsLibraryLoader_resolve(const XsLibraryLoader* thisp, const char* functionName) -{ -#ifdef __GNUC__ - return dlsym(thisp->m_handle, functionName); -#elif defined(_MSC_VER) -#pragma warning(push) -#pragma warning(disable: 4152) - return GetProcAddress(thisp->m_handle, functionName); -#pragma warning(pop) -#endif -} - -/*! \brief Unload the loaded library - - \param[in,out] thisp the library handle - \return zero on failure, non-zero otherwise -*/ -int XsLibraryLoader_unload(XsLibraryLoader* thisp) -{ - void *handle = thisp->m_handle; - thisp->m_handle = NULL; - if (handle) - { -#ifdef __GNUC__ - return dlclose(handle) == 0; -#elif defined(_MSC_VER) - return FreeLibrary(handle) != 0; -#endif - } - return 0; -} - -/*! \brief Check if a library is loaded - - \param[in] thisp the library handle - \return zero if nothing is loaded, non-zero otherwise -*/ -int XsLibraryLoader_isLoaded(const XsLibraryLoader* thisp) -{ - return thisp->m_handle != NULL; -} - -/*! \brief Get an error string after a failure occurred - \param[in,out] error the string to fill with the result error -*/ -void XsLibraryLoader_getErrorString(XsString* error) -{ -#ifdef __GNUC__ - XsString_assignCharArray(error, dlerror()); -#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); - - XsString_assignWCharArray(error, errorText); - LocalFree(errorText); -#endif -} diff --git a/extern/xstypes/xsmalloc.c b/extern/xstypes/xsmalloc.c deleted file mode 100644 index 2031a2d..0000000 --- a/extern/xstypes/xsmalloc.c +++ /dev/null @@ -1,164 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -/*! \file - This file contains platform-specific memory allocation routines -*/ - -#include "xsmalloc.h" -#if !(defined __ICCARM__) && !(defined _ADI_COMPILER) && !defined(__APPLE__) && !defined(__CRCC__) && !(defined(__arm__) && defined(__ARMCC_VERSION)) -#include -#endif -#include - -#ifdef XSENS_ASSERT_MALLOC -#include -#undef XSENS_ASSERT_MALLOC -#define XSENS_ASSERT_MALLOC(x) assert(x) -#else -#define XSENS_ASSERT_MALLOC(x) -#endif - -#if !(defined __ICCARM__) && !(defined _ADI_COMPILER) && defined(XSENS_DEBUG) -//#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]; -#endif - -#ifndef _MSC_VER -# ifdef __ANDROID__ -# define _aligned_malloc(size, align) memalign(align, size) -# elif (defined __ICCARM__) || (defined _ADI_COMPILER) || (defined __CRCC__) || (defined IAR_ARM_CM3) || (defined __ARMEL__) || (defined(__arm__) && defined(__ARMCC_VERSION)) -# define _aligned_malloc(a, b) malloc(a) -# else - -void* __cdecl _aligned_malloc(size_t _Size, size_t _Alignment) -{ - void* rv = 0; - int err = posix_memalign(&rv, _Alignment, _Size); - if (err == 0) - return rv; - return NULL; -} - -# endif - -#define _aligned_realloc(p, n, a) realloc(p, n) -#define _aligned_free(_Memory) free(_Memory) - -#endif //!_MSC_VER - - -//! \brief Allocates \a sz bytes of memory, optionally tracking the allocation -void* xsMalloc(size_t sz) -{ -#ifdef TRACK_ALLOCS - void* ptr = malloc(sz); - XSENS_ASSERT_MALLOC(ptr); - lastAllocIdx = (lastAllocIdx + 1) & (TRACK_ALLOCS-1); - lastAllocs[lastAllocIdx] = ptr; - return ptr; -#else - void* ptr = malloc(sz); - XSENS_ASSERT_MALLOC(ptr); - return ptr; -#endif -} - -//! \brief Reallocates \a sz bytes of memory, optionally tracking the allocation -void* xsRealloc(void* ptr, size_t sz) -{ -#ifdef TRACK_ALLOCS - lastFreeIdx = (lastFreeIdx + 1) & (TRACK_ALLOCS-1); - lastFrees[lastFreeIdx] = ptr; - - ptr = realloc(ptr, sz); - XSENS_ASSERT_MALLOC(ptr); - lastAllocIdx = (lastAllocIdx + 1) & (TRACK_ALLOCS-1); - lastAllocs[lastAllocIdx] = ptr; - return ptr; -#else - void* mem = realloc(ptr, sz); - XSENS_ASSERT_MALLOC(mem); - return mem; -#endif -} - -//! \brief Frees the memory pointed to by \a ptr, optionally tracking the allocation -void xsFree(void* ptr) -{ -#ifdef TRACK_ALLOCS - lastFreeIdx = (lastFreeIdx + 1) & (TRACK_ALLOCS-1); - lastFrees[lastFreeIdx] = ptr; -#endif - free(ptr); -} - -//! \brief Allocates \a sz bytes of memory on a 16 byte boundary, optionally tracking the allocation -void* xsAlignedMalloc(size_t sz) -{ -#ifdef TRACK_ALLOCS - void* ptr = _aligned_malloc(sz, 16); - XSENS_ASSERT_MALLOC(ptr); - lastAlignedAllocIdx = (lastAlignedAllocIdx + 1) & (TRACK_ALLOCS-1); - lastAlignedAllocs[lastAlignedAllocIdx] = ptr; - return ptr; -#else - void* ptr = _aligned_malloc(sz, 16); - XSENS_ASSERT_MALLOC(ptr); - return ptr; -#endif -} - -//! \brief Reallocates \a sz bytes of memory on a 16 byte boundary, optionally tracking the allocation -void* xsAlignedRealloc(void* ptr, size_t sz) -{ -#ifdef TRACK_ALLOCS - 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); - lastAlignedAllocs[lastAlignedAllocIdx] = ptr; - return ptr; -#else - void* mem = _aligned_realloc(ptr, sz, 16); - XSENS_ASSERT_MALLOC(mem); - return mem; -#endif -} - -//! \brief Frees the (aligned) memory pointed to by \a ptr, optionally tracking the allocation -void xsAlignedFree(void* ptr) -{ -#ifdef TRACK_ALLOCS - lastAlignedFreeIdx = (lastAlignedFreeIdx + 1) & (TRACK_ALLOCS-1); - lastAlignedFrees[lastAlignedFreeIdx] = ptr; -#endif - _aligned_free(ptr); -} - - - diff --git a/extern/xstypes/xsmath.c b/extern/xstypes/xsmath.c deleted file mode 100644 index 5149181..0000000 --- a/extern/xstypes/xsmath.c +++ /dev/null @@ -1,189 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#include "xsmath.h" -#include -#include - -/*! \namespace XsMath - \brief Namespace for mathematical constants and operations. -*/ -/*! \addtogroup cinterface C Interface - @{ -*/ - -//! \brief The value e -const XsReal XsMath_e = 2.7182818284590452353602874713527; -//! \brief The value pi -const XsReal XsMath_pi = 3.1415926535897932384626433832795028841971693993751058209749; -//! \brief A really small value -const XsReal XsMath_tinyValue = 1.0e-16; -//! \brief A convincingly large number -const XsReal XsMath_hugeValue = 1.0e+16; - -//! \brief A value related to the precisson of floating point arithmetic (2.2204460492503131e-016) -const XsReal XsMath_epsilon = 2.2204460492503131e-016; -//! \brief Square root of XsMath_epsilon -const XsReal XsMath_sqrtEpsilon = 1.4901161193847656e-008; - -#ifdef XSENS_SINGLE_PRECISION - //! \brief Value that represents the subnormal number in floating point wizardry - const XsReal XsMath_denormalized = 1e-37; - //! \brief Square root of XsMath_denormalized - const XsReal XsMath_sqrtDenormalized = 3.1622776601683793319988935444327e-19; -#else - //! \brief Value that represents the subnormal number in floating point wizardry - const XsReal XsMath_denormalized = 1e-307; - //! \brief Square root of XsMath_denormalized - const XsReal XsMath_sqrtDenormalized = 3.1622776601683793319988935444327e-154; -#endif - -//! \brief Value to convert radians to degrees by multiplication -const XsReal XsMath_rad2degValue = 57.295779513082320876798154814105; // (180.0/pi) -//! \brief Value to convert degrees to radians by multiplication -const XsReal XsMath_deg2radValue = 0.017453292519943295769236907684886; // (pi/180.0) - -//! \brief 0 -const XsReal XsMath_zero = 0.0; -//! \brief 0.25 -const XsReal XsMath_pt25 = 0.25; -//! \brief 0.5 -const XsReal XsMath_pt5 = 0.5; -//! \brief -0.5 -const XsReal XsMath_minusPt5 = -0.5; -//! \brief 1.0 -const XsReal XsMath_one = 1.0; -//! \brief -1.0 -const XsReal XsMath_minusOne = -1.0; -//! \brief 2 -const XsReal XsMath_two = 2.0; -//! \brief 4 -const XsReal XsMath_four = 4.0; -//! \brief -2 -const XsReal XsMath_minusTwo = -2.0; - -//! \brief -pi/2 -const XsReal XsMath_minusHalfPi = -1.5707963267948966192313216916397514420985846996875529104874; -//! \brief pi/2 -const XsReal XsMath_halfPi = 1.5707963267948966192313216916397514420985846996875529104874; -//! \brief 2*pi -const XsReal XsMath_twoPi = 6.2831853071795864769252867665590057683943387987502116419498; -//! \brief sqrt(2) -const XsReal XsMath_sqrt2 = 1.4142135623730950488016887242097; -//! \brief sqrt(0.5) -const XsReal XsMath_sqrtHalf = 0.5*1.4142135623730950488016887242097; - -#ifdef XSENS_SINGLE_PRECISION -//! \brief infinity value -const XsReal XsMath_infinity = FLT_MAX; -#else -//! \brief infinity value -const XsReal XsMath_infinity = DBL_MAX; -#endif - -/*! \brief Returns asin(\a x) for -1 < x < 1 -*/ -XsReal XsMath_asinClamped(XsReal x) -{ - if (x <= XsMath_minusOne) - return XsMath_minusHalfPi; - - if (x >= XsMath_one) - return XsMath_halfPi; - - return asin(x); -} - -/*! \brief Convert radians to degrees -*/ -XsReal XsMath_rad2deg(XsReal radians) -{ - return XsMath_rad2degValue * radians; -} - -/*! \brief Convert degrees to radians -*/ -XsReal XsMath_deg2rad(XsReal degrees) -{ - return XsMath_deg2radValue * degrees; -} - -/*! \brief Returns \a a to the power of 2 -*/ -XsReal XsMath_pow2(XsReal a) -{ - return a*a; -} - -/*! \brief Returns \a a to the power of 3 -*/ -XsReal XsMath_pow3(XsReal a) -{ - return a*a*a; -} - -/*! \brief Returns non-zero if \a x is finite -*/ -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_NN: - case _FPCLASS_ND: - case _FPCLASS_NZ: - case _FPCLASS_PZ: - case _FPCLASS_PD: - case _FPCLASS_PN: - return 1; - } - return _finite(x); -#elif __GNUC__ - return isfinite(x); -#elif defined(isfinite) - return isfinite(x); -#elif defined(_ADI_COMPILER) - return !(isnan(x) || isinf(x)); -#else - return 1; -#endif -} - -/*! \brief Returns \a d integer converted from a double precision floating point value -*/ -int32_t XsMath_doubleToLong(double d) -{ - if (d >= 0) - return (int32_t) floor(d+0.5); - else - return (int32_t) ceil(d-0.5); -} - -/*! \brief Returns \a d integer converted from a double precision floating point value -*/ -int64_t XsMath_doubleToInt64(double d) -{ - if (d >= 0) - return (int64_t) floor(d+0.5); - else - return (int64_t) ceil(d-0.5); -} - -/*! @} */ diff --git a/extern/xstypes/xsmatrix.c b/extern/xstypes/xsmatrix.c deleted file mode 100644 index a4a21c5..0000000 --- a/extern/xstypes/xsmatrix.c +++ /dev/null @@ -1,344 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#include "xsmatrix.h" -#include -#include -#include "xsatomicint.h" -#include -#include "xsmalloc.h" -#include "xsquaternion.h" -#include - -//lint -e123 -e40 -e522 the INC_ALLOC and INC_ALLOC(L) definitions confuse PCLint - -XsAtomicInt XSTYPES_DLL_API XsMatrix_allocCount = XSATOMICINT_INITIALIZER; //!< The number of times XsMatrix_ functions have allocated memory -XsAtomicInt XSTYPES_DLL_API XsMatrix_freeCount = XSATOMICINT_INITIALIZER; //!< The number of times XsMatrix_ functions have freed memory - -#ifdef XSENS_DEBUG -#define INC_ALLOC() (void)XsAtomicInt_preIncrement(&XsMatrix_allocCount) -#define INC_FREE() (void)XsAtomicInt_preIncrement(&XsMatrix_freeCount) -#else -#define INC_ALLOC() ((void)0) -#define INC_FREE() ((void)0) -#endif - -//lint -e641 conversion from enum to int should not be a problem - -/*! \class XsMatrix - \brief A class that represents a matrix of real numbers -*/ - -/*! \addtogroup cinterface C Interface - @{ -*/ - -/*! \relates XsMatrix \brief Construct the %XsMatrix as a reference to data in \a buffer */ -void XsMatrix_ref(XsMatrix* thisPtr, XsSize rows, XsSize cols, XsSize stride, XsReal* buffer, XsDataFlags flags) -{ - //assert(!(thisPtr->m_flags & XSDF_FixedSize)); - //XsMatrix_destruct(thisPtr); - - *((XsReal**) &thisPtr->m_data) = buffer; - *((int*) &thisPtr->m_flags) = (int) flags; - *((XsSize*) &thisPtr->m_rows) = rows; - *((XsSize*) &thisPtr->m_cols) = cols; - *((XsSize*) &thisPtr->m_stride) = stride; -} - -/*! \relates XsMatrix \brief Init the %XsMatrix and copy the data from \a src into the matrix if \a src is not null */ -void XsMatrix_construct(XsMatrix* thisPtr, XsSize rows, XsSize cols, XsSize stride, const XsReal* src, XsSize srcStride) -{ - XsSize r,c; - XsSize size = rows*stride; - - if (stride == 0) - { - stride = cols; - size = rows * stride; - } - if (size) - { - // init to size - XsReal* data = (XsReal*) xsMathMalloc(size*sizeof(XsReal)); - assert(data); - *((XsReal**) &thisPtr->m_data) = data; - INC_ALLOC(); - } - else - *((XsReal**) &thisPtr->m_data) = 0; - *((int*) &thisPtr->m_flags) = XSDF_Managed; - *((XsSize*) &thisPtr->m_rows) = rows; - *((XsSize*) &thisPtr->m_cols) = cols; - *((XsSize*) &thisPtr->m_stride) = stride; - - if (src && size) - { - if (srcStride == 0 || srcStride == stride) - memcpy(thisPtr->m_data, src, size*sizeof(XsReal)); - else - { - for (r = 0; r < rows; ++r) - for (c = 0; c < cols; ++c) - thisPtr->m_data[r*stride+c] = src[r*srcStride + c]; - } - } -} - -/*! \relates XsMatrix \brief Init the %XsMatrix and copy the data from \a src into the matrix if \a src is not null */ -void XsMatrix_assign(XsMatrix* thisPtr, XsSize rows, XsSize cols, XsSize stride, const XsReal* src, XsSize srcStride) -{ - XsSize r,c; - XsSize size = rows*stride; - - if (thisPtr->m_flags & XSDF_FixedSize) - { - if (rows == 0 && cols == 0) - { - *((int*) &thisPtr->m_flags) |= XSDF_Empty; - return; - } - - assert(thisPtr->m_rows == rows && thisPtr->m_cols == cols); - stride = thisPtr->m_stride; - size = thisPtr->m_rows * stride; - *((int*) &thisPtr->m_flags) &= ~XSDF_Empty; - } - else - { - if (thisPtr->m_rows == rows && thisPtr->m_cols == cols && - (stride == 0 || stride == thisPtr->m_stride)) - { - stride = thisPtr->m_stride; - size = rows * stride; - } - else - { - if (stride == 0) - { - stride = cols; - size = rows * stride; - } - 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)); - assert(data); - *((XsReal**) &thisPtr->m_data) = data; - *((int*) &thisPtr->m_flags) = XSDF_Managed; - INC_ALLOC(); - } - } - *((XsSize*) &thisPtr->m_rows) = rows; - *((XsSize*) &thisPtr->m_cols) = cols; - *((XsSize*) &thisPtr->m_stride) = stride; - } - } - if (src && size) - { - if (srcStride == 0 || srcStride == stride) - memcpy(thisPtr->m_data, src, size*sizeof(XsReal)); - else - { - for (r = 0; r < rows; ++r) - for (c = 0; c < cols; ++c) - thisPtr->m_data[r*stride+c] = src[r*srcStride + c]; - } - } -} - -/*! \relates XsMatrix \brief Clear the XsMatrix and release allocated resources */ -void XsMatrix_destruct(XsMatrix* thisPtr) -{ - if (thisPtr->m_data && (thisPtr->m_flags & XSDF_Managed)) - { - // clear contents - xsMathFree((void*) thisPtr->m_data); - INC_FREE(); - } - // init to 0 - if (!(thisPtr->m_flags & XSDF_FixedSize)) - { - *((XsReal**) &thisPtr->m_data) = 0; - *((XsSize*) &thisPtr->m_rows) = 0; - *((XsSize*) &thisPtr->m_cols) = 0; - *((XsSize*) &thisPtr->m_stride) = 0; - *((int*) &thisPtr->m_flags) = 0; - } - else - *((int*) &thisPtr->m_flags) |= XSDF_Empty; -} - -/*! \relates XsMatrix \brief Copy the contents of \a copy to the %XsMatrix */ -void XsMatrix_copy(XsMatrix* copy, XsMatrix const* src) -{ - if (copy == src) - { - return; - } - XsMatrix_assign(copy, src->m_rows, src->m_cols, 0, src->m_data, src->m_stride); -} - -/*! \relates XsMatrix \brief Set all the values in the matrix to zero */ -void XsMatrix_setZero(XsMatrix* thisPtr) -{ - 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; -} - -/*! \relates XsMatrix \brief Returns not zero if the matrix contains no values */ -int XsMatrix_empty(const XsMatrix* thisPtr) -{ - return (thisPtr->m_rows == 0) || (thisPtr->m_cols == 0) || (thisPtr->m_flags & XSDF_Empty); -} - -/*! \relates XsMatrix \brief Multiplies all values in this XsMatrix by \a scalar - \param scalar : Value to multiply by - \param dest : The XsMatrix to store the result in -*/ -void XsMatrix_multiplyScalar(const XsMatrix* thisPtr, XsReal scalar, XsMatrix* dest) -{ - 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; -} - -/*! \relates XsMatrix - \brief Returns the offset in the data for accessing the value at \a row and \a column - \param row The row of the value - \param column The column of the value - \returns The offset of the requested item in the internal buffer -*/ -XsSize XsMatrix_offset(const XsMatrix* thisPtr, XsSize row, XsSize column) -{ - return XsMatrix_offsetM(thisPtr, row, column); -} - -//! \relates XsMatrix \brief Returns the data value at \a row and \a column -XsReal XsMatrix_value(const XsMatrix* thisPtr, XsSize row, XsSize column) -{ - return thisPtr->m_data[XsMatrix_offsetM(thisPtr, row, column)]; -} - - -//! \relates XsMatrix \brief Sets the data \a value at \a row and \a column -void XsMatrix_setValue(XsMatrix* thisPtr, XsSize row, XsSize column, XsReal value) -{ - thisPtr->m_data[XsMatrix_offsetM(thisPtr, row, column)] = value; -} - -//! \relates XsMatrix \brief Returns not zero if the dimensions of the %XsMatrix are equal to \a rows and \a columns -int XsMatrix_dimensionsMatch(const XsMatrix* thisPtr, XsSize rows, XsSize columns) -{ - return thisPtr->m_rows == rows && thisPtr->m_cols == columns; -} - -/*! \relates XsMatrix \brief Get an orientation matrix representation of the quaternion. */ -void XsMatrix_fromQuaternion(XsMatrix* thisPtr, const XsQuaternion* quat) -{ - XsReal q00, q11, q22, q33, q01, q02, q03, q12, q13, q23; - - if (XsQuaternion_empty(quat)) - { - XsMatrix_destruct(thisPtr); - 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; - - 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; - - XsMatrix_assign(thisPtr, 3, 3, 3, 0, 0); - - XsMatrix_setValue(thisPtr, 0, 0, q00 + q11 - q22 - q33); - XsMatrix_setValue(thisPtr, 0, 1, (q12 - q03) * XsMath_two); - XsMatrix_setValue(thisPtr, 0, 2, (q13 + q02) * XsMath_two); - - XsMatrix_setValue(thisPtr, 1, 0, (q12 + q03) * XsMath_two); - XsMatrix_setValue(thisPtr, 1, 1, q00 - q11 + q22 - q33); - XsMatrix_setValue(thisPtr, 1, 2, (q23 - q01) * XsMath_two); - - XsMatrix_setValue(thisPtr, 2, 0, (q13 - q02) * XsMath_two); - XsMatrix_setValue(thisPtr, 2, 1, (q23 + q01) * XsMath_two); - XsMatrix_setValue(thisPtr, 2, 2, q00 - q11 - q22 + q33); -} - -/*! \relates XsMatrix \brief Swap the contents of \a a and \a b - \details This function swaps the internal buffers so no actual data is moved around. - This won't work for unmanaged data such as fixed size matrices (XsMatrix3x3) - \param a Object whose contents will be placed in \a b - \param b Object whose contents will be placed in \a a -*/ -void XsMatrix_swap(XsMatrix* a, XsMatrix* b) -{ - XsMatrix tmp; - if ((!a->m_data || (a->m_flags & XSDF_Managed)) && (!b->m_data || (b->m_flags & XSDF_Managed))) - { - *((XsReal**) &tmp.m_data) = a->m_data; - *((XsSize*) &tmp.m_rows) = a->m_rows; - *((XsSize*) &tmp.m_cols) = a->m_cols; - *((XsSize*) &tmp.m_stride) = a->m_stride; - *((int*) &tmp.m_flags) = a->m_flags; - - *((XsReal**) &a->m_data) = b->m_data; - *((XsSize*) &a->m_rows) = b->m_rows; - *((XsSize*) &a->m_cols) = b->m_cols; - *((XsSize*) &a->m_stride) = b->m_stride; - *((int*) &a->m_flags) = b->m_flags; - - *((XsReal**) &b->m_data) = tmp.m_data; - *((XsSize*) &b->m_rows) = tmp.m_rows; - *((XsSize*) &b->m_cols) = tmp.m_cols; - *((XsSize*) &b->m_stride) = tmp.m_stride; - *((int*) &b->m_flags) = tmp.m_flags; - } - else - { - XsSize r, c; - XsReal v; - 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); - for (c = 0; c < a->m_cols; ++c, ++aa, ++bb) - { - v = *aa; - *aa = *bb; - *bb = v; - } - } - } -} - -/*! @} */ - -//lint +e123 +e40 +e522 diff --git a/extern/xstypes/xsmatrix3x3.c b/extern/xstypes/xsmatrix3x3.c deleted file mode 100644 index d893122..0000000 --- a/extern/xstypes/xsmatrix3x3.c +++ /dev/null @@ -1,67 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#include "xsmatrix3x3.h" -#include - -//lint -e641 conversion from enum to int should not be a problem - -/*! \class XsMatrix3x3 - \brief A class that represents a fixed size (3x3) matrix -*/ - -/*! \addtogroup cinterface C Interface - @{ -*/ - -/*! \relates XsMatrix3x3 \brief Init the %XsMatrix3x3 */ -void XsMatrix3x3_construct(XsMatrix3x3* thisPtr) -{ - XsMatrix_ref(&thisPtr->m_matrix, 3, 3, 3, (XsReal*) thisPtr->m_fixedData, XSDF_FixedSize); -} - -/*! \relates XsMatrix3x3 \brief Init the %XsMatrix3x3 and copy the data from \a src into the matrix if \a src is not null */ -void XsMatrix3x3_assign(XsMatrix3x3* thisPtr, const XsReal* src, XsSize srcStride) -{ - XsSize r, c; - - if (src) - { - if (srcStride == 0 || srcStride == 3) - 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]; - } - } -} - -/*! \relates XsMatrix3x3 \brief Frees the Matrix3x3 */ -void XsMatrix3x3_destruct(XsMatrix3x3* thisPtr) -{ - // don't do anything, no memory needs to be freed, which is what XsMatrix_destruct will figure out - assert(thisPtr->m_matrix.m_flags & XSDF_FixedSize); - (void) thisPtr; - //XsMatrix_destruct(&thisPtr->m_matrix); -} - -/*! \relates XsMatrix3x3 \brief Copy the contents of the %XsMatrix3x3 to \a copy */ -void XsMatrix3x3_copy(XsMatrix* copy, XsMatrix3x3 const* src) -{ - XsMatrix_copy(copy, &src->m_matrix); -} - -/*! @} */ diff --git a/extern/xstypes/xsmessage.c b/extern/xstypes/xsmessage.c deleted file mode 100644 index e2b1f77..0000000 --- a/extern/xstypes/xsmessage.c +++ /dev/null @@ -1,1261 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#include "xsmessage.h" -#include -#include // memset -#include "xsbusid.h" -#include "xsdataformat.h" -#include - -//lint -e64 -e18 ignore exact type mismatches here - -////////////////////////////////////////////////////////////////////////////////////////// -// 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_DATAEXT0 6 - -#define XS_SELFTEST_OK 0x1FF - -// message data lengths -#define XS_LEN_TRANSPORTMODE 1 -#define XS_LEN_DEVICEID 4 -#define XS_LEN_INITBUSRESULTS 4 -#define XS_LEN_PERIOD 2 -#define XS_LEN_BUSPWR 2 -#define XS_LEN_DATALENGTH 2 -#define XS_LEN_CONFIGURATION 118 -#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_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_SYNCINMODE 2 -#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 -#define XS_LEN_SYNCOUTPULSEWIDTH 4 -#define XS_LEN_ERRORMODE 2 -#define XS_LEN_TRANSMITDELAY 2 -#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_LOCATIONID 2 -#define XS_LEN_EXTOUTPUTMODE 2 -#define XS_LEN_INITTRACKMODE 2 -#define XS_LEN_STOREFILTERSTATE 0 -#define XS_LEN_UTCTIME 12 -#define XS_LEN_FILTERPROFILELABEL 20 -#define XS_LEN_FILTERPROFILEFULL (1+1+XS_LEN_FILTERPROFILELABEL) -#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_GPSLEVERARM XS_LEN_GNSSLEVERARM //!< \deprecated -#define XS_LEN_GNSSLEVERARM 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_GNSSSTATUS (1+5*16) -#define XS_LEN_GPSSTATUS XS_LEN_GNSSSTATUS //!< \deprecated -#define XS_LEN_CLIENTUSAGE 1 -#define XS_LEN_CLIENTPRIORITY 1 -#define XS_LEN_WIRELESSCONFIG 6 -#define XS_LEN_INFOREQUEST 1 -#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_ORIENT_EULERDATA 12 -#define XS_LEN_ORIENT_MATRIXSTA 36 -#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_ORIENT_QUATDATA_FLT 4 -#define XS_LEN_ORIENT_EULERDATA_FLT 3 -#define XS_LEN_ORIENT_MATRIXSTA_FLT 9 - -#pragma pack(push, 1) -// little endian -union Itypes { - int64_t i64; - struct { - int32_t i1,i0; - } i32; - struct { - int16_t s3,s2,s1,s0; - } i16; - struct { - signed char b7,b6,b5,b4,b3,b2,b1,b0; - } i8; - - double d; - struct { - float f1,f0; - } f32; -}; -#pragma pack(pop) - - -/*! \addtogroup cinterface C Interface - @{ -*/ - -typedef union Itypes Itypes; - -/*! \brief Calculate the sum of the values in the buffer - \details - This function calculates the sum of the byte values for the first \a count bytes in the \a buffer - \param buffer An array of (unsigned) bytes - \param count The number of bytes in the buffer - \returns The unsigned sum of the byte values in the buffer modulo 256 -*/ -uint8_t byteSum(const uint8_t* buffer, XsSize count) -{ - register uint8_t sum = 0; - for (; count; --count, sum += *(buffer++)); - return sum; -} - -/*! \brief Get the buffer at offset \a 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; - 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) -{ - 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) -{ - 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) -{ - if (thisPtr->m_autoUpdateChecksum) - { - thisPtr->m_checksum[0] += byteSum(XsMessage_getDataBuffer(thisPtr, offset), sizeofValue); - thisPtr->m_checksum[0] -= byteSum(value, sizeofValue); - } -} - -/*! \brief Swap the endianness based on the data size */ -static inline void swapEndian(void *data, const XsSize size) -{ - uint16_t *i16; - uint32_t *i32; - uint64_t *i64; - - switch (size) - { - case sizeof(char): - break; - case sizeof(*i16): - i16 = data; - *i16 = swapEndian16(*i16); - break; - case sizeof(*i32): - i32 = data; - *i32 = swapEndian32(*i32); - break; - case sizeof(*i64): - i64 = data; - *i64 = swapEndian64(*i64); - break; - default: - assert(0); - } -} - -/*! \brief Get data of size \a size at \a offset, and put it byteswapped into \a value */ -void XsMessage_getEndianCorrectData(XsMessage const* thisPtr, void *value, XsSize size, XsSize offset) -{ - memcpy(value, XsMessage_cdataAtOffset(thisPtr, offset), size); - swapEndian(value, size); -} - -/*! \brief Set value \a value of size \a size byteswapped at \a offset */ -void XsMessage_setEndianCorrectData(XsMessage *thisPtr, void const *value, XsSize size, XsSize offset) -{ - void* dest; - XsMessage_ensureDataSize(thisPtr, offset, size); - XsMessage_updateChecksumWithValue(thisPtr, value, size, offset); - dest = XsMessage_dataAtOffset(thisPtr, offset); - memcpy(dest, value, size); - swapEndian(dest, size); -} - -/*! \brief This function initializes the %XsMessage object and reserves \a dataSize bytes for data - - \param[in] dataSize the expected size of the message payload -*/ -void XsMessage_constructSized(XsMessage* thisPtr, XsSize dataSize) -{ - XsSize msgSize; - XsMessageHeader* hdr; - - if (dataSize < 255) - msgSize = dataSize + 5; - else - msgSize = dataSize + 7; - - XsByteArray_construct(&thisPtr->m_message, msgSize, 0); - memset(thisPtr->m_message.m_data, 0, msgSize); - hdr = (XsMessageHeader*) (void*) thisPtr->m_message.m_data; - hdr->m_preamble = XS_PREAMBLE; - hdr->m_messageId = 0; - hdr->m_busId = XS_BID_MASTER; - - if (dataSize < XS_EXTLENCODE) - { - hdr->m_length = (uint8_t) dataSize; - *((uint8_t**) &thisPtr->m_checksum) = &hdr->m_datlen.m_data[dataSize]; //lint !e662 - thisPtr->m_checksum[0] = -(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_low = (uint8_t) dataSize; - *((uint8_t**) &thisPtr->m_checksum) = &hdr->m_datlen.m_extended.m_data[dataSize]; //lint !e662 - thisPtr->m_checksum[0] = -(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; -} - -/*! \brief This function initializes the %XsMessage object -*/ -void XsMessage_construct(XsMessage* thisPtr) -{ - XsMessage_constructSized(thisPtr, 0); -} - -/*! \brief Construct an XsMessage as a copy of XsMessage \a src -*/ -void XsMessage_copyConstruct(XsMessage* thisPtr, XsMessage const* src) -{ - if (!src) - XsMessage_construct(thisPtr); - else - { - XsMessageHeader* hdr; - XsSize dataSize; - XsArray_copyConstruct(&thisPtr->m_message, &src->m_message); - thisPtr->m_autoUpdateChecksum = src->m_autoUpdateChecksum; - 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]; //lint !e662 - else - *((uint8_t**) &thisPtr->m_checksum) = &hdr->m_datlen.m_data[dataSize]; //lint !e662 - } -} - -/*! \brief This function reinitializes the %XsMessage object and reserves \a dataSize bytes for data - \param[in] dataSize the expected size of the message payload -*/ -void XsMessage_assign(XsMessage* thisPtr, XsSize dataSize) -{ - XsMessage_destruct(thisPtr); - XsMessage_constructSized(thisPtr, dataSize); -} - -/*! \brief This function initializes the %XsMessage object and reserves \a msgSize bytes for data, it then copies in the data from \a src - \param msgSize the size of the data pointed to by src - \param src the data to load the message from -*/ -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]; -} - -/*! \brief This function clears the data in the message - -*/ -void XsMessage_destruct(XsMessage* thisPtr) -{ - XsArray_destruct(&thisPtr->m_message); - *((uint8_t**) &thisPtr->m_checksum) = 0; -} - -/*! \brief This function copies from \a thisPtr to \a copy - \param copy the object to copy to -*/ -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]; - copy->m_autoUpdateChecksum = thisPtr->m_autoUpdateChecksum; -} - -/*! \brief This function returns the datasize of the message in \a thisptr - \returns the size of the message payload -*/ -XsSize XsMessage_dataSize(XsMessage const* thisPtr) -{ - XsMessageHeader* hdr; - - if (!thisPtr->m_message.m_data) - return 0; - - 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; - else - return ((XsSize) hdr->m_length); -} - -/*! \brief This function returns a const pointer to the \a offset in the data of the message in \a thisptr - - \param offset the offset of the data to be returned - - \returns a pointer to the data at offset \a offset -*/ -const uint8_t* XsMessage_constData(XsMessage const* thisPtr, XsSize offset) -{ - if (!thisPtr->m_message.m_data) - return 0; - return XsMessage_cdataAtOffset(thisPtr, offset); -} - -/*! \brief This function returns a const pointer to the header of the message in \a thisptr - - - \returns a pointer to the start of the message -*/ -const uint8_t* XsMessage_getMessageStart(XsMessage const* thisPtr) -{ - return thisPtr->m_message.m_data; -} - -/*! \brief Return the length of the message buffer. - - The function returns the total size of the message, including the checksum. This - is in effect the number of bytes that would be transferred if the message were to - be sent over a communications channel. - - \returns the total message size -*/ -XsSize XsMessage_getTotalMessageSize(XsMessage const* thisPtr) -{ - XsMessageHeader* hdr; - - if (!thisPtr->m_message.m_data) - return 0; - - 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 - return ((XsSize) hdr->m_length) + 5; -} - -//lint -save -e661 -e662 -e415 -e416 -e419 - -/*! \brief Returns the byte value at \a offset in the data of the message - - \param offset the offset in the payload at which to read data - - \returns the byte at offset \a offset in the message payload -*/ -uint8_t XsMessage_getDataByte(XsMessage const* thisPtr, XsSize offset) -{ - return *XsMessage_cdataAtOffset(thisPtr, offset); -} - -/*! \brief Returns the short value at \a offset in the data of the message - - \param offset the offset in the payload at which to read data - - \returns the 16-bit integer value at offset \a offset in the message payload -*/ -uint16_t XsMessage_getDataShort(XsMessage const* thisPtr, XsSize offset) -{ - uint16_t ret; - XsMessage_getEndianCorrectData(thisPtr, &ret, sizeof(ret), offset); - return ret; -} - -/*! \brief Returns the long value at \a offset in the data of the message - - \param offset the offset in the payload at which to read data - - \returns the 32-bit integer value at offset \a offset in the message payload -*/ -uint32_t XsMessage_getDataLong(XsMessage const* thisPtr, XsSize offset) -{ - uint32_t ret; - XsMessage_getEndianCorrectData(thisPtr, &ret, sizeof(ret), offset); - return ret; -} - -/*! \brief Returns the long value at \a offset in the data of the message - - \param offset the offset in the payload at which to read data - - \returns the 64-bit integer value at offset \a offset in the message payload -*/ -uint64_t XsMessage_getDataLongLong(XsMessage const* thisPtr, XsSize offset) -{ - uint64_t ret; - XsMessage_getEndianCorrectData(thisPtr, &ret, sizeof(ret), offset); - return ret; -} - -/*! \brief Returns the float value at \a offset in the data of the message - - \param offset the offset in the payload at which to read data - - \returns the single precision float value at offset \a offset in the message payload -*/ -float XsMessage_getDataFloat(XsMessage const* thisPtr, XsSize offset) -{ - float ret; - XsMessage_getEndianCorrectData(thisPtr, &ret, sizeof(ret), offset); - return ret; -} - -/*! \brief Returns the double at \a offset in the data of the message - \param offset the offset in the payload at which to read data - - \returns the double precision floating point value at offset \a offset in the message payload -*/ -double XsMessage_getDataDouble(XsMessage const* thisPtr, XsSize offset) -{ - double ret; - XsMessage_getEndianCorrectData(thisPtr, &ret, sizeof(ret), offset); - return ret; -} - -/*! \brief Returns the F12.20 value at \a offset in the data of the message - \param offset the offset in the payload at which to read data - - \returns the 12.20 fixed point value at offset \a offset in the message payload -*/ -double XsMessage_getDataF1220(XsMessage const* thisPtr, XsSize offset) -{ - int32_t tmp; - Itypes rv; - tmp = (int32_t) XsMessage_getDataLong(thisPtr, offset); - - rv.d = ((double) tmp)/1048576.0; - rv.i64 = (rv.i64 & ~1LL) | (tmp & 1); - return rv.d; -} - -/*! \brief Returns the F16.32 value at \a offset in the data of the message - \param offset the offset in the payload at which to read data - - \returns the 16.32 fixed point value at offset \a offset in the message payload -*/ -double XsMessage_getDataFP1632(XsMessage const* thisPtr, XsSize offset) -{ - int16_t fpint; - int32_t fpfrac; - Itypes fp, rv; - - fpfrac = (int32_t) XsMessage_getDataLong(thisPtr, offset); - fpint = (int16_t) XsMessage_getDataShort(thisPtr, offset+4); - - fp.i32.i0 = fpint; - fp.i32.i1 = fpfrac; - - rv.d = (double) fp.i64 / 4294967296.0; - rv.i64 = (rv.i64 & ~1LL) | (fpfrac & 1); - return rv.d; -} - -/*! \brief Returns a const pointer to the data buffer of the message - - \param offset the offset in the payload at which to read data - - - \returns a const pointer to the data buffer of the message -*/ -const uint8_t* XsMessage_getDataBuffer(XsMessage const* thisPtr, XsSize offset) -{ - return XsMessage_constData(thisPtr, offset); -} - -/*! \brief Set the byte at \a offset in the message to \a value - - \param value the 8-bit value to set - \param offset the offset in the message payload at which to write the data -*/ -void XsMessage_setDataByte(XsMessage* thisPtr, uint8_t value, XsSize offset) -{ - XsMessage_setEndianCorrectData(thisPtr, &value, sizeof(value), offset); -} - -/*! \brief Sets the short at \a offset in the message to \a value - - \param value the 16-bit value to set - \param offset the offset in the message payload at which to write the data -*/ -void XsMessage_setDataShort(XsMessage* thisPtr, uint16_t value, XsSize offset) -{ - XsMessage_setEndianCorrectData(thisPtr, &value, sizeof(value), offset); -} - -/*! \brief Sets the long at \a offset in the message to \a value - - \param value the 32-bit value to set - \param offset the offset in the message payload at which to write the data -*/ -void XsMessage_setDataLong(XsMessage* thisPtr, uint32_t value, XsSize offset) -{ - XsMessage_setEndianCorrectData(thisPtr, &value, sizeof(value), offset); -} - -/*! \brief Sets the long at \a offset in the message to \a value - - \param value the 64-bit value to set - \param offset the offset in the message payload at which to write the data -*/ -void XsMessage_setDataLongLong(XsMessage* thisPtr, uint64_t value, XsSize offset) -{ - XsMessage_setEndianCorrectData(thisPtr, &value, sizeof(value), offset); -} - - -/*! \brief Sets the float at \a offset in the message to \a value - - \param value the single precision floating point value to set - \param offset the offset in the message payload at which to write the data -*/ -void XsMessage_setDataFloat(XsMessage* thisPtr, float value, XsSize offset) -{ - XsMessage_setEndianCorrectData(thisPtr, &value, sizeof(value), offset); -} - -/*! \brief Sets the double at \a offset in the message to \a value - - \param value the double precision floating point value to set - \param offset the offset in the message payload at which to write the data -*/ -void XsMessage_setDataDouble(XsMessage* thisPtr, double value, XsSize offset) -{ - XsMessage_setEndianCorrectData(thisPtr, &value, sizeof(value), offset); -} - -/*! \brief Sets the F12.20 at \a offset in the message to \a value - - \param value the 12.20 fixed point value to set - \param offset the offset in the message payload at which to write the data -*/ -void XsMessage_setDataF1220(XsMessage* thisPtr, double value, XsSize offset) -{ - Itypes fp; - uint32_t val; - - fp.d = value; - val = (uint32_t) (int32_t) (value*1048576.0); - - XsMessage_setDataLong(thisPtr, (val & ~1UL) | (fp.i64 & 1), offset); -} - -/*! \brief Sets the F16.32 at \a offset in the message to \a value - - \param value the 16.32 fixed point value to set - \param offset the offset in the message payload at which to write the data -*/ -void XsMessage_setDataFP1632(XsMessage* thisPtr, double value, XsSize offset) -{ - Itypes fp; - int16_t fpint; - int32_t fpfrac; - int32_t dexp; - uint32_t b; - - fp.d = value; - b = (uint32_t) (fp.i64 & 1); - dexp = ((fp.i32.i0 & (0x7fffffffL)) >> 20) - 1023; - - if (dexp <= 14) - { - fp.i16.s0 = (fp.i16.s0 & 0x000F) | 0x0010; - if (value < 0) - fp.i64 = -fp.i64; - if (dexp > -32) - fp.i64 = fp.i64 >> (20-dexp); // 52-32 - exponent - else - fp.i64 = fp.i64 >> 52; // this could be optimized? - fpint = fp.i16.s1; - fpfrac = fp.i32.i1; - } - else - { - if (value < 0) - { - fpint = ((int16_t) (uint16_t) (0x8000)); - fpfrac = 0; - } - else - { - fpint = 0x7fff; - fpfrac = -1; - } - } - - XsMessage_setDataLong(thisPtr, (fpfrac & ~1L) | b, offset); - XsMessage_setDataShort(thisPtr, fpint, offset+4); -} - -/*! \brief Puts \a size number of bytes from \a buffer into the message at \a offset - \remarks The buffersize of will be increased if \a buffer is too large - - \param buffer the buffer to copy - \param size the size of the buffer - \param offset the offset at which to copy the data -*/ -void XsMessage_setDataBuffer(XsMessage* thisPtr, const uint8_t* buffer, XsSize size, XsSize offset) -{ - XsMessage_ensureDataSize(thisPtr, offset, size); - XsMessage_updateChecksumWithValue(thisPtr, buffer, size, offset); - memcpy(XsMessage_dataAtOffset(thisPtr, offset), buffer, size); -} - -/*! \relates XsMessage - \brief Returns the byte size of \a id if the format is a floating point format - - \param id : The XsDataIdentifier to query - \returns Returns the byte size of XsDataIdentifier \a id -*/ -uint8_t XsMessage_getFPValueSize(XsDataIdentifier id) -{ - switch (id & XDI_SubFormatMask) - { - case XDI_SubFormatFloat: - return 4; - - case XDI_SubFormatDouble: - return 8; - - case XDI_SubFormatFp1632: - return 6; - - case XDI_SubFormatFp1220: - return 4; - - default: - return 0; - } -} - -/*! \returns a double converted from float \a f - - \param[in] f The float to convert -*/ -static double convertFromFloat(float f) -{ - Itypes rv, tmp; - tmp.f32.f0 = f; - rv.d = (double) f; - rv.i64 = (rv.i64 & ~1LL) | (tmp.i32.i0 & 1); - return rv.d; -} - -/*! \returns a float converted from double \a d - - \param[in] d The double to cenvert -*/ -static float convertToFloat(double d) -{ - Itypes rv, tmp; - tmp.d = d; - rv.f32.f0 = (float) d; - rv.i32.i0 = (rv.i32.i0 & ~1L) | (tmp.i64 & 1); - return rv.f32.f0; -} - -/*! \brief Return current data values as a double(64 bits), after converting it from float, double, FP1632 or FP1220 depending on outputSettings - - \param outputSettings the output settings containing the current floating/fixed point settings - \param dest an array of double values containing at least \a numValues items - \param offset the offset in the message payload at which to start reading - \param numValues the amount of numbers to read -*/ -void XsMessage_getDataFPValues(XsMessage const* thisPtr, uint64_t outputSettings, double *dest, XsSize offset, XsSize numValues) -{ - XsSize i; - for (i=0; im_message.m_data[i]; - - return cs; -} - -/*! \brief Update the checksum for the message -*/ -void XsMessage_recomputeChecksum(XsMessage* thisPtr) -{ - assert(thisPtr->m_checksum); - thisPtr->m_checksum[0] = XsMessage_computeChecksum(thisPtr); -} - -/*! \brief Returns non-zero if the checksum inside the message is correct for the message, zero otherwise - - - \returns true (non-zero) if the checksum inside the message is correct, false (zero) otherwise -*/ -int XsMessage_isChecksumOk(XsMessage const* thisPtr) -{ - assert(thisPtr->m_checksum); - return thisPtr->m_checksum[0] == XsMessage_computeChecksum(thisPtr); -} - -/*! \brief Returns a pointer to the message header for this message */ -XsMessageHeader* XsMessage_getHeader(XsMessage* thisPtr) -{ - 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; -} - -/*! \brief Test if this message is empty - - - - \returns non-zero if this message is empty, zero otherwise -*/ -int XsMessage_empty(XsMessage const* thisPtr) -{ - XsMessageHeader* hdr = (XsMessageHeader*) (void*) thisPtr->m_message.m_data; - if (!hdr) - return 1; - return (hdr->m_messageId == 0 && hdr->m_busId == XS_BID_MASTER); -} - -/*! \brief Resize the buffer of message to \a newSize bytes - - \param newSize the new size of the message payload buffer -*/ -void XsMessage_resizeData(XsMessage* thisPtr, XsSize newSize) -{ - XsSize index, oldSize; - XsByteArray old = XSBYTEARRAY_INITIALIZER; - XsMessageHeader* oldHdr, *newHdr; - uint8_t* oldData, *newData; - - oldSize = XsMessage_dataSize(thisPtr); - if (oldSize == newSize) - return; - - XsArray_swap(&thisPtr->m_message, &old); - XsMessage_constructSized(thisPtr, newSize); - - newHdr = (XsMessageHeader*) thisPtr->m_message.m_data; - oldHdr = (XsMessageHeader*) old.m_data; - if (!oldHdr) // our original message may have been empty / uninitialized - return; - - if (thisPtr->m_autoUpdateChecksum) - thisPtr->m_checksum[0] += newHdr->m_busId; - - newHdr->m_busId = oldHdr->m_busId; - newHdr->m_preamble = oldHdr->m_preamble; - newHdr->m_messageId = oldHdr->m_messageId; - - if (thisPtr->m_autoUpdateChecksum) - thisPtr->m_checksum[0] -= newHdr->m_busId + newHdr->m_messageId; - - if (oldSize >= XS_EXTLENCODE) - oldData = oldHdr->m_datlen.m_extended.m_data; - else - oldData = oldHdr->m_datlen.m_data; - - if (newSize >= XS_EXTLENCODE) - newData = newHdr->m_datlen.m_extended.m_data; - else - newData = newHdr->m_datlen.m_data; - - if (oldSize > newSize) - oldSize = newSize; // speed up the loop - for (index = 0; index < oldSize; ++index) - newData[index] = oldData[index]; - - if (thisPtr->m_autoUpdateChecksum) - thisPtr->m_checksum[0] -= byteSum(oldData, oldSize); - - XsArray_destruct(&old); -} - -/*! \brief Set the bus id for this message to \a busId - - \param busId the bus identifier - - \sa XS_BID_BROADCAST XS_BID_MASTER XS_BID_MT -*/ -void XsMessage_setBusId(XsMessage* thisPtr, uint8_t busId) -{ - XsMessageHeader* hdr; - if (!thisPtr->m_message.m_data) - XsMessage_construct(thisPtr); - - hdr = (XsMessageHeader*) (void*) thisPtr->m_message.m_data; - if (thisPtr->m_autoUpdateChecksum) - thisPtr->m_checksum[0] += hdr->m_busId; - hdr->m_busId = busId; - if (thisPtr->m_autoUpdateChecksum) - thisPtr->m_checksum[0] -= hdr->m_busId; -} - -/*! \brief Set the message id for this message to \a msgId - - \param msgId the message identifier -*/ -void XsMessage_setMessageId(XsMessage* thisPtr, enum XsXbusMessageId msgId) -{ - XsMessageHeader* hdr; - if (!thisPtr->m_message.m_data) - XsMessage_construct(thisPtr); - - 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; - if (thisPtr->m_autoUpdateChecksum) - thisPtr->m_checksum[0] -= hdr->m_messageId; -} - -/*! \brief Create \a count bytes of empty space at \a offset in this message - - \param count the number of bytes to reserve - \param offset the offset at which to allocate the space -*/ -void XsMessage_insertData(XsMessage* thisPtr, XsSize count, XsSize offset) -{ - XsSize index, oldSize, newSize; - XsByteArray old = XSBYTEARRAY_INITIALIZER; - XsMessageHeader* oldHdr, *newHdr; - uint8_t* oldData, *newData; - - if (!count) - return; - - oldSize = XsMessage_dataSize(thisPtr); - newSize = oldSize+count; - - if (newSize < offset+count) - newSize = offset+count; - - XsArray_swap(&thisPtr->m_message, &old); - XsMessage_constructSized(thisPtr, newSize); - - newHdr = (XsMessageHeader*) thisPtr->m_message.m_data; - oldHdr = (XsMessageHeader*) old.m_data; - if (!oldHdr) // our original message may have been empty / uninitialized - return; - - newHdr->m_busId = oldHdr->m_busId; - newHdr->m_preamble = oldHdr->m_preamble; - newHdr->m_messageId = oldHdr->m_messageId; - - if (oldSize >= XS_EXTLENCODE) - oldData = &oldHdr->m_datlen.m_extended.m_data[0]; - else - oldData = &oldHdr->m_datlen.m_data[0]; - - if (newSize >= XS_EXTLENCODE) - newData = &newHdr->m_datlen.m_extended.m_data[0]; - else - newData = &newHdr->m_datlen.m_data[0]; - - if (offset <= oldSize) - { - for (index = 0; index < offset; ++index) - newData[index] = oldData[index]; - } - else - { - for (index = 0; index < oldSize; ++index) - newData[index] = oldData[index]; - for (; index < offset; ++index) - newData[index] = 0; //lint !e661 - } - for (; index < oldSize; ++index) - newData[index+count] = oldData[index]; //lint !e661 !e662 - - if (thisPtr->m_autoUpdateChecksum) - thisPtr->m_checksum[0] -= byteSum(oldData, oldSize); - - XsArray_destruct(&old); -} - -/*! \brief Remove \a count bytes of data from the message at \a offset - - \param count the number of bytes to remove - \param offset the offset at which to remove the bytes -*/ -void XsMessage_deleteData(XsMessage* thisPtr, XsSize count, XsSize offset) -{ - XsSize index, oldSize, newSize; - XsByteArray old = XSBYTEARRAY_INITIALIZER; - XsMessageHeader* oldHdr, *newHdr; - uint8_t* oldData, *newData; - - oldSize = XsMessage_dataSize(thisPtr); - if (!count || offset >= oldSize) - return; - - if (offset + count >= oldSize) - { - XsMessage_resizeData(thisPtr, offset); - return; - } - newSize = oldSize-count; - - XsArray_swap(&thisPtr->m_message, &old); - XsMessage_constructSized(thisPtr, newSize); - - newHdr = (XsMessageHeader*) thisPtr->m_message.m_data; - oldHdr = (XsMessageHeader*) old.m_data; - if (!oldHdr) // our original message may have been empty / uninitialized - return; - - newHdr->m_busId = oldHdr->m_busId; - newHdr->m_preamble = oldHdr->m_preamble; - newHdr->m_messageId = oldHdr->m_messageId; - - if (oldSize >= XS_EXTLENCODE) - oldData = oldHdr->m_datlen.m_extended.m_data; - else - oldData = oldHdr->m_datlen.m_data; - - if (newSize >= XS_EXTLENCODE) - newData = newHdr->m_datlen.m_extended.m_data; - else - newData = newHdr->m_datlen.m_data; - - for (index = 0; index < offset; ++index) - newData[index] = oldData[index]; - for (; index < newSize; ++index) - newData[index] = oldData[index+count]; //lint !e661 - - if (thisPtr->m_autoUpdateChecksum) - thisPtr->m_checksum[0] -= byteSum(newData, newSize); - - XsArray_destruct(&old); -} - -/*! \brief Swap the contents of \a a and \a b - - \details This function swaps the internal buffers so no actual data is moved around. - A result is that it won't work for unmanaged data such as fixed size vectors - - \param a the object to receive \a b's contents - \param b the object to receive \a a's contents -*/ -void XsMessage_swap(XsMessage* a, XsMessage* b) -{ - XsMessage tmp; - - *((uint8_t**)&tmp.m_checksum) = a->m_checksum; - tmp.m_autoUpdateChecksum = a->m_autoUpdateChecksum; - - *((uint8_t**)&a->m_checksum) = b->m_checksum; - a->m_autoUpdateChecksum = b->m_autoUpdateChecksum; - - *((uint8_t**)&b->m_checksum) = tmp.m_checksum; - b->m_autoUpdateChecksum = tmp.m_autoUpdateChecksum; - - XsArray_swap(&a->m_message, &b->m_message); -} - -/*! \brief Compare the contents of the messages \a a and \a b, returning non-0 if they are different -*/ -int XsMessage_compare(XsMessage const* a, XsMessage const* b) -{ - return XsArray_compare(&a->m_message, &b->m_message); -} - -/*! \brief Return a string containing the first \a maxBytes bytes of the message in hex format - \param maxBytes the maximum number of bytes to include in the string, when set to 0, the full message will be used - \param resultValue the resulting string -*/ -void XsMessage_toHexString(XsMessage const* thisPtr, XsSize maxBytes, XsString* resultValue) -{ - char* s; - XsSize i = XsMessage_getTotalMessageSize(thisPtr); - if (maxBytes == 0 || maxBytes > i) - maxBytes = i; - - XsString_resize(resultValue, maxBytes*3); - s = (char*) resultValue->m_data; - for (i = 0; i < maxBytes; ++i) - sprintf(s+(i*3), "%02X ", (int) ((uint8_t const*) thisPtr->m_message.m_data)[i]); -} - -/*! @} */ - - -//lint +e64 +e18 ignore exact type mismatches here diff --git a/extern/xstypes/xsmessagearray.c b/extern/xstypes/xsmessagearray.c deleted file mode 100644 index b6f4ee2..0000000 --- a/extern/xstypes/xsmessagearray.c +++ /dev/null @@ -1,42 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#include "xsmessagearray.h" -#include "xsmessage.h" - -/*! \struct XsMessageArray - \brief A list of XsMessage values - \sa XsArray -*/ - -//! \brief Descriptor for XsMessageArray -XsArrayDescriptor const g_xsMessageArrayDescriptor = { - //lint --e{64} ignore exact type mismatches here - sizeof(XsMessage), - XSEXPCASTITEMSWAP XsMessage_swap, - XSEXPCASTITEMMAKE XsMessage_construct, - XSEXPCASTITEMCOPY XsMessage_copyConstruct, - XSEXPCASTITEMMAKE XsMessage_destruct, - XSEXPCASTITEMCOPY XsMessage_copy, - XSEXPCASTITEMCOMP XsMessage_compare, - 0 -}; - -/*! \copydoc XsArray_construct - \note Specialization for XsStringArray -*/ -void XsMessageArray_construct(XsMessageArray* thisPtr, XsSize count, XsMessage const* src) -{ - XsArray_construct(thisPtr, &g_xsMessageArrayDescriptor, count, src); -} diff --git a/extern/xstypes/xsoutputconfiguration.c b/extern/xstypes/xsoutputconfiguration.c deleted file mode 100644 index 3ece2e2..0000000 --- a/extern/xstypes/xsoutputconfiguration.c +++ /dev/null @@ -1,30 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#include "xsoutputconfiguration.h" - -void XsOutputConfiguration_swap(struct XsOutputConfiguration* a, struct XsOutputConfiguration* b) -{ - { - XsDataIdentifier t = a->m_dataIdentifier; - a->m_dataIdentifier = b->m_dataIdentifier; - b->m_dataIdentifier = t; - } - - { - uint16_t t = a->m_frequency; - a->m_frequency = b->m_frequency; - b->m_frequency = t; - } -} diff --git a/extern/xstypes/xsoutputconfigurationarray.c b/extern/xstypes/xsoutputconfigurationarray.c deleted file mode 100644 index 7b5117a..0000000 --- a/extern/xstypes/xsoutputconfigurationarray.c +++ /dev/null @@ -1,73 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#include "xsoutputconfigurationarray.h" -#include "xsoutputconfiguration.h" - -/*! \struct XsOutputConfigurationArray - \brief A list of XsOutputConfiguration values - \sa XsArray -*/ - -/*! \copydoc XsArrayDescriptor::itemSwap - \note Specialization for XsOutputConfiguration*/ -void swapXsOutputConfiguration(XsOutputConfiguration* a, XsOutputConfiguration* b) -{ - XsOutputConfiguration tmp = *a; - *a = *b; - *b = tmp; -} - -/*! \copydoc XsArrayDescriptor::itemCopy - \note Specialization for XsOutputConfiguration*/ -void copyXsOutputConfiguration(XsOutputConfiguration* to, XsOutputConfiguration const* from) -{ - *to = *from; -} - -/*! \copydoc XsArrayDescriptor::itemCompare - \note Specialization for XsOutputConfiguration*/ -int compareXsOutputConfiguration(XsOutputConfiguration const* a, XsOutputConfiguration const* b) -{ - if (a->m_dataIdentifier != b->m_dataIdentifier || a->m_frequency != b->m_frequency) - { - if (a->m_dataIdentifier == b->m_dataIdentifier) - return (a->m_frequency < b->m_frequency) ? -1 : 1; - return (a->m_dataIdentifier < b->m_dataIdentifier) ? -1 : 1; - } - - return 0; -} - - -//! \brief Descriptor for XsOutputConfigurationArray -XsArrayDescriptor const g_xsOutputConfigurationArrayDescriptor = { - //lint --e{64} ignore exact type mismatches here - sizeof(XsOutputConfiguration), - XSEXPCASTITEMSWAP swapXsOutputConfiguration, // swap - 0, // construct - XSEXPCASTITEMCOPY copyXsOutputConfiguration, // copy construct - 0, // destruct - XSEXPCASTITEMCOPY copyXsOutputConfiguration, // copy - XSEXPCASTITEMCOMP compareXsOutputConfiguration, // compare - XSEXPCASTRAWCOPY XsArray_rawCopy // raw copy -}; - -/*! \copydoc XsArray_construct - \note Specialization for XsOutputConfigurationArray -*/ -void XsOutputConfigurationArray_construct(XsOutputConfigurationArray* thisPtr, XsSize count, XsOutputConfiguration const* src) -{ - XsArray_construct(thisPtr, &g_xsOutputConfigurationArrayDescriptor, count, src); -} diff --git a/extern/xstypes/xsportinfo.c b/extern/xstypes/xsportinfo.c deleted file mode 100644 index 8c36960..0000000 --- a/extern/xstypes/xsportinfo.c +++ /dev/null @@ -1,154 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#include "xsportinfo.h" -#include -#include // strlen -#include // atoi - -/*! \class XsPortInfo - \brief Contains a descriptor for opening a communication port to an Xsens device. -*/ -/*! \addtogroup cinterface C Interface - @{ -*/ - -/*! \relates XsPortInfo - \brief Initializes the object to the empty state -*/ -void XsPortInfo_clear(XsPortInfo* thisPtr) -{ - thisPtr->m_baudrate = XBR_Invalid; - thisPtr->m_deviceId.m_deviceId = 0; - thisPtr->m_portName[0] = '\0'; - thisPtr->m_linesOptions = XPLO_All_Ignore; -} - -/*! \relates XsPortInfo - \brief Returns true if the XsPortInfo object is empty -*/ -int XsPortInfo_empty(const struct XsPortInfo* thisPtr) -{ - return (thisPtr->m_portName[0] == '\0'); -} - -/*! \relates XsPortInfo - \brief The port number - \returns Returns the port number - \note Available on Windows only -*/ -int XsPortInfo_portNumber(const struct XsPortInfo* thisPtr) -{ - size_t i; - - if (XsPortInfo_empty(thisPtr)) - return 0; - - 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 - return strncmp("\\\\?\\usb", thisPtr->m_portName, 7) == 0; -#else - return strncmp("USB", thisPtr->m_portName, 3) == 0; // libusb devices start with USB -#endif -} - -/*! - * \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 - */ -const char* XsPortInfo_networkServiceName(const struct XsPortInfo* thisPtr) -{ - return &thisPtr->m_portName[4]; -} - -/*! \relates XsPortInfo - \brief The usb bus - \returns Returns the Usb bus number - \note Available on Linux only -*/ -int XsPortInfo_usbBus(const struct XsPortInfo* thisPtr) -{ -#ifndef XSENS_WINDOWS - if (XsPortInfo_isUsb(thisPtr)) - return atoi(&thisPtr->m_portName[3]); -#else - (void) thisPtr; -#endif - return 0; -} - -/*! \relates XsPortInfo - \brief The usb address - \returns Returns the usb address - \note Available on Linux only -*/ -int XsPortInfo_usbAddress(const struct XsPortInfo* thisPtr) -{ -#ifndef XSENS_WINDOWS - if (XsPortInfo_isUsb(thisPtr)) - return atoi(&thisPtr->m_portName[7]); -#else - (void) thisPtr; -#endif - return 0; -} - -/*! \brief Swap the contents of \a a with those of \a b -*/ -void XsPortInfo_swap(struct XsPortInfo* a, struct XsPortInfo* b) -{ - int i; - char c; - XsPortLinesOptions pLineOpts; - - XsBaudRate t = a->m_baudrate; - a->m_baudrate = b->m_baudrate; - b->m_baudrate = t; - - XsDeviceId_swap(&a->m_deviceId, &b->m_deviceId); - - for (i = 0; i < 256; ++i) - { - c = a->m_portName[i]; - a->m_portName[i] = b->m_portName[i]; - b->m_portName[i] = c; - } - - pLineOpts = a->m_linesOptions; - a->m_linesOptions = b->m_linesOptions; - b->m_linesOptions = pLineOpts; -} - -/*! @} */ diff --git a/extern/xstypes/xsportinfoarray.c b/extern/xstypes/xsportinfoarray.c deleted file mode 100644 index 61b1ee9..0000000 --- a/extern/xstypes/xsportinfoarray.c +++ /dev/null @@ -1,51 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#include "xsportinfoarray.h" - -/*! \struct XsPortInfoArray - \brief A list of XsPortInfo values - \sa XsArray -*/ - -void copyPortInfo(XsPortInfo* dest, XsPortInfo const* src) -{ - memcpy(dest, src, sizeof(XsPortInfo)); -} - -int comparePortInfo(XsPortInfo const* a, XsPortInfo const* b) -{ - return memcmp(a, b, sizeof(XsPortInfo)); -} - -//! \brief Descriptor for XsPortInfoArray -XsArrayDescriptor const g_xsPortInfoArrayDescriptor = { - //lint --e{64} ignore exact type mismatches here - sizeof(XsPortInfo), - XSEXPCASTITEMSWAP XsPortInfo_swap, // swap - XSEXPCASTITEMMAKE XsPortInfo_clear, // construct - XSEXPCASTITEMCOPY copyPortInfo, // copy construct - 0, // destruct - XSEXPCASTITEMCOPY copyPortInfo, // copy - XSEXPCASTITEMCOMP comparePortInfo, // compare - XSEXPCASTRAWCOPY XsArray_rawCopy // raw copy -}; - -/*! \copydoc XsArray_construct - \note Specialization for XsPortInfoArray -*/ -void XsPortInfoArray_construct(XsPortInfoArray* thisPtr, XsSize count, XsPortInfo const* src) -{ - XsArray_construct(thisPtr, &g_xsPortInfoArrayDescriptor, count, src); -} diff --git a/extern/xstypes/xsquaternion.c b/extern/xstypes/xsquaternion.c deleted file mode 100644 index 8cc6bc6..0000000 --- a/extern/xstypes/xsquaternion.c +++ /dev/null @@ -1,333 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#include "xsquaternion.h" -#include "xseuler.h" -#include "xsmatrix.h" -#include "xsvector.h" -#include - - -/*! \class XsQuaternion - \brief A class that implements a quaternion -*/ - -/*! \addtogroup cinterface C Interface - @{ -*/ - -/*! \relates XsQuaternion - \brief Sets the contents to 0, which is an invalid XsQuaternion - \details The normal destructor does not call this function, it is intended for explicit signalling of errors -*/ -void XsQuaternion_destruct(XsQuaternion* thisPtr) -{ - thisPtr->m_w = XsMath_zero; - thisPtr->m_x = XsMath_zero; - thisPtr->m_y = XsMath_zero; - thisPtr->m_z = XsMath_zero; -} - -/*! \relates XsQuaternion \brief Test if this is a null object. */ -int XsQuaternion_empty(const XsQuaternion* thisPtr) -{ - return thisPtr->m_w == XsMath_zero && thisPtr->m_x == XsMath_zero && thisPtr->m_y == XsMath_zero && thisPtr->m_z == XsMath_zero; -} - -/*! \relates XsQuaternion - \brief Invert this quaternion - \details Where \a q = a + bi + cj + dk, this function will replace it with a - bi - cj - dk. -*/ -void XsQuaternion_invert(XsQuaternion* thisPtr) -{ - XsQuaternion_inverse(thisPtr, thisPtr); -} - -/*! \relates XsQuaternion - \brief Compute the inverse/conjugate of this quaternion - \details Where \a q = a + bi + cj + dk, this function will return a - bi - cj - dk in \a dest. - \param dest The object to write to -*/ -void XsQuaternion_inverse(const XsQuaternion* thisPtr, XsQuaternion* dest) -{ - dest->m_w = thisPtr->m_w; - dest->m_x = -thisPtr->m_x; - dest->m_y = -thisPtr->m_y; - dest->m_z = -thisPtr->m_z; -} - -/*! \relates XsQuaternion \brief Create a normalized version of this quaternion -*/ -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); -#ifndef XSENS_MATH_MATLAB_COMPLIANT - divisor = XsMath_one/length; - if (thisPtr->m_w < 0) - divisor = -divisor; - dest->m_w = thisPtr->m_w * divisor; - dest->m_x = thisPtr->m_x * divisor; - dest->m_y = thisPtr->m_y * divisor; - dest->m_z = thisPtr->m_z * divisor; -#else - if (thisPtr->m_w < 0) - divisor = -length; - else - divisor = length; - dest->m_w = thisPtr->m_w / divisor; - dest->m_x = thisPtr->m_x / divisor; - dest->m_y = thisPtr->m_y / divisor; - dest->m_z = thisPtr->m_z / divisor; -#endif - return length; -} - -/*! \relates XsQuaternion \brief Normalize this quaternion -*/ -XsReal XsQuaternion_normalize(XsQuaternion* thisPtr) -{ - return XsQuaternion_normalized(thisPtr, thisPtr); -} - -/*! \relates XsQuaternion \brief Create a quaternion representation from \a euler angles */ -void XsQuaternion_fromEulerAngles(XsQuaternion* thisPtr, const XsEuler* src) -{ - XsReal cosX, sinX, cosY, sinY, cosZ, sinZ; - - if (XsEuler_empty(src)) - { - *thisPtr = *XsQuaternion_identity(); - return; - } - - cosX = cos(XsMath_pt5 * XsMath_deg2rad(src->m_x)); - sinX = sin(XsMath_pt5 * XsMath_deg2rad(src->m_x)); - cosY = cos(XsMath_pt5 * XsMath_deg2rad(src->m_y)); - sinY = sin(XsMath_pt5 * XsMath_deg2rad(src->m_y)); - cosZ = cos(XsMath_pt5 * XsMath_deg2rad(src->m_z)); - sinZ = sin(XsMath_pt5 * XsMath_deg2rad(src->m_z)); - - thisPtr->m_w = cosX * cosY * cosZ + sinX * sinY * sinZ; - thisPtr->m_x = sinX * cosY * cosZ - cosX * sinY * sinZ; - thisPtr->m_y = cosX * sinY * cosZ + sinX * cosY * sinZ; - thisPtr->m_z = cosX * cosY * sinZ - sinX * sinY * cosZ; -} - -/*! \relates XsQuaternion - \brief Create a quaternion representation of orientation matrix \a ori - \details The matrix \a ori is interpreted as an orthonormal orientation matrix, which is - translated into a quaternion representation. If \a ori is not a 3x3 matrix, a null-quaternion is - returned. - \param ori The source orientation matrix -*/ -void XsQuaternion_fromRotationMatrix(XsQuaternion* thisPtr, const XsMatrix* ori) -{ - XsReal trace; // Trace of matrix - XsReal s; - - if (!XsMatrix_dimensionsMatch(ori, 3,3)) - { - XsQuaternion_destruct(thisPtr); - return; - } - -// XsQuaternion result; - - // Calculate trace of matrix - // T = 4 - 4x^2 - 4y^2 - 4z^2 - // = 4 ( 1 - x^2 - y^2 - z^2) - // = 1 + fRgs[0][0] + fRgs[1][1] + fRgs[2][2] (= 4q0^2) - - trace = XsMatrix_value(ori, 0, 0) + XsMatrix_value(ori, 1, 1) + XsMatrix_value(ori, 2, 2) + XsMath_one; - - // If the trace of the matrix is greater than zero, - // then perform an "instant" calculation. - // Important note wrt. rounding errors: - // 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) - { - s = XsMath_two * sqrt(trace); - thisPtr->m_w = XsMath_pt25 * s; - - #ifndef XSENS_MATH_MATLAB_COMPLIANT - 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; - #else - 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; - #endif - } - else if ((XsMatrix_value(ori, 0, 0) > XsMatrix_value(ori, 1, 1)) && (XsMatrix_value(ori, 0, 0) > XsMatrix_value(ori, 2, 2))) - { - trace = XsMath_one + XsMatrix_value(ori, 0, 0) - XsMatrix_value(ori, 1, 1) - XsMatrix_value(ori, 2, 2); - s = XsMath_two * sqrt(trace); - thisPtr->m_x = XsMath_pt25 * s; - - #ifndef XSENS_MATH_MATLAB_COMPLIANT - 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; - #else - 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; - #endif - } - else if (XsMatrix_value(ori, 1, 1) > XsMatrix_value(ori, 2, 2)) - { - trace = XsMath_one + XsMatrix_value(ori, 1, 1) - XsMatrix_value(ori, 0, 0) - XsMatrix_value(ori, 2, 2); - s = XsMath_two * sqrt(trace); - thisPtr->m_y = XsMath_pt25 * s; - - #ifndef XSENS_MATH_MATLAB_COMPLIANT - 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; - #else - 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; - #endif - } - else - { - trace = XsMath_one + XsMatrix_value(ori, 2, 2) - XsMatrix_value(ori, 0, 0) - XsMatrix_value(ori, 1, 1); - s = XsMath_two * sqrt(trace); - thisPtr->m_z = XsMath_pt25 * s; - - #ifndef XSENS_MATH_MATLAB_COMPLIANT - 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; - #else - 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; - #endif - } - - XsQuaternion_inverse(thisPtr, thisPtr); -} - -/*! \relates XsQuaternion \brief Returns an XsQuaternion that represents the identity quaternion */ -const XsQuaternion* XsQuaternion_identity(void) -{ - static const XsQuaternion sIdentity = { { { 1, 0, 0, 0 } } }; - return &sIdentity; -} - -/*! \relates XsQuaternion \brief Multiply \a left quaternion with \a right quaternion and put the result in \a dest. The parameters may point to the same XsQuaternion(s). */ -void XsQuaternion_multiply(const XsQuaternion* left, const XsQuaternion* right, XsQuaternion* dest) -{ - XsReal qa0 = left->m_w; - XsReal qa1 = left->m_x; - XsReal qa2 = left->m_y; - XsReal qa3 = left->m_z; - - XsReal qb0 = right->m_w; - XsReal qb1 = right->m_x; - XsReal qb2 = right->m_y; - XsReal qb3 = right->m_z; - - dest->m_w = qa0 * qb0 - qa1 * qb1 - qa2 * qb2 - qa3 * qb3; - dest->m_x = qa1 * qb0 + qa0 * qb1 - qa3 * qb2 + qa2 * qb3; - dest->m_y = qa2 * qb0 + qa3 * qb1 + qa0 * qb2 - qa1 * qb3; - dest->m_z = qa3 * qb0 - qa2 * qb1 + qa1 * qb2 + qa0 * qb3; -} - -/*! \relates XsQuaternion \brief Swap the contents of \a a and \a b -*/ -void XsQuaternion_swap(XsQuaternion* a, XsQuaternion* b) -{ - XsReal t; - int i; - for (i = 0; i < 4; ++i) - { - t = a->m_data[i]; - a->m_data[i] = b->m_data[i]; - b->m_data[i] = t; - } -} - -/*! \relates XsQuaternion \brief Copies the contents of \a thisPtr into \a copy -*/ -void XsQuaternion_copy(XsQuaternion* copy, XsQuaternion const* src) -{ - copy->m_w = src->m_w; - copy->m_x = src->m_x; - copy->m_y = src->m_y; - copy->m_z = src->m_z; -} - -/*! \relates XsQuaternion \brief returns non-zero if \a a and \a b are numerically equal -*/ -int XsQuaternion_equal(XsQuaternion const* a, XsQuaternion const* b) -{ - return (a->m_w == b->m_w && - a->m_x == b->m_x && - a->m_y == b->m_y && - a->m_z == b->m_z); -} - -/*! \brief Checks whether \a a and \a b are equal with tolerance \a tolerance - \param[in] a double a - \param[in] b double b - \param[in] tolerance The tolerance - \returns a positive value when \a a and \a b are considered equal -*/ -static int fuzzyCompare(double a, double b, double tolerance) -{ - return fabs(a - b) <= tolerance; -} - -/*! \relates XsQuaternion - \brief Returns non-zero if the values at \a thisPtr and \a other are within \a tolerance of each other -*/ -int XsQuaternion_compare(XsQuaternion const* thisPtr, XsQuaternion const* other, XsReal tolerance) -{ - if (thisPtr == other) - return 1; - - if (fuzzyCompare(thisPtr->m_data[0], other->m_data[0], tolerance) && - fuzzyCompare(thisPtr->m_data[1], other->m_data[1], tolerance) && - fuzzyCompare(thisPtr->m_data[2], other->m_data[2], tolerance) && - fuzzyCompare(thisPtr->m_data[3], other->m_data[3], tolerance)) - return 1; - // add extra check for q == -q (negative-definite vs positive-definite comparison) - return (fuzzyCompare(thisPtr->m_data[0], -other->m_data[0], tolerance) && - fuzzyCompare(thisPtr->m_data[1], -other->m_data[1], tolerance) && - fuzzyCompare(thisPtr->m_data[2], -other->m_data[2], tolerance) && - fuzzyCompare(thisPtr->m_data[3], -other->m_data[3], tolerance)); -} - -/*! \relates XsQuaternion - \brief Returns the dot product of the \a thisPtr with \a 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); -} - -/*! @} */ diff --git a/extern/xstypes/xsrange.c b/extern/xstypes/xsrange.c deleted file mode 100644 index c5fc7d6..0000000 --- a/extern/xstypes/xsrange.c +++ /dev/null @@ -1,70 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#include "xsrange.h" - -/*! \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. -*/ - -/*! \addtogroup cinterface C Interface - @{ -*/ - -/*! \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 - \returns The number of values in the range (inclusive) -*/ -int XsRange_count(const XsRange* thisPtr) -{ - if (thisPtr->m_last < thisPtr->m_first) - return 0; - return 1 + thisPtr->m_last - thisPtr->m_first; -} - -/*! \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 - \returns The number of values in the range (exclusive) -*/ -int XsRange_interval(const XsRange* thisPtr) -{ - if (thisPtr->m_last <= thisPtr->m_first) - return 0; - return thisPtr->m_last - thisPtr->m_first; -} - -/*! \relates XsRange \brief Test if the range contains the given value \a i. */ -int XsRange_contains(const XsRange* thisPtr, int i) -{ - return (i >= thisPtr->m_first && i <= thisPtr->m_last); -} - -/*! \relates XsRange \brief Set a new range. */ -void XsRange_setRange(XsRange* thisPtr, int f, int l) -{ - thisPtr->m_first = f; - thisPtr->m_last = l; -} - -/*! \relates XsRange \brief Test if the range is empty. - \details An empty range has a last element that is lower than its first element. - \returns true if the range is empty, false otherwise -*/ -int XsRange_empty(const XsRange* thisPtr) -{ - return thisPtr->m_last < thisPtr->m_first; -} - -/*! @} */ diff --git a/extern/xstypes/xsresultvalue.c b/extern/xstypes/xsresultvalue.c deleted file mode 100644 index 4f65684..0000000 --- a/extern/xstypes/xsresultvalue.c +++ /dev/null @@ -1,134 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#include "xsresultvalue.h" - -/*! \addtogroup cinterface C Interface - @{ -*/ - -////////////////////////////////////////////////////////////////////////////////////////// -/*! \brief Retrieve a character string corresponding to the given result code. -*/ -const char* XsResultValue_toString(XsResultValue result) -{ - switch(result) - { - case XRV_OK: return "No error"; - 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_MEASUREMENTFAILED: return "Failed to start measurement"; - 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_CONFIGCHECKFAIL: return "Failure during device configuration"; - 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 Xbus Master LEDs"; - case XRV_ONLYONEXMFOUND: return "Found only one responding Xbus Master, check connections and Xbus Master 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_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_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_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_STARTRECORDINGFAILED: return "A device could not start recording"; - case XRV_SAMPLESTREAMERROR: return "Sample stream detected an error in ordering input data"; - case XRV_RADIO_CHANNEL_IN_USE: return "Detected another system using the selected radio channel"; - case XRV_WIRELESSFAIL: return "Wireless communication system failed"; - - case XRV_OVERHEATING: return "Device temperature exceded operational limits"; - case XRV_BATTERYLOW: return "Battery level reached lower limit"; - - 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_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_SHUTTINGDOWN: return "The device is shutting down"; - 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"; - - default: return "!!Undefined Result Value!!"; - } -} - -/*! @} */ diff --git a/extern/xstypes/xsrssi.c b/extern/xstypes/xsrssi.c deleted file mode 100644 index 6b1f902..0000000 --- a/extern/xstypes/xsrssi.c +++ /dev/null @@ -1,46 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#include "xsrssi.h" - -/*! \namespace XsRssi - \brief Contains Received Signal Strength Indication (RSSI) constants -*/ -/*! \addtogroup cinterface C Interface - @{ -*/ - -/*! \brief The maximum RSSI value. */ -XSTYPES_DLL_API int XsRssi_max(void) -{ - return XS_RSSI_MAX; -} - -/*! \brief The RSSI value that was reserved for when the RSSI is unknown. */ -XSTYPES_DLL_API int XsRssi_unknown(void) -{ - return XS_RSSI_UNKNOWN; -} - -/*! \brief Returns the raw RSSI value transformed into a usable (unbiased) number. - \details Actual rssi is calculated by \a raw + XS_RSSI_MAX, where \a raw is a negative number. - \param raw The RSSI value as reported by the device. - \returns The unbiased RSSI value. -*/ -XSTYPES_DLL_API int XsRssi_unbiased(int raw) -{ - return raw + XS_RSSI_MAX; -} - -/*! @} */ diff --git a/extern/xstypes/xssdidata.c b/extern/xstypes/xssdidata.c deleted file mode 100644 index f589268..0000000 --- a/extern/xstypes/xssdidata.c +++ /dev/null @@ -1,59 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#include "xssdidata.h" - -/*! \class XsSdiData - \brief Contains StrapDown Integration (SDI) data. - \details SDI data consists of a rotation and an acceleration, expressed as an orientation increment - (also known as deltaQ) and a velocity increment (also known as deltaV). -*/ - -/*! \addtogroup cinterface C Interface - @{ -*/ - -/*! \relates XsSdiData - \brief Initialize an %XsSdiData object with the optional arguments. - \param orientationIncrement The orientation increment to initialize the object with, may be 0 - \param velocityIncrement The velocity increment to initialize the object with, may be 0 -*/ -void XsSdiData_construct(XsSdiData* thisPtr, const XsReal* orientationIncrement, const XsReal* velocityIncrement) -{ - if (orientationIncrement) - { - thisPtr->m_orientationIncrement.m_data[0] = orientationIncrement[0]; - thisPtr->m_orientationIncrement.m_data[1] = orientationIncrement[1]; - thisPtr->m_orientationIncrement.m_data[2] = orientationIncrement[2]; - thisPtr->m_orientationIncrement.m_data[3] = orientationIncrement[3]; - } - else - XsQuaternion_destruct(&thisPtr->m_orientationIncrement); - - if (velocityIncrement) - XsVector3_assign(&thisPtr->m_velocityIncrement, velocityIncrement); - else - XsVector3_destruct(&thisPtr->m_velocityIncrement); -} - -/*! \relates XsSdiData - \brief Destruct the object, makes the fields invalid -*/ -void XsSdiData_destruct(XsSdiData* thisPtr) -{ - XsQuaternion_destruct(&thisPtr->m_orientationIncrement); - XsVector3_destruct(&thisPtr->m_velocityIncrement); -} - -/*! @} */ diff --git a/extern/xstypes/xssimpleversion.c b/extern/xstypes/xssimpleversion.c deleted file mode 100644 index eed04c5..0000000 --- a/extern/xstypes/xssimpleversion.c +++ /dev/null @@ -1,59 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - - -#include "xssimpleversion.h" - -/*! \class XsSimpleVersion - \brief A class to store version information -*/ - -/*! \addtogroup cinterface C Interface - @{ -*/ - -/*! \relates XsSimpleVersion \brief Test if this is a null-version. */ -int XsSimpleVersion_empty(const XsSimpleVersion* thisPtr) -{ -#ifdef __cplusplus - return thisPtr->major() == 0 && thisPtr->minor() == 0 && thisPtr->revision() == 0; -#else - return thisPtr->m_major == 0 && thisPtr->m_minor == 0 && thisPtr->m_revision == 0; -#endif -} - -/*! \brief Swap the contents of \a a with those of \a b -*/ -void XsSimpleVersion_swap(struct XsSimpleVersion* a, struct XsSimpleVersion* b) -{ - XsSimpleVersion tmp = *a; - *a = *b; - *b = tmp; -} - -/*! \brief Compare two XsSimpleVersion objects. - \param a The left hand side of the comparison - \param b The right hand side of the comparison - \return 0 when they're equal -*/ -int XsSimpleVersion_compare(XsSimpleVersion const* a, XsSimpleVersion const* b) -{ -#ifdef __cplusplus - return a->major() != b->major() || a->minor() != b->minor() || a->revision() != b->revision(); -#else - return a->m_major != b->m_major || a->m_minor != b->m_minor || a->m_revision != b->m_revision; -#endif -} - -/*! @} */ diff --git a/extern/xstypes/xssnapshot.c b/extern/xstypes/xssnapshot.c deleted file mode 100644 index d0b48a0..0000000 --- a/extern/xstypes/xssnapshot.c +++ /dev/null @@ -1,41 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#include "xssnapshot.h" - -/*! \struct XsAwindaSnapshot - \brief Container for Awinda Snapshot data. -*/ - -/*! \addtogroup cinterface C Interface - @{ -*/ - -/*! \relates XsAwindaSnapshot - \brief Construct an %XsAwindaSnapshot object -*/ -void XsSnapshot_construct(XsSnapshot* thisPtr) -{ - memset(thisPtr, 0, sizeof(XsSnapshot)); -} - -/*! \relates XsAwindaSnapshot - \brief Destruct an %XsAwindaSnapshot object -*/ -void XsSnapshot_destruct(XsSnapshot* thisPtr) -{ - (void) thisPtr; -} - -/*! @} */ diff --git a/extern/xstypes/xssocket.c b/extern/xstypes/xssocket.c deleted file mode 100644 index fc2802e..0000000 --- a/extern/xstypes/xssocket.c +++ /dev/null @@ -1,1031 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#ifdef _WIN32 -#include -#include -#include -#include - -#pragma comment(lib, "ws2_32.lib") -#else -#include -#include -#include -#include -#include -#include -#include -#include -#include -#define SOCKET int32_t -#define SOCKET_ERROR -1 -#define INVALID_SOCKET ((int32_t)-1) - -/* capture the fact that windows has a specific closesocket function */ -static int closesocket(SOCKET s) -{ - close(s); - return 0; -} -#endif - -#include - -// MSG_NOSIGNAL is linux stuff -#ifndef MSG_NOSIGNAL -#define MSG_NOSIGNAL 0 -#endif - -#include "xssocket.h" - -/* the socket data */ -struct XsSocketPrivate -{ - SOCKET m_sd; - - enum NetworkLayerProtocol m_ipVersion; - enum IpProtocol m_ipProtocol; - - struct sockaddr_storage m_remoteAddr; - socklen_t m_remoteAddrLen; - - XsResultValue m_lastResult; - int m_lastSystemError; - - XsDataFlags m_flags; - -#ifdef _WIN32 - WSADATA m_sockData; -#endif -}; - -/* Return non-zero if the native socket s is usable */ -static int socketIsUsable(SOCKET s) -{ - return (s != INVALID_SOCKET && s > 0); -} - - -/*! - * \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; - socklen_t length; - - switch (remote->ss_family) - { - case AF_INET: - default: - src = &(((struct sockaddr_in*)remote)->sin_addr); - length = INET_ADDRSTRLEN; - break; - case AF_INET6: - src = &(((struct sockaddr_in6*)remote)->sin6_addr); - length = INET6_ADDRSTRLEN; - break; - } - XsString_resize(address, length); - if (inet_ntop(remote->ss_family, src, address->m_data, length) == NULL) - XsString_erase(address, 0, address->m_size); -} - -/* Update the last result of thisPtr to retval, if thisPtr is non-null - - Returns retval for easy use in return statements. -*/ -static XsResultValue setLastResult(XsSocket* thisPtr, XsResultValue retval, int systemError) -{ - if (thisPtr && thisPtr->d) - { - thisPtr->d->m_lastResult = retval; - thisPtr->d->m_lastSystemError = systemError; - } - return retval; -} - -/* Translate a platform error into an XsResultValue - - If thisPtr is non-null, the socket's last result will be updated. -*/ -static XsResultValue translateAndReturnSocketError(XsSocket* thisPtr, int functionResult) -{ - XsResultValue result; - int err; - if (functionResult == 0) - return setLastResult(thisPtr, XRV_OK, 0); -#ifdef _WIN32 - 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 WSA_IO_INCOMPLETE: - case WSA_IO_PENDING: - case WSAEINTR: - case WSAEBADF: - case WSAEACCES: - case WSAEFAULT: - case WSAEMFILE: - case WSAEWOULDBLOCK: - case WSAEINPROGRESS: - case WSAEALREADY: - case WSAENOTSOCK: - case WSAEDESTADDRREQ: - case WSAEMSGSIZE: - case WSAEPROTOTYPE: - case WSAENOPROTOOPT: - case WSAEPROTONOSUPPORT: - case WSAESOCKTNOSUPPORT: - case WSAEOPNOTSUPP: - case WSAEPFNOSUPPORT: - case WSAEAFNOSUPPORT: - case WSAEADDRINUSE: - case WSAEADDRNOTAVAIL: - case WSAENETDOWN: - case WSAENETUNREACH: - case WSAENETRESET: - case WSAECONNABORTED: - case WSAECONNRESET: - case WSAENOBUFS: - case WSAEISCONN: - case WSAENOTCONN: - case WSAESHUTDOWN: - case WSAETOOMANYREFS: - case WSAETIMEDOUT: - 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; - } -#endif - return setLastResult(thisPtr, result, err); -} - -static void translateSocketError(XsSocket* thisPtr, int functionResult) -{ - (void)translateAndReturnSocketError(thisPtr, functionResult); -} - -/* Initialize the socket - - This function performs some basic initialization on the socket -*/ -static void XsSocket_initialize(XsSocket* thisPtr, XsDataFlags flags) -{ - thisPtr->d = (struct XsSocketPrivate*)malloc(sizeof(struct XsSocketPrivate)); - memset(thisPtr->d, 0, sizeof(*thisPtr->d)); - thisPtr->d->m_sd = INVALID_SOCKET; - thisPtr->d->m_flags = flags; -#ifdef _WIN32 - (void)WSAStartup(MAKEWORD(2,0), &thisPtr->d->m_sockData); -#endif -} - -/*! \brief Create socket \a thisPtr for IP version \a ip with protocol \a protocol - - \param[in] ip the IP version - \param[in] protocol the protocol. Currently supported are TCP and UDP - \relates XsSocket -*/ -void XsSocket_create(XsSocket* thisPtr, enum NetworkLayerProtocol ip, enum IpProtocol protocol) -{ - XsSocket_initialize(thisPtr, XSDF_Managed); - - thisPtr->d->m_sd = socket( - (ip == NLP_IPV4) ? PF_INET : PF_INET6, - (protocol == IP_UDP) ? SOCK_DGRAM : SOCK_STREAM, - (protocol == IP_UDP) ? IPPROTO_UDP : IPPROTO_TCP); - - thisPtr->d->m_ipVersion = ip; - thisPtr->d->m_ipProtocol = protocol; -} - -/* 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 *theirInfo, socklen_t infolen, XsDataFlags flags) -{ - XsSocket_initialize(thisPtr, flags); - thisPtr->d->m_sd = nativeSocket; - - if (theirInfo) - { - memcpy(&thisPtr->d->m_remoteAddr, theirInfo, infolen); - thisPtr->d->m_remoteAddrLen = infolen; - } - else - { - int rv; - thisPtr->d->m_remoteAddrLen = sizeof(thisPtr->d->m_remoteAddr); - rv = getpeername(thisPtr->d->m_sd, (struct sockaddr*)&thisPtr->d->m_remoteAddr, &thisPtr->d->m_remoteAddrLen); - translateSocketError(thisPtr, rv); - } - - 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; - } -} - -/*! - * \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. - */ -XSOCKET XsSocket_nativeDescriptor(XsSocket const* thisPtr) -{ - return thisPtr->d->m_sd; -} - -/*! \brief Close a socket - \details This function closes the socket. The XsSocket object is reusable after this, but this is - not recommended. - \return XRV_OK if the socket was already closed or is now closed, XRV_ERROR if some error occurred while closing the socket - \relates XsSocket -*/ -XsResultValue XsSocket_close(XsSocket* thisPtr) -{ - if (thisPtr->d->m_sd == INVALID_SOCKET) - return XRV_OK; - - if (closesocket(thisPtr->d->m_sd) == SOCKET_ERROR) - return XRV_ERROR; - - thisPtr->d->m_sd = INVALID_SOCKET; - return XRV_OK; -} - -/*! \brief Destroy the given socket - - After calling this function, the XsSocket will no longer be usable for socket communications. - - It is safe to call this function twice on the same XsSocket. - - If the socket was created from a file descriptor using - XsSocket_createFromFileDescriptor and had the XSDF_Managed flag set then - the underlying socket will be closed. - - \relates XsSocket -*/ -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 - // decrease ref counter and possibly clean up socket dll - // http://msdn.microsoft.com/en-us/library/windows/desktop/ms741549%28v=vs.85%29.aspx - (void)WSACleanup(); -#endif - } -} - -/*! \brief Wait for read or write for mstimeout milliseconds on socket thisPtr - \param[in] mstimeout timeout in milliseconds, set to a negative value to do a blocking call - \param[in,out] canRead if not null will be set to non-zero if the socket has data to read - \param[in,out] canWrite if not null will be set to non-zero if the socket can be written to - \returns -1 on error, 0 when no data is available, a positive number otherwise - \relates XsSocket - - Use canRead and canWrite to determine whether you can read from or write to the socket. - They will be set to non-zero if you can write, zero if you can't. - 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) -{ - fd_set readfd; - fd_set writefd; - fd_set errorfd; - struct timeval timeout; - int rv; - FD_ZERO(&readfd); - FD_ZERO(&writefd); - FD_ZERO(&errorfd); - FD_SET(thisPtr->d->m_sd, &readfd); - FD_SET(thisPtr->d->m_sd, &writefd); - FD_SET(thisPtr->d->m_sd, &errorfd); - - if (canRead) - *canRead = 0; - if (canWrite) - *canWrite = 0; - - 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); - - 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; - break; - } - - if (canRead) - *canRead = FD_ISSET(thisPtr->d->m_sd, &readfd); - if (canWrite) - *canWrite = FD_ISSET(thisPtr->d->m_sd, &writefd); - break; - } - return rv; -} - -/*! \brief Read \a size data into \a dest - - \param[in,out] dest a pointer to a data buffer - \param[in] size the size of the buffer \a dest points to - \param[in] timeout the amount of time in milliseconds to wait for data - - \returns the size of the data read from the socket, -1 on error. - \relates XsSocket -*/ -int XsSocket_read(XsSocket* thisPtr, void* dest, XsSize size, int timeout) -{ - return XsSocket_readFrom(thisPtr, dest, (int)size, NULL, NULL, timeout); -} - -#ifndef PEEKBUFSIZE -#define PEEKBUFSIZE 2048 -#endif -/* peek at the size of the incoming data */ -static int peekPendingDataSize(XsSocket* thisPtr) -{ - char buf[PEEKBUFSIZE]; - int rv; - - rv = recvfrom(thisPtr->d->m_sd, buf, PEEKBUFSIZE, MSG_PEEK, NULL, NULL); - if (rv < 0) - translateSocketError(thisPtr, rv); - return rv; -} - -/*! \brief Read \a size data into \a dest - - \param[in,out] dest a pointer to a data buffer, may be NULL, in which case only the size of the pending data is reported. - \param[in] size the size of the buffer \a dest points to - \param[in,out] hostname a pointer to a string that contains the hostname of the sender after returning from this function. May be NULL. - \param[in,out] port a pointer that will contain the port of the sender after returning from this function. May be NULL. - \param[in] timeout the amount of time in milliseconds to wait for data - - \returns the size of the data read from the socket, -1 on error. - \relates XsSocket -*/ -int XsSocket_readFrom(XsSocket* thisPtr, void* dest, XsSize size, XsString* hostname, uint16_t* port, int timeout) -{ - int rv; - int canRead; - struct sockaddr_storage sender; - socklen_t l = sizeof(sender); - - rv = XsSocket_select(thisPtr, timeout, &canRead, NULL); - if (rv <= 0) - return rv; - - if (!dest) - return peekPendingDataSize(thisPtr); - - rv = recvfrom(thisPtr->d->m_sd, dest, (int)size, 0, (struct sockaddr *)&sender, &l); - - if (hostname) - { - getRemoteHostAddress(&sender, hostname); - } - - if (port) - *port = ntohs(((struct sockaddr_in*)&sender)->sin_port); - - translateSocketError(thisPtr, rv); - return rv; -} - -/*! \brief Write data to the socket - - \param[in] data a pointer to the data to write - \param[in] size the size of the data to write - - \returns the number of bytes written, -1 or 0 on error - \relates XsSocket -*/ -int XsSocket_write(XsSocket* thisPtr, const void* data, XsSize size) -{ - int canWrite; - int rv = XsSocket_select(thisPtr, 0, NULL, &canWrite); - if (rv <= 0) - return rv; - - return send(thisPtr->d->m_sd, data, (int)size, MSG_NOSIGNAL); -} - -/* Return non-zero if the hostname is actually an IPv4 address */ -int isIPv4Address(const XsString* hostname) -{ - char *c; - int expectNum = 1; - int expectDot = 0; - int numbersFound = 0; - - if (!hostname || !hostname->m_data) - return 0; - - for (c = hostname->m_data; *c != '\0'; c++) - { - if (*c == '.') - { - if (!expectDot) - return 0; - expectNum = 1; - numbersFound = 0; - expectDot = 0; - } - else if (isdigit(*c)) - { - if (!expectNum) - return 0; - expectDot = 1; - ++numbersFound; - if (numbersFound == 3) - expectNum = 0; - } - else - { - return 0; - } - } - return 1; -} - -/* Prefix the hostname with ::ffff: if we're on ipv6 and hostname looks like a ipv4 address */ -void XsSocket_fixupHostname(XsSocket* thisPtr, XsString* hostname) -{ - if (!hostname || !hostname->m_data) - return; - - if (thisPtr->d->m_ipVersion == NLP_IPV4) - return; - - if (isIPv4Address(hostname)) - { - const char prefix[] = "::ffff:"; - XsArray_insert(hostname, 0, strlen(prefix), prefix); - } -} - -typedef int (*lookupTestFunction)(XsSocket* thisPtr, SOCKET currentSocket, struct addrinfo* info); - -/* Do a lookup of the given hostname and port - - This is an internal function that centralizes the lookup code. - - thisPtr - the socket to use information from - hostname - the hostname to look up - port - the port to check the connection on - hints_flags - the flags we should put into the lookup hints, e.g. AI_PASSIVE - tester - a function that performs some sanity checks on the passed info - info - a pointer to a sockaddr buffer. Should be at least as big as sockaddr, preferably be sockaddr_storage - addrlen - the size of info. This function will change addrlen based on the actual size of info if necessary - - 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) -{ - struct addrinfo *lookupInfo, *p; - SOCKET s; - char gaport[7]; - struct addrinfo hints; - int ret; - memset(&hints, 0, sizeof(hints)); - - 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; - } - hints.ai_socktype = (thisPtr->d->m_ipProtocol == IP_UDP) ? SOCK_DGRAM : SOCK_STREAM; - hints.ai_flags = hints_flags; - - sprintf(gaport, "%u", port); - if (hostname) - { - XsString host; - - XsString_construct(&host); - XsArray_copy(&host, hostname); - - XsSocket_fixupHostname(thisPtr, &host); - - ret = getaddrinfo(host.m_data, gaport, &hints, &lookupInfo); - - 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); - } - } - - for (p = lookupInfo; p != NULL; p = p->ai_next) - { - s = socket(p->ai_family, p->ai_socktype, p->ai_protocol); - if (!socketIsUsable(s)) - continue; - - ret = tester(thisPtr, s, p); - if (ret == 0) - { - (void)closesocket(s); - if (info && addrlen) - { - if ((socklen_t)p->ai_addrlen < *addrlen) - *addrlen = (socklen_t)p->ai_addrlen; - memcpy(info, p->ai_addr, *addrlen); - } - break; - } - (void)closesocket(s); - } - freeaddrinfo(lookupInfo); - return setLastResult(thisPtr, (ret == 0) ? XRV_OK : XRV_NOTFOUND, ret); -} - -/* try and see if we can connect on currentSocket to the remote host */ -static int defaultLookupTest(XsSocket* thisPtr, SOCKET currentSocket, struct addrinfo* info) -{ - (void)thisPtr; - return connect(currentSocket, info->ai_addr, (int)info->ai_addrlen); -} - -/*! \brief Perform a lookup */ -static XsResultValue XsSocket_lookup(XsSocket* thisPtr, const XsString* hostname, uint16_t port, struct sockaddr* info, socklen_t* addrlen) -{ - return XsSocket_internalLookup(thisPtr, hostname, port, 0, defaultLookupTest, info, addrlen); -} - -/*! \brief Write data to the socket to the host indicated by \a hostname, \a port - - \param[in] data the data to write - \param[in] size the size of the data - \param[in] hostname the name of the host to send data to - \param[in] port the port to send data to - - \returns the number of bytes written, -1 on error. - \relates XsSocket -*/ -int XsSocket_writeTo(XsSocket* thisPtr, const void* data, XsSize size, const XsString* hostname, uint16_t port) -{ - struct sockaddr_storage storage; - struct sockaddr* addr = NULL; - socklen_t addrlen = 0; - int sent; - int canWrite; - int rv; - - rv = XsSocket_select(thisPtr, 0, NULL, &canWrite); - if (rv <= 0) - return rv; - - if (thisPtr->d->m_ipProtocol == IP_UDP) - { - addr = (struct sockaddr*)&storage; - addrlen = sizeof(storage); - - if (XsSocket_lookup(thisPtr, hostname, port, addr, &addrlen) != XRV_OK) - return -1; - } - - sent = sendto(thisPtr->d->m_sd, data, (int)size, MSG_NOSIGNAL, addr, addrlen); - translateSocketError(thisPtr, sent); - return sent; -} - -/*! \brief Flush the incoming data - - Flush all data from the incoming buffer. - - \relates XsSocket -*/ -void XsSocket_flush(XsSocket* thisPtr) -{ - char buf[512]; - - while (XsSocket_read(thisPtr, buf, sizeof(buf), 0) > 0) - { - /* nop */ - } -} - -/*! \brief Accept an incoming connection - - This function requires that XsSocket_listen() has already been called. - - The returned pointer should be freed using XsSocket_freeAcceptedSocket(). - - \param[in] mstimeout the timeout in milliseconds - - \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; - struct sockaddr_storage theirInfo; - socklen_t infoLength = sizeof(struct sockaddr_storage); - SOCKET sd; - - if (mstimeout >= 0) - { - int read; - int rv = XsSocket_select(thisPtr, mstimeout, &read, NULL); - if (rv == 0) - { - (void)setLastResult(thisPtr, XRV_TIMEOUTNODATA, 0); - return NULL; - } - else if (rv == -1) - { - translateSocketError(thisPtr, rv); - return NULL; - } - } - - sd = accept(thisPtr->d->m_sd, (struct sockaddr *)&theirInfo, &infoLength); - if (!socketIsUsable(sd)) - return NULL; - - ns = XsSocket_allocate(); - XsSocket_createFromNativeSocket(ns, sd, (struct sockaddr*)&theirInfo, infoLength, XSDF_Managed); - return ns; -} - -/*! \brief Dynamically allocate an XsSocket - - After calling this function, it is still required to call - XsSocket_create or the internal function XsSocket_createFromNativeSocket - - Use XsSocket_free to safely free the returned XsSocket. - - \returns a pointer to a newly allocated XsSocket, or NULL on error -*/ -XsSocket* XsSocket_allocate() -{ - return (XsSocket*)malloc(sizeof(XsSocket)); -} - -/*! \brief Free a socket returned from XsSocket_accept() or XsSocket_allocate() - - Use this function only on sockets returned from XsSocket_accept(). - - This function calls XsSocket_destroy() before freeing the memory. - - \see XsSocket_destroy - \relates XsSocket -*/ -void XsSocket_free(XsSocket* thisPtr) -{ - XsSocket_destroy(thisPtr); - free(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) -{ - int res; - int nativeOption; -#ifdef _WIN32 - const char *valPtr = (const char*)valuePtr; -#else - const void *valPtr = valuePtr; -#endif - - switch(option) - { - case XSO_ReuseAddress: - nativeOption = SO_REUSEADDR; - break; - -#if !defined(_WIN32) && !defined(__ANDROID__) - case XSO_ReusePort: - nativeOption = SO_REUSEPORT; - break; -#endif - - default: - return XRV_OK; - } - - res = setsockopt(thisPtr->d->m_sd, SOL_SOCKET, nativeOption, valPtr, valueSize); - return translateAndReturnSocketError(thisPtr, res); -} - - -/* test if we can bind to info - - keeps the bind alive after leaving the function -*/ -static int binder(XsSocket* thisPtr, SOCKET currentSocket, struct addrinfo* info) -{ - int res; - int yesval = 1; -#ifdef _WIN32 - const char *yes = (const char*)&yesval; -#else - const void *yes = &yesval; -#endif - - (void)currentSocket; - - res = setsockopt(thisPtr->d->m_sd, SOL_SOCKET, SO_REUSEADDR, yes, sizeof(yesval)); - if (res) - return res; - - return bind(thisPtr->d->m_sd, info->ai_addr, (int)info->ai_addrlen); -} - -/*! \brief Bind to the \a hostname and \a port combination - - \param[in] hostname the hostname to bind to, may be NULL, in which we will bind to any address - \param[in] port the port to bind to. Must be a value above 1023, unless we have elevated privileges - - \return an XsResultValue indicating the result of the operation, possibly pointing towards a cause - \relates XsSocket -*/ -XsResultValue XsSocket_bind(XsSocket* thisPtr, const XsString* hostname, uint16_t port) -{ - int rv; - struct sockaddr_storage s; - socklen_t addrlen; - - if (hostname) - return XsSocket_internalLookup(thisPtr, hostname, port, AI_PASSIVE, binder, NULL, NULL); - - memset(&s, 0, sizeof(s)); - - if (thisPtr->d->m_ipVersion == NLP_IPV4) - { - 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; - addrlen = sizeof(*sin); - } - else - { - struct sockaddr_in6 *sin6 = (struct sockaddr_in6*)&s; - sin6->sin6_family = AF_INET6; - sin6->sin6_port = htons(port); - sin6->sin6_addr = in6addr_any; - addrlen = sizeof(*sin6); - } - - rv = bind(thisPtr->d->m_sd, (struct sockaddr*)&s, addrlen); - return translateAndReturnSocketError(thisPtr, rv); -} - -/*! \brief Start listening for incoming connections on this socket - - \param[in] maxPending the maximum amount of pending connections - - \return an XsResultValue indicating the result of the operation - \relates XsSocket -*/ -XsResultValue XsSocket_listen(XsSocket* thisPtr, int maxPending) -{ - int r = listen(thisPtr->d->m_sd, maxPending); - return translateAndReturnSocketError(thisPtr, r); -} - -/* Connect to info - - Keep the connection alive -*/ -static int connector(XsSocket* thisPtr, SOCKET currentSocket, struct addrinfo* info) -{ - int ret; - (void)currentSocket; - - ret = connect(thisPtr->d->m_sd, info->ai_addr, (int)info->ai_addrlen); - - if (ret == 0) - { - thisPtr->d->m_ipVersion = (info->ai_family == AF_INET ? NLP_IPV4 : NLP_IPV6); - memcpy(&thisPtr->d->m_remoteAddr, info->ai_addr, info->ai_addrlen); - thisPtr->d->m_remoteAddrLen = (socklen_t)(info->ai_addrlen); - } - else - translateSocketError(thisPtr, ret); - - return ret; -} - -/*! \brief Connect to \a hostame on \a port - - \param[in] host the hostname to connect to - \param[in] port the port to connect to - - \returns an XsResultValue indicating the result of the operation - \relates XsSocket -*/ -XsResultValue XsSocket_connect(XsSocket* thisPtr, const XsString* host, uint16_t port) -{ - return XsSocket_internalLookup(thisPtr, host, port, 0, connector, NULL, NULL); -} - -/*! \brief Return non-zero if the socket is usable for reading and writing data - - \returns non-zero if the socket is usable for reading and writing data, zero otherwise - \relates XsSocket -*/ -int XsSocket_isUsable(const XsSocket* thisPtr) -{ - return socketIsUsable(thisPtr->d->m_sd); -} - -/*! - * \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 - */ -int XsSocket_getLastSystemError(const XsSocket* thisPtr) -{ - return thisPtr->d->m_lastSystemError; -} diff --git a/extern/xstypes/xsstring.c b/extern/xstypes/xsstring.c deleted file mode 100644 index 94b7ee6..0000000 --- a/extern/xstypes/xsstring.c +++ /dev/null @@ -1,486 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#include "xsstring.h" -#include -#include "xsatomicint.h" -#include // memcpy -#include - -#if defined(WIN32) -#include -#endif - -/*! \struct XsString - \brief A 0-terminated managed string of characters - \details This structure uses XsArray to manage its internal data. - The C++ interface reports the size of the string excluding the terminating 0, but since the C version - uses XsArray directly, its m_size member includes the terminating 0. - When using the C version, make sure that the 0 character is always preserved when manipulating the data. - \sa XsArray -*/ - -/*! \copydoc XsArrayDescriptor::itemSwap - \note Specialization for char*/ -void swapChar(char* a, char* b) -{ - char tmp = *a; - *a = *b; - *b = tmp; -} - -/*! \copydoc XsArrayDescriptor::itemCopy - \note Specialization for char*/ -void copyChar(char* to, char const* from) -{ - *to = *from; -} - -/*! \copydoc XsArrayDescriptor::itemCompare - \note Specialization for char*/ -int compareChar(void const* a, void const* b) -{ - if (*(char*)a < *(char*)b) - return -1; - if (*(char*)a > *(char*)b) - return 1; - return 0; -} - -//! \brief Descriptor for XsInt64Array -XsArrayDescriptor const g_xsStringDescriptor = { - //lint --e{64} ignore exact type mismatches here - 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); - XSEXPCASTITEMCOPY copyChar, // void (*itemCopyConstruct)(void* e, void const* s); - 0, // void (*itemDestruct)(void* e); - XSEXPCASTITEMCOPY copyChar, // void (*itemCopy)(void const* from, void* to); - XSEXPCASTITEMCOMP compareChar, // int (*itemCompare)(void const* a, void const* b); - XSEXPCASTRAWCOPY XsArray_rawCopy // void (*rawCopy)(void* to, void const* from, XsSize count, XsSize iSize) -}; - -/*! \copydoc XsArray_construct - \note Specialization for XsString -*/ -void XsString_construct(XsString* thisPtr) -{ - XsArray_construct(thisPtr, &g_xsStringDescriptor, 0, 0); -} - -/*! \copydoc XsArray_destruct - \note Specialization for XsString -*/ -void XsString_destruct(XsString* thisPtr) -{ - XsArray_destruct(thisPtr); -} - -/*! \copydoc XsArray_assign - \note Specialization for XsString -*/ -void XsString_assign(XsString* thisPtr, XsSize count, const char* src) -{ - if (!count && src) - count = strlen(src)+1; - - if (src) - { - if (src[count-1]) - { - XsArray_assign(thisPtr, count+1, 0); - memcpy(thisPtr->m_data, src, count); - thisPtr->m_data[count] = 0; - } - else - XsArray_assign(thisPtr, count, src); - } - else - { - if (count) - { - XsArray_assign(thisPtr, count+1, 0); - memset(thisPtr->m_data, ' ', count); - thisPtr->m_data[count] = 0; - } - else - XsArray_assign(thisPtr, 0, 0); - } -} - -/*! \brief This function determines the size of \a src and copies the contents to the object */ -void XsString_assignCharArray(XsString* thisPtr, const char* src) -{ - XsString_assign(thisPtr, 0, src); -} - -#ifndef XSENS_NO_WCHAR -/*! \brief This function determines the size of \a src and copies the contents to the object after - converting it from a unicode string to a multibyte character string. -*/ -void XsString_assignWCharArray(XsString* thisPtr, const wchar_t* src) -{ - if (src) - { -#ifdef WIN32 - 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) - { - if (((XsSize)(unsigned int)required)+1 > thisPtr->m_reserved) { - XsArray_reserve(thisPtr, required+1); - } - WideCharToMultiByte(CP_UTF8, 0, src, unicodeLength, thisPtr->m_data, required+1, NULL, NULL); //lint !e534 - thisPtr->m_data[required] = '\0'; - *((XsSize*) &thisPtr->m_size) = required+1; - return; - } -#else - 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); //lint !e534 - *((XsSize*) &thisPtr->m_size) = required+1; - return; - } -#endif - } - XsArray_assign(thisPtr, 0, 0); -} - -//lint -save -e429 -/*! \brief This function copies the contents of the object to a unicode wchar_t array -*/ -XsSize XsString_copyToWCharArray(const XsString* thisPtr, wchar_t* dest, XsSize size) -{ -#ifdef WIN32 - return MultiByteToWideChar(CP_UTF8, 0, thisPtr->m_data, (int) thisPtr->m_size, dest, (int) size); -#else - return mbstowcs(dest, thisPtr->m_data, size) + (dest?0:1); -#endif -} -//lint -restore - -/*! \brief Append unicode character \a c to the string - \param c The character to append -*/ -void XsString_push_backWChar(XsString* thisPtr, wchar_t c) -{ - wchar_t buf[2] = { c, 0 }; - XsString tmp; - - XsString_construct(&tmp); - XsString_assignWCharArray(&tmp, buf); - XsString_append(thisPtr, &tmp); - XsString_destruct(&tmp); -} -#endif // XSENS_NO_WCHAR - -/*! \brief This function resizes the contained string to the desired size, while retaining its contents - \param count The desired size of the string. This excludes the terminating 0 character. - \sa XsArray_resize -*/ -void XsString_resize(XsString* thisPtr, XsSize count) -{ - XsSize sz = thisPtr->m_size; - XsArray_resize(thisPtr, count?count+1:0); - if (count) - { - for (;sz < count; ++sz) - thisPtr->m_data[sz] = ' '; - thisPtr->m_data[count] = 0; - } -} - -/*! \brief This function concatenates the \a other to this -*/ -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_append(thisPtr, other); - if (thisPtr == other) - { - // add terminating null again - static const char nullChar = 0; - XsArray_insert(thisPtr, (XsSize) -1, 1, &nullChar); - } - } -} - -/*! \copydoc XsArray_erase - \note The function maintains the terminating 0 character -*/ -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); - else - XsArray_erase(thisPtr, 0, thisPtr->m_size); - } - else - XsArray_erase(thisPtr, index, count); -} - -/*! \brief Append character \a c to the string - \param c The character to append -*/ -void XsString_push_back(XsString* thisPtr, char c) -{ - XsSize sz = thisPtr->m_size; - if (!sz) - sz = 1; - XsString_resize(thisPtr, sz); - thisPtr->m_data[sz-1] = c; -} - -char const * advanceUtf8(char const *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 - p += 4; - else - p += 3; - else - p += 2; - return p; -} - -/*! \brief Returns the number of characters in a UTF-8 encoded string - \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 count = 0; - char const * p = thisPtr->m_data; - - if (!thisPtr || !thisPtr->m_data) - return 0; - - while (*p != 0) - { - ++count; - p = advanceUtf8(p); - } - return count; -} - -#ifndef XSENS_NO_WCHAR -int32_t shiftUtf8(int32_t t, char const* p, int bytes) -{ - int i; - for (i = 0; i < bytes; ++i) - t = (t << 6) | (p[i] & 0x3F); - return t; -} - -/*! \brief The decoded UTF-8 character at index \a index in the UTF-8 encoded string - \details http://en.wikipedia.org/wiki/Utf-8#Description - \param index The index of the character to return. - \returns the decoded UTF-8 character at index \a index in the UTF-8 encoded string -*/ -wchar_t XsString_utf8At(XsString const* thisPtr, XsSize index) -{ - int32_t t = 0; - char const * p = thisPtr->m_data; - - if (!thisPtr || !thisPtr->m_data) - return 0; - - while (*p != 0 && index) - { - --index; - p = advanceUtf8(p); - } - - if (*p == 0) - return 0; - - // translate! - - 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 - t = shiftUtf8(p[0] & 0x07, p+1, 3); - else - t = shiftUtf8(p[0] & 0x0F, p+1, 2); - else - t = shiftUtf8(p[0] & 0x1F, p+1, 1); - return (wchar_t) t; -} -#endif - -/*! \brief Returns whether this string ends with \a other - \param other The string to match with the end of this string - \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) -{ - const char* left; - const char* right; - - // we can never find a bigger string than our own string - if (thisPtr->m_size < other->m_size) - return 0; - - // we always match an empty string - if (other->m_size <= 1) - return 1; - - left = thisPtr->m_data + thisPtr->m_size - other->m_size; - right = other->m_data; - - if (caseSensitive) - for (; *left == *right && *right; ++left, ++right); - else - for (; tolower((unsigned char)*left) == tolower((unsigned char)*right) && *right; ++left, ++right); - - if (!*right) - return 1; - - return 0; -} - -/*! \brief Returns whether this string starts with \a other - \param other The string to match with the start of this string - \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) -{ - const char* left = thisPtr->m_data; - const char* right = other->m_data; - - // we can never find a bigger string than our own string - if (thisPtr->m_size < other->m_size) - return 0; - - // we always match an empty string - if (other->m_size <= 1) - return 1; - - if (caseSensitive) - for (; *left == *right && *right; ++left, ++right); - else - for (; tolower((unsigned char)*left) == tolower((unsigned char)*right) && *right; ++left, ++right); - - if (!*right) - return 1; - - return 0; -} - -/*! \brief Returns whether this string contains \a other - \param other The string to match with this string - \param caseSensitive Whether to compare case sensitive or not (case insensitive is the default) - \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) -{ - XsSize offsetI = 0; - if (!offset) - offset = &offsetI; - *offset = 0; - - // we always match an empty string - if (other->m_size <= 1) - return 1; - - // we can never find a bigger string than our own string - while (thisPtr->m_size-*offset >= other->m_size) - { - const char* left = thisPtr->m_data+*offset; - const char* right = other->m_data; - if (caseSensitive) - for (; *left == *right && *right; ++left, ++right); - else - for (; tolower((unsigned char)*left) == tolower((unsigned char)*right) && *right; ++left, ++right); - - if (!*right) - return 1; - - ++*offset; - } - *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) -{ - if (!thisPtr) - return 1; - if (!thisPtr->m_size || (thisPtr->m_flags & XSDF_Empty)) - return 1; - return !(thisPtr->m_size-1); -} - -/*! \brief Sorts the string - \details This function sorts using qsort -*/ -void XsString_sort(XsString* thisPtr) -{ - if (thisPtr->m_size > 2) - qsort(thisPtr->m_data, thisPtr->m_size-1, sizeof(char), compareChar); -} - -/*! \brief Reverses the contents of the string - \details This reverses the contents in-place - \note This does not take into account utf-8 encoded characters -*/ -void XsString_reverse(XsString* thisPtr) -{ - int i, half; - char* data, tmp, *right; - if (thisPtr->m_size > 2) - { - half = (int) ((thisPtr->m_size-1) >> 1); - data = (char*) thisPtr->m_data; - right = data + thisPtr->m_size-2; - for (i = 0; i < half; ++i) - { - tmp = data[i]; - data[i] = right[-i]; - right[-i] = tmp; - } - } -} diff --git a/extern/xstypes/xsstringarray.c b/extern/xstypes/xsstringarray.c deleted file mode 100644 index a15af52..0000000 --- a/extern/xstypes/xsstringarray.c +++ /dev/null @@ -1,121 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#include "xsstringarray.h" -#include "xsstring.h" - -/*! \struct XsStringArray - \brief A list of XsString values - \sa XsArray -*/ - -//! \brief Descriptor for XsStringArray -XsArrayDescriptor const g_xsStringArrayDescriptor = { - //lint --e{64} ignore exact type mismatches here - sizeof(XsString), - XSEXPCASTITEMSWAP XsArray_swap, - XSEXPCASTITEMMAKE XsString_construct, - XSEXPCASTITEMCOPY XsArray_copyConstruct, - XSEXPCASTITEMMAKE XsArray_destruct, - XSEXPCASTITEMCOPY XsArray_copy, - XSEXPCASTITEMCOMP XsArray_compare, - 0 -}; - -/*! \copydoc XsArray_construct - \note Specialization for XsStringArray -*/ -void XsStringArray_construct(XsStringArray* thisPtr, XsSize count, XsString const* src) -{ - XsArray_construct(thisPtr, &g_xsStringArrayDescriptor, count, src); -} - -/*! \relates XsStringArray - \brief Splice the supplied string and put the resulting substrings in the string array - \details The source string will be searched for instances of characters in \a separators and spliced whereever - one was found. The spliced list will not include characters in \a separators and empty substrings will be - discarded. - \param src The source string to splice - \param separators A list of separator characters that will be used to splice \a src. -*/ -void XsStringArray_fromSplicedString(struct XsStringArray* thisPtr, struct XsString const* src, struct XsString const* separators) -{ - XsString s; - XsString_construct(&s); - XsArray_destruct(thisPtr); - if (src->m_size > 0) - { - // check against 1 because an empty string can either be size 0 or size 1 (just the null-terminator) - if (separators->m_size <= 1) - { - // no separator - XsArray_insert(thisPtr, 0, 1, src); - } - else - { - char const* sep = (char const*) separators->m_data; - char const* idx = (char const*) src->m_data; - char const* newIdx = strpbrk(idx, sep); - while (newIdx && *idx) - { - if (newIdx != idx) - { - XsString_assign(&s, newIdx-idx, idx); - XsArray_insert(thisPtr, thisPtr->m_size, 1, &s); - } - idx = newIdx+1; - newIdx = strpbrk(idx, sep); - } - if (*idx) - { - XsString_assignCharArray(&s, idx); - XsArray_insert(thisPtr, thisPtr->m_size, 1, &s); - } - } - } - XsString_destruct(&s); -} - -/*! \relates XsStringArray - \brief Join the string array into a single string, inserting \a separator between substrings. - \param result The result of the join - \param separator The separator to insert between successive items -*/ -void XsStringArray_join(struct XsStringArray const* thisPtr, struct XsString* result, struct XsString const* separator) -{ - // determine required buffer size - XsSize i; - 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); - } - - XsArray_destruct(result); - if (chars) - { - XsArray_reserve(result, chars+1); - for (i = 0; i < thisPtr->m_size; ++i) - { - const XsString* s = (const XsString*)XsArray_at(thisPtr, i); - if (s->m_size > 1) - { - if (result->m_size > 1) - XsString_append(result, separator); - XsString_append(result, s); - } - } - } -} diff --git a/extern/xstypes/xssyncsetting.c b/extern/xstypes/xssyncsetting.c deleted file mode 100644 index 4d9e404..0000000 --- a/extern/xstypes/xssyncsetting.c +++ /dev/null @@ -1,105 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#include "xssyncsetting.h" -#include - -/*! \addtogroup cinterface C Interface - @{ -*/ - -/*! \brief Returns whether the selected line is configured as an input line -*/ -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; - - default: - return 0; - } -} - -/*! \brief Returns whether the selected line is configured as an output line -*/ -int XsSyncSetting_isOutput(const XsSyncSetting* thisPtr) -{ - switch (thisPtr->m_line) - { - case XSL_Out1: - case XSL_Out2: - case XSL_Bi1Out: - case XSL_RtsOut: - return 1; - - default: - return 0; - } -} - -/*! \brief Swap the contents of \a a with \a b -*/ -void XsSyncSetting_swap(XsSyncSetting* a, XsSyncSetting* b) -{ - XsSyncSetting tmp; - memcpy(&tmp, a, sizeof(XsSyncSetting)); - memcpy(a, b, sizeof(XsSyncSetting)); - memcpy(b, &tmp, sizeof(XsSyncSetting)); -} - -/*! \brief Compares \a a with \a b - \returns 0 if \a a equals \a b, negative value if \a a < \a b, positive value if \a a > \a b - \param[in] a Sync setting a - \param[in] b Sync setting b -*/ -int XsSyncSetting_compare(const struct XsSyncSetting* a, const struct XsSyncSetting* b) -{ - 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 (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_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_triggerOnce < b->m_triggerOnce) return -1; - if (b->m_triggerOnce < a->m_triggerOnce) return 1; - - return 0; -} - -/*! @} */ diff --git a/extern/xstypes/xssyncsettingarray.c b/extern/xstypes/xssyncsettingarray.c deleted file mode 100644 index 6d5a049..0000000 --- a/extern/xstypes/xssyncsettingarray.c +++ /dev/null @@ -1,65 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#include "xssyncsettingarray.h" -#include "xssyncsetting.h" -#include -#include - -/*! \struct XsSyncSettingArray - \brief A list of XsSyncSetting values - \sa XsArray -*/ - -/*! \copydoc XsArrayDescriptor::itemCopy - \note Specialization for XsSyncSetting*/ -void copySyncSetting(XsSyncSetting* to, XsSyncSetting const* from) -{ - *to = *from; -} - -/*! \copydoc XsArrayDescriptor::itemCompare - \note Specialization for XsSyncSetting*/ -int compareSyncSetting(XsSyncSetting const* a, XsSyncSetting const* b) -{ - return XsSyncSetting_compare(a, b); -} - -//! \brief zero the pointer value -void zeroSyncSetting(XsSyncSetting* a) -{ - memset(a, 0, sizeof(XsSyncSetting)); - a->m_line = XSL_Invalid; -} - -//! \brief Descriptor for XsSyncSettingArray -XsArrayDescriptor const g_xsSyncSettingArrayDescriptor = { - //lint --e{64} ignore exact type mismatches here - sizeof(XsSyncSetting), - XSEXPCASTITEMSWAP XsSyncSetting_swap, - XSEXPCASTITEMMAKE zeroSyncSetting, // construct - XSEXPCASTITEMCOPY copySyncSetting, // copy construct - 0, // destruct - XSEXPCASTITEMCOPY copySyncSetting, - XSEXPCASTITEMCOMP compareSyncSetting, - XSEXPCASTRAWCOPY XsArray_rawCopy // raw copy -}; - -/*! \copydoc XsArray_construct - \note Specialization for XsSyncSettingArray -*/ -void XsSyncSettingArray_construct(XsSyncSettingArray* thisPtr, XsSize count, XsSyncSetting const* src) -{ - XsArray_construct(thisPtr, &g_xsSyncSettingArrayDescriptor, count, src); -} diff --git a/extern/xstypes/xsthread.c b/extern/xstypes/xsthread.c deleted file mode 100644 index 8fc2619..0000000 --- a/extern/xstypes/xsthread.c +++ /dev/null @@ -1,78 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#include "xsthread.h" - -#ifdef _WIN32 -#define MS_VC_EXCEPTION 0x406D1388 - -#pragma pack(push,8) -// Copied from windows API -struct THREADNAME_INFO -{ - DWORD dwType; // Must be 0x1000. - LPCSTR szName; // Pointer to name (in user addr space). - DWORD dwThreadID; // XsThread ID (-1=caller thread). - DWORD dwFlags; // Reserved for future use, must be zero. -}; -#pragma pack(pop) -typedef struct THREADNAME_INFO THREADNAME_INFO; - -/*! \addtogroup cinterface C Interface - @{ -*/ - -/*! \brief Set the name of the current thread to \a threadName */ -void XSTYPES_DLL_API xsNameThisThread(const char* threadName) -{ - DWORD dwThreadID = GetCurrentThreadId(); - - //Sleep(10); - THREADNAME_INFO info; - info.dwType = 0x1000; - info.szName = threadName; - info.dwThreadID = dwThreadID; - info.dwFlags = 0; - - __try - { - RaiseException( MS_VC_EXCEPTION, 0, sizeof(info)/sizeof(ULONG_PTR), (ULONG_PTR*)&info ); - } - __except(EXCEPTION_EXECUTE_HANDLER) - { - } -} -#else -/*! \brief Set the name of the current thread to \a threadName - \note Not implemented in POSIX -*/ -void XSTYPES_DLL_API xsNameThisThread(const char* threadName) -{ - // no implementation for this in POSIX -- pthread_key_t should be known. - // adding this function does remove some - // checking from xs4 though. - (void)threadName; -} - -pthread_t XSTYPES_DLL_API xsStartThread(void *(func)(void *), void *param, void *pid) { - (void)pid; - pthread_t thread; - if (pthread_create(&thread, NULL, func, param)) { - return XSENS_INVALID_THREAD; - } - return thread; -} -#endif - -/*! @} */ diff --git a/extern/xstypes/xstime.c b/extern/xstypes/xstime.c deleted file mode 100644 index 865e402..0000000 --- a/extern/xstypes/xstime.c +++ /dev/null @@ -1,328 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -// Note, this function requires compiler option "-lrt" to be set when compiling with gcc -#include "xstime.h" -#include "xstimestamp.h" -#include -#include - -#ifdef _WIN32 -# include -# define snprintf _snprintf -#else -# include -# include -# include -# include -# include -#endif -#include - -/*! \namespace XsTime - \brief Namespace for time and date constants and operations -*/ - -//! The number of seconds in a normal day -const XsTimeStamp XsTime_secPerDay = { 60*60*24LL }; - -//! The number of milliseconds in a normal day -const XsTimeStamp XsTime_milliSecPerDay = { 60*60*24*1000LL }; - -//! The maximum positive value of an Xsens TimeStamp value -const XsTimeStamp XsTime_timeStampMax = { 0x7FFFFFFFFFFFFFFFLL }; - - -////////////////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////// Other functions //////////////////////////////////// -////////////////////////////////////////////////////////////////////////////////////////// -#ifdef __APPLE__ -/*! \brief An implementation of linux' clock_gettime() - - clock_gettime() is not available on Apple/Darwin platforms. This function helps - maintaining compatibility with Linux code. - */ -#ifndef CLOCK_REALTIME -#define CLOCK_REALTIME 0 -#endif -static int clock_gettime(int clk_id, struct timespec *tp) -{ - (void)clk_id; - struct timeval now; - - int rv = gettimeofday(&now, NULL); - if (rv != 0) - return rv; - - tp->tv_sec = now.tv_sec; - tp->tv_nsec = now.tv_usec * 1000; - - return 0; -} -#endif - -/*! \addtogroup cinterface C Interface - @{ -*/ - -////////////////////////////////////////////////////////////////////////////////////////// -/*! \brief The function returns the current time of day in ms since midnight - \param date_ When not 0, the corresponding current date is put in here. - \param secs_ When not 0, the full timestamp in seconds since the epoch is put in here. - \returns The number of milliseconds that have passed since midnight. -*/ -uint32_t XsTime_getTimeOfDay(struct tm* date_, time_t* secs_) -{ -#ifdef _WIN32 - // CORRECTION_DELTA_MS is the maximum allowed difference between the system time and the performance counter time. This depends on the granularity of the clocks. - static const int64_t CORRECTION_DELTA_MS = 32; - static int64_t startTimePerfCount; - static int64_t startTimeSysTime; - static int64_t perfCountFreq = 0; - int64_t t; - LARGE_INTEGER pc; - FILETIME now; - __time64_t tin; - - GetSystemTimeAsFileTime(&now); - t = (int64_t) (((((uint64_t) now.dwHighDateTime << 32) | now.dwLowDateTime)/10000) - 11644473600000); - - if (QueryPerformanceCounter(&pc)) - { - int64_t tNow; - if (!perfCountFreq) - { - LARGE_INTEGER tmp; - QueryPerformanceFrequency(&tmp); //lint !e534 the return value should match that of QueryPerformanceCounter above - perfCountFreq = tmp.QuadPart; - startTimePerfCount = pc.QuadPart; - startTimeSysTime = t; - } - - tNow = startTimeSysTime + (1000*(pc.QuadPart - startTimePerfCount))/perfCountFreq; - - if (t > tNow || (tNow-t) > CORRECTION_DELTA_MS) - { - startTimePerfCount = pc.QuadPart; - startTimeSysTime = t; - } - else - t = tNow; - } - - tin = t/1000; - if (date_ != NULL) - _localtime64_s(date_,&tin); //lint !e534 - if (secs_ != NULL) - *secs_ = (time_t) tin; - return (uint32_t) (t % (XsTime_secPerDay.m_msTime*1000)); //lint !e571 cast is ok -#else - struct timespec tp; - clock_gettime(CLOCK_REALTIME, &tp); // compile with -lrt - - if (date_ != NULL) - 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 (1000 * (tp.tv_sec % XsTime_secPerDay.m_msTime)) + (tp.tv_nsec/1000000); -#endif -} - -/*! \brief Retrieves the date and time (platform-independent) - \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) -{ -#ifdef _WIN32 - __time64_t t; - _time64(&t); //lint !e534 return value is stored in t - if (date != 0) - _localtime64_s(date, &t); //lint !e534 - return (int64_t)t; -#else - time_t t; - time(&t); - if(date != 0) - { - struct tm *result = localtime(&t); - memcpy(date, result, sizeof(struct tm)); - } - return (int64_t)t; -#endif -} - -/*! \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' } - \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; - - if(date != 0) - dt = *date; - else - XsTime_getDateTime(&dt); //lint !e534 return value is stored in dt - - year = dt.tm_year + 1900; - month = dt.tm_mon + 1; - snprintf(dest, 9, "%04d%02d%02d", year, month, dt.tm_mday); //lint !e534 -} - -/*! \brief Retrieves the time as binary - The format is HHMMSShh (where H is hour and 'h' is hundredths) - so 14:25:01.23 is stored as an array dest[8] = { '1', '4', '2', '5', '0', '1', '2', '3'} - \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) - \note (for now hundreths are set to 0) -*/ -void XsTime_getTimeAsString(char* dest, const struct tm* date) -{ - struct tm dt; - - if (date != 0) - dt = *date; - else - XsTime_getDateTime(&dt); //lint !e534 return value is stored in dt - - snprintf(dest, 9, "%02d%02d%02d%02d", dt.tm_hour, dt.tm_min, dt.tm_sec, 0); //lint !e534 -} - -/*! \brief Retrieves the date as wstring representation - The format is YYYYMMDD - so 25 dec 2010 is stored as an array dest[8] = {'2', '0', '1', '0', '1', '2', '2', '5' } - \param dest : A pointer to an array of at least (!) 8 wchars - \param date : If date is non-zero this date is converted, otherwise the current date is retrieved and used) -*/ -void XsTime_getDateAsWString(wchar_t* dest, const struct tm* date) -{ - struct tm dt; - int year, month; - - if(date != 0) - dt = *date; - else - XsTime_getDateTime(&dt); //lint !e534 return value is stored in dt - - year = dt.tm_year + 1900; - month = dt.tm_mon + 1; - swprintf(dest, 9, L"%04d%02d%02d", year, month, dt.tm_mday); //lint !e534 -} - -/*! \brief Retrieves the time as binary - The format is HHMMSShh (where H is hour and 'h' is hundredths) - so 14:25:01.23 is stored as an array dest[8] = { '1', '4', '2', '5', '0', '1', '2', '3'} - \param dest : A pointer to an array of at least (!) 8 wchars - \param date : If date is non-zero this date is converted, otherwise the current date is retrieved and used) - \note (for now hundreths are set to 0) -*/ -void XsTime_getTimeAsWString(wchar_t* dest, const struct tm* date) -{ - struct tm dt; - - if (date != 0) - dt = *date; - else - XsTime_getDateTime(&dt); //lint !e534 return value is stored in dt - - swprintf(dest, 9, L"%02d%02d%02d%02d", dt.tm_hour, dt.tm_min, dt.tm_sec, 0); //lint !e534 -} - -/*! \brief Make the current thread sleep for at least \a ms milliseconds - \details A platform independent sleep routine to sleep for at least \a ms milliseconds. - - \param ms The number of milliseconds to sleep - - \sa XsTime_udelay -*/ -void XsTime_msleep(uint32_t ms) -{ -#ifdef _WIN32 - Sleep(ms); -#else - XsTime_udelay(ms * 1000); -#endif -} - -/*! \brief Delays the current thread for at least \a us microseconds - \details A platform independent delay routine to sleep for at least \a us microseconds. - - \param us The number of microseconds to delay - - \sa XsTime_msleep -*/ -void XsTime_udelay(uint64_t us) -{ -#ifdef _WIN32 - //lint --e{534} Ignoring return value of query functions. - LARGE_INTEGER freq, start, stop; - double countPerMicroSecond ; - QueryPerformanceCounter(&start); - QueryPerformanceFrequency(&freq); - countPerMicroSecond = freq.QuadPart / 1.0e6; - do { - QueryPerformanceCounter(&stop); - } - while (start.QuadPart + ((double)(us) * countPerMicroSecond) > stop.QuadPart); -#else - struct timespec ts; - int ret = -1; - - ts.tv_sec = us / 1000000; - ts.tv_nsec = (us - (ts.tv_sec * 1000000)) * 1000; - - while (ret) - { - ret = nanosleep(&ts, &ts); - if (ret) - assert(errno == EINTR); - } -#endif -} - -/*! \brief Returns the current time in ms since the epoch (Jan 1st 1970) - \param now Pointer to %XsTimeStamp container for the returned value, may be 0 - \returns The current time in ms since the epoch (Jan 1st 1970) as a 64-bit integer -*/ -int64_t XsTime_timeStampNow(XsTimeStamp* now) -{ - XsTimeStamp tmp; - time_t s; - - if (now == 0) - now = &tmp; - - now->m_msTime = (long long) XsTime_getTimeOfDay(NULL,&s); - now->m_msTime = (now->m_msTime % 1000) + (((long long)s)*1000); - - return now->m_msTime; -} - -/*! \brief Stabilize the clock - \details Repeatedly call XsTime_timeStampNow for 16-32 ms to stabilize the clock -*/ -void XsTime_initializeTime() -{ - long long start = XsTime_timeStampNow(0); - while (XsTime_timeStampNow(0) - start < 32) {} -} - -/*! @} */ diff --git a/extern/xstypes/xstimestamp.c b/extern/xstypes/xstimestamp.c deleted file mode 100644 index c59a7c0..0000000 --- a/extern/xstypes/xstimestamp.c +++ /dev/null @@ -1,174 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#include "xstimestamp.h" -#include "xstime.h" -#include "xsutctime.h" -#include -#include - -/*! \class XsTimeStamp - \brief This class contains method to set, retrieve and compare timestamps -*/ - -/*! \addtogroup cinterface C Interface - @{ -*/ - -/*! \relates XsTimeStamp \brief Set the stored time to \a miliseconds. */ -void XsTimeStamp_setMilliSecondTime(XsTimeStamp* thisPtr, int64_t t) -{ - thisPtr->m_msTime = 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; -} - -/*! \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; -} - -/*! \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); -} - -/*! \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); -} - -/*! \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); -} - -/*! \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); -} - -/*! \relates XsTimeStamp - \brief Returns the current time in ms since the epoch (Jan 1st 1970) - \see XsTime_timeStampNow - \param dest The object to write the time to, may be 0 in which case only the return value is generated. - \returns The current time in ms since the epoch (Jan 1st 1970) -*/ -int64_t XsTimeStamp_now(XsTimeStamp* dest) -{ - return (int64_t) XsTime_timeStampNow(dest); -} - -/*! \relates XsTimeStamp - \brief Returns the maximum value of an %XsTimeStamp -*/ -int64_t XsTimeStamp_maxValue(void) -{ - return 9223372036854775807LL; //INT64_MAX; -} - -/*! \relates XsTimeStamp - \brief Creates an XsTimeStamp from an XsUtcTime object - \param utc The UTC time to convert - \returns The converted time in ms since the epoch (Jan 1st 1970) -*/ -int64_t XsTimeStamp_fromUtcTime(struct XsTimeStamp* thisPtr, const struct XsUtcTime* utc) -{ - int64_t rv = -1; - if (utc && utc->m_valid) - { - struct tm utctm; - utctm.tm_year = utc->m_year; - utctm.tm_mon = utc->m_month; - utctm.tm_mday = utc->m_day; - - utctm.tm_hour = utc->m_hour; - utctm.tm_min = utc->m_minute; - utctm.tm_sec = utc->m_second; - - // unused - utctm.tm_wday = 0; - utctm.tm_yday = 0; - utctm.tm_isdst = 0; - - rv = (((int64_t) mktime(&utctm)) * 1000LL) + (utc->m_nano / 1000000); - } - if (thisPtr) - thisPtr->m_msTime = rv; - return rv; -} - -/*! \relates XsTimeStamp - \brief Converts the timestamp into an XsUtcTime object - \param utc The UTC time object to write the conversion result to -*/ -void XsTimeStamp_toUtcTime(struct XsTimeStamp* thisPtr, struct XsUtcTime* utc) -{ - struct tm tmUtc; -#ifdef _WIN32 - __time64_t t; - t = (__time64_t)(thisPtr->m_msTime / 1000); - if (_gmtime64_s(&tmUtc, &t)) - { - //in case of an error the result is an invalid XsUtctime - utc->m_valid = 0; - return; - } -#elif (defined(__arm__) && defined(__ARMCC_VERSION)) - #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); - if (tmUtcPtr == 0) - { - utc->m_valid = 0; - return; - } - memcpy(&tmUtc, tmUtcPtr, sizeof(tmUtc)); -#else - time_t t; - t = (time_t)(thisPtr->m_msTime / 1000); - if (gmtime_r(&t, &tmUtc) == 0) - { - //in case of an error the result is an invalid XsUtctime - utc->m_valid = 0; - return; - } -#endif - - utc->m_day = (uint8_t) tmUtc.tm_mday; - utc->m_hour = (uint8_t) tmUtc.tm_hour; - utc->m_minute = (uint8_t) tmUtc.tm_min; - utc->m_month = (uint8_t) tmUtc.tm_mon + 1; - utc->m_nano = (uint32_t)((thisPtr->m_msTime % 1000) * 1e6); - utc->m_second = (uint8_t) tmUtc.tm_sec; - utc->m_year = (uint16_t) (tmUtc.tm_year + 1900); - utc->m_valid = 1; -} - -/*! @} */ diff --git a/extern/xstypes/xstriggerindicationdata.c b/extern/xstypes/xstriggerindicationdata.c deleted file mode 100644 index 1c305fa..0000000 --- a/extern/xstypes/xstriggerindicationdata.c +++ /dev/null @@ -1,34 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#include "xstriggerindicationdata.h" -#include - -/*! \addtogroup cinterface C Interface - @{ -*/ - -/*! \brief Destroy the %XsTriggerIndicationData object */ -void XsTriggerIndicationData_destruct(XsTriggerIndicationData* thisPtr) -{ - memset(thisPtr, 0, sizeof(XsTriggerIndicationData)); -} - -/*! \brief Returns true if the object is valid (line and polarity may not be 0) */ -int XsTriggerIndicationData_valid(const XsTriggerIndicationData* thisPtr) -{ - return thisPtr->m_line != 0 && thisPtr->m_polarity != 0; -} - -/*! @} */ diff --git a/extern/xstypes/xstypedefs.c b/extern/xstypes/xstypedefs.c deleted file mode 100644 index 1a7fbe3..0000000 --- a/extern/xstypes/xstypedefs.c +++ /dev/null @@ -1,52 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#include "xstypedefs.h" -#include - -/*! \addtogroup cinterface C Interface - @{ -*/ - -/*! \brief Convert the %XsDataFlags to a human readable string - \param f The flags to translate - \returns A pointer to a statically allocated memory buffer. Do not free this buffer. - \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) -{ - static char rv[4*20]; - if (f == XSDF_None) - return "XSDF_None"; - - rv[0] = 0; - if (f & XSDF_Managed) - strcpy(rv, "XSDF_Managed"); - if (f & XSDF_FixedSize) - { - if (rv[0]) - strcat(rv, " | "); - strcat(rv, "XSDF_FixedSize"); - } - if (f & XSDF_Empty) - { - if (rv[0]) - strcat(rv, " | "); - strcat(rv, "XSDF_Empty"); - } - return rv; -} - -/*! @} */ diff --git a/extern/xstypes/xsutctime.c b/extern/xstypes/xsutctime.c deleted file mode 100644 index a7061db..0000000 --- a/extern/xstypes/xsutctime.c +++ /dev/null @@ -1,41 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#include "xstimestamp.h" -#include "xsutctime.h" - -/*! \class XsUtcTime - \brief This class can contain UTC time information -*/ - -/*! \addtogroup cinterface C Interface - @{ -*/ - -/*! \relates XsUtcTime - \brief Returns the current time in UTC date time format -*/ -void XsUtcTime_currentTime(struct XsUtcTime * thisPtr) -{ - XsTimeStamp timeStamp; - - if (thisPtr == 0) - return; - - (void)XsTimeStamp_now(&timeStamp); - XsTimeStamp_toUtcTime(&timeStamp, thisPtr); -} - - -/*! @} */ diff --git a/extern/xstypes/xsvector.c b/extern/xstypes/xsvector.c deleted file mode 100644 index 9414610..0000000 --- a/extern/xstypes/xsvector.c +++ /dev/null @@ -1,295 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#include "xsvector.h" -#include -#include -#include "xsatomicint.h" -#include -#include "xsmalloc.h" -#include "xsquaternion.h" -#include "xscopy.h" -#include - -//lint -e123 -e40 -e522 the INC_ALLOC and INC_ALLOC(L) definitions confuse PCLint - -XsAtomicInt XSTYPES_DLL_API XsVector_allocCount = XSATOMICINT_INITIALIZER; //!< The number of times XsVector_ functions have allocated memory -XsAtomicInt XSTYPES_DLL_API XsVector_freeCount = XSATOMICINT_INITIALIZER; //!< The number of times XsVector_ functions have freed memory - -#ifdef XSENS_DEBUG -#define INC_ALLOC() (void)XsAtomicInt_preIncrement(&XsVector_allocCount) -#define INC_FREE() (void)XsAtomicInt_preIncrement(&XsVector_freeCount) -#else -#define INC_ALLOC() ((void)0) -#define INC_FREE() ((void)0) -#endif - -//lint -e641 conversion from enum to int should not be a problem -#define realSwap(a,b) { XsReal t = *a; *a = *b; *b = t; } - -/*! \class XsVector - \brief A class that represents a vector of real numbers -*/ - -/*! \addtogroup cinterface C Interface - @{ -*/ - -//! \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); - *((XsReal**) &thisPtr->m_data) = buffer; - *((XsSize*) &thisPtr->m_size) = sz; - *((int*) &thisPtr->m_flags) = flags; -} - -//! \relates XsVector \brief Initialize the %XsVector using \a sz number of items from \a src -void XsVector_construct(XsVector* thisPtr, XsSize sz, const XsReal* src) -{ - if (sz) - { - // init to size - XsReal* data = (XsReal*) xsMathMalloc(sz*sizeof(XsReal)); - assert(data); - *((XsReal**) &thisPtr->m_data) = data; - INC_ALLOC(); - } - else - *((XsReal**) &thisPtr->m_data) = 0; - *((int*) &thisPtr->m_flags) = XSDF_Managed; - *((XsSize*) &thisPtr->m_size) = sz; - if (src && sz) - memcpy(thisPtr->m_data, src, sz*sizeof(XsReal)); -} - -/*! \relates XsVector - \brief Initialize the XsVector using \a sz number of items from \a src - \param sz The desired size of the vector - \param src 0 or a pointer to a buffer containing \a sz items to copy into the %XsVector -*/ -void XsVector_assign(XsVector* thisPtr, XsSize sz, const XsReal* src) -{ - if (thisPtr->m_flags == XSDF_FixedSize) - { - if (sz == 0) - { - *((int*) &thisPtr->m_flags) |= XSDF_Empty; - return; - } - assert(sz == thisPtr->m_size); - *((int*) &thisPtr->m_flags) &= ~XSDF_Empty; - } - - if (sz > thisPtr->m_size || sz == 0) - { - XsVector_destruct(thisPtr); - if (sz) - { - // init to size - XsReal* data = (XsReal*) xsMathMalloc(sz*sizeof(XsReal)); - assert(data); - *((XsReal**) &thisPtr->m_data) = data; - *((int*) &thisPtr->m_flags) = XSDF_Managed; - INC_ALLOC(); - } - } - *((XsSize*) &thisPtr->m_size) = sz; - if (src && sz) - memcpy(thisPtr->m_data, src, sz*sizeof(XsReal)); -} - -//! \relates XsVector \brief Release and clear the contents of the vector -void XsVector_destruct(XsVector* thisPtr) -{ - if (thisPtr->m_data && (thisPtr->m_flags & XSDF_Managed)) - { - // clear contents - xsMathFree((void*) thisPtr->m_data); - INC_FREE(); - } - // init to 0 - if (!(thisPtr->m_flags & XSDF_FixedSize)) - { - *((XsReal**) &thisPtr->m_data) = 0; - *((XsSize*) &thisPtr->m_size) = 0; - *((int*) &thisPtr->m_flags) = 0; - } - else - *((int*) &thisPtr->m_flags) |= XSDF_Empty; -} - -//! \relates XsVector \brief Copy the contents of the %XsVector to \a copy -void XsVector_copy(XsVector* copy, XsVector const* src) -{ - if (copy == src) - return; - - if (src->m_flags & XSDF_Empty) - XsVector_destruct(copy); - else - XsVector_assign(copy, src->m_size, src->m_data); -} - -/*! \relates XsVector - \brief Compute and return the dot product of XsVectors \a a and \a b -*/ -XsReal XsVector_dotProduct(const XsVector* a, const XsVector* b) -{ - XsSize i; - 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]; - return r; -} - -/*! \relates XsVector - \brief Compute and return the cartesian length - \returns The cartesian length (square root of the dot product) of the vector -*/ -XsReal XsVector_cartesianLength(const XsVector* thisPtr) -{ - return sqrt(XsVector_dotProduct(thisPtr, thisPtr)); -} - -/*! \relates XsVector - \brief Normalize the vector -*/ -void XsVector_normalize(XsVector* thisPtr) -{ - XsReal length = XsVector_cartesianLength(thisPtr); - - if (length < XsMath_tinyValue) - { - XsVector_setZero(thisPtr); - return; - } - - XsVector_multiplyScalar(thisPtr, XsMath_one / length, thisPtr); -} - -//! \relates XsVector \brief Sets all elements of the %XsVector to 0 -void XsVector_setZero(XsVector* thisPtr) -{ - XsSize i; - for (i = 0; i < thisPtr->m_size; ++i) - thisPtr->m_data[i] = XsMath_zero; -} - -//! \relates XsVector \brief Sets all elements of the %XsVector to \a value -void XsVector_fill(struct XsVector* thisPtr, XsReal value) -{ - XsSize i; - for (i = 0; i < thisPtr->m_size; ++i) - thisPtr->m_data[i] = value; -} - -//! \relates XsVector \brief Returns a non-zero value if the XsVector does not contain any values -int XsVector_empty(const XsVector* thisPtr) -{ - return (thisPtr->m_size == 0) || (thisPtr->m_flags & XSDF_Empty); -} - -//! \relates XsVector \brief Multiplies all values in this XsVector by \a scalar and puts the result in XsVector \a dest -void XsVector_multiplyScalar(const XsVector* thisPtr, XsReal scalar, XsVector* dest) -{ - XsSize i; - XsVector_assign(dest, thisPtr->m_size, 0); - for (i = 0; i < thisPtr->m_size; ++i) - dest->m_data[i] = thisPtr->m_data[i] * scalar; -} - -/*! \relates XsVector - \brief Get an effective angular velocity from the quaternion, which must represent a delta angle. - \param deltaT The length of the time interval over which \a quat was integrated in seconds - \param quat The orientation increment to convert to an angular velocity - \returns A vector containing the effective angular velocity in radians around each axis. -*/ -void XsVector_angularVelocityFromQuaternion(XsVector* thisPtr, XsReal deltaT, const XsQuaternion* quat) -{ - XsReal a; - if (XsQuaternion_empty(quat)) - { - XsVector_destruct(thisPtr); - return; - } - - 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); -} - -/*! \relates XsVector \brief Swap the contents of \a a and \a b - \details This function swaps the internal buffers so no actual data is moved around. For unmanaged - data an elementwise swap is done, but only if the vectors are the same size. - \param a Object whose contents will be placed in \a b - \param b Object whose contents will be placed in \a a -*/ -void XsVector_swap(XsVector* a, XsVector* b) -{ - XSLISTSWAP3(XsReal, XsVector, realSwap) //lint !e123 -} - -/*! \relates XsVector - \brief Returns non-zero when the two vectors are identical - \param a Vector to compare against \a b - \param b Vector to compare against \a a - \returns non-zero when the vectors are identical -*/ -int XsVector_equal(const struct XsVector* a, const struct XsVector* b) -{ - if (a == b) - return 1; - if (!a || !b) - return 0; - if (XsVector_empty(a) && XsVector_empty(b)) - return 1; - if (a->m_size != b->m_size) - return 0; - return memcmp(a->m_data, b->m_data, a->m_size*sizeof(XsReal)) == 0; -} - -/*! \relates XsVector - \brief Returns non-zero if the two vectors are equal within \a epsilon - - \param thatPtr the vector to compare with - \param epsilon the maximum difference between values - \returns non-zero when the vectors are equal within \a epsilon -*/ -int XsVector_compare(const struct XsVector* thisPtr, const struct XsVector* thatPtr, XsReal epsilon) -{ - XsSize i; - - if (thisPtr == thatPtr) - return 1; - if (!thisPtr || !thatPtr) - return 0; - if (XsVector_empty(thisPtr) && XsVector_empty(thatPtr)) - return 1; - if (thisPtr->m_size != thatPtr->m_size) - return 0; - - for (i = 0; i < thisPtr->m_size; i++) - { - if (fabs(thisPtr->m_data[i] - thatPtr->m_data[i]) > epsilon) - return 0; - } - return 1; -} - -/*! @} */ - -//lint +e123 +e40 +e522 diff --git a/extern/xstypes/xsvector3.c b/extern/xstypes/xsvector3.c deleted file mode 100644 index 0e094d3..0000000 --- a/extern/xstypes/xsvector3.c +++ /dev/null @@ -1,57 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#include "xsvector3.h" -#include - -//lint -e641 conversion from enum to int should not be a problem - -/*! \class XsVector3 - \brief A class that represents a fixed size (3) vector -*/ - -/*! \addtogroup cinterface C Interface - @{ -*/ - -/*! \relates XsVector3 \brief Init the %XsVector3 and copy the data from \a src into the vector if \a src is not null */ -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)); -} - -/*! \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)); -} - -/*! \relates XsVector3 \brief Frees the XsVector3 */ -void XsVector3_destruct(XsVector3* thisPtr) -{ - // don't do anything, no memory needs to be freed - assert(thisPtr->m_vector.m_flags & XSDF_FixedSize); - (void)thisPtr; -} - -/*! \relates XsVector3 \brief Copy the contents of the %XsVector3 to \a copy */ -void XsVector3_copy(XsVector* copy, XsVector3 const* src) -{ - XsVector_copy(copy, &src->m_vector); -} - -/*! @} */ diff --git a/extern/xstypes/xsversion.c b/extern/xstypes/xsversion.c deleted file mode 100644 index 806d95b..0000000 --- a/extern/xstypes/xsversion.c +++ /dev/null @@ -1,132 +0,0 @@ -/* WARNING: COPYRIGHT (C) 2017 XSENS TECHNOLOGIES OR SUBSIDIARIES WORLDWIDE. ALL RIGHTS RESERVED. - THIS FILE AND THE SOURCE CODE IT CONTAINS (AND/OR THE BINARY CODE FILES FOUND IN THE SAME - FOLDER THAT CONTAINS THIS FILE) AND ALL RELATED SOFTWARE (COLLECTIVELY, "CODE") ARE SUBJECT - TO A RESTRICTED LICENSE AGREEMENT ("AGREEMENT") BETWEEN XSENS AS LICENSOR AND THE AUTHORIZED - LICENSEE UNDER THE AGREEMENT. THE CODE MUST BE USED SOLELY WITH XSENS PRODUCTS INCORPORATED - INTO LICENSEE PRODUCTS IN ACCORDANCE WITH THE AGREEMENT. ANY USE, MODIFICATION, COPYING OR - DISTRIBUTION OF THE CODE IS STRICTLY PROHIBITED UNLESS EXPRESSLY AUTHORIZED BY THE AGREEMENT. - IF YOU ARE NOT AN AUTHORIZED USER OF THE CODE IN ACCORDANCE WITH THE AGREEMENT, YOU MUST STOP - USING OR VIEWING THE CODE NOW, REMOVE ANY COPIES OF THE CODE FROM YOUR COMPUTER AND NOTIFY - XSENS IMMEDIATELY BY EMAIL TO INFO@XSENS.COM. ANY COPIES OR DERIVATIVES OF THE CODE (IN WHOLE - OR IN PART) IN SOURCE CODE FORM THAT ARE PERMITTED BY THE AGREEMENT MUST RETAIN THE ABOVE - COPYRIGHT NOTICE AND THIS PARAGRAPH IN ITS ENTIRETY, AS REQUIRED BY THE AGREEMENT. -*/ - -#include "xsversion.h" -#include "xsstring.h" -#include - -/*! \class XsVersion - \brief A class to store version information -*/ - -/*! \addtogroup cinterface C Interface - @{ -*/ - -/*! \relates XsVersion \brief Test if this is a null-version. */ -int XsVersion_empty(const XsVersion* thisPtr) -{ - return thisPtr->m_major == 0 && thisPtr->m_minor == 0 && thisPtr->m_revision == 0; -} - -/*! \relates XsVersion \brief Get a string with the version expressed in a readable format. */ -void XsVersion_toString(const XsVersion* thisPtr, XsString* version) -{ - char buffer[256]; - size_t chars; - - if (thisPtr->m_build != 0 && thisPtr->m_reposVersion != 0) - chars = sprintf(buffer, "%d.%d.%d build %d rev %d", thisPtr->m_major, thisPtr->m_minor, thisPtr->m_revision, thisPtr->m_build, thisPtr->m_reposVersion); - else - chars = sprintf(buffer, "%d.%d.%d", thisPtr->m_major, thisPtr->m_minor, thisPtr->m_revision); - - XsString_assign(version, chars, buffer); - if (thisPtr->m_extra.m_size != 0) - { - const char space = ' '; - XsArray_insert(version, version->m_size-1, 1, &space); //lint !e64 - XsString_append(version, &thisPtr->m_extra); - } -} - -/*! - * \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]; - size_t chars; - - chars = sprintf(buffer, "%d.%d.%d", thisPtr->m_major, thisPtr->m_minor, thisPtr->m_revision); - XsString_assign(version, chars, buffer); -} - -/*! - * \relates XsVersion - * \brief Set the version to the values in the string - */ -void XsVersion_fromString(XsVersion* thisPtr, const XsString* version) -{ - int major = 0; - int minor = 0; - int revision = 0; - int build = 0; - int reposVersion = 0; - int result = 0; - size_t count = 0; - - assert(thisPtr); - thisPtr->m_major = 0; - thisPtr->m_minor = 0; - thisPtr->m_revision = 0; - thisPtr->m_build = 0; - thisPtr->m_reposVersion = 0; - XsString_resize(&thisPtr->m_extra, 0); - if (!version || XsString_empty(version)) - return; - - result = sscanf(version->m_data, "%d.%d.%d build %d rev %d%zn", &major, &minor, &revision, &build, &reposVersion, &count); - - if (result > 0) - { - thisPtr->m_major = (uint8_t)major; - thisPtr->m_minor = (uint8_t)minor; - thisPtr->m_revision = (uint8_t)revision; - } - - if (result > 3) - { - thisPtr->m_build = (uint8_t)build; - thisPtr->m_reposVersion = (uint8_t)reposVersion; - } - - if ((result == 5) && ((count + 1) < version->m_size)) - XsString_assignCharArray(&thisPtr->m_extra, &version->m_data[count + 1]); -} - -/*! \relates XsVersion - * \brief Create a XsVersion a XsSimpleVersion, \a simpleVersion. - */ -void XsVersion_fromSimpleVersion(XsVersion* thisPtr, const XsSimpleVersion* simpleVersion) -{ - thisPtr->m_major = simpleVersion->m_major; - thisPtr->m_minor = simpleVersion->m_minor; - thisPtr->m_revision = simpleVersion->m_revision; - thisPtr->m_build = 0; - thisPtr->m_reposVersion = 0; - XsString_resize(&thisPtr->m_extra, 0); -} - -/*! \relates XsVersion - * \brief Create a XsSimpleVersion (\a version) from a XsVersion. - */ -void XsVersion_toSimpleVersion(const XsVersion* thisPtr, XsSimpleVersion* simpleVersion) -{ - simpleVersion->m_major = thisPtr->m_major; - simpleVersion->m_minor = thisPtr->m_minor; - simpleVersion->m_revision = thisPtr->m_revision; -} - -/*! @} */ diff --git a/xsensmt/CMakeLists.txt b/xsensmt/CMakeLists.txt index e0e6e50..906475a 100644 --- a/xsensmt/CMakeLists.txt +++ b/xsensmt/CMakeLists.txt @@ -15,9 +15,10 @@ if(ENABLE_xsensmt) yarp_add_plugin(xsensmt ${PLUGIN_SOURCES} ${PLUGIN_HEADERS}) - target_link_libraries(xsensmt YARP::YARP_OS YARP::YARP_dev YARP::YARP_sig xsens_mt_software_suite) + target_link_libraries(xsensmt YARP::YARP_OS YARP::YARP_dev YARP::YARP_sig) + target_link_libraries(xsensmt xsens_mt_software_suite) + target_include_directories(xsensmt PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} ${YARP_INCLUDE_DIRS}) - target_include_directories(xsensmt PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/extern/include) yarp_install(TARGETS xsensmt COMPONENT runtime @@ -26,4 +27,9 @@ if(ENABLE_xsensmt) yarp_install(FILES "${CMAKE_CURRENT_SOURCE_DIR}/xsensmt.ini" DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) + # Install the example file from Xsens + add_executable(yarp-xsensmt-example exampleScanDevicesAndReceiveData.cpp) + target_link_libraries(yarp-xsensmt-example xsens_mt_software_suite) + install(TARGETS yarp-xsensmt-example RUNTIME DESTINATION bin) + endif() diff --git a/xsensmt/XsensMT.cpp b/xsensmt/XsensMT.cpp index ed162b5..345c7e0 100644 --- a/xsensmt/XsensMT.cpp +++ b/xsensmt/XsensMT.cpp @@ -5,6 +5,7 @@ */ +#include #include #include #include @@ -17,12 +18,10 @@ #include #include -#include -#include -#include -#include - #include "XsensMT.h" +#include "xsbaud.h" + +Journaller* gJournal; using namespace yarp::os; using namespace yarp::dev; @@ -125,99 +124,127 @@ bool XsensMT::open(yarp::os::Searchable &config) m_portInfo = XsPortInfo(comPortString, XsBaud::numericToRate(baudRate)); + m_xsensControl = XsControl::construct(); + assert(m_xsensControl != 0); + + yInfo("xsensmt: Opening serial port %s with baud rate %d and output period %4.4f seconds.", comPortString.c_str(), baudRate, m_outputPeriod); - if (!m_xsensDevice.openPort(m_portInfo)) + if (!m_xsensControl->openPort(comPortString, XsBaud::numericToRate(baudRate))) { - yError("xsensmt: Could not open serial port."); + yError("Could not open port. Aborting."); return false; } + // Get the device object + m_xsensDevice = m_xsensControl->device(m_portInfo.deviceId()); + assert(m_xsensDevice != 0); + + yInfo("Device: %s, with ID: %s opened.", m_xsensDevice->productCode().toStdString().c_str(), m_xsensDevice->deviceId().toString().toStdString().c_str()); + // << m_xsensDevice->productCode().toStdString() << ", with ID: " << m_xsensDevice->deviceId().toString().toStdString() << " opened."; + yInfo("xsensmt: Putting device into configuration mode."); // Put the device into configuration mode before configuring the device - if (!m_xsensDevice.gotoConfig()) + if (!m_xsensDevice->gotoConfig()) { yError("xsensmt: Could not put device in configuration mode."); return false; } // Request the device Id to check the device type - m_portInfo.setDeviceId(m_xsensDevice.getDeviceId()); + m_portInfo.setDeviceId(m_xsensDevice->deviceId()); - // Check if we have an MTi / MTx / MTmk4 device - if (!m_portInfo.deviceId().isMt9c() && !m_portInfo.deviceId().isLegacyMtig() && !m_portInfo.deviceId().isMtMk4() && !m_portInfo.deviceId().isFmt_X000()) + // Check if we have an MTi + if (!m_portInfo.deviceId().isMti()) { - yError("xsensmt: No MTi / MTx / MTmk4 device found. Aborting."); + yError("xsensmt: No MTi device found. Aborting."); return false; } - yInfo() << "xsensmt: Found a device with id: " << m_portInfo.deviceId().toString().toStdString() << " @ port: " << m_portInfo.portName().toStdString() << ", baudrate: " << m_portInfo.baudrate() << " ."; - yInfo() << "xsensmt: Device: " << m_xsensDevice.getProductCode().toStdString() << " opened."; - - // Configure the device. Note the differences between MTix and MTmk4 - yInfo("xsensmt: Configuring the device of type %s.", m_portInfo.deviceId().toString().c_str()); - if (m_portInfo.deviceId().isMt9c() || m_portInfo.deviceId().isLegacyMtig()) + // Configure the device. First Check if the device is an MTi + yInfo("xsensmt: Configuring the device %s.", m_xsensDevice->productCode().toStdString().c_str()); + if (!m_portInfo.deviceId().isMti()) { yError("xsensmt: Device of type %s is not supported by the driver, aborting.", m_portInfo.deviceId().toString().c_str()); return false; - } - else if (m_portInfo.deviceId().isMtMk4() || m_portInfo.deviceId().isFmt_X000()) - { - XsOutputConfiguration euler(XDI_EulerAngles, m_outputFrequency); - XsOutputConfiguration acc(XDI_Acceleration, m_outputFrequency); - XsOutputConfiguration gyro(XDI_RateOfTurn, m_outputFrequency); - XsOutputConfiguration mag(XDI_MagneticField, m_outputFrequency); - + } + else + { XsOutputConfigurationArray configArray; - configArray.push_back(euler); - configArray.push_back(acc); - configArray.push_back(gyro); - configArray.push_back(mag); - if (!m_xsensDevice.setOutputConfiguration(configArray)) + + if (m_xsensDevice->deviceId().isImu()) { - yError("xsensmt: Could not configure MTmk4 device. Aborting."); - return false; + XsOutputConfiguration acc(XDI_Acceleration, m_outputFrequency); + XsOutputConfiguration gyro(XDI_RateOfTurn, m_outputFrequency); + XsOutputConfiguration mag(XDI_MagneticField, m_outputFrequency); + + configArray.push_back(acc); + configArray.push_back(gyro); + configArray.push_back(mag); + + if (!m_xsensDevice->setOutputConfiguration(configArray)) + { + yError("xsensmt: Could not configure device. Aborting."); + return false; + } } - - // Paramater configuration - - /// Profile configuration - // Possible profiles - // Profile numbers extracted from Document MT0101P.2018.B - MT Low Level Communication Documentation - // Documentation of setFilterProfile message, page 48 - // https://xsens.com/download/usermanual/ISM/MT_LowLevelCommunicationProtocol_Documentation.pdf - uint16_t VRU_general = 43; - if (!this->setFilterProfile(VRU_general)) + else if (m_xsensDevice->deviceId().isVru() || m_xsensDevice->deviceId().isAhrs()) { - yError("xsensmt: Failing in sending SetFilterProfile message. Aborting."); - return false; + XsOutputConfiguration euler(XDI_EulerAngles, m_outputFrequency); + // IMU stuff + XsOutputConfiguration acc(XDI_Acceleration, m_outputFrequency); + XsOutputConfiguration gyro(XDI_RateOfTurn, m_outputFrequency); + XsOutputConfiguration mag(XDI_MagneticField, m_outputFrequency); + + configArray.push_back(euler); + configArray.push_back(acc); + configArray.push_back(gyro); + configArray.push_back(mag); + + if (!m_xsensDevice->setOutputConfiguration(configArray)) + { + yError("xsensmt: Could not configure device. Aborting."); + return false; + } } - - /// Options configuration - // Flags numbers extracted from Document MT0101P.2018.B - MT Low Level Communication Documentation - // Documentation of SetOptionFlags message, page 17 - // https://xsens.com/download/usermanual/ISM/MT_LowLevelCommunicationProtocol_Documentation.pdf - uint32_t EnableAhs = 0x00000010; - uint32_t SetFlags = EnableAhs; - uint32_t ClearFlags = 0x0; - if (!this->setOptionFlags(SetFlags, ClearFlags)) + else if (m_xsensDevice->deviceId().isGnss()) { - yError("xsensmt: Failing in sending SetOptionFlags message. Aborting."); - return false; + XsOutputConfiguration euler(XDI_EulerAngles, m_outputFrequency); + XsOutputConfiguration posHorizontal(XDI_LatLon, m_outputFrequency); + XsOutputConfiguration posVertical(XDI_AltitudeEllipsoid, m_outputFrequency); + XsOutputConfiguration linVel(XDI_VelocityXYZ, m_outputFrequency); + // IMU stuff + XsOutputConfiguration acc(XDI_Acceleration, m_outputFrequency); + XsOutputConfiguration gyro(XDI_RateOfTurn, m_outputFrequency); + XsOutputConfiguration mag(XDI_MagneticField, m_outputFrequency); + + configArray.push_back(euler); + configArray.push_back(posHorizontal); + configArray.push_back(posVertical); + configArray.push_back(linVel); + configArray.push_back(acc); + configArray.push_back(gyro); + configArray.push_back(mag); + + if (!m_xsensDevice->setOutputConfiguration(configArray)) + { + yError("xsensmt: Could not configure device. Aborting."); + return false; + } } - } - else - { - yError("xsensmt: Unknown device while configuring. Aborting."); - return false; - } + else + { + yError("xsensmt: Unknown device while configuring. Aborting."); + return false; + } + } - // Put the device in measurement mode - yInfo() << "xsensmt: Putting device into measurement mode."; - if (!m_xsensDevice.gotoMeasurement()) - { + // Put the device in measurement mode + yInfo() << "xsensmt: Putting device into measurement mode."; + if (!m_xsensDevice->gotoMeasurement()) + { yError("xsensmt: Could not put device into measurement mode. Aborting."); return false; - } + } // Initialize the sensor in timeout mode m_isSensorMeasurementAvailable = false; @@ -227,6 +254,9 @@ bool XsensMT::open(yarp::os::Searchable &config) // start thread m_sensorThread = std::thread(std::bind(&XsensMT::sensorReadLoop, this)); + // Create and attach callback handler to device + m_xsensDevice->addCallbackHandler(&m_callback); + return true; } @@ -237,7 +267,18 @@ bool XsensMT::close() { m_sensorThread.join(); } - return true; + + if(m_xsensControl) + { + yDebug("xsensmt: Closing Xsens port %s.", m_portInfo.portName().toStdString().c_str()); + m_xsensControl->closePort(m_portInfo.portName().toStdString()); + + yDebug("xsensmt: Freeing Xsens control object."); + m_xsensControl->destruct(); + m_xsensControl = nullptr; + } + + return true; } yarp::os::Stamp XsensMT::getLastInputStamp() @@ -248,9 +289,6 @@ yarp::os::Stamp XsensMT::getLastInputStamp() void XsensMT::sensorReadLoop() { - XsByteArray data; - std::deque msgs; - XsEuler euler; XsVector acc; XsVector gyro; @@ -258,26 +296,10 @@ void XsensMT::sensorReadLoop() while (!m_isDeviceClosing) { - m_xsensDevice.readDataToBuffer(data); - m_xsensDevice.processBufferedData(data, msgs); - for (std::deque::iterator it = msgs.begin(); it != msgs.end(); ++it) + if (m_callback.packetAvailable()) { // Retrieve a packet - XsDataPacket packet; - if ((*it).getMessageId() == XMID_MtData) { - LegacyDataPacket lpacket(1, false); - lpacket.setMessage((*it)); - lpacket.setXbusSystem(false); - lpacket.setDeviceId(m_portInfo.deviceId(), 0); - lpacket.setDataFormat(XOM_Orientation, XOS_OrientationMode_Quaternion,0); - yError("xsensmt: Legacy packet received, but the legacy packet is not currently supported by the driver. Ignoring message."); - continue; - //XsDataPacket_assignFromLegacyDataPacket(&packet, &lpacket, 0); - } - else if ((*it).getMessageId() == XMID_MtData2) { - packet.setMessage((*it)); - packet.setDeviceId(m_portInfo.deviceId()); - } + XsDataPacket packet = m_callback.getNextPacket(); // Get the euler data (for compatibility with the old xsensmtx icub-main driver) euler = packet.orientationEuler(); @@ -328,7 +350,6 @@ void XsensMT::sensorReadLoop() m_lastStamp.update(yarp::os::SystemClock::nowSystem()); m_bufferMutex.unlock(); } - msgs.clear(); // The Xsens API does not support a blocking read, so this delay // is necessary to avoid busy waiting, but influence the latency // of when a measurement is available in the YARP interface @@ -336,25 +357,6 @@ void XsensMT::sensorReadLoop() } } -bool XsensMT::setFilterProfile(const uint16_t profile) -{ - XsMessage snd(XMID_SetFilterProfile, 2), rcv; - snd.setDataShort(profile, 0); - m_xsensDevice.writeMessage(snd); - return m_xsensDevice.waitForMessage(XMID_SetFilterProfileAck, rcv); -} - -bool XsensMT::setOptionFlags(const uint32_t setFlags, const uint32_t clearFlags) -{ - XsMessage snd(XMID_SetOptionFlags, 8), rcv; - // SetFlags (4 Bytes) - snd.setDataLong(setFlags, 0); - // ClearFlags (4 bytes) - snd.setDataLong(clearFlags, 4); - m_xsensDevice.writeMessage(snd); - return m_xsensDevice.waitForMessage(XMID_SetOptionFlagsAck, rcv); -} - yarp::dev::MAS_status XsensMT::genericGetStatus(size_t sens_index) const { if (sens_index != 0) diff --git a/xsensmt/XsensMT.h b/xsensmt/XsensMT.h index d9dec56..14db65b 100644 --- a/xsensmt/XsensMT.h +++ b/xsensmt/XsensMT.h @@ -8,7 +8,10 @@ #ifndef XSENS_MT_YARP_DRIVER #define XSENS_MT_YARP_DRIVER -#include "deviceclass.h" +#include +#include +#include +#include #include #include @@ -21,6 +24,7 @@ #include #include #include +#include namespace yarp { @@ -30,6 +34,59 @@ namespace dev } } +/** + * \section CallbackHandler Controls reading data from XsDevice object + * \brief This class is copied from the Public Xsens device API example MTi receive data. +**/ +class CallbackHandler : public XsCallback +{ +public: + CallbackHandler(size_t maxBufferSize = 5) + : m_maxNumberOfPacketsInBuffer(maxBufferSize) + , m_numberOfPacketsInBuffer(0) + { + } + + virtual ~CallbackHandler() throw() + { + } + + bool packetAvailable() const + { + xsens::Lock locky(&m_mutex); + return m_numberOfPacketsInBuffer > 0; + } + + XsDataPacket getNextPacket() + { + assert(packetAvailable()); + xsens::Lock locky(&m_mutex); + XsDataPacket oldestPacket(m_packetBuffer.front()); + m_packetBuffer.pop_front(); + --m_numberOfPacketsInBuffer; + return oldestPacket; + } + +protected: + void onLiveDataAvailable(XsDevice*, const XsDataPacket* packet) override + { + xsens::Lock locky(&m_mutex); + assert(packet != 0); + while (m_numberOfPacketsInBuffer >= m_maxNumberOfPacketsInBuffer) + (void)getNextPacket(); + + m_packetBuffer.push_back(*packet); + ++m_numberOfPacketsInBuffer; + assert(m_numberOfPacketsInBuffer <= m_maxNumberOfPacketsInBuffer); + } +private: + mutable xsens::Mutex m_mutex; + + size_t m_maxNumberOfPacketsInBuffer; + size_t m_numberOfPacketsInBuffer; + std::list m_packetBuffer; +}; + /** * @@ -272,12 +329,6 @@ class yarp::dev::XsensMT : public yarp::dev::IGenericSensor, double m_outputPeriod; double m_outputFrequency; - // Send a SetFilterProfile message to set the filter profile - bool setFilterProfile(const uint16_t profile); - - // Send a SetOptionFlags message to set the option flags - bool setOptionFlags(const uint32_t setFlags, const uint32_t clearFlags); - int m_nchannels{12}; double m_timeoutInSecond{0.1}; @@ -288,8 +339,10 @@ class yarp::dev::XsensMT : public yarp::dev::IGenericSensor, bool m_isSensorMeasurementAvailable{false}; // Interface exposed by the Xsens MT Software suite - DeviceClass m_xsensDevice; + XsControl* m_xsensControl{nullptr}; + XsDevice* m_xsensDevice{nullptr}; XsPortInfo m_portInfo; + CallbackHandler m_callback; yarp::os::Stamp m_lastReadStamp; diff --git a/xsensmt/exampleScanDevicesAndReceiveData.cpp b/xsensmt/exampleScanDevicesAndReceiveData.cpp new file mode 100644 index 0000000..7c4d629 --- /dev/null +++ b/xsensmt/exampleScanDevicesAndReceiveData.cpp @@ -0,0 +1,298 @@ + +// Copyright (c) 2003-2022 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. +// + +//-------------------------------------------------------------------------------- +// Public Xsens device API C++ example MTi receive data. +//-------------------------------------------------------------------------------- +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +Journaller* gJournal = 0; + +using namespace std; + +class CallbackHandler : public XsCallback +{ +public: + CallbackHandler(size_t maxBufferSize = 5) + : m_maxNumberOfPacketsInBuffer(maxBufferSize) + , m_numberOfPacketsInBuffer(0) + { + } + + virtual ~CallbackHandler() throw() + { + } + + bool packetAvailable() const + { + xsens::Lock locky(&m_mutex); + return m_numberOfPacketsInBuffer > 0; + } + + XsDataPacket getNextPacket() + { + assert(packetAvailable()); + xsens::Lock locky(&m_mutex); + XsDataPacket oldestPacket(m_packetBuffer.front()); + m_packetBuffer.pop_front(); + --m_numberOfPacketsInBuffer; + return oldestPacket; + } + +protected: + void onLiveDataAvailable(XsDevice*, const XsDataPacket* packet) override + { + xsens::Lock locky(&m_mutex); + assert(packet != 0); + while (m_numberOfPacketsInBuffer >= m_maxNumberOfPacketsInBuffer) + (void)getNextPacket(); + + m_packetBuffer.push_back(*packet); + ++m_numberOfPacketsInBuffer; + assert(m_numberOfPacketsInBuffer <= m_maxNumberOfPacketsInBuffer); + } +private: + mutable xsens::Mutex m_mutex; + + size_t m_maxNumberOfPacketsInBuffer; + size_t m_numberOfPacketsInBuffer; + list m_packetBuffer; +}; + +//-------------------------------------------------------------------------------- +int main(void) +{ + cout << "Creating XsControl object..." << endl; + XsControl* control = XsControl::construct(); + assert(control != 0); + + // Lambda function for error handling + auto handleError = [=](string errorString) + { + control->destruct(); + cout << errorString << endl; + cout << "Press [ENTER] to continue." << endl; + cin.get(); + return -1; + }; + + cout << "Scanning for devices..." << endl; + XsPortInfoArray portInfoArray = XsScanner::scanPorts(); + + // Find an MTi device + XsPortInfo mtPort; + for (auto const &portInfo : portInfoArray) + { + if (portInfo.deviceId().isMti() || portInfo.deviceId().isMtig()) + { + mtPort = portInfo; + break; + } + } + + if (mtPort.empty()) + return handleError("No MTi device found. Aborting."); + + cout << "Found a device with ID: " << mtPort.deviceId().toString().toStdString() << " @ port: " << mtPort.portName().toStdString() << ", baudrate: " << mtPort.baudrate() << endl; + + cout << "Opening port..." << endl; + if (!control->openPort(mtPort.portName().toStdString(), mtPort.baudrate())) + return handleError("Could not open port. Aborting."); + + // Get the device object + XsDevice* device = control->device(mtPort.deviceId()); + assert(device != 0); + + cout << "Device: " << device->productCode().toStdString() << ", with ID: " << device->deviceId().toString() << " opened." << endl; + + // Create and attach callback handler to device + CallbackHandler callback; + device->addCallbackHandler(&callback); + + // Put the device into configuration mode before configuring the device + cout << "Putting device into configuration mode..." << endl; + if (!device->gotoConfig()) + return handleError("Could not put device into configuration mode. Aborting."); + + cout << "Configuring the device..." << endl; + + // Important for Public XDA! + // Call this function if you want to record a mtb file: + device->readEmtsAndDeviceConfiguration(); + + XsOutputConfigurationArray configArray; + configArray.push_back(XsOutputConfiguration(XDI_PacketCounter, 0)); + configArray.push_back(XsOutputConfiguration(XDI_SampleTimeFine, 0)); + + if (device->deviceId().isImu()) + { + configArray.push_back(XsOutputConfiguration(XDI_Acceleration, 100)); + configArray.push_back(XsOutputConfiguration(XDI_RateOfTurn, 100)); + configArray.push_back(XsOutputConfiguration(XDI_MagneticField, 100)); + } + else if (device->deviceId().isVru() || device->deviceId().isAhrs()) + { + configArray.push_back(XsOutputConfiguration(XDI_Quaternion, 100)); + } + else if (device->deviceId().isGnss()) + { + configArray.push_back(XsOutputConfiguration(XDI_Quaternion, 100)); + configArray.push_back(XsOutputConfiguration(XDI_LatLon, 100)); + configArray.push_back(XsOutputConfiguration(XDI_AltitudeEllipsoid, 100)); + configArray.push_back(XsOutputConfiguration(XDI_VelocityXYZ, 100)); + } + else + { + return handleError("Unknown device while configuring. Aborting."); + } + + if (!device->setOutputConfiguration(configArray)) + return handleError("Could not configure MTi device. Aborting."); + + cout << "Creating a log file..." << endl; + string logFileName = "logfile.mtb"; + if (device->createLogFile(logFileName) != XRV_OK) + return handleError("Failed to create a log file. Aborting."); + else + cout << "Created a log file: " << logFileName.c_str() << endl; + + cout << "Putting device into measurement mode..." << endl; + if (!device->gotoMeasurement()) + return handleError("Could not put device into measurement mode. Aborting."); + + cout << "Starting recording..." << endl; + if (!device->startRecording()) + return handleError("Failed to start recording. Aborting."); + + cout << "\nMain loop. Recording data for 10 seconds." << endl; + cout << string(79, '-') << endl; + + int64_t startTime = XsTime::timeStampNow(); + while (XsTime::timeStampNow() - startTime <= 10000) + { + if (callback.packetAvailable()) + { + cout << setw(5) << fixed << setprecision(2); + + // Retrieve a packet + XsDataPacket packet = callback.getNextPacket(); + if (packet.containsCalibratedData()) + { + XsVector acc = packet.calibratedAcceleration(); + cout << "\r" + << "Acc X:" << acc[0] + << ", Acc Y:" << acc[1] + << ", Acc Z:" << acc[2]; + + XsVector gyr = packet.calibratedGyroscopeData(); + cout << " |Gyr X:" << gyr[0] + << ", Gyr Y:" << gyr[1] + << ", Gyr Z:" << gyr[2]; + + XsVector mag = packet.calibratedMagneticField(); + cout << " |Mag X:" << mag[0] + << ", Mag Y:" << mag[1] + << ", Mag Z:" << mag[2]; + } + + if (packet.containsOrientation()) + { + XsQuaternion quaternion = packet.orientationQuaternion(); + cout << "\r" + << "q0:" << quaternion.w() + << ", q1:" << quaternion.x() + << ", q2:" << quaternion.y() + << ", q3:" << quaternion.z(); + + XsEuler euler = packet.orientationEuler(); + cout << " |Roll:" << euler.roll() + << ", Pitch:" << euler.pitch() + << ", Yaw:" << euler.yaw(); + } + + if (packet.containsLatitudeLongitude()) + { + XsVector latLon = packet.latitudeLongitude(); + cout << " |Lat:" << latLon[0] + << ", Lon:" << latLon[1]; + } + + if (packet.containsAltitude()) + cout << " |Alt:" << packet.altitude(); + + if (packet.containsVelocity()) + { + XsVector vel = packet.velocity(XDI_CoordSysEnu); + cout << " |E:" << vel[0] + << ", N:" << vel[1] + << ", U:" << vel[2]; + } + + cout << flush; + } + XsTime::msleep(0); + } + cout << "\n" << string(79, '-') << "\n"; + cout << endl; + + cout << "Stopping recording..." << endl; + if (!device->stopRecording()) + return handleError("Failed to stop recording. Aborting."); + + cout << "Closing log file..." << endl; + if (!device->closeLogFile()) + return handleError("Failed to close log file. Aborting."); + + cout << "Closing port..." << endl; + control->closePort(mtPort.portName().toStdString()); + + cout << "Freeing XsControl object..." << endl; + control->destruct(); + + cout << "Successful exit." << endl; + + cout << "Press [ENTER] to continue." << endl; + cin.get(); + + return 0; +}