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