cmt2.cpp

00001 /*! \file Cmt2.cpp
00002 
00003         For information about objects in this file, see the appropriate header:
00004         \ref Cmt2.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-05, 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 "cmt2.h"
00026 
00027 namespace xsens {
00028 
00029 #ifdef _LOG_CMT2
00030 #        define CMT2LOG                CMTLOG
00031 #else
00032 #        define CMT2LOG(...)
00033 #endif
00034 
00035 //////////////////////////////////////////////////////////////////////////////////////////
00036 int32_t findValidMessage(const uint8_t* buffer, const uint16_t bufferLength)
00037 {
00038         MessageHeader* hdr = NULL;
00039         uint16_t pre = 0;
00040         uint16_t target;
00041         bool extended;
00042         uint16_t length;
00043         int32_t res;
00044         Message* msg;
00045 
00046         // find the preamble
00047         while(pre < bufferLength && (buffer[pre] != CMT_PREAMBLE))
00048                 ++pre;
00049 
00050         if (pre >= bufferLength)
00051                 return -1;
00052 
00053         // check if the message is valid
00054         length = bufferLength-pre;
00055         if (length < CMT_LEN_MSGHEADERCS)
00056                 return -1;
00057 
00058         // parse header
00059         
00060         hdr = (MessageHeader*) (buffer + pre);
00061         if (hdr->m_length == CMT_EXTLENCODE)
00062         {
00063                 extended = true;
00064                 if (length < CMT_LEN_MSGEXTHEADERCS)
00065                         return -1;
00066         }
00067         else
00068                 extended = false;
00069 
00070         // check the reported size
00071         target = extended?
00072                                 ((uint16_t) hdr->m_datlen.m_extended.m_length.m_high * 256 + (uint16_t) hdr->m_datlen.m_extended.m_length.m_low)
00073                                 + CMT_LEN_MSGEXTHEADERCS:
00074                                 (uint16_t) (hdr->m_length) + CMT_LEN_MSGHEADERCS;
00075         if (target <= length)
00076         {
00077                 // header seems to be ok, check checksum
00078                 // check the checksum
00079 
00080                 msg=new Message(buffer+pre,(uint16_t) target,(uint16_t) target);
00081                 if (msg->isChecksumOk())
00082                 {
00083                         delete msg;
00084                         return (int32_t) pre;
00085                 }
00086                 delete msg;
00087         }
00088 
00089         // try next message
00090         res = findValidMessage(buffer+pre+1,length-1);
00091         if (res == -1)
00092                 return -1;
00093         return res + pre + 1;
00094 }
00095 
00096 //////////////////////////////////////////////////////////////////////////////////////////
00097 ///////////////////////////////////////// Cmt2s  /////////////////////////////////////////
00098 //////////////////////////////////////////////////////////////////////////////////////////
00099 
00100 //////////////////////////////////////////////////////////////////////////////////////////
00101 // Default constructor, initializes all members to their default values.
00102 Cmt2s::Cmt2s() :
00103         m_onMessageReceived(NULL),
00104         m_onMessageSent(NULL)
00105 {
00106         m_lastResult = XRV_OK;
00107         m_readBufferCount = 0;
00108         m_timeout = CMT2_DEFAULT_TIMEOUT;
00109         m_baudrate = CMT_DEFAULT_BAUD_RATE;
00110         m_toEnd = 0;
00111 }
00112 
00113 //////////////////////////////////////////////////////////////////////////////////////////
00114 // Destructor, de-initializes, frees memory allocated for buffers, etc.
00115 Cmt2s::~Cmt2s()
00116 {
00117 }
00118 
00119 
00120 XsensResultValue Cmt2s::close(void)
00121 {
00122         if (m_cmt1s.getPortNr())
00123         {
00124                 CMT2LOG("L2: Closing port %d\n",(int32_t)m_cmt1s.getPortNr());
00125                 m_toEnd = 0;
00126                 return m_lastResult = m_cmt1s.close();
00127         }
00128         else
00129                 return m_lastResult = XRV_NOPORTOPEN;
00130 }
00131 
00132 //////////////////////////////////////////////////////////////////////////////////////////
00133 // Retrieve the port that the object is connected to.
00134 XsensResultValue Cmt2s::getPortName(char *portname) const
00135 {
00136         m_cmt1s.getPortName(portname);
00137         // we know there should be at least "/dev/x"
00138         if (strlen(portname) < 6)
00139                 return m_lastResult = XRV_ERROR;
00140         return m_lastResult = XRV_OK;
00141 }
00142 
00143 //////////////////////////////////////////////////////////////////////////////////////////
00144 // Retrieve the port that the object is connected to.
00145 XsensResultValue Cmt2s::getPortNr(uint16_t& port) const
00146 {
00147         port = m_cmt1s.getPortNr();
00148         if (port == 0)
00149                 return m_lastResult = XRV_ERROR;
00150         return m_lastResult = XRV_OK;
00151 }
00152 
00153 //////////////////////////////////////////////////////////////////////////////////////////
00154 // Retrieve the port that the object is connected to.
00155 XsensResultValue Cmt2s::getPortNr(int32_t& port) const
00156 {
00157         port = m_cmt1s.getPortNr();
00158         if (port == 0)
00159                 return m_lastResult = XRV_ERROR;
00160         return m_lastResult = XRV_OK;
00161 }
00162 
00163 //////////////////////////////////////////////////////////////////////////////////////////
00164 // Open a communication channel to the given serial port name.
00165 XsensResultValue Cmt2s::open (const char *portName, const uint32_t baudRate, uint32_t readBufSize, uint32_t writeBufSize)
00166 {
00167         CMT2LOG("L2: Opening port %s @baud %d\n", portName, baudRate);
00168         m_baudrate = baudRate;
00169         m_lastResult = m_cmt1s.open(portName, baudRate, readBufSize, writeBufSize);
00170         m_toEnd = 0;
00171         CMT2LOG("L2: Port open result %d: %s\n", (int32_t) m_lastResult, xsensResultText(m_lastResult));
00172         return m_lastResult;
00173 }
00174 
00175 #ifdef _WIN32
00176 //////////////////////////////////////////////////////////////////////////////////////////
00177 // Open a communication channel to the given COM port number.
00178 XsensResultValue Cmt2s::open (const uint32_t portNumber, const uint32_t baudRate, uint32_t readBufSize, uint32_t writeBufSize)
00179 {
00180         CMT2LOG("L2: Opening port %d @baud %d\n",(int32_t)portNumber,baudRate);
00181         m_baudrate = baudRate;
00182         m_lastResult = m_cmt1s.open(portNumber, baudRate, readBufSize, writeBufSize);
00183         m_toEnd = 0;
00184         CMT2LOG("L2: Port open result %d: %s\n",(int32_t)m_lastResult,xsensResultText(m_lastResult));
00185         return m_lastResult;
00186 }
00187 #endif
00188 
00189 //////////////////////////////////////////////////////////////////////////////////////////
00190 // Read a message from the COM port.
00191 XsensResultValue Cmt2s::readMessage(Message* rcv)
00192 {
00193         MessageHeader* hdr = (MessageHeader*) m_readBuffer;
00194         uint16_t pre = 0;
00195         uint32_t length = 0;
00196         uint32_t target;
00197         uint16_t i;
00198         bool extended;
00199 
00200         CMT2LOG("L2: readMessage started, bufferCount=%u\n",m_readBufferCount);
00201 
00202         if (m_readBufferCount == 0)
00203                 m_readBuffer[0] = (uint8_t) ~CMT_PREAMBLE;        // create a value that is definitely NOT a preamble
00204 
00205         if (m_readBufferCount < CMT_MAXMSGLEN)
00206                 m_lastResult = m_cmt1s.readData(CMT_MAXMSGLEN-m_readBufferCount,m_readBuffer+m_readBufferCount,&length);
00207         m_readBufferCount += (uint16_t) length;
00208 
00209         while(m_readBufferCount > 0)
00210         {
00211                 while(m_readBufferCount > 0)
00212                 {
00213                         // find preamble
00214                         while ((pre < m_readBufferCount) && (m_readBuffer[pre] != CMT_PREAMBLE))
00215                                 ++pre;
00216 
00217                         if (pre == m_readBufferCount)
00218                         {
00219                                 CMT2LOG("L2: readMessage no preamble found in buffer\n");
00220                                 m_readBufferCount = 0;
00221                                 return m_lastResult = XRV_TIMEOUT;
00222                         }
00223 
00224                         CMT2LOG("L2: readMessage preamble found at position %u\n",(uint32_t) pre);
00225                         // shift buffer to start
00226                         if (pre)
00227                         {
00228                                 m_readBufferCount -= pre;
00229                                 for (i=0;i<m_readBufferCount;++i)
00230                                         m_readBuffer[i] = m_readBuffer[i+pre];
00231                         }
00232 
00233                         if (m_readBufferCount < CMT_LEN_MSGHEADERCS)
00234                         {
00235                                 CMT2LOG("L2: readMessage not enough header data read\n");
00236                                 return m_lastResult = XRV_TIMEOUT;
00237                         }
00238 
00239                         // read header
00240                         if (hdr->m_length == CMT_EXTLENCODE)
00241                         {
00242                                 extended = true;
00243                                 if (m_readBufferCount < CMT_LEN_MSGEXTHEADERCS)
00244                                 {
00245                                         CMT2LOG("L2: readMessage not enough extended header data read\n");
00246                                         return m_lastResult = XRV_TIMEOUT;
00247                                 }
00248                         }
00249                         else
00250                                 extended = false;
00251 
00252                         // check the reported size
00253                         target = (extended?((uint16_t) hdr->m_datlen.m_extended.m_length.m_high * 256 + (uint16_t) hdr->m_datlen.m_extended.m_length.m_low):(uint16_t) (hdr->m_length));
00254                         CMT2LOG("L2: readMessage bytes in buffer=%u, extended=%u, target=%u\n",(uint32_t) m_readBufferCount,(uint32_t) extended, (uint32_t) target);
00255                         if ((uint32_t) target > (uint32_t) CMT_MAXDATALEN)
00256                         {
00257                                 // skip current preamble
00258                                 pre = 1;
00259                                 CMT2LOG("L2: readMessage invalid message length %u\n",(uint32_t) target);
00260                                 continue;
00261                         }
00262                         break;        // everything seems to be ok, get out of inner while() loop
00263                 }
00264 
00265                 // header seems to be ok, read until end and check checksum
00266                 if (extended)
00267                         target += CMT_LEN_MSGEXTHEADERCS;
00268                 else
00269                         target += CMT_LEN_MSGHEADERCS;
00270 
00271                 // read the entire message
00272                 if (m_readBufferCount < target)
00273                 {
00274                         CMT2LOG("L2: readMessage readBufferCount %u < target %u\n",(uint32_t) m_readBufferCount, (uint32_t) target);
00275                         return m_lastResult = XRV_TIMEOUT;
00276                 }
00277 
00278                 // check the checksum
00279                 if (rcv->loadFromString(m_readBuffer,(uint16_t) target) == XRV_OK)
00280                 {
00281                         CMT2LOG("L2: readMessage OK\n");
00282                         if (m_onMessageReceived != NULL)
00283                         {
00284                                 CmtBinaryData* bytes = (CmtBinaryData*) malloc(sizeof(CmtBinaryData));
00285                                 bytes->m_size = target;
00286                                 bytes->m_portNr = m_cmt1s.getPortNr();
00287 //                                bytes->m_type = CMT_CALLBACK_ONMESSAGERECEIVED;
00288                                 memcpy(bytes->m_data,m_readBuffer,target);
00289 #ifdef _LOG_CALLBACKS
00290                                 CMTLOG("C2: m_onMessageReceived(%d,(%d,%d),%p)\n",(int32_t) m_onMessageReceivedInstance, (int32_t) bytes->m_size, (int32_t) bytes->m_portNr, m_onMessageReceivedParam);
00291 #endif
00292                                 m_onMessageReceived(m_onMessageReceivedInstance,CMT_CALLBACK_ONMESSAGERECEIVED,bytes,m_onMessageReceivedParam);
00293                         }
00294 
00295                         m_readBufferCount -= (uint16_t) target;
00296                         if (m_readBufferCount)
00297                                 for (i=0;i<m_readBufferCount;++i)
00298                                         m_readBuffer[i] = m_readBuffer[i+target];
00299 
00300                         return m_lastResult = XRV_OK;
00301                 }
00302                 CMT2LOG("L2: readMessage invalid checksum %02x %02x %02x %02x %02x\n",rcv->getMessageStart()[0]
00303                                                                                                                                                         ,rcv->getMessageStart()[1]
00304                                                                                                                                                         ,rcv->getMessageStart()[2]
00305                                                                                                                                                         ,rcv->getMessageStart()[3]
00306                                                                                                                                                         ,rcv->getMessageStart()[4]);
00307                 // skip current preamble
00308                 pre = 1;
00309         }
00310 
00311         CMT2LOG("L2: readMessage timed out\n");
00312         // a timeout occurred
00313         return m_lastResult = XRV_TIMEOUT;
00314 }
00315         
00316 //////////////////////////////////////////////////////////////////////////////////////////
00317 // Set the callback function for when a message has been received or sent
00318 XsensResultValue Cmt2s::setCallbackFunction(CmtCallbackType tp, int32_t instance, CmtCallbackFunction func, void* param)
00319 {
00320         switch (tp)
00321         {
00322         case CMT_CALLBACK_ONMESSAGERECEIVED:
00323                 m_onMessageReceived = func;
00324                 m_onMessageReceivedInstance = instance;
00325                 m_onMessageReceivedParam = param;
00326                 return m_lastResult = XRV_OK;
00327         case CMT_CALLBACK_ONMESSAGESENT:
00328                 m_onMessageSent = func;
00329                 m_onMessageSentInstance = instance;
00330                 m_onMessageSentParam = param;
00331                 return m_lastResult = XRV_OK;
00332         default:
00333                 break;
00334         }
00335         return m_lastResult = XRV_INVALIDPARAM;
00336 }
00337 
00338 //////////////////////////////////////////////////////////////////////////////////////////
00339 // Set the default timeout value to use in blocking operations.
00340 XsensResultValue Cmt2s::setTimeout (const uint32_t ms)
00341 {
00342         CMT2LOG("L2: Setting timeout to %u ms\n",ms);
00343         if ((m_lastResult = m_cmt1s.setTimeout(ms/2)) != XRV_OK)
00344                 return m_lastResult;                // this can't actually happen
00345         m_timeout = ms;
00346         m_toEnd = 0;
00347         return (m_lastResult = XRV_OK);
00348 }
00349 
00350 //////////////////////////////////////////////////////////////////////////////////////////
00351 // Wait for a message to arrive.
00352 XsensResultValue Cmt2s::waitForMessage(Message* rcv, const uint8_t msgId, uint32_t timeoutOverride, bool acceptErrorMessage)
00353 {
00354         MessageHeader* hdr = (MessageHeader*) m_readBuffer;
00355         uint16_t pre = 0;
00356         uint32_t length = 0;
00357         uint32_t target;
00358         uint16_t i;
00359         bool extended;
00360         bool readsome = (m_readBufferCount > 0);
00361         uint32_t toRestore = m_toEnd;
00362 
00363         CMT2LOG("L2: waitForMessage x%02x, TO=%u, TOend=%u, TOO=%u\n",(uint32_t) msgId, m_timeout, m_toEnd, timeoutOverride);
00364 
00365         // The end-time may be misinterpreted around midnight, where it may be considered
00366         // expired even if this is not the case. However, this is extremely rare.
00367         if (m_toEnd == 0)
00368         {
00369                 if (timeoutOverride != 0)
00370                         m_toEnd = (getTimeOfDay() + (uint32_t) timeoutOverride) % (XSENS_MS_PER_DAY);
00371                 else
00372                         m_toEnd = (getTimeOfDay() + (uint32_t) m_timeout) % (XSENS_MS_PER_DAY);
00373                 if (m_toEnd == 0)
00374                         m_toEnd = 1;
00375         }
00376 
00377         if (m_readBufferCount == 0)
00378                 m_readBuffer[0] = (uint8_t) ~CMT_PREAMBLE;        // create a value that is definitely NOT a preamble
00379 
00380         do {
00381                 // find preamble
00382                 while (m_readBuffer[pre] != CMT_PREAMBLE)
00383                 {
00384                         if ((++pre) >= m_readBufferCount)
00385                         {
00386                                 m_readBufferCount = 0;
00387                                 pre = 0;
00388                                 if (m_toEnd >= getTimeOfDay())
00389                                 {
00390                                         m_lastResult = m_cmt1s.readData(CMT_LEN_MSGHEADER,m_readBuffer,&length);
00391                                         m_readBufferCount = (uint16_t) length;
00392                                         if (m_readBufferCount > 0)
00393                                                 readsome = true;
00394                                 }
00395                         }
00396                         if (m_toEnd < getTimeOfDay())
00397                                 break;
00398                 }
00399                 // shift buffer to start
00400                 if (pre)
00401                 {
00402                         m_readBufferCount -= pre;
00403                         for (i=0;i<m_readBufferCount;++i)
00404                         {
00405                                 m_readBuffer[i] = m_readBuffer[i+pre];
00406                         }
00407                 }
00408 
00409                 pre = 1;        // make sure we skip the first item in the next iteration
00410                 // read header
00411                 while (m_readBufferCount < CMT_LEN_MSGHEADERCS && (m_toEnd >= getTimeOfDay()))
00412                 {
00413                         m_cmt1s.readData(CMT_LEN_MSGHEADERCS - m_readBufferCount,&m_readBuffer[m_readBufferCount],&length);
00414                         m_readBufferCount += (uint16_t) length;
00415                 }
00416                 if ((m_readBufferCount < CMT_LEN_MSGHEADERCS) && (m_toEnd < getTimeOfDay()))
00417                 {
00418                         CMT2LOG("L2: waitForMessage timeout occurred trying to read header\n");
00419                         break;
00420                 }
00421 
00422                 if (hdr->m_length == CMT_EXTLENCODE)
00423                 {
00424                         extended = true;
00425                         while (m_readBufferCount < CMT_LEN_MSGEXTHEADERCS && (m_toEnd >= getTimeOfDay()))
00426                         {
00427                                 m_cmt1s.readData(CMT_LEN_MSGEXTHEADERCS - m_readBufferCount,&m_readBuffer[m_readBufferCount],&length);
00428                                 m_readBufferCount += (uint16_t) length;
00429                         }
00430                         if ((m_readBufferCount < CMT_LEN_MSGEXTHEADERCS) && (m_toEnd < getTimeOfDay()))
00431                         {
00432                                 CMT2LOG("L2: waitForMessage timeout occurred trying to read extended header\n");
00433                                 break;
00434                         }
00435                 }
00436                 else
00437                         extended = false;
00438 
00439                 // check the reported size
00440                 if (extended && (((uint16_t) hdr->m_datlen.m_extended.m_length.m_high * 256 + (uint16_t) hdr->m_datlen.m_extended.m_length.m_low) > (uint16_t) CMT_MAXDATALEN))
00441                         continue;
00442 
00443                 // header seems to be ok, read until end and check checksum
00444                 if (extended)
00445                         target = ((uint16_t) hdr->m_datlen.m_extended.m_length.m_high * 256 + (uint16_t) hdr->m_datlen.m_extended.m_length.m_low) + CMT_LEN_MSGEXTHEADERCS;
00446                 else
00447                         target = hdr->m_length + CMT_LEN_MSGHEADERCS;
00448 
00449                 // read the entire message
00450                 while ((m_readBufferCount < target) && (m_toEnd >= getTimeOfDay()))
00451                 {
00452                         m_cmt1s.readData(target - m_readBufferCount,&m_readBuffer[m_readBufferCount],&length);
00453                         m_readBufferCount += (uint16_t) length;
00454                 }
00455                 if ((m_readBufferCount < target) && (m_toEnd < getTimeOfDay()))
00456                 {
00457                         CMT2LOG("L2: waitForMessage timeout occurred\n");
00458                         break;
00459                 }
00460 
00461                 // check the checksum
00462                 //msg=new Message(m_readBuffer,(uint16_t) target, (uint16_t) target);
00463                 if (rcv->loadFromString(m_readBuffer,(uint16_t) target) == XRV_OK)
00464                 {
00465                         CMT2LOG("L2: waitForMessage received msg Id x%02x while expecting x%02x, msg size=%u\n",(uint32_t) rcv->getMessageId(),(uint32_t) msgId,target);
00466                         if (m_onMessageReceived != NULL)
00467                         {
00468                                 CmtBinaryData* bytes = (CmtBinaryData*) malloc(sizeof(CmtBinaryData));
00469                                 bytes->m_size = target;
00470                                 bytes->m_portNr = m_cmt1s.getPortNr();
00471 //                                bytes->m_type = CMT_CALLBACK_ONMESSAGERECEIVED;
00472                                 memcpy(bytes->m_data,m_readBuffer,target);
00473 #ifdef _LOG_CALLBACKS
00474                                 CMTLOG("C2: m_onMessageReceived(%d,(%d,%d),%p)\n",(int32_t) m_onMessageReceivedInstance, (int32_t) bytes->m_size, (int32_t) bytes->m_portNr, m_onMessageReceivedParam);
00475 #endif
00476                                 m_onMessageReceived(m_onMessageReceivedInstance,CMT_CALLBACK_ONMESSAGERECEIVED,bytes,m_onMessageReceivedParam);
00477                         }
00478 
00479                         m_readBufferCount -= (uint16_t) target;
00480                         if (m_readBufferCount)
00481                         {
00482                                 for (i=0;i<m_readBufferCount;++i)
00483                                 {
00484                                         m_readBuffer[i] = m_readBuffer[i+target];
00485                                 }
00486                         }
00487 
00488                         if ((msgId == 0) || (msgId == rcv->getMessageId()) || (acceptErrorMessage && rcv->getMessageId() == CMT_MID_ERROR))
00489                         {
00490                                 m_toEnd = toRestore;
00491                                 return m_lastResult = XRV_OK;
00492                         }
00493                 }
00494                 else
00495                 {
00496                         rcv->clear();
00497                         CMT2LOG("L2: waitForMessage load from string failed\n");
00498                 }
00499         } while (m_toEnd >= getTimeOfDay());
00500 
00501         // a timeout occurred
00502         if (readsome)
00503         {
00504                 // check if the current data contains a valid message
00505                 int32_t pos = findValidMessage(m_readBuffer,m_readBufferCount);
00506 
00507                 if (pos != -1)
00508                 {
00509                         CMT2LOG("L2: waitForMessage found message in message\n");
00510                         // shift data to start of buffer
00511                         pre = (uint16_t) pos;
00512                         m_readBufferCount -= pre;
00513                         for (i=0;i<m_readBufferCount;++i)
00514                         {
00515                                 m_readBuffer[i] = m_readBuffer[i+pre];
00516                         }
00517                         waitForMessage(rcv,msgId,0,acceptErrorMessage);        // parse the message
00518                         m_toEnd = toRestore;
00519                         return m_lastResult;        // set by waitForMessage
00520                 }
00521 
00522                 m_lastResult = XRV_TIMEOUT;
00523         }
00524         else
00525                 m_lastResult = XRV_TIMEOUTNODATA;
00526         m_toEnd = toRestore;
00527         return m_lastResult;
00528 }
00529 
00530 //////////////////////////////////////////////////////////////////////////////////////////
00531 // Send a message over the COM port.
00532 XsensResultValue Cmt2s::writeMessage(Message* msg)
00533 {
00534         CMT2LOG("L2: writeMessage %2x %2x %2x %2x %2x\n",(int32_t) msg->getMessageStart()[0]
00535                                                                                                         ,(int32_t) msg->getMessageStart()[1]
00536                                                                                                         ,(int32_t) msg->getMessageStart()[2]
00537                                                                                                         ,(int32_t) msg->getMessageStart()[3]
00538                                                                                                         ,(int32_t) msg->getMessageStart()[4]);
00539         uint32_t written = 0;
00540         m_lastResult = 
00541                         m_cmt1s.writeData(msg->getTotalMessageSize(),msg->getMessageStart(),&written);
00542 
00543         if (m_lastResult != XRV_OK)
00544         {
00545                 CMT2LOG("L2: writeMessage returns %d: %s\n",(int32_t) m_lastResult, xsensResultText(m_lastResult));
00546                 return m_lastResult;
00547         }
00548 
00549         if (written != msg->getTotalMessageSize())
00550         {
00551                 CMT2LOG("L2: writeMessage wrote %u of %u bytes, returns %d: %s\n",written,msg->getTotalMessageSize(),(int32_t) XRV_ERROR, xsensResultText(XRV_ERROR));
00552                 return (m_lastResult = XRV_ERROR);
00553         }
00554 
00555         if (m_onMessageSent != NULL)
00556         {
00557                 CmtBinaryData* bytes = (CmtBinaryData*) malloc(sizeof(CmtBinaryData));
00558                 bytes->m_size = msg->getTotalMessageSize();
00559                 bytes->m_portNr = m_cmt1s.getPortNr();
00560                 memcpy(bytes->m_data,msg->getMessageStart(),msg->getTotalMessageSize());
00561 #ifdef _LOG_CALLBACKS
00562                 CMTLOG("C2: m_onMessageSent(%d,(%d,%d),%p)\n",(int32_t) m_onMessageSentInstance, (int32_t) bytes->m_size, (int32_t) bytes->m_portNr, m_onMessageSentParam);
00563 #endif
00564                 m_onMessageSent(m_onMessageSentInstance,CMT_CALLBACK_ONMESSAGESENT,bytes,m_onMessageSentParam);
00565         }
00566 
00567         CMT2LOG("L2: writeMessage successful\n");
00568         return (m_lastResult = XRV_OK);
00569 }
00570         
00571 //////////////////////////////////////////////////////////////////////////////////////////
00572 ///////////////////////////////////////// Cmt2f  /////////////////////////////////////////
00573 //////////////////////////////////////////////////////////////////////////////////////////
00574 
00575 //////////////////////////////////////////////////////////////////////////////////////////
00576 // Default constructor
00577 Cmt2f::Cmt2f()
00578 {
00579         m_lastResult = XRV_OK;
00580         m_readOnly = true;
00581 }
00582 
00583 //////////////////////////////////////////////////////////////////////////////////////////
00584 // Destructor.
00585 Cmt2f::~Cmt2f()
00586 {
00587         close();
00588 }
00589 
00590 //////////////////////////////////////////////////////////////////////////////////////////
00591 // Close the file.
00592 XsensResultValue Cmt2f::close(void)
00593 {
00594         if (!m_cmt1f.isOpen())
00595                 return m_lastResult = XRV_NOFILEOPEN;
00596 
00597         // save any unsaved data
00598         // close the file
00599         m_cmt1f.close();
00600         m_readOnly = true;
00601         return m_lastResult = XRV_OK;
00602 }
00603 
00604 //////////////////////////////////////////////////////////////////////////////////////////
00605 // Close the file and delete it.
00606 XsensResultValue Cmt2f::closeAndDelete(void)
00607 {
00608         if (!m_cmt1f.isOpen())
00609                 return m_lastResult = XRV_NOFILEOPEN;
00610 
00611         // close the file
00612         m_cmt1f.closeAndDelete();
00613         m_readOnly = true;
00614         return m_lastResult = XRV_OK;
00615 }
00616 
00617 //////////////////////////////////////////////////////////////////////////////////////////
00618 // Create a new file with level 2 header
00619 XsensResultValue Cmt2f::create(const char* filename)
00620 {
00621         if (m_cmt1f.isOpen())
00622                 return m_lastResult = XRV_ALREADYOPEN;
00623 
00624         // create file
00625         m_lastResult = m_cmt1f.create(filename);
00626         if (m_lastResult != XRV_OK)
00627                 return m_lastResult;
00628 
00629         m_readOnly = false;
00630 
00631         // check if we can actually write to the file
00632         m_lastResult = m_cmt1f.writeData(5,"Xsens");
00633         if (m_lastResult == XRV_OK)
00634                 m_lastResult = m_cmt1f.deleteData(0,5);
00635         if (m_lastResult != XRV_OK)
00636                 m_cmt1f.close();
00637         return m_lastResult;
00638 }
00639 
00640 //////////////////////////////////////////////////////////////////////////////////////////
00641 // Create a new file with level 2 header
00642 XsensResultValue Cmt2f::create(const wchar_t* filename)
00643 {
00644         if (m_cmt1f.isOpen())
00645                 return m_lastResult = XRV_ALREADYOPEN;
00646 
00647         // create file
00648         m_lastResult = m_cmt1f.create(filename);
00649         if (m_lastResult != XRV_OK)
00650                 return m_lastResult;
00651 
00652         m_readOnly = false;
00653 
00654         // check if we can actually write to the file
00655         m_lastResult = m_cmt1f.writeData(5,"Xsens");
00656         if (m_lastResult == XRV_OK)
00657                 m_lastResult = m_cmt1f.deleteData(0,5);
00658         if (m_lastResult != XRV_OK)
00659                 m_cmt1f.close();
00660         return m_lastResult;
00661 }
00662 
00663 //////////////////////////////////////////////////////////////////////////////////////////
00664 Cmt1f* Cmt2f::getCmt1f(void)
00665 {
00666         return &m_cmt1f;
00667 }
00668 
00669 //////////////////////////////////////////////////////////////////////////////////////////
00670 // Return the error code of the last operation.
00671 XsensResultValue Cmt2f::getLastResult(void) const
00672 {
00673         return m_lastResult;
00674 }
00675 
00676 //////////////////////////////////////////////////////////////////////////////////////////
00677 // Retrieve the filename that was last successfully opened.
00678 XsensResultValue Cmt2f::getName(char* filename) const
00679 {
00680         return m_lastResult = m_cmt1f.getName(filename);
00681 }
00682 
00683 //////////////////////////////////////////////////////////////////////////////////////////
00684 // Retrieve the filename that was last successfully opened.
00685 XsensResultValue Cmt2f::getName(wchar_t* filename) const
00686 {
00687         return m_lastResult = m_cmt1f.getName(filename);
00688 }
00689 
00690 //////////////////////////////////////////////////////////////////////////////////////////
00691 // Return whether the file is open or not.
00692 bool Cmt2f::isOpen(void) const
00693 {
00694         return m_cmt1f.isOpen();
00695 }
00696 
00697 //////////////////////////////////////////////////////////////////////////////////////////
00698 // Open a file and read the header
00699 XsensResultValue Cmt2f::open(const char* filename, const bool readOnly)
00700 {
00701         if (m_cmt1f.isOpen())
00702                 return m_lastResult = XRV_ALREADYOPEN;
00703         m_lastResult = m_cmt1f.open(filename,!readOnly,readOnly);
00704         m_readOnly = readOnly;
00705         return m_lastResult;
00706 }
00707 
00708 //////////////////////////////////////////////////////////////////////////////////////////
00709 // Open a file and read the header
00710 XsensResultValue Cmt2f::open(const wchar_t* filename, const bool readOnly)
00711 {
00712         if (m_cmt1f.isOpen())
00713                 return m_lastResult = XRV_ALREADYOPEN;
00714         m_lastResult = m_cmt1f.open(filename,!readOnly,readOnly);
00715         m_readOnly = readOnly;
00716         return m_lastResult;
00717 }
00718 
00719 //////////////////////////////////////////////////////////////////////////////////////////
00720 // Read the next message from the file
00721 XsensResultValue Cmt2f::readMessage(Message* msg, const uint8_t msgId)
00722 {
00723         CmtFilePos pos;
00724         uint8_t needle = CMT_PREAMBLE;
00725         uint8_t buffer[CMT_MAXMSGLEN];
00726         uint32_t length, bcount;
00727         MessageHeader* hdr = (MessageHeader*) buffer;
00728         bool extended;
00729         uint16_t target;
00730 
00731         while (m_lastResult == XRV_OK)
00732         {
00733                 bcount = 0;
00734 
00735                 // find a message preamble
00736                 m_lastResult = m_cmt1f.find(&needle,1,pos);
00737 
00738                 if (m_lastResult != XRV_OK)
00739                         return m_lastResult;
00740 
00741                 // read header
00742                 m_lastResult = m_cmt1f.readData(CMT_LEN_MSGHEADERCS,buffer,&length);
00743                 bcount += length;
00744                 if (m_lastResult != XRV_OK)
00745                         return m_lastResult;
00746 
00747                 if (hdr->m_length == CMT_EXTLENCODE)
00748                 {
00749                         extended = true;
00750                         m_lastResult = m_cmt1f.readData(CMT_LEN_MSGEXTHEADERCS - bcount,&buffer[bcount],&length);
00751                         bcount += length;
00752                         if (m_lastResult != XRV_OK)
00753                         {
00754                                 m_cmt1f.setReadPos(pos+1);
00755                                 continue;
00756                         }
00757                 }
00758                 else
00759                         extended = false;
00760 
00761                 // check the reported size
00762                 if (extended && (((uint16_t) hdr->m_datlen.m_extended.m_length.m_high * 256 + (uint16_t) hdr->m_datlen.m_extended.m_length.m_low) > (uint16_t) CMT_MAXDATALEN))
00763                 {
00764                         m_cmt1f.setReadPos(pos+1);
00765                         continue;
00766                 }
00767 
00768                 // header seems to be ok, read until end and check checksum
00769                 if (extended)
00770                         target = ((uint16_t) hdr->m_datlen.m_extended.m_length.m_high * 256 + (uint16_t) hdr->m_datlen.m_extended.m_length.m_low) + CMT_LEN_MSGEXTHEADERCS;
00771                 else
00772                         target = hdr->m_length + CMT_LEN_MSGHEADERCS;
00773 
00774                 // read the entire message
00775                 m_lastResult = m_cmt1f.readData(target - bcount,&buffer[bcount],&length);
00776                 bcount += length;
00777                 if (m_lastResult != XRV_OK)
00778                 {
00779                         m_cmt1f.setReadPos(pos+1);
00780                         continue;
00781                 }
00782 
00783                 // check the checksum
00784                 //msg=new Message(m_readBuffer,(uint16_t) target, (uint16_t) target);
00785                 if (msg->loadFromString(buffer,(uint16_t) target) == XRV_OK)
00786                 {
00787                         if ((msgId == 0) || (msgId == msg->getMessageId()))
00788                                 return m_lastResult = XRV_OK;
00789                         pos += target-1;
00790                 }
00791                 msg->clear();
00792                 m_cmt1f.setReadPos(pos+1);
00793         }
00794         return m_lastResult;
00795 }
00796 
00797 //////////////////////////////////////////////////////////////////////////////////////////
00798 // Get the current file size
00799 CmtFilePos Cmt2f::getFileSize(void)
00800 {
00801         return m_cmt1f.getFileSize();
00802 }
00803 
00804 //////////////////////////////////////////////////////////////////////////////////////////
00805 // Get the current read position of the file
00806 CmtFilePos Cmt2f::getReadPosition(void)
00807 {
00808         return m_cmt1f.getReadPos();
00809 }
00810 
00811 //////////////////////////////////////////////////////////////////////////////////////////
00812 // Set the read position to the given position
00813 XsensResultValue Cmt2f::setReadPosition(CmtFilePos pos)
00814 {
00815         return m_lastResult = m_cmt1f.setReadPos(pos);
00816 }
00817 
00818 //////////////////////////////////////////////////////////////////////////////////////////
00819 // Write a message to the end of the file
00820 XsensResultValue Cmt2f::writeMessage(const Message* msg)
00821 {
00822         if (m_readOnly)
00823                 return m_lastResult = XRV_READONLY;
00824         return m_lastResult = m_cmt1f.appendData(msg->getTotalMessageSize(),msg->getMessageStart());
00825 }
00826 
00827 } // end of xsens namespace
Generated on Sun May 8 08:41:32 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3