cmt1.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */
 
/*! \file cmt1.cpp
 
        For information about objects in this file, see the appropriate header:
        \ref Cmt1.h
 
        \section FileCopyright Copyright Notice
        Copyright (C) Xsens Technologies B.V., 2006.  All rights reserved.
 
        This source code is intended for use only by Xsens Technologies BV and
        those that have explicit written permission to use it from
        Xsens Technologies BV.
 
        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.
 
        \section FileChangelog  Changelog
        \par 2006-04-12, v0.0.1
        \li Job Mulder: Created
        \par 2006-07-21, v0.1.0
        \li Job Mulder: Updated file for release 0.1.0
*/
 
#include "cmt1.h"
#include <errno.h>
 
#include <mrpt/config.h>  // For HAVE_MALLOC_H
 
#ifndef _WIN32
#include <unistd.h>  // close
#include <sys/ioctl.h>  // ioctl
#include <fcntl.h>  // open, O_RDWR
#include <string.h>  // strcpy
 
/* Jerome Monceaux : bilock@gmail.com
 * Add a specific case for apple
 */
#ifdef HAVE_MALLOC_H
#include <malloc.h>
#elif defined(HAVE_MALLOC_MALLOC_H)
#include <malloc/malloc.h>
#endif
 
#include <stdarg.h>  // va_start, etc...
#include <sys/param.h>
// We have to redefine PATH_MAX from 4096 to CMT_MAX_FILENAME_LENGTH to mainain
// compatibility
// The PATH_MAX definition is used by realpath() to determine the maximum path
// length. According
// to the realpath (3) man page, the function is best avoided and it might be
// necessary to
// write a custom function for it (couldn't find a proper replacement).
#undef PATH_MAX
#define PATH_MAX CMT_MAX_FILENAME_LENGTH
#include <stdlib.h>
#else
#include <stdio.h>  // fseek
#include <io.h>
#endif
 
#ifndef _CRT_SECURE_NO_DEPRECATE
#define _CRT_SECURE_NO_DEPRECATE
#ifdef _WIN32
#pragma warning(disable : 4996)
#endif
#endif
 
#ifdef _WIN32
#ifdef _MSC_VER
#define FSEEK(x) _fseeki64(m_handle, x, SEEK_SET)
#define FSEEK_R(x) _fseeki64(m_handle, x, SEEK_END)
#define FTELL() _ftelli64(m_handle)
#else
#define FSEEK(x) fseek(m_handle, x, SEEK_SET)
#define FSEEK_R(x) fseek(m_handle, x, SEEK_END)
#define FTELL() ftell(m_handle)
#endif
#else
#define FSEEK(x) fseeko(m_handle, x, SEEK_SET)
#define FSEEK_R(x) fseeko(m_handle, x, SEEK_END)
#define FTELL() ftello(m_handle)
#endif
 
// The namespace of all Xsens software since 2006.
namespace xsens
{
#ifndef _WIN32
int _wcsnicmp(const wchar_t* s1, const wchar_t* s2, int count)
{
        for (int i = 0; i < count; ++i, ++s1, ++s2)
                if (*s1 == L'\0')
                        if (*s2 == L'\0')
                                return 0;
                        else
                                return -1;
                else if (*s2 == L'\0')
                        return 1;
                else if (*s1 < *s2)
                        return -1;
                else if (*s1 > *s2)
                        return 1;
        return 0;
}
#endif
 
#if defined(_DEBUG) || defined(_LOG_ALWAYS)
#if !defined(_LOG_TO_DBVIEW)
#ifdef _LOG_TO_STDOUT
#else  // !dbview && !stdout
FILE* debug_log_fp = nullptr;
int32_t debug_log_valid = 0;
 
FILE* debug_qlog_fp = nullptr;
int32_t debug_qlog_valid = 0;
#endif
#endif
 
// write to a log file/screen/debug-stream
void CMTLOG(const char* str, ...)
{
#ifdef _LOG_TO_STDOUT
        va_list ptr;
        va_start(ptr, str);
        vprintf(str, ptr);
#else
#ifdef _LOG_TO_DBVIEW
        char buf[2048];
 
        va_list ptr;
        va_start(ptr, str);
        vsprintf(buf, str, ptr);
 
        OutputDebugString(buf);
#else
        if (debug_log_valid == 0)
        {
                debug_log_fp = fopen("debug_log_cmt.log", "w");
                if (debug_log_fp != nullptr)
                        debug_log_valid = 1;
                else
                        debug_log_valid = -1;
        }
        if (debug_log_valid == 1)
        {
                char buf[2048];
 
                va_list ptr;
                va_start(ptr, str);
                int32_t sz = vsprintf_s(buf, str, ptr);
 
                uint32_t nw = getTimeOfDay();
                fprintf(debug_log_fp, "%5u.%03u %s", nw / 1000, nw % 1000, buf);
                // fwrite(buf,1,sz,debug_log_fp);
                fflush(debug_log_fp);
        }
#endif
#endif
}
#endif
 
// maybe log to nothing at this level
#ifdef _LOG_CMT1
#define CMT1LOG CMTLOG
#else
#define CMT1LOG(...)
#endif
 
//////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////// Cmt1s
////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////
 
//////////////////////////////////////////////////////////////////////////////////////////
// Default constructor, initializes all members to their default values.
Cmt1s::Cmt1s() : m_onBytesReceived(nullptr)
{
        m_port = 0;
        m_isOpen = false;
        m_lastResult = XRV_OK;
        m_timeout = CMT1_DEFAULT_TIMEOUT;
        m_endTime = 0;
        m_baudrate = 0;
 
#ifdef _LOG_RX_TX
        rx_log = nullptr;
        tx_log = nullptr;
#endif
}
 
//////////////////////////////////////////////////////////////////////////////////////////
// Destructor, de-initializes, frees memory allocated for buffers, etc.
Cmt1s::~Cmt1s() { close(); }
//////////////////////////////////////////////////////////////////////////////////////////
// Close the serial communication port.
XsensResultValue Cmt1s::close(void)
{
#ifdef _LOG_RX_TX
        if (rx_log != nullptr) fclose(rx_log);
        if (tx_log != nullptr) fclose(tx_log);
        rx_log = nullptr;
        tx_log = nullptr;
#endif
        if (!m_isOpen) return m_lastResult = XRV_NOPORTOPEN;
 
#ifdef _WIN32
        ::FlushFileBuffers(m_handle);
        // read all data before closing the handle, a Flush is not enough for FTDI
        // devices unfortunately
        // we first need to set the COMM timeouts to instantly return when no more
        // data is available
        COMMTIMEOUTS cto;
        ::GetCommTimeouts(m_handle, &cto);
        cto.ReadIntervalTimeout = MAXDWORD;
        cto.ReadTotalTimeoutConstant = 0;
        cto.ReadTotalTimeoutMultiplier = 0;
        ::SetCommTimeouts(m_handle, &cto);
        char buffer[1024];
        DWORD length;
        do
        {
                ::ReadFile(m_handle, buffer, 1024, &length, nullptr);
        } while (length > 0);
        ::CloseHandle(m_handle);
#else
        ::close(m_handle);
#endif
        m_isOpen = false;
        m_endTime = 0;
 
        return m_lastResult = XRV_OK;
}
 
//////////////////////////////////////////////////////////////////////////////////////////
// Manipulate the Serial control lines
XsensResultValue Cmt1s::escape(
        const CmtControlLine mask, const CmtControlLine state)
{
        if (!m_isOpen) return (m_lastResult = XRV_NOPORTOPEN);
#ifdef _WIN32
        BOOL rv = 0;
        if (mask & CMT_CONTROL_DTR)
        {
                if (state & CMT_CONTROL_DTR)
                        rv = EscapeCommFunction(m_handle, SETDTR);
                else
                        rv = EscapeCommFunction(m_handle, CLRDTR);
        }
 
        if (mask & CMT_CONTROL_RTS)
        {
                if (state & CMT_CONTROL_RTS)
                        rv = EscapeCommFunction(m_handle, SETRTS);
                else
                        rv = EscapeCommFunction(m_handle, CLRRTS);
        }
        if (rv)
                return m_lastResult = XRV_OK;
        else
                return m_lastResult = XRV_ERROR;
#else
        bool rv = true;
        int32_t status;
        if (mask & CMT_CONTROL_DTR)
        {
                if (ioctl(m_handle, TIOCMGET, &status) == -1)
                {
                        if (state & CMT_CONTROL_DTR)
                                status |= TIOCM_DTR;
                        else
                                status &= ~TIOCM_DTR;
                        rv = (ioctl(m_handle, TIOCMSET, &status) == -1);
                }
                else
                        rv = false;
        }
        if (rv && (mask & CMT_CONTROL_RTS))
        {
                if (ioctl(m_handle, TIOCMGET, &status) == -1)
                {
                        if (state & CMT_CONTROL_RTS)
                                status |= TIOCM_RTS;
                        else
                                status &= ~TIOCM_RTS;
                        rv = (ioctl(m_handle, TIOCMSET, &status) == -1);
                }
                else
                        rv = false;
        }
        if (rv)
                return m_lastResult = XRV_OK;
        else
                return m_lastResult = XRV_ERROR;
#endif
}
 
//////////////////////////////////////////////////////////////////////////////////////////
// Flush all data to be transmitted / received.
XsensResultValue Cmt1s::flushData(void)
{
#ifdef _WIN32
        // Remove any 'old' data in buffer
        PurgeComm(m_handle, PURGE_TXCLEAR | PURGE_RXCLEAR);
#else
        tcflush(m_handle, TCIOFLUSH);
#endif
        m_endTime = 0;
        return (m_lastResult = XRV_OK);
}
 
//////////////////////////////////////////////////////////////////////////////////////////
// Open a communication channel to the given serial port name.
XsensResultValue Cmt1s::open(
        const char* portName, const uint32_t baudRate, uint32_t readBufSize,
        uint32_t writeBufSize)
{
        MRPT_UNUSED_PARAM(readBufSize);
        MRPT_UNUSED_PARAM(writeBufSize);
        m_endTime = 0;
 
        CMT1LOG("L1: Open port %s at %d baud\n", portName, baudRate);
 
        if (m_isOpen)
        {
                CMT1LOG("L1: Port already open\n");
                return (m_lastResult = XRV_ALREADYOPEN);
        }
        m_baudrate = baudRate;
 
#ifdef _WIN32
        char winPortName[32];
 
        // Open port
        sprintf(winPortName, "\\\\.\\%s", portName);
        m_handle = CreateFileA(
                winPortName, GENERIC_READ | GENERIC_WRITE, 0, nullptr, OPEN_EXISTING, 0,
                nullptr);
        if (m_handle == INVALID_HANDLE_VALUE)
        {
                CMT1LOG("L1: Port cannot be opened\n");
                return (m_lastResult = XRV_INPUTCANNOTBEOPENED);
        }
 
        // Once here, port is open
        m_isOpen = true;
 
        // Get the current state & then change it
        GetCommState(m_handle, &m_commState);  // Get current state
 
        m_commState.BaudRate = baudRate;  // Setup the baud rate
        m_commState.Parity = NOPARITY;  // Setup the Parity
        m_commState.ByteSize = 8;  // Setup the data bits
        m_commState.StopBits = TWOSTOPBITS;  // Setup the stop bits
        m_commState.fDsrSensitivity = FALSE;  // Setup the flow control
        m_commState.fOutxCtsFlow = FALSE;  // NoFlowControl:
        m_commState.fOutxDsrFlow = FALSE;
        m_commState.fOutX = FALSE;
        m_commState.fInX = FALSE;
        if (!SetCommState(m_handle, (LPDCB)&m_commState))
        {  // Set new state
                // Bluetooth ports cannot always be opened with 2 stopbits
                // Now try to open port with 1 stopbit.
                m_commState.StopBits = ONESTOPBIT;
                if (!SetCommState(m_handle, (LPDCB)&m_commState))
                {
                        CloseHandle(m_handle);
                        m_handle = INVALID_HANDLE_VALUE;
                        m_isOpen = false;
                        return (m_lastResult = XRV_INPUTCANNOTBEOPENED);
                }
        }
        m_port = atoi(&portName[3]);
        sprintf(m_portname, "%s", portName);
 
        setTimeout(m_timeout);
 
        // Other initialization functions
        EscapeCommFunction(m_handle, SETRTS);  // Enable RTS (for Xbus Master use)
        // Set DTR (Calibration sensors need DTR to startup, won't hurt otherwise
        EscapeCommFunction(m_handle, SETDTR);
        SetupComm(m_handle, readBufSize, writeBufSize);  // Set queue size
 
        // Remove any 'old' data in buffer
        // PurgeComm(m_handle, PURGE_TXCLEAR | PURGE_RXCLEAR);
        PurgeComm(
                m_handle,
                PURGE_TXABORT | PURGE_RXABORT | PURGE_TXCLEAR | PURGE_RXCLEAR);
#else  // !_WIN32
        // 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_lastResult = XRV_INPUTCANNOTBEOPENED;
        }
 
        // Once here, port is open
        m_isOpen = true;
 
        /* Start configuring of port for non-canonical transfer mode */
        // Get current options for the port
        tcgetattr(m_handle, &m_commState);
 
        // Set baudrate.
        cfsetispeed(&m_commState, baudRate);
        cfsetospeed(&m_commState, baudRate);
 
        // Enable the receiver and set local mode
        m_commState.c_cflag |= (CLOCAL | CREAD);
        // Set character size to data bits and set no parity Mask the characte size
        // bits
        m_commState.c_cflag &= ~(CSIZE | PARENB);
        m_commState.c_cflag |= CS8;  // Select 8 data bits
        m_commState.c_cflag |= CSTOPB;  // send 2 stop bits
        // Disable hardware flow control
        m_commState.c_cflag &= ~CRTSCTS;
        m_commState.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
        // Disable software flow control
        m_commState.c_iflag &=
                ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
        // Set Raw output
        m_commState.c_oflag &= ~OPOST;
        // Timeout 0.001 sec for first byte, read minimum of 0 bytes
        m_commState.c_cc[VMIN] = 0;
        m_commState.c_cc[VTIME] = (m_timeout + 99) / 100;  // 1
 
        // Set the new options for the port
        tcsetattr(m_handle, TCSANOW, &m_commState);
 
        m_port = 0;
        sprintf(m_portname, "%s", portName);
 
        tcflush(m_handle, TCIOFLUSH);
 
        // setting RTS and DTR; RTS for Xbus Master, DTR for calibration sensors
        int cmbits;
        if (ioctl(m_handle, TIOCMGET, &cmbits) < 0)
        {
                return (m_lastResult = XRV_ERROR);
        }
 
        cmbits |= TIOCM_RTS | TIOCM_DTR;
 
        if (ioctl(m_handle, TIOCMSET, &cmbits) < 0)
        {
                return (m_lastResult = XRV_ERROR);
        }
#endif  // !_WIN32
 
        CMT1LOG("L1: Port opened\n");
        return (m_lastResult = XRV_OK);
}
 
#ifdef _WIN32
//////////////////////////////////////////////////////////////////////////////////////////
// Open a communication channel to the given COM port number.
XsensResultValue Cmt1s::open(
        const uint32_t portNumber, const uint32_t baudRate, uint32_t readBufSize,
        uint32_t writeBufSize)
{
        char comFileName[32];
 
        // Create file name
        sprintf(comFileName, "COM%u", portNumber);
 
        return Cmt1s::open(comFileName, baudRate, readBufSize, writeBufSize);
}
#endif
 
//////////////////////////////////////////////////////////////////////////////////////////
// Read data from the serial port and put it into the data buffer.
XsensResultValue Cmt1s::readData(
        const uint32_t maxLength, uint8_t* data, uint32_t* length)
{
        CMT1LOG("L1: readData, maxlength=%u, length=%p\n", maxLength, length);
        uint32_t ln;
        if (length == nullptr) length = &ln;
 
        if (!m_isOpen) return (m_lastResult = XRV_NOPORTOPEN);
 
#ifdef _WIN32
        BOOL rres = ::ReadFile(m_handle, data, maxLength, (DWORD*)length, nullptr);
        if (m_onBytesReceived != nullptr && *length > 0)
        {
                CmtBinaryData* bytes = (CmtBinaryData*)malloc(sizeof(CmtBinaryData));
                bytes->m_size = *length;
                bytes->m_portNr = m_port;
                memcpy(bytes->m_data, data, *length);
#ifdef _LOG_CALLBACKS
                CMTLOG(
                        "C1: onBytesReceived(%d,(%d,%d),%p)\n",
                        (int32_t)m_onBytesReceivedInstance, (int32_t)bytes->m_size,
                        (int32_t)bytes->m_portNr, m_onBytesReceivedParam);
#endif
                m_onBytesReceived(
                        m_onBytesReceivedInstance, CMT_CALLBACK_ONBYTESRECEIVED, bytes,
                        m_onBytesReceivedParam);
        }
 
        if (!rres)
        {
                CMT1LOG("L1: readData, ReadFile returned error %u\n", ::GetLastError());
                return (m_lastResult = XRV_ERROR);
        }
#else
        *length = read(m_handle, data, maxLength);
#endif
 
#ifdef _LOG_RX_TX
        if (*length > 0)
        {
                if (rx_log == nullptr)
                {
                        char fname[CMT_MAX_FILENAME_LENGTH];
                        sprintf(fname, "rx_%03d_%d.log", (int32_t)m_port, m_baudrate);
                        rx_log = fopen(fname, "wb");
                }
                fwrite(data, 1, *length, rx_log);
        }
#endif
 
        CMT1LOG(
                (length[0] ? "L1: readData returned success, read %u of %u bytes, "
                                         "first: %02x\n"
                                   : "L1: readData returned success, read %u bytes\n"),
                length[0], maxLength, data[0]);
        return (m_lastResult = XRV_OK);
}
 
//////////////////////////////////////////////////////////////////////////////////////////
// Set the callback function for when bytes have been received
XsensResultValue Cmt1s::setCallbackFunction(
        CmtCallbackSelector tp, int32_t instance, CmtCallbackFunction func,
        void* param)
{
        if (tp == CMT_CALLBACK_ONBYTESRECEIVED)
        {
                m_onBytesReceived = func;
                m_onBytesReceivedInstance = instance;
                m_onBytesReceivedParam = param;
                return m_lastResult = XRV_OK;
        }
        return m_lastResult = XRV_INVALIDPARAM;
}
 
//////////////////////////////////////////////////////////////////////////////////////////
// Set the default timeout value to use in blocking operations.
XsensResultValue Cmt1s::setTimeout(const uint32_t ms)
{
        CMT1LOG("L1: Setting timeout to %u ms\n", ms);
 
        m_timeout = ms;
#ifdef _WIN32
        // Set COM timeouts
        COMMTIMEOUTS commTimeouts;
 
        GetCommTimeouts(m_handle, &commTimeouts);  // Fill CommTimeouts structure
 
        // immediate return if data is available, wait 1ms otherwise
        if (m_timeout > 0)
        {
                commTimeouts.ReadIntervalTimeout = 0;
                commTimeouts.ReadTotalTimeoutConstant = m_timeout;  // ms time
                commTimeouts.ReadTotalTimeoutMultiplier = 0;
                commTimeouts.WriteTotalTimeoutConstant = m_timeout;
                commTimeouts.WriteTotalTimeoutMultiplier = 0;
        }
        else
        {
                // immediate return whether data is available or not
                commTimeouts.ReadIntervalTimeout = MAXDWORD;
                commTimeouts.ReadTotalTimeoutConstant = 0;
                commTimeouts.ReadTotalTimeoutMultiplier = 0;
                commTimeouts.WriteTotalTimeoutConstant = 0;
                commTimeouts.WriteTotalTimeoutMultiplier = 0;
        }
 
        SetCommTimeouts(m_handle, &commTimeouts);  // Set CommTimeouts structure
#else
        // Timeout 0.1 sec for first byte, read minimum of 0 bytes
        m_commState.c_cc[VMIN] = 0;
        m_commState.c_cc[VTIME] = (m_timeout + 99) / 100;  // ds time
 
        // Set the new options for the port if it is open
        if (m_isOpen) tcsetattr(m_handle, TCSANOW, &m_commState);
#endif
        return (m_lastResult = XRV_OK);
}
 
//////////////////////////////////////////////////////////////////////////////////////////
// Wait for data to arrive or a timeout to occur.
XsensResultValue Cmt1s::waitForData(
        const uint32_t maxLength, uint8_t* data, uint32_t* length)
{
        CMT1LOG("L1: waitForData, mto=%u, length=%p\n", m_timeout, length);
        uint32_t timeout = m_timeout;
 
        uint32_t ln;
        if (length == nullptr) length = &ln;
        uint32_t eTime = getTimeOfDay(nullptr) + timeout;
        uint32_t newLength = 0;
 
        *length = 0;
        while ((*length < maxLength) && (getTimeOfDay() <= eTime))
        {
                readData(maxLength - *length, data + *length, &newLength);
                *length += newLength;
        }
        CMT1LOG(
                "L1: waitForData result: read %u of %u bytes\n", length[0], maxLength);
 
        if (length[0] < maxLength)
                return (m_lastResult = XRV_TIMEOUT);
        else
                return (m_lastResult = XRV_OK);
}
 
//////////////////////////////////////////////////////////////////////////////////////////
// Write the data to the serial port.
XsensResultValue Cmt1s::writeData(
        const uint32_t length, const uint8_t* data, uint32_t* written)
{
        uint32_t bytes;
        if (written == nullptr) written = &bytes;
 
        if (!m_isOpen) return (m_lastResult = XRV_NOPORTOPEN);
 
#ifdef _WIN32
        if (WriteFile(m_handle, data, length, (DWORD*)written, nullptr))
        {
#ifdef _LOG_RX_TX
                if (written[0] > 0)
                {
                        if (tx_log == nullptr)
                        {
                                char fname[CMT_MAX_FILENAME_LENGTH];
                                sprintf(fname, "tx_%03d_%d.log", (int32_t)m_port, m_baudrate);
                                tx_log = fopen(fname, "wb");
                        }
                        fwrite(data, 1, *written, tx_log);
                }
#endif
                return (m_lastResult = XRV_OK);
        }
        else
                return (m_lastResult = XRV_ERROR);
#else
        *written = write(m_handle, data, length);
        //      if (*written == length)
        return (m_lastResult = XRV_OK);
//      else
//              return (m_lastResult = XRV_ERROR);
#endif
}
 
//////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////// Cmt1f
////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////
 
//////////////////////////////////////////////////////////////////////////////////////////
// Default constructor, initializes all members to their default values.
Cmt1f::Cmt1f()
{
        m_readPos = 0;
        m_writePos = 0;
        m_lastResult = XRV_OK;
        m_reading = true;
        m_isOpen = false;
        m_filename[0] = '\0';
        m_fileSize = 0;
        m_readOnly = false;
        m_unicode = false;
}
 
//////////////////////////////////////////////////////////////////////////////////////////
// Destructor.
Cmt1f::~Cmt1f() { close(); }
//////////////////////////////////////////////////////////////////////////////////////////
// Write data to the end of the file.
XsensResultValue Cmt1f::appendData(const uint32_t length, const void* data)
{
        if (!m_isOpen) return m_lastResult = XRV_NOFILEOPEN;
        if (m_readOnly) return m_lastResult = XRV_READONLY;
 
        if (m_reading || m_writePos != m_fileSize)
        {
                m_reading = false;
                FSEEK_R(0);
        }
        fwrite(data, 1, length, m_handle);
        m_writePos = FTELL();
        m_fileSize = m_writePos;
 
        return (m_lastResult = XRV_OK);
}
 
//////////////////////////////////////////////////////////////////////////////////////////
// Close the file.
XsensResultValue Cmt1f::close(void)
{
        if (m_isOpen)
        {
#ifdef _WIN32
                fflush(m_handle);
                fclose(m_handle);
#else
                ::fflush(m_handle);
                ::fclose(m_handle);
#endif
        }
        m_isOpen = false;
        m_readPos = 0;
        m_writePos = 0;
        m_reading = true;
        m_fileSize = 0;
        m_readOnly = false;
 
        return m_lastResult = XRV_OK;
}
 
//////////////////////////////////////////////////////////////////////////////////////////
// Close the file and delete it.
XsensResultValue Cmt1f::closeAndDelete(void)
{
        if (m_isOpen)
        {
#ifdef _WIN32
                fflush(m_handle);
                fclose(m_handle);
#else
                ::fflush(m_handle);
                ::fclose(m_handle);
#endif
                if (m_readOnly)
                        m_lastResult = XRV_READONLY;
                else
                {
#ifdef _WIN32
                        if (m_unicode)
                        {
                                if (_wremove(m_filename_w) != 0)
                                        m_lastResult = XRV_READONLY;
                                else
                                        m_lastResult = XRV_OK;
                        }
                        else
#endif
                        {
#ifdef _WIN32
                                if (_unlink(m_filename) != 0)
#else
                                if (unlink(m_filename) != 0)
#endif
                                        m_lastResult = XRV_READONLY;
                                else
                                        m_lastResult = XRV_OK;
                        }
                }
        }
        else
                m_lastResult = XRV_NOFILEOPEN;
 
        m_isOpen = false;
        m_readPos = 0;
        m_writePos = 0;
        m_reading = true;
        m_fileSize = 0;
        m_readOnly = false;
 
        return m_lastResult;
}
 
//////////////////////////////////////////////////////////////////////////////////////////
// Create a new file.
XsensResultValue Cmt1f::create(const char* filename)
{
        if (m_isOpen) return m_lastResult = XRV_ALREADYOPEN;
 
        //! \test does this work for non-existing files? Or do we need a check and
        //! create?
        m_handle = fopen(filename, "w+b");  // open for update (r/w)
        if (m_handle == nullptr) return m_lastResult = XRV_OUTPUTCANNOTBEOPENED;
 
#ifdef _WIN32
        if (_fullpath(m_filename, filename, CMT_MAX_FILENAME_LENGTH) == nullptr)
        {
                fclose(m_handle);
                remove(filename);
                return m_lastResult = XRV_INVALIDPARAM;
        }
#else
        // based on the assumption that this doesn't concern the serial port, handle
        // it the same way using realpath(). Apparently realpath() doesn't require a
        // maximum length. One would possibly want to write a wrapper for it.
        if (realpath(filename, m_filename) == nullptr)
        {
                fclose(m_handle);
                remove(filename);
                return m_lastResult = XRV_INVALIDPARAM;
        }
#endif
        mbstowcs(m_filename_w, m_filename, CMT_MAX_FILENAME_LENGTH);
        m_unicode = false;
 
        m_isOpen = true;
        m_readPos = 0;
        m_writePos = 0;
        m_fileSize = 0;
        m_reading = true;
        m_readOnly = false;
        return m_lastResult = XRV_OK;
}
 
//////////////////////////////////////////////////////////////////////////////////////////
// Create a new file.
XsensResultValue Cmt1f::create(const wchar_t* filename)
{
        if (m_isOpen) return m_lastResult = XRV_ALREADYOPEN;
 
#ifdef _WIN32
        //! \test does this work for non-existing files? Or do we need a check and
        //! create?
        m_handle = _wfopen(filename, L"w+b");  // open for update (r/w)
        if (m_handle == nullptr) return m_lastResult = XRV_OUTPUTCANNOTBEOPENED;
 
        if (_wfullpath(m_filename_w, filename, CMT_MAX_FILENAME_LENGTH) == nullptr)
        {
                fclose(m_handle);
                _wremove(filename);
                return m_lastResult = XRV_INVALIDPARAM;
        }
        wcstombs(m_filename, m_filename_w, CMT_MAX_FILENAME_LENGTH);
 
        m_isOpen = true;
        m_readPos = 0;
        m_writePos = 0;
        m_fileSize = 0;
        m_reading = true;
        m_readOnly = false;
#else
        MRPT_UNUSED_PARAM(filename);
        char tFilename[CMT_MAX_FILENAME_LENGTH * 2];
        wcstombs(tFilename, m_filename_w, CMT_MAX_FILENAME_LENGTH);
        XsensResultValue res = create(tFilename);
        if (res != XRV_OK) return res;
#endif
        m_unicode = true;
        return m_lastResult = XRV_OK;
}
 
//////////////////////////////////////////////////////////////////////////////////////////
// Delete the given data from the file.
XsensResultValue Cmt1f::deleteData(
        const CmtFilePos start, const uint32_t length)
{
        if (!m_isOpen) return m_lastResult = XRV_NOFILEOPEN;
        if (m_readOnly) return m_lastResult = XRV_READONLY;
 
        gotoWrite();
 
        CmtFilePos wPos = start;
        CmtFilePos rPos = wPos + length;
 
        size_t read1;
        CmtFilePos endPos = (start + (CmtFilePos)length);
        if (endPos < m_fileSize)
        {
                CmtFilePos remaining = m_fileSize - endPos;
                char buffer[512];
 
                // copy data
                FSEEK(rPos);
 
                while (remaining > 0)
                {
                        if (remaining >= 512)
                                read1 = fread(buffer, 1, 512, m_handle);
                        else
                                read1 = fread(buffer, 1, (size_t)remaining, m_handle);
 
                        remaining -= read1;
                        rPos += read1;
 
                        // write block to the correct position
                        FSEEK(wPos);
                        wPos += fwrite(buffer, 1, read1, m_handle);
                        FSEEK(rPos);
                }
                m_fileSize -= length;
        }
        else
        {
                m_fileSize = start;
        }
 
#ifdef _WIN32
        int32_t rv = _chsize(_fileno(m_handle), (int32_t)m_fileSize);
#else
        int32_t rv = (ftruncate(fileno(m_handle), (int32_t)m_fileSize) == 0);
#endif
        int32_t eno = 0;
        if (rv != 0) eno = errno;
        m_writePos = start;
        FSEEK(wPos);
        if (rv != 0)
        {
                switch (eno)
                {
                        case EACCES:
                                return m_lastResult = XRV_BUSY;
                        case EBADF:
                                return m_lastResult = XRV_INVALIDINSTANCE;
                        case ENOSPC:
                                return m_lastResult = XRV_OUTOFMEMORY;
                        case EINVAL:
                                return m_lastResult = XRV_INVALIDPARAM;
                        default:
                                return m_lastResult = XRV_ERROR;
                }
        }
 
        return m_lastResult = XRV_OK;
}
 
//////////////////////////////////////////////////////////////////////////////////////////
// Find a string of bytes in the file
XsensResultValue Cmt1f::find(
        const void* needleV, const uint32_t needleLength, CmtFilePos& pos)
{
        if (!m_isOpen) return m_lastResult = XRV_NOFILEOPEN;
 
        const char* needle = (const char*)needleV;
 
        gotoRead();
 
        pos = 0;
 
        char buffer[512];
        uint32_t bufferPos, needlePos = 0;
        size_t readBytes;
        if (m_readPos & 0x1FF)  // read a block of data
                readBytes =
                        fread(buffer, 1, (512 - ((size_t)m_readPos & 0x1FF)), m_handle);
        else
                readBytes = fread(buffer, 1, 512, m_handle);  // read a block of data
 
        while (readBytes > 0)
        {
                m_readPos += readBytes;
                bufferPos = 0;
 
                while (bufferPos < readBytes && needlePos < needleLength)
                {
                        if (buffer[bufferPos] == needle[needlePos])
                        {
                                // found a byte
                                ++needlePos;
                        }
                        else
                        {
                                if (needlePos > 0)
                                        needlePos = 0;
                                else if (buffer[bufferPos] == needle[0])
                                {
                                        // found a byte
                                        needlePos = 1;
                                }
                        }
                        ++bufferPos;
                }
                if (needlePos < needleLength)
                        readBytes = fread(buffer, 1, 512, m_handle);  // read next block
                else
                {
                        m_readPos = m_readPos + bufferPos - readBytes -
                                                needleLength;  // or without needleLength
                        pos = m_readPos;  // - needleLength;
                        FSEEK(m_readPos);
                        return m_lastResult = XRV_OK;
                }
        }
        return m_lastResult = XRV_ENDOFFILE;
}
 
//////////////////////////////////////////////////////////////////////////////////////////
// Flush all data to be written.
XsensResultValue Cmt1f::flushData(void)
{
        fflush(m_handle);
 
        return m_lastResult = XRV_OK;
}
 
//////////////////////////////////////////////////////////////////////////////////////////
// Retrieve the filename that was last successfully opened.
XsensResultValue Cmt1f::getName(char* filename) const
{
        strcpy(filename, m_filename);
        return m_lastResult = XRV_OK;
}
 
//////////////////////////////////////////////////////////////////////////////////////////
// Retrieve the filename that was last successfully opened.
XsensResultValue Cmt1f::getName(wchar_t* filename) const
{
#ifdef _WIN32
        wcscpy(filename, m_filename_w);
#else
        mbstowcs(filename, m_filename, CMT_MAX_FILENAME_LENGTH);
#endif
        return m_lastResult = XRV_OK;
}
 
//////////////////////////////////////////////////////////////////////////////////////////
// Change from writing to reading mode
void Cmt1f::gotoRead(void)
{
        if (m_reading) return;
 
        FSEEK(m_readPos);
        m_reading = true;
}
 
//////////////////////////////////////////////////////////////////////////////////////////
// Change from reading to writing mode
void Cmt1f::gotoWrite(void)
{
        if (!m_reading) return;
 
        FSEEK(m_writePos);
        m_reading = false;
}
 
//////////////////////////////////////////////////////////////////////////////////////////
// Insert the given data into the file.
XsensResultValue Cmt1f::insertData(
        const CmtFilePos start, const uint32_t length, const void* data)
{
        if (!m_isOpen) return m_lastResult = XRV_NOFILEOPEN;
        if (m_readOnly) return m_lastResult = XRV_READONLY;
 
        gotoWrite();
 
        CmtFilePos rPos = start;
        CmtFilePos wPos = rPos + length;
 
        size_t read1, read2;
        CmtFilePos remaining = m_fileSize - start;
        size_t bsize = (length > 512) ? length : 512;
        char* buffer1 = (char*)malloc(bsize);
        char* buffer2 = (char*)malloc(bsize);
        char* btemp;
 
        // copy data
        FSEEK(rPos);
 
        if (remaining >= (CmtFilePos)bsize)
                read1 = fread(buffer1, 1, bsize, m_handle);
        else
                read1 = fread(buffer1, 1, (size_t)remaining, m_handle);
 
        remaining -= read1;
        rPos += read1;
 
        while (remaining > 0)
        {
                // move data to correct buffer
                read2 = read1;
                btemp = buffer1;
                buffer1 = buffer2;
                buffer2 = btemp;
 
                // read next block
                if (remaining >= (CmtFilePos)bsize)
                        read1 = fread(buffer1, 1, bsize, m_handle);
                else
                        read1 = fread(buffer1, 1, (size_t)remaining, m_handle);
 
                remaining -= read1;
                rPos += read1;
 
                // write block to the correct position
                FSEEK(wPos);
                wPos += fwrite(buffer2, 1, read2, m_handle);
                FSEEK(rPos);
        }
 
        FSEEK(wPos);
        wPos += fwrite(buffer1, 1, read1, m_handle);
 
        FSEEK(start);
        m_writePos = start + fwrite(data, 1, length, m_handle);
        m_fileSize += length;
 
        free(buffer1);
        free(buffer2);
        return m_lastResult = XRV_OK;
}
 
//////////////////////////////////////////////////////////////////////////////////////////
// Open a file.
XsensResultValue Cmt1f::open(
        const char* filename, const bool create, const bool readOnly)
{
        if (m_isOpen) return m_lastResult = XRV_ALREADYOPEN;
 
        //! \test does this work for non-existing files? Or do we need a check and
        //! create?
        m_readOnly = readOnly;
        if (readOnly)
                m_handle = fopen(filename, "rb");  // open for read only (r)
        else
                m_handle = fopen(filename, "r+b");  // open for update (r/w)
        if (m_handle == nullptr)
        {
                if (create)
                        m_handle = fopen(filename, "w+b");  // create for update (r/w)
                else
                {
                        m_handle = fopen(filename, "rb");  // open for read only (r)
                        m_readOnly = true;
                }
        }
        if (m_handle == nullptr) return m_lastResult = XRV_INPUTCANNOTBEOPENED;
 
#ifdef _WIN32
        if (_fullpath(m_filename, filename, CMT_MAX_FILENAME_LENGTH) == nullptr)
        {
                fclose(m_handle);
                return m_lastResult = XRV_INVALIDPARAM;
        }
#else
        // use the same trick again.
        if (realpath(filename, m_filename) == nullptr)
        {
                fclose(m_handle);
                return m_lastResult = XRV_INVALIDPARAM;
        }
#endif
        mbstowcs(m_filename_w, m_filename, CMT_MAX_FILENAME_LENGTH);
        m_unicode = false;
 
        m_isOpen = true;
        m_readPos = 0;
        m_writePos = 0;
        m_reading = true;
        FSEEK_R(0);
        m_fileSize = FTELL();
        FSEEK(0);
        return (m_lastResult = XRV_OK);
}
 
//////////////////////////////////////////////////////////////////////////////////////////
// Open a file.
XsensResultValue Cmt1f::open(
        const wchar_t* filename, const bool create, const bool readOnly)
{
        if (m_isOpen) return m_lastResult = XRV_ALREADYOPEN;
 
#ifdef _WIN32
        //! \test does this work for non-existing files? Or do we need a check and
        //! create?
        m_readOnly = readOnly;
        if (readOnly)
                m_handle = _wfopen(filename, L"rb");  // open for read only (r)
        else
                m_handle = _wfopen(filename, L"r+b");  // open for update (r/w)
        if (m_handle == nullptr)
        {
                if (create)
                        m_handle = _wfopen(filename, L"w+b");  // create for update (r/w)
                else
                {
                        m_handle = _wfopen(filename, L"rb");  // open for read only (r)
                        m_readOnly = true;
                }
        }
        if (m_handle == nullptr) return m_lastResult = XRV_INPUTCANNOTBEOPENED;
 
        if (_wfullpath(m_filename_w, filename, CMT_MAX_FILENAME_LENGTH) == nullptr)
        {
                fclose(m_handle);
                return m_lastResult = XRV_INVALIDPARAM;
        }
        wcstombs(m_filename, m_filename_w, CMT_MAX_FILENAME_LENGTH);
 
        m_isOpen = true;
        m_readPos = 0;
        m_writePos = 0;
        m_reading = true;
        FSEEK_R(0);
        m_fileSize = FTELL();
        FSEEK(0);
#else
        char tFilename[CMT_MAX_FILENAME_LENGTH * 2];
        wcstombs(tFilename, filename, CMT_MAX_FILENAME_LENGTH * 2);
        XsensResultValue res = open(tFilename, create, readOnly);
        if (res != XRV_OK) return res;
#endif
        m_unicode = true;
        return m_lastResult = XRV_OK;
}
 
//////////////////////////////////////////////////////////////////////////////////////////
// Read data from the file and put it into the data buffer.
XsensResultValue Cmt1f::readData(
        const uint32_t maxLength, void* data, uint32_t* length)
{
        if (!m_isOpen) return m_lastResult = XRV_NOFILEOPEN;
 
        if (maxLength == 0) return m_lastResult = XRV_OK;
 
        uint32_t len;
        if (length == nullptr) length = &len;
 
        gotoRead();
 
        length[0] = (uint32_t)fread(data, 1, maxLength, m_handle);
        if (length[0] == 0) return (m_lastResult = XRV_ENDOFFILE);
 
        m_readPos += length[0];
        return m_lastResult = XRV_OK;
}
 
//////////////////////////////////////////////////////////////////////////////////////////
// Read data from the file until the terminator and put it into the data buffer.
XsensResultValue Cmt1f::readData(
        const uint32_t maxLength, const char terminator, void* dataV,
        uint32_t* length)
{
        if (!m_isOpen) return m_lastResult = XRV_NOFILEOPEN;
 
        uint32_t len;
        if (length == nullptr) length = &len;
 
        char* data = (char*)dataV;
        int32_t readChar;
 
        gotoRead();
 
        *length = 0;
        readChar = (uint32_t)fgetc(m_handle);
 
        while (!feof(m_handle) && !ferror(m_handle))
        {
                data[*length] = (char)readChar;
                ++(*length);
                ++m_readPos;
 
                if (((char)readChar == terminator) || ((*length) >= maxLength))
                        return m_lastResult = XRV_OK;
        }
        return m_lastResult = XRV_ENDOFFILE;
}
 
//////////////////////////////////////////////////////////////////////////////////////////
// Set the new absolute read position
XsensResultValue Cmt1f::setReadPos(const CmtFilePos pos)
{
        if (!m_isOpen) return m_lastResult = XRV_NOFILEOPEN;
 
        if (m_readPos != pos)
        {
                m_readPos = pos;
                if (m_reading) FSEEK(m_readPos);
        }
 
        return m_lastResult = XRV_OK;
}
 
//////////////////////////////////////////////////////////////////////////////////////////
// Set the new absolute write position
XsensResultValue Cmt1f::setWritePos(const CmtFilePos pos)
{
        if (!m_isOpen) return m_lastResult = XRV_NOFILEOPEN;
        if (m_readOnly) return m_lastResult = XRV_READONLY;
 
        if (pos == -1)
        {
                if (m_reading) m_reading = false;
                FSEEK_R(0);
                m_writePos = FTELL();
        }
        else
        {
                if (m_writePos != pos)
                {
                        m_writePos = pos;
                        if (!m_reading) FSEEK(m_writePos);
                }
        }
 
        return m_lastResult = XRV_OK;
}
 
//////////////////////////////////////////////////////////////////////////////////////////
// Write data to the file.
XsensResultValue Cmt1f::writeData(const uint32_t length, const void* data)
{
        if (!m_isOpen) return m_lastResult = XRV_NOFILEOPEN;
        if (m_readOnly) return m_lastResult = XRV_READONLY;
 
        gotoWrite();
        m_writePos += fwrite(data, 1, length, m_handle);
 
        if (m_writePos > m_fileSize) m_fileSize = m_writePos;
 
        return m_lastResult = XRV_OK;
}
 
}  // end of xsens namespace