XsensMTiModule.cpp

Go to the documentation of this file.

/*
 *  Player - One Hell of a Robot Server
 *  Copyright (C) 2006 Radu Bogdan Rusu (rusu@cs.tum.edu)
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */
/*
 Desc: Driver for XSens MTx/MTi IMU. CMTComm class borrowed from XSens under GPL.
 Author: Radu Bogdan Rusu
 Date: 1 Aug 2006
 */
// MTComm.cpp: implementation of the CMTComm class.
//
// Version 1.2.0
// Public release
//
// v1.2.0
// 27-02-2006 - Renamed Xbus class to Motion Tracker C++ Communication class, short MTComm
//            - Defines XBRV_* accordingly renamed to MTRV_*
//            - Fixed device length not correct for bid 0 when using Xbus Master and setDeviceMode function
//
// v1.1.7
// 15-02-2006 - Added fixed point signed 12.20 dataformat support
//              Added selective calibrated data output per sensor type support
//              Added outputmode temperature support
//              Fixed warning C4244: '=' : conversion from '' to '', possible loss of data
// v1.1.6
// 25-01-2006 - Added escape function for CLRDTR, CLRRTS, SETDTR, SETRTS, SETXOFF, SETXON, SETBREAK, CLRBREAK
//
// v1.1.5
// 14-11-2005 - Made swapEndian a static member function, Job Mulder
//
// v1.1.4
// 08-11-2005 - Changed practically all uses of m_timeOut into uses of the new m_clkEnd
//            - Changed COM timeout in win32 to return immediately if data is available,
//              but wait 1ms otherwise
//
// v1.1.3
// 18-10-2005 - Added MID_REQPRODUCTCODE, MID_REQ/SETTRANSMITDELAY
//            - Added MTRV_TIMEOUTNODATA indicating timeout occurred due to no data read
//
// v1.1.2
// 16-09-2005 - Added eMTS version 0.1->1.0 changes (EMTS_FACTORYMODE)
//            - Added factory output mode defines
//
// v1.1.1
// 01-09-2005 - Added defines for Extended output mode
//            - Added reqSetting (byte array in + out & no param variant)
//
// v1.1
// 08-08-2005 - Added file read and write support
//            - Added functions for data retrieval (getValue etc)
//                for easy data retrieval of acc, gyr, mag etc
//            - ReadMessageRaw:
//              - added a temporary buffer for unprocessed bytes
//              - check for invalid length messages
//            - Changed BID_MT into 1 and added BID_MASTER (=0xFF)
//            - Changed various ....SerialPort functions to .....Port
//            - Changed mtSendMessage functions to mtWriteMessage
//            - Added numerous defines
//            - Deleted obsolete functions
//            - Changed function getLastErrorCode into getLastDeviceError
//            - Changed OpenPort function for compatiblity with Bluetooth ports
//            - Added workaround for lockup of USB driver (close function)
//            - Added workaround for clock() problem with read function of USB driver
//
// v1.0.2
// 29-06-2005 - Inserted initSerialPort with devicename input
//            - Changed return value defines names from X_ to MTRV_
//            - Removed unneeded includes for linux
//
// v1.0.1
// 22-06-2005 - Fixed ReqSetting functions (byte array & param variant)
//              mtSendRawString had wrong length input
//
// v1.0.0
// 20-06-2005 - Initial release
//
// ----------------------------------------------------------------------------
//  This file is an Xsens Code Examples.
//
//  Copyright (C) Xsens Technologies B.V., 2005.
//
//  This source code is intended only as a example of the implementation
//  of the Xsens MT Communication protocol.
//  It was written for cross platform capabilities.
//
//  THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY
//  KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
//  IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A
//  PARTICULAR PURPOSE.
//////////////////////////////////////////////////////////////////////
#include "XsensMTiModule.h"
 
#ifdef _IMU_USE_XSENS_DEVICE_
 
#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#define new DEBUG_NEW
#endif
 
#ifndef CRTSCTS
#ifdef IHFLOW
#ifdef OHFLOW
#define CRTSCTS ((IHFLOW) | (OHFLOW))
#endif
#endif
#endif
 
namespace IMU::Xsens
{
    //////////////////////////////////////////////////////////////////////
    // Construction/Destruction
    //////////////////////////////////////////////////////////////////////
    //
    CXsensMTiModule::CXsensMTiModule()
    {
        m_handle = -1;
        m_portOpen = false;
        m_fileOpen = false;
        m_deviceError = 0; // No error
        m_retVal = MTRV_OK;
        m_timeOut = TO_DEFAULT;
        m_nTempBufferLen = 0;
        m_clkEnd = 0;
 
        for (int i = 0; i < MAXDEVICES + 1; i++)
        {
            m_storedOutputMode[i] = INVALIDSETTINGVALUE;
            m_storedOutputSettings[i] = INVALIDSETTINGVALUE;
            m_storedDataLength[i] = 0;
        }
    }
 
    CXsensMTiModule::~CXsensMTiModule()
    {
        close();
    }
 
    ////////////////////////////////////////////////////////////////////
    // clockms
    //
    // Calculates the processor time used by the calling process.
    // For linux use gettimeofday instead of clock() function
    // When using read from FTDI serial port in a loop the
    // clock() function very often keeps returning 0.
    //
    // Output
    //   returns processor time in milliseconds
    //
    clock_t
    CXsensMTiModule::clockms()
    {
        clock_t clk;
#ifdef WIN32
        clk = clock(); // Get current processor time
#if (CLOCKS_PER_SEC != 1000)
        clk /= (CLOCKS_PER_SEC / 1000);
        //  clk = (clk * 1000) / CLOCKS_PER_SEC;
#endif
#else
        struct timeval tv;
        struct timezone tz;
        gettimeofday(&tv, &tz);
        clk = tv.tv_sec * 1000 + (tv.tv_usec / 1000);
#endif
        return clk;
    }
 
    ////////////////////////////////////////////////////////////////////
    // openPort (serial port number as input parameter)
    //
    // Opens a 'live' connection to a MT or XM
    //
    // Input
    //   portNumber  : number of serial port to open (1-99)
    //   baudrate    : baudrate value (One of the PBR_* defines), default = PBR_115K2
    //   inqueueSize : size of input queue, default = 4096
    //   outqueueSize: size of output queue, default = 1024
    //
    // Output
    //   returns MTRV_OK if serial port is successfully opened, else MTRV_INPUTCANNOTBEOPENED
    //
    short
    CXsensMTiModule::openPort(const int portNumber,
                              const unsigned long baudrate,
                              const unsigned long /*inqueueSize*/,
                              const unsigned long /*outqueueSize*/)
    {
        m_clkEnd = 0;
 
        if (m_fileOpen || m_portOpen)
        {
            return (m_retVal = MTRV_ANINPUTALREADYOPEN);
        }
 
#ifdef WIN32
        char pchFileName[10];
 
        sprintf(pchFileName, "\\\\.\\COM%d", portNumber); // Create file name
 
        m_handle =
            CreateFile(pchFileName, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL);
 
        if (m_handle == INVALID_HANDLE_VALUE)
        {
            return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
        }
 
        // Once here, port is open
        m_portOpen = true;
 
        //Get the current state & then change it
        DCB dcb;
 
        GetCommState(m_handle, &dcb); // Get current state
 
        dcb.BaudRate = baudrate; // Setup the baud rate
        dcb.Parity = NOPARITY; // Setup the Parity
        dcb.ByteSize = 8; // Setup the data bits
        dcb.StopBits = TWOSTOPBITS; // Setup the stop bits
        dcb.fDsrSensitivity = FALSE; // Setup the flow control
        dcb.fOutxCtsFlow = FALSE; // NoFlowControl:
        dcb.fOutxDsrFlow = FALSE;
        dcb.fOutX = FALSE;
        dcb.fInX = FALSE;
 
        if (!SetCommState(m_handle, (LPDCB)&dcb))
        {
            // Set new state
            // Bluetooth ports cannot always be opened with 2 stopbits
            // Now try to open port with 1 stopbit.
            dcb.StopBits = ONESTOPBIT;
 
            if (!SetCommState(m_handle, (LPDCB)&dcb))
            {
                CloseHandle(m_handle);
                m_handle = INVALID_HANDLE_VALUE;
                m_portOpen = false;
                return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
            }
        }
 
        // Set COM timeouts
        COMMTIMEOUTS CommTimeouts;
 
        GetCommTimeouts(m_handle, &CommTimeouts); // Fill CommTimeouts structure
 
        // immediate return if data is available, wait 1ms otherwise
        CommTimeouts.ReadTotalTimeoutConstant = 1;
        CommTimeouts.ReadIntervalTimeout = MAXDWORD;
        CommTimeouts.ReadTotalTimeoutMultiplier = MAXDWORD;
 
        // immediate return whether data is available or not
        //  CommTimeouts.ReadTotalTimeoutConstant = 0;
        //  CommTimeouts.ReadIntervalTimeout = MAXDWORD;
        //  CommTimeouts.ReadTotalTimeoutMultiplier = 0;
 
        SetCommTimeouts(m_handle, &CommTimeouts); // Set CommTimeouts structure
 
        // Other initialization functions
        EscapeCommFunction(m_handle, SETRTS); // Enable RTS (for Xbus Master use)
        SetupComm(m_handle, inqueueSize, outqueueSize); // Set queue size
 
        // Remove any 'old' data in buffer
        PurgeComm(m_handle, PURGE_TXCLEAR | PURGE_RXCLEAR);
 
        return (m_retVal = MTRV_OK);
#else
        char chPort[32];
        struct termios options;
 
        /* Open port */
        sprintf(chPort, "/dev/ttyS%d", (portNumber - 1));
        m_handle = open(chPort, 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_retVal = MTRV_INPUTCANNOTBEOPENED);
        }
 
        // Once here, port is open
        m_portOpen = true;
 
        /* Start configuring of port for non-canonical transfer mode */
        // Get current options for the port
        tcgetattr(m_handle, &options);
 
        // Set baudrate.
        cfsetispeed(&options, baudrate);
        cfsetospeed(&options, baudrate);
 
        // Enable the receiver and set local mode
        options.c_cflag |= (CLOCAL | CREAD);
        // Set character size to data bits and set no parity Mask the characte size bits
        options.c_cflag &= ~(CSIZE | PARENB);
        options.c_cflag |= CS8; // Select 8 data bits
        options.c_cflag |= CSTOPB; // send 2 stop bits
        // Disable hardware flow control
        options.c_cflag &= ~CRTSCTS;
        options.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
        // Disable software flow control
        options.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
        // Set Raw output
        options.c_oflag &= ~OPOST;
        // Timeout 0.005 sec for first byte, read minimum of 0 bytes
        options.c_cc[VMIN] = 0;
        options.c_cc[VTIME] = 5;
 
        // Set the new options for the port
        tcsetattr(m_handle, TCSANOW, &options);
 
        tcflush(m_handle, TCIOFLUSH);
 
        return (m_retVal = MTRV_OK);
#endif
    }
 
    ////////////////////////////////////////////////////////////////////
    // openPort (string as input parameter)
    //
    // Opens a 'live' connection to a MT or XM
    //
    // Input
    //   portName    : device name of serial port ("/dev/ttyUSB0" or "\\\\.\\COM1")
    //   baudrate    : baudrate value (One of the PBR_* defines), default = PBR_115K2
    //   inqueueSize : size of input queue, default = 4096
    //   outqueueSize: size of output queue, default = 1024
    //
    // Output
    //   returns MTRV_OK if serial port is successfully opened, else MTRV_INPUTCANNOTBEOPENED
    //
    short
    CXsensMTiModule::openPort(const char* portName,
                              const unsigned long baudrate,
                              const unsigned long /*inqueueSize*/,
                              const unsigned long /*outqueueSize*/)
    {
        m_clkEnd = 0;
 
        if (m_fileOpen || m_portOpen)
        {
            return (m_retVal = MTRV_ANINPUTALREADYOPEN);
        }
 
#ifdef WIN32
        m_handle =
            CreateFile(portName, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL);
 
        if (m_handle == INVALID_HANDLE_VALUE)
        {
            return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
        }
 
        // Once here, port is open
        m_portOpen = true;
 
        //Get the current state & then change it
        DCB dcb;
 
        GetCommState(m_handle, &dcb); // Get current state
 
        dcb.BaudRate = baudrate; // Setup the baud rate
        dcb.Parity = NOPARITY; // Setup the Parity
        dcb.ByteSize = 8; // Setup the data bits
        dcb.StopBits = TWOSTOPBITS; // Setup the stop bits
        dcb.fDsrSensitivity = FALSE; // Setup the flow control
        dcb.fOutxCtsFlow = FALSE; // NoFlowControl:
        dcb.fOutxDsrFlow = FALSE;
        dcb.fOutX = FALSE;
        dcb.fInX = FALSE;
 
        if (!SetCommState(m_handle, (LPDCB)&dcb))
        {
            // Set new state
            // Bluetooth ports cannot always be opened with 2 stopbits
            // Now try to open port with 1 stopbit.
            dcb.StopBits = ONESTOPBIT;
 
            if (!SetCommState(m_handle, (LPDCB)&dcb))
            {
                CloseHandle(m_handle);
                m_handle = INVALID_HANDLE_VALUE;
                m_portOpen = false;
                return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
            }
        }
 
        // Set COM timeouts
        COMMTIMEOUTS CommTimeouts;
 
        GetCommTimeouts(m_handle, &CommTimeouts); // Fill CommTimeouts structure
 
        // immediate return if data is available, wait 1ms otherwise
        CommTimeouts.ReadTotalTimeoutConstant = 1;
        CommTimeouts.ReadIntervalTimeout = MAXDWORD;
        CommTimeouts.ReadTotalTimeoutMultiplier = MAXDWORD;
 
        // immediate return whether data is available or not
        //  CommTimeouts.ReadTotalTimeoutConstant = 0;
        //  CommTimeouts.ReadIntervalTimeout = MAXDWORD;
        //  CommTimeouts.ReadTotalTimeoutMultiplier = 0;
        SetCommTimeouts(m_handle, &CommTimeouts); // Set CommTimeouts structure
 
        // Other initialization functions
        EscapeCommFunction(m_handle, SETRTS); // Enable RTS (for Xbus Master use)
        SetupComm(m_handle, inqueueSize, outqueueSize); // Set queue size
 
        // Remove any 'old' data in buffer
        PurgeComm(m_handle, PURGE_TXCLEAR | PURGE_RXCLEAR);
 
        return (m_retVal = MTRV_OK);
#else
        struct termios options;
 
        /* Open port */
 
        m_handle = open(portName, 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_retVal = MTRV_INPUTCANNOTBEOPENED);
        }
 
        // Once here, port is open
        m_portOpen = true;
 
        /* Start configuring of port for non-canonical transfer mode */
        // Get current options for the port
        tcgetattr(m_handle, &options);
 
        // Set baudrate.
        cfsetispeed(&options, baudrate);
        cfsetospeed(&options, baudrate);
 
        // Enable the receiver and set local mode
        options.c_cflag |= (CLOCAL | CREAD);
        // Set character size to data bits and set no parity Mask the characte size bits
        options.c_cflag &= ~(CSIZE | PARENB);
        options.c_cflag |= CS8; // Select 8 data bits
        options.c_cflag |= CSTOPB; // send 2 stop bits
        // Disable hardware flow control
        options.c_cflag &= ~CRTSCTS;
        options.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
        // Disable software flow control
        options.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
        // Set Raw output
        options.c_oflag &= ~OPOST;
        // Timeout 0.005 sec for first byte, read minimum of 0 bytes
        options.c_cc[VMIN] = 0;
        options.c_cc[VTIME] = 5;
 
        // Set the new options for the port
        tcsetattr(m_handle, TCSANOW, &options);
 
        tcflush(m_handle, TCIOFLUSH);
 
        return (m_retVal = MTRV_OK);
#endif
    }
 
    ////////////////////////////////////////////////////////////////////
    // openFile
    //
    // Open file for reading and writing
    // Filepos is at start of file
    //
    // Input
    //   fileName    : file name including path
    //   createAlways: empties the log file, if existing
    //
    // Output
    //   returns MTRV_OK if the file is opened
    //   returns MTRV_INPUTCANNOTBEOPENED if the file can not be opened
    //   returns MTRV_ANINPUTALREADYOPEN if a serial port / file is already opened
    //
    short
    CXsensMTiModule::openFile(const char* fileName, bool createAlways)
    {
        m_clkEnd = 0;
 
        if (m_portOpen || m_portOpen)
        {
            return (m_retVal = MTRV_ANINPUTALREADYOPEN);
        }
 
#ifdef WIN32
        DWORD disposition = OPEN_ALWAYS;
 
        if (createAlways == true)
        {
            disposition = CREATE_ALWAYS;
        }
 
        m_handle = CreateFile(
            fileName, GENERIC_READ | GENERIC_WRITE, FILE_SHARE_READ, NULL, disposition, 0, NULL);
 
        if (m_handle == INVALID_HANDLE_VALUE)
        {
            return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
        }
 
#else
        int openMode = O_RDWR | O_CREAT;
 
        if (createAlways == true)
        {
            openMode |= O_TRUNC;
        }
 
        m_handle = open(fileName, openMode, S_IRWXU);
 
        if (m_handle < 0)
        {
            return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
        }
 
#endif
        m_timeOut = 0; // No timeout when using file input
        m_fileOpen = true;
        return (m_retVal = MTRV_OK);
    }
 
    ////////////////////////////////////////////////////////////////////
    // isPortOpen
    //
    // Return if serial port is open or not
    //
    bool
    CXsensMTiModule::isPortOpen()
    {
        return m_portOpen;
    }
 
    ////////////////////////////////////////////////////////////////////
    // isFileOpen
    //
    // Return if serial port is open or not
    //
    bool
    CXsensMTiModule::isFileOpen()
    {
        return m_fileOpen;
    }
 
    ////////////////////////////////////////////////////////////////////
    // readData
    //
    // Reads bytes from serial port or file
    //
    // Input
    //   msgBuffer      : pointer to buffer in which next string will be stored
    //   nBytesToRead   : number of buffer bytes to read from serial port
    //   retval         : return value, either MTRV_OK, MTRV_ENDOFFILE or MTRV_NOINPUTINITIALIZED
    //
    // Output
    //   number of bytes actually read
    int
    CXsensMTiModule::readData(unsigned char* msgBuffer, const int nBytesToRead)
    {
        //if(!m_fileOpen && !m_portOpen)
        if (!(m_portOpen || m_fileOpen))
        {
            m_retVal = MTRV_NOINPUTINITIALIZED;
            return 0;
        }
 
#ifdef WIN32
        DWORD nBytesRead;
        BOOL retval = ReadFile(m_handle, msgBuffer, nBytesToRead, &nBytesRead, NULL);
 
        if (retval && nBytesRead == 0 && m_fileOpen)
        {
            m_retVal = MTRV_ENDOFFILE;
        }
        else
        {
            m_retVal = MTRV_OK;
        }
 
        return nBytesRead;
#else
        const int nBytesRead = read(m_handle, msgBuffer, nBytesToRead);
 
        m_retVal = MTRV_OK;
 
        if (nBytesRead)
        {
            return nBytesRead;
        }
        else if (m_fileOpen)
        {
            m_retVal = MTRV_ENDOFFILE;
        }
 
        return nBytesRead;
        /*
         if (nBytesRead == 0 && m_fileOpen)
         {
         nBytesRead = 0;
         m_retVal = MTRV_ENDOFFILE;
         }
         else
         m_retVal = MTRV_OK;
         return nBytesRead;*/
 
        // In Linux it is sometimes better to read per byte instead of a block of bytes at once
        // Use this block if experiencing problems with the above code
        /*
         int j = 0; // Index in buffer for read data
         int k = 0; // Timeout factor
         int nRead = 0; // Bytes read from port, return by read function
 
         do {
         nRead = read(m_handle, &msgBuffer[j], 1);
         if (nRead == 1) {  // Byte read
         k = 0;
         j++;
         }
         else {
         k++;
         }
 
         if (k == 3)    { // Timeout, too long no data read from port
         return nRead;
         }
         }
         while (j < nBytesToRead);
 
         return j;
         */
#endif
    }
 
    ////////////////////////////////////////////////////////////////////
    // writeData
    //
    // Writes bytes to serial port (to do: file)
    //
    // Input
    //   msgBuffer      : a pointer to a char buffer containing
    //                    the characters to be written to serial port
    //   nBytesToWrite  : number of buffer bytes to write to serial port
    //
    // Output
    //   number of bytes actually written
    //
    // The MTComm return value is != MTRV_OK if serial port is closed
    int
    CXsensMTiModule::writeData(const unsigned char* msgBuffer, const int nBytesToWrite)
    {
        if (!m_fileOpen && !m_portOpen)
        {
            m_retVal = MTRV_NOINPUTINITIALIZED;
            return 0;
        }
 
        m_retVal = MTRV_OK;
#ifdef WIN32
        DWORD nBytesWritten;
        WriteFile(m_handle, msgBuffer, nBytesToWrite, &nBytesWritten, NULL);
        return nBytesWritten;
#else
        return write(m_handle, msgBuffer, nBytesToWrite);
#endif
    }
 
    ////////////////////////////////////////////////////////////////////
    // flush
    //
    // Remove any 'old' data in COM port buffer and flushes internal
    //   temporary buffer
    //
    void
    CXsensMTiModule::flush()
    {
        if (m_portOpen)
        {
#ifdef WIN32
            // Remove any 'old' data in buffer
            PurgeComm(m_handle, PURGE_TXCLEAR | PURGE_RXCLEAR);
#else
            tcflush(m_handle, TCIOFLUSH);
#endif
        }
 
        m_nTempBufferLen = 0;
        m_retVal = MTRV_OK;
    }
 
    ////////////////////////////////////////////////////////////////////
    // escape
    //
    // Directs a COM port to perform an extended function
    //
    // Input
    //  function    : Windows define. Can be one of following:
    //                CLRDTR, CLRRTS, SETDTR, SETRTS, SETXOFF, SETXON, SETBREAK, CLRBREAK
    void
    CXsensMTiModule::escape(unsigned long /*function*/)
    {
#ifdef WIN32
        EscapeCommFunction(m_handle, function);
#else
#endif
    }
 
    ////////////////////////////////////////////////////////////////////
    // setPortQueueSize
    //
    // Set input and output buffer size of serial port
    //
    void
    CXsensMTiModule::setPortQueueSize(const unsigned long /*inqueueSize  = 4096 */,
                                      const unsigned long /*outqueueSize  = 1024 */)
    {
        if (m_portOpen)
        {
#ifdef WIN32
            SetupComm(m_handle, inqueueSize, outqueueSize); // Set queue size
#else
            // Not yet implemented
#endif
        }
 
        m_retVal = MTRV_OK;
    }
 
    ////////////////////////////////////////////////////////////////////
    // setFilePos
    //
    // Sets the current position of the file pointer for file input
    //
    // Input
    //   relPos     : 32-bit value specifying the relative move in bytes
    //                  origin is specified in moveMethod
    //   moveMethod : FILEPOS_BEGIN, FILEPOS_CURRENT or FILEPOS_END
    // Output
    //
    //   returns MTRV_OK if file pointer is successfully set
    //
    short
    CXsensMTiModule::setFilePos(long relPos, unsigned long moveMethod)
    {
#ifdef WIN32
 
        if (m_fileOpen)
        {
            if (SetFilePointer(m_handle, relPos, NULL, moveMethod) != INVALID_SET_FILE_POINTER)
            {
                return (m_retVal = MTRV_OK);
            }
        }
 
#else
 
        if (m_fileOpen)
        {
            if (lseek(m_handle, relPos, moveMethod) != -1)
            {
                return (m_retVal = MTRV_OK);
            }
        }
 
#endif
        return (m_retVal = MTRV_NOTSUCCESSFUL);
    }
 
    ////////////////////////////////////////////////////////////////////
    // getFileSize
    //
    // Retrieves the file size of the opened file
    //
    // Input
    // Output
    //   fileSize  : Number of bytes of the current file
    //
    //   returns MTRV_OK if successful
    //
    short
    CXsensMTiModule::getFileSize(unsigned long& fileSize)
    {
#ifdef WIN32
 
        if (m_fileOpen)
        {
            if ((fileSize = GetFileSize(m_handle, NULL)) != INVALID_FILE_SIZE)
            {
                return (m_retVal = MTRV_OK);
            }
        }
 
#else
 
        if (m_fileOpen)
        {
            struct stat buf;
 
            if (fstat(m_handle, &buf) == 0)
            {
                fileSize = buf.st_size;
                return (m_retVal = MTRV_OK);
            }
        }
 
#endif
        return (m_retVal = MTRV_NOTSUCCESSFUL);
    }
 
    ////////////////////////////////////////////////////////////////////
    // close
    //
    // Closes handle of file or serial port
    //
    short
    CXsensMTiModule::close()
    {
        if (m_portOpen || m_fileOpen)
        {
#ifdef WIN32
 
            if (m_portOpen)
            {
                // Because of USB-serial driver bug
                flush();
            }
 
            CloseHandle(m_handle);
#else
            ::close(m_handle);
#endif
        }
 
        m_fileOpen = m_portOpen = false;
        m_timeOut = TO_DEFAULT; // Restore timeout value (file input)
        m_clkEnd = 0;
        m_nTempBufferLen = 0;
        m_deviceError = 0; // No error
 
        for (int i = 0; i < MAXDEVICES + 1; i++)
        {
            m_storedOutputMode[i] = INVALIDSETTINGVALUE;
            m_storedOutputSettings[i] = INVALIDSETTINGVALUE;
            m_storedDataLength[i] = 0;
        }
 
        return (m_retVal = MTRV_OK);
    }
 
    ////////////////////////////////////////////////////////////////////
    // readMessage
    //
    // Reads the next message from serial port buffer or file.
    // To be read within current time out period
    //
    // Input
    // Output
    //   mid        : MessageID of message received
    //   data       : array pointer to data bytes (no header)
    //   dataLen    : number of data bytes read
    //   bid        : BID or address of message read (optional)
    //
    //   returns MTRV_OK if a message has been read else <>MTRV_OK
    //
    // Remarks
    //   allocate enough memory for message buffer
    //   use setTimeOut for different timeout value
    short
    CXsensMTiModule::readMessage(unsigned char& mid,
                                 unsigned char data[],
                                 short& dataLen,
                                 unsigned char* bid)
    {
        unsigned char buffer[MAXMSGLEN];
        short msgLen;
 
        if (!(m_fileOpen || m_portOpen))
        {
            return (m_retVal = MTRV_NOINPUTINITIALIZED);
        }
 
        if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
        {
            // Message read
            mid = buffer[IND_MID];
 
            if (bid != nullptr)
            {
                *bid = buffer[IND_BID];
            }
 
            if (buffer[IND_LEN] != EXTLENCODE)
            {
                dataLen = buffer[IND_LEN];
                memcpy(data, &buffer[IND_DATA0], dataLen);
            }
            else
            {
                dataLen = buffer[IND_LENEXTH] * 256 + buffer[IND_LENEXTL];
                memcpy(data, &buffer[IND_DATAEXT0], dataLen);
            }
        }
 
        return m_retVal;
    }
 
    ////////////////////////////////////////////////////////////////////
    // readDataMessage
    //
    // Read a MTData or XMData message from serial port (using TimeOut val)
    //
    // Input
    //   data       : pointer to buffer in which the DATA field of MTData/XMData is stored
    //   dataLen    : number of bytes in buffer (= number bytes in DATA field)
    // Output
    //   returns MTRV_OK if MTData / XMData message has been read else <>MTRV_OK
    //
    // Remarks
    //   allocate enough memory for data buffer
    //   use setTimeOut for different timeout value
    short
    CXsensMTiModule::readDataMessage(unsigned char data[], short& dataLen)
    {
        if (!(m_portOpen || m_fileOpen))
        {
            return (m_retVal = MTRV_NOINPUTINITIALIZED);
        }
 
        unsigned char buffer[MAXMSGLEN];
        short buflen;
 
        if (readMessageRaw(buffer, &buflen) == MTRV_OK)
        {
            if (buffer[IND_MID] == MID_MTDATA)
            {
                // MID_XMDATA is same
                if (buffer[IND_LEN] != EXTLENCODE)
                {
                    dataLen = buffer[IND_LEN];
                    memcpy(data, &buffer[IND_DATA0], dataLen);
                }
                else
                {
                    dataLen = buffer[IND_LENEXTH] * 256 + buffer[IND_LENEXTL];
                    memcpy(data, &buffer[IND_DATAEXT0], dataLen);
                }
 
                return (m_retVal = MTRV_OK);
            }
            else if (buffer[IND_MID] == MID_ERROR)
            {
                m_deviceError = buffer[IND_DATA0];
                return (m_retVal = MTRV_RECVERRORMSG); // Error message received
            }
        }
 
        return m_retVal;
    }
 
    ////////////////////////////////////////////////////////////////////
    // readMessageRaw
    //
    // Read a message from serial port for a certain period
    //
    // Input
    //   msgBuffer      : pointer to buffer in which next msg will be stored
    //   msgBufferLength: integer to number of bytes written in buffer (header + data + chksum)
    // Output
    //   returns MTRV_OK if a message has been read else <>MTRV_OK
    //
    // Remarks
    //   allocate enough memory for message buffer
    //   use setTimeOut for different timeout value
    short
    CXsensMTiModule::readMessageRaw(unsigned char* msgBuffer, short* msgBufferLength)
    {
        int state = 0;
        int nBytesToRead = 1;
        int nBytesRead = 0;
        int nOffset = 0;
        int nMsgDataLen = 0;
        int nMsgLen;
        unsigned char chCheckSum;
        bool Synced = false;
 
        if (!(m_portOpen || m_fileOpen))
        {
            return (m_retVal = MTRV_NOINPUTINITIALIZED);
        }
 
        // Copy previous read bytes if available
        if (m_nTempBufferLen > 0)
        {
            memcpy(msgBuffer, m_tempBuffer, m_nTempBufferLen);
            nBytesRead = m_nTempBufferLen;
            m_nTempBufferLen = 0;
        }
 
        clock_t clkStart = clockms(); // Get current processor time
        clock_t clkEnd = m_clkEnd; // check if the end timer is already set
 
        if (clkEnd == 0)
        {
            clkEnd = clkStart + m_timeOut;
        }
 
        while (true)
        {
            do
            {
                // First check if we already have some bytes read
                if (nBytesRead > 0 && nBytesToRead > 0)
                {
                    if (nBytesToRead > nBytesRead)
                    {
                        nOffset += nBytesRead;
                        nBytesToRead -= nBytesRead;
                        nBytesRead = 0;
                    }
                    else
                    {
                        nOffset += nBytesToRead;
                        nBytesRead -= nBytesToRead;
                        nBytesToRead = 0;
                    }
                }
 
                // Check if serial port buffer must be read
                if (nBytesToRead > 0)
                {
                    nBytesRead = readData(msgBuffer + nOffset, nBytesToRead);
 
                    if (m_retVal == MTRV_ENDOFFILE)
                    {
                        return (m_retVal = MTRV_ENDOFFILE);
                    }
 
                    nOffset += nBytesRead;
                    nBytesToRead -= nBytesRead;
                    nBytesRead = 0;
                }
 
                if (!nBytesToRead)
                {
                    switch (state)
                    {
                        case 0: // Check preamble
                            if (msgBuffer[IND_PREAMBLE] == PREAMBLE)
                            {
                                state++;
                                nBytesToRead = 3;
                            }
                            else
                            {
                                nOffset = 0;
                                nBytesToRead = 1;
                            }
 
                            break;
 
                        case 1: // Check ADDR, MID, LEN
                            if (msgBuffer[IND_LEN] != 0xFF)
                            {
                                state = 3;
                                nBytesToRead =
                                    (nMsgDataLen = msgBuffer[IND_LEN]) + 1; // Read LEN + 1 (chksum)
                            }
                            else
                            {
                                state = 2;
                                nBytesToRead = 2; // Read extended length
                            }
 
                            break;
 
                        case 2:
                            state = 3;
                            nBytesToRead = (nMsgDataLen = msgBuffer[IND_LENEXTH] * 256 +
                                                          msgBuffer[IND_LENEXTL]) +
                                           1; // Read LENEXT + CS
 
                            if (nMsgDataLen > MAXMSGLEN - LEN_MSGEXTHEADERCS)
                            {
                                // Not synced - check for preamble in the bytes read
                                for (int i = 1; i < LEN_MSGEXTHEADER; i++)
                                    if (msgBuffer[i] == PREAMBLE)
                                    {
                                        // Found a maybe preamble - 'fill buffer'
                                        nBytesRead = LEN_MSGEXTHEADER - i;
                                        memmove(msgBuffer, msgBuffer + i, nBytesRead);
                                        break;
                                    }
 
                                Synced = false;
                                nOffset = 0;
                                state = 0;
                                nBytesToRead = 1; // Start looking for preamble
                            }
 
                            break;
 
                        case 3: // Check msg
                            chCheckSum = 0;
                            nMsgLen = nMsgDataLen + 5 + (msgBuffer[IND_LEN] == 0xFF ? 2 : 0);
 
                            for (int i = 1; i < nMsgLen; i++)
                            {
                                chCheckSum += msgBuffer[i];
                            }
 
                            if (chCheckSum == 0)
                            {
                                // Checksum ok?
                                if (nBytesRead > 0)
                                {
                                    // Store bytes if read too much
                                    memcpy(m_tempBuffer, msgBuffer + nMsgLen, nBytesRead);
                                    m_nTempBufferLen = nBytesRead;
                                }
 
                                *msgBufferLength = nMsgLen;
                                Synced = true;
                                return (m_retVal = MTRV_OK);
                            }
                            else
                            {
                                if (!Synced)
                                    for (
                                        int i = 1; i < nMsgLen;
                                        i++) // Not synced - maybe recheck for preamble in this incorrect message
                                        if (msgBuffer[i] == PREAMBLE)
                                        {
                                            // Found a maybe preamble - 'fill buffer'
                                            nBytesRead = nMsgLen - i;
                                            memmove(msgBuffer, msgBuffer + i, nBytesRead);
                                            break;
                                        }
 
                                Synced = false;
                                nOffset = 0;
                                state = 0;
                                nBytesToRead = 1; // Start looking for preamble
                            }
 
                            break;
                    }
                }
            } while ((clkEnd >= clockms()) || m_timeOut == 0 || nBytesRead != 0);
 
            // Check if pending message has a valid message
            if (state > 0)
            {
                int i;
 
                // Search for preamble
                for (i = 1; i < nOffset; i++)
                    if (msgBuffer[i] == PREAMBLE)
                    {
                        // Found a maybe preamble - 'fill buffer'
                        nBytesRead = nOffset - i - 1; // no preamble
                        memmove(msgBuffer + 1, msgBuffer + i + 1, nBytesRead);
                        break;
                    }
 
                if (i < nOffset)
                {
                    // Found preamble in message - recheck
                    nOffset = 1;
                    state = 1;
                    nBytesToRead = 3; // Start looking for preamble
                    continue;
                }
            }
 
            break;
        }
 
        return (m_retVal = MTRV_TIMEOUT);
    }
 
    ////////////////////////////////////////////////////////////////////
    // writeMessage (optional: integer value)
    //
    // Sends a message and in case of an serial port interface it checks
    //   if the return message (ack, error or timeout). See return value
    //   In case an file is opened the functions returns directly after
    //   'sending' the message
    //
    //   Use this function for GotoConfig, Reset, ResetOrientation etc
    //
    // Input
    //   mid          : MessageID of message to send
    //   dataValue    : A integer value that will be included into the data message field
    //                  can be a 1,2 or 4 bytes values
    //   dataValueLen : Size of dataValue in bytes
    //   bid          : BID or address to use in message to send (default = 0xFF)
    //
    // Return value
    //   = MTRV_OK if an Ack message received / or data successfully written to file
    //   = MTRV_RECVERRORMSG if an error message received
    //   = MTRV_TIMEOUT if timeout occurred
    //   = MTRV_NOINPUTINITIALIZED
    //
    short
    CXsensMTiModule::writeMessage(const unsigned char mid,
                                  const unsigned long dataValue,
                                  const unsigned char dataValueLen,
                                  const unsigned char bid)
    {
        unsigned char buffer[MAXMSGLEN];
        short msgLen;
 
        if (!(m_fileOpen || m_portOpen))
        {
            return (m_retVal = MTRV_NOINPUTINITIALIZED);
        }
 
        buffer[IND_PREAMBLE] = PREAMBLE;
        buffer[IND_BID] = bid;
        buffer[IND_MID] = mid;
        buffer[IND_LEN] = dataValueLen;
 
        if (dataValueLen)
        {
            swapEndian((const unsigned char*)&dataValue, &buffer[IND_DATA0], dataValueLen);
        }
 
        calcChecksum(buffer, LEN_MSGHEADER + dataValueLen);
 
        // Send message
        writeData(buffer, LEN_MSGHEADERCS + dataValueLen);
 
        // Return if file opened
        if (m_fileOpen)
        {
            return (m_retVal = MTRV_OK);
        }
 
        // Keep reading until an Ack or Error message is received (or timeout)
        clock_t clkStart, clkOld;
        bool msgRead = false;
 
        clkStart = clockms(); // Get current processor time
        clkOld = m_clkEnd;
 
        if (clkOld == 0)
        {
            m_clkEnd = m_timeOut + clkStart;
        }
 
        while (m_clkEnd >= clockms() || (m_timeOut == 0))
        {
            if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
            {
                // Message received
                msgRead = true;
 
                if (buffer[IND_MID] == (mid + 1))
                {
                    m_clkEnd = clkOld;
                    return (m_retVal = MTRV_OK); // Acknowledge received
                }
                else if (buffer[IND_MID] == MID_ERROR)
                {
                    m_deviceError = buffer[IND_DATA0];
                    m_clkEnd = clkOld;
                    return (m_retVal = MTRV_RECVERRORMSG); // Error message received
                }
            }
        }
 
        m_clkEnd = clkOld;
 
        if (msgRead)
        {
            return (m_retVal = MTRV_TIMEOUT);
        }
        else
        {
            return (m_retVal = MTRV_TIMEOUTNODATA);
        }
    }
 
    ////////////////////////////////////////////////////////////////////
    // writeMessage (data array)
    //
    // Sends a message and in case of an serial port interface it checks
    //   if the return message (ack, error or timeout). See return value
    //   In case an file is opened the functions returns directly after
    //   'sending' the message
    //
    // Input
    //   mid        : MessageID of message to send
    //   data       : array pointer to data bytes
    //   dataLen    : number of bytes to include in message
    //   bid        : BID or address to use in message to send (default = 0xFF)
    //
    // Output
    //   = MTRV_OK if an Ack message received
    //   = MTRV_RECVERRORMSG if an error message received
    //   = MTRV_TIMEOUT if timeout occurred
    //   = MTRV_NOINPUTINITIALIZED
    //
    short
    CXsensMTiModule::writeMessage(const unsigned char mid,
                                  const unsigned char data[],
                                  const unsigned short& dataLen,
                                  const unsigned char bid)
    {
        unsigned char buffer[MAXMSGLEN];
        short msgLen;
        short headerLength;
 
        if (!(m_fileOpen || m_portOpen))
        {
            return (m_retVal = MTRV_NOINPUTINITIALIZED);
        }
 
        // Build message to send
        buffer[IND_PREAMBLE] = PREAMBLE;
        buffer[IND_BID] = bid;
        buffer[IND_MID] = mid;
 
        if (dataLen < EXTLENCODE)
        {
            buffer[IND_LEN] = (unsigned char)dataLen;
            headerLength = LEN_MSGHEADER;
        }
        else
        {
            buffer[IND_LEN] = EXTLENCODE;
            buffer[IND_LENEXTH] = (unsigned char)(dataLen >> 8);
            buffer[IND_LENEXTL] = (unsigned char)(dataLen & 0x00FF);
            headerLength = LEN_MSGEXTHEADER;
        }
 
        memcpy(&buffer[headerLength], data, dataLen);
        calcChecksum(buffer, headerLength + dataLen);
 
        // Send message
        writeData(buffer, headerLength + dataLen + LEN_CHECKSUM);
 
        // Return if file opened
        if (m_fileOpen)
        {
            return (m_retVal = MTRV_OK);
        }
 
        // Keep reading until an Ack or Error message is received (or timeout)
        bool msgRead = false;
        clock_t clkStart, clkOld;
 
        clkStart = clockms(); // Get current processor time
        clkOld = m_clkEnd;
 
        if (clkOld == 0)
        {
            m_clkEnd = m_timeOut + clkStart;
        }
 
        while (m_clkEnd >= clockms() || (m_timeOut == 0))
        {
            if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
            {
                // Message received
                msgRead = true;
 
                if (buffer[IND_MID] == (mid + 1))
                {
                    m_clkEnd = clkOld;
                    return (m_retVal = MTRV_OK); // Acknowledge received
                }
                else if (buffer[IND_MID] == MID_ERROR)
                {
                    m_deviceError = buffer[IND_DATA0];
                    m_clkEnd = clkOld;
                    return (m_retVal = MTRV_RECVERRORMSG); // Error message received
                }
            }
        }
 
        m_clkEnd = clkOld;
 
        if (msgRead)
        {
            return (m_retVal = MTRV_TIMEOUT);
        }
        else
        {
            return (m_retVal = MTRV_TIMEOUTNODATA);
        }
    }
 
    ////////////////////////////////////////////////////////////////////
    // waitForMessage
    //
    // Read messages from serial port or file using the current timeout period
    //  until the received message is equal to a specific message identifier
    // By default the timeout period by file input is set to infinity (= until
    //  end of file is reached)
    //
    // Input/Output
    //   mid            : message identifier of message that should be returned
    //   data           : pointer to buffer in which the data of the requested msg will be stored
    //   dataLen        : integer to number of data bytes
    //   bid            : optional, pointer which holds the bid of the returned message
    // Output
    //   returns MTRV_OK if the message has been read else != MTRV_OK
    //
    // Remarks
    //   allocate enough memory for data message buffer
    //   use setTimeOut for different timeout value
    short
    CXsensMTiModule::waitForMessage(const unsigned char mid,
                                    unsigned char data[],
                                    short* dataLen,
                                    unsigned char* bid)
    {
        unsigned char buffer[MAXMSGLEN];
        short buflen;
 
        clock_t clkStart, clkOld;
 
        if (!(m_fileOpen || m_portOpen))
        {
            return (m_retVal = MTRV_NOINPUTINITIALIZED);
        }
 
        clkStart = clockms(); // Get current processor time
        clkOld = m_clkEnd;
 
        if (clkOld == 0)
        {
            m_clkEnd = m_timeOut + clkStart;
        }
 
        while (m_clkEnd >= clockms() || (m_timeOut == 0))
        {
            if (readMessageRaw(buffer, &buflen) == MTRV_OK)
            {
                if (buffer[IND_MID] == mid)
                {
                    if (bid != nullptr)
                    {
                        *bid = buffer[IND_BID];
                    }
 
                    if (data != nullptr && dataLen != nullptr)
                    {
                        if (buffer[IND_LEN] != EXTLENCODE)
                        {
                            *dataLen = buffer[IND_LEN];
                            memcpy(data, &buffer[IND_DATA0], *dataLen);
                        }
                        else
                        {
                            *dataLen = buffer[IND_LENEXTH] * 256 + buffer[IND_LENEXTL];
                            memcpy(data, &buffer[IND_DATAEXT0], *dataLen);
                        }
                    }
                    else if (dataLen != nullptr)
                    {
                        dataLen = nullptr;
                    }
 
                    m_clkEnd = clkOld;
                    return (m_retVal = MTRV_OK);
                }
            }
            else if (m_retVal == MTRV_ENDOFFILE)
            {
                m_clkEnd = clkOld;
                return (m_retVal = MTRV_ENDOFFILE);
            }
        }
 
        m_clkEnd = clkOld;
        return (m_retVal = MTRV_TIMEOUT);
    }
 
    ////////////////////////////////////////////////////////////////////
    // reqSetting (integer & no param variant)
    //
    // Request a integer setting from the device. This setting
    // can be an unsigned 1,2 or 4 bytes setting. Only valid
    // for serial port connections
    //
    // Input
    //   mid        : Message ID of message to send
    //   bid        : Bus ID of message to send (def 0xFF)
    //
    // Output
    //   = MTRV_OK if an Ack message is received
    //   = MTRV_RECVERRORMSG if an error message is received
    //   = MTRV_TIMEOUT if timeout occurred
    //
    //   value contains the integer value of the data field of the ack message
    //
    short
    CXsensMTiModule::reqSetting(const unsigned char mid,
                                unsigned long& value,
                                const unsigned char bid)
    {
        unsigned char buffer[MAXMSGLEN];
        short msgLen;
 
        if (m_fileOpen)
        {
            return (m_retVal = MTRV_INVALIDFORFILEINPUT);
        }
 
        if (!m_portOpen)
        {
            return (m_retVal = MTRV_NOINPUTINITIALIZED);
        }
 
        buffer[IND_PREAMBLE] = PREAMBLE;
        buffer[IND_BID] = bid;
        buffer[IND_MID] = mid;
        buffer[IND_LEN] = 0;
        calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
 
        // Send message
        writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
 
        // Read next message or else timeout
        if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
        {
            // Message received
            if (buffer[IND_MID] == (mid + 1))
            {
                // Acknowledge received
                value = 0;
                swapEndian(&buffer[IND_DATA0], (unsigned char*)&value, buffer[IND_LEN]);
                return (m_retVal = MTRV_OK);
            }
            else if (buffer[IND_MID] == MID_ERROR)
            {
                m_deviceError = buffer[IND_DATA0];
                return (m_retVal = MTRV_RECVERRORMSG); // Error message received
            }
            else
            {
                return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
            }
        }
 
        return m_retVal;
    }
 
    ////////////////////////////////////////////////////////////////////
    // reqSetting (integer & param variant)
    //
    // Request a integer setting from the device. This setting
    // can be an unsigned 1,2 or 4 bytes setting. Only valid
    // for serial port connections.
    //
    // Input
    //   mid        : Message ID of message to send
    //   param      : For messages that need a parameter
    //   bid        : Bus ID of message to send (def 0xFF)
    //
    // Output
    //   = MTRV_OK if an Ack message is received
    //   = MTRV_RECVERRORMSG if an error message is received
    //   = MTRV_TIMEOUT if timeout occurred
    //
    //   value contains the integer value of the data field of the ack message
    //
    short
    CXsensMTiModule::reqSetting(const unsigned char mid,
                                const unsigned char param,
                                unsigned long& value,
                                const unsigned char bid)
    {
        unsigned char buffer[MAXMSGLEN];
        short msgLen;
 
        if (m_fileOpen)
        {
            return (m_retVal = MTRV_INVALIDFORFILEINPUT);
        }
 
        if (!m_portOpen)
        {
            return (m_retVal = MTRV_NOINPUTINITIALIZED);
        }
 
        buffer[IND_PREAMBLE] = PREAMBLE;
        buffer[IND_BID] = bid;
        buffer[IND_MID] = mid;
 
        if (param != 0xFF)
        {
            buffer[IND_LEN] = 1;
            buffer[IND_DATA0] = param;
        }
        else
        {
            buffer[IND_LEN] = 0;
        }
 
        calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
 
        // Send message
        writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
 
        // Read next message or else timeout
        if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
        {
            // Message received
            if (buffer[IND_MID] == (mid + 1))
            {
                // Acknowledge received
                value = 0;
 
                if (param == 0xFF)
                {
                    swapEndian(&buffer[IND_DATA0], (unsigned char*)&value, buffer[IND_LEN]);
                }
                else
                {
                    swapEndian(&buffer[IND_DATA0] + 1, (unsigned char*)&value, buffer[IND_LEN] - 1);
                }
 
                return (m_retVal = MTRV_OK);
            }
            else if (buffer[IND_MID] == MID_ERROR)
            {
                m_deviceError = buffer[IND_DATA0];
                return (m_retVal = MTRV_RECVERRORMSG); // Error message received
            }
            else
            {
                return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
            }
        }
 
        return m_retVal;
    }
 
    ////////////////////////////////////////////////////////////////////
    // reqSetting (float & no param variant)
    //
    // Request a float setting from the device. Only valid
    // for serial port connections.
    //
    // Input
    //   mid        : Message ID of message to send
    //   bid        : Bus ID of message to send (def 0xFF)
    //
    // Output
    //   = MTRV_OK if an Ack message is received
    //   = MTRV_RECVERRORMSG if an error message is received
    //   = MTRV_TIMEOUT if timeout occurred
    //
    //   value contains the float value of the acknowledge data field
    //
    short
    CXsensMTiModule::reqSetting(const unsigned char mid, float& value, const unsigned char bid)
    {
        unsigned char buffer[MAXMSGLEN];
        short msgLen;
 
        if (m_fileOpen)
        {
            return (m_retVal = MTRV_INVALIDFORFILEINPUT);
        }
 
        if (!m_portOpen)
        {
            return (m_retVal = MTRV_NOINPUTINITIALIZED);
        }
 
        buffer[IND_PREAMBLE] = PREAMBLE;
        buffer[IND_BID] = bid;
        buffer[IND_MID] = mid;
        buffer[IND_LEN] = 0;
        calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
 
        // Send message
        writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
 
        // Read next message or else timeout
        if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
        {
            // Message received
            if (buffer[IND_MID] == (mid + 1))
            {
                // Acknowledge received
                value = 0;
                swapEndian(&buffer[IND_DATA0], (unsigned char*)&value, buffer[IND_LEN]);
                return (m_retVal = MTRV_OK);
            }
            else if (buffer[IND_MID] == MID_ERROR)
            {
                m_deviceError = buffer[IND_DATA0];
                return (m_retVal = MTRV_RECVERRORMSG); // Error message received
            }
            else
            {
                return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
            }
        }
 
        return m_retVal;
    }
 
    ////////////////////////////////////////////////////////////////////
    // reqSetting (float & param variant)
    //
    // Request a float setting from the device. Only valid
    // for serial port connections.
    //
    // Input
    //   mid        : Message ID of message to send
    //   param      : For messages that need a parameter (optional)
    //   bid        : Bus ID of message to send (def 0xFF)
    //
    // Output
    //   = MTRV_OK if an Ack message is received
    //   = MTRV_RECVERRORMSG if an error message is received
    //   = MTRV_TIMEOUT if timeout occurred
    //
    //   value contains the float value of the acknowledge data field
    //
    short
    CXsensMTiModule::reqSetting(const unsigned char mid,
                                const unsigned char param,
                                float& value,
                                const unsigned char bid)
    {
        unsigned char buffer[MAXMSGLEN];
        short msgLen;
 
        if (m_fileOpen)
        {
            return (m_retVal = MTRV_INVALIDFORFILEINPUT);
        }
 
        if (!m_portOpen)
        {
            return (m_retVal = MTRV_NOINPUTINITIALIZED);
        }
 
        buffer[IND_PREAMBLE] = PREAMBLE;
        buffer[IND_BID] = bid;
        buffer[IND_MID] = mid;
 
        if (param != 0xFF)
        {
            buffer[IND_LEN] = 1;
            buffer[IND_DATA0] = param;
        }
        else
        {
            buffer[IND_LEN] = 0;
        }
 
        calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
 
        // Send message
        writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
 
        // Read next message or else timeout
        if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
        {
            // Message received
            if (buffer[IND_MID] == (mid + 1))
            {
                // Acknowledge received
                value = 0;
 
                if (param == 0xFF)
                {
                    swapEndian(&buffer[IND_DATA0], (unsigned char*)&value, buffer[IND_LEN]);
                }
                else
                {
                    swapEndian(&buffer[IND_DATA0] + 1, (unsigned char*)&value, buffer[IND_LEN] - 1);
                }
 
                return (m_retVal = MTRV_OK);
            }
            else if (buffer[IND_MID] == MID_ERROR)
            {
                m_deviceError = buffer[IND_DATA0];
                return (m_retVal = MTRV_RECVERRORMSG); // Error message received
            }
            else
            {
                return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
            }
        }
 
        return m_retVal;
    }
 
    ////////////////////////////////////////////////////////////////////
    // reqSetting (byte array & no param variant)
    //
    // Send a message to the device and the data of acknowledge message
    // will be returned. Only valid for serial port connections
    //
    // Input
    //   mid        : Message ID of message to send
    //   bid        : Bus ID of message to send (def 0xFF)
    //
    // Output
    //   = MTRV_OK if an Ack message is received
    //   = MTRV_RECVERRORMSG if an error message is received
    //   = MTRV_TIMEOUT if timeout occurred
    //
    //   data[] contains the data of the acknowledge message
    //   dataLen contains the number bytes returned
    //
    short
    CXsensMTiModule::reqSetting(const unsigned char mid,
                                unsigned char data[],
                                short& dataLen,
                                const unsigned char bid)
    {
        unsigned char buffer[MAXMSGLEN];
        short msgLen;
 
        if (m_fileOpen)
        {
            return (m_retVal = MTRV_INVALIDFORFILEINPUT);
        }
 
        if (!m_portOpen)
        {
            return (m_retVal = MTRV_NOINPUTINITIALIZED);
        }
 
        buffer[IND_PREAMBLE] = PREAMBLE;
        buffer[IND_BID] = bid;
        buffer[IND_MID] = mid;
        buffer[IND_LEN] = 0;
        calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
 
        // Send message
        writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
 
        dataLen = 0;
 
        // Read next message or else timeout
        if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
        {
            // Message received
            if (buffer[IND_MID] == (mid + 1))
            {
                // Acknowledge received
                if (buffer[IND_LEN] != EXTLENCODE)
                {
                    dataLen = buffer[IND_LEN];
                    memcpy(data, &buffer[IND_DATA0], dataLen);
                }
                else
                {
                    dataLen = buffer[IND_LENEXTH] * 256 + buffer[IND_LENEXTL];
                    memcpy(data, &buffer[IND_DATAEXT0], dataLen);
                }
 
                return (m_retVal = MTRV_OK);
            }
            else if (buffer[IND_MID] == MID_ERROR)
            {
                m_deviceError = buffer[IND_DATA0];
                return (m_retVal = MTRV_RECVERRORMSG); // Error message received
            }
            else
            {
                return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
            }
        }
 
        return m_retVal;
    }
 
    ////////////////////////////////////////////////////////////////////
    // reqSetting (byte array in + out & no param variant)
    //
    // Send a message to the device and the data of acknowledge message
    // will be returned. Only valid for serial port connections
    //
    // Input
    //   mid        : Message ID of message to send
    //   bid        : Bus ID of message to send (def 0xFF)
    //   dataIn     : Data to be included in request
    //   dataInLen  : Number of bytes in dataIn
    //
    // Output
    //   = MTRV_OK if an Ack message is received
    //   = MTRV_RECVERRORMSG if an error message is received
    //   = MTRV_TIMEOUT if timeout occurred
    //
    //   dataOut[] contains the data of the acknowledge message
    //   dataOutLen contains the number bytes returned
    //
    short
    CXsensMTiModule::reqSetting(const unsigned char mid,
                                unsigned char dataIn[],
                                short dataInLen,
                                unsigned char dataOut[],
                                short& dataOutLen,
                                const unsigned char bid)
    {
        unsigned char buffer[MAXMSGLEN];
        short headerLength;
        short msgLen;
 
        if (m_fileOpen)
        {
            return (m_retVal = MTRV_INVALIDFORFILEINPUT);
        }
 
        if (!m_portOpen)
        {
            return (m_retVal = MTRV_NOINPUTINITIALIZED);
        }
 
        buffer[IND_PREAMBLE] = PREAMBLE;
        buffer[IND_BID] = bid;
        buffer[IND_MID] = mid;
 
        if (dataInLen < EXTLENCODE)
        {
            buffer[IND_LEN] = (unsigned char)dataInLen;
            headerLength = LEN_MSGHEADER;
        }
        else
        {
            buffer[IND_LEN] = EXTLENCODE;
            buffer[IND_LENEXTH] = (unsigned char)(dataInLen >> 8);
            buffer[IND_LENEXTL] = (unsigned char)(dataInLen & 0x00FF);
            headerLength = LEN_MSGEXTHEADER;
        }
 
        memcpy(&buffer[headerLength], dataIn, dataInLen);
        calcChecksum(buffer, headerLength + dataInLen);
 
        // Send message
        writeData(buffer, headerLength + dataInLen + LEN_CHECKSUM);
 
        dataOutLen = 0;
 
        // Read next message or else timeout
        if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
        {
            // Message received
            if (buffer[IND_MID] == (mid + 1))
            {
                // Acknowledge received
                if (buffer[IND_LEN] != EXTLENCODE)
                {
                    dataOutLen = buffer[IND_LEN];
                    memcpy(dataOut, &buffer[IND_DATA0], dataOutLen);
                }
                else
                {
                    dataOutLen = buffer[IND_LENEXTH] * 256 + buffer[IND_LENEXTL];
                    memcpy(dataOut, &buffer[IND_DATAEXT0], dataOutLen);
                }
 
                return (m_retVal = MTRV_OK);
            }
            else if (buffer[IND_MID] == MID_ERROR)
            {
                m_deviceError = buffer[IND_DATA0];
                return (m_retVal = MTRV_RECVERRORMSG); // Error message received
            }
            else
            {
                return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
            }
        }
 
        return m_retVal;
    }
 
    ////////////////////////////////////////////////////////////////////
    // reqSetting (byte array & param variant)
    //
    // Send a message to the device and the data of acknowledge message
    // will be returned. Only valid for serial port connections
    //
    // Input
    //   mid        : Message ID of message to send
    //   param      : For messages that need a parameter (optional)
    //   bid        : Bus ID of message to send (def 0xFF)
    //
    // Output
    //   = MTRV_OK if an Ack message is received
    //   = MTRV_RECVERRORMSG if an error message is received
    //   = MTRV_TIMEOUT if timeout occurred
    //
    //   data[] contains the data of the acknowledge message (including param!!)
    //   dataLen contains the number bytes returned
    //
    short
    CXsensMTiModule::reqSetting(const unsigned char mid,
                                const unsigned char param,
                                unsigned char data[],
                                short& dataLen,
                                const unsigned char bid)
    {
        unsigned char buffer[MAXMSGLEN];
        short msgLen;
 
        if (m_fileOpen)
        {
            return (m_retVal = MTRV_INVALIDFORFILEINPUT);
        }
 
        if (!m_portOpen)
        {
            return (m_retVal = MTRV_NOINPUTINITIALIZED);
        }
 
        buffer[IND_PREAMBLE] = PREAMBLE;
        buffer[IND_BID] = bid;
        buffer[IND_MID] = mid;
 
        if (param != 0xFF)
        {
            buffer[IND_LEN] = 1;
            buffer[IND_DATA0] = param;
        }
        else
        {
            buffer[IND_LEN] = 0;
        }
 
        calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
 
        // Send message
        writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
 
        dataLen = 0;
 
        // Read next message or else timeout
        if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
        {
            // Message received
            if (buffer[IND_MID] == (mid + 1))
            {
                // Acknowledge received
                if (buffer[IND_LEN] != EXTLENCODE)
                {
                    dataLen = buffer[IND_LEN];
                    memcpy(data, &buffer[IND_DATA0], dataLen);
                }
                else
                {
                    dataLen = buffer[IND_LENEXTH] * 256 + buffer[IND_LENEXTL];
                    memcpy(data, &buffer[IND_DATAEXT0], dataLen);
                }
 
                return (m_retVal = MTRV_OK);
            }
            else if (buffer[IND_MID] == MID_ERROR)
            {
                m_deviceError = buffer[IND_DATA0];
                return (m_retVal = MTRV_RECVERRORMSG); // Error message received
            }
            else
            {
                return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
            }
        }
 
        return m_retVal;
    }
 
    ////////////////////////////////////////////////////////////////////
    // setSetting (integer & no param variant)
    //
    // Sets a integer setting of the device. This setting
    // can be an unsigned 1,2 or 4 bytes setting. Only valid
    // for serial port connections.
    //
    // Input
    //   mid        : Message ID of message to send
    //   bid        : Bus ID of message to send (def 0xFF)
    //   value      : Contains the integer value to be used
    //   valuelen   : Length in bytes of the value
    //
    // Output
    //   = MTRV_OK if an Ack message is received
    //   = MTRV_RECVERRORMSG if an error message is received
    //   = MTRV_TIMEOUT if timeout occurred
    //
    //
    short
    CXsensMTiModule::setSetting(const unsigned char mid,
                                const unsigned long value,
                                const unsigned short valuelen,
                                const unsigned char bid)
    {
        unsigned char buffer[MAXMSGLEN];
        short msgLen;
 
        if (m_fileOpen)
        {
            return (m_retVal = MTRV_INVALIDFORFILEINPUT);
        }
 
        if (!m_portOpen)
        {
            return (m_retVal = MTRV_NOINPUTINITIALIZED);
        }
 
        msgLen = LEN_MSGHEADER;
        buffer[IND_PREAMBLE] = PREAMBLE;
        buffer[IND_BID] = bid;
        buffer[IND_MID] = mid;
        buffer[IND_LEN] = (unsigned char)valuelen;
        swapEndian((unsigned char*)&value, &buffer[msgLen], valuelen);
        calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
 
        // Send message
        writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
 
        // Read next received message
        if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
        {
            // Message received
            if (buffer[IND_MID] == (mid + 1))
            {
                return (m_retVal = MTRV_OK); // Acknowledge received
            }
            else if (buffer[IND_MID] == MID_ERROR)
            {
                m_deviceError = buffer[IND_DATA0];
                return (m_retVal = MTRV_RECVERRORMSG); // Error message received
            }
        }
 
        return m_retVal;
    }
 
    ////////////////////////////////////////////////////////////////////
    // setSetting (integer & param variant)
    //
    // Sets a integer setting of the device. This setting
    // can be an unsigned 1,2 or 4 bytes setting. Only valid
    // for serial port connections.
    //
    // Input
    //   mid        : Message ID of message to send
    //   param      : For messages that need a parameter (optional)
    //   bid        : Bus ID of message to send (def 0xFF)
    //   value      : Contains the integer value to be used
    //   valuelen   : Length in bytes of the value
    //
    // Output
    //   = MTRV_OK if an Ack message is received
    //   = MTRV_RECVERRORMSG if an error message is received
    //   = MTRV_TIMEOUT if timeout occurred
    //
    //
    short
    CXsensMTiModule::setSetting(const unsigned char mid,
                                const unsigned char param,
                                const unsigned long value,
                                const unsigned short valuelen,
                                const unsigned char bid)
    {
        unsigned char buffer[MAXMSGLEN];
        short msgLen;
 
        if (m_fileOpen)
        {
            return (m_retVal = MTRV_INVALIDFORFILEINPUT);
        }
 
        if (!m_portOpen)
        {
            return (m_retVal = MTRV_NOINPUTINITIALIZED);
        }
 
        msgLen = LEN_MSGHEADER;
        buffer[IND_PREAMBLE] = PREAMBLE;
        buffer[IND_BID] = bid;
        buffer[IND_MID] = mid;
 
        if (param != 0xFF)
        {
            msgLen++;
            buffer[IND_LEN] = valuelen + 1;
            buffer[IND_DATA0] = param;
        }
        else
        {
            buffer[IND_LEN] = (unsigned char)valuelen;
        }
 
        swapEndian((unsigned char*)&value, &buffer[msgLen], valuelen);
        calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
 
        // Send message
        writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
 
        // Read next received message
        if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
        {
            // Message received
            if (buffer[IND_MID] == (mid + 1))
            {
                return (m_retVal = MTRV_OK); // Acknowledge received
            }
            else if (buffer[IND_MID] == MID_ERROR)
            {
                m_deviceError = buffer[IND_DATA0];
                return (m_retVal = MTRV_RECVERRORMSG); // Error message received
            }
        }
 
        return m_retVal;
    }
 
    ////////////////////////////////////////////////////////////////////
    // setSetting (float & no param variant)
    //
    // Sets a float setting of the device. Only valid
    // for serial port connections.
    //
    // Input
    //   mid        : Message ID of message to send
    //   bid        : Bus ID of message to send (def 0xFF)
    //   value      : Contains the float value to be used
    //
    // Output
    //   = MTRV_OK if an Ack message is received
    //   = MTRV_RECVERRORMSG if an error message is received
    //   = MTRV_TIMEOUT if timeout occurred
    //
    short
    CXsensMTiModule::setSetting(const unsigned char mid, const float value, const unsigned char bid)
    {
        unsigned char buffer[MAXMSGLEN];
        short msgLen;
 
        if (m_fileOpen)
        {
            return (m_retVal = MTRV_INVALIDFORFILEINPUT);
        }
 
        if (!m_portOpen)
        {
            return (m_retVal = MTRV_NOINPUTINITIALIZED);
        }
 
        msgLen = LEN_MSGHEADER;
        buffer[IND_PREAMBLE] = PREAMBLE;
        buffer[IND_BID] = bid;
        buffer[IND_MID] = mid;
        buffer[IND_LEN] = LEN_FLOAT;
        swapEndian((unsigned char*)&value, &buffer[msgLen], LEN_FLOAT);
        calcChecksum(buffer, LEN_MSGHEADER + LEN_FLOAT);
 
        // Send message
        writeData(buffer, LEN_MSGHEADERCS + LEN_FLOAT);
 
        // Read next received message
        if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
        {
            // Message received
            if (buffer[IND_MID] == (mid + 1))
            {
                return (m_retVal = MTRV_OK); // Acknowledge received
            }
            else if (buffer[IND_MID] == MID_ERROR)
            {
                m_deviceError = buffer[IND_DATA0];
                return (m_retVal = MTRV_RECVERRORMSG); // Error message received
            }
        }
 
        return m_retVal;
    }
 
    ////////////////////////////////////////////////////////////////////
    // setSetting (float & param variant)
    //
    // Sets a float setting of the device. Only valid
    // for serial port connections.
    //
    // Input
    //   mid        : Message ID of message to send
    //   param      : For messages that need a parameter (optional)
    //   bid        : Bus ID of message to send (def 0xFF)
    //   value      : Contains the float value to be used
    //
    // Output
    //   = MTRV_OK if an Ack message is received
    //   = MTRV_RECVERRORMSG if an error message is received
    //   = MTRV_TIMEOUT if timeout occurred
    //
    //
    short
    CXsensMTiModule::setSetting(const unsigned char mid,
                                const unsigned char param,
                                const float value,
                                const unsigned char bid)
    {
        unsigned char buffer[MAXMSGLEN];
        short msgLen;
 
        if (m_fileOpen)
        {
            return (m_retVal = MTRV_INVALIDFORFILEINPUT);
        }
 
        if (!m_portOpen)
        {
            return (m_retVal = MTRV_NOINPUTINITIALIZED);
        }
 
        msgLen = LEN_MSGHEADER;
        buffer[IND_PREAMBLE] = PREAMBLE;
        buffer[IND_BID] = bid;
        buffer[IND_MID] = mid;
 
        if (param != 0xFF)
        {
            msgLen++;
            buffer[IND_LEN] = LEN_FLOAT + 1;
            buffer[IND_DATA0] = param;
        }
        else
        {
            buffer[IND_LEN] = LEN_FLOAT;
        }
 
        swapEndian((unsigned char*)&value, &buffer[msgLen], LEN_FLOAT);
        calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
 
        // Send message
        writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
 
        // Read next received message
        if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
        {
            // Message received
            if (buffer[IND_MID] == (mid + 1))
            {
                return (m_retVal = MTRV_OK); // Acknowledge received
            }
            else if (buffer[IND_MID] == MID_ERROR)
            {
                m_deviceError = buffer[IND_DATA0];
                return (m_retVal = MTRV_RECVERRORMSG); // Error message received
            }
        }
 
        return m_retVal;
    }
 
    ////////////////////////////////////////////////////////////////////
    // setSetting (float & param & store variant)
    //
    // Sets a float setting of the device and with the Store field.
    // Only valid for serial port connections
    //
    // Input
    //   mid        : Message ID of message to send
    //   param      : For messages that need a parameter (optional)
    //   value      : Contains the float value to be used
    //   store      ; Store in non-volatile memory (1) or not (0)
    //   bid        : Bus ID of message to send (def 0xFF)
    //
    // Output
    //   = MTRV_OK if an Ack message is received
    //   = MTRV_RECVERRORMSG if an error message is received
    //   = MTRV_TIMEOUT if timeout occurred
    //
    //
    short
    CXsensMTiModule::setSetting(const unsigned char mid,
                                const unsigned char param,
                                const float value,
                                const bool store,
                                const unsigned char bid)
    {
        unsigned char buffer[MAXMSGLEN];
        short msgLen;
 
        if (m_fileOpen)
        {
            return (m_retVal = MTRV_INVALIDFORFILEINPUT);
        }
 
        if (!m_portOpen)
        {
            return (m_retVal = MTRV_NOINPUTINITIALIZED);
        }
 
        msgLen = LEN_MSGHEADER;
        buffer[IND_PREAMBLE] = PREAMBLE;
        buffer[IND_BID] = bid;
        buffer[IND_MID] = mid;
 
        if (param != 0xFF)
        {
            msgLen++;
            buffer[IND_LEN] = LEN_FLOAT + 2;
            buffer[IND_DATA0] = param;
        }
        else
        {
            buffer[IND_LEN] = LEN_FLOAT + 1;
        }
 
        swapEndian((unsigned char*)&value, &buffer[msgLen], LEN_FLOAT);
        buffer[msgLen + LEN_FLOAT] = store;
        calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
 
        // Send message
        writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
 
        // Read next received message
        if (readMessageRaw(buffer, &msgLen) == MTRV_OK)
        {
            // Message received
            if (buffer[IND_MID] == (mid + 1))
            {
                return (m_retVal = MTRV_OK); // Acknowledge received
            }
            else if (buffer[IND_MID] == MID_ERROR)
            {
                m_deviceError = buffer[IND_DATA0];
                return (m_retVal = MTRV_RECVERRORMSG); // Error message received
            }
        }
 
        return m_retVal;
    }
 
    ////////////////////////////////////////////////////////////////////
    // getDeviceMode
    //
    // Requests the current output mode/setting of input (file or serialport)
    //  the Outputmode, Outputsettings, DataLength & number of devices
    //  are stored in member variables of the MTComm class. These values
    //  are needed for the GetValue functions.
    //  The function optionally returns the number of devices
    //
    // File: expects the Configuration message at the start of the file
    //       which holds the OutputMode & OutputSettings. File position
    //       is after the first message
    //
    // Input
    // Output
    //   numDevices : [optional] number of devices connected to port or
    //                found in configuration file
    //
    //   returns MTRV_OK if the mode & settings are read
    //
    short
    CXsensMTiModule::getDeviceMode(unsigned short* numDevices)
    {
        unsigned char mid = 0, data[MAXMSGLEN];
        short datalen;
 
        if (numDevices != nullptr)
        {
            *numDevices = 0;
        }
 
        // In case serial port is used (live device / XM or MT)
        if (m_portOpen)
        {
            if (reqSetting(MID_INITBUS, data, datalen) != MTRV_OK)
            {
                return m_retVal;
            }
 
            // Retrieve outputmode + outputsettings
            for (int i = 0; i < datalen / LEN_DEVICEID; i++)
            {
                if (reqSetting(MID_REQOUTPUTMODE, m_storedOutputMode[BID_MT + i], BID_MT + i) !=
                    MTRV_OK)
                {
                    return m_retVal;
                }
 
                if (reqSetting(MID_REQOUTPUTSETTINGS,
                               m_storedOutputSettings[BID_MT + i],
                               BID_MT + i) != MTRV_OK)
                {
                    return m_retVal;
                }
 
                if (reqSetting(MID_REQDATALENGTH, m_storedDataLength[BID_MT + i], BID_MT + i) !=
                    MTRV_OK)
                {
                    return m_retVal;
                }
            }
 
            if (numDevices != nullptr)
            {
                *numDevices = datalen / LEN_DEVICEID;
            }
 
            unsigned char masterDID[4];
            short DIDlen;
 
            if (reqSetting(MID_REQDID, masterDID, DIDlen) != MTRV_OK)
            {
                return m_retVal;
            }
 
            if (memcmp(masterDID, data, LEN_DEVICEID) != 0)
            {
                // Using an XbusMaster
                m_storedOutputMode[0] = OUTPUTMODE_XM;
                m_storedOutputSettings[0] = OUTPUTSETTINGS_XM;
                m_storedDataLength[0] = LEN_SAMPLECNT;
            }
            else
            {
                m_storedOutputMode[0] = m_storedOutputMode[BID_MT];
                m_storedOutputSettings[0] = m_storedOutputSettings[BID_MT];
                m_storedDataLength[0] = m_storedDataLength[BID_MT];
            }
 
            return (m_retVal = MTRV_OK);
        }
        else if (m_fileOpen)
        {
            // Configuration message should be the first message in the file
            setFilePos(0);
 
            if (readMessage(mid, data, datalen) == MTRV_OK)
            {
                if (mid == MID_CONFIGURATION)
                {
                    unsigned short _numDevices = 0;
                    swapEndian(
                        data + CONF_NUMDEVICES, (unsigned char*)&_numDevices, CONF_NUMDEVICESLEN);
 
                    for (unsigned int i = 0; i < _numDevices; i++)
                    {
                        m_storedOutputMode[BID_MT + i] = 0;
                        swapEndian(data + CONF_OUTPUTMODE + i * CONF_BLOCKLEN,
                                   (unsigned char*)(m_storedOutputMode + BID_MT + i),
                                   CONF_OUTPUTMODELEN);
                        m_storedOutputSettings[BID_MT + i] = 0;
                        swapEndian(data + CONF_OUTPUTSETTINGS + i * CONF_BLOCKLEN,
                                   (unsigned char*)(m_storedOutputSettings + BID_MT + i),
                                   CONF_OUTPUTSETTINGSLEN);
                        m_storedDataLength[BID_MT + i] = 0;
                        swapEndian(data + CONF_DATALENGTH + i * CONF_BLOCKLEN,
                                   (unsigned char*)(m_storedDataLength + BID_MT + i),
                                   CONF_DATALENGTHLEN);
                    }
 
                    if (numDevices != nullptr)
                    {
                        *numDevices = _numDevices;
                    }
 
                    if (memcmp(data + CONF_MASTERDID, data + CONF_DID, LEN_DEVICEID) != 0)
                    {
                        // Using an XbusMaster
                        m_storedOutputMode[0] = OUTPUTMODE_XM;
                        m_storedOutputSettings[0] = OUTPUTSETTINGS_XM;
                        m_storedDataLength[0] = LEN_SAMPLECNT;
                    }
                    else
                    {
                        m_storedOutputMode[0] = m_storedOutputMode[BID_MT];
                        m_storedOutputSettings[0] = m_storedOutputSettings[BID_MT];
                        m_storedDataLength[0] = m_storedDataLength[BID_MT];
                    }
 
                    return (m_retVal = MTRV_OK);
                }
            }
 
            return (m_retVal = MTRV_NOTSUCCESSFUL);
        }
 
        return (m_retVal = MTRV_NOINPUTINITIALIZED);
    }
 
    ////////////////////////////////////////////////////////////////////
    // setDeviceMode
    //
    // Sets the current output mode/setting of input (not for file-based
    //   inputs)
    //
    // Input
    //   OutputMode     : OutputMode to be set in device & stored in MTComm
    //                      class member variable
    //   OutputSettings : OutputSettings to be set in device & stored in
    //                      MTComm class member variable
    // Output
    //
    //   returns MTRV_OK if the mode & settings are read
    //
    short
    CXsensMTiModule::setDeviceMode(unsigned long OutputMode,
                                   unsigned long OutputSettings,
                                   const unsigned char bid)
    {
        // In case serial port is used (live XM / MT)
        if (m_portOpen)
        {
            // Set OutputMode
            if (setSetting(MID_SETOUTPUTMODE, OutputMode, LEN_OUTPUTMODE, bid) != MTRV_OK)
            {
                return m_retVal;
            }
 
            if (bid == BID_MASTER || (bid == BID_MT && m_storedOutputMode[0] != OUTPUTMODE_XM))
            {
                m_storedOutputMode[0] = m_storedOutputMode[BID_MT] = OutputMode;
            }
            else
            {
                m_storedOutputMode[bid] = OutputMode;
            }
 
            // Set OutputSettings
            if (setSetting(MID_SETOUTPUTSETTINGS, OutputSettings, LEN_OUTPUTSETTINGS, bid) !=
                MTRV_OK)
            {
                return m_retVal;
            }
 
            if (bid == BID_MASTER || (bid == BID_MT && m_storedOutputMode[0] != OUTPUTMODE_XM))
            {
                m_storedOutputSettings[0] = m_storedOutputSettings[BID_MT] = OutputSettings;
            }
            else
            {
                m_storedOutputSettings[bid] = OutputSettings;
            }
 
            // Get DataLength from device
            if (OutputMode != OUTPUTMODE_XM)
            {
                unsigned long value;
 
                if (reqSetting(MID_REQDATALENGTH, value, bid) == MTRV_OK)
                {
                    if ((bid == BID_MASTER) ||
                        ((bid == BID_MT) && (m_storedOutputMode[0] != OUTPUTMODE_XM)))
                    {
                        m_storedDataLength[0] = m_storedDataLength[BID_MT] = value;
                    }
                    else
                    {
                        m_storedDataLength[bid] = value;
                    }
                }
            }
            else
            {
                m_storedDataLength[0] = LEN_SAMPLECNT;
            }
 
            return (m_retVal = MTRV_OK);
        }
 
        return (m_retVal = MTRV_INVALIDFORFILEINPUT);
    }
 
    ////////////////////////////////////////////////////////////////////
    // getMode
    //
    // Gets the output mode/setting used in MTComm class and the corresponding
    //  datalength. These variables are set by the functions GetDeviceMode,
    //  SetDeviceMode or SetMode
    //
    // Input
    // Output
    //   OutputMode     : OutputMode stored in MTComm class member variable
    //   OutputSettings : OutputSettings stored in MTComm class member variable
    //
    //   returns always MTRV_OK
    //
    short
    CXsensMTiModule::getMode(unsigned long& OutputMode,
                             unsigned long& OutputSettings,
                             unsigned short& dataLength,
                             const unsigned char bid)
    {
        unsigned char nbid = (bid == BID_MASTER) ? 0 : bid;
        OutputMode = m_storedOutputMode[nbid];
        OutputSettings = m_storedOutputSettings[nbid];
        dataLength = (unsigned short)m_storedDataLength[nbid];
        return (m_retVal = MTRV_OK);
    }
 
    ////////////////////////////////////////////////////////////////////
    // setMode
    //
    // Sets the output mode/setting used in MTComm class. Use the function
    //  GetDeviceMode to retrieve the current values of file/device.
    // This function will also calculate the data length field
    //
    // Input
    //   OutputMode     : OutputMode to be stored in MTComm class member variable
    //   OutputSettings : OutputSettings to be stored in MTComm class member variable
    // Output
    //
    //   returns always MTRV_OK
    //
    short
    CXsensMTiModule::setMode(unsigned long OutputMode,
                             unsigned long OutputSettings,
                             const unsigned char bid)
    {
        unsigned char nbid = bid;
 
        if (nbid == BID_MASTER)
        {
            nbid = 0;
        }
 
        m_storedOutputMode[nbid] = OutputMode;
        m_storedOutputSettings[nbid] = OutputSettings;
 
        if (OutputMode == INVALIDSETTINGVALUE || OutputSettings == INVALIDSETTINGVALUE)
        {
            m_storedDataLength[nbid] = 0;
        }
        else
        {
            unsigned short dataLength = 0;
 
            if (OutputMode & OUTPUTMODE_MT9)
            {
                dataLength = ((OutputSettings & OUTPUTSETTINGS_TIMESTAMP_MASK) ==
                              OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT)
                                 ? LEN_SAMPLECNT
                                 : 0 + LEN_RAWDATA;
            }
            else if (OutputMode == OUTPUTMODE_XM)
            {
                // XbusMaster concatenates sample counter
                dataLength = LEN_SAMPLECNT;
            }
            else
            {
                if (OutputMode & OUTPUTMODE_RAW)
                {
                    dataLength = LEN_RAWDATA;
                }
                else
                {
                    if (OutputMode & OUTPUTMODE_CALIB)
                    {
                        dataLength = LEN_CALIBDATA;
                    }
 
                    if (OutputMode & OUTPUTMODE_ORIENT)
                    {
                        switch (OutputSettings & OUTPUTSETTINGS_ORIENTMODE_MASK)
                        {
                            case OUTPUTSETTINGS_ORIENTMODE_QUATERNION:
                                dataLength += LEN_ORIENT_QUATDATA;
                                break;
 
                            case OUTPUTSETTINGS_ORIENTMODE_EULER:
                                dataLength += LEN_ORIENT_EULERDATA;
                                break;
 
                            case OUTPUTSETTINGS_ORIENTMODE_MATRIX:
                                dataLength += LEN_ORIENT_MATRIXDATA;
                                break;
 
                            default:
                                break;
                        }
                    }
                }
 
                switch (OutputSettings & OUTPUTSETTINGS_TIMESTAMP_MASK)
                {
                    case OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT:
                        dataLength += LEN_SAMPLECNT;
                        break;
 
                    default:
                        break;
                }
            }
 
            m_storedDataLength[nbid] = dataLength;
        }
 
        // If not XbusMaster store also in BID_MT
        if (bid == BID_MASTER && OutputMode != OUTPUTMODE_XM)
        {
            m_storedOutputMode[BID_MT] = m_storedOutputMode[0];
            m_storedOutputSettings[BID_MT] = m_storedOutputSettings[0];
            m_storedDataLength[BID_MT] = m_storedDataLength[0];
        }
 
        return (m_retVal = MTRV_OK);
    }
 
    ////////////////////////////////////////////////////////////////////
    // getValue (unsigned short variant)
    //
    // Retrieves a unsigned short value from the data input parameter
    // This function is valid for the following value specifiers:
    //      VALUE_RAW_TEMP
    //      VALUE_SAMPLECNT
    //
    // Use getDeviceMode or setMode to initialize the Outputmode
    // and Outputsettings member variables used to retrieve the correct
    // value
    //
    // Input
    //   valueSpec      : Specifier of the value to be retrieved
    //   data[]         : Data field of a MTData / BusData message
    //   bid            : bus identifier of the device of which the
    //                      value should be returned (default = BID_MT)
    // Output
    //   value          : reference to unsigned short in which the retrieved
    //                      value will be returned
    //
    //  Return value
    //    MTRV_OK       : value is successfully retrieved
    //    != MTRV_OK    : not successful
    //
    short
    CXsensMTiModule::getValue(const unsigned long valueSpec,
                              unsigned short& value,
                              const unsigned char data[],
                              const unsigned char bid)
    {
        short offset = 0;
        unsigned char nbid = bid;
 
        if (nbid == BID_MASTER)
        {
            nbid = 0;
        }
 
        // Check for invalid mode/settings
        if (m_storedOutputMode[nbid] == INVALIDSETTINGVALUE ||
            m_storedOutputSettings[nbid] == INVALIDSETTINGVALUE)
        {
            return (m_retVal = MTRV_NOVALIDMODESPECIFIED);
        }
 
        // Calculate offset for XM input
        if (m_storedOutputMode[0] == OUTPUTMODE_XM)
        {
            int i = 0;
 
            while (i < nbid)
            {
                offset += (short)m_storedDataLength[i++];
            }
        }
 
        // Check if data is unsigned short & available in data
        m_retVal = MTRV_INVALIDVALUESPEC;
 
        if (valueSpec == VALUE_RAW_TEMP)
        {
            if (m_storedOutputMode[nbid] & (OUTPUTMODE_RAW | OUTPUTMODE_MT9))
            {
                offset += (m_storedOutputMode[nbid] == OUTPUTMODE_MT9 &&
                           m_storedOutputSettings[nbid] == OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT)
                              ? LEN_SAMPLECNT
                              : 0;
                swapEndian(data + offset + valueSpec * LEN_UNSIGSHORT * 3,
                           (unsigned char*)&value,
                           LEN_RAW_TEMP);
                m_retVal = MTRV_OK;
            }
        }
        else if (valueSpec == VALUE_SAMPLECNT)
        {
            if ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_TIMESTAMP_MASK) ==
                OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT)
            {
                if (!(m_storedOutputMode[nbid] == OUTPUTMODE_MT9))
                {
                    offset += (short)m_storedDataLength[nbid] - LEN_SAMPLECNT;
                }
 
                swapEndian(data + offset, (unsigned char*)&value, LEN_SAMPLECNT);
                m_retVal = MTRV_OK;
            }
        }
 
        return m_retVal;
    }
 
    ////////////////////////////////////////////////////////////////////
    // getValue (array of unsigned short variant)
    //
    // Retrieves an array of unsigned short values from the data input
    // parameter. This function is valid for the following value specifiers:
    //      VALUE_RAW_ACC
    //      VALUE_RAW_GYR
    //      VALUE_RAW_MAG
    //
    // Use getDeviceMode or setMode to initialize the Outputmode
    // and Outputsettings member variables used to retrieve the correct
    // value
    //
    // Input
    //   valueSpec      : Specifier of the value to be retrieved
    //   data[]         : Data field of a MTData / BusData message
    //   bid            : bus identifier of the device of which the
    //                      value should be returned (default = BID_MT)
    // Output
    //   value[]        : pointer to array of unsigned shorts in which the
    //                      retrieved values will be returned
    //
    //  Return value
    //    MTRV_OK       : value is successfully retrieved
    //    != MTRV_OK    : not successful
    //
    short
    CXsensMTiModule::getValue(const unsigned long valueSpec,
                              unsigned short value[],
                              const unsigned char data[],
                              const unsigned char bid)
    {
        short offset = 0;
        unsigned char nbid = bid;
 
        if (nbid == BID_MASTER)
        {
            nbid = 0;
        }
 
        // Check for invalid mode/settings
        if (m_storedOutputMode[nbid] == INVALIDSETTINGVALUE ||
            m_storedOutputSettings[nbid] == INVALIDSETTINGVALUE)
        {
            return (m_retVal = MTRV_NOVALIDMODESPECIFIED);
        }
 
        // Calculate offset for XM input
        if (m_storedOutputMode[0] == OUTPUTMODE_XM)
        {
            int i = 0;
 
            while (i < nbid)
            {
                offset += (short)m_storedDataLength[i++];
            }
        }
 
        // Check if data is unsigned short, available in data & retrieve data
        m_retVal = MTRV_INVALIDVALUESPEC;
 
        //if (valueSpec >= VALUE_RAW_ACC && valueSpec <= VALUE_RAW_MAG)
        if (valueSpec <= VALUE_RAW_MAG)
        {
            if (m_storedOutputMode[nbid] & (OUTPUTMODE_RAW | OUTPUTMODE_MT9))
            {
                offset += (short)(valueSpec * LEN_UNSIGSHORT * 3);
                offset += (m_storedOutputMode[nbid] == OUTPUTMODE_MT9 &&
                           m_storedOutputSettings[nbid] == OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT)
                              ? LEN_SAMPLECNT
                              : 0;
 
                for (int i = 0; i < 3; i++)
                {
                    swapEndian(data + offset + i * LEN_UNSIGSHORT,
                               (unsigned char*)value + i * LEN_UNSIGSHORT,
                               LEN_UNSIGSHORT);
                }
 
                m_retVal = MTRV_OK;
            }
        }
 
        return m_retVal;
    }
 
    ////////////////////////////////////////////////////////////////////
    // getValue (array of floats variant)
    //
    // Retrieves an array of float values from the data input parameter.
    // This function is valid for the following value specifiers:
    //      VALUE_TEMP
    //      VALUE_CALIB_ACC
    //      VALUE_CALIB_GYR
    //      VALUE_CALIB_MAG
    //      VALUE_ORIENT_QUAT
    //      VALUE_ORIENT_EULER
    //      VALUE_ORIENT_MATRIX
    //
    // Use getDeviceMode or setMode to initialize the Outputmode
    // and Outputsettings member variables used to retrieve the correct
    // value
    //
    // Input
    //   valueSpec      : Specifier of the value to be retrieved
    //   data[]         : Data field of a MTData / BusData message
    //   bid            : bus identifier of the device of which the
    //                      value should be returned (default = BID_MT)
    // Output
    //   value[]        : pointer to array of floats in which the
    //                      retrieved values will be returned
    //
    //  Return value
    //    MTRV_OK       : value is successfully retrieved
    //    != MTRV_OK    : not successful
    //
    short
    CXsensMTiModule::getValue(const unsigned long valueSpec,
                              float value[],
                              const unsigned char data[],
                              const unsigned char bid)
    {
        short offset = 0;
        int nElements = 0;
        unsigned char nbid = bid;
 
        if (nbid == BID_MASTER)
        {
            nbid = 0;
        }
 
        // Check for invalid mode/settings
        if (m_storedOutputMode[nbid] == INVALIDSETTINGVALUE ||
            m_storedOutputSettings[nbid] == INVALIDSETTINGVALUE)
        {
            return (m_retVal = MTRV_NOVALIDMODESPECIFIED);
        }
 
        // Calculate offset for XM input
        if (m_storedOutputMode[0] == OUTPUTMODE_XM)
        {
            int i = 0;
 
            while (i < nbid)
            {
                offset += (short)m_storedDataLength[i++];
            }
        }
 
        // Check if data is float & available in data
        m_retVal = MTRV_INVALIDVALUESPEC;
 
        if (valueSpec == VALUE_TEMP)
        {
            if (m_storedOutputMode[nbid] & OUTPUTMODE_TEMP)
            {
                nElements = LEN_TEMPDATA / LEN_FLOAT;
                m_retVal = MTRV_OK;
            }
        }
        else if (valueSpec == VALUE_CALIB_ACC)
        {
            offset += ((m_storedOutputMode[nbid] & OUTPUTMODE_TEMP) != 0) ? LEN_TEMPDATA : 0;
 
            if ((m_storedOutputMode[nbid] & OUTPUTMODE_CALIB) &&
                (m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_ACC_MASK) == 0)
            {
                nElements = LEN_CALIB_ACCDATA / LEN_FLOAT;
                m_retVal = MTRV_OK;
            }
        }
        else if (valueSpec == VALUE_CALIB_GYR)
        {
            offset += ((m_storedOutputMode[nbid] & OUTPUTMODE_TEMP) != 0) ? LEN_TEMPDATA : 0;
 
            if ((m_storedOutputMode[nbid] & OUTPUTMODE_CALIB) &&
                (m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_GYR_MASK) == 0)
            {
                offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_ACC_MASK) == 0)
                              ? LEN_CALIB_ACCX * 3
                              : 0;
                nElements = LEN_CALIB_GYRDATA / LEN_FLOAT;
                m_retVal = MTRV_OK;
            }
        }
        else if (valueSpec == VALUE_CALIB_MAG)
        {
            offset += ((m_storedOutputMode[nbid] & OUTPUTMODE_TEMP) != 0) ? LEN_TEMPDATA : 0;
 
            if ((m_storedOutputMode[nbid] & OUTPUTMODE_CALIB) &&
                (m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_MAG_MASK) == 0)
            {
                offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_ACC_MASK) == 0)
                              ? LEN_CALIB_ACCX * 3
                              : 0;
                offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_GYR_MASK) == 0)
                              ? LEN_CALIB_GYRX * 3
                              : 0;
                nElements = LEN_CALIB_MAGDATA / LEN_FLOAT;
                m_retVal = MTRV_OK;
            }
        }
        else if (valueSpec >= VALUE_ORIENT_QUAT && valueSpec <= VALUE_ORIENT_MATRIX)
        {
            offset += ((m_storedOutputMode[nbid] & OUTPUTMODE_TEMP) != 0) ? LEN_TEMPDATA : 0;
 
            if ((m_storedOutputMode[nbid] & OUTPUTMODE_CALIB))
            {
                offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_ACC_MASK) == 0)
                              ? LEN_CALIB_ACCX * 3
                              : 0;
                offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_GYR_MASK) == 0)
                              ? LEN_CALIB_GYRX * 3
                              : 0;
                offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_MAG_MASK) == 0)
                              ? LEN_CALIB_MAGX * 3
                              : 0;
            }
 
            if (m_storedOutputMode[nbid] & OUTPUTMODE_ORIENT)
            {
                unsigned long orientmode =
                    m_storedOutputSettings[nbid] & OUTPUTSETTINGS_ORIENTMODE_MASK;
 
                switch (valueSpec)
                {
                    case VALUE_ORIENT_QUAT:
                        if (orientmode == OUTPUTSETTINGS_ORIENTMODE_QUATERNION)
                        {
                            nElements = LEN_ORIENT_QUATDATA / LEN_FLOAT;
                            m_retVal = MTRV_OK;
                        }
 
                        break;
 
                    case VALUE_ORIENT_EULER:
                        if (orientmode == OUTPUTSETTINGS_ORIENTMODE_EULER)
                        {
                            nElements = LEN_ORIENT_EULERDATA / LEN_FLOAT;
                            m_retVal = MTRV_OK;
                        }
 
                        break;
 
                    case VALUE_ORIENT_MATRIX:
                        if (orientmode == OUTPUTSETTINGS_ORIENTMODE_MATRIX)
                        {
                            nElements = LEN_ORIENT_MATRIXDATA / LEN_FLOAT;
                            m_retVal = MTRV_OK;
                        }
 
                        break;
 
                    default:
                        break;
                }
            }
        }
 
        if (m_retVal == MTRV_OK)
        {
            if ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_DATAFORMAT_F1220) == 0)
            {
                for (int i = 0; i < nElements; i++)
                {
                    swapEndian(data + offset + i * LEN_FLOAT,
                               (unsigned char*)value + i * LEN_FLOAT,
                               LEN_FLOAT);
                }
            }
            else
            {
                int temp;
 
                for (int i = 0; i < nElements; i++)
                {
                    swapEndian(data + offset + i * LEN_FLOAT, (unsigned char*)&temp, 4);
                    value[i] = (float)temp / 1048576;
                }
            }
        }
 
        return m_retVal;
    }
 
    //////////////////////////////////////////////////////////////////////
    // getLastDeviceError
    //
    // Returns the last reported device error of the latest received Error
    //  message
    //
    // Output
    //   Error code
    short
    CXsensMTiModule::getLastDeviceError()
    {
        return m_deviceError;
    }
 
    //////////////////////////////////////////////////////////////////////
    // getLastRetVal
    //
    // Returns the returned value of the last called function
    //
    // Output
    //   Return value
    short
    CXsensMTiModule::getLastRetVal()
    {
        return m_retVal;
    }
 
    //////////////////////////////////////////////////////////////////////
    // setTimeOut
    //
    // Sets the time out value in milliseconds used by the functions
    // Use 0 for infinite timeout
    //
    // Output
    //   MTRV_OK is set, MTRV_INVALIDTIMEOUT if time value < 0
    short
    CXsensMTiModule::setTimeOut(short timeOutMs)
    {
        if (timeOutMs >= 0)
        {
            m_timeOut = timeOutMs;
            return (m_retVal = MTRV_OK);
        }
        else
        {
            return (m_retVal = MTRV_INVALIDTIMEOUT);
        }
    }
 
    //////////////////////////////////////////////////////////////////////
    // swapEndian
    //
    // Convert 2 or 4 bytes data from little to big endian or back
    //
    // Input
    //   input  : Pointer to data to be converted
    //   output : Pointer where converted data is stored
    //   length : Length of setting (0,2 & 4)
    //
    // Remarks:
    //   Allocate enough bytes for output buffer
 
    void
    CXsensMTiModule::swapEndian(const unsigned char input[],
                                unsigned char output[],
                                const int length)
    {
        switch (length)
        {
            case 4:
                output[0] = input[3];
                output[1] = input[2];
                output[2] = input[1];
                output[3] = input[0];
                break;
 
            case 2:
                output[0] = input[1];
                output[1] = input[0];
                break;
 
            case 1:
                output[0] = input[0];
                break;
 
            default:
                for (int i = 0, j = length - 1; i < length; i++, j--)
                {
                    output[j] = input[i];
                }
 
                break;
        }
    }
 
    //////////////////////////////////////////////////////////////////////
    // calcChecksum
    //
    // Calculate and append checksum to msgBuffer
    //
    void
    CXsensMTiModule::calcChecksum(unsigned char* msgBuffer, const int msgBufferLength)
    {
        unsigned char checkSum = 0;
        int i;
 
        for (i = 1; i < msgBufferLength; i++)
        {
            checkSum += msgBuffer[i];
        }
 
        msgBuffer[msgBufferLength] = -checkSum; // Store chksum
    }
 
    //////////////////////////////////////////////////////////////////////
    // checkChecksum
    //
    // Checks if message checksum is valid
    //
    // Output
    //   returns true checksum is OK
    bool
    CXsensMTiModule::checkChecksum(const unsigned char* msgBuffer, const int msgBufferLength)
    {
        unsigned char checkSum = 0;
        int i;
 
        for (i = 1; i < msgBufferLength; i++)
        {
            checkSum += msgBuffer[i];
        }
 
        if (checkSum == 0)
        {
            return true;
        }
        else
        {
            return false;
        }
    }
} // namespace IMU::Xsens
 
#endif