cmt3.cpp

00001 /*! \file Cmt3.cpp
00002 
00003         For information about objects in this file, see the appropriate header:
00004         \ref Cmt3.h
00005 
00006         \section FileCopyright Copyright Notice
00007         Copyright (C) Xsens Technologies B.V., 2006.  All rights reserved.
00008 
00009         This source code is intended for use only by Xsens Technologies BV and
00010         those that have explicit written permission to use it from
00011         Xsens Technologies BV.
00012 
00013         THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY
00014         KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
00015         IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A
00016         PARTICULAR PURPOSE.
00017 
00018         \section FileChangelog        Changelog
00019         \par 2006-04-28, v0.0.1
00020         \li Job Mulder:        Created
00021         \par 2006-07-21, v0.1.0
00022         \li Job Mulder:        Updated file for release 0.1.0
00023 */
00024 
00025 #include "cmt3.h"
00026 #include <math.h>
00027 #include "xsens_janitors.h"
00028 
00029 #ifdef _LOG_CMT3
00030 #        define CMT3LOG                CMTLOG
00031 #        define CMT3EXITLOG        JanitorLogFunc<XsensResultValue,uint32_t> _cmtExitLog(CMTLOG,"L3: " __FUNCTION__ " returns %u\n",m_lastResult);
00032 #else
00033 #        define CMT3LOG(...)
00034 #        define CMT3EXITLOG
00035 #endif
00036 
00037 #ifdef _LOG_CMT3_DATA
00038 #        define CMT3LOGDAT                CMTLOG
00039 #        define CMT3EXITLOGDAT        JanitorLogFunc<XsensResultValue,uint32_t> _cmtExitLog(CMTLOG,"L3: " __FUNCTION__ " returns %u\n",m_lastResult);
00040 #else
00041 #        define CMT3LOGDAT(...)
00042 #        define CMT3EXITLOGDAT
00043 #endif
00044 
00045 #define CMT3F_DEVINFO_SIZE                (sizeof(CmtDeviceId) + sizeof(CmtDataFormat))
00046 
00047 //////////////////////////////////////////////////////////////////////////////////////////
00048 //////////////////////////////////// Support  classes ////////////////////////////////////
00049 //////////////////////////////////////////////////////////////////////////////////////////
00050 
00051 void CmtDeviceConfiguration::readFromMessage(const void* message)
00052 {
00053         xsens::Message msg((const uint8_t*) message,0);
00054 
00055         m_masterDeviceId = msg.getDataLong(0);
00056         m_samplingPeriod = msg.getDataShort(4);
00057         m_outputSkipFactor = msg.getDataShort(6);
00058         m_syncinMode = msg.getDataShort(8);
00059         m_syncinSkipFactor = msg.getDataShort(10);
00060         m_syncinOffset = msg.getDataLong(12);
00061         memcpy(m_date,msg.getDataBuffer(16),8);
00062         memcpy(m_time,msg.getDataBuffer(24),8);
00063         memcpy(m_reservedForHost,msg.getDataBuffer(32),32);
00064         memcpy(m_reservedForClient,msg.getDataBuffer(64),32);
00065         m_numberOfDevices = msg.getDataShort(96);
00066 
00067         for (uint16_t i = 0; i < m_numberOfDevices && i < CMT_MAX_DEVICES_PER_PORT; ++i)
00068         {
00069                 m_deviceInfo[i].m_deviceId = msg.getDataLong(98+i*20);
00070                 m_deviceInfo[i].m_dataLength = msg.getDataShort(102+i*20);
00071                 m_deviceInfo[i].m_outputMode = msg.getDataShort(104+i*20);
00072                 m_deviceInfo[i].m_outputSettings = msg.getDataLong(106+i*20);
00073                 m_deviceInfo[i].m_currentScenario = msg.getDataShort(110+i*20);
00074                 m_deviceInfo[i].m_fwRevMajor = msg.getDataByte(112+i*20);
00075                 m_deviceInfo[i].m_fwRevMinor = msg.getDataByte(113+i*20);
00076                 m_deviceInfo[i].m_fwRevRevision = msg.getDataByte(114+i*20);
00077                 m_deviceInfo[i].m_filterType = msg.getDataByte(115+i*20);
00078                 m_deviceInfo[i].m_filterMajor = msg.getDataByte(116+i*20);
00079                 m_deviceInfo[i].m_filterMinor = msg.getDataByte(117+i*20);
00080         }
00081 }
00082 
00083 //////////////////////////////////////////////////////////////////////////////////////////
00084 // Compute the period and skip factor.
00085 void CmtDeviceMode::getPeriodAndSkipFactor(uint16_t& period,uint16_t& skip) const
00086 {
00087         if (m_sampleFrequency == 0)
00088         {
00089                 period = 0;
00090                 skip = 0;
00091                 return;
00092         }
00093         if (m_sampleFrequency >= 512)
00094         {
00095                 period = 225;
00096                 skip = 0;
00097                 return;
00098         }
00099 
00100         int32_t freq, sf = m_sampleFrequency;
00101 
00102         // first try the simple ones
00103         skip = 0;
00104         freq = sf;
00105         if (120 % freq == 0 && freq < 120) {
00106                 freq = 120;
00107         } else if (100 % freq == 0 && freq < 100) {
00108                 freq = 100;
00109         }
00110         while (freq < 100)
00111         {
00112                 ++skip;
00113                 freq += sf;
00114         }
00115 
00116         period = (uint16_t) (115200 / freq);
00117 }
00118 
00119 //////////////////////////////////////////////////////////////////////////////////////////
00120 // Return the real sample frequency in Hz
00121 double CmtDeviceMode::getRealSampleFrequency(void) const
00122 {
00123         uint16_t skip, period;
00124         getPeriodAndSkipFactor(period,skip);
00125         return (115200.0 / ((1.0 + (double) skip) * (double) period));
00126 }
00127 
00128 //////////////////////////////////////////////////////////////////////////////////////////
00129 // Compute sample frequency from the period and skip factor.
00130 void CmtDeviceMode::setPeriodAndSkipFactor(uint16_t period, uint16_t skip)
00131 {
00132         m_sampleFrequency = (uint16_t) floor((115200.0 / ((1.0 + (double) skip) * (double) period))+0.5);
00133 }
00134 
00135 //////////////////////////////////////////////////////////////////////////////////////////
00136 // Compute sample frequency from the period and skip factor.
00137 bool CmtDeviceMode::operator == (const CmtDeviceMode& dev) const
00138 {
00139         return m_outputMode == dev.m_outputMode && m_outputSettings == dev.m_outputSettings && m_sampleFrequency == dev.m_sampleFrequency;
00140 }
00141 
00142 //////////////////////////////////////////////////////////////////////////////////////////
00143 // Return the real sample frequency in Hz
00144 double CmtDeviceMode2::getRealSampleFrequency(void) const
00145 {
00146         if (m_skip != 0xFFFF)
00147                 return (115200.0 / ((1.0 + (double) m_skip) * (double) m_period));
00148         else
00149                 return (115200.0 / ((double) m_period));
00150 }
00151 
00152 //////////////////////////////////////////////////////////////////////////////////////////
00153 // Return the sample frequency in Hz
00154 uint16_t CmtDeviceMode2::getSampleFrequency(void) const
00155 {
00156         if (m_skip != 0xFFFF)
00157                 return (uint16_t) floor((115200.0 / ((1.0 + (double) m_skip) * (double) m_period))+0.5);
00158         else
00159                 return (uint16_t) floor((115200.0 / ((double) m_period))+0.5);
00160 }
00161 
00162 void CmtDeviceMode2::setSampleFrequency(uint16_t frequency)
00163 {
00164         if (frequency == 0)
00165         {
00166                 m_period = 0;
00167                 m_skip = 0;
00168                 return;
00169         }
00170         if (frequency >= 512)
00171         {
00172                 m_period = 225;
00173                 m_skip = 0;
00174                 return;
00175         }
00176 
00177         int32_t freq, sf = frequency;
00178 
00179         // first try the simple ones
00180         m_skip = 0;
00181         freq = sf;
00182         while (freq < 100)
00183         {
00184                 ++m_skip;
00185                 freq += sf;
00186         }
00187 
00188         m_period = (uint16_t) (115200 / freq);
00189 }
00190 
00191 //////////////////////////////////////////////////////////////////////////////////////////
00192 // Compute sample frequency from the period and skip factor.
00193 bool CmtDeviceMode2::operator == (const CmtDeviceMode2& dev) const
00194 {
00195         return m_outputMode == dev.m_outputMode && m_outputSettings == dev.m_outputSettings && m_period == dev.m_period && m_skip == dev.m_skip;
00196 }
00197 
00198 //////////////////////////////////////////////////////////////////////////////////////////
00199 // Common part of a data request function
00200 #define DO_DATA_REQUEST_BID(req,bid)\
00201         Message snd(req,0);\
00202         Message rcv;\
00203         if (bid == CMT_BID_INVALID || bid == CMT_BID_BROADCAST)\
00204                 return (m_lastResult = XRV_INVALIDID);\
00205         if (!m_readFromFile)\
00206         {\
00207                 snd.setBusId(bid);\
00208                 m_serial.writeMessage(&snd);\
00209                 if ((m_lastResult = m_serial.waitForMessage(&rcv,req+1,0,true)) != XRV_OK)\
00210                         return m_lastResult;\
00211                 if (m_logging)\
00212                         m_logFile.writeMessage(&rcv);\
00213                 if (rcv.getMessageId() == CMT_MID_ERROR)\
00214                 {        m_lastHwErrorDeviceId = m_config.m_masterDeviceId;\
00215                         if (rcv.getDataSize() >= 2)\
00216                         {\
00217                                 uint8_t biddy = rcv.getDataByte(1);\
00218                                 getDeviceId(biddy,m_lastHwErrorDeviceId);\
00219                         }\
00220                         return m_lastResult = m_lastHwError = (XsensResultValue) rcv.getDataByte();\
00221                 }\
00222         }\
00223         else\
00224         while (1)\
00225         {\
00226                 if ((m_lastResult = m_logFile.readMessage(&rcv,req+1)) != XRV_OK)\
00227                         return m_lastResult;\
00228                 if (rcv.getBusId() == bid || (rcv.getBusId() == 1 && bid == CMT_BID_MASTER))\
00229                         break;\
00230         }
00231 
00232 #define DO_DATA_REQUEST(req)\
00233         uint8_t bid = getBusIdInternal(deviceId);\
00234         DO_DATA_REQUEST_BID(req,bid);
00235 
00236 //////////////////////////////////////////////////////////////////////////////////////////
00237 // Common part of a serial data set function
00238 #define DO_DATA_SET_BID(req,size,type,data,bid)\
00239         Message        snd(req,size);\
00240         Message rcv;\
00241         snd.setData ## type (data);\
00242         if (bid == CMT_BID_BROADCAST)\
00243         {\
00244                 for (uint8_t i=0;i<m_config.m_numberOfDevices;++i)\
00245                 {\
00246                         snd.setBusId(i+1);\
00247                         m_serial.writeMessage(&snd);\
00248                         m_lastResult = m_serial.waitForMessage(&rcv,req+1,0,true);\
00249                         if (m_lastResult != XRV_OK)\
00250                                 return m_lastResult;\
00251                         if (m_logging)\
00252                                 m_logFile.writeMessage(&rcv);\
00253                         if (rcv.getMessageId() == CMT_MID_ERROR)\
00254                         {        m_lastHwErrorDeviceId = m_config.m_masterDeviceId;\
00255                                 if (rcv.getDataSize() >= 2)\
00256                                 {\
00257                                         uint8_t biddy = rcv.getDataByte(1);\
00258                                         getDeviceId(biddy,m_lastHwErrorDeviceId);\
00259                                 }\
00260                                 return m_lastResult = m_lastHwError = (XsensResultValue) rcv.getDataByte();\
00261                         }\
00262                 }\
00263         } else {\
00264                 snd.setBusId(bid);\
00265                 m_serial.writeMessage(&snd);\
00266                 m_lastResult = m_serial.waitForMessage(&rcv,req+1,0,true);\
00267                 if (m_lastResult != XRV_OK)\
00268                         return m_lastResult;\
00269                 if (m_logging)\
00270                         m_logFile.writeMessage(&rcv);\
00271                 if (rcv.getMessageId() == CMT_MID_ERROR)\
00272                 {        m_lastHwErrorDeviceId = m_config.m_masterDeviceId;\
00273                         if (rcv.getDataSize() >= 2)\
00274                         {\
00275                                 uint8_t biddy = rcv.getDataByte(1);\
00276                                 getDeviceId(biddy,m_lastHwErrorDeviceId);\
00277                         }\
00278                         return m_lastResult = m_lastHwError = (XsensResultValue) rcv.getDataByte();\
00279                 }\
00280         }
00281 
00282 #define DO_DATA_SET(req,size,type,data)\
00283         uint8_t bid = getBusIdInternal(deviceId);\
00284         if (bid == CMT_BID_INVALID)\
00285                 return (m_lastResult = XRV_INVALIDID);\
00286         DO_DATA_SET_BID(req,size,type,data,bid);
00287 
00288 #define HANDLE_ERR_RESULT\
00289         if (rcv.getMessageId() == CMT_MID_ERROR)\
00290         {\
00291                 m_lastHwErrorDeviceId = m_config.m_masterDeviceId;\
00292                 if (rcv.getDataSize() >= 2)\
00293                 {\
00294                         uint8_t biddy = rcv.getDataByte(1);\
00295                         getDeviceId(biddy,m_lastHwErrorDeviceId);\
00296                 }\
00297                 return m_lastResult = m_lastHwError = (XsensResultValue) rcv.getDataByte(0);\
00298         }
00299 
00300 
00301 //////////////////////////////////////////////////////////////////////////////////////////
00302 ///////////////////////////////////////// Cmt3  /////////////////////////////////////////
00303 //////////////////////////////////////////////////////////////////////////////////////////
00304 
00305 namespace xsens {
00306 
00307 //////////////////////////////////////////////////////////////////////////////////////////
00308 // Default constructor, initializes all members to their default values.
00309 Cmt3::Cmt3()
00310 {
00311         m_lastResult = XRV_OK;
00312         m_rtcInitialized = false;
00313         m_useRtc = true;
00314         m_measuring = false;
00315         m_timeoutConf = CMT3_DEFAULT_TIMEOUT_CONF;
00316         m_timeoutMeas = CMT3_DEFAULT_TIMEOUT_MEAS;
00317         m_gotoConfigTries = CMT_GOTO_CONFIG_TRIES;
00318         m_readFromFile = false;
00319         //m_sampleFrequency = 100.0;
00320         m_period = CMT_DEFAULT_PERIOD;
00321         m_skip = CMT_DEFAULT_SKIP;
00322         memset(m_eMtsData,0,sizeof(m_eMtsData));
00323         memset(&m_config,0,sizeof(m_config));
00324         m_logging = false;
00325 
00326         clearHwError();
00327 }
00328 
00329 //////////////////////////////////////////////////////////////////////////////////////////
00330 // Destructor, de-initializes, frees memory allocated for buffers, etc.
00331 Cmt3::~Cmt3()
00332 {
00333         m_serial.close();
00334         m_logFile.close();
00335         for (uint32_t i = 0; i < CMT_MAX_DEVICES_PER_PORT; ++i)
00336         {
00337                 CHKFREENUL(m_eMtsData[i]);
00338         }
00339 }
00340 
00341 //////////////////////////////////////////////////////////////////////////////////////////
00342 // Close the serial communication port.
00343 XsensResultValue Cmt3::closePort(bool gotoConfigFirst)
00344 {
00345         CMT3LOG(__FUNCTION__ " closePort\n");
00346         CMT3EXITLOG;
00347 
00348         if (m_measuring && gotoConfigFirst)
00349                 gotoConfig();
00350         CMT3LOG(__FUNCTION__ " Closing L2 port\n");
00351         m_serial.close();
00352         m_measuring = false;
00353         if (m_logFile.isOpen())
00354         {
00355                 m_readFromFile = true;
00356                 m_logging = false;
00357         }
00358         return m_lastResult = XRV_OK;
00359 }
00360 
00361 //////////////////////////////////////////////////////////////////////////////////////////
00362 void Cmt3::fillRtc(Packet* pack)
00363 {
00364 #if defined(_DEBUG) || defined(_LOG_ALWAYS)
00365         TimeStamp rt = 0;
00366 #endif
00367 
00368         if (!m_rtcInitialized)
00369         {
00370                 m_rtcStart = pack->m_toa;
00371                 m_rtcLastSc = pack->getSampleCounter();
00372                 m_rtcCount = m_rtcLastSc;
00373 #ifdef _CORRECT_FOR_CLOCK_MISMATCH
00374                 m_lastToaTouch = m_rtcLastSc;
00375 #endif
00376                 CmtDeviceMode2 mode;
00377                 mode.m_period = m_period;
00378                 mode.m_skip = m_skip;
00379                 m_rtcMsPerSample = 1000.0 / mode.getRealSampleFrequency();
00380                 pack->m_rtc = m_rtcStart;
00381                 m_rtcInitialized = true;
00382         }
00383         else
00384         {
00385                 CmtMtTimeStamp sc = pack->getSampleCounter();
00386                 CmtMtTimeStamp scdiff = sc - m_rtcLastSc;
00387                 m_rtcLastSc = sc;
00388                 m_rtcCount += scdiff;
00389                 pack->m_rtc = m_rtcStart + (TimeStamp) floor(m_rtcMsPerSample * (double) m_rtcCount + 0.5);
00390 #ifdef _CORRECT_FOR_CLOCK_MISMATCH
00391 #if defined(_DEBUG) || defined(_LOG_ALWAYS)
00392                 rt = pack->m_rtc;
00393 #endif
00394                 // detect if arrival times are lower than expected
00395                 // if so, we update the start time to correct for this
00396                 if (pack->m_toa < pack->m_rtc)
00397                 {
00398                         m_lastToaTouch = m_rtcCount;
00399                         // adjust start time to match new, earlier TOA
00400                         TimeStamp diff = pack->m_rtc - pack->m_toa;
00401 
00402                         m_rtcStart -= diff;
00403                         pack->m_rtc = pack->m_toa;
00404                 }
00405                 else if (pack->m_toa == pack->m_rtc)
00406                         m_lastToaTouch = m_rtcCount;
00407                 else if (((m_rtcCount-m_lastToaTouch) & 127) == 0)
00408                         ++m_rtcStart;        // we adjust the start time upwards by max 1ms per 128 samples
00409 #endif
00410         }
00411         //CMT3LOG(__FUNCTION__ " rtcStart: %I64u rtcPack: %I64u toa: %I64u\n",m_rtcStart,pack->m_rtc,pack->m_toa);
00412 //        CMT3LOG("%I64u %I64u %I64u %I64u 101010101 %u\n",m_rtcStart,rt,pack->m_toa,pack->m_rtc,(long) this);
00413 }
00414 
00415 #if 0
00416 obsolete:
00417 //////////////////////////////////////////////////////////////////////////////////////////
00418 // Get the state (enabled/disabled) of the AMD algorithm
00419 XsensResultValue Cmt3::getAmdState(bool& state, const CmtDeviceId deviceId)
00420 {
00421         CMT3LOG("L3: getAmdState %08x\n",deviceId);
00422         CMT3EXITLOG;
00423 
00424         DO_DATA_REQUEST(CMT_MID_REQAMD);
00425         state = (rcv.getDataShort() != 0);
00426         return m_lastResult = XRV_OK;
00427 }
00428 #endif
00429 
00430 //////////////////////////////////////////////////////////////////////////////////////////
00431 // Get the XM batery level
00432 XsensResultValue Cmt3::getBatteryLevel(uint8_t& level)
00433 {
00434         CMT3LOG(__FUNCTION__ "\n");
00435         CMT3EXITLOG;
00436 
00437         DO_DATA_REQUEST_BID(CMT_MID_REQBATLEVEL,CMT_BID_MASTER);
00438         level = rcv.getDataByte();
00439         return m_lastResult = XRV_OK;
00440 }
00441 
00442 //////////////////////////////////////////////////////////////////////////////////////////
00443 // Get the baudrate that is currently being used by the port
00444 XsensResultValue Cmt3::getBaudrate(uint32_t& baudrate)
00445 {
00446         CMT3LOG(__FUNCTION__ "\n");
00447         CMT3EXITLOG;
00448 
00449         if (!m_serial.isOpen())
00450                 return m_lastResult = XRV_NOPORTOPEN;
00451         baudrate = m_baudrate;
00452         return m_lastResult = XRV_OK;
00453 }
00454 
00455 //////////////////////////////////////////////////////////////////////////////////////////
00456 // Get the state of the bluetooth communication, on (true) or off (false)
00457 XsensResultValue Cmt3::getBluetoothState(bool& enabled)
00458 {
00459         CMT3LOG(__FUNCTION__ "\n");
00460         CMT3EXITLOG;
00461 
00462         DO_DATA_REQUEST_BID(CMT_MID_REQBTDISABLE,CMT_BID_MASTER);
00463         enabled = (rcv.getDataByte() == 0);        // invert the result as it is a disable field
00464         return m_lastResult = XRV_OK;
00465 }
00466 
00467 //////////////////////////////////////////////////////////////////////////////////////////
00468 // Retrieve the BusId of a device.
00469 XsensResultValue Cmt3::getBusId (uint8_t& busId, const CmtDeviceId deviceId) const
00470 {
00471         CMT3LOG(__FUNCTION__ " %08x\n",deviceId);
00472         CMT3EXITLOG;
00473 
00474         if (deviceId == CMT_DID_MASTER || deviceId == m_config.m_masterDeviceId)
00475         {
00476                 busId = CMT_BID_MASTER;
00477                 return m_lastResult = XRV_OK;
00478         }
00479 
00480         for (uint16_t i=0 ; i <= m_config.m_numberOfDevices ; ++i)
00481                 if (m_config.m_deviceInfo[i].m_deviceId == deviceId)
00482                 {
00483                         busId = i+1;
00484                         return m_lastResult = XRV_OK;
00485                 }
00486         return m_lastResult = XRV_NOTFOUND;
00487 }
00488 
00489 //////////////////////////////////////////////////////////////////////////////////////////
00490 uint8_t Cmt3::getBusIdInternal(const CmtDeviceId devId) const
00491 {
00492         if (devId == CMT_DID_MASTER)
00493                 return CMT_BID_MASTER;
00494 
00495         if (devId == CMT_DID_BROADCAST)
00496                 return CMT_BID_BROADCAST;
00497 
00498         if (m_config.m_masterDeviceId == devId)
00499                 return CMT_BID_MASTER;
00500 
00501         for (uint16_t i = 0;i <= m_config.m_numberOfDevices;++i)
00502         {
00503                 if (m_config.m_deviceInfo[i].m_deviceId == devId)
00504                         return (uint8_t) (i+1);
00505         }
00506         return CMT_BID_INVALID;
00507 }
00508 
00509 //////////////////////////////////////////////////////////////////////////////////////////
00510 // Retrieve the XM bus power state.
00511 XsensResultValue Cmt3::getBusPowerState(bool& enabled)
00512 {
00513         CMT3LOG(__FUNCTION__ "\n");
00514         CMT3EXITLOG;
00515 
00516         DO_DATA_REQUEST_BID(CMT_MID_BUSPWR,CMT_BID_MASTER);
00517         enabled = (rcv.getDataShort() != 0);        // invert the result as it is a disable field
00518         return m_lastResult = XRV_OK;
00519 }
00520 
00521 //////////////////////////////////////////////////////////////////////////////////////////
00522 Cmt2f* Cmt3::getCmt2f(void)
00523 {
00524         return &m_logFile;
00525 }
00526 
00527 //////////////////////////////////////////////////////////////////////////////////////////
00528 Cmt2s* Cmt3::getCmt2s(void)
00529 {
00530         return &m_serial;
00531 }
00532 
00533 //////////////////////////////////////////////////////////////////////////////////////////
00534 // Retrieve the complete device configuration of a device.
00535 XsensResultValue Cmt3::getConfiguration(CmtDeviceConfiguration& configuration)
00536 {
00537         CMT3LOG(__FUNCTION__ "\n");
00538         CMT3EXITLOG;
00539 
00540         if (!(m_serial.isOpen() || m_logFile.isOpen()))
00541                 return m_lastResult = XRV_INVALIDOPERATION;
00542 
00543         if (m_config.m_numberOfDevices > CMT_MAX_DEVICES_PER_PORT)
00544         return m_lastResult = XRV_CONFIGCHECKFAIL;
00545 
00546         memcpy(&configuration,&m_config,sizeof(CmtDeviceConfiguration));
00547 
00548         if (m_logging)
00549         {
00550                 // fake the message receipt
00551                 Message msg(CMT_MID_CONFIGURATION,98 + CMT_CONF_BLOCKLEN*m_config.m_numberOfDevices);
00552                 msg.setBusId(CMT_BID_MASTER);
00553 
00554                 msg.setDataLong(m_config.m_masterDeviceId, 0);
00555                 msg.setDataShort(m_config.m_samplingPeriod, 4);
00556                 msg.setDataShort(m_config.m_outputSkipFactor, 6);
00557                 msg.setDataShort(m_config.m_syncinMode, 8);
00558                 msg.setDataShort(m_config.m_syncinSkipFactor, 10);
00559                 msg.setDataLong(m_config.m_syncinOffset, 12);
00560                 memcpy(msg.getDataBuffer(16), m_config.m_date,8);
00561                 memcpy(msg.getDataBuffer(24), m_config.m_time, 8);
00562                 memcpy(msg.getDataBuffer(32), m_config.m_reservedForHost,32);
00563                 memcpy(msg.getDataBuffer(64), m_config.m_reservedForClient,32);
00564                 msg.setDataShort(m_config.m_numberOfDevices, 96);
00565 
00566                 for (uint16_t i = 0; i < m_config.m_numberOfDevices; ++i)
00567                 {
00568                         msg.setDataLong(m_config.m_deviceInfo[i].m_deviceId, 98+i*20);
00569                         msg.setDataShort(m_config.m_deviceInfo[i].m_dataLength, 102+i*20);
00570                         msg.setDataShort(m_config.m_deviceInfo[i].m_outputMode, 104+i*20);
00571                         msg.setDataLong(m_config.m_deviceInfo[i].m_outputSettings, 106+i*20);
00572                         msg.setDataShort(m_config.m_deviceInfo[i].m_currentScenario, 110+i*20);
00573                         msg.setDataByte(m_config.m_deviceInfo[i].m_fwRevMajor, 112+i*20);
00574                         msg.setDataByte(m_config.m_deviceInfo[i].m_fwRevMinor, 113+i*20);
00575                         msg.setDataByte(m_config.m_deviceInfo[i].m_fwRevRevision, 114+i*20);
00576                         msg.setDataByte(m_config.m_deviceInfo[i].m_filterType, 115+i*20);
00577                         msg.setDataByte(m_config.m_deviceInfo[i].m_filterMajor, 116+i*20);
00578                         msg.setDataByte(m_config.m_deviceInfo[i].m_filterMinor, 117+i*20);
00579                 }
00580 
00581                 msg.recomputeChecksum();
00582 
00583                 writeMessageToLogFile(msg);
00584         }
00585         return m_lastResult = XRV_OK;
00586 }
00587 
00588 //////////////////////////////////////////////////////////////////////////////////////////
00589 // Retrieve the number of bytes that are in a data message as sent by the device.
00590 XsensResultValue Cmt3::getDataLength(uint32_t& length, const CmtDeviceId deviceId)
00591 {
00592         CMT3LOG(__FUNCTION__ " %08x\n",deviceId);
00593         CMT3EXITLOG;
00594 
00595         DO_DATA_REQUEST(CMT_MID_REQDATALENGTH);
00596         length = rcv.getDataShort();        // the sensor returns this value as a int16_t
00597         return m_lastResult = XRV_OK;
00598 }
00599 
00600 //////////////////////////////////////////////////////////////////////////////////////////
00601 // Return the number of connected devices. Returns 0 if not connected.
00602 uint32_t Cmt3::getDeviceCount (void) const
00603 {
00604         CMT3LOG(__FUNCTION__ "\n");
00605         if (m_serial.isOpen() || m_logFile.isOpen())
00606         {
00607                 if (isXm())
00608                         return m_config.m_numberOfDevices + 1;
00609                 else
00610                         return m_config.m_numberOfDevices;
00611         }
00612         return 0;
00613 }
00614 
00615 //////////////////////////////////////////////////////////////////////////////////////////
00616 // Retrieve the CmtDeviceId of the device at the given BusId
00617 XsensResultValue Cmt3::getDeviceId(const uint8_t busId, CmtDeviceId& deviceId) const
00618 {
00619         CMT3LOG(__FUNCTION__ " %u\n",(uint32_t) busId);
00620         CMT3EXITLOG;
00621 
00622         if (busId == CMT_BID_MASTER || busId == 0)
00623                 deviceId = m_config.m_masterDeviceId;
00624         else
00625         {
00626                 if (busId > m_config.m_numberOfDevices)
00627                         return m_lastResult = XRV_INVALIDID;
00628                 deviceId = m_config.m_deviceInfo[busId-1].m_deviceId;
00629         }
00630         return m_lastResult = XRV_OK;
00631 }
00632 
00633 //////////////////////////////////////////////////////////////////////////////////////////
00634 // Retrieve the complete device output mode of a device.
00635 XsensResultValue Cmt3::getDeviceMode(CmtDeviceMode& mode, const CmtDeviceId deviceId)
00636 {
00637         CMT3LOG(__FUNCTION__ " %08x\n",deviceId);
00638         CMT3EXITLOG;
00639 
00640         CmtDeviceMode2 mode2;
00641         XsensResultValue rv;
00642         rv = getDeviceMode2(mode2, deviceId);
00643         if (rv == XRV_OK) {
00644                 mode.m_outputMode = mode2.m_outputMode;
00645                 mode.m_outputSettings = mode2.m_outputSettings;
00646                 mode.m_sampleFrequency = mode2.getSampleFrequency();
00647         }
00648         return rv;
00649 }
00650 
00651 //////////////////////////////////////////////////////////////////////////////////////////
00652 // Retrieve the complete device output mode of a device.
00653 XsensResultValue Cmt3::getDeviceMode2(CmtDeviceMode2& mode, const CmtDeviceId deviceId)
00654 {
00655         CMT3LOG(__FUNCTION__ " %08x\n",deviceId);
00656         CMT3EXITLOG;
00657 
00658         uint8_t bid = getBusIdInternal(deviceId);
00659         if (bid == CMT_BID_INVALID)
00660                 return m_lastResult = XRV_INVALIDID;
00661         if (bid == 0)
00662                 return m_lastResult = XRV_INVALIDID;
00663         if (bid == CMT_BID_MASTER)
00664                 bid = 1;
00665 
00666         mode.m_period = m_period;
00667         mode.m_skip = m_skip;
00668         //mode.m_sampleFrequency = (uint16_t) floor(m_sampleFrequency+0.5);
00669         mode.m_outputMode = m_config.m_deviceInfo[bid-1].m_outputMode;
00670         mode.m_outputSettings = m_config.m_deviceInfo[bid-1].m_outputSettings;
00671 
00672         return m_lastResult = XRV_OK;
00673 }
00674 
00675 //////////////////////////////////////////////////////////////////////////////////////////
00676 // Retrieve the eMts data of the specified sensor(s)
00677 #define _XM_SMALL_PACKETS_EMTS 0
00678 XsensResultValue Cmt3::getEMtsData(void* buffer, const CmtDeviceId deviceId)
00679 {
00680         CMT3LOG(__FUNCTION__ " %p %08x\n",buffer,deviceId);
00681         CMT3EXITLOG;
00682 
00683         uint8_t bid = getBusIdInternal(deviceId);
00684         if (bid == CMT_BID_INVALID)
00685                 return m_lastResult = XRV_INVALIDID;
00686         if (!m_readFromFile && !m_serial.isOpen())
00687                 return m_lastResult = XRV_NOPORTOPEN;
00688         if (m_readFromFile && !m_logFile.isOpen())
00689                 return m_lastResult = XRV_NOFILEOPEN;
00690         if (buffer == NULL)
00691                 return m_lastResult = XRV_NULLPTR;
00692 
00693         if (bid == CMT_BID_BROADCAST)
00694         {
00695                 memset(buffer,0,m_config.m_numberOfDevices * CMT_EMTS_SIZE);
00696                 uint8_t* buf = (uint8_t*) buffer;
00697                 // loop over devices and request emts data for all of them
00698                 for (uint32_t dev=0; dev < m_config.m_numberOfDevices; ++dev)
00699                         if (getEMtsData(buf+dev*CMT_EMTS_SIZE,m_config.m_deviceInfo[dev].m_deviceId) != XRV_OK)
00700                                 return m_lastResult;
00701                 //CMT3LOG("L3: getEMtsData (%08x) returns %d\n",deviceId,(int32_t)m_lastResult);
00702                 return m_lastResult;
00703         }
00704 
00705         uint8_t dataIndex;
00706 
00707         if (isXm())
00708         {
00709                 if (bid == CMT_BID_MASTER)
00710                 {
00711                         //CMT3LOG("L3: getEMtsData (%08x) returns XRV_INVALIDID\n",deviceId);
00712                         return m_lastResult = XRV_INVALIDID;
00713                 }
00714                 dataIndex = bid-1;
00715         }
00716         else
00717                 dataIndex = 0;                // when we have a single MT, we should use bank 0
00718 
00719         if (m_eMtsData[dataIndex] == NULL)
00720         {
00721                 m_eMtsData[dataIndex] = malloc(CMT_EMTS_SIZE);
00722 
00723                 // Xbus requires small data packets
00724 #if _XM_SMALL_PACKETS_EMTS
00725                 if (isXm())
00726                 {
00727                         Message msg(CMT_MID_REQEMTS,3);
00728                         Message rcv;
00729                         uint8_t* tme = (uint8_t*) m_eMtsData[dataIndex];
00730                         memset(tme,0,CMT_EMTS_SIZE);        // clear eMTS data
00731                         msg.setBusId(bid);
00732                         msg.setDataByte(0,0);                        // read bank 0
00733                         uint16_t emtPos = 0;
00734                         for (uint8_t page=0;page < 4;++page)        // read all 4 pages, one at a time
00735                         {
00736                                 msg.setDataByte(page,1);
00737                                 for (uint8_t part=0;part < 4;++part, ++emtPos)
00738                                 {
00739                                         if (!m_readFromFile)
00740                                         {
00741                                                 msg.setDataByte(part,2);
00742                                                 m_serial.writeMessage(&msg);
00743                                                 m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_EMTSDATA,0,false);
00744                                         }
00745                                         else
00746                                                 m_lastResult = m_logFile.readMessage(&rcv,CMT_MID_EMTSDATA);
00747 
00748                                         if (m_lastResult != XRV_OK)
00749                                         {
00750                                                 FREENUL(m_eMtsData[dataIndex]);
00751                                                 //CMT3LOG("L3: getEMtsData (%08x) returns %d\n",deviceId,(int32_t)m_lastResult);
00752                                                 return m_lastResult;
00753                                         }
00754                                         if (!m_readFromFile && m_logging)
00755                                                 m_logFile.writeMessage(&rcv);
00756                                         if (rcv.getMessageId() == CMT_MID_ERROR)
00757                                         {
00758                                                 m_lastHwErrorDeviceId = m_config.m_masterDeviceId;
00759                                                 if (rcv.getDataSize() >= 2)
00760                                                 {
00761                                                         uint8_t biddy = rcv.getDataByte(1);
00762                                                         getDeviceId(biddy,m_lastHwErrorDeviceId);
00763                                                 }
00764                                                 m_lastResult = m_lastHwError = (XsensResultValue) rcv.getDataByte(0);
00765                                                 FREENUL(m_eMtsData[dataIndex]);
00766                                                 //CMT3LOG("L3: getEMtsData (%08x) returns %d\n",deviceId,(int32_t)m_lastResult);
00767                                                 return m_lastResult;
00768                                         }
00769 
00770                                         memcpy(tme+(emtPos*66),rcv.getDataBuffer(),66);
00771                                 }
00772                         }
00773                 }        // if (isXm())
00774                 else        // single MT, ask in one packet
00775                 {
00776 #endif
00777                         Message msg(CMT_MID_REQEMTS,2);
00778                         Message rcv;
00779                         memset(m_eMtsData[dataIndex],0,CMT_EMTS_SIZE);        // clear eMTS data
00780                         msg.setBusId(bid);
00781                         msg.setDataByte(0,0);        // read bank 0
00782                         msg.setDataByte(255,1);        // read all pages
00783 
00784                         if (!m_readFromFile)
00785                         {
00786                                 m_serial.writeMessage(&msg);
00787                                 m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_EMTSDATA,0,false);
00788                         }
00789                         else
00790                                 m_lastResult = m_logFile.readMessage(&rcv,CMT_MID_EMTSDATA);
00791                         if (m_lastResult != XRV_OK)
00792                         {
00793                                 FREENUL(m_eMtsData[dataIndex]);
00794                                 //CMT3LOG("L3: getEMtsData (%08x) returns %d\n",deviceId,(int32_t)m_lastResult);
00795                                 return m_lastResult;
00796                         }
00797                         if (!m_readFromFile && m_logging)
00798                                 m_logFile.writeMessage(&rcv);
00799                         if (rcv.getMessageId() == CMT_MID_ERROR)
00800                         {
00801                                 m_lastHwErrorDeviceId = m_config.m_masterDeviceId;
00802                                 if (rcv.getDataSize() >= 2)
00803                                 {
00804                                         uint8_t biddy = rcv.getDataByte(1);
00805                                         getDeviceId(biddy,m_lastHwErrorDeviceId);
00806                                 }
00807                                 m_lastResult = m_lastHwError = (XsensResultValue) rcv.getDataByte(0);
00808                                 FREENUL(m_eMtsData[dataIndex]);
00809                                 //CMT3LOG("L3: getEMtsData (%08x) returns %d\n",deviceId,(int32_t)m_lastResult);
00810                                 return m_lastResult;
00811                         }
00812 
00813                         memcpy(m_eMtsData[dataIndex],rcv.getDataBuffer(),rcv.getDataSize());//CMT_EMTS_SIZE);
00814 #if _XM_SMALL_PACKETS_EMTS
00815                 }
00816 #endif
00817         } else if (m_logging)
00818         {
00819                 // fake the message receipt
00820                 Message msg(CMT_MID_EMTSDATA,CMT_EMTS_SIZE);
00821                 msg.setBusId(bid);
00822                 msg.setDataBuffer((const uint8_t*) m_eMtsData[dataIndex],0,CMT_EMTS_SIZE);
00823                 writeMessageToLogFile(msg);
00824         }
00825 
00826         memcpy(buffer,m_eMtsData[dataIndex],CMT_EMTS_SIZE);
00827         //CMT3LOG("L3: getEMtsData (%08x) returns XRV_OK\n",deviceId);
00828         return m_lastResult = XRV_OK;
00829 }
00830 
00831 //////////////////////////////////////////////////////////////////////////////////////////
00832 // Retrieve the error mode
00833 XsensResultValue Cmt3::getErrorMode(uint16_t& mode, const CmtDeviceId deviceId)
00834 {
00835         CMT3LOG(__FUNCTION__ " %08x\n",deviceId);
00836         CMT3EXITLOG;
00837 
00838         if (isXm())
00839         {
00840                 DO_DATA_REQUEST_BID(CMT_MID_REQXMERRORMODE,CMT_BID_MASTER);
00841                 mode = rcv.getDataShort();
00842         }
00843         else
00844         {
00845                 DO_DATA_REQUEST(CMT_MID_REQERRORMODE);
00846                 mode = rcv.getDataShort();
00847         }
00848         return m_lastResult = XRV_OK;
00849 }
00850 
00851 #if 0
00852 obsolete:
00853 //////////////////////////////////////////////////////////////////////////////////////////
00854 // Retrieve the filter settings of a device.
00855 XsensResultValue Cmt3::getFilterSettings(CmtFilterSettings& settings, const CmtDeviceId deviceId)
00856 {
00857         Message snd;
00858         Message rcv;
00859         uint8_t bid = getBusIdInternal(deviceId);
00860         if (bid == CMT_BID_INVALID)
00861                 return (m_lastResult = XRV_INVALIDID);
00862 
00863         bool xm = isXm();
00864 
00865         if (xm && deviceId == m_config.m_masterDeviceId)
00866                 return m_lastResult = XRV_INVALIDOPERATION;
00867 
00868         if (!xm)
00869                 bid = CMT_BID_MASTER;
00870 
00871         // first setting: gain
00872         snd.setBusId(bid);
00873         snd.setDataByte(0);
00874         snd.setMessageId(CMT_MID_REQFILTERSETTINGS);
00875         m_serial.writeMessage(&snd);
00876         m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_REQFILTERSETTINGSACK,0,true);
00877         if (m_lastResult != XRV_OK)
00878                 return m_lastResult;
00879         if (m_logging)
00880                 m_logFile.writeMessage(&rcv);
00881         HANDLE_ERR_RESULT;
00882         settings.m_gain = rcv.getDataFloat(1);
00883 
00884         // second setting: weighting
00885         snd.setBusId(bid);
00886         snd.setDataByte(1);
00887         snd.setMessageId(CMT_MID_REQFILTERSETTINGS);
00888         m_serial.writeMessage(&snd);
00889         m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_REQFILTERSETTINGSACK,0,true);
00890         if (m_lastResult != XRV_OK)
00891                 return m_lastResult;
00892         if (m_logging)
00893                 m_logFile.writeMessage(&rcv);
00894         HANDLE_ERR_RESULT;
00895         settings.m_weighting = rcv.getDataFloat(1);
00896 
00897         return m_lastResult = XRV_OK;
00898 }
00899 #endif
00900 
00901 //////////////////////////////////////////////////////////////////////////////////////////
00902 // Retrieve the firmware revision of a device.
00903 XsensResultValue Cmt3::getFirmwareRevision(CmtVersion& revision, const CmtDeviceId deviceId)
00904 {
00905         CMT3LOG(__FUNCTION__ " %08x\n",deviceId);
00906         CMT3EXITLOG;
00907 
00908         DO_DATA_REQUEST(CMT_MID_REQFWREV);
00909 
00910         revision.m_major        = rcv.getDataByte(0);
00911         revision.m_minor        = rcv.getDataByte(1);
00912         revision.m_revision        = rcv.getDataByte(2);
00913 
00914         return m_lastResult = XRV_OK;
00915 }
00916 
00917 //////////////////////////////////////////////////////////////////////////////////////////
00918 // Retrieve the heading offset of a device. The range is -pi to +pi.
00919 XsensResultValue Cmt3::getHeading(double& heading, const CmtDeviceId deviceId)
00920 {
00921         CMT3LOG(__FUNCTION__ " %08x\n",deviceId);
00922         CMT3EXITLOG;
00923 
00924         DO_DATA_REQUEST(CMT_MID_REQHEADING);
00925         heading = rcv.getDataFloat();
00926         return m_lastResult = XRV_OK;
00927 }
00928 
00929 //////////////////////////////////////////////////////////////////////////////////////////
00930 // Retrieve the last stored position of the sensor
00931 XsensResultValue Cmt3::getLatLonAlt(CmtVector& lla, const CmtDeviceId deviceId)
00932 {
00933         CMT3LOG(__FUNCTION__ " %08x\n",deviceId);
00934         CMT3EXITLOG;
00935 
00936         DO_DATA_REQUEST(CMT_MID_REQLATLONALT);
00937         lla.m_data[0] = rcv.getDataFloat(0);
00938         lla.m_data[1] = rcv.getDataFloat(4);
00939         lla.m_data[2] = rcv.getDataFloat(8);
00940         return m_lastResult = XRV_OK;
00941 }
00942 
00943 //////////////////////////////////////////////////////////////////////////////////////////
00944 // Retrieve the location ID of a device. The buffer should be at least 20 bytes.
00945 XsensResultValue Cmt3::getLocationId(uint16_t& locationId, const CmtDeviceId deviceId)
00946 {
00947         CMT3LOG(__FUNCTION__ " %08x\n",deviceId);
00948         CMT3EXITLOG;
00949 
00950         DO_DATA_REQUEST(CMT_MID_REQLOCATIONID);
00951         locationId = rcv.getDataShort();
00952         return m_lastResult = XRV_OK;
00953 }
00954 
00955 //////////////////////////////////////////////////////////////////////////////////////////
00956 // Retrieve the current log file read position
00957 XsensResultValue Cmt3::getLogFileReadPosition(CmtFilePos& pos)
00958 {
00959         CMT3LOG(__FUNCTION__ "\n");
00960         CMT3EXITLOG;
00961 
00962         if (m_logFile.isOpen())
00963         {
00964                 pos = m_logFile.getReadPosition();
00965                 return m_lastResult = XRV_OK;
00966         }
00967         pos = 0;
00968         return m_lastResult = XRV_NOFILEOPEN;
00969 }
00970 
00971 //////////////////////////////////////////////////////////////////////////////////////////
00972 // Retrieve the size of the log file
00973 XsensResultValue Cmt3::getLogFileSize(CmtFilePos& size)
00974 {
00975         CMT3LOG(__FUNCTION__ "\n");
00976         CMT3EXITLOG;
00977 
00978         if (m_logFile.isOpen())
00979         {
00980                 size = m_logFile.getFileSize();
00981                 return m_lastResult = XRV_OK;
00982         }
00983         size = 0;
00984         return m_lastResult = XRV_NOFILEOPEN;
00985 }
00986 
00987 //////////////////////////////////////////////////////////////////////////////////////////
00988 // Retrieve the name of the open log file or an empty string if no logfile is open
00989 XsensResultValue Cmt3::getLogFileName(char* filename)
00990 {
00991         CMT3LOG(__FUNCTION__ " (char) %p\n",filename);
00992         CMT3EXITLOG;
00993 
00994         if (m_logFile.isOpen())
00995                 return m_lastResult = m_logFile.getName(filename);
00996         filename[0] = '\0';
00997         return m_lastResult = XRV_NOFILEOPEN;
00998 }
00999 
01000 //////////////////////////////////////////////////////////////////////////////////////////
01001 // Retrieve the name of the open log file or an empty string if no logfile is open
01002 XsensResultValue Cmt3::getLogFileName(wchar_t* filename)
01003 {
01004         CMT3LOG(__FUNCTION__ " (wchar_t) %p\n",filename);
01005         CMT3EXITLOG;
01006 
01007         if (m_logFile.isOpen())
01008                 return m_lastResult = m_logFile.getName(filename);
01009         filename[0] = L'\0';
01010         return m_lastResult = XRV_NOFILEOPEN;
01011 }
01012 
01013 //////////////////////////////////////////////////////////////////////////////////////////
01014 // Return the magnetic declination. The range is -pi to +pi.
01015 XsensResultValue Cmt3::getMagneticDeclination(double& declination, const CmtDeviceId deviceId)
01016 {
01017         CMT3LOG(__FUNCTION__ " %08x\n",deviceId);
01018         CMT3EXITLOG;
01019 
01020         DO_DATA_REQUEST(CMT_MID_REQMAGNETICDECLINATION);
01021         declination = rcv.getDataFloat();
01022         return m_lastResult = XRV_OK;
01023 }
01024 
01025 //////////////////////////////////////////////////////////////////////////////////////////
01026 // Return the device Id of the first device (master)
01027 CmtDeviceId Cmt3::getMasterId(void)
01028 {
01029         CMT3LOG(__FUNCTION__ "\n");
01030 
01031         if (m_serial.isOpen() || m_logFile.isOpen())
01032                 return m_config.m_masterDeviceId;
01033         return 0;
01034 }
01035 
01036 //////////////////////////////////////////////////////////////////////////////////////////
01037 // Return the nr of connected MTs (excludes XMs)
01038 const uint16_t Cmt3::getMtCount(void) const
01039 {
01040         CMT3LOGDAT(__FUNCTION__ "\n");
01041 
01042         if (m_serial.isOpen() || m_logFile.isOpen())
01043                 return m_config.m_numberOfDevices;
01044         return 0;
01045 }
01046 
01047 //////////////////////////////////////////////////////////////////////////////////////////
01048 // Retrieve the CmtDeviceId of the MT device with the given index
01049 XsensResultValue Cmt3::getMtDeviceId (const uint8_t index, CmtDeviceId& deviceId) const
01050 {
01051         CMT3LOG(__FUNCTION__ " %u\n",(uint32_t) index);
01052         CMT3EXITLOG;
01053 
01054         if (index >= m_config.m_numberOfDevices)
01055                 return m_lastResult = XRV_INVALIDPARAM;
01056         deviceId = m_config.m_deviceInfo[index].m_deviceId;
01057         return m_lastResult = XRV_OK;
01058 }
01059 
01060 XsensResultValue Cmt3::getObjectAlignmentMatrix(CmtMatrix& matrix, const CmtDeviceId deviceId)
01061 {
01062         CMT3LOG(__FUNCTION__ " %08x\n",deviceId);
01063         CMT3EXITLOG;
01064 
01065         DO_DATA_REQUEST(CMT_MID_REQOBJECTALIGNMENT);
01066         matrix.m_data[0][0] = rcv.getDataFloat(0);
01067         matrix.m_data[0][1] = rcv.getDataFloat(4);
01068         matrix.m_data[0][2] = rcv.getDataFloat(8);
01069         matrix.m_data[1][0] = rcv.getDataFloat(12);
01070         matrix.m_data[1][1] = rcv.getDataFloat(16);
01071         matrix.m_data[1][2] = rcv.getDataFloat(20);
01072         matrix.m_data[2][0] = rcv.getDataFloat(24);
01073         matrix.m_data[2][1] = rcv.getDataFloat(28);
01074         matrix.m_data[2][2] = rcv.getDataFloat(32);
01075         return m_lastResult = XRV_OK;
01076 }
01077 
01078 //////////////////////////////////////////////////////////////////////////////////////////
01079 // Retrieve the port that the object is connected to.
01080 XsensResultValue Cmt3::getPortNr(uint16_t& port) const
01081 {
01082         CMT3LOG(__FUNCTION__ "\n");
01083         CMT3EXITLOG;
01084 
01085         return m_lastResult = m_serial.getPortNr(port);
01086 }
01087 
01088 //////////////////////////////////////////////////////////////////////////////////////////
01089 // Retrieve the processing flags of a device.
01090 XsensResultValue Cmt3::getProcessingFlags(uint16_t& processingFlags, const CmtDeviceId deviceId)
01091 {
01092         CMT3LOG(__FUNCTION__ " %08x\n",deviceId);
01093         CMT3EXITLOG;
01094 
01095         DO_DATA_REQUEST(CMT_MID_REQPROCESSINGFLAGS);
01096         processingFlags = rcv.getDataShort();
01097         return m_lastResult = XRV_OK;
01098 }
01099 
01100 //////////////////////////////////////////////////////////////////////////////////////////
01101 // Retrieve the product code of a device. The buffer should be at least 21 bytes.
01102 XsensResultValue Cmt3::getProductCode (char* productCode, const CmtDeviceId deviceId)
01103 {
01104         CMT3LOG(__FUNCTION__ " %p %08x\n",productCode,deviceId);
01105         CMT3EXITLOG;
01106 
01107         DO_DATA_REQUEST(CMT_MID_REQPRODUCTCODE);
01108 
01109         uint16_t len = rcv.getDataSize();
01110         memcpy(productCode,rcv.getDataBuffer(),len);
01111         productCode[len] = '\0';
01112 
01113         return m_lastResult = XRV_OK;
01114 }
01115 
01116 //////////////////////////////////////////////////////////////////////////////////////////
01117 // Retrieve the sample frequency of the devices on the bus.
01118 uint16_t Cmt3::getSampleFrequency(void)
01119 {
01120         CMT3LOG(__FUNCTION__ "\n");
01121         CmtDeviceMode2 mode;
01122         mode.m_period = m_period;
01123         mode.m_skip = m_skip;
01124 
01125         return mode.getSampleFrequency();
01126 }
01127 
01128 //////////////////////////////////////////////////////////////////////////////////////////
01129 // Get the baudrate that is reported for the serial connection
01130 XsensResultValue Cmt3::getSerialBaudrate(uint32_t& baudrate)
01131 {
01132         CMT3LOG(__FUNCTION__ "\n");
01133         CMT3EXITLOG;
01134 
01135         DO_DATA_REQUEST_BID(CMT_MID_REQBAUDRATE,CMT_BID_MASTER);
01136         switch (rcv.getDataByte())
01137         {
01138         case CMT_BAUDCODE_4K8:
01139                 baudrate = CMT_BAUD_RATE_4800;
01140                 break;
01141         case CMT_BAUDCODE_9K6:
01142                 baudrate = CMT_BAUD_RATE_9600;
01143                 break;
01144 //        case CMT_BAUDCODE_14K4:
01145 //                baudrate = CMT_BAUD_RATE_14K4;
01146 //                break;
01147         case CMT_BAUDCODE_19K2:
01148                 baudrate = CMT_BAUD_RATE_19K2;
01149                 break;
01150 //        case CMT_BAUDCODE_28K8:
01151 //                baudrate = CMT_BAUD_RATE_28K8;
01152 //                break;
01153         case CMT_BAUDCODE_38K4:
01154                 baudrate = CMT_BAUD_RATE_38K4;
01155                 break;
01156         case CMT_BAUDCODE_57K6:
01157                 baudrate = CMT_BAUD_RATE_57K6;
01158                 break;
01159 //        case CMT_BAUDCODE_76K8:
01160 //                baudrate = CMT_BAUD_RATE_76K8;
01161 //                break;
01162         case CMT_BAUDCODE_115K2:
01163                 baudrate = CMT_BAUD_RATE_115K2;
01164                 break;
01165         case CMT_BAUDCODE_230K4:
01166                 baudrate = CMT_BAUD_RATE_230K4;
01167                 break;
01168         case CMT_BAUDCODE_460K8:
01169                 baudrate = CMT_BAUD_RATE_460K8;
01170                 break;
01171         case CMT_BAUDCODE_921K6_ALT:
01172         case CMT_BAUDCODE_921K6:
01173                 baudrate = CMT_BAUD_RATE_921K6;
01174                 break;
01175         default:
01176                 return m_lastResult = XRV_BAUDRATEINVALID;
01177         }
01178         return m_lastResult = XRV_OK;
01179 }
01180 
01181 //////////////////////////////////////////////////////////////////////////////////////////
01182 // Retrieve the inbound synchronization settings of a device.
01183 XsensResultValue Cmt3::getSyncInSettings(CmtSyncInSettings& settings)
01184 {
01185         CMT3LOG(__FUNCTION__ "\n");
01186         CMT3EXITLOG;
01187 
01188         Message snd(CMT_MID_REQSYNCINSETTINGS,1);
01189         Message rcv;
01190 
01191         snd.setBusId(CMT_BID_MASTER);
01192 
01193         snd.setDataByte(CMT_PARAM_SYNCIN_MODE);
01194         m_serial.writeMessage(&snd);
01195         m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_REQSYNCINSETTINGSACK,0,true);
01196         if (m_lastResult != XRV_OK)
01197                 return m_lastResult;
01198         if (m_logging)
01199                 m_logFile.writeMessage(&rcv);
01200         HANDLE_ERR_RESULT;
01201         settings.m_mode = rcv.getDataShort(1);
01202 
01203         snd.setDataByte(CMT_PARAM_SYNCIN_SKIPFACTOR);
01204         m_serial.writeMessage(&snd);
01205         m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_REQSYNCINSETTINGSACK,0,true);
01206         if (m_lastResult != XRV_OK)
01207                 return m_lastResult;
01208         if (m_logging)
01209                 m_logFile.writeMessage(&rcv);
01210         HANDLE_ERR_RESULT;
01211         settings.m_skipFactor = rcv.getDataShort(1);
01212 
01213         snd.setDataByte(CMT_PARAM_SYNCIN_OFFSET);
01214         m_serial.writeMessage(&snd);
01215         m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_REQSYNCINSETTINGSACK,0,true);
01216         if (m_lastResult != XRV_OK)
01217                 return m_lastResult;
01218         if (m_logging)
01219                 m_logFile.writeMessage(&rcv);
01220         HANDLE_ERR_RESULT;
01221         settings.m_offset = rcv.getDataLong(1);
01222 
01223         // convert the offset to ns
01224         settings.m_offset = (uint32_t) ((((double)settings.m_offset)*CMT_SYNC_CLOCK_TICKS_TO_NS)+0.5);
01225 
01226         return m_lastResult = XRV_OK;
01227 }
01228 
01229 //////////////////////////////////////////////////////////////////////////////////////////
01230 // Retrieve the inbound synchronization mode of a device.
01231 XsensResultValue Cmt3::getSyncInMode(uint16_t& mode)
01232 {
01233         CMT3LOG(__FUNCTION__ "\n");
01234         CMT3EXITLOG;
01235 
01236         Message snd(CMT_MID_REQSYNCINSETTINGS,1);
01237         Message rcv;
01238 
01239         snd.setBusId(CMT_BID_MASTER);
01240 
01241         snd.setDataByte(CMT_PARAM_SYNCIN_MODE);
01242         m_serial.writeMessage(&snd);
01243         m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_REQSYNCINSETTINGSACK,0,true);
01244         if (m_lastResult != XRV_OK)
01245                 return m_lastResult;
01246         if (m_logging)
01247                 m_logFile.writeMessage(&rcv);
01248         HANDLE_ERR_RESULT;
01249         mode = rcv.getDataShort(1);
01250 
01251         return m_lastResult = XRV_OK;
01252 }
01253 
01254 //////////////////////////////////////////////////////////////////////////////////////////
01255 // Retrieve the inbound synchronization skip factor of a device.
01256 XsensResultValue Cmt3::getSyncInSkipFactor(uint16_t& skipFactor)
01257 {
01258         CMT3LOG(__FUNCTION__ "\n");
01259         CMT3EXITLOG;
01260 
01261         Message snd(CMT_MID_REQSYNCINSETTINGS,1);
01262         Message rcv;
01263 
01264         snd.setBusId(CMT_BID_MASTER);
01265 
01266         snd.setDataByte(CMT_PARAM_SYNCIN_SKIPFACTOR);
01267         m_serial.writeMessage(&snd);
01268         m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_REQSYNCINSETTINGSACK,0,true);
01269         if (m_lastResult != XRV_OK)
01270                 return m_lastResult;
01271         if (m_logging)
01272                 m_logFile.writeMessage(&rcv);
01273         HANDLE_ERR_RESULT;
01274         skipFactor = rcv.getDataShort(1);
01275 
01276         return m_lastResult = XRV_OK;
01277 }
01278 
01279 //////////////////////////////////////////////////////////////////////////////////////////
01280 // Retrieve the inbound synchronization offset of a device.
01281 XsensResultValue Cmt3::getSyncInOffset(uint32_t& offset)
01282 {
01283         CMT3LOG(__FUNCTION__ "\n");
01284         CMT3EXITLOG;
01285 
01286         Message snd(CMT_MID_REQSYNCINSETTINGS,1);
01287         Message rcv;
01288 
01289         snd.setBusId(CMT_BID_MASTER);
01290 
01291         snd.setDataByte(CMT_PARAM_SYNCIN_OFFSET);
01292         m_serial.writeMessage(&snd);
01293         m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_REQSYNCINSETTINGSACK,0,true);
01294         if (m_lastResult != XRV_OK)
01295                 return m_lastResult;
01296         if (m_logging)
01297                 m_logFile.writeMessage(&rcv);
01298         HANDLE_ERR_RESULT;
01299         offset = rcv.getDataLong(1);
01300 
01301         // convert the offset to ns
01302         offset = (uint32_t) ((((double)offset)*CMT_SYNC_CLOCK_TICKS_TO_NS)+0.5);
01303 
01304         return m_lastResult = XRV_OK;
01305 }
01306 
01307 //////////////////////////////////////////////////////////////////////////////////////////
01308 // Get the synchronization mode of the XM
01309 XsensResultValue Cmt3::getSyncMode(uint8_t& mode)
01310 {
01311         CMT3LOG(__FUNCTION__ "\n");
01312         CMT3EXITLOG;
01313 
01314         DO_DATA_REQUEST_BID(CMT_MID_REQSYNCMODE,CMT_BID_MASTER);
01315         mode = rcv.getDataByte();
01316         return m_lastResult = XRV_OK;
01317 }
01318 
01319 //////////////////////////////////////////////////////////////////////////////////////////
01320 // Retrieve the outbound synchronization settings of a device.
01321 XsensResultValue Cmt3::getSyncOutSettings(CmtSyncOutSettings& settings)
01322 {
01323         CMT3LOG(__FUNCTION__ "\n");
01324         CMT3EXITLOG;
01325 
01326         Message snd(CMT_MID_REQSYNCOUTSETTINGS,1);
01327         Message rcv;
01328 
01329         snd.setBusId(CMT_BID_MASTER);
01330 
01331         snd.setDataByte(CMT_PARAM_SYNCOUT_MODE);
01332         m_serial.writeMessage(&snd);
01333         m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_REQSYNCOUTSETTINGSACK,0,true);
01334         if (m_lastResult != XRV_OK)
01335                 return m_lastResult;
01336         if (m_logging)
01337                 m_logFile.writeMessage(&rcv);
01338         HANDLE_ERR_RESULT;
01339         settings.m_mode = rcv.getDataShort(1);
01340 
01341         snd.setDataByte(CMT_PARAM_SYNCOUT_SKIPFACTOR);
01342         m_serial.writeMessage(&snd);
01343         m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_REQSYNCOUTSETTINGSACK,0,true);
01344         if (m_lastResult != XRV_OK)
01345                 return m_lastResult;
01346         if (m_logging)
01347                 m_logFile.writeMessage(&rcv);
01348         HANDLE_ERR_RESULT;
01349         settings.m_skipFactor = rcv.getDataShort(1);
01350 
01351         snd.setDataByte(CMT_PARAM_SYNCOUT_OFFSET);
01352         m_serial.writeMessage(&snd);
01353         m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_REQSYNCOUTSETTINGSACK,0,true);
01354         if (m_lastResult != XRV_OK)
01355                 return m_lastResult;
01356         if (m_logging)
01357                 m_logFile.writeMessage(&rcv);
01358         HANDLE_ERR_RESULT;
01359         settings.m_offset = rcv.getDataLong(1);
01360 
01361         snd.setDataByte(CMT_PARAM_SYNCOUT_PULSEWIDTH);
01362         m_serial.writeMessage(&snd);
01363         m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_REQSYNCOUTSETTINGSACK,0,true);
01364         if (m_lastResult != XRV_OK)
01365                 return m_lastResult;
01366         if (m_logging)
01367                 m_logFile.writeMessage(&rcv);
01368         HANDLE_ERR_RESULT;
01369         settings.m_pulseWidth = rcv.getDataLong(1);
01370 
01371         // convert the offset and pulse width to ns
01372         settings.m_offset = (uint32_t)        ((((double)settings.m_offset)*CMT_SYNC_CLOCK_TICKS_TO_NS)+0.5);
01373         settings.m_pulseWidth = (uint32_t)        ((((double)settings.m_pulseWidth)*CMT_SYNC_CLOCK_TICKS_TO_NS)+0.5);
01374 
01375         return m_lastResult = XRV_OK;
01376 }
01377 
01378 //////////////////////////////////////////////////////////////////////////////////////////
01379 // Retrieve the outbound synchronization mode of a device.
01380 XsensResultValue Cmt3::getSyncOutMode(uint16_t& mode)
01381 {
01382         CMT3LOG(__FUNCTION__ "\n");
01383         CMT3EXITLOG;
01384 
01385         Message snd(CMT_MID_REQSYNCOUTSETTINGS,1);
01386         Message rcv;
01387 
01388         snd.setBusId(CMT_BID_MASTER);
01389 
01390         snd.setDataByte(CMT_PARAM_SYNCOUT_MODE);
01391         m_serial.writeMessage(&snd);
01392         m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_REQSYNCOUTSETTINGSACK,0,true);
01393         if (m_lastResult != XRV_OK)
01394                 return m_lastResult;
01395         if (m_logging)
01396                 m_logFile.writeMessage(&rcv);
01397         HANDLE_ERR_RESULT;
01398         mode = rcv.getDataShort(1);
01399 
01400         return m_lastResult = XRV_OK;
01401 }
01402 
01403 //////////////////////////////////////////////////////////////////////////////////////////
01404 // Retrieve the outbound synchronization pulse width of a device.
01405 XsensResultValue Cmt3::getSyncOutPulseWidth(uint32_t& pulseWidth)
01406 {
01407         CMT3LOG(__FUNCTION__ "\n");
01408         CMT3EXITLOG;
01409 
01410         Message snd(CMT_MID_REQSYNCOUTSETTINGS,1);
01411         Message rcv;
01412 
01413         snd.setBusId(CMT_BID_MASTER);
01414 
01415         snd.setDataByte(CMT_PARAM_SYNCOUT_PULSEWIDTH);
01416         m_serial.writeMessage(&snd);
01417         m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_REQSYNCOUTSETTINGSACK,0,true);
01418         if (m_lastResult != XRV_OK)
01419                 return m_lastResult;
01420         if (m_logging)
01421                 m_logFile.writeMessage(&rcv);
01422         HANDLE_ERR_RESULT;
01423         pulseWidth = rcv.getDataLong(1);
01424 
01425         // convert the pulse width to ns
01426         pulseWidth = (uint32_t)        ((((double)pulseWidth)*CMT_SYNC_CLOCK_TICKS_TO_NS)+0.5);
01427 
01428         return m_lastResult = XRV_OK;
01429 }
01430 
01431 //////////////////////////////////////////////////////////////////////////////////////////
01432 // Retrieve the outbound synchronization skip factor of a device.
01433 XsensResultValue Cmt3::getSyncOutSkipFactor(uint16_t& skipFactor)
01434 {
01435         CMT3LOG(__FUNCTION__ "\n");
01436         CMT3EXITLOG;
01437 
01438         Message snd(CMT_MID_REQSYNCOUTSETTINGS,1);
01439         Message rcv;
01440 
01441         snd.setBusId(CMT_BID_MASTER);
01442 
01443         snd.setDataByte(CMT_PARAM_SYNCOUT_SKIPFACTOR);
01444         m_serial.writeMessage(&snd);
01445         m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_REQSYNCOUTSETTINGSACK,0,true);
01446         if (m_lastResult != XRV_OK)
01447                 return m_lastResult;
01448         if (m_logging)
01449                 m_logFile.writeMessage(&rcv);
01450         HANDLE_ERR_RESULT;
01451         skipFactor = rcv.getDataShort(1);
01452 
01453         return m_lastResult = XRV_OK;
01454 }
01455 
01456 //////////////////////////////////////////////////////////////////////////////////////////
01457 // Retrieve the outbound synchronization offset of a device.
01458 XsensResultValue Cmt3::getSyncOutOffset(uint32_t& offset)
01459 {
01460         CMT3LOG(__FUNCTION__ "\n");
01461         CMT3EXITLOG;
01462 
01463         Message snd(CMT_MID_REQSYNCOUTSETTINGS,1);
01464         Message rcv;
01465 
01466         snd.setBusId(CMT_BID_MASTER);
01467 
01468         snd.setDataByte(CMT_PARAM_SYNCOUT_OFFSET);
01469         m_serial.writeMessage(&snd);
01470         m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_REQSYNCOUTSETTINGSACK,0,true);
01471         if (m_lastResult != XRV_OK)
01472                 return m_lastResult;
01473         if (m_logging)
01474                 m_logFile.writeMessage(&rcv);
01475         HANDLE_ERR_RESULT;
01476         offset = rcv.getDataLong(1);
01477 
01478         // convert the offset to ns
01479         offset = (uint32_t)        ((((double)offset)*CMT_SYNC_CLOCK_TICKS_TO_NS)+0.5);
01480 
01481         return m_lastResult = XRV_OK;
01482 }
01483 
01484 //////////////////////////////////////////////////////////////////////////////////////////
01485 uint32_t Cmt3::getTimeoutConfig (void) const
01486 {
01487         return m_timeoutConf;
01488 }
01489 
01490 //////////////////////////////////////////////////////////////////////////////////////////
01491 uint32_t Cmt3::getTimeoutMeasurement (void) const
01492 {
01493         return m_timeoutMeas;
01494 }
01495 
01496 //////////////////////////////////////////////////////////////////////////////////////////
01497 // Retrieve the transmission delay
01498 XsensResultValue Cmt3::getTransmissionDelay(uint16_t& delay, const CmtDeviceId deviceId)
01499 {
01500         CMT3LOG(__FUNCTION__ " %08x\n",deviceId);
01501         CMT3EXITLOG;
01502 
01503         DO_DATA_REQUEST(CMT_MID_REQTRANSMITDELAY);
01504         delay = rcv.getDataShort();
01505 
01506         return m_lastResult = XRV_OK;
01507 }
01508 
01509 //////////////////////////////////////////////////////////////////////////////////////////
01510 // Retrieve the UTC time of the last received sample
01511 XsensResultValue Cmt3::getUtcTime(CmtUtcTime& utc, const CmtDeviceId deviceId)
01512 {
01513         CMT3LOG(__FUNCTION__ " %08x\n",deviceId);
01514         CMT3EXITLOG;
01515 
01516         DO_DATA_REQUEST(CMT_MID_REQUTCTIME);
01517 
01518         utc.m_nano        = rcv.getDataLong(0);
01519         utc.m_year        = rcv.getDataShort(4);
01520         utc.m_month        = rcv.getDataByte(6);
01521         utc.m_day        = rcv.getDataByte(7);
01522         utc.m_hour        = rcv.getDataByte(8);
01523         utc.m_minute= rcv.getDataByte(9);
01524         utc.m_second= rcv.getDataByte(10);
01525         utc.m_valid        = rcv.getDataByte(11);
01526 
01527 //        if (utc.m_valid == 0)
01528 //                return m_lastResult = XRV_INVALID_TIME;
01529 
01530         return m_lastResult = XRV_OK;
01531 }
01532 
01533 //////////////////////////////////////////////////////////////////////////////////////////
01534 // Get the dual-mode output settings of the XM
01535 XsensResultValue Cmt3::getXmOutputMode(uint8_t& mode)
01536 {
01537         CMT3LOG(__FUNCTION__ "\n");
01538         CMT3EXITLOG;
01539 
01540         DO_DATA_REQUEST_BID(CMT_MID_REQOPMODE,CMT_BID_MASTER);
01541         mode = rcv.getDataByte();
01542         return m_lastResult = XRV_OK;
01543 }
01544 
01545 //////////////////////////////////////////////////////////////////////////////////////////
01546 // Place all connected sensors into Configuration Mode.
01547 XsensResultValue Cmt3::gotoConfig(void)
01548 {
01549         CMT3LOG(__FUNCTION__ " port %u\n",(uint32_t)m_serial.getCmt1s()->getPortNr());
01550         CMT3EXITLOG;
01551 
01552         Message snd(CMT_MID_GOTOCONFIG);
01553         Message rcv;
01554         int32_t tries = 0;
01555 
01556         srand( (unsigned int)timeStampNow());
01557 
01558         m_serial.setTimeout(CMT3_CONFIG_TIMEOUT);
01559         snd.setBusId(CMT_BID_MASTER);
01560         while (tries++ < m_gotoConfigTries)
01561         {
01562                 m_serial.getCmt1s()->flushData();                        // special case for goto config. we want the buffer to be empty
01563                 CMT3LOG(__FUNCTION__ " Attempt to goto config %d\n",tries);
01564                 m_serial.writeMessage(&snd);
01565                 m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_GOTOCONFIGACK,0,false);
01566 #ifndef _PRODUCTION // Production version must retry on nodata, normal versions do not.
01567                 if (m_lastResult == XRV_TIMEOUTNODATA)
01568                         break;
01569 #endif
01570                 if (m_lastResult == XRV_OK)
01571                 {
01572                         if (m_logging)
01573                                 m_logFile.writeMessage(&rcv);
01574                         if (rcv.getMessageId() == CMT_MID_ERROR)
01575                         {
01576                                 m_lastHwErrorDeviceId = CMT_DID_MASTER;
01577                                 if (rcv.getDataSize() >= 2)
01578                                 {
01579                                         uint8_t biddy = rcv.getDataByte(1);
01580                                         getDeviceId(biddy,m_lastHwErrorDeviceId);
01581                                 }
01582                                 m_lastResult = m_lastHwError = (XsensResultValue) rcv.getDataByte(0);
01583                                 CMT3LOG(__FUNCTION__ " Goto config failed, error received %d: %s\n",(int32_t)m_lastResult,xsensResultText(m_lastResult));
01584                                 m_serial.setTimeout(m_timeoutConf);
01585                                 return m_lastResult;
01586                         }
01587                         CMT3LOG(__FUNCTION__ " Goto config succeeded\n");
01588                         m_measuring = false;
01589                         m_serial.setTimeout(m_timeoutConf);
01590                         return m_lastResult = XRV_OK;
01591                 }
01592                 m_lastResult = m_serial.getLastResult();
01593                 msleep(((long)rand() * 10)/RAND_MAX);
01594         }
01595         m_serial.setTimeout(m_timeoutConf);
01596         m_serial.getCmt1s()->flushData(); // flush buffers once more to remove any errors that are still there.
01597         m_measuring = (m_lastResult != XRV_OK);
01598         CMT3LOG(__FUNCTION__ " returns %d: %s\n",(int32_t)m_lastResult,xsensResultText(m_lastResult));
01599         return m_lastResult;
01600 }
01601 
01602 //////////////////////////////////////////////////////////////////////////////////////////
01603 // Place all connected sensors into Measurement Mode.
01604 XsensResultValue Cmt3::gotoMeasurement(void)
01605 {
01606         CMT3LOG(__FUNCTION__ " port %u\n",(uint32_t)m_serial.getCmt1s()->getPortNr());
01607         CMT3EXITLOG;
01608 
01609         Message snd(CMT_MID_GOTOMEASUREMENT);
01610         Message rcv;
01611 
01612         snd.setBusId(CMT_BID_MASTER);
01613 
01614         m_serial.writeMessage(&snd);
01615         m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_GOTOMEASUREMENTACK,0,false);
01616         if (m_lastResult != XRV_OK)
01617                 return m_lastResult;
01618         if (m_logging)
01619                 m_logFile.writeMessage(&rcv);
01620         HANDLE_ERR_RESULT;
01621 
01622         m_rtcInitialized = false;
01623         m_measuring = true;
01624         m_serial.setTimeout(m_timeoutMeas);
01625         return (m_lastResult = XRV_OK);
01626 }
01627 
01628 XsensResultValue Cmt3::initBus(void)
01629 {
01630         CMT3LOG(__FUNCTION__ " port %u\n",(uint32_t)m_serial.getCmt1s()->getPortNr());
01631         CMT3EXITLOG;
01632 
01633         DO_DATA_REQUEST_BID(CMT_MID_INITBUS,CMT_BID_MASTER);
01634         return m_lastResult = XRV_OK;        // m_serial.waitForMessage(&rcv,CMT_MID_INITBUSRESULTS);
01635 }
01636 
01637 //////////////////////////////////////////////////////////////////////////////////////////
01638 // Return whether the main device is an Xbus Master or not.
01639 bool Cmt3::isXm(void) const
01640 {
01641         return ((m_config.m_masterDeviceId & CMT_DID_TYPEH_MASK) == CMT_DID_TYPEH_XM);
01642 }
01643 
01644 //////////////////////////////////////////////////////////////////////////////////////////
01645 // Open a communication channel to the given serial port name.
01646 XsensResultValue Cmt3::openPort(const char *portName, const uint32_t baudRate, uint32_t readBufSize, uint32_t writeBufSize)
01647 {
01648         CMT3LOG(__FUNCTION__ " port %s @baud %d, timeoutC=%u, timeoutM=%u\n", portName, baudRate, m_timeoutConf, m_timeoutMeas);
01649         CMT3EXITLOG;
01650 
01651         if (m_logFile.isOpen())
01652                 return m_lastResult = XRV_ALREADYOPEN;
01653 
01654         m_serial.setTimeout(m_timeoutConf);
01655         if ((m_lastResult = m_serial.open(portName, baudRate, readBufSize, writeBufSize)) != XRV_OK)
01656                 return m_lastResult;
01657 
01658         m_readBufSize = readBufSize;
01659         m_writeBufSize = writeBufSize;
01660 
01661         CMT3LOG(__FUNCTION__ " Low level port opened, gotoConfig\n");
01662 
01663     m_baudrate = baudRate;
01664     m_rtcInitialized = false;
01665     m_measuring = true;         // required for faster operation of refreshCache
01666     m_logging = false;
01667 
01668     // place the device in config mode
01669     if (gotoConfig() != XRV_OK)
01670     {
01671         CMT3LOG(__FUNCTION__ " gotoConfig failed: [%d]%s\n",m_lastResult,xsensResultText(m_lastResult));
01672         m_serial.close();
01673         return XRV_CONFIGCHECKFAIL;
01674     }
01675 
01676     CMT3LOG(__FUNCTION__ " gotoConfig succeeded, requesting initBus\n");
01677     Message snd,rcv;
01678 
01679     if (initBus() != XRV_OK)
01680     {
01681         CMT3LOG(__FUNCTION__ " initBus failed: [%d]%s\n",m_lastResult,xsensResultText(m_lastResult));
01682         m_serial.close();
01683         return XRV_CONFIGCHECKFAIL;
01684     }
01685 
01686     CMT3LOG(__FUNCTION__ " initBus succeeded, cleaning up the cache\n");
01687     if (refreshCache() != XRV_OK)
01688     {
01689         m_serial.close();
01690         CMT3LOG(__FUNCTION__ " refreshCache failed: [%d]%s\n",m_lastResult,xsensResultText(m_lastResult));
01691         return XRV_CONFIGCHECKFAIL;
01692     }
01693 
01694     CMT3LOG(__FUNCTION__ " returning OK\n");
01695     return m_lastResult = XRV_OK;
01696 }
01697 
01698 #ifdef _WIN32
01699 //////////////////////////////////////////////////////////////////////////////////////////
01700 // Open a communication channel to the given COM port number.
01701 XsensResultValue Cmt3::openPort(const uint32_t portNumber, const uint32_t baudRate, uint32_t readBufSize, uint32_t writeBufSize)
01702 {
01703         // open the port
01704         CMT3LOG(__FUNCTION__ " port %d @baud %d, timeoutC=%u, timeoutM=%u\n",(int32_t)portNumber,baudRate,m_timeoutConf, m_timeoutMeas);
01705         CMT3EXITLOG;
01706 
01707         if (m_logFile.isOpen())
01708                 return m_lastResult = XRV_ALREADYOPEN;
01709 
01710         m_serial.setTimeout(m_timeoutConf);        // then update L2 (and L1)
01711         if ((m_lastResult = m_serial.open(portNumber, baudRate, readBufSize, writeBufSize)) != XRV_OK)
01712                 return m_lastResult;
01713 
01714         m_readBufSize = readBufSize;
01715         m_writeBufSize = writeBufSize;
01716 
01717         CMT3LOG(__FUNCTION__ " Low level port opened, gotoConfig\n");
01718 
01719         m_baudrate = baudRate;
01720         m_rtcInitialized = false;
01721         m_measuring = true;                        // required for faster operation of refreshCache
01722         m_logging = false;
01723 
01724         // place the device in config mode
01725         if (gotoConfig() != XRV_OK)
01726         {
01727                 CMT3LOG(__FUNCTION__ " gotoConfig failed: [%d]%s\n",m_lastResult,xsensResultText(m_lastResult));
01728                 m_serial.close();
01729                 return XRV_CONFIGCHECKFAIL;
01730         }
01731 
01732         CMT3LOG(__FUNCTION__ " gotoConfig succeeded, requesting initBus\n");
01733         Message snd,rcv;
01734 
01735         while (initBus() != XRV_OK)
01736         {
01737                 if (m_lastResult == XRV_NOBUS)
01738                 {
01739                         // attempt a bus power up and try again
01740                         setBusPowerState(true);
01741                         if (initBus() == XRV_OK)
01742                                 break;
01743                 }
01744 
01745                 CMT3LOG(__FUNCTION__ " initBus failed: [%d]%s\n",m_lastResult,xsensResultText(m_lastResult));
01746                 m_serial.close();
01747                 return XRV_CONFIGCHECKFAIL;
01748         }
01749 
01750         CMT3LOG(__FUNCTION__ " initBus succeeded, cleaning up the cache\n");
01751         if (refreshCache() != XRV_OK)
01752         {
01753                 m_serial.close();
01754                 CMT3LOG(__FUNCTION__ " refreshCache failed: [%d]%s\n",m_lastResult,xsensResultText(m_lastResult));
01755                 return XRV_CONFIGCHECKFAIL;
01756         }
01757 
01758         CMT3LOG(__FUNCTION__ " returning OK\n");
01759         return m_lastResult = XRV_OK;
01760 }
01761 #endif
01762 
01763 //////////////////////////////////////////////////////////////////////////////////////////
01764 // Get the MessageId of the next logged message
01765 XsensResultValue Cmt3::peekLogMessageId(uint8_t& messageId)
01766 {
01767         CMT3LOG(__FUNCTION__ "\n");
01768         CMT3EXITLOG;
01769 
01770         if (!m_readFromFile)
01771                 return m_lastResult = XRV_INVALIDOPERATION;
01772 
01773         Message msg;
01774 
01775         CmtFilePos pos =  m_logFile.getReadPosition();
01776         m_lastResult = m_logFile.readMessage(&msg);
01777         m_logFile.setReadPosition(pos);
01778 
01779         if (m_lastResult != XRV_OK)
01780         {
01781                 CMT3LOG(__FUNCTION__ " no messages to be read\n");
01782                 return m_lastResult;
01783         }
01784         messageId = msg.getMessageId();
01785         CMT3LOG(__FUNCTION__ " found msg with ID %02x\n",(int32_t) messageId);
01786         return m_lastResult = XRV_OK;
01787 }
01788 
01789 //////////////////////////////////////////////////////////////////////////////////////////
01790 // Retrieve a data message.
01791 XsensResultValue Cmt3::readDataPacket(Packet* pack, bool acceptOther)
01792 {
01793         CMT3LOGDAT(__FUNCTION__ " %p %u\n",pack,acceptOther?1:0);
01794         CMT3EXITLOGDAT;
01795 
01796         if (!m_readFromFile)
01797         {
01798                 while(1)
01799                 {
01800                         m_lastResult = m_serial.readMessage(&pack->m_msg);
01801                         if (m_lastResult != XRV_OK)
01802                         {
01803                                 CMT3LOGDAT(__FUNCTION__ " no data messages to be read (s)\n");
01804                                 return m_lastResult;
01805                         }
01806                         if (m_logging)
01807                                 m_logFile.writeMessage(&pack->m_msg);
01808                         if (pack->m_msg.getMessageId() == CMT_MID_MTDATA)
01809                         {
01810                                 pack->setXbus(m_config.m_masterDeviceId != m_config.m_deviceInfo[0].m_deviceId,false);
01811                                 pack->m_itemCount = m_config.m_numberOfDevices;
01812                                 for (uint16_t i = 0;i < m_config.m_numberOfDevices;++i)
01813                                         pack->setDataFormat(m_config.m_deviceInfo[i].m_outputMode,m_config.m_deviceInfo[i].m_outputSettings,i);
01814                                 pack->m_toa = timeStampNow();
01815                                 if (m_useRtc)
01816                                         fillRtc(pack);
01817                                 CMT3LOGDAT(__FUNCTION__ " data message read (s)\n");
01818                                 return m_lastResult = XRV_OK;
01819                         }
01820                         else if (pack->m_msg.getMessageId() == CMT_MID_ERROR)
01821                         {
01822                                 m_lastHwErrorDeviceId = m_config.m_masterDeviceId;
01823                                 if (pack->m_msg.getDataSize() >= 2)
01824                                 {
01825                                         uint8_t biddy = pack->m_msg.getDataByte(1);
01826                                         getDeviceId(biddy,m_lastHwErrorDeviceId);
01827                                 }
01828                                 return m_lastResult = m_lastHwError = (XsensResultValue) pack->m_msg.getDataByte(0);
01829                         }
01830                         CMT3LOGDAT(__FUNCTION__ " non-data message read (s): %2x\n",(int32_t) pack->m_msg.getMessageId());
01831                         if (acceptOther)
01832                         {
01833                                 CMT3LOGDAT(__FUNCTION__ " accepting other message (s)\n");
01834                                 return m_lastResult = XRV_OTHER;
01835                         }
01836                 }
01837         }
01838         else
01839         {
01840                 while(1)
01841                 {
01842                         m_lastResult = m_logFile.readMessage(&pack->m_msg);
01843                         if (m_lastResult != XRV_OK)
01844                         {
01845                                 CMT3LOGDAT(__FUNCTION__ " no data messages to be read (f)\n");
01846                                 return m_lastResult;
01847                         }
01848                         if (pack->m_msg.getMessageId() == CMT_MID_MTDATA)
01849                         {
01850                                 pack->setXbus(m_config.m_masterDeviceId != m_config.m_deviceInfo[0].m_deviceId,false);
01851                                 pack->m_itemCount = m_config.m_numberOfDevices;
01852                                 for (uint16_t i = 0;i < m_config.m_numberOfDevices;++i)
01853                                         pack->setDataFormat(m_config.m_deviceInfo[i].m_outputMode,m_config.m_deviceInfo[i].m_outputSettings,i);
01854                                 pack->m_toa = timeStampNow();
01855                                 if (m_useRtc)
01856                                         fillRtc(pack);
01857                                 CMT3LOGDAT(__FUNCTION__ " data message read (f)\n");
01858                                 return m_lastResult = XRV_OK;
01859                         }
01860                         else if (pack->m_msg.getMessageId() == CMT_MID_ERROR)
01861                         {
01862                                 m_lastHwErrorDeviceId = m_config.m_masterDeviceId;
01863                                 if (pack->m_msg.getDataSize() >= 2)
01864                                 {
01865                                         uint8_t biddy = pack->m_msg.getDataByte(1);
01866                                         getDeviceId(biddy,m_lastHwErrorDeviceId);
01867                                 }
01868                                 return m_lastResult = m_lastHwError = (XsensResultValue) pack->m_msg.getDataByte(0);
01869                         }
01870                         CMT3LOGDAT(__FUNCTION__ " non-data message read (f): %2x\n",(int32_t) pack->m_msg.getMessageId());
01871                         if (acceptOther)
01872                         {
01873                                 CMT3LOGDAT(__FUNCTION__ " accepting other message (f)\n");
01874                                 return m_lastResult = XRV_OTHER;
01875                         }
01876                 }
01877         }
01878 }
01879 
01880 //////////////////////////////////////////////////////////////////////////////////////////
01881 // Request a data message and wait for it to arrive.
01882 XsensResultValue Cmt3::requestData(Packet* pack, const uint8_t *data, const uint16_t count)
01883 {
01884         CMT3LOGDAT(__FUNCTION__ " %p\n",pack);
01885         CMT3EXITLOGDAT;
01886 
01887         if (isXm())
01888                 return m_lastResult = XRV_INVALIDOPERATION;
01889         Message snd(CMT_MID_REQDATA);
01890         snd.setDataBuffer(data, 0, count);
01891         m_lastResult = m_serial.writeMessage(&snd);
01892         if (m_lastResult != XRV_OK)
01893                 return m_lastResult;
01894         m_lastResult = m_serial.waitForMessage(&pack->m_msg,CMT_MID_MTDATA,0,true);
01895         if (m_lastResult != XRV_OK)
01896                 return m_lastResult;
01897         for (uint16_t i = 0;i < m_config.m_numberOfDevices;++i)
01898                 pack->setDataFormat(m_config.m_deviceInfo[i].m_outputMode, m_config.m_deviceInfo[i].m_outputSettings,i);
01899         if (m_logging)
01900                 m_logFile.writeMessage(&pack->m_msg);
01901         if (pack->m_msg.getMessageId() == CMT_MID_ERROR)
01902         {
01903                 m_lastHwErrorDeviceId = m_config.m_masterDeviceId;
01904                 if (pack->m_msg.getDataSize() >= 2)
01905                 {
01906                         uint8_t biddy = pack->m_msg.getDataByte(1);
01907                         getDeviceId(biddy,m_lastHwErrorDeviceId);
01908                 }
01909                 return m_lastResult = m_lastHwError = (XsensResultValue) pack->m_msg.getDataByte();
01910         }
01911         return m_lastResult;
01912 }
01913 
01914 //////////////////////////////////////////////////////////////////////////////////////////
01915 // Reset all connected sensors.
01916 XsensResultValue Cmt3::reset(void)
01917 {
01918         CMT3LOG(__FUNCTION__ "\n");
01919         CMT3EXITLOG;
01920 
01921         Message snd(CMT_MID_RESET,0);
01922         Message rcv;
01923         snd.setBusId(CMT_BID_MASTER);
01924         m_serial.writeMessage(&snd);
01925         m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_RESETACK,100,false);
01926         if (m_lastResult != XRV_OK)
01927                 return m_lastResult;
01928         if (m_logging)
01929                 m_logFile.writeMessage(&rcv);
01930         bool wasMeasuring = m_measuring;
01931         m_measuring = true;
01932         refreshCache();
01933         if (wasMeasuring)
01934                 gotoMeasurement();
01935         return m_lastResult = XRV_OK;
01936 }
01937 
01938 //////////////////////////////////////////////////////////////////////////////////////////
01939 //! Perform an orientation reset on a device.
01940 XsensResultValue Cmt3::resetOrientation(const CmtResetMethod method, const CmtDeviceId deviceId)
01941 {
01942         CMT3LOG(__FUNCTION__ " %u %08x\n",(uint32_t) method,deviceId);
01943         CMT3EXITLOG;
01944 
01945         DO_DATA_SET(CMT_MID_RESETORIENTATION,CMT_LEN_RESETORIENTATION,Short,(uint16_t) method);
01946         return m_lastResult = XRV_OK;
01947 }
01948 
01949 //////////////////////////////////////////////////////////////////////////////////////////
01950 //! Initiates the XKF3 'no rotation' update procedure.
01951 XsensResultValue Cmt3::setNoRotation(const uint16_t duration, const CmtDeviceId deviceId)
01952 {
01953         CMT3LOG(__FUNCTION__ " %u %08x\n",(uint32_t) duration,deviceId);
01954         CMT3EXITLOG;
01955 
01956         DO_DATA_SET(CMT_MID_SETNOROTATION,CMT_LEN_SETNOROTATION,Short,(uint16_t) duration);
01957         return m_lastResult = XRV_OK;
01958 }
01959 
01960 //////////////////////////////////////////////////////////////////////////////////////////
01961 //! Restore the factory defaults of a device.
01962 XsensResultValue Cmt3::restoreFactoryDefaults(const CmtDeviceId deviceId)
01963 {
01964         CMT3LOG(__FUNCTION__ " %08x\n",deviceId);
01965         CMT3EXITLOG;
01966 
01967         DO_DATA_REQUEST(CMT_MID_RESTOREFACTORYDEF);
01968         return m_lastResult = XRV_OK;
01969 }
01970 
01971 #if 0
01972 obsolete:
01973 //////////////////////////////////////////////////////////////////////////////////////////
01974 // Set the state (enabled/disabled) of the AMD algorithm
01975 XsensResultValue Cmt3::setAmdState (const bool state, const CmtDeviceId deviceId)
01976 {
01977         CMT3LOG(__FUNCTION__ " %u %08x\n",state?1:0,deviceId);
01978         CMT3EXITLOG;
01979 
01980         uint16_t dat = ((state)?1:0);
01981         DO_DATA_SET(CMT_MID_REQAMD,CMT_LEN_AMD,Short,dat);
01982         return m_lastResult = XRV_OK;
01983 }
01984 #endif
01985 
01986 //////////////////////////////////////////////////////////////////////////////////////////
01987 // Set the baudrate and reconnect at the new baudrate if successful.
01988 XsensResultValue Cmt3::setBaudrate(const uint32_t baudrate, bool reconnect)
01989 {
01990         CMT3LOG(__FUNCTION__ " %u %u\n",baudrate,reconnect?1:0);
01991         CMT3EXITLOG;
01992 
01993         uint8_t tmp;
01994         switch (baudrate)
01995         {
01996         case CMT_BAUD_RATE_4800:
01997                 tmp = CMT_BAUDCODE_4K8;
01998                 break;
01999         case CMT_BAUD_RATE_9600:
02000                 tmp = CMT_BAUDCODE_9K6;
02001                 break;
02002 //        case CMT_BAUD_RATE_14K4:
02003 //                tmp = CMT_BAUDCODE_14K4;
02004 //                break;
02005         case CMT_BAUD_RATE_19K2:
02006                 tmp = CMT_BAUDCODE_19K2;
02007                 break;
02008 //        case CMT_BAUD_RATE_28K8:
02009 //                tmp = CMT_BAUDCODE_28K8;
02010 //                break;
02011         case CMT_BAUD_RATE_38K4:
02012                 tmp = CMT_BAUDCODE_38K4;
02013                 break;
02014         case CMT_BAUD_RATE_57K6:
02015                 tmp = CMT_BAUDCODE_57K6;
02016                 break;
02017 //        case CMT_BAUD_RATE_76K8:
02018 //                tmp = CMT_BAUDCODE_76K8;
02019 //                break;
02020         case CMT_BAUD_RATE_115K2:
02021                 tmp = CMT_BAUDCODE_115K2;
02022                 break;
02023         case CMT_BAUD_RATE_230K4:
02024                 tmp = CMT_BAUDCODE_230K4;
02025                 break;
02026         case CMT_BAUD_RATE_460K8:
02027                 tmp = CMT_BAUDCODE_460K8;
02028                 break;
02029         case CMT_BAUD_RATE_921K6:
02030                 tmp = CMT_BAUDCODE_921K6;
02031                 break;
02032         default:
02033                 return m_lastResult = XRV_BAUDRATEINVALID;
02034         }
02035 
02036         if (baudrate != m_baudrate) {
02037                 DO_DATA_SET_BID(CMT_MID_REQBAUDRATE,CMT_LEN_BAUDRATE,Byte,tmp,CMT_BID_MASTER);
02038 
02039                 if (reconnect)
02040                 {
02041                         CMT3LOG(__FUNCTION__ " sending reset for reconnect\n");
02042                         // Reset devices on this port and reopen port @ new baudrate
02043                         Message sndReset(CMT_MID_RESET,0);
02044                         Message rcvReset;
02045                         sndReset.setBusId(CMT_BID_MASTER);
02046                         m_serial.writeMessage(&sndReset);
02047                         m_lastResult = m_serial.waitForMessage(&rcvReset,CMT_MID_RESETACK,0,false);
02048                         if (m_lastResult != XRV_OK)
02049                                 return m_lastResult;
02050                         if (m_logging)
02051                                 m_logFile.writeMessage(&rcvReset);
02052                         bool wasMeasuring = m_measuring;
02053                         m_measuring = true;
02054 
02055                         // XM needs time to reboot
02056                         if (isXm())
02057                                 msleep(750);
02058 
02059                         CMT3LOG(__FUNCTION__ " reopening port at new baud rate\n");
02060 #ifdef _WIN32
02061                         int32_t port;
02062                         m_serial.getPortNr(port);
02063                         closePort(false);
02064                         m_lastResult = openPort(port, baudrate, m_readBufSize, m_writeBufSize);
02065 #else
02066                         char portname[32];
02067                         m_serial.getPortName(portname);
02068                         closePort(false);
02069                         m_lastResult = openPort(portname, baudrate, m_readBufSize, m_writeBufSize);
02070 #endif
02071                         if (m_lastResult != XRV_OK)
02072                                 return m_lastResult;
02073                         if (wasMeasuring)
02074                                 gotoMeasurement();
02075                         return m_lastResult;
02076                 }
02077         }
02078 
02079         return m_lastResult = XRV_OK;
02080 }
02081 
02082 //////////////////////////////////////////////////////////////////////////////////////////
02083 // Set the state of the bluetooth communication to on (true) or off (false)
02084 XsensResultValue Cmt3::setBluetoothState(const bool enabled)
02085 {
02086         CMT3LOG(__FUNCTION__ " %u\n",enabled?1:0);
02087         CMT3EXITLOG;
02088 
02089         uint8_t dat = (enabled?0:1);
02090         DO_DATA_SET_BID(CMT_MID_REQBTDISABLE,CMT_LEN_BTDISABLE,Byte,dat,CMT_BID_MASTER);
02091         return m_lastResult = XRV_OK;
02092 }
02093 
02094 //////////////////////////////////////////////////////////////////////////////////////////
02095 // Switch the XM bus power on or off.
02096 XsensResultValue Cmt3::setBusPowerState(const bool enabled)
02097 {
02098         CMT3LOG(__FUNCTION__ " %u\n",enabled?1:0);
02099         CMT3EXITLOG;
02100 
02101         uint16_t tmp = enabled?1:0;
02102         DO_DATA_SET_BID(CMT_MID_BUSPWR,CMT_LEN_BUSPWR,Short,tmp,CMT_BID_MASTER);
02103         return m_lastResult = XRV_OK;
02104 }
02105 
02106 //////////////////////////////////////////////////////////////////////////////////////////
02107 // Set the complete device output mode of a device.
02108 XsensResultValue Cmt3::setDeviceMode (const CmtDeviceMode& mode, bool force, const CmtDeviceId deviceId)
02109 {
02110         CMT3LOG(__FUNCTION__ " %04x %08x %u %u %08x\n",(uint32_t) mode.m_outputMode,(uint32_t) mode.m_outputSettings,(uint32_t) mode.m_sampleFrequency,force?1:0,deviceId);
02111         CMT3EXITLOG;
02112 
02113         CmtDeviceMode2 mode2;
02114         mode2.m_outputMode = mode.m_outputMode;
02115         mode2.m_outputSettings = mode.m_outputSettings;
02116         mode2.setSampleFrequency(mode.m_sampleFrequency);
02117 
02118         return setDeviceMode2(mode2, force, deviceId);
02119 }
02120 
02121 //////////////////////////////////////////////////////////////////////////////////////////
02122 // Set the complete device output mode of a device.
02123 XsensResultValue Cmt3::setDeviceMode2 (const CmtDeviceMode2& mode, bool force, const CmtDeviceId deviceId)
02124 {
02125         CMT3LOG(__FUNCTION__ " %04x %08x %u %u %u %08x\n",(uint32_t) mode.m_outputMode,(uint32_t) mode.m_outputSettings,(uint32_t) mode.m_period, (uint32_t)mode.m_skip,force?1:0,deviceId);
02126         CMT3EXITLOG;
02127 
02128         Message snd;
02129         Message rcv;
02130         uint8_t bid = getBusIdInternal(deviceId);
02131         if (bid == CMT_BID_INVALID)
02132                 return (m_lastResult = XRV_INVALIDID);
02133 
02134         //uint16_t period, skip;
02135         //mode.getPeriodAndSkipFactor(period, skip);
02136         uint16_t xperiod;
02137         //bool changed = false;
02138 
02139         bool xm = isXm();
02140         if (bid == CMT_BID_BROADCAST)
02141         {
02142                 if (xm)
02143                 {
02144                         // set the device modes of all connected devices first
02145                         for (uint16_t i = 0; i < m_config.m_numberOfDevices;++i)
02146                                 if ((m_lastResult = setDeviceMode2(mode,force,m_config.m_deviceInfo[i].m_deviceId)) != XRV_OK)
02147                                         return m_lastResult;
02148                 }
02149                 bid = CMT_BID_MASTER;
02150         }
02151 
02152         // set sample frequency if this is an Xbus Master or the primary device
02153         if (deviceId == CMT_DID_BROADCAST || deviceId == m_config.m_masterDeviceId)
02154         {
02155                 m_period = mode.m_period;
02156                 m_skip = mode.m_skip;
02157                 //m_sampleFrequency = (double) mode.getSampleFrequency();
02158                 snd.setBusId(CMT_BID_MASTER);
02159                 if (xm)
02160                         xperiod = mode.m_period * (mode.m_skip + 1);
02161                 else
02162                         xperiod = mode.m_period;
02163 
02164                 if (force || m_config.m_samplingPeriod != xperiod)
02165                 {
02166                         CMT3LOG(__FUNCTION__ " setting device %08x period to %u\n",deviceId,(uint32_t) xperiod);
02167                         //changed = true;
02168                         snd.setDataShort(xperiod);
02169                         snd.setMessageId(CMT_MID_REQPERIOD);
02170                         m_serial.writeMessage(&snd);
02171                         m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_REQPERIODACK,0,true);
02172                         if (m_lastResult != XRV_OK)
02173                                 return m_lastResult;
02174                         if (m_logging)
02175                                 m_logFile.writeMessage(&rcv);
02176                         HANDLE_ERR_RESULT;
02177                         m_config.m_samplingPeriod = xperiod;        // update device info
02178                 }
02179 
02180                 if (!xm && (force || m_config.m_outputSkipFactor != mode.m_skip))
02181                 {
02182                         CMT3LOG(__FUNCTION__ " setting MT %08x skip factor to %u\n",deviceId,(uint32_t) mode.m_skip);
02183                         //changed = true;
02184                         snd.setDataShort(mode.m_skip);
02185                         snd.setMessageId(CMT_MID_REQOUTPUTSKIPFACTOR);
02186                         m_serial.writeMessage(&snd);
02187                         m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_REQOUTPUTSKIPFACTORACK,0,true);
02188                         if (m_lastResult != XRV_OK)
02189                                 return m_lastResult;
02190                         if (m_logging)
02191                                 m_logFile.writeMessage(&rcv);
02192                         HANDLE_ERR_RESULT;
02193                         m_config.m_outputSkipFactor = mode.m_skip;        // update device info
02194                 }
02195         }
02196 
02197         // set the output mode and settings if this device is not an Xbus Master
02198         if ((deviceId & CMT_DID_TYPEH_MASK) != CMT_DID_TYPEH_XM)
02199         {
02200                 if (bid == CMT_BID_BROADCAST || bid == CMT_BID_MASTER) {
02201                         snd.setBusId(CMT_BID_MASTER);
02202                         bid = 1;
02203                 }
02204                 else
02205                         snd.setBusId(bid);
02206 
02207                 if (force || m_config.m_deviceInfo[bid-1].m_outputMode != (uint16_t) mode.m_outputMode)
02208                 {
02209                         CMT3LOG(__FUNCTION__ " setting MT %08x output mode to %04X\n",deviceId,(uint32_t) mode.m_outputMode);
02210                         //changed = true;
02211                         snd.resizeData(2);
02212                         snd.setMessageId(CMT_MID_REQOUTPUTMODE);
02213                         snd.setDataShort((uint16_t) mode.m_outputMode);
02214                         m_serial.writeMessage(&snd);
02215                         m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_REQOUTPUTMODEACK,0,true);
02216                         if (m_lastResult != XRV_OK)
02217                                 return m_lastResult;
02218                         if (m_logging)
02219                                 m_logFile.writeMessage(&rcv);
02220                         HANDLE_ERR_RESULT;
02221                         m_config.m_deviceInfo[bid-1].m_outputMode = (uint16_t) mode.m_outputMode;        // update device info
02222                 }
02223 
02224                 uint32_t settings = (uint32_t) mode.m_outputSettings;
02225                 if (xm)
02226                         settings &= ~(uint32_t) CMT_OUTPUTSETTINGS_TIMESTAMP_MASK;
02227 
02228                 if (force || m_config.m_deviceInfo[bid-1].m_outputSettings != settings)
02229                 {
02230                         CMT3LOG(__FUNCTION__ " setting MT %08x output settings to %08X\n",deviceId,(uint32_t) settings);
02231                         //changed = true;
02232                         snd.setMessageId(CMT_MID_REQOUTPUTSETTINGS);
02233                         snd.setDataLong(settings);
02234                         m_serial.writeMessage(&snd);
02235                         m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_REQOUTPUTSETTINGSACK,0,true);
02236                         if (m_lastResult != XRV_OK)
02237                                 return m_lastResult;
02238                         if (m_logging)
02239                                 m_logFile.writeMessage(&rcv);
02240                         HANDLE_ERR_RESULT;
02241                         m_config.m_deviceInfo[bid-1].m_outputSettings = settings;        // update device info
02242                 }
02243         }
02244 
02245         return m_lastResult = XRV_OK;
02246 }
02247 
02248 //////////////////////////////////////////////////////////////////////////////////////////
02249 // Retrieve the error mode
02250 XsensResultValue Cmt3::setErrorMode(const uint16_t mode)
02251 {
02252         CMT3LOG(__FUNCTION__ " %u\n",(uint32_t) mode);
02253         CMT3EXITLOG;
02254 
02255         uint8_t mid;
02256         if (isXm())
02257                 mid = CMT_MID_REQXMERRORMODE;
02258         else
02259                 mid = CMT_MID_REQERRORMODE;
02260 
02261         DO_DATA_SET_BID(mid,CMT_LEN_ERRORMODE,Short,mode,CMT_BID_MASTER);
02262         return m_lastResult = XRV_OK;
02263 }
02264 
02265 //////////////////////////////////////////////////////////////////////////////////////////
02266 // Set the number of times the gotoConfig function will attempt gotoConfig before failing
02267 XsensResultValue Cmt3::setGotoConfigTries(const uint16_t tries)
02268 {
02269         CMT3LOG(__FUNCTION__ " %u\n",(uint32_t) tries);
02270         CMT3EXITLOG;
02271 
02272         m_gotoConfigTries = tries;
02273         return m_lastResult = XRV_OK;
02274 }
02275 
02276 //////////////////////////////////////////////////////////////////////////////////////////
02277 // Set the heading offset of a device. The valid range is -pi to +pi.
02278 XsensResultValue Cmt3::setHeading (const double heading, const CmtDeviceId deviceId)
02279 {
02280         CMT3LOG(__FUNCTION__ " %f %08x\n",heading,deviceId);
02281         CMT3EXITLOG;
02282 
02283         DO_DATA_SET(CMT_MID_REQHEADING,CMT_LEN_HEADING,Float,(float) heading);
02284         return m_lastResult = XRV_OK;
02285 }
02286 
02287 //////////////////////////////////////////////////////////////////////////////////////////
02288 // Sets the current position of the sensor
02289 XsensResultValue Cmt3::setLatLonAlt(const CmtVector& lla, const CmtDeviceId deviceId)
02290 {
02291         CMT3LOG(__FUNCTION__ " [%f %f %f] %08x\n",lla.m_data[0],lla.m_data[1],lla.m_data[2],deviceId);
02292         CMT3EXITLOG;
02293 
02294         uint8_t bid = getBusIdInternal(deviceId);
02295         if (bid == CMT_BID_INVALID || bid == CMT_BID_BROADCAST)
02296                 return (m_lastResult = XRV_INVALIDID);
02297 
02298         Message        snd(CMT_MID_SETLATLONALT,CMT_LEN_LATLONALT);
02299         Message rcv;
02300         snd.setDataFloat((float) lla.m_data[0],0);
02301         snd.setDataFloat((float) lla.m_data[1],4);
02302         snd.setDataFloat((float) lla.m_data[2],8);
02303 
02304         snd.setBusId(bid);
02305         m_serial.writeMessage(&snd);
02306         m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_SETLATLONALTACK,0,true);
02307         if (m_lastResult != XRV_OK)
02308                 return m_lastResult;
02309         if (m_logging)
02310                 m_logFile.writeMessage(&rcv);
02311         if (rcv.getMessageId() == CMT_MID_ERROR)
02312         {
02313                 m_lastHwErrorDeviceId = m_config.m_masterDeviceId;
02314                 if (rcv.getDataSize() >= 2)
02315                 {
02316                         uint8_t biddy = rcv.getDataByte(1);
02317                         getDeviceId(biddy,m_lastHwErrorDeviceId);
02318                 }
02319                 return m_lastResult = m_lastHwError = (XsensResultValue) rcv.getDataByte();
02320         }
02321         return m_lastResult = XRV_OK;
02322 }
02323 
02324 //////////////////////////////////////////////////////////////////////////////////////////
02325 // Set the location ID of a device. The buffer should be no more than 20 bytes.
02326 XsensResultValue Cmt3::setLocationId (uint16_t locationId, const CmtDeviceId deviceId)
02327 {
02328         CMT3LOG(__FUNCTION__ " %u %08x\n",(uint32_t) locationId,deviceId);
02329         CMT3EXITLOG;
02330 
02331         DO_DATA_SET(CMT_MID_REQLOCATIONID,CMT_LEN_LOCATIONID,Short,locationId);
02332         return m_lastResult = XRV_OK;
02333 }
02334 
02335 //////////////////////////////////////////////////////////////////////////////////////////
02336 // Set the magnetic declination offset of a device. The valid range is -pi to +pi.
02337 XsensResultValue Cmt3::setMagneticDeclination(const double declination, const CmtDeviceId deviceId)
02338 {
02339         CMT3LOG(__FUNCTION__ " %f %08x\n",declination,deviceId);
02340         CMT3EXITLOG;
02341 
02342         DO_DATA_SET(CMT_MID_SETMAGNETICDECLINATION,CMT_LEN_MAGNETICDECLINATION,Float,(float) declination);
02343         return m_lastResult = XRV_OK;
02344 }
02345 
02346 XsensResultValue Cmt3::setObjectAlignmentMatrix(const CmtMatrix& matrix, const CmtDeviceId deviceId)
02347 {
02348         CMT3LOG(__FUNCTION__ " [[%f %f %f][%f %f %f][%f %f %f]] %08x\n",
02349                 matrix.m_data[0][0],matrix.m_data[0][1],matrix.m_data[0][2],
02350                 matrix.m_data[1][0],matrix.m_data[1][1],matrix.m_data[1][2],
02351                 matrix.m_data[2][0],matrix.m_data[2][1],matrix.m_data[2][2],deviceId);
02352         CMT3EXITLOG;
02353 
02354         uint8_t bid = getBusIdInternal(deviceId);
02355         if (bid == CMT_BID_INVALID || bid == CMT_BID_BROADCAST)
02356                 return (m_lastResult = XRV_INVALIDID);
02357 
02358         Message        snd(CMT_MID_SETOBJECTALIGNMENT,CMT_LEN_OBJECTALIGNMENT);
02359         Message rcv;
02360         snd.setDataFloat((float) matrix.m_data[0][0],0);
02361         snd.setDataFloat((float) matrix.m_data[0][1],4);
02362         snd.setDataFloat((float) matrix.m_data[0][2],8);
02363         snd.setDataFloat((float) matrix.m_data[1][0],12);
02364         snd.setDataFloat((float) matrix.m_data[1][1],16);
02365         snd.setDataFloat((float) matrix.m_data[1][2],20);
02366         snd.setDataFloat((float) matrix.m_data[2][0],24);
02367         snd.setDataFloat((float) matrix.m_data[2][1],28);
02368         snd.setDataFloat((float) matrix.m_data[2][2],32);
02369 
02370         snd.setBusId(bid);
02371         m_serial.writeMessage(&snd);
02372         m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_SETOBJECTALIGNMENTACK,0,true);
02373         if (m_lastResult != XRV_OK)
02374                 return m_lastResult;
02375         if (m_logging)
02376                 m_logFile.writeMessage(&rcv);
02377         if (rcv.getMessageId() == CMT_MID_ERROR)
02378         {
02379                 m_lastHwErrorDeviceId = m_config.m_masterDeviceId;
02380                 if (rcv.getDataSize() >= 2)
02381                 {
02382                         uint8_t biddy = rcv.getDataByte(1);
02383                         getDeviceId(biddy,m_lastHwErrorDeviceId);
02384                 }
02385                 return m_lastResult = m_lastHwError = (XsensResultValue) rcv.getDataByte();
02386         }
02387         return m_lastResult = XRV_OK;
02388 }
02389 
02390 //////////////////////////////////////////////////////////////////////////////////////////
02391 // Set the processing flags of a device.
02392 XsensResultValue Cmt3::setProcessingFlags (uint16_t processingFlags, const CmtDeviceId deviceId)
02393 {
02394         CMT3LOG(__FUNCTION__ " %u %08x\n",(uint32_t) processingFlags,deviceId);
02395         CMT3EXITLOG;
02396 
02397         DO_DATA_SET(CMT_MID_REQPROCESSINGFLAGS,CMT_LEN_PROCESSINGFLAGS,Short,processingFlags);
02398         return m_lastResult = XRV_OK;
02399 }
02400 
02401 //////////////////////////////////////////////////////////////////////////////////////////
02402 // Switch the XM off
02403 XsensResultValue Cmt3::setXmPowerOff(void)
02404 {
02405         CMT3LOG(__FUNCTION__ "\n");
02406         CMT3EXITLOG;
02407 
02408         if (!isXm())
02409                 return m_lastResult = XRV_INVALIDOPERATION;
02410         Message snd(CMT_MID_XMPWROFF,0);
02411         snd.setBusId(CMT_BID_MASTER);
02412         return m_lastResult = m_serial.writeMessage(&snd);
02413 }
02414 
02415 //////////////////////////////////////////////////////////////////////////////////////////
02416 // Set the inbound synchronization settings of a device.
02417 XsensResultValue Cmt3::setSyncInSettings (const CmtSyncInSettings& settings)
02418 {
02419         CMT3LOG(__FUNCTION__ " %u %u %u\n",(uint32_t) settings.m_mode,settings.m_offset,(uint32_t) settings.m_skipFactor);
02420         CMT3EXITLOG;
02421 
02422         if (isXm())
02423                 return m_lastResult = XRV_INVALIDOPERATION;
02424 
02425         Message snd(CMT_MID_REQSYNCINSETTINGS,3);
02426         Message rcv;
02427 
02428         snd.setBusId(CMT_BID_MASTER);
02429 
02430         snd.setDataByte(CMT_PARAM_SYNCIN_MODE);
02431         snd.setDataShort(settings.m_mode,1);
02432         m_serial.writeMessage(&snd);
02433         m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_REQSYNCINSETTINGSACK,0,true);
02434         if (m_lastResult != XRV_OK)
02435                 return m_lastResult;
02436         if (m_logging)
02437                 m_logFile.writeMessage(&rcv);
02438         HANDLE_ERR_RESULT;
02439 
02440         snd.setDataByte(CMT_PARAM_SYNCIN_SKIPFACTOR);
02441         snd.setDataShort(settings.m_skipFactor,1);
02442         m_serial.writeMessage(&snd);
02443         m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_REQSYNCINSETTINGSACK,0,true);
02444         if (m_lastResult != XRV_OK)
02445                 return m_lastResult;
02446         if (m_logging)
02447                 m_logFile.writeMessage(&rcv);
02448         HANDLE_ERR_RESULT;
02449 
02450         snd.setDataByte(CMT_PARAM_SYNCIN_OFFSET);
02451         snd.setDataLong((uint32_t) (((double) settings.m_offset)
02452                 * CMT_SYNC_CLOCK_NS_TO_TICKS + 0.5),1);
02453         m_serial.writeMessage(&snd);
02454         m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_REQSYNCINSETTINGSACK,0,true);
02455         if (m_lastResult == XRV_OK)
02456         {
02457                 if (m_logging)
02458                         m_logFile.writeMessage(&rcv);
02459                 HANDLE_ERR_RESULT;
02460         }
02461 
02462         return m_lastResult;
02463 }
02464 
02465 //////////////////////////////////////////////////////////////////////////////////////////
02466 // Set the inbound synchronization mode of a device.
02467 XsensResultValue Cmt3::setSyncInMode (const uint16_t mode)
02468 {
02469         CMT3LOG(__FUNCTION__ " %u\n",(uint32_t) mode);
02470         CMT3EXITLOG;
02471 
02472         if (isXm())
02473                 return m_lastResult = XRV_INVALIDOPERATION;
02474 
02475         Message snd(CMT_MID_REQSYNCINSETTINGS,3);
02476         Message rcv;
02477 
02478         snd.setBusId(CMT_BID_MASTER);
02479 
02480         snd.setDataByte(CMT_PARAM_SYNCIN_MODE);
02481         snd.setDataShort(mode,1);
02482         m_serial.writeMessage(&snd);
02483         m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_REQSYNCINSETTINGSACK,0,true);
02484         if (m_lastResult != XRV_OK)
02485                 return m_lastResult;
02486         if (m_logging)
02487                 m_logFile.writeMessage(&rcv);
02488         HANDLE_ERR_RESULT;
02489 
02490         return m_lastResult;
02491 }
02492 
02493 //////////////////////////////////////////////////////////////////////////////////////////
02494 // Set the inbound synchronization skip factor of a device.
02495 XsensResultValue Cmt3::setSyncInSkipFactor (const uint16_t skipFactor)
02496 {
02497         CMT3LOG(__FUNCTION__ " %u\n",skipFactor);
02498         CMT3EXITLOG;
02499 
02500         if (isXm())
02501                 return m_lastResult = XRV_INVALIDOPERATION;
02502 
02503         Message snd(CMT_MID_REQSYNCINSETTINGS,3);
02504         Message rcv;
02505 
02506         snd.setBusId(CMT_BID_MASTER);
02507 
02508         snd.setDataByte(CMT_PARAM_SYNCIN_SKIPFACTOR);
02509         snd.setDataShort(skipFactor,1);
02510         m_serial.writeMessage(&snd);
02511         m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_REQSYNCINSETTINGSACK,0,true);
02512         if (m_lastResult != XRV_OK)
02513                 return m_lastResult;
02514         if (m_logging)
02515                 m_logFile.writeMessage(&rcv);
02516         HANDLE_ERR_RESULT;
02517 
02518         return m_lastResult;
02519 }
02520 
02521 //////////////////////////////////////////////////////////////////////////////////////////
02522 // Set the inbound synchronization offset of a device.
02523 XsensResultValue Cmt3::setSyncInOffset (const uint32_t offset)
02524 {
02525         CMT3LOG(__FUNCTION__ " %u\n",offset);
02526         CMT3EXITLOG;
02527 
02528         if (isXm())
02529                 return m_lastResult = XRV_INVALIDOPERATION;
02530 
02531         Message snd(CMT_MID_REQSYNCINSETTINGS,3);
02532         Message rcv;
02533 
02534         snd.setBusId(CMT_BID_MASTER);
02535 
02536         snd.setDataByte(CMT_PARAM_SYNCIN_OFFSET);
02537         snd.setDataLong((uint32_t) (((double) offset)
02538                 * CMT_SYNC_CLOCK_NS_TO_TICKS + 0.5),1);
02539         m_serial.writeMessage(&snd);
02540         m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_REQSYNCINSETTINGSACK,0,true);
02541         if (m_lastResult == XRV_OK)
02542         {
02543                 if (m_logging)
02544                         m_logFile.writeMessage(&rcv);
02545                 HANDLE_ERR_RESULT;
02546         }
02547 
02548         return m_lastResult;
02549 }
02550 
02551 
02552 //////////////////////////////////////////////////////////////////////////////////////////
02553 // Set the synchronization mode of the XM
02554 XsensResultValue Cmt3::setSyncMode(const uint8_t mode)
02555 {
02556         CMT3LOG(__FUNCTION__ " %u\n",(uint32_t) mode);
02557         CMT3EXITLOG;
02558 
02559         if (!isXm())
02560                 return m_lastResult = XRV_INVALIDOPERATION;
02561 
02562         DO_DATA_SET_BID(CMT_MID_REQSYNCMODE,CMT_LEN_SYNCMODE,Byte,mode,CMT_BID_MASTER);
02563         return m_lastResult = XRV_OK;
02564 }
02565 
02566 //////////////////////////////////////////////////////////////////////////////////////////
02567 // Set the outbound synchronization settings of a device.
02568 XsensResultValue Cmt3::setSyncOutSettings (const CmtSyncOutSettings& settings)
02569 {
02570         CMT3LOG(__FUNCTION__ " %u %u %u %u\n",(uint32_t) settings.m_mode, settings.m_offset, (uint32_t) settings.m_pulseWidth, (uint32_t) settings.m_skipFactor);
02571         CMT3EXITLOG;
02572 
02573         if (isXm())
02574                 return m_lastResult = XRV_INVALIDOPERATION;
02575 
02576         Message snd(CMT_MID_REQSYNCOUTSETTINGS,3);
02577         Message rcv;
02578 
02579         snd.setBusId(CMT_BID_MASTER);
02580 
02581         snd.setDataByte(CMT_PARAM_SYNCOUT_MODE);
02582         snd.setDataShort(settings.m_mode,1);
02583         m_serial.writeMessage(&snd);
02584         m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_REQSYNCOUTSETTINGSACK,0,true);
02585         if (m_lastResult != XRV_OK)
02586                 return m_lastResult;
02587         if (m_logging)
02588                 m_logFile.writeMessage(&rcv);
02589         HANDLE_ERR_RESULT;
02590 
02591         snd.setDataByte(CMT_PARAM_SYNCOUT_SKIPFACTOR);
02592         snd.setDataShort(settings.m_skipFactor,1);
02593         m_serial.writeMessage(&snd);
02594         m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_REQSYNCOUTSETTINGSACK,0,true);
02595         if (m_lastResult != XRV_OK)
02596                 return m_lastResult;
02597         if (m_logging)
02598                 m_logFile.writeMessage(&rcv);
02599         HANDLE_ERR_RESULT;
02600 
02601         snd.setDataByte(CMT_PARAM_SYNCOUT_OFFSET);
02602         snd.setDataLong((uint32_t) (((double) settings.m_offset)
02603                                                                 * CMT_SYNC_CLOCK_NS_TO_TICKS + 0.5),1);
02604         m_serial.writeMessage(&snd);
02605         m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_REQSYNCOUTSETTINGSACK,0,true);
02606         if (m_lastResult != XRV_OK)
02607                 return m_lastResult;
02608         if (m_logging)
02609                 m_logFile.writeMessage(&rcv);
02610         HANDLE_ERR_RESULT;
02611 
02612         snd.setDataByte(CMT_PARAM_SYNCOUT_PULSEWIDTH);
02613         snd.setDataLong((uint32_t) (((double) settings.m_pulseWidth)
02614                                                                 * CMT_SYNC_CLOCK_NS_TO_TICKS + 0.5),1);
02615         m_serial.writeMessage(&snd);
02616         m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_REQSYNCOUTSETTINGSACK,0,true);
02617         if (m_lastResult != XRV_OK)
02618                 return m_lastResult;
02619         if (m_logging)
02620                 m_logFile.writeMessage(&rcv);
02621         HANDLE_ERR_RESULT;
02622 
02623         return m_lastResult = XRV_OK;
02624 }
02625 
02626 //////////////////////////////////////////////////////////////////////////////////////////
02627 // Set the outbound synchronization mode of a device.
02628 XsensResultValue Cmt3::setSyncOutMode (const uint16_t mode)
02629 {
02630         CMT3LOG(__FUNCTION__ " %u\n",(uint32_t) mode);
02631         CMT3EXITLOG;
02632 
02633         if (isXm())
02634                 return m_lastResult = XRV_INVALIDOPERATION;
02635 
02636         Message snd(CMT_MID_REQSYNCOUTSETTINGS,3);
02637         Message rcv;
02638 
02639         snd.setBusId(CMT_BID_MASTER);
02640 
02641         snd.setDataByte(CMT_PARAM_SYNCOUT_MODE);
02642         snd.setDataShort(mode,1);
02643         m_serial.writeMessage(&snd);
02644         m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_REQSYNCOUTSETTINGSACK,0,true);
02645         if (m_lastResult != XRV_OK)
02646                 return m_lastResult;
02647         if (m_logging)
02648                 m_logFile.writeMessage(&rcv);
02649         HANDLE_ERR_RESULT;
02650 
02651         return m_lastResult = XRV_OK;
02652 }
02653 
02654 //////////////////////////////////////////////////////////////////////////////////////////
02655 // Set the outbound synchronization pulse width of a device.
02656 XsensResultValue Cmt3::setSyncOutPulseWidth(const uint32_t pulseWidth)
02657 {
02658         CMT3LOG(__FUNCTION__ " %u\n",pulseWidth);
02659         CMT3EXITLOG;
02660 
02661         if (isXm())
02662                 return m_lastResult = XRV_INVALIDOPERATION;
02663 
02664         Message snd(CMT_MID_REQSYNCOUTSETTINGS,3);
02665         Message rcv;
02666 
02667         snd.setBusId(CMT_BID_MASTER);
02668 
02669         snd.setDataByte(CMT_PARAM_SYNCOUT_PULSEWIDTH);
02670         snd.setDataLong((uint32_t) (((double) pulseWidth)
02671                 * CMT_SYNC_CLOCK_NS_TO_TICKS + 0.5),1);
02672         m_serial.writeMessage(&snd);
02673         m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_REQSYNCOUTSETTINGSACK,0,true);
02674         if (m_lastResult != XRV_OK)
02675                 return m_lastResult;
02676         if (m_logging)
02677                 m_logFile.writeMessage(&rcv);
02678         HANDLE_ERR_RESULT;
02679 
02680         return m_lastResult = XRV_OK;
02681 }
02682 
02683 //////////////////////////////////////////////////////////////////////////////////////////
02684 // Set the outbound synchronization skip factor of a device.
02685 XsensResultValue Cmt3::setSyncOutSkipFactor(const uint16_t skipFactor)
02686 {
02687         CMT3LOG(__FUNCTION__ " %u\n",skipFactor);
02688         CMT3EXITLOG;
02689 
02690         if (isXm())
02691                 return m_lastResult = XRV_INVALIDOPERATION;
02692 
02693         Message snd(CMT_MID_REQSYNCOUTSETTINGS,3);
02694         Message rcv;
02695 
02696         snd.setBusId(CMT_BID_MASTER);
02697 
02698         snd.setDataByte(CMT_PARAM_SYNCOUT_SKIPFACTOR);
02699         snd.setDataShort(skipFactor,1);
02700         m_serial.writeMessage(&snd);
02701         m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_REQSYNCOUTSETTINGSACK,0,true);
02702         if (m_lastResult != XRV_OK)
02703                 return m_lastResult;
02704         if (m_logging)
02705                 m_logFile.writeMessage(&rcv);
02706         HANDLE_ERR_RESULT;
02707 
02708         return m_lastResult = XRV_OK;
02709 }
02710 
02711 //////////////////////////////////////////////////////////////////////////////////////////
02712 // Set the outbound synchronization offset of a device.
02713 XsensResultValue Cmt3::setSyncOutOffset(const uint32_t offset)
02714 {
02715         CMT3LOG(__FUNCTION__ " %u\n", offset);
02716         CMT3EXITLOG;
02717 
02718         if (isXm())
02719                 return m_lastResult = XRV_INVALIDOPERATION;
02720 
02721         Message snd(CMT_MID_REQSYNCOUTSETTINGS,3);
02722         Message rcv;
02723 
02724         snd.setBusId(CMT_BID_MASTER);
02725 
02726         snd.setDataByte(CMT_PARAM_SYNCOUT_OFFSET);
02727         snd.setDataLong((uint32_t) (((double) offset)
02728                 * CMT_SYNC_CLOCK_NS_TO_TICKS + 0.5),1);
02729         m_serial.writeMessage(&snd);
02730         m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_REQSYNCOUTSETTINGSACK,0,true);
02731         if (m_lastResult != XRV_OK)
02732                 return m_lastResult;
02733         if (m_logging)
02734                 m_logFile.writeMessage(&rcv);
02735         HANDLE_ERR_RESULT;
02736 
02737         return m_lastResult = XRV_OK;
02738 }
02739 
02740 //////////////////////////////////////////////////////////////////////////////////////////
02741 // Set the configuration mode timeout value in ms.
02742 XsensResultValue Cmt3::setTimeoutConfig(const uint32_t timeout)
02743 {
02744         CMT3LOG(__FUNCTION__ " %u\n",timeout);
02745         CMT3EXITLOG;
02746 
02747         m_timeoutConf = timeout;
02748         if (!m_measuring)
02749                 return m_lastResult = m_serial.setTimeout(m_timeoutConf);
02750         return m_lastResult = XRV_OK;
02751 }
02752 
02753 //////////////////////////////////////////////////////////////////////////////////////////
02754 // Set the measurement mode timeout value in ms.
02755 XsensResultValue Cmt3::setTimeoutMeasurement(const uint32_t timeout)
02756 {
02757         CMT3LOG(__FUNCTION__ " %u\n",timeout);
02758         CMT3EXITLOG;
02759 
02760         m_timeoutMeas = timeout;
02761         if (m_measuring)
02762                 return m_lastResult = m_serial.setTimeout(m_timeoutMeas);
02763         return m_lastResult = XRV_OK;
02764 }
02765 
02766 //////////////////////////////////////////////////////////////////////////////////////////
02767 // Retrieve the transmission delay
02768 XsensResultValue Cmt3::setTransmissionDelay(const uint16_t delay)
02769 {
02770         CMT3LOG(__FUNCTION__ " %u\n",(uint32_t) delay);
02771         CMT3EXITLOG;
02772 
02773         if (isXm())
02774                 return m_lastResult = XRV_INVALIDOPERATION;
02775 
02776         DO_DATA_SET_BID(CMT_MID_SETTRANSMITDELAY,CMT_LEN_TRANSMITDELAY,Short,delay,CMT_BID_MASTER);
02777         return m_lastResult = XRV_OK;
02778 }
02779 
02780 //////////////////////////////////////////////////////////////////////////////////////////
02781 // Set the dual-mode output settings of the XM
02782 XsensResultValue Cmt3::setXmOutputMode(const uint8_t mode)
02783 {
02784         CMT3LOG(__FUNCTION__ " %u\n",(uint32_t) mode);
02785         CMT3EXITLOG;
02786 
02787         if (!isXm())
02788                 return m_lastResult = XRV_INVALIDOPERATION;
02789 
02790         DO_DATA_SET_BID(CMT_MID_REQOPMODE,CMT_LEN_OPMODE,Byte,mode,CMT_BID_MASTER);
02791         return m_lastResult = XRV_OK;
02792 }
02793 
02794 //////////////////////////////////////////////////////////////////////////////////////////
02795 XsensResultValue Cmt3::refreshCache(const bool file)
02796 {
02797         CMT3LOG(__FUNCTION__ " %d\n",file?1:0);
02798         CMT3EXITLOG;
02799 
02800         if (m_serial.isOpen() && (!file || !m_logFile.isOpen()))
02801         {
02802                 // clear eMts cache
02803                 for (uint32_t i = 0; i < CMT_MAX_DEVICES_PER_PORT; ++i)
02804                 {
02805                         CHKFREENUL(m_eMtsData[i]);
02806                 }
02807 
02808                 // port open, go to configuration mode
02809                 if (m_measuring && gotoConfig() != XRV_OK)
02810                         return m_lastResult;                // m_lastResult is already set by gotoConfig()
02811 
02812                 // now in configuration mode, read device information
02813                 CMT3LOG(__FUNCTION__ " Device in configuration mode, reading device information\n");
02814 
02815                 Message snd;
02816                 Message rcv;
02817 
02818                 // all information is in the Configuration message
02819                 snd.setMessageId(CMT_MID_REQCONFIGURATION);
02820                 m_serial.writeMessage(&snd);
02821                 if ((m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_CONFIGURATION,0,false)) != XRV_OK)
02822                         return m_lastResult;
02823                 if (m_logging)
02824                         m_logFile.writeMessage(&rcv);
02825                 m_config.readFromMessage(rcv.getMessageStart());
02826 
02827         if (m_config.m_numberOfDevices > CMT_MAX_DEVICES_PER_PORT)
02828             return m_lastResult = XRV_CONFIGCHECKFAIL;
02829 
02830                 m_period = m_config.m_samplingPeriod;
02831                 m_skip = m_config.m_outputSkipFactor;
02832 
02833                 return m_lastResult = XRV_OK;
02834         }
02835         else if (m_logFile.isOpen() && (file || !m_serial.isOpen()))
02836         {
02837                 // clear eMts cache
02838                 for (uint32_t i = 0; i < CMT_MAX_DEVICES_PER_PORT; ++i)
02839                 {
02840                         CHKFREENUL(m_eMtsData[i]);
02841                 }
02842 
02843                 // now in configuration mode, read device information
02844                 CMT3LOG(__FUNCTION__ " Reading device configuration information from file\n");
02845 
02846                 Message rcv;
02847 
02848                 if ((m_lastResult = m_logFile.readMessage(&rcv,CMT_MID_CONFIGURATION)) != XRV_OK)
02849                         return m_lastResult;
02850                 m_config.readFromMessage(rcv.getMessageStart());
02851 
02852                 m_period = m_config.m_samplingPeriod;
02853                 m_skip = m_config.m_outputSkipFactor;
02854                 //CmtDeviceMode2 mode;
02855                 //mode.setPeriodAndSkipFactor(m_config.m_samplingPeriod,m_config.m_outputSkipFactor);
02856                 //mode.m_period = m_config.m_samplingPeriod;
02857                 //mode.m_skip = m_config.m_outputSkipFactor;
02858                 //m_sampleFrequency = mode.getRealSampleFrequency();
02859 
02860                 return m_lastResult = XRV_OK;
02861         }
02862         else
02863                 return m_lastResult = XRV_NOFILEORPORTOPEN;
02864 }
02865 
02866 //////////////////////////////////////////////////////////////////////////////////////////
02867 //        Wait for a data message to arrive.
02868 XsensResultValue Cmt3::waitForDataMessage(Packet* pack)
02869 {
02870         CMT3LOGDAT(__FUNCTION__ " %p\n",pack);
02871         CMT3EXITLOGDAT;
02872 
02873         m_lastResult = XRV_TIMEOUTNODATA;
02874 
02875         uint32_t toEnd = (getTimeOfDay() + (uint32_t) m_timeoutMeas) % (XSENS_MS_PER_DAY);
02876         while(toEnd >= getTimeOfDay())
02877         {
02878                 m_lastResult = m_serial.waitForMessage(&pack->m_msg,CMT_MID_MTDATA,0,true);
02879                 if (m_lastResult == XRV_OK)
02880                 {
02881                         if (m_logging)
02882                                 m_logFile.writeMessage(&pack->m_msg);
02883                         if (pack->m_msg.getMessageId() == CMT_MID_ERROR)
02884                         {
02885                                 m_lastHwErrorDeviceId = m_config.m_masterDeviceId;
02886                                 if (pack->m_msg.getDataSize() >= 2)
02887                                 {
02888                                         uint8_t biddy = pack->m_msg.getDataByte(1);
02889                                         getDeviceId(biddy,m_lastHwErrorDeviceId);
02890                                 }
02891                                 return m_lastResult = m_lastHwError = (XsensResultValue) pack->m_msg.getDataByte(0);
02892                         }
02893 
02894                         pack->m_itemCount = m_config.m_numberOfDevices;
02895                         for (uint16_t i = 0;i < m_config.m_numberOfDevices;++i)
02896                                 pack->setDataFormat(m_config.m_deviceInfo[i].m_outputMode,m_config.m_deviceInfo[i].m_outputSettings,i);
02897                         pack->m_toa = timeStampNow();
02898                         if (m_useRtc)
02899                                 fillRtc(pack);
02900 
02901                         return m_lastResult = XRV_OK;
02902                 }
02903         }
02904         return m_lastResult;// = XRV_TIMEOUT;
02905 }
02906 
02907 //////////////////////////////////////////////////////////////////////////////////////////
02908 XsensResultValue Cmt3::createLogFile(const char* filename, bool startLogging)
02909 {
02910         CMT3LOG(__FUNCTION__ " \"%s\" %u\n",filename?filename:"",startLogging?1:0);
02911         CMT3EXITLOG;
02912 
02913         if (!m_serial.isOpen())
02914                 return m_lastResult = XRV_NOPORTOPEN;
02915         if (m_logFile.isOpen())
02916                 return m_lastResult = XRV_ALREADYOPEN;
02917         m_lastResult = m_logFile.create(filename);
02918         if (m_lastResult == XRV_OK)
02919         {
02920                 m_logging = true;
02921                 CmtDeviceConfiguration config;
02922                 if (getConfiguration(config) == XRV_OK)
02923                 {
02924                         void* buffer = malloc(CMT_EMTS_SIZE*(m_config.m_numberOfDevices+1));
02925                         getEMtsData(buffer,CMT_DID_BROADCAST);
02926                         free(buffer);
02927                         m_logging = startLogging;
02928                 }
02929         }
02930 
02931         if (m_lastResult != XRV_OK)
02932         {
02933                 m_logFile.closeAndDelete();
02934                 m_logging = false;
02935         }
02936         return m_lastResult;
02937 }
02938 
02939 //////////////////////////////////////////////////////////////////////////////////////////
02940 XsensResultValue Cmt3::createLogFile(const wchar_t* filename, bool startLogging)
02941 {
02942         CMT3LOG(__FUNCTION__ " \"%S\" %u\n",filename?filename:L"",startLogging?1:0);
02943         CMT3EXITLOG;
02944 
02945         if (!m_serial.isOpen())
02946                 return m_lastResult = XRV_NOPORTOPEN;
02947         if (m_logFile.isOpen())
02948                 return m_lastResult = XRV_ALREADYOPEN;
02949         m_lastResult = m_logFile.create(filename);
02950         if (m_lastResult == XRV_OK)
02951         {
02952                 m_logging = true;
02953                 CmtDeviceConfiguration config;
02954                 if (getConfiguration(config) == XRV_OK)
02955                 {
02956                         void* buffer = malloc(CMT_EMTS_SIZE*(m_config.m_numberOfDevices+1));
02957                         getEMtsData(buffer,CMT_DID_BROADCAST);
02958                         free(buffer);
02959                         m_logging = startLogging;
02960                 }
02961         }
02962 
02963         if (m_lastResult != XRV_OK)
02964         {
02965                 m_logFile.closeAndDelete();
02966                 m_logging = false;
02967         }
02968         return m_lastResult;
02969 }
02970 
02971 //////////////////////////////////////////////////////////////////////////////////////////
02972 XsensResultValue Cmt3::closeLogFile(bool del)
02973 {
02974         CMT3LOG(__FUNCTION__ " %u\n",del?1:0);
02975         CMT3EXITLOG;
02976 
02977         m_logging = false;
02978         if (!m_logFile.isOpen())
02979                 return m_lastResult = XRV_NOFILEOPEN;
02980         if (del)
02981                 return m_lastResult = m_logFile.closeAndDelete();
02982         else
02983                 return m_lastResult = m_logFile.close();
02984 }
02985 
02986 //////////////////////////////////////////////////////////////////////////////////////////
02987 bool Cmt3::isLogFileOpen(const char* filename) const
02988 {
02989         CMT3LOG(__FUNCTION__ " \"%s\"\n",filename?filename:"");
02990 
02991         if (m_logFile.isOpen())
02992         {
02993                 if (filename != NULL && filename[0] != 0)
02994                 {
02995                         char fn[CMT_MAX_FILENAME_LENGTH];
02996                         m_logFile.getName(fn);
02997                         if (_strnicmp(filename,fn,CMT_MAX_FILENAME_LENGTH) != 0)
02998                                 return false;
02999                 }
03000                 return true;
03001         }
03002         return false;
03003 }
03004 
03005 //////////////////////////////////////////////////////////////////////////////////////////
03006 bool Cmt3::isLogFileOpen(const wchar_t* filename) const
03007 {
03008         CMT3LOG(__FUNCTION__ " \"%S\"\n",filename?filename:L"");
03009 
03010         if (m_logFile.isOpen())
03011         {
03012                 if (filename != NULL && filename[0] != L'\0')
03013                 {
03014                         wchar_t fn[CMT_MAX_FILENAME_LENGTH];
03015                         m_logFile.getName(fn);
03016                         if (_wcsnicmp(filename,fn,CMT_MAX_FILENAME_LENGTH) != 0)
03017                                 return false;
03018                 }
03019                 return true;
03020         }
03021         return false;
03022 }
03023 
03024 //////////////////////////////////////////////////////////////////////////////////////////
03025 XsensResultValue Cmt3::openLogFile(const char* filename)
03026 {
03027         CMT3LOG(__FUNCTION__ " \"%s\"\n",filename?filename:"");
03028         CMT3EXITLOG;
03029 
03030         m_logging = false;
03031         if (m_serial.isOpen())
03032                 return m_lastResult = XRV_INVALIDOPERATION;
03033         if (m_logFile.isOpen())
03034                 return m_lastResult = XRV_ALREADYOPEN;
03035         m_lastResult = m_logFile.open(filename,true);
03036         if (m_lastResult == XRV_OK)
03037         {
03038                 // check first two bytes of file for FA FF
03039                 uint8_t hdrBuf[2] = { 0,0 };
03040                 m_logFile.getCmt1f()->readData(2,hdrBuf,NULL);
03041                 if (hdrBuf[0] != 0xFA || hdrBuf[1] != 0xFF)
03042                         return m_lastResult = XRV_DATACORRUPT;
03043                 m_logFile.getCmt1f()->setReadPos(0);
03044 
03045                 if (refreshCache() == XRV_OK)
03046                         m_readFromFile = true;
03047                 else
03048                 {
03049                         m_logFile.close();
03050                         m_readFromFile = false;
03051                 }
03052         }
03053         return m_lastResult;
03054 }
03055 
03056 //////////////////////////////////////////////////////////////////////////////////////////
03057 XsensResultValue Cmt3::openLogFile(const wchar_t* filename)
03058 {
03059         CMT3LOG(__FUNCTION__ " \"%S\"\n",filename?filename:L"");
03060         CMT3EXITLOG;
03061 
03062         m_logging = false;
03063         if (m_serial.isOpen())
03064                 return m_lastResult = XRV_INVALIDOPERATION;
03065         if (m_logFile.isOpen())
03066                 return m_lastResult = XRV_ALREADYOPEN;
03067         m_lastResult = m_logFile.open(filename,true);
03068         if (m_lastResult == XRV_OK)
03069         {
03070                 // check first two bytes of file for FA FF
03071                 uint8_t hdrBuf[2] = { 0,0 };
03072                 m_logFile.getCmt1f()->readData(2,hdrBuf,NULL);
03073                 if (hdrBuf[0] != 0xFA || hdrBuf[1] != 0xFF)
03074                         return m_lastResult = XRV_DATACORRUPT;
03075                 m_logFile.getCmt1f()->setReadPos(0);
03076 
03077                 if (refreshCache() == XRV_OK)
03078                         m_readFromFile = true;
03079                 else
03080                 {
03081                         m_logFile.close();
03082                         m_readFromFile = false;
03083                 }
03084         }
03085         return m_lastResult;
03086 }
03087 
03088 //////////////////////////////////////////////////////////////////////////////////////////
03089 XsensResultValue Cmt3::setDataSource(bool readFromFile)
03090 {
03091         CMT3LOG(__FUNCTION__ " %s\n",readFromFile?"file":"port");
03092         CMT3EXITLOG;
03093 
03094         if (readFromFile)
03095         {
03096                 m_logging = false;
03097                 if (m_logFile.isOpen())
03098                 {
03099                         m_readFromFile = true;
03100                         return m_lastResult = XRV_OK;
03101                 }
03102                 m_readFromFile = false;
03103                 return m_lastResult = XRV_INVALIDOPERATION;
03104         }
03105         if (m_serial.isOpen())
03106         {
03107                 m_readFromFile = false;
03108                 return m_lastResult = XRV_OK;
03109         }
03110         if (m_logFile.isOpen())
03111         {
03112                 m_readFromFile = true;
03113                 return m_lastResult = XRV_INVALIDOPERATION;
03114         }
03115         m_readFromFile = false;
03116         return m_lastResult = XRV_NOFILEORPORTOPEN;
03117 }
03118 
03119 //////////////////////////////////////////////////////////////////////////////////////////
03120 XsensResultValue Cmt3::setLogMode(bool active)
03121 {
03122         CMT3LOG(__FUNCTION__ " %u\n",active?1:0);
03123         CMT3EXITLOG;
03124 
03125         if (active && (m_readFromFile || !m_logFile.isOpen()))
03126                 return m_lastResult = XRV_NOFILEOPEN;
03127         m_logging = active;
03128         return m_lastResult = XRV_OK;
03129 }
03130 
03131 //////////////////////////////////////////////////////////////////////////////////////////
03132 XsensResultValue Cmt3::resetLogFileReadPos(void)
03133 {
03134         CMT3LOG(__FUNCTION__ "\n");
03135         CMT3EXITLOG;
03136 
03137         return m_lastResult = m_logFile.setReadPosition(0);
03138 }
03139 
03140 //////////////////////////////////////////////////////////////////////////////////////////
03141 XsensResultValue Cmt3::writeMessageToLogFile(const Message& msg)
03142 {
03143         CMT3LOG(__FUNCTION__ "\n");
03144         CMT3EXITLOG;
03145 
03146         if (!m_logFile.isOpen())
03147                 return m_lastResult = XRV_NOFILEOPEN;
03148 
03149         return m_lastResult = m_logFile.writeMessage(&msg);
03150 }
03151 
03152 //////////////////////////////////////////////////////////////////////////////////////////
03153 XsensResultValue Cmt3::getAvailableScenarios(CmtScenario* scenarios, const CmtDeviceId deviceId)
03154 {
03155         CMT3LOG(__FUNCTION__ " %p %08x\n",scenarios,deviceId);
03156         CMT3EXITLOG;
03157 
03158         DO_DATA_REQUEST(CMT_MID_REQAVAILABLESCENARIOS);
03159 
03160         char tp = 0;
03161         switch (deviceId & CMT_DID_TYPEH_MASK)
03162         {
03163         case CMT_DID_TYPEH_MTIG:
03164                 tp = '6';
03165                 break;
03166         case CMT_DID_TYPEH_MTI_MTX:
03167                 tp = '3';
03168                 break;
03169         }
03170 
03171         for (int i = 0; i < CMT_MAX_SCENARIOS_IN_MT; ++i)
03172         {
03173                 scenarios[i].m_type = rcv.getDataByte(0 + i*(1+1+CMT_LEN_SCENARIOLABEL));
03174                 scenarios[i].m_version = rcv.getDataByte(1 + i*(1+1+CMT_LEN_SCENARIOLABEL));
03175                 memcpy(scenarios[i].m_label,rcv.getDataBuffer(2 + i*(1+1+CMT_LEN_SCENARIOLABEL)),CMT_LEN_SCENARIOLABEL);
03176                 scenarios[i].m_label[CMT_LEN_SCENARIOLABEL] = 0;
03177                 scenarios[i].m_filterType = tp;
03178         }
03179         return m_lastResult = XRV_OK;
03180 }
03181 
03182 XsensResultValue Cmt3::getScenario(uint8_t& scenarioType, uint8_t& scenarioVersion, const CmtDeviceId deviceId)
03183 {
03184         CMT3LOG(__FUNCTION__ " %08x\n",deviceId);
03185         CMT3EXITLOG;
03186 
03187         DO_DATA_REQUEST(CMT_MID_REQSCENARIO);
03188         scenarioType = rcv.getDataByte(1);
03189         scenarioVersion = rcv.getDataByte(0);
03190         return m_lastResult = XRV_OK;
03191 }
03192 
03193 XsensResultValue Cmt3::setScenario(const uint8_t scenarioType, const CmtDeviceId deviceId)
03194 {
03195         CMT3LOG(__FUNCTION__ " %u %08x\n",(uint32_t) scenarioType,deviceId);
03196         CMT3EXITLOG;
03197 
03198         uint8_t bid = getBusIdInternal(deviceId);
03199         if (bid == CMT_BID_INVALID)
03200                 return (m_lastResult = XRV_INVALIDID);
03201 
03202         uint16_t scenario = (uint16_t) scenarioType;
03203         DO_DATA_SET_BID(CMT_MID_SETSCENARIO,CMT_LEN_SETSCENARIO,Short,scenario,bid);
03204 
03205         if (bid == CMT_BID_BROADCAST || bid == CMT_BID_MASTER) {
03206                 bid = 1;
03207         }
03208         m_config.m_deviceInfo[bid-1].m_currentScenario = scenario;        // update device info
03209         //m_config.m_deviceInfo[bid-1].m_currentScenario += (uint16_t)scenarioVersion << 8;
03210 
03211         return m_lastResult = XRV_OK;
03212 }
03213 
03214 XsensResultValue Cmt3::getGravityMagnitude(double& magnitude, const CmtDeviceId deviceId)
03215 {
03216         CMT3LOG(__FUNCTION__ " %08x\n",deviceId);
03217         CMT3EXITLOG;
03218 
03219         DO_DATA_REQUEST(CMT_MID_REQGRAVITYMAGNITUDE);
03220         magnitude = rcv.getDataFloat(0);
03221         return m_lastResult = XRV_OK;
03222 }
03223 
03224 XsensResultValue Cmt3::setGravityMagnitude(const double magnitude, const CmtDeviceId deviceId)
03225 {
03226         CMT3LOG(__FUNCTION__ " %f %08x\n",magnitude,deviceId);
03227         CMT3EXITLOG;
03228 
03229         DO_DATA_SET(CMT_MID_SETGRAVITYMAGNITUDE,CMT_LEN_GRAVITYMAGNITUDE,Float,(float) magnitude);
03230         return m_lastResult = XRV_OK;
03231 }
03232 
03233 XsensResultValue Cmt3::getGpsLeverArm(CmtVector& arm, const CmtDeviceId deviceId)
03234 {
03235         CMT3LOG(__FUNCTION__ " %08x\n",deviceId);
03236         CMT3EXITLOG;
03237 
03238         DO_DATA_REQUEST(CMT_MID_REQGPSLEVERARM);
03239         arm.m_data[0] = rcv.getDataFloat(0);
03240         arm.m_data[1] = rcv.getDataFloat(4);
03241         arm.m_data[2] = rcv.getDataFloat(8);
03242         return m_lastResult = XRV_OK;
03243 }
03244 
03245 XsensResultValue Cmt3::getGpsStatus(CmtGpsStatus& status, const CmtDeviceId deviceId)
03246 {
03247         CMT3LOG(__FUNCTION__ " %08x\n",deviceId);
03248         CMT3EXITLOG;
03249 
03250         DO_DATA_REQUEST(CMT_MID_REQGPSSTATUS);
03251         for (uint16_t i = 0; i < CMT_MAX_SVINFO; ++i)
03252         {
03253                 status.m_svInfo[i].m_id = rcv.getDataByte(i*5+2);
03254                 status.m_svInfo[i].m_navigationStatus = rcv.getDataByte(i*5+3);
03255                 status.m_svInfo[i].m_signalQuality= rcv.getDataByte(i*5+4);
03256                 status.m_svInfo[i].m_signalStrength = rcv.getDataByte(i*5+5);
03257         }
03258         return m_lastResult = XRV_OK;
03259 }
03260 
03261 XsensResultValue Cmt3::setGpsLeverArm(const CmtVector& arm, const CmtDeviceId deviceId)
03262 {
03263         CMT3LOG(__FUNCTION__ " [%f %f %f] %08x\n",arm.m_data[0],arm.m_data[1],arm.m_data[2],deviceId);
03264         CMT3EXITLOG;
03265 
03266         uint8_t bid = getBusIdInternal(deviceId);
03267         if (bid == CMT_BID_INVALID || bid == CMT_BID_BROADCAST)
03268                 return (m_lastResult = XRV_INVALIDID);
03269 
03270         Message        snd(CMT_MID_SETGPSLEVERARM,CMT_LEN_GPSLEVERARM);
03271         Message rcv;
03272         snd.setDataFloat((float) arm.m_data[0],0);
03273         snd.setDataFloat((float) arm.m_data[1],4);
03274         snd.setDataFloat((float) arm.m_data[2],8);
03275 
03276         snd.setBusId(bid);
03277         m_serial.writeMessage(&snd);
03278         m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_SETGPSLEVERARMACK,0,true);
03279         if (m_lastResult != XRV_OK)
03280                 return m_lastResult;
03281         if (m_logging)
03282                 m_logFile.writeMessage(&rcv);
03283         if (rcv.getMessageId() == CMT_MID_ERROR)
03284         {
03285                 m_lastHwErrorDeviceId = m_config.m_masterDeviceId;
03286                 if (rcv.getDataSize() >= 2)
03287                 {
03288                         uint8_t biddy = rcv.getDataByte(1);
03289                         getDeviceId(biddy,m_lastHwErrorDeviceId);
03290                 }
03291                 return m_lastResult = m_lastHwError = (XsensResultValue) rcv.getDataByte();
03292         }
03293         return m_lastResult = XRV_OK;
03294 }
03295 
03296 XsensResultValue Cmt3::storeXkfState(const CmtDeviceId deviceId)
03297 {
03298         CMT3LOG(__FUNCTION__ " %08x\n",deviceId);
03299         CMT3EXITLOG;
03300 
03301         uint8_t bid = getBusIdInternal(deviceId);
03302         if (bid == CMT_BID_INVALID || bid == CMT_BID_BROADCAST)
03303                 return (m_lastResult = XRV_INVALIDID);
03304 
03305         Message        snd(CMT_MID_STOREXKFSTATE,CMT_LEN_STOREXKFSTATE);
03306         Message rcv;
03307 
03308         snd.setBusId(bid);
03309         m_serial.writeMessage(&snd);
03310         m_lastResult = m_serial.waitForMessage(&rcv,CMT_MID_STOREXKFSTATEACK,0,true);
03311         if (m_lastResult != XRV_OK)
03312                 return m_lastResult;
03313         if (m_logging)
03314                 m_logFile.writeMessage(&rcv);
03315         if (rcv.getMessageId() == CMT_MID_ERROR)
03316         {
03317                 m_lastHwErrorDeviceId = m_config.m_masterDeviceId;
03318                 if (rcv.getDataSize() >= 2)
03319                 {
03320                         uint8_t biddy = rcv.getDataByte(1);
03321                         getDeviceId(biddy,m_lastHwErrorDeviceId);
03322                 }
03323                 return m_lastResult = m_lastHwError = (XsensResultValue) rcv.getDataByte();
03324         }
03325         return m_lastResult = XRV_OK;
03326 }
03327 } // end of xsens namespace
Generated on Sun May 8 08:41:34 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3