xsdevice_def.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 "xsdevice_def.h"
#include <xstypes/xsdatapacket.h>
#include <xscommon/xsens_threadpool.h>
#include <xstypes/xsdeviceid.h>
#include <xstypes/xsstring.h>
#include <xstypes/xsoutputconfigurationarray.h>
#include <xstypes/xscanoutputconfigurationarray.h>
#include <xstypes/xsdeviceidarray.h>
#include <xstypes/xsintarray.h>
#include <xstypes/xsportinfo.h>
#include <xstypes/xssyncsettingarray.h>
#include "protocolhandler.h"
#include "communicator.h"
#include "mtbdatalogger.h"
#include <xstypes/xsbaud.h>
#include <xstypes/xsfilterprofile.h>
#include "xsselftestresult.h"
#include <xstypes/xsscrdata.h>
#include <xstypes/xscalibrateddata.h>
#include <xstypes/xsresetmethod.h>
#include <xstypes/xssyncsetting.h>
#include "nmea_protocolhandler.h"
#include <xstypes/xsrssi.h>
#include "supportedsyncsettings.h"
#include "messageserializer.h"
#include "xsdeviceptrarray.h"
#include <functional>
#include <xstypes/xsdatapacketptrarray.h>
#include <xstypes/xsfilterprofilearray.h>
#include <xstypes/xsstringoutputtypearray.h>
#include "xsdef.h"
#include "xsiccrepmotionresult.h"
#include <xscommon/xprintf.h>
 
using namespace xsens;
using namespace XsTime;
 
#if 1 && defined(XSENS_RELEASE) && defined(XSENS_DEBUG)
        // prevent spamming logs with identical "packet missed" loglines
        #define ONLYFIRSTMTX2   if (!deviceId().isMtx2() || firstChild() == this)
#else
        #define ONLYFIRSTMTX2
#endif
 
#define TOADUMP 0       // set to 0 to disable
#if TOADUMP
        #define CREATETOADUMPFILE()     if (this == master()) do { if (m_toaDumpFile != nullptr) { fflush(m_toaDumpFile); fclose(m_toaDumpFile); } m_toaDumpFile = fopen(xprintf("toadump_%08X_%p_%llu.csv", deviceId().toInt(), this, XsTimeStamp::nowMs()).c_str(), "wt"); } while(0)
#else
        #define CREATETOADUMPFILE()     ((void)0)
#endif
 
// set to 1 to log all messages sent and received through the doTransaction functions
#ifndef XSENS_RELEASE
        #define LOGTRANSACTIONS 1       // enable in full debug build
#else
        #define LOGTRANSACTIONS 0       // disable in release and rd builds
#endif
 
#if LOGTRANSACTIONS
std::string msgToString(XsMessage const& msg)
{
        std::string rv;
        XsSize sz = msg.getTotalMessageSize();
        uint8_t const* buffy = msg.getMessageStart();
        rv.reserve((sz + 1) * 2);
        for (XsSize i = 0; i < sz; ++i)
                rv.append(xprintf("%02X", (unsigned int)buffy[i]));
        return rv;
}
#define TRANSACTIONLOG(...)     JLDEBUGG(__VA_ARGS__)
#else
#define TRANSACTIONLOG(...)     ((void) 0)
#endif
 
XsDevice::XsDevice(XsDeviceId const& id)
        : m_latestLivePacket(new XsDataPacket)
        , m_latestBufferedPacket(new XsDataPacket)
        , m_unavailableDataBoundary(-1)
        , m_deviceId(id)
        , m_state(XDS_Initial)
        , m_connectivity(XCS_Disconnected)
        , m_communicator(nullptr)
        , m_logFileInterface(nullptr)
        , m_master(this)
        , m_refCounter(0)
        , m_writeToFile(false)
        , m_isInitialized(false)
        , m_terminationPrepared(false)
        , m_gotoConfigOnClose(true)
        , m_justWriteSetting(false)
        , m_skipEmtsReadOnInit(true)
        , m_options(XSO_None)
        , m_startRecordingPacketId(-1)
        , m_stopRecordingPacketId(-1)
        , m_stoppedRecordingPacketId(-1)
        , m_lastAvailableLiveDataCache(new XsDataPacket)
        , m_toaDumpFile(nullptr)
{
        CREATETOADUMPFILE();
        JLDEBUGG(this << " Created device " << deviceId());
}
 
XsDevice::XsDevice(Communicator* comm)
        : m_latestLivePacket(new XsDataPacket)
        , m_latestBufferedPacket(new XsDataPacket)
        , m_unavailableDataBoundary(-1)
        , m_deviceId(0)
        , m_state(XDS_Initial)
        , m_connectivity(XCS_Disconnected)
        , m_communicator(comm)
        , m_logFileInterface(nullptr)
        , m_master(this)
        , m_refCounter(0)
        , m_writeToFile(false)
        , m_isInitialized(false)
        , m_terminationPrepared(false)
        , m_gotoConfigOnClose(true)
        , m_justWriteSetting(false)
        , m_skipEmtsReadOnInit(false)
        , m_options(XSO_None)
        , m_startRecordingPacketId(-1)
        , m_stopRecordingPacketId(-1)
        , m_stoppedRecordingPacketId(-1)
        , m_lastAvailableLiveDataCache(new XsDataPacket)
        , m_toaDumpFile(nullptr)
{
        // put callback managers and callbacks in place
        copyCallbackHandlersFrom(m_communicator);
        addChainedManager(m_communicator);
 
        m_deviceId = m_communicator->masterDeviceId();
 
        m_communicator->setMasterDevice(this);
 
        if (m_communicator->isPortOpen())
                m_connectivity = XCS_PluggedIn;
        else if (m_communicator->isReadingFromFile())
        {
                m_state = XDS_Measurement;
                m_connectivity = XCS_File;
        }
        CREATETOADUMPFILE();
        JLDEBUGG(this << " Created device " << deviceId());
}
 
XsDevice::XsDevice(XsDevice* masterDevice, const XsDeviceId& childDeviceId)
        : m_latestLivePacket(new XsDataPacket)
        , m_latestBufferedPacket(new XsDataPacket)
        , m_unavailableDataBoundary(-1)
        , m_deviceId(childDeviceId)
        , m_state(XDS_Initial)
        , m_connectivity(XCS_Unknown)
        , m_communicator(nullptr)
        , m_logFileInterface(nullptr)
        , m_master(masterDevice)
        , m_refCounter(0)
        , m_writeToFile(false)
        , m_isInitialized(false)
        , m_terminationPrepared(false)
        , m_gotoConfigOnClose(true)
        , m_justWriteSetting(false)
        , m_skipEmtsReadOnInit(true)
        , m_options(XSO_None)
        , m_startRecordingPacketId(-1)
        , m_stopRecordingPacketId(-1)
        , m_stoppedRecordingPacketId(-1)
        , m_lastAvailableLiveDataCache(new XsDataPacket)
        , m_toaDumpFile(nullptr)
{
        (void)masterDevice;
        JLDEBUGG(this << " Created device " << deviceId() << " child of " << masterDevice << " " << (masterDevice ? masterDevice->deviceId() : XsDeviceId()));
}
 
XsDevice::~XsDevice()
{
        JLDEBUGG(this << " did " << deviceId() << " refcounter " << m_refCounter.load());
        LockSuspendable locky(&m_deviceMutex, LS_Write);        // This is to make sure that we're not deleting the object while some other thread is using it. Especially the communicator threads tend to still be active at this point.
 
        assert(m_refCounter.load() == 0);
        assert(m_terminationPrepared);
 
        try
        {
                if (m_latestLivePacket)
                {
                        delete m_latestLivePacket;
                        m_latestLivePacket = nullptr;
                }
                if (m_latestBufferedPacket)
                {
                        delete m_latestBufferedPacket;
                        m_latestBufferedPacket = nullptr;
                }
                if (m_lastAvailableLiveDataCache)
                {
                        delete m_lastAvailableLiveDataCache;
                        m_lastAvailableLiveDataCache = nullptr;
                }
 
                locky.unlock(); // the lock will interfere with thread termination
                if (isMasterDevice() && m_communicator)
                {
                        removeChainedManager(m_communicator);
                        m_communicator->destroy();
                        m_communicator = nullptr;
                }
 
                if (m_logFileInterface)
                {
                        m_logFileInterface->close();
                        delete m_logFileInterface;
                }
 
                JLDEBUGG(this << " destroyed");
 
                if (m_toaDumpFile)
                {
                        fflush(m_toaDumpFile);
                        fclose(m_toaDumpFile);
                        m_toaDumpFile = nullptr;
                }
        }
        catch (...)
        {
        }
}
 
XsDevice* XsDevice::master() const
{
        return m_master;
}
 
void XsDevice::clearProcessors()
{
}
 
void XsDevice::prepareForTermination()
{
        if (!m_terminationPrepared)
        {
                JLDEBUGG("Preparing " << deviceId() << " for termination");
 
                updateDeviceState(XDS_Destructing);
 
                if (isMasterDevice() && m_communicator != nullptr && m_communicator->isPortOpen())
                {
                        if (m_gotoConfigOnClose)
                                gotoConfig();
                        m_communicator->closePort();
                }
                // finishLastProcessingTask();  // make sure no processing is going on in the background either...
                m_terminationPrepared = true;
        }
}
 
void XsDevice::updateDeviceState(XsDeviceState newState)
{
        LockSuspendable locky(&m_deviceMutex, LS_Write);
        LockGuarded lockG(&m_deviceMutex);
 
        // we don't allow any transitions out of the (final) destructing state
        if (m_state == XDS_Destructing)
                return;
 
        XsDeviceState oldState = m_state;
        if (oldState != newState)
        {
                ONLYFIRSTMTX2
                JLDEBUGG("did: " << m_deviceId << " new: " << newState << " old: " << m_state);
                // some special case handling
                switch (newState)
                {
                        case XDS_FlushingData:  // to
                                switch (oldState)
                                {
                                        case XDS_Measurement:   // from
                                                return;
 
                                        case XDS_Recording:     // from
                                                if (m_stopRecordingPacketId == -1 && isMasterDevice())
                                                        m_stopRecordingPacketId = latestLivePacketId();
                                                m_stoppedRecordingPacketId = m_stopRecordingPacketId;
                                                ONLYFIRSTMTX2
                                                JLDEBUGG(this << " " << deviceId() << " m_startRecordingPacketId = " << m_startRecordingPacketId << " m_stopRecordingPacketId = " << m_stopRecordingPacketId << " m_stoppedRecordingPacketId = " << m_stoppedRecordingPacketId);
                                                break;
 
                                        case XDS_WaitingForRecordingStart:      // from
                                                updateDeviceState(XDS_Measurement);
                                                return;
 
                                        default:
                                                break;
                                }
                                break;
 
                        case XDS_Recording:     // to
                                switch (oldState)
                                {
                                        case XDS_Measurement:   // from
                                                m_stopRecordingPacketId = -1;
                                                m_stoppedRecordingPacketId = -1;
                                                if (m_startRecordingPacketId == -1 && isMasterDevice() && latestLivePacketId() >= 0)
                                                        m_startRecordingPacketId = latestLivePacketId() + 1;
                                                ONLYFIRSTMTX2
                                                JLDEBUGG(this << " " << deviceId() << " m_startRecordingPacketId = " << m_startRecordingPacketId << " m_stopRecordingPacketId = " << m_stopRecordingPacketId << " m_stoppedRecordingPacketId = " << m_stoppedRecordingPacketId);
                                                break;
                                        default:
                                                break;
                                }
                                break;
 
                        case XDS_WaitingForRecordingStart:      // to
                                switch (oldState)
                                {
                                        case XDS_Measurement:   // from
                                                m_stopRecordingPacketId = -1;
                                                m_startRecordingPacketId = -1;
                                                m_stoppedRecordingPacketId = -1;
                                                ONLYFIRSTMTX2
                                                JLDEBUGG(this << " " << deviceId() << " m_startRecordingPacketId = " << m_startRecordingPacketId << " m_stopRecordingPacketId = " << m_stopRecordingPacketId << " m_stoppedRecordingPacketId = " << m_stoppedRecordingPacketId);
                                                break;
                                        default:
                                                break;
                                }
                                break;
 
                        case XDS_Measurement:   // to
                                switch (oldState)
                                {
                                        case XDS_Recording:             // from
                                        case XDS_FlushingData:  // from
                                                m_stoppedRecordingPacketId = m_stopRecordingPacketId;
                                                if (m_stoppedRecordingPacketId == -1 && isMasterDevice())
                                                        m_stoppedRecordingPacketId = latestLivePacketId();
                                                m_stopRecordingPacketId = -1;
                                                m_startRecordingPacketId = -1;
                                                //m_latestBufferedPacket->clear();
                                                ONLYFIRSTMTX2
                                                JLDEBUGG(this << " " << deviceId() << " m_startRecordingPacketId = " << m_startRecordingPacketId << " m_stopRecordingPacketId = " << m_stopRecordingPacketId << " m_stoppedRecordingPacketId = " << m_stoppedRecordingPacketId);
                                                break;
 
                                        case XDS_Config:
                                        default:
                                                m_stopRecordingPacketId = -1;
                                                m_startRecordingPacketId = -1;
                                                m_stoppedRecordingPacketId = -1;
                                                ONLYFIRSTMTX2
                                                JLDEBUGG(this << " " << deviceId() << " m_startRecordingPacketId = " << m_startRecordingPacketId << " m_stopRecordingPacketId = " << m_stopRecordingPacketId << " m_stoppedRecordingPacketId = " << m_stoppedRecordingPacketId);
                                                resetPacketStamping();
                                                reinitializeProcessors();
                                                break;
                                }
                                break;
 
                        default:
                                break;
                }
 
                m_state = newState;
                lockG.unlock();
                locky.unlock();
                onDeviceStateChanged(this, newState, oldState);
        }
}
 
bool XsDevice::isSoftwareFilteringEnabled() const
{
        return false;
}
 
bool XsDevice::isSoftwareCalibrationEnabled() const
{
        return false;
}
 
XsDevice* XsDevice::findDevice(XsDeviceId const& deviceid) const
{
        if (deviceid == m_deviceId)
                return const_cast<XsDevice*>(this);
        return nullptr;
}
 
XsDevice const* XsDevice::findDeviceConst(XsDeviceId const& deviceid) const
{
        return const_cast<XsDevice const*>(const_cast<XsDevice*>(this)->findDevice(deviceid));
}
 
void XsDevice::setGotoConfigOnClose(bool gotoConfigOnClose)
{
        m_gotoConfigOnClose = gotoConfigOnClose;
}
int XsDevice::batteryLevel() const
{
        return 0;
}
 
int XsDevice::updateRate() const
{
        return 0;
}
 
bool XsDevice::setUpdateRate(int)
{
        return false;
}
 
XsDeviceOptionFlag XsDevice::deviceOptionFlags() const
{
        return XDOF_None;
}
 
XsOutputConfiguration XsDevice::findConfiguration(XsDataIdentifier dataType) const
{
        XsDataIdentifier mask;
        if ((dataType & XDI_TypeMask) == dataType)
                mask = XDI_TypeMask;
        else
                mask = XDI_FullTypeMask;
 
        XsOutputConfigurationArray cfg = outputConfiguration();
        auto item = std::find_if(cfg.begin(), cfg.end(),
                        [&](const XsOutputConfiguration & cfg)
        {
                return (cfg.m_dataIdentifier & mask) == dataType;
        }
 
                );
        if (item == cfg.end())
                return XsOutputConfiguration();
        return *item;
}
int XsDevice::updateRateForDataIdentifier(XsDataIdentifier dataType) const
{
        XsOutputConfiguration cfg = findConfiguration(dataType);
 
        if (cfg.m_dataIdentifier == XDI_None)
                return updateRate();
 
        return cfg.m_frequency;
}
 
int XsDevice::updateRateForProcessedDataIdentifier(XsDataIdentifier dataType) const
{
        return updateRateForDataIdentifier(dataType);
}
 
bool XsDevice::hasDataEnabled(XsDataIdentifier dataType) const
{
        return checkDataEnabled(dataType, outputConfiguration());
}
 
bool XsDevice::checkDataEnabled(XsDataIdentifier dataType, XsOutputConfigurationArray const& configurations)
{
        XsDataIdentifier mask;
        if ((dataType & XDI_TypeMask) == dataType)
                mask = XDI_TypeMask;
        else // if ((dataType & XDI_FullTypeMask) == dataType)
                mask = XDI_FullTypeMask;
 
        dataType = dataType & mask;
 
        for (XsOutputConfigurationArray::const_iterator i = configurations.begin(); i != configurations.end(); ++i)
        {
                if (dataType == (i->m_dataIdentifier & mask))
                        return true;
        }
        return false;
}
bool XsDevice::hasProcessedDataEnabled(XsDataIdentifier) const
{
        return false;
}
 
XsVersion XsDevice::firmwareVersion() const
{
        return m_firmwareVersion;
}
 
void XsDevice::setFirmwareVersion(const XsVersion& version)
{
        if (m_firmwareVersion != version)
#ifdef XSENS_DEBUG
                JLDEBUGG("Device " << deviceId() << " has firmware version " << version.toString());
#else
                JLWRITEG("Device " << deviceId() << " has firmware version " << version.toString());
#endif
        m_firmwareVersion = version;
}
 
void XsDevice::extractFirmwareVersion(XsMessage const& message)
{
        XsVersion old = m_firmwareVersion;
        m_firmwareVersion = XsVersion(message.getDataByte(0), message.getDataByte(1), message.getDataByte(2));
        if (message.getDataSize() > 3)
                m_firmwareVersion.setBuild((int)(int32_t) message.getDataLong(3));
        if (message.getDataSize() > 7)
                m_firmwareVersion.setReposVersion((int)(int32_t) message.getDataLong(7));
        if (old != m_firmwareVersion)
#ifdef XSENS_DEBUG
                JLDEBUGG("Device " << deviceId() << " has firmware version " << m_firmwareVersion.toString());
#else
                JLWRITEG("Device " << deviceId() << " has firmware version " << m_firmwareVersion.toString());
#endif
}
 
Communicator* XsDevice::communicator() const
{
        return m_communicator;
}
 
DataLogger* XsDevice::logFileInterface(std::unique_ptr<xsens::Lock>& myLock) const
{
        if (!myLock)
                myLock.reset(new xsens::Lock(&m_logFileMutex, true));
        else if (!myLock->isLocked())
                myLock->lock();
        return m_logFileInterface;
}
 
void XsDevice::setDeviceId(const XsDeviceId& deviceid)
{
        m_deviceId = deviceid;
}
XsDeviceId const&  XsDevice::deviceId() const
{
        return m_deviceId;
}
 
bool XsDevice::isSyncMaster() const
{
        return false;
}
 
bool XsDevice::isSyncSlave() const
{
        return false;
}
 
XsDevice* XsDevice::deviceAtBusId(int busid)
{
        if (isMasterDevice() && (busid == XS_BID_MASTER || busid == 1))
                return this;
        return nullptr;
}
 
const XsDevice* XsDevice::deviceAtBusIdConst(int busid) const
{
        // prevent code duplication, go through the non-const implementation
        return const_cast<XsDevice const*>(const_cast<XsDevice*>(this)->deviceAtBusId(busid));
}
 
void XsDevice::restartFilter()
{
}
 
XsResultValue XsDevice::lastResult() const
{
        return m_lastResult;
}
 
XsString XsDevice::lastResultText() const
{
        return m_lastResult.lastResultText();
}
 
bool XsDevice::isMotionTracker() const
{
        return false;
}
 
int XsDevice::dataLength() const
{
        return 0;
}
 
XsBaudRate XsDevice::serialBaudRate() const
{
        return XBR_Invalid;
}
 
XsBaudRate XsDevice::baudRate() const
{
        Communicator* comm = communicator();
        if (!comm)
                return XBR_Invalid;
        return comm->portInfo().baudrate();
}
 
int XsDevice::busId() const
{
        return XS_BID_MASTER;
}
 
bool XsDevice::setSerialBaudRate(XsBaudRate baudrate)
{
        if (!isMasterDevice())
                return false;
 
        Communicator* comm = communicator();
        if (!comm)
                return false;
 
        if (comm->isReadingFromFile())
                return false;
 
        XsMessage snd(XMID_SetBaudrate, XS_LEN_BAUDRATE);
        snd.setBusId(XS_BID_MASTER);
        snd.setDataByte(XsBaud::rateToCode(baudrate));
 
        if (!doTransaction(snd, 1000))//increased from 500 to 1000, since the One Series timed out very rarely with the old value
                return false;
 
        if (comm->portInfo().baudrate() == XBR_Invalid ||
                comm->portInfo().baudrate() == baudrate ||
                m_justWriteSetting)
                return true;
 
        if (!resetRemovesPort())
                return reset();
        else
                return true;
}
 
XsIntArray XsDevice::portConfiguration() const
{
        return XsIntArray();
}
 
bool XsDevice::setPortConfiguration(XsIntArray& config)
{
        (void)config;
        return false;
}
 
void XsDevice::resetPacketStamping()
{
        LockGuarded lockG(&m_deviceMutex);
        JLDEBUGG("did: " << deviceId());
        m_latestLivePacket->clear();
        m_latestBufferedPacket->clear();
        m_lastDataOkStamp = 0;
        m_unavailableDataBoundary = -1;
        m_packetStamper.resetTosEstimation();
        CREATETOADUMPFILE();
}
bool XsDevice::gotoMeasurement()
{
        JLDEBUGG(deviceId());
 
        if (!isMasterDevice())
        {
                m_lastResult.set(XRV_OTHER, deviceId().toString() << " is not a master device, can't switch to measurement mode");
                JLERRORG(m_lastResult.lastResultText());
                return false;
        }
 
        if (m_state == XDS_Measurement) // if we're already in measurement mode, we can ignore this command
        {
                m_lastResult.set(XRV_OK, deviceId().toString() << " was already in measurement mode");
                JLDEBUGG(m_lastResult.lastResultText());
                return true;
        }
 
        Communicator* comm = communicator();
        if (!comm)
        {
                m_lastResult.set(XRV_OTHER, deviceId().toString() << " doesn't have a communicator, can't switch to measurement mode");
                JLERRORG(m_lastResult.lastResultText());
                return false;
        }
 
        if (comm->isReadingFromFile())
        {
                m_lastResult.set(XRV_OTHER, deviceId().toString() << " is reading from file, can't switch to measurement mode");
                JLERRORG(m_lastResult.lastResultText());
                return false;
        }
 
        XsResultValue res = comm->gotoMeasurement();
        if (res == XRV_OK)
        {
                // switch device (and children) to measuring state
                setDeviceState(XDS_Measurement);
                m_lastResult.set(XRV_OK, deviceId().toString() << " now in measurement mode");
                JLDEBUGG(m_lastResult.lastResultText());
                return true;
        }
 
        m_lastResult.set(XRV_OTHER, deviceId().toString() << " communicator refused with code " << res << " (" << XsResultValue_toString(res) << "), switching children back to config mode");
        JLERRORG(m_lastResult.lastResultText());
        return false;
}
 
bool XsDevice::gotoConfig()
{
        JLDEBUGG(deviceId());
        if (!isMasterDevice())
        {
                m_lastResult.set(XRV_OTHER, deviceId().toString() << " is not a master device, can't switch to config mode");
                JLERRORG(m_lastResult.lastResultText());
                return false;
        }
 
        if (m_state == XDS_Config)      // if we're already in config mode, we can ignore this command
        {
                m_lastResult.set(XRV_OK, deviceId().toString() << " was already in config mode");
                JLDEBUGG(m_lastResult.lastResultText());
                return true;
        }
 
        Communicator* comm = communicator();
        if (!comm)
        {
                m_lastResult.set(XRV_OTHER, deviceId().toString() << " doesn't have a communicator, can't switch to config mode");
                JLERRORG(m_lastResult.lastResultText());
                return false;
        }
 
        if (comm->isReadingFromFile())
        {
                m_lastResult.set(XRV_INVALIDOPERATION, deviceId().toString() << " is reading from file, can't switch to config mode");
                JLDEBUGG(m_lastResult.lastResultText());
                return false;
        }
 
        XsResultValue res = comm->gotoConfig();
        if (res == XRV_OK)
        {
                setDeviceState(XDS_Config);
                m_lastResult.set(XRV_OK, deviceId().toString() << " now in config mode");
                JLDEBUGG(m_lastResult.lastResultText());
                return true;
        }
 
        m_lastResult.set(XRV_OTHER, deviceId().toString() << " communicator refused with code " << res << " (" << XsResultValue_toString(res) << "), can't switch to config mode");
        JLERRORG(m_lastResult.lastResultText());
        return false;
}
 
XsDeviceState XsDevice::deviceState() const
{
        return m_state;
}
 
void XsDevice::setDeviceState(XsDeviceState state)
{
        // LockSuspendable locky(&m_deviceMutex, LS_Write);
        // lock is already acquired by updateDeviceState,
        updateDeviceState(state);
}
bool XsDevice::isMasterDevice() const
{
        return this == master();
}
 
bool XsDevice::isContainerDevice() const
{
        return false;
}
 
bool XsDevice::isStandaloneDevice() const
{
        return isMasterDevice() && !isContainerDevice();
}
 
bool XsDevice::setDeviceOptionFlags(XsDeviceOptionFlag setFlags, XsDeviceOptionFlag clearFlags)
{
        XsMessage snd(XMID_SetOptionFlags, 8);
        snd.setBusId(busId());
        MessageSerializer(snd) << (uint32_t)setFlags << (uint32_t)clearFlags;
 
        XsMessage rcv;
        if (!doTransaction(snd, rcv))
                return false;
 
        return true;
}
 
bool XsDevice::setOutputConfiguration(XsOutputConfigurationArray& config)
{
        auto const rv = setOutputConfigurationInternal(config);
        if (rv != XRV_OK)
                return false;
        return true;
}
 
XsResultValue XsDevice::setOutputConfigurationInternal(XsOutputConfigurationArray& config)
{
        XsMessage snd(XMID_SetOutputConfiguration, 4);
        snd.setBusId(busId());
        bool wasEmpty = config.empty();
        MessageSerializer(snd) << config;
 
        XsMessage rcv;
        if (!doTransaction(snd, rcv))
        {
                // deal w/ possible race condition on m_lastResult
                auto const rv = rcv.toResultValue();
                m_lastResult = rv;
                return rv;
        }
 
        MessageDeserializer(rcv) >> config;
        if (wasEmpty && config.size() == 1 && config[0] == XsOutputConfiguration(XDI_None, 0))
                config.clear();
 
        m_outputConfiguration = config;
        return XRV_OK;
}
/* \endcond */
 
bool XsDevice::setCanOutputConfiguration(XsCanOutputConfigurationArray& config)
{
        (void)config;
        return false;
}
 
bool XsDevice::setCanConfiguration(uint32_t config)
{
        (void)config;
        return false;
}
 
XsOutputConfigurationArray XsDevice::processedOutputConfiguration() const
{
        return outputConfiguration();
}
 
bool XsDevice::setStringOutputMode(uint16_t type, uint16_t period, uint16_t skipFactor)
{
        XsMessage sndType(XMID_SetStringOutputType);
        sndType.setBusId(XS_BID_MASTER);        // Always send to master device
        sndType.resizeData(2);
        sndType.setDataShort(type);
 
        if (!doTransaction(sndType))
                return false;
 
        if (type == 0)
                return true;
 
        XsMessage sndSkip(XMID_SetOutputSkipFactor);
        sndSkip.setBusId(busId());
        sndSkip.resizeData(2);
        sndSkip.setDataShort(skipFactor);
 
        if (!doTransaction(sndSkip))
                return false;
 
        m_config.masterInfo().m_outputSkipFactor = skipFactor;
 
        XsMessage sndPer(XMID_SetPeriod);
        sndPer.setBusId(XS_BID_MASTER); // Always send to master device
        sndPer.resizeData(2);
        sndPer.setDataShort(period);
 
        if (!doTransaction(sndPer))
                return false;
 
        m_config.masterInfo().m_samplingPeriod = period;
 
        return true;
}
 
XsStringOutputTypeArray XsDevice::supportedStringOutputTypes() const
{
        return XsStringOutputTypeArray();
}
 
bool XsDevice::scheduleOrientationReset(XsResetMethod method)
{
        if (method == XRM_StoreAlignmentMatrix && deviceState() != XDS_Config)
                return false;
 
        if (method != XRM_StoreAlignmentMatrix && deviceState() != XDS_Measurement && deviceState() != XDS_Recording)
                return false;
 
        XsMessage snd(XMID_ResetOrientation);
        snd.setBusId(busId());
        snd.resizeData(2);
        snd.setDataShort((uint16_t)method);
 
        if (!doTransaction(snd))
                return false;
        return true;
}
bool XsDevice::sendCustomMessage(const XsMessage& messageSend, bool waitForResult, XsMessage& messageReceive, int timeout)
{
        return sendCustomMessage(messageSend, waitForResult, static_cast<XsXbusMessageId>(messageSend.getMessageId() + 1), messageReceive, timeout);
}
 
bool XsDevice::sendRawMessage(const XsMessage& message)
{
        Communicator* comm = communicator();
        if (!comm)
                return false;
        TRANSACTIONLOG(m_deviceId << " SND: " << msgToString(message));
        return comm->writeMessage(message);
}
 
bool XsDevice::sendCustomMessage(const XsMessage& messageSend, bool waitForResult, XsXbusMessageId messageId, XsMessage& messageReceive, int timeout)
{
        Communicator* comm = communicator();
        if (!comm)
                return false;
 
        TRANSACTIONLOG(m_deviceId << " SND: " << msgToString(messageSend));
        std::shared_ptr<ReplyObject> reply = comm->addReplyObject(messageId);
        if (!comm->writeMessage(messageSend))
                return false;
 
        if (waitForResult)
        {
                if (!timeout)
                        timeout = (int)(int32_t) comm->defaultTimeout();
                messageReceive = reply->message((uint32_t)(unsigned int)timeout);
                TRANSACTIONLOG(m_deviceId << " RCV: " << msgToString(messageReceive));
                if (messageReceive.getMessageId() != messageId)
                        return false;
        }
 
        return true;
}
 
bool XsDevice::waitForCustomMessage(XsXbusMessageId messageId, XsMessage& messageReceive, int timeout)
{
        Communicator* comm = communicator();
        if (!comm)
                return false;
 
        std::shared_ptr<ReplyObject> reply = comm->addReplyObject(messageId);
 
        if (!timeout)
                timeout = (int)(int32_t) comm->defaultTimeout();
        messageReceive = reply->message((uint32_t)(unsigned int)timeout);
 
        if (messageReceive.getMessageId() != messageId)
                return false;
 
        return true;
}
 
bool XsDevice::waitForCustomMessage(std::shared_ptr<ReplyObject> reply, XsMessage& messageReceive, int timeout)
{
        if (!timeout && communicator())
                timeout = (int)(int32_t) communicator()->defaultTimeout();
        messageReceive = reply->message((uint32_t)(unsigned int)timeout);
 
        if (messageReceive.getMessageId() != reply->msgId())
                return false;
 
        return true;
}
 
std::shared_ptr<ReplyObject> XsDevice::addReplyObject(XsXbusMessageId messageId, uint8_t data)
{
        Communicator* comm = communicator();
        if (!comm)
                return std::shared_ptr<ReplyObject>();
 
        return comm->addReplyObject(messageId, 0, 1, &data);
}
 
bool XsDevice::shouldDataMsgBeRecorded(const XsMessage& msg) const
{
        (void)msg;
        return (deviceState() == XDS_Recording);
}
 
void XsDevice::handleMessage(const XsMessage& msg)
{
        if (isMasterDevice())
                writeMessageToLogFile(msg);
 
        switch (msg.getMessageId())
        {
                case XMID_MtData2:
                {
                        XsDataPacket packet(&msg);
                        packet.setDeviceId(deviceId());
                        handleDataPacket(packet);
                        break;
                }
 
                case XMID_Error:
                        handleErrorMessage(msg);
                        break;
 
                case XMID_Warning:
                        handleWarningMessage(msg);
                        break;
 
                case XMID_MasterIndication:
                        handleMasterIndication(msg);
                        break;
 
                case XMID_Wakeup:
                        handleWakeupMessage(msg);
                        break;
 
                default:
                        JLDEBUGG("Handling non-data msg " << msg.getMessageId() << " bid " << JLHEXLOG((int)msg.getBusId()));
                        handleNonDataMessage(msg);
                        break;
        }
}
 
void XsDevice::handleNonDataMessage(const XsMessage& msg)
{
        onNonDataMessage(this, &msg);
}
 
void XsDevice::handleErrorMessage(const XsMessage& msg)
{
        uint8_t errorCode = msg.getDataByte(0);
        XsResultValue xsResultValue = static_cast<XsResultValue>(errorCode);
        onNonDataMessage(this, &msg);
        onError(this, xsResultValue);
}
 
void XsDevice::handleWarningMessage(const XsMessage& msg)
{
        onNonDataMessage(this, &msg);
}
 
void XsDevice::handleWakeupMessage(const XsMessage& msg)
{
        (void)msg;
        onWakeupReceived(this);
 
        //"reset" some of the states after a wake up of a device
        resetPacketStamping();
        setDeviceState(XDS_Config);
        if ((deviceOptionFlags() & XDOF_DisableAutoMeasurement) != XDOF_DisableAutoMeasurement)
                setDeviceState(XDS_Measurement);
}
 
void XsDevice::insertIntoDataCache(int64_t pid, XsDataPacket* pack)
{
        LockGuarded lockG(&m_deviceMutex);
 
        if (m_dataCache.empty())
                m_dataCache.insert(std::make_pair(pid, pack));
        else
        {
                auto it = m_dataCache.lower_bound(pid);
                if (it == m_dataCache.end())
                        m_dataCache.insert(it, std::make_pair(pid, pack));
                else
                {
                        if (it->first != pid)
                                m_dataCache.insert(it, std::make_pair(pid, pack));
                        else
                        {
                                it->second->merge(*pack, true);
                                delete pack;
                        }
                }
        }
}
 
void XsDevice::clearDataCache()
{
        LockGuarded lockG(&m_deviceMutex);
 
        for (auto& it : m_dataCache)
                delete it.second;
        m_dataCache.clear();
        //m_latestLivePacket->clear();
        m_latestBufferedPacket->clear();
        m_unavailableDataBoundary = -1;
}
 
void XsDevice::handleUnavailableData(int64_t frameNumber)
{
        LockGuarded lockG(&m_deviceMutex);
 
        if (frameNumber < latestBufferedPacketId())
                return; // The recording stream already advanced itself beyond this frame. We don't need it
        if (latestLivePacketId() == -1)
                return;
        if (m_stopRecordingPacketId >= 0 && frameNumber > m_stopRecordingPacketId)
                return; // we're missing data past the end of the recording, ignore
 
        //JLDEBUGG("Device " << m_deviceId << " Updating m_unavailableDataBoundary from " << m_unavailableDataBoundary << " to " << (std::max)(m_unavailableDataBoundary, frameNumber));
        m_unavailableDataBoundary = (std::max)(m_unavailableDataBoundary, frameNumber); // Note: (std::max) to prevent a macro for max() to be substituted
        checkDataCache();
}
 
bool XsDevice::shouldDoRecordedCallback(XsDataPacket const& p) const
{
        if (p.empty())
                return false;
 
        if (isReadingFromFile())
                return true;
 
        switch (deviceState())
        {
                case XDS_Recording:
                case XDS_FlushingData:
                        break;
 
                default:
                        return false;
        }
 
        if (isStandaloneDevice())
                return true;
 
        if (m_stopRecordingPacketId >= 0 && p.packetId() > m_stopRecordingPacketId)
                return false;
 
        if (!p.containsFrameRange())
                return p.packetId() >= m_startRecordingPacketId;
 
        int64_t startPid = p.packetId() - p.frameRange().interval();
        return startPid >= m_startRecordingPacketId;
}
 
void XsDevice::handleDataPacket(const XsDataPacket& packet)
{
        LockGuarded locky(&m_deviceMutex);
        if (m_terminationPrepared)
                return; // we're being destroyed, abort handling of datapacket, state may be invalid
 
        m_lastDataOkStamp = XsTimeStamp::now();
        int64_t fastest = latestLivePacketConst().packetId();
        int64_t slowest = latestBufferedPacketConst().packetId();
 
        std::unique_ptr<XsDataPacket> pack(new XsDataPacket(packet));
        master()->m_packetStamper.stampPacket(*pack, latestLivePacket());       // always go through master for stamping packets so we have consistent timing
        int64_t current = pack->packetId();
 
#if TOADUMP
        fprintf(master()->m_toaDumpFile, "%llu,%llu,%llu\n", current, pack->timeOfArrival().msTime(), pack->estimatedTimeOfSampling().msTime());
#endif
 
#if 0
        ONLYFIRSTMTX2
        JLWRITEG(this << " [TOALOG] pid: " << current <<
                " did: " << deviceId() <<
                " awindaframenr: " << packet.awindaSnapshot().m_frameNumber <<
                " packetTOA: " << pack->timeOfArrival().msTime() <<
                " fastest: " << fastest <<
                " slowest: " << slowest <<
                " etos " << pack->estimatedTimeOfSampling().msTime());
        JLDEBUGG("stamped: " << current << " new latestlive: " << latestLivePacketConst().packetId());
#endif
 
        bool interpolate = false;
        if (fastest >= 0)
        {
                int64_t dpc = current - fastest;
 
                if (dpc > 1)
                {
                        int64_t firstMissed = fastest + 1;
                        int64_t lastMissed = current - 1;
                        ONLYFIRSTMTX2
                        JLDEBUGG("Detected " << (dpc - 1) << " packets have been missed by device " << deviceId() << ", last was " << fastest << " (" << (uint16_t) fastest << ") current is " << current << " (" << (uint16_t) current << ")");
                        onMissedPackets(this, (int) dpc - 1, (int) firstMissed, (int) lastMissed);
 
                        for (int64_t i = firstMissed; i <= lastMissed; ++i)
                        {
                                if (!expectingRetransmissionForPacket(i))
                                {
                                        //JLDEBUGG("not expecting retransmission for packet " << i);
                                        handleUnavailableData(i);
                                        if (m_options & XSO_InterpolateMissingData)
                                                interpolate = true;
                                }
                        }
                }
        }
        else if (isReadingFromFile() && getStartRecordingPacketId() == -1)
                setStartRecordingPacketId(current);
 
        if (current >= fastest)
        {
                JLTRACEG("Processing (live) packet " << current);
                std::unique_ptr<XsDataPacket> copy(new XsDataPacket(*pack));
                processLivePacket(*copy);
 
                if (interpolate)
                {
                        // when this returns true, the packet has been processed properly already so we should return
                        if (interpolateMissingData(*copy, latestLivePacketConst(),
                                        [this](XsDataPacket * ppp)
                {
                        handleDataPacket(*ppp);
                                delete ppp;
                        }))
                        {
                                return;
                        }
                }
 
                // store result
                latestLivePacket().swap(*copy);
 
                // do callbacks
                if (!latestLivePacketConst().empty())
                {
                        if (m_options & XSO_KeepLastLiveData)
                                updateLastAvailableLiveDataCache(latestLivePacketConst());
                        if (m_options & XSO_RetainLiveData)
                                retainPacket(latestLivePacketConst());
 
                        //if (isRecording())
                        //      JLDEBUGG("Device " << m_deviceId << " triggering onLiveDataAvailable " << latestLivePacketConst().packetId());
                        onLiveDataAvailable(this, &latestLivePacketConst());
                        if (!isReadingFromFile())
                                onDataAvailable(this, &latestLivePacketConst());
                        if (isStandaloneDevice())
                        {
                                XsDevicePtrArray devs;
                                const XsDataPacket* ppack = &latestLivePacketConst();
                                devs.push_back(this);
                                XsDataPacketPtrArray packs;
                                packs.push_back(const_cast<XsDataPacket*>(ppack));
                                onAllLiveDataAvailable(&devs, &packs);
                                if (!isReadingFromFile())
                                        onAllDataAvailable(&devs, &packs);
                        }
                }
        }
        else
        {
                // probably a retransmission
                //JLDEBUGG("Device " << deviceId() << " ignoring (live) packet " << current << " because it is older than " << fastest);
        }
 
        if (latestBufferedPacketConst().empty() || current > slowest)
        {
                // insert into cache
                //JLDEBUGG("Device " << deviceId() << " Adding (buffered) packet " << current);
                insertIntoDataCache(current, pack.get());
                pack.release();
                checkDataCache();
        }
        else
        {
                // not correct, weird old retransmission
                ONLYFIRSTMTX2
                JLDEBUGG("Device " << deviceId() << " ignoring (buffered) packet " << current << " because it is not newer than " << slowest);
        }
}
bool XsDevice::isMeasuring() const
{
        switch (deviceState())
        {
                case XDS_Measurement:
                case XDS_WaitingForRecordingStart:
                case XDS_Recording:
                case XDS_FlushingData:
                        return true;
 
                case XDS_Initial:
                case XDS_Config:
                case XDS_Destructing:
                default:
                        return false;
        }
}
 
bool XsDevice::isRecording() const
{
        switch (deviceState())
        {
                case XDS_WaitingForRecordingStart:
                case XDS_Recording:
                case XDS_FlushingData:
                        return true;
 
                case XDS_Measurement:
                case XDS_Initial:
                case XDS_Config:
                case XDS_Destructing:
                default:
                        return false;
        }
}
 
bool XsDevice::isReadingFromFile() const
{
        if (!communicator())
                return false;
        assert(communicator());
        return communicator()->isReadingFromFile();
}
 
void XsDevice::processLivePacket(XsDataPacket&)
{
}
 
void XsDevice::processBufferedPacket(XsDataPacket&)
{
}
void XsDevice::flushInputBuffers()
{
        JLDEBUGG(this);
        if (isMasterDevice() && communicator())
                communicator()->flushPort();
        clearDataCache();
        resetPacketStamping();
}
 
XsSyncRole XsDevice::syncRole() const
{
        return XSR_None;
}
 
bool XsDevice::readDeviceConfiguration()
{
        JLDEBUGG("");
        if (isMasterDevice())
        {
                Communicator* comm = communicator();
                if (!comm)
                        return false;
 
                if (comm->isReadingFromFile())
                {
                        XsMessage rcv;
 
                        rcv = comm->readMessageFromStartOfFile(XMID_Configuration, Communicator::configurationMessageSearchLimit());
                        if (comm->lastResult() != XRV_OK)
                                return false;
                        m_config.readFromMessage(rcv);
 
                        rcv = comm->readMessageFromStartOfFile(XMID_FirmwareRevision, Communicator::configurationMessageSearchLimit() * (busId() == XS_BID_MASTER ? 2 : (1 + busId())));
                        if (comm->lastResult() != XRV_OK)
                        {
                                // We cannot determine the firmware version of the master, use the first sensor (fallback)
                                auto const& tmp = m_config.deviceInfo((XsSize)(ptrdiff_t)busId());
                                m_firmwareVersion = XsVersion(tmp.m_fwRevMajor, tmp.m_fwRevMinor, tmp.m_fwRevRevision);
                        }
                        else
                                extractFirmwareVersion(rcv);
                }
                else
                {
                        if (!gotoConfig())
                        {
                                JLALERTG("Failed to go to config");
                                return false;
                        }
 
                        XsMessage snd(XMID_Initbus);
                        snd.setBusId(busId());
 
                        if (!doTransaction(snd, 500))
                        {
                                JLALERTG("Failed to init bus");
                                return false;
                        }
 
                        snd.setMessageId(XMID_ReqConfiguration);
                        snd.setBusId(busId());
 
                        XsMessage rcv;
                        if (!doTransaction(snd, rcv, 5000))
                        {
                                JLALERTG("Failed to req configuration");
                                return false;
                        }
 
                        m_config.readFromMessage(rcv);
                        m_firmwareVersion = XsVersion();
                        snd.setMessageId(XMID_ReqFirmwareRevision);
                        snd.setBusId(busId());
 
                        if (doTransaction(snd, rcv) && rcv.getMessageId() == XMID_FirmwareRevision)
                                extractFirmwareVersion(rcv);
 
                        m_outputConfiguration = outputConfiguration();
                }
                return true;
        }
        else
        {
                try
                {
                        const XsMtDeviceConfiguration& devinf = deviceConfigurationConst().deviceInfo(deviceId());
                        XsVersion old = m_firmwareVersion;
                        m_firmwareVersion = XsVersion(devinf.m_fwRevMajor, devinf.m_fwRevMinor, devinf.m_fwRevRevision);
#ifndef XSENS_DEBUG
                        // this is support debug info, we don't want to spam the debugger windows
                        if (old != m_firmwareVersion)
                                JLWRITEG("Device " << deviceId() << " has firmware version " << m_firmwareVersion.toString());
#endif
                        return true;
                }
                catch (XsDeviceConfigurationException&)
                {
                        return false;
                }
        }
        return false;
}
 
XsDeviceConfiguration const& XsDevice::deviceConfigurationConst() const
{
        return m_config;
}
 
XsDeviceConfiguration& XsDevice::deviceConfigurationRef()
{
        return const_cast<XsDeviceConfiguration&>(deviceConfigurationConst());
}
XsDeviceConfiguration XsDevice::deviceConfiguration() const
{
        return deviceConfigurationConst();
}
 
bool XsDevice::restoreFactoryDefaults()
{
        if (deviceState() == XDS_Measurement || deviceState() == XDS_Recording)
                return false;
 
        XsMessage snd(XMID_RestoreFactoryDef);
        snd.setBusId(busId());
 
        if (!doTransaction(snd, 2000))
                return false;
 
        return true;
}
 
bool XsDevice::reset()
{
        return reset(false);
}
 
bool XsDevice::reset(bool skipDeviceIdCheck)
{
        if (!isMasterDevice())
                return false;
 
        Communicator* comm = communicator();
        if (!comm)
                return false;
 
        if (!gotoConfig())
                return false;
 
        XsMessage snd(XMID_Reset);
        snd.setBusId(XS_BID_MASTER);
 
        TRANSACTIONLOG(m_deviceId << " SND: " << msgToString(snd));
        std::shared_ptr<ReplyObject> wakeup = comm->addReplyObject(XMID_Wakeup);
        comm->writeMessage(snd);
 
        if (!reopenPort(false, skipDeviceIdCheck))
                return false;
 
        if (!gotoConfig())
                return false;
 
        return true;
}
bool XsDevice::reopenPort(bool gotoConfig, bool skipDeviceIdCheck)
{
        setGotoConfigOnClose(gotoConfig);
 
        Communicator* comm = communicator();
        if (!comm)
                return false;
 
        return comm->reopenPort(OPS_OpenPort, skipDeviceIdCheck);
}
 
void XsDevice::writeDeviceSettingsToFile()
{
}
 
XsResultValue XsDevice::createLogFile(const XsString& filename)
{
        JLDEBUGG(filename);
        Communicator* comm = communicator();
        if (!comm || !comm->isPortOpen())
        {
                JLALERTG("No port open");
                return XRV_NOPORTOPEN;
        }
 
        std::unique_ptr<xsens::Lock> myLock(new xsens::Lock(&m_logFileMutex, true));
        if (logFileInterface(myLock))
        {
                JLERRORG("A file is already open");
                return XRV_ALREADYOPEN;
        }
 
        std::unique_ptr<MtbDataLogger> newfile(new MtbDataLogger);
        if (newfile->create(filename))
        {
                m_logFileInterface = newfile.release();
                JLDEBUGG("Creation ok");
                tm dateTime;
                getDateTime(&dateTime);
                getDateAsString((char*) m_config.masterInfo().m_date, &dateTime);
                getTimeAsString((char*) m_config.masterInfo().m_time, &dateTime);
 
                XsMessage msg;
                deviceConfiguration().writeToMessage(msg);
                m_logFileInterface->writeMessage(msg);
                writeDeviceSettingsToFile();
                return XRV_OK;
        }
        JLDEBUGG("Creation failed");
        newfile->close(true);
        removeChainedManager(m_logFileInterface);
        newfile.reset();
        return XRV_OUTPUTCANNOTBEOPENED;
}
 
XsResultValue XsDevice::createConfigFile(const XsString&)
{
        return XRV_NOTIMPLEMENTED;
}
 
bool XsDevice::setAlignmentRotationQuaternion(XsAlignmentFrame frame, const XsQuaternion& quat)
{
        (void)frame;
        (void)quat;
        return false;
}
 
XsQuaternion XsDevice::alignmentRotationQuaternion(XsAlignmentFrame frame) const
{
        (void)frame;
        return XsQuaternion();
}
 
bool XsDevice::setAlignmentRotationMatrix(XsAlignmentFrame frame, const XsMatrix& matrix)
{
        (void)frame;
        (void)matrix;
        return false;
}
 
XsMatrix XsDevice::alignmentRotationMatrix(XsAlignmentFrame frame) const
{
        (void)frame;
        return XsMatrix();
}
 
XsResultValue XsDevice::applyConfigFile(const XsString&)
{
        return XRV_NOTIMPLEMENTED;
}
 
bool XsDevice::closeLogFile()
{
        xsens::Lock myLock(&m_logFileMutex, true);
        if (m_logFileInterface)
        {
                JLDEBUGG(m_logFileInterface);
                m_logFileInterface->close();
                removeChainedManager(m_logFileInterface);
                delete m_logFileInterface;
                m_logFileInterface = NULL;
        }
        return true;
}
 
bool XsDevice::startRecording()
{
        JLDEBUGG("");
        if (!isMasterDevice())
                return false;
 
        if (deviceState() != XDS_Measurement)
                return false;
 
        writeFilterStateToFile();
        setDeviceState(XDS_Recording);
 
        return true;
}
 
bool XsDevice::triggerStartRecording()
{
        return startRecording();
}
 
bool XsDevice::stopRecording()
{
        JLDEBUGG("");
        if (!isMasterDevice())
                return false;
 
        if (deviceState() != XDS_Recording)
                return false;
 
        setDeviceState(XDS_Measurement);
        return true;
}
 
bool XsDevice::packetContainsRetransmission(XsDataPacket const& packet)
{
        if (packet.containsAwindaSnapshot())
                return packet.isAwindaSnapshotARetransmission();
 
        return false;
}
 
void XsDevice::endRecordingStream()
{
        LockGuarded lockG(&m_deviceMutex);
 
        if (m_dataCache.empty() || (m_startRecordingPacketId < 0))
                return;
 
        if (m_stopRecordingPacketId >= 0)
        {
                //The first packetId following the latestBufferedPacket (head of recording stream) is what is blocking possible cached data
                //If the recording stream is empty the beginning of the recording is unavailable (from start recording until the first packet in the cache)
                int64_t blockingPacketId = (latestBufferedPacketId() < 0) ? m_startRecordingPacketId : latestBufferedPacketId() + 1;
 
                while (blockingPacketId <= m_stopRecordingPacketId)
                {
                        //Mark blocking packet as unavailable.
                        handleUnavailableData(blockingPacketId);
                        //If marking the blocking packet unavailable did not advance the head of the recording stream the next blocking packet follows the current one immediately
                        blockingPacketId = (std::max)(blockingPacketId + 1, latestBufferedPacketId() + 1); // Note: (std::max) to prevent a macro for max() to be substituted
                }
        }
 
        checkDataCache();
        // clearDataCache();
}
 
void XsDevice::checkDataCache()
{
        LockGuarded lockG(&m_deviceMutex);
        // process available data
        while (!m_dataCache.empty())
        {
                auto it = m_dataCache.begin();
                //JLWRITEG("pid: " << it->second->packetId() << " range? " << it->second->containsFrameRange() << " retransmission? " << it->second->isAwindaSnapshotARetransmission() << " snapshotA,F? " << it->second->containsAwindaSnapshot() << "," << it->second->containsFullSnapshot());
 
                int64_t expectedPacketId = latestBufferedPacketId() < 0 ? -1 : latestBufferedPacketId() + 1;
                if (expectedPacketId < m_startRecordingPacketId)
                {
                        //If there is a frame range (i.e. we get an interval) the packetId is equal to the end of that interval
                        //The startRecordingPacketId always is equal to the start value of an interval. Therefore if using the startRecordingPacketId to
                        //calculate the expected packetId for an ideal interval (no missing data) is to +1 the startRecordingPacketId
                        //For an ideal (expected) situation this would be 1 higher than the startRecordingPacketId
                        expectedPacketId = it->second->containsFrameRange() ? m_startRecordingPacketId + 1 : getStartRecordingPacketId();
                        expectedPacketId = PacketStamper::calculateLargePacketCounter(expectedPacketId, latestLivePacketId(), PacketStamper::MTSCBOUNDARY);
                }
                int64_t packetId = it->first;
 
                auto missingDataIsUnavailable = [this](int64_t rFirst, int64_t rLast)
                {
                        if (m_options & XSO_ExpectNoRetransmissionsInFile)
                                return true;
                        if ((m_stopRecordingPacketId >= 0 && rFirst > m_stopRecordingPacketId) ||
                                (rLast - 1 <= m_unavailableDataBoundary))
                        {
                                // notify unavailable data in recording states
                                // when not recording, unavailable data is expected and should not be reported
                                if (isRecording())
                                {
                                        //JLDEBUGG("Device " << m_deviceId << " data unavailable in range " << rFirst << " - " << rLast);
                                        if (m_stopRecordingPacketId != -1 && rLast > m_stopRecordingPacketId)
                                                rLast = m_stopRecordingPacketId;
                                        if (rFirst < rLast)
                                        {
                                                ONLYFIRSTMTX2
                                                JLDEBUGG("Device " << m_deviceId << " Data is unavailable in reported range " << rFirst << " - " << rLast << " Stop Recording Packet ID " << m_stopRecordingPacketId << " Unavailable Data Boundary " << m_unavailableDataBoundary);
                                                for (int64_t r = rFirst; r < rLast; ++r)
                                                        onDataUnavailable(this, r);
                                        }
                                        else
                                        {
                                                //ONLYFIRSTMTX2
                                                //JLDEBUGG("Device " << m_deviceId << " NOT Reporting data unavailable");
                                        }
                                }
 
                                return true;
                        }
                        return false;
                };
 
                int64_t rFirst = expectedPacketId >= 0 ? expectedPacketId : packetId;
                int64_t rLast = packetId;
                if (it->second->containsFrameRange())
                {
                        XsRange rng = it->second->frameRange();
                        rFirst = rng.first() + 1;
                }
 
                if ((expectedPacketId >= 0) && (packetId > expectedPacketId))
                {
                        if (!missingDataIsUnavailable(rFirst, rLast))
                        {
                                //JLDEBUGG("Device " << m_deviceId << " Waiting for retransmissions in range " << rFirst << " - " << rLast);
                                break;  // waiting for retransmissions
                        }
                }
                // we need to 'else' here to avoid duplicate and erroneous missed packet handling
                else if (it->second->containsFrameRange())
                {
                        if (rLast > rFirst)
                        {
                                if (!missingDataIsUnavailable(rFirst, rLast))
                                        break;  // waiting for retransmissions
                        }
                }
 
                // do 'buffered' processing
                processBufferedPacket(*it->second);
 
                // store result
                latestBufferedPacket().swap(*it->second);
                //              ONLYFIRSTMTX2
                //              JLDEBUGG("latestBufferedPacket is now " << latestBufferedPacket().packetId() << " old: " << it->second->packetId());
                delete it->second;
                m_dataCache.erase(it);
 
                if (latestBufferedPacketConst().empty())
                        continue;
 
                if (m_options & XSO_RetainBufferedData)
                        retainPacket(latestBufferedPacketConst());
 
                // do callbacks
                const XsDataPacket& buf = latestBufferedPacketConst();
                //if (isRecording())
                //      JLDEBUGG("Device " << m_deviceId << " triggering onBufferedDataAvailable " << buf.packetId());
                onBufferedDataAvailable(this, &buf);
                bool doRecCallback = shouldDoRecordedCallback(buf);
                if (isReadingFromFile())
                        onDataAvailable(this, &buf);
                if (doRecCallback)
                        onRecordedDataAvailable(this, &buf);
 
                if (isStandaloneDevice())
                {
                        XsDevicePtrArray devs;
                        const XsDataPacket* ppack = &latestBufferedPacketConst();
                        devs.push_back(this);
                        XsDataPacketPtrArray packs;
                        packs.push_back((XsDataPacket*)ppack);
                        onAllBufferedDataAvailable(&devs, &packs);
                        if (isReadingFromFile())
                                onAllDataAvailable(&devs, &packs);
                        if (doRecCallback)
                                onAllRecordedDataAvailable(&devs, &packs);
                }
        }
}
 
void XsDevice::writeMessageToLogFile(const XsMessage& message)
{
        if (communicator()->isReadingFromFile())
                return;
 
        if (!onWriteMessageToLogFile(this, &message))
                return;
 
        std::unique_ptr<xsens::Lock> myLock;
        DataLogger* logInt = logFileInterface(myLock);
        if (!logInt)
                return;
 
        logInt->writeMessage(message);
}
 
void XsDevice::writeFilterStateToFile()
{
}
std::vector<int> XsDevice::supportedUpdateRates(XsDataIdentifier dataType) const
{
        (void)dataType;
        return std::vector<int>();
}
 
bool XsDevice::isBlueToothEnabled() const
{
        return false;
}
bool XsDevice::setBlueToothEnabled(bool enabled)
{
        (void)enabled;
        return false;
}
 
bool XsDevice::isBusPowerEnabled() const
{
        return false;
}
bool XsDevice::setBusPowerEnabled(bool enabled)
{
        (void)enabled;
        return false;
}
bool XsDevice::powerDown()
{
        return false;
}
XsErrorMode XsDevice::errorMode() const
{
        return XEM_Invalid;
}
bool XsDevice::setErrorMode(XsErrorMode errormode)
{
        (void)errormode;
        return false;
}
 
// MT Device functions
 
bool XsDevice::setHeadingOffset(double offset)
{
        (void)offset;
        return false;
}
 
double XsDevice::headingOffset() const
{
        return 0;
}
 
bool XsDevice::setLocationId(int id)
{
        (void)id;
        return false;
}
 
int XsDevice::locationId() const
{
        return 0;
}
 
XsDevice* XsDevice::getDeviceFromLocationId(uint16_t locId)
{
        if (locationId() == locId)
                return this;
 
        return nullptr;
}
 
XsMatrix XsDevice::objectAlignment() const
{
        return XsMatrix();
}
 
bool XsDevice::setObjectAlignment(const XsMatrix& matrix)
{
        (void)matrix;
        return false;
}
 
double XsDevice::gravityMagnitude() const
{
        return 0;
}
 
bool XsDevice::setGravityMagnitude(double mag)
{
        (void)mag;
        return false;
}
 
bool XsDevice::reinitialize()
{
        return false;
}
 
XsFilterProfile XsDevice::xdaFilterProfile() const
{
        return XsFilterProfile();
}
 
bool XsDevice::setXdaFilterProfile(int profileType)
{
        (void)profileType;
        return false;
}
 
bool XsDevice::setXdaFilterProfile(XsString const& profileType)
{
        (void)profileType;
        return false;
}
 
XsFilterProfile XsDevice::onboardFilterProfile() const
{
        return XsFilterProfile();
}
 
bool XsDevice::setOnboardFilterProfile(int profileType)
{
        (void)profileType;
        return false;
}
 
bool XsDevice::setOnboardFilterProfile(XsString const& profileType)
{
        (void)profileType;
        return false;
}
 
bool XsDevice::replaceFilterProfile(XsFilterProfile const& profileCurrent, XsFilterProfile const& profileNew)
{
        (void)profileCurrent;
        (void)profileNew;
        return false;
}
 
XsFilterProfileArray XsDevice::availableOnboardFilterProfiles() const
{
        return XsFilterProfileArray();
}
 
XsFilterProfileArray XsDevice::availableXdaFilterProfiles() const
{
        return XsFilterProfileArray();
}
 
double XsDevice::accelerometerRange() const
{
        return 0;
}
 
double XsDevice::gyroscopeRange() const
{
        return 0;
}
 
bool XsDevice::setNoRotation(uint16_t duration)
{
        (void)duration;
        return false;
}
 
bool XsDevice::startRepresentativeMotion()
{
        return false;
}
 
bool XsDevice::representativeMotionState()
{
        return false;
}
 
XsIccRepMotionResult XsDevice::stopRepresentativeMotion()
{
        return XsIccRepMotionResult();
}
 
bool XsDevice::storeIccResults()
{
        return false;
}
 
XsVector XsDevice::initialPositionLLA() const
{
        return XsVector();
}
 
bool XsDevice::setInitialPositionLLA(const XsVector& lla)
{
        (void)lla;
        return false;
}
 
XsTimeInfo XsDevice::utcTime() const
{
        return XsTimeInfo();
}
 
bool XsDevice::setUtcTime(const XsTimeInfo& time)
{
        (void)time;
        return false;
}
 
uint16_t XsDevice::rs485TransmissionDelay() const
{
        return 0;
}
 
bool XsDevice::setRs485TransmissionDelay(uint16_t delay)
{
        (void)delay;
        return false;
}
 
XsSelfTestResult XsDevice::runSelfTest()
{
        return XsSelfTestResult();
}
 
bool XsDevice::requestData()
{
        return false;
}
 
bool XsDevice::storeFilterState()
{
        return false;
}
 
// MTix device
bool XsDevice::isInitialBiasUpdateEnabled() const
{
        return false;
}
 
bool XsDevice::setInitialBiasUpdateEnabled(bool enable)
{
        (void)enable;
        return false;
}
 
bool XsDevice::isFixedGravityEnabled() const
{
        return false;
}
 
bool XsDevice::setFixedGravityEnabled(bool enable)
{
        (void)enable;
        return false;
}
 
bool XsDevice::setGnssLeverArm(const XsVector& arm)
{
        (void)arm;
        return false;
}
 
XsVector XsDevice::gnssLeverArm() const
{
        return XsVector();
}
 
bool XsDevice::requestUtcTime()
{
        return false;
}
// Mtw device
bool XsDevice::requestBatteryLevel()
{
        return false;
}
 
XsTimeStamp XsDevice::batteryLevelTime()
{
        return 0;
}
 
bool XsDevice::setTransportMode(bool transportModeEnabled)
{
        (void)transportModeEnabled;
        return false;
}
 
bool XsDevice::transportMode()
{
        return false;
}
 
bool XsDevice::isInStringOutputMode() const
{
        return stringOutputType() != 0;
}
 
bool XsDevice::usesLegacyDeviceMode() const
{
        return isInStringOutputMode();
}
 
void XsDevice::useLogInterface(DataLogger* logger)
{
        xsens::Lock myLock(&m_logFileMutex, true);
        m_logFileInterface = logger;
}
bool XsDevice::loadLogFile()
{
        Communicator* comm = communicator();
        if (!comm)
                return false;
 
        if (!comm->isReadingFromFile())
                return false;
 
        comm->loadLogFile(this);
        return true;
}
 
bool XsDevice::abortLoadLogFile()
{
        Communicator* comm = communicator();
        if (comm && comm->isReadingFromFile())
        {
                comm->abortLoadLogFile();
                comm->waitForLastTaskCompletion();
                return true;
        }
        return false;
}
 
XsString XsDevice::logFileName() const
{
        if (!isReadingFromFile())
        {
                std::unique_ptr<xsens::Lock> myLock;
                if (logFileInterface(myLock))
                {
                        MtbDataLogger const* mtb = dynamic_cast<MtbDataLogger const*>(logFileInterface(myLock));
                        if (mtb != nullptr)
                                return mtb->filename();
                }
                return XsString();
        }
 
        Communicator* object = communicator();
        if (!object)
                return XsString();
        return object->logFileName();
}
 
int XsDevice::maximumUpdateRate() const
{
        // Get the supported update rates
        std::vector<int> frameRates = supportedUpdateRates();
 
        // Determine the maximum value
        std::vector<int>::iterator p = std::max_element(frameRates.begin(), frameRates.end());
        if (p != frameRates.end())
                return *p;
        else
                return 0;
}
 
int XsDevice::recordingQueueLength() const
{
        LockGuarded lockG(&m_deviceMutex);
 
        if (m_dataCache.empty())
                return 0;
 
        return (int)(m_dataCache.rbegin()->first - latestBufferedPacketId());
}
 
int XsDevice::cacheSize() const
{
        LockGuarded lockG(&m_deviceMutex);
        return (int) m_dataCache.size();
}
 
 
XsSyncSettingArray XsDevice::syncSettings() const
{
        return XsSyncSettingArray();
}
 
bool XsDevice::setSyncSettings(const XsSyncSettingArray& settingList)
{
        (void)settingList;
        return false;
}
 
XsSyncSettingArray XsDevice::supportedSyncSettings() const
{
        return Synchronization::supportedSyncSettings(m_deviceId);
}
 
bool XsDevice::resetOrientation(XsResetMethod resetmethod)
{
        return scheduleOrientationReset(resetmethod);
}
 
bool XsDevice::resetLogFileReadPosition()
{
        JLDEBUGG("");
 
        Communicator* comm = communicator();
        if (!comm || !comm->isReadingFromFile())
                return false;
 
        comm->resetLogFileReadPosition();
 
        clearDataCache();
        resetPacketStamping();
        return true;
}
 
XsFilePos XsDevice::logFileSize() const
{
        JLDEBUGG("");
 
        Communicator* comm = communicator();
        if (!comm || !comm->isReadingFromFile())
                return 0;
 
        return comm->logFileSize();
}
 
XsFilePos XsDevice::logFileReadPosition() const
{
        JLDEBUGG("");
 
        Communicator* comm = communicator();
        if (!comm || !comm->isReadingFromFile())
                return -1;
 
        return comm->logFileReadPosition();
}
 
bool XsDevice::updateCachedDeviceInformation()
{
        JLDEBUGG("");
 
        if (isMeasuring())
                return false;
 
        return initialize();
}
 
bool XsDevice::initialize()
{
        m_isInitialized = true;
        return true;
}
bool XsDevice::enableProtocol(XsProtocolType protocol)
{
        Communicator* comm = communicator();
        if (!comm)
                return false;
 
        switch (protocol)
        {
                case XPT_Xbus:
                        comm->addProtocolHandler(new ProtocolHandler());
                        return true;
                case XPT_Nmea:
                        comm->addProtocolHandler(new nmea::ProtocolHandler());
                        return true;
                default:
                        return false;   // unknown type
        }
}
 
bool XsDevice::disableProtocol(XsProtocolType protocol)
{
        Communicator* comm = communicator();
        if (!comm)
                return false;
 
        switch (protocol)
        {
                case XPT_Xbus:
                        comm->removeProtocolHandler(XPT_Xbus);
                        return true;
                case XPT_Nmea:
                        comm->removeProtocolHandler(XPT_Nmea);
                        return true;
                default:
                        return false;   // unknown type
        }
}
 
bool XsDevice::isProtocolEnabled(XsProtocolType protocol) const
{
        Communicator* comm = communicator();
        if (!comm)
                return false;
        return comm->hasProtocol(protocol);
}
 
uint16_t XsDevice::stringSamplePeriod() const
{
        return 0xFFFF;
}
 
uint16_t XsDevice::stringSkipFactor() const
{
        return 0xFFFF;
}
 
uint16_t XsDevice::stringOutputType() const
{
        return 0x0000;
}
 
XsOperationalMode XsDevice::operationalMode() const
{
        return XOP_LiveStream;
}
 
bool XsDevice::setOperationalMode(XsOperationalMode mode)
{
        (void)mode;
        return true;
}
 
XsOutputConfigurationArray XsDevice::outputConfiguration() const
{
        return XsOutputConfigurationArray();
}
 
XsCanOutputConfigurationArray XsDevice::canOutputConfiguration() const
{
        return XsCanOutputConfigurationArray();
}
 
uint32_t XsDevice::canConfiguration() const
{
        return 0;
}
 
void XsDevice::setOptions(XsOption enable, XsOption disable)
{
        if (communicator() && !communicator()->allowReprocessing())
        {
                enable = enable & ~(XSO_Calibrate | XSO_Orientation);
                disable = disable | (XSO_Calibrate | XSO_Orientation);
        }
 
        XsOption upd = XsOption_purify((XsOption)((m_options & ~disable) | enable));
        static const XsOption mask = XSO_KeepLastLiveData | XSO_RetainLiveData | XSO_RetainBufferedData;
 
        // this has to be done in this order to prevent timing issues
        bool clr = ((m_options & mask) != (upd & mask));
        m_options = upd;
        if (clr)
                clearExternalPacketCaches();
}
 
bool XsDevice::areOptionsEnabled(XsOption options) const
{
        return (m_options & options) == options;
}
 
XsOption XsDevice::getOptions() const
{
        return m_options;
}
 
XsString XsDevice::productCode() const
{
        return XsString();
}
 
XsString XsDevice::shortProductCode() const
{
        return productCode();
}
 
XsVersion XsDevice::hardwareVersion() const
{
        return XsVersion();
}
 
bool XsDevice::enableRadio(int channel)
{
        (void)channel;
        return false;
}
 
bool XsDevice::disableRadio()
{
        return false;
}
 
bool XsDevice::isRadioEnabled() const
{
        return false;
}
 
int XsDevice::radioChannel() const
{
        return -1;
}
 
#if 0
 
int XsDevice::radioQualityIndication() const
{
        return -1;
}
#endif
 
void XsDevice::handleMasterIndication(const XsMessage& message)
{
        onNonDataMessage(this, &message);
}
bool XsDevice::abortFlushing()
{
        return true;
}
 
bool XsDevice::acceptConnection()
{
        return false;
}
 
bool XsDevice::rejectConnection()
{
        return false;
}
 
int XsDevice::wirelessPriority() const
{
        return 0;
}
 
bool XsDevice::setWirelessPriority(int priority)
{
        (void)priority;
        return false;
}
 
void XsDevice::updateConnectivityState(XsConnectivityState newState)
{
        if (newState != m_connectivity)
        {
                JLDEBUGG(deviceId() << " newState " << newState << " oldState " << m_connectivity);
                m_connectivity = newState;
                onConnectivityChanged(this, newState);
        }
}
 
XsConnectivityState XsDevice::defaultChildConnectivityState() const
{
        return m_connectivity;
}
XsRejectReason XsDevice::rejectReason() const
{
        return XRR_Unknown;
}
 
int16_t XsDevice::lastKnownRssi() const
{
        return XS_RSSI_UNKNOWN;
}
 
void XsDevice::setPacketErrorRate(int per)
{
        (void)per;
}
int XsDevice::packetErrorRate() const
{
        return 0;
}
 
XsConnectivityState XsDevice::connectivityState() const
{
        return m_connectivity;
}
 
bool XsDevice::deviceIsDocked(XsDevice* dev) const
{
        (void)dev;
        return false;
}
 
XsString XsDevice::portName() const
{
        Communicator* comm = communicator();
        if (!comm)
                return XsString();
 
        return comm->portInfo().portName();
}
 
XsPortInfo XsDevice::portInfo() const
{
        Communicator* comm = communicator();
        if (!comm)
                return XsPortInfo();
 
        return comm->portInfo();
}
 
#ifndef XSENS_NO_PORT_NUMBERS
 
int XsDevice::portNumber() const
{
        Communicator* comm = communicator();
        if (!comm)
                return 0;
        return comm->portInfo().portNumber();
}
#endif
 
bool XsDevice::isInitialized() const
{
        return m_isInitialized;
}
 
bool XsDevice::setDeviceAccepted(const XsDeviceId&)
{
        return false;
}
 
bool XsDevice::setDeviceRejected(const XsDeviceId&)
{
        return false;
}
 
bool XsDevice::setAccessControlMode(XsAccessControlMode, const XsDeviceIdArray&)
{
        return false;
}
 
XsAccessControlMode XsDevice::accessControlMode() const
{
        return XACM_None;
}
 
XsDeviceIdArray XsDevice::currentAccessControlList() const
{
        return XsDeviceIdArray();
}
 
bool XsDevice::makeOperational()
{
        return false;
}
 
bool XsDevice::isOperational() const
{
        return false;
}
 
bool XsDevice::isInSyncStationMode()
{
        return false;
}
 
bool XsDevice::setSyncStationMode(bool)
{
        return false;
}
 
void XsDevice::addRef()
{
        JLTRACEG(this << " pre: " << m_refCounter.load());
        ++m_refCounter;
}
 
XsSize XsDevice::refCounter() const
{
        return (XsSize)(ptrdiff_t) m_refCounter.load();
}
 
void XsDevice::removeRef()
{
        JLTRACEG(this << " pre: " << m_refCounter.load());
 
        // Do not decrease when already at 0 ref count
        // In this case, the device is just waiting for all children to be dereferenced
        if (m_refCounter.load() != 0)
        {
                --m_refCounter;
                assert(m_refCounter.load() >= 0);
        }
 
        // Always check if this device can be removed
        removeIfNoRefs();
}
 
bool XsDevice::writeEmtsPage(uint8_t const* data, int pageNr, int bankNr)
{
        (void)data;
        (void)pageNr;
        (void)bankNr;
        return false;
}
 
void XsDevice::removeIfNoRefs()
{
        // If any child has refs, no need to delete this device
        LockSuspendable lock(&m_deviceMutex, LS_Write);
        if (childCount())
                return;
        //for (XsDevice* dev:m_children)
        //      if (dev->refCounter() > 0)
        //              return;
 
        if (m_refCounter.load() == 0)
        {
                lock.unlock();
                prepareForTermination();
                delete this;
        }
}
 
bool XsDevice::messageLooksSane(const XsMessage& msg) const
{
        return msg.getBusId() == XS_BID_MASTER;
}
 
void XsDevice::onMessageSent(const XsMessage& msg)
{
        onMessageSentToDevice(this, &msg);
}
 
void XsDevice::onMessageReceived(const XsMessage& msg)
{
        onMessageReceivedFromDevice(this, &msg);
}
 
void XsDevice::onMessageDetected2(XsProtocolType type, const XsByteArray& rawMessage)
{
        CallbackManagerXda::onMessageDetected(this, type, &rawMessage);
}
 
void XsDevice::onSessionRestarted()
{
}
 
void XsDevice::onConnectionLost()
{
        updateConnectivityState(XCS_Disconnected);
}
 
void XsDevice::onEofReached()
{
        LockGuarded locky(&m_deviceMutex);
        setStopRecordingPacketId(latestLivePacketId());
        endRecordingStream();
}
 
void XsDevice::onWirelessConnectionLost()
{
        updateConnectivityState(XCS_Disconnected);
}
 
int64_t XsDevice::latestLivePacketId() const
{
        LockGuarded locky(&m_deviceMutex);
        return latestLivePacketConst().packetId();
}
 
int64_t XsDevice::latestBufferedPacketId() const
{
        LockGuarded locky(&m_deviceMutex);
        return latestBufferedPacketConst().packetId();
}
 
bool XsDevice::shouldWriteMessageToLogFile(const XsMessage& msg) const
{
        return shouldWriteMessageToLogFile(this, msg);
}
 
bool XsDevice::shouldWriteMessageToLogFile(const XsDevice* dev, const XsMessage& message) const
{
        (void)dev;
        (void)message;
        return true;
}
 
bool XsDevice::doTransaction(const XsMessage& snd) const
{
        TRANSACTIONLOG(m_deviceId << " SND: " << msgToString(snd));
        bool rv = communicator() && communicator()->doTransaction(snd);
        TRANSACTIONLOG(m_deviceId << " RCV rv: " << rv);
        return rv;
}
 
bool XsDevice::doTransaction(const XsMessage& snd, uint32_t timeout) const
{
        TRANSACTIONLOG(m_deviceId << " SND: " << msgToString(snd) << " timeout: " << timeout);
        bool rv = communicator() && communicator()->doTransaction(snd, timeout);
        TRANSACTIONLOG(m_deviceId << " RCV rv: " << rv);
        return rv;
}
 
bool XsDevice::doTransaction(const XsMessage& snd, XsMessage& rcv) const
{
        TRANSACTIONLOG(m_deviceId << " SND: " << msgToString(snd));
        bool rv = communicator() && communicator()->doTransaction(snd, rcv);
        TRANSACTIONLOG(m_deviceId << " RCV: " << msgToString(rcv));
        return rv;
}
 
bool XsDevice::doTransaction(const XsMessage& snd, XsMessage& rcv, uint32_t timeout) const
{
        TRANSACTIONLOG(m_deviceId << " SND: " << msgToString(snd) << " timeout: " << timeout);
        bool rv = communicator() && communicator()->doTransaction(snd, rcv, timeout);
        TRANSACTIONLOG(m_deviceId << " RCV: " << msgToString(rcv));
        return rv;
}
bool XsDevice::setStealthMode(bool)
{
        return false;
}
 
bool XsDevice::stealthMode() const
{
        return false;
}
 
XsSyncSettingArray XsDevice::supportedSyncSettings(XsDeviceId const& deviceId)
{
        XsSyncSettingArray settings = Synchronization::supportedSyncSettings(deviceId);
        XsSyncSettingArray result;
        for (XsSyncSetting const& setting : settings)
                result.push_back(setting);
 
        return result;
}
 
bool XsDevice::supportsSyncSettings(XsDeviceId const& deviceId)
{
        return Synchronization::supportsSyncSettings(deviceId);
}
 
bool XsDevice::isCompatibleSyncSetting(XsDeviceId const& deviceId, XsSyncSetting const& setting1, XsSyncSetting const& setting2)
{
        return Synchronization::isCompatibleSyncSetting(deviceId, setting1, setting2);
}
 
unsigned int XsDevice::syncSettingsTimeResolutionInMicroSeconds(XsDeviceId const& deviceId)
{
        return Synchronization::timeResolutionInMicroseconds(deviceId);
}
 
int XsDevice::childCount() const
{
        return 0;
}
 
std::vector<XsDevice*> XsDevice::children() const
{
        return std::vector<XsDevice*>();
}
 
void XsDevice::setCommunicator(Communicator* comm)
{
        if (m_communicator)
                m_communicator->destroy();
        m_communicator = comm;
}
 
void XsDevice::setRecordingStartFrame(uint16_t startFrame)
{
        LockGuarded lock(&m_deviceMutex);
        m_stopRecordingPacketId = -1;
        m_startRecordingPacketId = PacketStamper::calculateLargePacketCounter(startFrame, latestLivePacketId(), PacketStamper::MTSCBOUNDARY);
        JLDEBUGG(this << " " << deviceId() << " m_startRecordingPacketId = " << m_startRecordingPacketId << " m_stopRecordingPacketId = " << m_stopRecordingPacketId << " m_stoppedRecordingPacketId = " << m_stoppedRecordingPacketId);
}
 
void XsDevice::setRecordingStopFrame(uint16_t stopFrame)
{
        LockGuarded lock(&m_deviceMutex);
        JLDEBUGG(this << " " << deviceId() << " Setting recording stop frame to " << stopFrame << " last known frame is " << latestBufferedPacketId() << " (" << (latestBufferedPacketId() & 0xffff) << ")");
        m_stopRecordingPacketId = PacketStamper::calculateLargePacketCounter(stopFrame, latestBufferedPacketId(), PacketStamper::MTSCBOUNDARY);
        m_stoppedRecordingPacketId = m_stopRecordingPacketId;
        JLDEBUGG(this << " " << deviceId() << " m_startRecordingPacketId = " << m_startRecordingPacketId << " m_stopRecordingPacketId = " << m_stopRecordingPacketId << " m_stoppedRecordingPacketId = " << m_stoppedRecordingPacketId);
}
 
void XsDevice::clearExternalPacketCaches()
{
        LockGuarded lock(&m_deviceMutex);
        for (auto item : m_linearPacketCache)
                delete item;
        m_linearPacketCache.clear();
        m_lastAvailableLiveDataCache->clear();
}
 
void XsDevice::clearCacheToRecordingStart()
{
        LockGuarded lockG(&m_deviceMutex);
        for (auto item : m_dataCache)
        {
                if (item.first < m_startRecordingPacketId)
                {
                        delete item.second;
                        m_dataCache.erase(item.first);
                }
        }
}
 
void XsDevice::updateLastAvailableLiveDataCache(XsDataPacket const& pack)
{
        LockGuarded lockG(&m_deviceMutex);
        m_lastAvailableLiveDataCache->merge(pack, true);
}
 
void XsDevice::retainPacket(XsDataPacket const& pack)
{
        LockGuarded lockG(&m_deviceMutex);
        if (!m_linearPacketCache.empty() && m_linearPacketCache.back()->packetId() == pack.packetId())
                m_linearPacketCache.back()->merge(pack, true);
        else
                m_linearPacketCache.push_back(new XsDataPacket(pack));
}
 
bool XsDevice::resetRemovesPort() const
{
        return true;
}
XsDataPacket XsDevice::getDataPacketByIndex(XsSize index) const
{
        LockGuarded lockG(&m_deviceMutex);
        if (index < m_linearPacketCache.size())
                return *m_linearPacketCache.at(index);
        return XsDataPacket();
}
 
XsSize XsDevice::getDataPacketCount() const
{
        LockGuarded lockG(&m_deviceMutex);
        return m_linearPacketCache.size();
}
 
XsDataPacket XsDevice::lastAvailableLiveData() const
{
        LockGuarded lockG(&m_deviceMutex);
        return *m_lastAvailableLiveDataCache;
}
 
XsDataPacket XsDevice::takeFirstDataPacketInQueue()
{
        LockGuarded lockG(&m_deviceMutex);
        if (m_linearPacketCache.empty())
                return XsDataPacket();
 
        std::unique_ptr<XsDataPacket> rv(m_linearPacketCache.at(0));
        m_linearPacketCache.pop_front();
        return *rv;     // copy and delete, hopefully optimizing the copy into a move operation
}
 
void XsDevice::setStartRecordingPacketId(int64_t startFrame)
{
        LockGuarded lockG(&m_deviceMutex);
        m_startRecordingPacketId = startFrame;
        ONLYFIRSTMTX2
        JLDEBUGG(this << " " << deviceId() << " m_startRecordingPacketId = " << m_startRecordingPacketId << " m_stopRecordingPacketId = " << m_stopRecordingPacketId << " m_stoppedRecordingPacketId = " << m_stoppedRecordingPacketId);
}
 
void XsDevice::setStopRecordingPacketId(int64_t stopFrame)
{
        LockGuarded lockG(&m_deviceMutex);
        m_stopRecordingPacketId = stopFrame;
        m_stoppedRecordingPacketId = m_stopRecordingPacketId;
        ONLYFIRSTMTX2
        JLDEBUGG(this << " " << deviceId() << " m_startRecordingPacketId = " << m_startRecordingPacketId << " m_stopRecordingPacketId = " << m_stopRecordingPacketId << " m_stoppedRecordingPacketId = " << m_stoppedRecordingPacketId);
}
 
void XsDevice::setSkipEmtsReadOnInit(bool skip)
{
        m_skipEmtsReadOnInit = skip;
}
 
bool XsDevice::readEmtsAndDeviceConfiguration()
{
        if (!gotoConfig())
                return false;
 
        XsMessage snd(XMID_ReqEmts, 2);
        snd.setBusId(busId());
        snd.setDataByte(0, 0);
        snd.setDataByte(255, 1);
 
        XsMessage rcv;
        if (!doTransaction(snd, rcv, 10000))
                return false;
 
        m_emtsBlob = rcv;
 
        snd.setMessageId(XMID_ReqConfiguration);
        snd.setBusId(busId());
 
        rcv.clear();
        if (!doTransaction(snd, rcv, 5000))
                return false;
 
        m_config.readFromMessage(rcv);
 
        return true;
}
 
int64_t XsDevice::deviceRecordingBufferItemCount(int64_t& lastCompletePacketId) const
{
        lastCompletePacketId = latestLivePacketId();
        return 0;
}
uint32_t XsDevice::deviceBufferSize()
{
        return 0;
}
 
bool XsDevice::setDeviceBufferSize(uint32_t frames)
{
        (void)frames;
        return true;
}
 
void XsDevice::waitForAllDevicesInitialized()
{
        while (!isInitialized())
                XsTime::msleep(0);
}
 
bool XsDevice::isLoadLogFileInProgress() const
{
        Communicator const* comm = communicator();
        if (!comm)
                return false;
 
        if (!comm->isReadingFromFile())
                return false;
 
        return comm->isLoadLogFileInProgress();
}
 
void XsDevice::waitForLoadLogFileDone() const
{
        while (isLoadLogFileInProgress() && !m_terminationPrepared)
                XsTime::msleep(10);
}
 
int64_t XsDevice::getStartRecordingPacketId() const
{
        return m_startRecordingPacketId;
}
 
int64_t XsDevice::getStopRecordingPacketId() const
{
        return m_stopRecordingPacketId == -1 ? m_stoppedRecordingPacketId : m_stopRecordingPacketId;
}
 
XsResultValue XsDevice::setDeviceParameter(XsDeviceParameter const& parameter)
{
        (void)parameter;
        return XRV_UNSUPPORTED;
}
 
XsResultValue XsDevice::deviceParameter(XsDeviceParameter& parameter) const
{
        (void)parameter;
        return XRV_UNSUPPORTED;
}
 
XsGnssPlatform XsDevice::gnssPlatform() const
{
        return static_cast<XsGnssPlatform>(ubloxGnssPlatform());
}
 
bool XsDevice::setGnssPlatform(XsGnssPlatform gnssPlatform)
{
        return setUbloxGnssPlatform(static_cast<XsUbloxGnssPlatform>(gnssPlatform));
}
XsUbloxGnssPlatform XsDevice::ubloxGnssPlatform() const
{
        return XGP_Portable;
}
 
bool XsDevice::setUbloxGnssPlatform(XsUbloxGnssPlatform ubloxGnssPlatform)
{
        (void) ubloxGnssPlatform;
        return false;
}
 
XsIntArray XsDevice::gnssReceiverSettings() const
{
        return XsIntArray();
}
 
bool XsDevice::setGnssReceiverSettings(const XsIntArray& gnssReceiverSettings)
{
        (void)gnssReceiverSettings;
        return false;
}
 
void XsDevice::reinitializeProcessors()
{
        // intentionally empty, private implementation
}
 
bool XsDevice::expectingRetransmissionForPacket(int64_t packetId) const
{
        if (isMasterDevice())
                return false;
 
        // child devices use their parent's decision
        return master()->expectingRetransmissionForPacket(packetId);
}
 
bool XsDevice::initializeSoftwareCalibration()
{
        // intentionally empty, private implementation
        return false;
}
 
void XsDevice::deinitializeSoftwareCalibration()
{
        // intentionally empty, private implementation
}
 
XsDevice const* XsDevice::firstChild() const
{
        return this;
}
 
bool XsDevice::interpolateMissingData(XsDataPacket const& pack, XsDataPacket const& prev, std::function<void (XsDataPacket*)> packetHandler)
{
        (void) pack;
        (void) prev;
        (void) packetHandler;
        return false;
}
void XsDevice::discardRetransmissions(int64_t firstNewPacketId)
{
        (void) firstNewPacketId;
}
 
uint32_t XsDevice::supportedStatusFlags() const
{
        // The basic device supports nothing
        return (uint32_t) 0;
}
 
XsResultValue XsDevice::updatePortInfo(XsPortInfo const& newInfo)
{
        (void) newInfo;
        return XRV_NOTIMPLEMENTED;
}
 
XsDevice* XsDevice::subDevice(int subDeviceId) const
{
        if (subDeviceId)
                return nullptr;
        return const_cast<XsDevice*>(this);
}
 
int XsDevice::subDeviceCount() const
{
        return 0;
}
 
XsByteArray XsDevice::readMetaDataFromLogFile()
{
        return XsByteArray();
}