cmt2.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

/*! \file cmt2.cpp

    For information about objects in this file, see the appropriate header:
    \ref Cmt2.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-05, 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 "cmt2.h"

namespace xsens
{
#ifdef _LOG_CMT2
#define CMT2LOG CMTLOG
#else
#define CMT2LOG(...)
#endif

//////////////////////////////////////////////////////////////////////////////////////////
int32_t findValidMessage(const uint8_t* buffer, const uint16_t bufferLength)
{
    MessageHeader* hdr = nullptr;
    uint16_t pre = 0;
    uint16_t target;
    bool extended;
    uint16_t length;
    int32_t res;
    Message* msg;

    // find the preamble
    while (pre < bufferLength && (buffer[pre] != CMT_PREAMBLE)) ++pre;

    if (pre >= bufferLength) return -1;

    // check if the message is valid
    length = bufferLength - pre;
    if (length < CMT_LEN_MSGHEADERCS) return -1;

    // parse header

    hdr = (MessageHeader*)(buffer + pre);
    if (hdr->m_length == CMT_EXTLENCODE)
    {
        extended = true;
        if (length < CMT_LEN_MSGEXTHEADERCS) return -1;
    }
    else
        extended = false;

    // check the reported size
    target = extended
                 ? ((uint16_t)hdr->m_datlen.m_extended.m_length.m_high * 256 +
                    (uint16_t)hdr->m_datlen.m_extended.m_length.m_low) +
                       CMT_LEN_MSGEXTHEADERCS
                 : (uint16_t)(hdr->m_length) + CMT_LEN_MSGHEADERCS;
    if (target <= length)
    {
        // header seems to be ok, check checksum
        // check the checksum

        msg = new Message(buffer + pre, (uint16_t)target, (uint16_t)target);
        if (msg->isChecksumOk())
        {
            delete msg;
            return (int32_t)pre;
        }
        delete msg;
    }

    // try next message
    res = findValidMessage(buffer + pre + 1, length - 1);
    if (res == -1) return -1;
    return res + pre + 1;
}

//////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////// Cmt2s
////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////////////////////////
// Default constructor, initializes all members to their default values.
Cmt2s::Cmt2s()
{
    m_lastResult = XRV_OK;
    m_readBufferCount = 0;
    m_timeout = CMT2_DEFAULT_TIMEOUT;
    m_baudrate = CMT_DEFAULT_BAUD_RATE;
    m_toEnd = 0;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Destructor, de-initializes, frees memory allocated for buffers, etc.
Cmt2s::~Cmt2s() {}
XsensResultValue Cmt2s::close(void)
{
    if (m_cmt1s.getPortNr())
    {
        CMT2LOG("L2: Closing port %d\n", (int32_t)m_cmt1s.getPortNr());
        m_toEnd = 0;
        return m_lastResult = m_cmt1s.close();
    }
    else
        return m_lastResult = XRV_NOPORTOPEN;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Retrieve the port that the object is connected to.
XsensResultValue Cmt2s::getPortName(char* portname) const
{
    m_cmt1s.getPortName(portname);
    // we know there should be at least "/dev/x"
    if (strlen(portname) < 6) return m_lastResult = XRV_ERROR;
    return m_lastResult = XRV_OK;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Retrieve the port that the object is connected to.
XsensResultValue Cmt2s::getPortNr(uint8_t& port) const
{
    port = m_cmt1s.getPortNr();
    if (port == 0) return m_lastResult = XRV_ERROR;
    return m_lastResult = XRV_OK;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Retrieve the port that the object is connected to.
XsensResultValue Cmt2s::getPortNr(int32_t& port) const
{
    port = m_cmt1s.getPortNr();
    if (port == 0) return m_lastResult = XRV_ERROR;
    return m_lastResult = XRV_OK;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Open a communication channel to the given serial port name.
XsensResultValue Cmt2s::open(const char* portName, const uint32_t baudRate)
{
    CMT2LOG("L2: Opening port %s @baud %d\n", portName, baudRate);
    m_baudrate = baudRate;
    m_lastResult = m_cmt1s.open(portName, baudRate);
    m_toEnd = 0;
    CMT2LOG(
        "L2: Port open result %d: %s\n", (int32_t)m_lastResult,
        xsensResultText(m_lastResult));
    return m_lastResult;
}

#ifdef _WIN32
//////////////////////////////////////////////////////////////////////////////////////////
// Open a communication channel to the given COM port number.
XsensResultValue Cmt2s::open(const uint32_t portNumber, const uint32_t baudRate)
{
    CMT2LOG("L2: Opening port %d @baud %d\n", (int32_t)portNumber, baudRate);
    m_baudrate = baudRate;
    m_lastResult = m_cmt1s.open(portNumber, baudRate);
    m_toEnd = 0;
    CMT2LOG(
        "L2: Port open result %d: %s\n", (int32_t)m_lastResult,
        xsensResultText(m_lastResult));
    return m_lastResult;
}
#endif

//////////////////////////////////////////////////////////////////////////////////////////
// Read a message from the COM port.
XsensResultValue Cmt2s::readMessage(Message* rcv)
{
    auto* hdr = (MessageHeader*)m_readBuffer;
    uint16_t pre = 0;
    uint32_t length = 0;
    uint32_t target = 0;
    uint16_t i;
    bool extended = false;

    CMT2LOG("L2: readMessage started, bufferCount=%u\n", m_readBufferCount);

    if (m_readBufferCount == 0)
        m_readBuffer[0] = (uint8_t)~CMT_PREAMBLE;  // create a value that is
    // definitely NOT a preamble

    if (m_readBufferCount < CMT_MAXMSGLEN)
        m_lastResult = m_cmt1s.readData(
            CMT_MAXMSGLEN - m_readBufferCount, m_readBuffer + m_readBufferCount,
            &length);
    m_readBufferCount += (uint16_t)length;

    while (m_readBufferCount > 0)
    {
        while (m_readBufferCount > 0)
        {
            // find preamble
            while ((pre < m_readBufferCount) &&
                   (m_readBuffer[pre] != CMT_PREAMBLE))
                ++pre;

            if (pre == m_readBufferCount)
            {
                CMT2LOG("L2: readMessage no preamble found in buffer\n");
                m_readBufferCount = 0;
                return m_lastResult = XRV_TIMEOUT;
            }

            CMT2LOG(
                "L2: readMessage preamble found at position %u\n",
                (uint32_t)pre);
            // shift buffer to start
            if (pre)
            {
                m_readBufferCount -= pre;
                for (i = 0; i < m_readBufferCount; ++i)
                    m_readBuffer[i] = m_readBuffer[i + pre];
            }

            if (m_readBufferCount < CMT_LEN_MSGHEADERCS)
            {
                CMT2LOG("L2: readMessage not enough header data read\n");
                return m_lastResult = XRV_TIMEOUT;
            }

            // read header
            if (hdr->m_length == CMT_EXTLENCODE)
            {
                extended = true;
                if (m_readBufferCount < CMT_LEN_MSGEXTHEADERCS)
                {
                    CMT2LOG(
                        "L2: readMessage not enough extended header data "
                        "read\n");
                    return m_lastResult = XRV_TIMEOUT;
                }
            }
            else
                extended = false;

            // check the reported size
            target =
                (extended
                     ? ((uint16_t)hdr->m_datlen.m_extended.m_length.m_high *
                            256 +
                        (uint16_t)hdr->m_datlen.m_extended.m_length.m_low)
                     : (uint16_t)(hdr->m_length));
            CMT2LOG(
                "L2: readMessage bytes in buffer=%u, extended=%u, target=%u\n",
                (uint32_t)m_readBufferCount, (uint32_t)extended,
                (uint32_t)target);
            if ((uint32_t)target > (uint32_t)CMT_MAXDATALEN)
            {
                // skip current preamble
                pre = 1;
                CMT2LOG(
                    "L2: readMessage invalid message length %u\n",
                    (uint32_t)target);
                continue;
            }
            break;  // everything seems to be ok, get out of inner while() loop
        }

        // header seems to be ok, read until end and check checksum
        if (extended)
            target += CMT_LEN_MSGEXTHEADERCS;
        else
            target += CMT_LEN_MSGHEADERCS;

        // read the entire message
        if (m_readBufferCount < target)
        {
            CMT2LOG(
                "L2: readMessage readBufferCount %u < target %u\n",
                (uint32_t)m_readBufferCount, (uint32_t)target);
            return m_lastResult = XRV_TIMEOUT;
        }

        // check the checksum
        if (rcv->loadFromString(m_readBuffer, (uint16_t)target) == XRV_OK)
        {
            CMT2LOG("L2: readMessage OK\n");
            if (m_onMessageReceived != nullptr)
            {
                auto* bytes = (CmtBinaryData*)malloc(sizeof(CmtBinaryData));
                bytes->m_size = target;
                bytes->m_portNr = m_cmt1s.getPortNr();
                //              bytes->m_type = CMT_CALLBACK_ONMESSAGERECEIVED;
                memcpy(bytes->m_data, m_readBuffer, target);
#ifdef _LOG_CALLBACKS
                CMTLOG(
                    "C2: m_onMessageReceived(%d,(%d,%d),%p)\n",
                    (int32_t)m_onMessageReceivedInstance,
                    (int32_t)bytes->m_size, (int32_t)bytes->m_portNr,
                    m_onMessageReceivedParam);
#endif
                m_onMessageReceived(
                    m_onMessageReceivedInstance, CMT_CALLBACK_ONMESSAGERECEIVED,
                    bytes, m_onMessageReceivedParam);
            }

            m_readBufferCount -= (uint16_t)target;
            if (m_readBufferCount)
                for (i = 0; i < m_readBufferCount; ++i)
                    m_readBuffer[i] = m_readBuffer[i + target];

            return m_lastResult = XRV_OK;
        }
        CMT2LOG(
            "L2: readMessage invalid checksum %02x %02x %02x %02x %02x\n",
            rcv->getMessageStart()[0], rcv->getMessageStart()[1],
            rcv->getMessageStart()[2], rcv->getMessageStart()[3],
            rcv->getMessageStart()[4]);
        // skip current preamble
        pre = 1;
    }

    CMT2LOG("L2: readMessage timed out\n");
    // a timeout occurred
    return m_lastResult = XRV_TIMEOUT;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Set the callback function for when a message has been received or sent
XsensResultValue Cmt2s::setCallbackFunction(
    CmtCallbackSelector tp, int32_t instance, CmtCallbackFunction func,
    void* param)
{
    switch (tp)
    {
        case CMT_CALLBACK_ONMESSAGERECEIVED:
            m_onMessageReceived = func;
            m_onMessageReceivedInstance = instance;
            m_onMessageReceivedParam = param;
            return m_lastResult = XRV_OK;
        case CMT_CALLBACK_ONMESSAGESENT:
            m_onMessageSent = func;
            m_onMessageSentInstance = instance;
            m_onMessageSentParam = param;
            return m_lastResult = XRV_OK;
        default:
            break;
    }
    return m_lastResult = XRV_INVALIDPARAM;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Set the default timeout value to use in blocking operations.
XsensResultValue Cmt2s::setTimeout(const uint32_t ms)
{
    CMT2LOG("L2: Setting timeout to %u ms\n", ms);
    if ((m_lastResult = m_cmt1s.setTimeout(ms / 2)) != XRV_OK)
        return m_lastResult;  // this can't actually happen
    m_timeout = ms;
    m_toEnd = 0;
    return (m_lastResult = XRV_OK);
}

//////////////////////////////////////////////////////////////////////////////////////////
// Wait for a message to arrive.
XsensResultValue Cmt2s::waitForMessage(
    Message* rcv, const uint8_t msgId, uint32_t timeoutOverride,
    bool acceptErrorMessage)
{
    auto* hdr = (MessageHeader*)m_readBuffer;
    uint16_t pre = 0;
    uint32_t length = 0;
    uint32_t target;
    uint16_t i;
    bool extended;
    bool readsome = (m_readBufferCount > 0);
    uint32_t toRestore = m_toEnd;

    CMT2LOG(
        "L2: waitForMessage x%02x, TO=%u, TOend=%u, TOO=%u\n", (uint32_t)msgId,
        m_timeout, m_toEnd, timeoutOverride);

    // The end-time may be misinterpreted around midnight, where it may be
    // considered
    // expired even if this is not the case. However, this is extremely rare.
    if (m_toEnd == 0)
    {
        if (timeoutOverride != 0)
            m_toEnd = (getTimeOfDay() + (uint32_t)timeoutOverride) %
                      (XSENS_MS_PER_DAY);
        else
            m_toEnd =
                (getTimeOfDay() + (uint32_t)m_timeout) % (XSENS_MS_PER_DAY);
        if (m_toEnd == 0) m_toEnd = 1;
    }

    if (m_readBufferCount == 0)
        m_readBuffer[0] = (uint8_t)~CMT_PREAMBLE;  // create a value that is
    // definitely NOT a preamble

    do
    {
        // find preamble
        while (m_readBuffer[pre] != CMT_PREAMBLE)
        {
            if ((++pre) >= m_readBufferCount)
            {
                m_readBufferCount = 0;
                pre = 0;
                if (m_toEnd >= getTimeOfDay())
                {
                    m_lastResult = m_cmt1s.readData(
                        CMT_LEN_MSGHEADER, m_readBuffer, &length);
                    m_readBufferCount = (uint16_t)length;
                    if (m_readBufferCount > 0) readsome = true;
                }
            }
            if (m_toEnd < getTimeOfDay()) break;
        }
        // shift buffer to start
        if (pre)
        {
            m_readBufferCount -= pre;
            for (i = 0; i < m_readBufferCount; ++i)
            {
                m_readBuffer[i] = m_readBuffer[i + pre];
            }
        }

        pre = 1;  // make sure we skip the first item in the next iteration
        // read header
        while (m_readBufferCount < CMT_LEN_MSGHEADERCS &&
               (m_toEnd >= getTimeOfDay()))
        {
            m_cmt1s.readData(
                CMT_LEN_MSGHEADERCS - m_readBufferCount,
                &m_readBuffer[m_readBufferCount], &length);
            m_readBufferCount += (uint16_t)length;
        }
        if ((m_readBufferCount < CMT_LEN_MSGHEADERCS) &&
            (m_toEnd < getTimeOfDay()))
        {
            CMT2LOG(
                "L2: waitForMessage timeout occurred trying to read header\n");
            break;
        }

        if (hdr->m_length == CMT_EXTLENCODE)
        {
            extended = true;
            while (m_readBufferCount < CMT_LEN_MSGEXTHEADERCS &&
                   (m_toEnd >= getTimeOfDay()))
            {
                m_cmt1s.readData(
                    CMT_LEN_MSGEXTHEADERCS - m_readBufferCount,
                    &m_readBuffer[m_readBufferCount], &length);
                m_readBufferCount += (uint16_t)length;
            }
            if ((m_readBufferCount < CMT_LEN_MSGEXTHEADERCS) &&
                (m_toEnd < getTimeOfDay()))
            {
                CMT2LOG(
                    "L2: waitForMessage timeout occurred trying to read "
                    "extended header\n");
                break;
            }
        }
        else
            extended = false;

        // check the reported size
        if (extended &&
            (((uint16_t)hdr->m_datlen.m_extended.m_length.m_high * 256 +
              (uint16_t)hdr->m_datlen.m_extended.m_length.m_low) >
             (uint16_t)CMT_MAXDATALEN))
            continue;

        // header seems to be ok, read until end and check checksum
        if (extended)
            target = ((uint16_t)hdr->m_datlen.m_extended.m_length.m_high * 256 +
                      (uint16_t)hdr->m_datlen.m_extended.m_length.m_low) +
                     CMT_LEN_MSGEXTHEADERCS;
        else
            target = hdr->m_length + CMT_LEN_MSGHEADERCS;

        // read the entire message
        while ((m_readBufferCount < target) && (m_toEnd >= getTimeOfDay()))
        {
            m_cmt1s.readData(
                target - m_readBufferCount, &m_readBuffer[m_readBufferCount],
                &length);
            m_readBufferCount += (uint16_t)length;
        }
        if ((m_readBufferCount < target) && (m_toEnd < getTimeOfDay()))
        {
            CMT2LOG("L2: waitForMessage timeout occurred\n");
            break;
        }

        // check the checksum
        // msg=new Message(m_readBuffer,(uint16_t) target, (uint16_t) target);
        if (rcv->loadFromString(m_readBuffer, (uint16_t)target) == XRV_OK)
        {
            CMT2LOG(
                "L2: waitForMessage received msg Id x%02x while expecting "
                "x%02x, msg size=%u\n",
                (uint32_t)rcv->getMessageId(), (uint32_t)msgId, target);
            if (m_onMessageReceived != nullptr)
            {
                auto* bytes = (CmtBinaryData*)malloc(sizeof(CmtBinaryData));
                bytes->m_size = target;
                bytes->m_portNr = m_cmt1s.getPortNr();
                //              bytes->m_type = CMT_CALLBACK_ONMESSAGERECEIVED;
                memcpy(bytes->m_data, m_readBuffer, target);
#ifdef _LOG_CALLBACKS
                CMTLOG(
                    "C2: m_onMessageReceived(%d,(%d,%d),%p)\n",
                    (int32_t)m_onMessageReceivedInstance,
                    (int32_t)bytes->m_size, (int32_t)bytes->m_portNr,
                    m_onMessageReceivedParam);
#endif
                m_onMessageReceived(
                    m_onMessageReceivedInstance, CMT_CALLBACK_ONMESSAGERECEIVED,
                    bytes, m_onMessageReceivedParam);
            }

            m_readBufferCount -= (uint16_t)target;
            if (m_readBufferCount)
            {
                for (i = 0; i < m_readBufferCount; ++i)
                {
                    m_readBuffer[i] = m_readBuffer[i + target];
                }
            }

            if ((msgId == 0) || (msgId == rcv->getMessageId()) ||
                (acceptErrorMessage && rcv->getMessageId() == CMT_MID_ERROR))
            {
                m_toEnd = toRestore;
                return m_lastResult = XRV_OK;
            }
        }
        else
        {
            rcv->clear();
            CMT2LOG("L2: waitForMessage load from string failed\n");
        }
    } while (m_toEnd >= getTimeOfDay());

    // a timeout occurred
    if (readsome)
    {
        // check if the current data contains a valid message
        int32_t pos = findValidMessage(m_readBuffer, m_readBufferCount);

        if (pos != -1)
        {
            CMT2LOG("L2: waitForMessage found message in message\n");
            // shift data to start of buffer
            pre = (uint16_t)pos;
            m_readBufferCount -= pre;
            for (i = 0; i < m_readBufferCount; ++i)
            {
                m_readBuffer[i] = m_readBuffer[i + pre];
            }
            waitForMessage(
                rcv, msgId, 0, acceptErrorMessage);  // parse the message
            m_toEnd = toRestore;
            return m_lastResult;  // set by waitForMessage
        }

        m_lastResult = XRV_TIMEOUT;
    }
    else
        m_lastResult = XRV_TIMEOUTNODATA;
    m_toEnd = toRestore;
    return m_lastResult;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Send a message over the COM port.
XsensResultValue Cmt2s::writeMessage(Message* msg)
{
    CMT2LOG(
        "L2: writeMessage %2x %2x %2x %2x %2x\n",
        (int32_t)msg->getMessageStart()[0], (int32_t)msg->getMessageStart()[1],
        (int32_t)msg->getMessageStart()[2], (int32_t)msg->getMessageStart()[3],
        (int32_t)msg->getMessageStart()[4]);
    uint32_t written = 0;
    m_lastResult = m_cmt1s.writeData(
        msg->getTotalMessageSize(), msg->getMessageStart(), &written);

    if (m_lastResult != XRV_OK)
    {
        CMT2LOG(
            "L2: writeMessage returns %d: %s\n", (int32_t)m_lastResult,
            xsensResultText(m_lastResult));
        return m_lastResult;
    }

    if (written != msg->getTotalMessageSize())
    {
        CMT2LOG(
            "L2: writeMessage wrote %u of %u bytes, returns %d: %s\n", written,
            msg->getTotalMessageSize(), (int32_t)XRV_ERROR,
            xsensResultText(XRV_ERROR));
        return (m_lastResult = XRV_ERROR);
    }

    if (m_onMessageSent != nullptr)
    {
        auto* bytes = (CmtBinaryData*)malloc(sizeof(CmtBinaryData));
        bytes->m_size = msg->getTotalMessageSize();
        bytes->m_portNr = m_cmt1s.getPortNr();
        memcpy(
            bytes->m_data, msg->getMessageStart(), msg->getTotalMessageSize());
#ifdef _LOG_CALLBACKS
        CMTLOG(
            "C2: m_onMessageSent(%d,(%d,%d),%p)\n",
            (int32_t)m_onMessageSentInstance, (int32_t)bytes->m_size,
            (int32_t)bytes->m_portNr, m_onMessageSentParam);
#endif
        m_onMessageSent(
            m_onMessageSentInstance, CMT_CALLBACK_ONMESSAGESENT, bytes,
            m_onMessageSentParam);
    }

    CMT2LOG("L2: writeMessage successful\n");
    return (m_lastResult = XRV_OK);
}

//////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////// Cmt2f
////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////////////////////////
// Default constructor
Cmt2f::Cmt2f()
{
    m_lastResult = XRV_OK;
    m_readOnly = true;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Destructor.
Cmt2f::~Cmt2f() { close(); }
//////////////////////////////////////////////////////////////////////////////////////////
// Close the file.
XsensResultValue Cmt2f::close(void)
{
    if (!m_cmt1f.isOpen()) return m_lastResult = XRV_NOFILEOPEN;

    // save any unsaved data
    // close the file
    m_cmt1f.close();
    m_readOnly = true;
    return m_lastResult = XRV_OK;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Close the file and delete it.
XsensResultValue Cmt2f::closeAndDelete(void)
{
    if (!m_cmt1f.isOpen()) return m_lastResult = XRV_NOFILEOPEN;

    // close the file
    m_cmt1f.closeAndDelete();
    m_readOnly = true;
    return m_lastResult = XRV_OK;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Create a new file with level 2 header
XsensResultValue Cmt2f::create(const char* filename)
{
    if (m_cmt1f.isOpen()) return m_lastResult = XRV_ALREADYOPEN;

    // create file
    m_lastResult = m_cmt1f.create(filename);
    if (m_lastResult != XRV_OK) return m_lastResult;

    m_readOnly = false;

    // check if we can actually write to the file
    m_lastResult = m_cmt1f.writeData(5, "Xsens");
    if (m_lastResult == XRV_OK) m_lastResult = m_cmt1f.deleteData(0, 5);
    if (m_lastResult != XRV_OK) m_cmt1f.close();
    return m_lastResult;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Create a new file with level 2 header
XsensResultValue Cmt2f::create(const wchar_t* filename)
{
    if (m_cmt1f.isOpen()) return m_lastResult = XRV_ALREADYOPEN;

    // create file
    m_lastResult = m_cmt1f.create(filename);
    if (m_lastResult != XRV_OK) return m_lastResult;

    m_readOnly = false;

    // check if we can actually write to the file
    m_lastResult = m_cmt1f.writeData(5, "Xsens");
    if (m_lastResult == XRV_OK) m_lastResult = m_cmt1f.deleteData(0, 5);
    if (m_lastResult != XRV_OK) m_cmt1f.close();
    return m_lastResult;
}

//////////////////////////////////////////////////////////////////////////////////////////
Cmt1f* Cmt2f::getCmt1f(void) { return &m_cmt1f; }
//////////////////////////////////////////////////////////////////////////////////////////
// Return the error code of the last operation.
XsensResultValue Cmt2f::getLastResult(void) const { return m_lastResult; }
//////////////////////////////////////////////////////////////////////////////////////////
// Retrieve the filename that was last successfully opened.
XsensResultValue Cmt2f::getName(char* filename) const
{
    return m_lastResult = m_cmt1f.getName(filename);
}

//////////////////////////////////////////////////////////////////////////////////////////
// Retrieve the filename that was last successfully opened.
XsensResultValue Cmt2f::getName(wchar_t* filename) const
{
    return m_lastResult = m_cmt1f.getName(filename);
}

//////////////////////////////////////////////////////////////////////////////////////////
// Return whether the file is open or not.
bool Cmt2f::isOpen(void) const { return m_cmt1f.isOpen(); }
//////////////////////////////////////////////////////////////////////////////////////////
// Open a file and read the header
XsensResultValue Cmt2f::open(const char* filename, const bool readOnly)
{
    if (m_cmt1f.isOpen()) return m_lastResult = XRV_ALREADYOPEN;
    m_lastResult = m_cmt1f.open(filename, !readOnly, readOnly);
    m_readOnly = readOnly;
    return m_lastResult;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Open a file and read the header
XsensResultValue Cmt2f::open(const wchar_t* filename, const bool readOnly)
{
    if (m_cmt1f.isOpen()) return m_lastResult = XRV_ALREADYOPEN;
    m_lastResult = m_cmt1f.open(filename, !readOnly, readOnly);
    m_readOnly = readOnly;
    return m_lastResult;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Read the next message from the file
XsensResultValue Cmt2f::readMessage(Message* msg, const uint8_t msgId)
{
    CmtFilePos pos;
    uint8_t needle = CMT_PREAMBLE;
    uint8_t buffer[CMT_MAXMSGLEN];
    uint32_t length, bcount;
    auto* hdr = (MessageHeader*)buffer;
    bool extended;
    uint16_t target;

    while (m_lastResult == XRV_OK)
    {
        bcount = 0;

        // find a message preamble
        m_lastResult = m_cmt1f.find(&needle, 1, pos);

        if (m_lastResult != XRV_OK) return m_lastResult;

        // read header
        m_lastResult = m_cmt1f.readData(CMT_LEN_MSGHEADERCS, buffer, &length);
        bcount += length;
        if (m_lastResult != XRV_OK) return m_lastResult;

        if (hdr->m_length == CMT_EXTLENCODE)
        {
            extended = true;
            m_lastResult = m_cmt1f.readData(
                CMT_LEN_MSGEXTHEADERCS - bcount, &buffer[bcount], &length);
            bcount += length;
            if (m_lastResult != XRV_OK)
            {
                m_cmt1f.setReadPos(pos + 1);
                continue;
            }
        }
        else
            extended = false;

        // check the reported size
        if (extended &&
            (((uint16_t)hdr->m_datlen.m_extended.m_length.m_high * 256 +
              (uint16_t)hdr->m_datlen.m_extended.m_length.m_low) >
             (uint16_t)CMT_MAXDATALEN))
        {
            m_cmt1f.setReadPos(pos + 1);
            continue;
        }

        // header seems to be ok, read until end and check checksum
        if (extended)
            target = ((uint16_t)hdr->m_datlen.m_extended.m_length.m_high * 256 +
                      (uint16_t)hdr->m_datlen.m_extended.m_length.m_low) +
                     CMT_LEN_MSGEXTHEADERCS;
        else
            target = hdr->m_length + CMT_LEN_MSGHEADERCS;

        // read the entire message
        m_lastResult =
            m_cmt1f.readData(target - bcount, &buffer[bcount], &length);
        bcount += length;
        if (m_lastResult != XRV_OK)
        {
            m_cmt1f.setReadPos(pos + 1);
            continue;
        }

        // check the checksum
        // msg=new Message(m_readBuffer,(uint16_t) target, (uint16_t) target);
        if (msg->loadFromString(buffer, (uint16_t)target) == XRV_OK)
        {
            if ((msgId == 0) || (msgId == msg->getMessageId()))
                return m_lastResult = XRV_OK;
            pos += target - 1;
        }
        msg->clear();
        m_cmt1f.setReadPos(pos + 1);
    }
    return m_lastResult;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Get the current file size
CmtFilePos Cmt2f::getFileSize(void) { return m_cmt1f.getFileSize(); }
//////////////////////////////////////////////////////////////////////////////////////////
// Get the current read position of the file
CmtFilePos Cmt2f::getReadPosition(void) { return m_cmt1f.getReadPos(); }
//////////////////////////////////////////////////////////////////////////////////////////
// Set the read position to the given position
XsensResultValue Cmt2f::setReadPosition(CmtFilePos pos)
{
    return m_lastResult = m_cmt1f.setReadPos(pos);
}

//////////////////////////////////////////////////////////////////////////////////////////
// Write a message to the end of the file
XsensResultValue Cmt2f::writeMessage(const Message* msg)
{
    if (m_readOnly) return m_lastResult = XRV_READONLY;
    return m_lastResult = m_cmt1f.appendData(
               msg->getTotalMessageSize(), msg->getMessageStart());
}

}  // namespace xsens