mtbfilecommunicator.cpp

Go to the documentation of this file.

 
//  Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide.
//  All rights reserved.
//  
//  Redistribution and use in source and binary forms, with or without modification,
//  are permitted provided that the following conditions are met:
//  
//  1.  Redistributions of source code must retain the above copyright notice,
//      this list of conditions, and the following disclaimer.
//  
//  2.  Redistributions in binary form must reproduce the above copyright notice,
//      this list of conditions, and the following disclaimer in the documentation
//      and/or other materials provided with the distribution.
//  
//  3.  Neither the names of the copyright holders nor the names of their contributors
//      may be used to endorse or promote products derived from this software without
//      specific prior written permission.
//  
//  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
//  EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
//  MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL
//  THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
//  SPECIAL, EXEMPLARY OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 
//  OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
//  HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY OR
//  TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
//  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.THE LAWS OF THE NETHERLANDS 
//  SHALL BE EXCLUSIVELY APPLICABLE AND ANY DISPUTES SHALL BE FINALLY SETTLED UNDER THE RULES 
//  OF ARBITRATION OF THE INTERNATIONAL CHAMBER OF COMMERCE IN THE HAGUE BY ONE OR MORE 
//  ARBITRATORS APPOINTED IN ACCORDANCE WITH SAID RULES.
//  
 
 
//  Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide.
//  All rights reserved.
//  
//  Redistribution and use in source and binary forms, with or without modification,
//  are permitted provided that the following conditions are met:
//  
//  1.  Redistributions of source code must retain the above copyright notice,
//      this list of conditions, and the following disclaimer.
//  
//  2.  Redistributions in binary form must reproduce the above copyright notice,
//      this list of conditions, and the following disclaimer in the documentation
//      and/or other materials provided with the distribution.
//  
//  3.  Neither the names of the copyright holders nor the names of their contributors
//      may be used to endorse or promote products derived from this software without
//      specific prior written permission.
//  
//  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
//  EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
//  MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL
//  THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
//  SPECIAL, EXEMPLARY OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 
//  OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
//  HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY OR
//  TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
//  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.THE LAWS OF THE NETHERLANDS 
//  SHALL BE EXCLUSIVELY APPLICABLE AND ANY DISPUTES SHALL BE FINALLY SETTLED UNDER THE RULES 
//  OF ARBITRATION OF THE INTERNATIONAL CHAMBER OF COMMERCE IN THE HAGUE BY ONE OR MORE 
//  ARBITRATORS APPOINTED IN ACCORDANCE WITH SAID RULES.
//  
 
#include "mtbfilecommunicator.h"
#include "xsdevice_def.h"
#include "xsdeviceconfiguration.h"
 
#include <xscommon/threading.h>
#include <xscommon/xprintf.h>
#include <xscommon/xsens_debugtools.h>
#include <xscommon/xsens_janitors.h>
#include <xstypes/xsportinfo.h>
#include "iointerfacefile.h"
#include "messageextractor.h"
 
using namespace xsens;
 
FileLoader::~FileLoader()
{
}
 
Communicator* MtbFileCommunicator::construct()
{
        return new MtbFileCommunicator;
}
 
MtbFileCommunicator::MtbFileCommunicator()
        : Communicator()
        , m_abortLoadLogFile(false)
        , m_loadFileTaskId(0)
        , m_extractor(nullptr)
        , m_extractedMessages(new std::deque<XsMessage>)
{
        m_extractor = new MessageExtractor(protocolManager());
        for (auto p : *protocolManager())
                p->ignoreMaximumMessageSize(true);
}
 
MtbFileCommunicator::MtbFileCommunicator(std::shared_ptr<IoInterfaceFile> const& ioInterfaceFile)
        : Communicator()
        , m_ioInterfaceFile(ioInterfaceFile)
        , m_abortLoadLogFile(false)
        , m_loadFileTaskId(0)
        , m_extractor(nullptr)
        , m_extractedMessages(new std::deque<XsMessage>)
{
        m_extractor = new MessageExtractor(protocolManager());
        for (auto p : *protocolManager())
                p->ignoreMaximumMessageSize(true);
}
 
MtbFileCommunicator::~MtbFileCommunicator()
{
        delete m_extractor;
        delete m_extractedMessages;
}
 
uint32_t MtbFileCommunicator::timeoutToMaxMessages(uint32_t timeout) const
{
        return timeout / 20; // 100 ms corresponds to 5 messages. Reasonable, no?
}
 
bool MtbFileCommunicator::doTransaction(const XsMessage& msg, XsMessage& rcv, uint32_t timeout)
{
        XsXbusMessageId expected = static_cast<XsXbusMessageId>(msg.getMessageId() + 1);
        std::deque<XsMessage> messages = readMessagesFromStartOfFile(expected, (int) timeoutToMaxMessages(timeout));
        rcv.clear();
        for (const XsMessage& r : messages)
        {
                if (r.getBusId() != msg.getBusId())
                        continue;
 
                if (msg.getMessageId() == XMID_ReqFrameRates && msg.getDataShort() != r.getDataShort()) // data identifier
                        continue;
 
                rcv = r;
                return true;
        }
        return (lastResult() == XRV_OK && !rcv.empty());
}
 
void MtbFileCommunicator::prepareForDestruction()
{
        abortLoadLogFile();
        waitForLastTaskCompletion();
 
        m_abortLoadLogFile = true;
        completeAllThreadedWork();
 
        Communicator::prepareForDestruction();
}
 
void MtbFileCommunicator::completeAllThreadedWork()
{
        ThreadPool::instance()->waitForCompletion(m_loadFileTaskId);
}
 
void MtbFileCommunicator::waitForLastTaskCompletion()
{
        completeAllThreadedWork();
}
 
/*  \brief Closes the log file
*/
void MtbFileCommunicator::closeLogFile()
{
        m_ioInterfaceFile.reset();
}
 
XsMessage MtbFileCommunicator::readMessageFromStartOfFile(uint8_t msgId, int maxMsgs)
{
        if (!m_ioInterfaceFile)
        {
                setLastResult(XRV_INVALIDOPERATION);
                return XsMessage();
        }
 
        // store current read state and make sure we reset to it when we're done
        auto oldPos = logFileReadPosition();
        MessageExtractor* ex = m_extractor;
        auto exm = m_extractedMessages;
        m_extractor = new MessageExtractor(protocolManager());
        m_extractedMessages = new std::deque<XsMessage>;
        JanitorStdFunc0<> resetExtractor([this, ex, exm, oldPos]()
        {
                delete this->m_extractor;
                this->m_extractor = ex;
                this->m_extractedMessages = exm;
                this->m_ioInterfaceFile->setReadPosition(oldPos);
        });
 
        // start reading from start of file
        m_ioInterfaceFile->setReadPosition(0);
 
        if (maxMsgs == 0)
                return readMessage(msgId);
 
        for (int count = 0; count < maxMsgs; ++count)
        {
                XsMessage msg = readMessage();
                if (msgId == 0 || msg.getMessageId() == msgId)
                        return msg;
                if (m_ioInterfaceFile->getLastResult() != XRV_OK)
                {
                        setLastResult(m_ioInterfaceFile->getLastResult());
                        return XsMessage();
                }
        }
 
        setLastResult(XRV_OTHER);
        return XsMessage();
}
 
std::deque<XsMessage> MtbFileCommunicator::readMessagesFromStartOfFile(uint8_t msgId, int maxMsgs)
{
        std::deque<XsMessage> rv;
 
        if (!m_ioInterfaceFile)
        {
                setLastResult(XRV_INVALIDOPERATION);
                return rv;
        }
 
        // store current read state and make sure we reset to it when we're done
        auto oldPos = logFileReadPosition();
        MessageExtractor* ex = m_extractor;
        auto exm = m_extractedMessages;
        m_extractor = new MessageExtractor(protocolManager());
        m_extractedMessages = new std::deque<XsMessage>;
        JanitorStdFunc0<> resetExtractor([this, ex, exm, oldPos]()
        {
                delete this->m_extractedMessages;
                delete this->m_extractor;
                this->m_extractor = ex;
                this->m_extractedMessages = exm;
                this->m_ioInterfaceFile->setReadPosition(oldPos);
        });
 
        // start reading from start of file
        m_ioInterfaceFile->setReadPosition(0);
 
        if (maxMsgs)
        {
                for (int count = 0; count < maxMsgs; ++count)
                {
                        XsMessage msg = readMessage();
                        if (lastResult() != XRV_OK)
                                break;
                        else if (msgId == 0 || msg.getMessageId() == msgId)
                                rv.push_back(msg);
                }
        }
        else
        {
                XsMessage msg = readMessage(msgId);
                while (lastResult() == XRV_OK)
                {
                        if (msgId == 0 || msg.getMessageId() == msgId)
                                rv.push_back(msg);
                        msg = readMessage(msgId);
                }
        }
 
        if (lastResult() == XRV_OTHER || (lastResult() == XRV_ENDOFFILE && m_ioInterfaceFile->getFileSize()))
                setLastResult(XRV_OK);
        if (lastResult() == XRV_ENDOFFILE)
                setLastResult(XRV_NODATA);
 
        return rv;
}
 
class Xs4FileTask : public ThreadPoolTask
{
public:
 
        Xs4FileTask(FileLoader* inf, XsDevice* device)
                : ThreadPoolTask()
                , m_loader(inf)
                , m_device(device)
                , m_thread(this)
        {
                assert(m_loader != 0);
                assert(m_device != 0);
        }
 
        bool exec() override
        {
                // short sleep to prevent thread starvation
                XsTime::msleep(1);
 
                if (m_thread.m_done)
                        return true;
 
                if (!m_thread.isRunning())
                {
                        JLDEBUGG("Starting dedicated read thread");
                        m_thread.startThread();
                }
 
                // reschedule
                return false;
        }
 
        virtual ~Xs4FileTask()
        {
                if (m_thread.isAlive())
                        m_thread.stopThread();
        }
 
private:
        FileLoader* m_loader;   
        XsDevice* m_device;
 
        class ReaderThread : public StandardThread
        {
        public:
                ReaderThread(Xs4FileTask* task);
                int32_t innerFunction(void) override;
                Xs4FileTask* m_task;
                bool m_done;
        } m_thread;
 
        friend class ReaderThread;
};
 
Xs4FileTask::ReaderThread::ReaderThread(Xs4FileTask* task)
        : m_task(task)
        , m_done(false)
{
}
 
int32_t Xs4FileTask::ReaderThread::innerFunction(void)
{
#ifndef ANDROID // Invoking XsDevice::logFileName() results in a crash on Android for some reason...
        {
                XsString filename = m_task->m_device->logFileName();
                std::string name = xprintf("FileReader: %s", filename.c_str());
                xsNameThisThread(name.c_str());
                JLDEBUGG("Loading file " << filename);
        }
#endif
 
        try
        {
                m_task->m_loader->readLogFile(m_task->m_device);
        }
        catch (XsException const& e)
        {
                m_task->m_device->onError(m_task->m_device, e.code());
                JLALERTG(e.what());
        }
        m_done = true;
        stopThread();
        return 0;
}
 
void MtbFileCommunicator::loadLogFile(XsDevice* device)
{
        assert(device != 0);
 
        abortLoadLogFile();
        waitForLastTaskCompletion();
 
        m_abortLoadLogFile = false;
        Xs4FileTask* tsk = new Xs4FileTask(this, device);
        m_loadFileTaskId = ThreadPool::instance()->addTask(tsk, m_loadFileTaskId);
}
 
XsResultValue MtbFileCommunicator::readLogFile(XsDevice* device)
{
        assert(device != 0);
        JLDEBUGG("");
 
        XsResultValue res = XRV_OK;
        XsFilePos prevFilePos = -1;
        XsString id = logFileName();
 
        do
        {
                try
                {
                        res = readSinglePacketFromFile();
                }
                catch (...)
                {
                        // Simply ignore the error and continue.
                        continue;
                }
                // Only emit progress updates at complete percentages, once per percentage
                XsFilePos pos = logFileReadPosition();
                XsFilePos size = logFileSize();
 
                if (size)
                {
                        const int percentage = (int)(pos * 100 / size);
                        if (prevFilePos != percentage && percentage < 100)
                        {
                                onProgressUpdated(device, percentage < 100 ? percentage : 99, 100, &id);        // never report 100% until we're really done
                                prevFilePos = percentage;
                        }
                }
        } while (!m_abortLoadLogFile && (res == XRV_OK || res == XRV_OTHER));
 
        if (!m_abortLoadLogFile)
        {
                masterDevice()->onEofReached();
                onProgressUpdated(device, 100, 100, &id);
        }
 
        return setAndReturnLastResult(res);
}
 
XsResultValue MtbFileCommunicator::readSinglePacketFromFile()
{
        JLTRACEG("Reading from file");
        XsMessage msg = readMessage(0);
        if (lastResult())
                return lastResult();
 
        handleMessage(msg);
        return lastResult();
}
 
void MtbFileCommunicator::abortLoadLogFile()
{
        m_abortLoadLogFile = true;
}
 
bool MtbFileCommunicator::openLogFile(const XsString& filename)
{
        if (m_ioInterfaceFile)
        {
                setLastResult(XRV_ALREADYOPEN);
                if (filename == logFileName())
                        return true;
                return false;
        }
        m_ioInterfaceFile = std::shared_ptr<IoInterfaceFile>(new IoInterfaceFile, [](IoInterfaceFile * f)
        {
                f->close();
        });
 
        setLastResult(m_ioInterfaceFile->open(filename, false, true));
        if (lastResult() != XRV_OK)
        {
                m_ioInterfaceFile.reset();
                return false;
        }
 
        // start with a sanity check on the file
        XsByteArray hdrBuf;
        XsResultValue rv;
 
        // Is the file readable and does it at least start with 0xFA marker?
        rv = m_ioInterfaceFile->readData(1, hdrBuf);
        if (rv != XRV_OK || hdrBuf.size() < 1 || hdrBuf[0] != 0xFA)
        {
                setLastResult(XRV_DATACORRUPT);
                m_ioInterfaceFile.reset();
                return false;
        }
 
        resetLogFileReadPosition();
 
        // try to read any message from the start
        XsMessage check = readMessage();
        if (check.getMessageId() == XMID_InvalidMessage)
        {
                setLastResult(XRV_DATACORRUPT);
                m_ioInterfaceFile.reset();
                return false;
        }
 
        resetLogFileReadPosition();
 
        // Now let's try to find the actual message we are looking for somewhere...
        XsMessage rcv = readMessageFromStartOfFile(XMID_Configuration, Communicator::configurationMessageSearchLimit());
 
        if (rcv.getMessageId() != XMID_Configuration)
        {
                setLastResult(XRV_DATACORRUPT);
                m_ioInterfaceFile.reset();
                return false;
        }
 
        XsDeviceConfiguration config;
        config.readFromMessage(rcv);
        setMasterDeviceId(XsDeviceId((char*)config.masterInfo().m_productCode, 0, 0, config.masterInfo().m_masterDeviceId));
 
        return true;
}
 
XsString MtbFileCommunicator::logFileName() const
{
        XsString retString;
        if (!m_ioInterfaceFile)
                return retString;
 
        if (m_ioInterfaceFile->getName(retString) != XRV_OK)
                return XsString();
        return retString;
}
 
XsFilePos MtbFileCommunicator::logFileSize() const
{
        if (!m_ioInterfaceFile)
                return 0;
 
        return m_ioInterfaceFile->getFileSize();
}
 
XsTimeStamp MtbFileCommunicator::logFileDate() const
{
        if (!m_ioInterfaceFile)
                return XsTimeStamp();
 
        return m_ioInterfaceFile->getFileDate();
}
 
XsFilePos MtbFileCommunicator::logFileReadPosition() const
{
        if (!m_ioInterfaceFile)
                return 0;
 
        auto pos = m_ioInterfaceFile->getReadPosition();
        if (pos < m_ioInterfaceFile->getFileSize())
                return pos;
 
        if (m_extractedMessages->empty())
                return pos;
 
        return pos > 0 ? pos - 1 : 0;
}
 
void MtbFileCommunicator::resetLogFileReadPosition(void)
{
        if (m_ioInterfaceFile)
        {
                m_extractor->clearBuffer();
                m_extractedMessages->clear();
                setLastResult(m_ioInterfaceFile->setReadPosition(0));
        }
        else
                setLastResult(XRV_NOFILEOPEN);
}
 
bool MtbFileCommunicator::isReadingFromFile() const
{
        return true;
}
 
XsMessage MtbFileCommunicator::readMessage(uint8_t msgId)
{
        if (!m_ioInterfaceFile)
        {
                setLastResult(XRV_INVALIDOPERATION);
                return XsMessage();
        }
 
        XsMessage msg;
 
        do
        {
                msg = readNextMessage();
        } while (!msg.empty() && msgId != 0 && msg.getMessageId() != msgId);
 
        if (msgId == 0 || msg.getMessageId() == msgId)
                return msg;
 
        setLastResult(XRV_OTHER);
        return XsMessage();
}
 
XsMessage MtbFileCommunicator::readNextMessage()
{
        while (m_extractedMessages->empty())
        {
                XsByteArray raw;
                XsResultValue res = m_ioInterfaceFile->readDataBlocks(1, raw);
                (void)res;
                if (raw.empty())
                {
                        // end of file really reached
                        setLastResult(XRV_ENDOFFILE);
                        return XsMessage();
                }
                m_extractor->processNewData(masterDevice(), raw, *m_extractedMessages);
        }
 
        setLastResult(XRV_OK);
        XsMessage msg = *m_extractedMessages->begin();
        m_extractedMessages->pop_front();
        return msg;
}
 
bool MtbFileCommunicator::isLoadLogFileInProgress() const
{
        return  ThreadPool::instance()->doesTaskExist(m_loadFileTaskId);
}
 
void MtbFileCommunicator::addProtocolHandler(IProtocolHandler* handler)
{
        handler->ignoreMaximumMessageSize(true);
        Communicator::addProtocolHandler(handler);
}
 
/* Now the disabled stuff starts */
 
XsResultValue MtbFileCommunicator::gotoConfig(bool detectRs485)
{
        (void)detectRs485;
        return XRV_UNSUPPORTED;
}
 
XsResultValue MtbFileCommunicator::gotoMeasurement()
{
        return XRV_UNSUPPORTED;
}
 
XsResultValue MtbFileCommunicator::getDeviceId()
{
        return XRV_UNSUPPORTED;
}
 
void MtbFileCommunicator::setGotoConfigTimeout(uint32_t timeout)
{
        (void)timeout;
}
 
bool MtbFileCommunicator::writeMessage(const XsMessage& message)
{
        (void)message;
        return false;
}
 
void MtbFileCommunicator::flushPort()
{
}
 
void MtbFileCommunicator::closePort()
{
}
 
bool MtbFileCommunicator::isPortOpen() const
{
        return false;
}
 
XsPortInfo MtbFileCommunicator::portInfo() const
{
        return XsPortInfo();
}
 
bool MtbFileCommunicator::openPort(const XsPortInfo& portInfo, OpenPortStage stage, bool detectRs485)
{
        (void)portInfo;
        (void)stage;
        (void)detectRs485;
        return false;
}
 
bool MtbFileCommunicator::reopenPort(OpenPortStage stage, bool skipDeviceIdCheck)
{
        (void)stage;
        (void)skipDeviceIdCheck;
        return false;
}
 
bool MtbFileCommunicator::isDockedAt(Communicator* other) const
{
        (void)other;
        return false;
}
 
void MtbFileCommunicator::setKeepAlive(bool enable)
{
        (void)enable;
}