Main MRPT website > C++ reference for MRPT 1.5.6
cmt2.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 /*! \file Cmt2.cpp
11 
12  For information about objects in this file, see the appropriate header:
13  \ref Cmt2.h
14 
15  \section FileCopyright Copyright Notice
16  Copyright (C) Xsens Technologies B.V., 2006. All rights reserved.
17 
18  This source code is intended for use only by Xsens Technologies BV and
19  those that have explicit written permission to use it from
20  Xsens Technologies BV.
21 
22  THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY
23  KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
24  IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A
25  PARTICULAR PURPOSE.
26 
27  \section FileChangelog Changelog
28  \par 2006-04-05, v0.0.1
29  \li Job Mulder: Created
30  \par 2006-07-21, v0.1.0
31  \li Job Mulder: Updated file for release 0.1.0
32 */
33 
34 #include "cmt2.h"
35 
36 namespace xsens {
37 
38 #ifdef _LOG_CMT2
39 # define CMT2LOG CMTLOG
40 #else
41 # define CMT2LOG(...)
42 #endif
43 
44 //////////////////////////////////////////////////////////////////////////////////////////
45 int32_t findValidMessage(const uint8_t* buffer, const uint16_t bufferLength)
46 {
47  MessageHeader* hdr = NULL;
48  uint16_t pre = 0;
50  bool extended;
52  int32_t res;
53  Message* msg;
54 
55  // find the preamble
56  while(pre < bufferLength && (buffer[pre] != CMT_PREAMBLE))
57  ++pre;
58 
59  if (pre >= bufferLength)
60  return -1;
61 
62  // check if the message is valid
63  length = bufferLength-pre;
64  if (length < CMT_LEN_MSGHEADERCS)
65  return -1;
66 
67  // parse header
68 
69  hdr = (MessageHeader*) (buffer + pre);
70  if (hdr->m_length == CMT_EXTLENCODE)
71  {
72  extended = true;
73  if (length < CMT_LEN_MSGEXTHEADERCS)
74  return -1;
75  }
76  else
77  extended = false;
78 
79  // check the reported size
80  target = extended?
84  if (target <= length)
85  {
86  // header seems to be ok, check checksum
87  // check the checksum
88 
89  msg=new Message(buffer+pre,(uint16_t) target,(uint16_t) target);
90  if (msg->isChecksumOk())
91  {
92  delete msg;
93  return (int32_t) pre;
94  }
95  delete msg;
96  }
97 
98  // try next message
99  res = findValidMessage(buffer+pre+1,length-1);
100  if (res == -1)
101  return -1;
102  return res + pre + 1;
103 }
104 
105 //////////////////////////////////////////////////////////////////////////////////////////
106 ///////////////////////////////////////// Cmt2s /////////////////////////////////////////
107 //////////////////////////////////////////////////////////////////////////////////////////
108 
109 //////////////////////////////////////////////////////////////////////////////////////////
110 // Default constructor, initializes all members to their default values.
112  m_onMessageReceived(NULL),
113  m_onMessageSent(NULL)
114 {
116  m_readBufferCount = 0;
119  m_toEnd = 0;
120 }
121 
122 //////////////////////////////////////////////////////////////////////////////////////////
123 // Destructor, de-initializes, frees memory allocated for buffers, etc.
125 {
126 }
127 
128 
130 {
131  if (m_cmt1s.getPortNr())
132  {
133  CMT2LOG("L2: Closing port %d\n",(int32_t)m_cmt1s.getPortNr());
134  m_toEnd = 0;
135  return m_lastResult = m_cmt1s.close();
136  }
137  else
138  return m_lastResult = XRV_NOPORTOPEN;
139 }
140 
141 //////////////////////////////////////////////////////////////////////////////////////////
142 // Retrieve the port that the object is connected to.
144 {
145  m_cmt1s.getPortName(portname);
146  // we know there should be at least "/dev/x"
147  if (strlen(portname) < 6)
148  return m_lastResult = XRV_ERROR;
149  return m_lastResult = XRV_OK;
150 }
151 
152 //////////////////////////////////////////////////////////////////////////////////////////
153 // Retrieve the port that the object is connected to.
155 {
156  port = m_cmt1s.getPortNr();
157  if (port == 0)
158  return m_lastResult = XRV_ERROR;
159  return m_lastResult = XRV_OK;
160 }
161 
162 //////////////////////////////////////////////////////////////////////////////////////////
163 // Retrieve the port that the object is connected to.
165 {
166  port = m_cmt1s.getPortNr();
167  if (port == 0)
168  return m_lastResult = XRV_ERROR;
169  return m_lastResult = XRV_OK;
170 }
171 
172 //////////////////////////////////////////////////////////////////////////////////////////
173 // Open a communication channel to the given serial port name.
174 XsensResultValue Cmt2s::open (const char *portName, const uint32_t baudRate)
175 {
176  CMT2LOG("L2: Opening port %s @baud %d\n", portName, baudRate);
177  m_baudrate = baudRate;
178  m_lastResult = m_cmt1s.open(portName, baudRate);
179  m_toEnd = 0;
180  CMT2LOG("L2: Port open result %d: %s\n", (int32_t) m_lastResult, xsensResultText(m_lastResult));
181  return m_lastResult;
182 }
183 
184 #ifdef _WIN32
185 //////////////////////////////////////////////////////////////////////////////////////////
186 // Open a communication channel to the given COM port number.
187 XsensResultValue Cmt2s::open (const uint32_t portNumber, const uint32_t baudRate)
188 {
189  CMT2LOG("L2: Opening port %d @baud %d\n",(int32_t)portNumber,baudRate);
190  m_baudrate = baudRate;
191  m_lastResult = m_cmt1s.open(portNumber,baudRate);
192  m_toEnd = 0;
193  CMT2LOG("L2: Port open result %d: %s\n",(int32_t)m_lastResult,xsensResultText(m_lastResult));
194  return m_lastResult;
195 }
196 #endif
197 
198 //////////////////////////////////////////////////////////////////////////////////////////
199 // Read a message from the COM port.
201 {
203  uint16_t pre = 0;
204  uint32_t length = 0;
205  uint32_t target=0;
206  uint16_t i;
207  bool extended=false;
208 
209  CMT2LOG("L2: readMessage started, bufferCount=%u\n",m_readBufferCount);
210 
211  if (m_readBufferCount == 0)
212  m_readBuffer[0] = (uint8_t) ~CMT_PREAMBLE; // create a value that is definitely NOT a preamble
213 
216  m_readBufferCount += (uint16_t) length;
217 
218  while(m_readBufferCount > 0)
219  {
220  while(m_readBufferCount > 0)
221  {
222  // find preamble
223  while ((pre < m_readBufferCount) && (m_readBuffer[pre] != CMT_PREAMBLE))
224  ++pre;
225 
226  if (pre == m_readBufferCount)
227  {
228  CMT2LOG("L2: readMessage no preamble found in buffer\n");
229  m_readBufferCount = 0;
230  return m_lastResult = XRV_TIMEOUT;
231  }
232 
233  CMT2LOG("L2: readMessage preamble found at position %u\n",(uint32_t) pre);
234  // shift buffer to start
235  if (pre)
236  {
237  m_readBufferCount -= pre;
238  for (i=0;i<m_readBufferCount;++i)
239  m_readBuffer[i] = m_readBuffer[i+pre];
240  }
241 
243  {
244  CMT2LOG("L2: readMessage not enough header data read\n");
245  return m_lastResult = XRV_TIMEOUT;
246  }
247 
248  // read header
249  if (hdr->m_length == CMT_EXTLENCODE)
250  {
251  extended = true;
253  {
254  CMT2LOG("L2: readMessage not enough extended header data read\n");
255  return m_lastResult = XRV_TIMEOUT;
256  }
257  }
258  else
259  extended = false;
260 
261  // check the reported size
262  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));
263  CMT2LOG("L2: readMessage bytes in buffer=%u, extended=%u, target=%u\n",(uint32_t) m_readBufferCount,(uint32_t) extended, (uint32_t) target);
264  if ((uint32_t) target > (uint32_t) CMT_MAXDATALEN)
265  {
266  // skip current preamble
267  pre = 1;
268  CMT2LOG("L2: readMessage invalid message length %u\n",(uint32_t) target);
269  continue;
270  }
271  break; // everything seems to be ok, get out of inner while() loop
272  }
273 
274  // header seems to be ok, read until end and check checksum
275  if (extended)
276  target += CMT_LEN_MSGEXTHEADERCS;
277  else
278  target += CMT_LEN_MSGHEADERCS;
279 
280  // read the entire message
281  if (m_readBufferCount < target)
282  {
283  CMT2LOG("L2: readMessage readBufferCount %u < target %u\n",(uint32_t) m_readBufferCount, (uint32_t) target);
284  return m_lastResult = XRV_TIMEOUT;
285  }
286 
287  // check the checksum
288  if (rcv->loadFromString(m_readBuffer,(uint16_t) target) == XRV_OK)
289  {
290  CMT2LOG("L2: readMessage OK\n");
291  if (m_onMessageReceived != NULL)
292  {
293  CmtBinaryData* bytes = (CmtBinaryData*) malloc(sizeof(CmtBinaryData));
294  bytes->m_size = target;
295  bytes->m_portNr = m_cmt1s.getPortNr();
296 // bytes->m_type = CMT_CALLBACK_ONMESSAGERECEIVED;
297  memcpy(bytes->m_data,m_readBuffer,target);
298 #ifdef _LOG_CALLBACKS
299  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);
300 #endif
302  }
303 
304  m_readBufferCount -= (uint16_t) target;
305  if (m_readBufferCount)
306  for (i=0;i<m_readBufferCount;++i)
307  m_readBuffer[i] = m_readBuffer[i+target];
308 
309  return m_lastResult = XRV_OK;
310  }
311  CMT2LOG("L2: readMessage invalid checksum %02x %02x %02x %02x %02x\n",rcv->getMessageStart()[0]
312  ,rcv->getMessageStart()[1]
313  ,rcv->getMessageStart()[2]
314  ,rcv->getMessageStart()[3]
315  ,rcv->getMessageStart()[4]);
316  // skip current preamble
317  pre = 1;
318  }
319 
320  CMT2LOG("L2: readMessage timed out\n");
321  // a timeout occurred
322  return m_lastResult = XRV_TIMEOUT;
323 }
324 
325 //////////////////////////////////////////////////////////////////////////////////////////
326 // Set the callback function for when a message has been received or sent
328 {
329  switch (tp)
330  {
332  m_onMessageReceived = func;
333  m_onMessageReceivedInstance = instance;
335  return m_lastResult = XRV_OK;
337  m_onMessageSent = func;
338  m_onMessageSentInstance = instance;
340  return m_lastResult = XRV_OK;
341  default:
342  break;
343  }
345 }
346 
347 //////////////////////////////////////////////////////////////////////////////////////////
348 // Set the default timeout value to use in blocking operations.
350 {
351  CMT2LOG("L2: Setting timeout to %u ms\n",ms);
352  if ((m_lastResult = m_cmt1s.setTimeout(ms/2)) != XRV_OK)
353  return m_lastResult; // this can't actually happen
354  m_timeout = ms;
355  m_toEnd = 0;
356  return (m_lastResult = XRV_OK);
357 }
358 
359 //////////////////////////////////////////////////////////////////////////////////////////
360 // Wait for a message to arrive.
361 XsensResultValue Cmt2s::waitForMessage(Message* rcv, const uint8_t msgId, uint32_t timeoutOverride, bool acceptErrorMessage)
362 {
364  uint16_t pre = 0;
365  uint32_t length = 0;
367  uint16_t i;
368  bool extended;
369  bool readsome = (m_readBufferCount > 0);
370  uint32_t toRestore = m_toEnd;
371 
372  CMT2LOG("L2: waitForMessage x%02x, TO=%u, TOend=%u, TOO=%u\n",(uint32_t) msgId, m_timeout, m_toEnd, timeoutOverride);
373 
374  // The end-time may be misinterpreted around midnight, where it may be considered
375  // expired even if this is not the case. However, this is extremely rare.
376  if (m_toEnd == 0)
377  {
378  if (timeoutOverride != 0)
379  m_toEnd = (getTimeOfDay() + (uint32_t) timeoutOverride) % (XSENS_MS_PER_DAY);
380  else
382  if (m_toEnd == 0)
383  m_toEnd = 1;
384  }
385 
386  if (m_readBufferCount == 0)
387  m_readBuffer[0] = (uint8_t) ~CMT_PREAMBLE; // create a value that is definitely NOT a preamble
388 
389  do {
390  // find preamble
391  while (m_readBuffer[pre] != CMT_PREAMBLE)
392  {
393  if ((++pre) >= m_readBufferCount)
394  {
395  m_readBufferCount = 0;
396  pre = 0;
397  if (m_toEnd >= getTimeOfDay())
398  {
400  m_readBufferCount = (uint16_t) length;
401  if (m_readBufferCount > 0)
402  readsome = true;
403  }
404  }
405  if (m_toEnd < getTimeOfDay())
406  break;
407  }
408  // shift buffer to start
409  if (pre)
410  {
411  m_readBufferCount -= pre;
412  for (i=0;i<m_readBufferCount;++i)
413  {
414  m_readBuffer[i] = m_readBuffer[i+pre];
415  }
416  }
417 
418  pre = 1; // make sure we skip the first item in the next iteration
419  // read header
421  {
423  m_readBufferCount += (uint16_t) length;
424  }
426  {
427  CMT2LOG("L2: waitForMessage timeout occurred trying to read header\n");
428  break;
429  }
430 
431  if (hdr->m_length == CMT_EXTLENCODE)
432  {
433  extended = true;
435  {
437  m_readBufferCount += (uint16_t) length;
438  }
440  {
441  CMT2LOG("L2: waitForMessage timeout occurred trying to read extended header\n");
442  break;
443  }
444  }
445  else
446  extended = false;
447 
448  // check the reported size
450  continue;
451 
452  // header seems to be ok, read until end and check checksum
453  if (extended)
455  else
456  target = hdr->m_length + CMT_LEN_MSGHEADERCS;
457 
458  // read the entire message
459  while ((m_readBufferCount < target) && (m_toEnd >= getTimeOfDay()))
460  {
462  m_readBufferCount += (uint16_t) length;
463  }
464  if ((m_readBufferCount < target) && (m_toEnd < getTimeOfDay()))
465  {
466  CMT2LOG("L2: waitForMessage timeout occurred\n");
467  break;
468  }
469 
470  // check the checksum
471  //msg=new Message(m_readBuffer,(uint16_t) target, (uint16_t) target);
472  if (rcv->loadFromString(m_readBuffer,(uint16_t) target) == XRV_OK)
473  {
474  CMT2LOG("L2: waitForMessage received msg Id x%02x while expecting x%02x, msg size=%u\n",(uint32_t) rcv->getMessageId(),(uint32_t) msgId,target);
475  if (m_onMessageReceived != NULL)
476  {
477  CmtBinaryData* bytes = (CmtBinaryData*) malloc(sizeof(CmtBinaryData));
478  bytes->m_size = target;
479  bytes->m_portNr = m_cmt1s.getPortNr();
480 // bytes->m_type = CMT_CALLBACK_ONMESSAGERECEIVED;
481  memcpy(bytes->m_data,m_readBuffer,target);
482 #ifdef _LOG_CALLBACKS
483  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);
484 #endif
486  }
487 
488  m_readBufferCount -= (uint16_t) target;
489  if (m_readBufferCount)
490  {
491  for (i=0;i<m_readBufferCount;++i)
492  {
494  }
495  }
496 
497  if ((msgId == 0) || (msgId == rcv->getMessageId()) || (acceptErrorMessage && rcv->getMessageId() == CMT_MID_ERROR))
498  {
499  m_toEnd = toRestore;
500  return m_lastResult = XRV_OK;
501  }
502  }
503  else
504  {
505  rcv->clear();
506  CMT2LOG("L2: waitForMessage load from string failed\n");
507  }
508  } while (m_toEnd >= getTimeOfDay());
509 
510  // a timeout occurred
511  if (readsome)
512  {
513  // check if the current data contains a valid message
515 
516  if (pos != -1)
517  {
518  CMT2LOG("L2: waitForMessage found message in message\n");
519  // shift data to start of buffer
520  pre = (uint16_t) pos;
521  m_readBufferCount -= pre;
522  for (i=0;i<m_readBufferCount;++i)
523  {
524  m_readBuffer[i] = m_readBuffer[i+pre];
525  }
526  waitForMessage(rcv,msgId,0,acceptErrorMessage); // parse the message
527  m_toEnd = toRestore;
528  return m_lastResult; // set by waitForMessage
529  }
530 
532  }
533  else
535  m_toEnd = toRestore;
536  return m_lastResult;
537 }
538 
539 //////////////////////////////////////////////////////////////////////////////////////////
540 // Send a message over the COM port.
542 {
543  CMT2LOG("L2: writeMessage %2x %2x %2x %2x %2x\n",(int32_t) msg->getMessageStart()[0]
544  ,(int32_t) msg->getMessageStart()[1]
545  ,(int32_t) msg->getMessageStart()[2]
546  ,(int32_t) msg->getMessageStart()[3]
547  ,(int32_t) msg->getMessageStart()[4]);
548  uint32_t written = 0;
549  m_lastResult =
550  m_cmt1s.writeData(msg->getTotalMessageSize(),msg->getMessageStart(),&written);
551 
552  if (m_lastResult != XRV_OK)
553  {
554  CMT2LOG("L2: writeMessage returns %d: %s\n",(int32_t) m_lastResult, xsensResultText(m_lastResult));
555  return m_lastResult;
556  }
557 
558  if (written != msg->getTotalMessageSize())
559  {
560  CMT2LOG("L2: writeMessage wrote %u of %u bytes, returns %d: %s\n",written,msg->getTotalMessageSize(),(int32_t) XRV_ERROR, xsensResultText(XRV_ERROR));
561  return (m_lastResult = XRV_ERROR);
562  }
563 
564  if (m_onMessageSent != NULL)
565  {
566  CmtBinaryData* bytes = (CmtBinaryData*) malloc(sizeof(CmtBinaryData));
567  bytes->m_size = msg->getTotalMessageSize();
568  bytes->m_portNr = m_cmt1s.getPortNr();
569  memcpy(bytes->m_data,msg->getMessageStart(),msg->getTotalMessageSize());
570 #ifdef _LOG_CALLBACKS
571  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);
572 #endif
573  m_onMessageSent(m_onMessageSentInstance,CMT_CALLBACK_ONMESSAGESENT,bytes,m_onMessageSentParam);
574  }
575 
576  CMT2LOG("L2: writeMessage successful\n");
577  return (m_lastResult = XRV_OK);
578 }
579 
580 //////////////////////////////////////////////////////////////////////////////////////////
581 ///////////////////////////////////////// Cmt2f /////////////////////////////////////////
582 //////////////////////////////////////////////////////////////////////////////////////////
583 
584 //////////////////////////////////////////////////////////////////////////////////////////
585 // Default constructor
587 {
589  m_readOnly = true;
590 }
591 
592 //////////////////////////////////////////////////////////////////////////////////////////
593 // Destructor.
595 {
596  close();
597 }
598 
599 //////////////////////////////////////////////////////////////////////////////////////////
600 // Close the file.
602 {
603  if (!m_cmt1f.isOpen())
604  return m_lastResult = XRV_NOFILEOPEN;
605 
606  // save any unsaved data
607  // close the file
608  m_cmt1f.close();
609  m_readOnly = true;
610  return m_lastResult = XRV_OK;
611 }
612 
613 //////////////////////////////////////////////////////////////////////////////////////////
614 // Close the file and delete it.
616 {
617  if (!m_cmt1f.isOpen())
618  return m_lastResult = XRV_NOFILEOPEN;
619 
620  // close the file
622  m_readOnly = true;
623  return m_lastResult = XRV_OK;
624 }
625 
626 //////////////////////////////////////////////////////////////////////////////////////////
627 // Create a new file with level 2 header
628 XsensResultValue Cmt2f::create(const char* filename)
629 {
630  if (m_cmt1f.isOpen())
631  return m_lastResult = XRV_ALREADYOPEN;
632 
633  // create file
634  m_lastResult = m_cmt1f.create(filename);
635  if (m_lastResult != XRV_OK)
636  return m_lastResult;
637 
638  m_readOnly = false;
639 
640  // check if we can actually write to the file
641  m_lastResult = m_cmt1f.writeData(5,"Xsens");
642  if (m_lastResult == XRV_OK)
644  if (m_lastResult != XRV_OK)
645  m_cmt1f.close();
646  return m_lastResult;
647 }
648 
649 //////////////////////////////////////////////////////////////////////////////////////////
650 // Create a new file with level 2 header
651 XsensResultValue Cmt2f::create(const wchar_t* filename)
652 {
653  if (m_cmt1f.isOpen())
654  return m_lastResult = XRV_ALREADYOPEN;
655 
656  // create file
657  m_lastResult = m_cmt1f.create(filename);
658  if (m_lastResult != XRV_OK)
659  return m_lastResult;
660 
661  m_readOnly = false;
662 
663  // check if we can actually write to the file
664  m_lastResult = m_cmt1f.writeData(5,"Xsens");
665  if (m_lastResult == XRV_OK)
667  if (m_lastResult != XRV_OK)
668  m_cmt1f.close();
669  return m_lastResult;
670 }
671 
672 //////////////////////////////////////////////////////////////////////////////////////////
674 {
675  return &m_cmt1f;
676 }
677 
678 //////////////////////////////////////////////////////////////////////////////////////////
679 // Return the error code of the last operation.
681 {
682  return m_lastResult;
683 }
684 
685 //////////////////////////////////////////////////////////////////////////////////////////
686 // Retrieve the filename that was last successfully opened.
687 XsensResultValue Cmt2f::getName(char* filename) const
688 {
689  return m_lastResult = m_cmt1f.getName(filename);
690 }
691 
692 //////////////////////////////////////////////////////////////////////////////////////////
693 // Retrieve the filename that was last successfully opened.
694 XsensResultValue Cmt2f::getName(wchar_t* filename) const
695 {
696  return m_lastResult = m_cmt1f.getName(filename);
697 }
698 
699 //////////////////////////////////////////////////////////////////////////////////////////
700 // Return whether the file is open or not.
701 bool Cmt2f::isOpen(void) const
702 {
703  return m_cmt1f.isOpen();
704 }
705 
706 //////////////////////////////////////////////////////////////////////////////////////////
707 // Open a file and read the header
708 XsensResultValue Cmt2f::open(const char* filename, const bool readOnly)
709 {
710  if (m_cmt1f.isOpen())
711  return m_lastResult = XRV_ALREADYOPEN;
712  m_lastResult = m_cmt1f.open(filename,!readOnly,readOnly);
713  m_readOnly = readOnly;
714  return m_lastResult;
715 }
716 
717 //////////////////////////////////////////////////////////////////////////////////////////
718 // Open a file and read the header
719 XsensResultValue Cmt2f::open(const wchar_t* filename, const bool readOnly)
720 {
721  if (m_cmt1f.isOpen())
722  return m_lastResult = XRV_ALREADYOPEN;
723  m_lastResult = m_cmt1f.open(filename,!readOnly,readOnly);
724  m_readOnly = readOnly;
725  return m_lastResult;
726 }
727 
728 //////////////////////////////////////////////////////////////////////////////////////////
729 // Read the next message from the file
731 {
732  CmtFilePos pos;
733  uint8_t needle = CMT_PREAMBLE;
735  uint32_t length, bcount;
736  MessageHeader* hdr = (MessageHeader*) buffer;
737  bool extended;
739 
740  while (m_lastResult == XRV_OK)
741  {
742  bcount = 0;
743 
744  // find a message preamble
745  m_lastResult = m_cmt1f.find(&needle,1,pos);
746 
747  if (m_lastResult != XRV_OK)
748  return m_lastResult;
749 
750  // read header
752  bcount += length;
753  if (m_lastResult != XRV_OK)
754  return m_lastResult;
755 
756  if (hdr->m_length == CMT_EXTLENCODE)
757  {
758  extended = true;
759  m_lastResult = m_cmt1f.readData(CMT_LEN_MSGEXTHEADERCS - bcount,&buffer[bcount],&length);
760  bcount += length;
761  if (m_lastResult != XRV_OK)
762  {
763  m_cmt1f.setReadPos(pos+1);
764  continue;
765  }
766  }
767  else
768  extended = false;
769 
770  // check the reported size
772  {
773  m_cmt1f.setReadPos(pos+1);
774  continue;
775  }
776 
777  // header seems to be ok, read until end and check checksum
778  if (extended)
780  else
781  target = hdr->m_length + CMT_LEN_MSGHEADERCS;
782 
783  // read the entire message
784  m_lastResult = m_cmt1f.readData(target - bcount,&buffer[bcount],&length);
785  bcount += length;
786  if (m_lastResult != XRV_OK)
787  {
788  m_cmt1f.setReadPos(pos+1);
789  continue;
790  }
791 
792  // check the checksum
793  //msg=new Message(m_readBuffer,(uint16_t) target, (uint16_t) target);
794  if (msg->loadFromString(buffer,(uint16_t) target) == XRV_OK)
795  {
796  if ((msgId == 0) || (msgId == msg->getMessageId()))
797  return m_lastResult = XRV_OK;
798  pos += target-1;
799  }
800  msg->clear();
801  m_cmt1f.setReadPos(pos+1);
802  }
803  return m_lastResult;
804 }
805 
806 //////////////////////////////////////////////////////////////////////////////////////////
807 // Get the current file size
809 {
810  return m_cmt1f.getFileSize();
811 }
812 
813 //////////////////////////////////////////////////////////////////////////////////////////
814 // Get the current read position of the file
816 {
817  return m_cmt1f.getReadPos();
818 }
819 
820 //////////////////////////////////////////////////////////////////////////////////////////
821 // Set the read position to the given position
823 {
824  return m_lastResult = m_cmt1f.setReadPos(pos);
825 }
826 
827 //////////////////////////////////////////////////////////////////////////////////////////
828 // Write a message to the end of the file
830 {
831  if (m_readOnly)
832  return m_lastResult = XRV_READONLY;
834 }
835 
836 } // end of xsens namespace
GLuint GLsizei GLsizei * length
Definition: glew.h:1732
void BASE_IMPEXP memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) MRPT_NO_THROWS
An OS and compiler independent version of "memcpy".
Definition: os.cpp:358
XsensResultValue getLastResult(void) const
Return the error code of the last operation.
Definition: cmt2.cpp:680
Operation was performed successfully.
Definition: xsens_std.h:32
bool isOpen(void) const
Return whether the file is open or not.
Definition: cmt1.h:313
A message header.
Definition: cmtmessage.h:35
uint8_t getPortNr(void) const
Retrieve the port number that was last successfully opened.
Definition: cmt1.h:125
__int64 CmtFilePos
Definition: cmtf.h:24
unsigned __int16 uint16_t
Definition: rptypes.h:46
XsensResultValue appendData(const uint32_t length, const void *data)
Write data to the end of the file.
Definition: cmt1.cpp:684
#define CMT_MAXDATALEN
Definition: cmtdef.h:73
XsensResultValue setReadPos(const CmtFilePos pos)
Set the new absolute read position.
Definition: cmt1.cpp:1273
A timeout occurred.
Definition: xsens_std.h:60
XsensResultValue open(const char *filename, const bool readOnly=false)
Open a file and read the header.
Definition: cmt2.cpp:708
XsensResultValue writeData(const uint32_t length, const uint8_t *data, uint32_t *written)
Write the data to the serial port.
Definition: cmt1.cpp:618
XsensResultValue getPortNr(uint8_t &port) const
Retrieve the port that the object is connected to.
Definition: cmt2.cpp:154
#define XSENS_MS_PER_DAY
The number of milliseconds in a normal day.
Definition: xsens_time.h:21
CmtCallbackFunction m_onMessageReceived
The message received function.
Definition: cmt2.h:47
Operation aborted because of no data read.
Definition: xsens_std.h:61
~Cmt2f()
Destructor.
Definition: cmt2.cpp:594
XsensResultValue setCallbackFunction(CmtCallbackSelector tp, int32_t instance, CmtCallbackFunction func, void *param)
Set the callback function for when a message has been received or sent.
Definition: cmt2.cpp:327
#define CMTLOG(...)
Definition: cmt1.h:38
#define CMT_DEFAULT_BAUD_RATE
The default baud rate of the Cmt1s serial communication.
Definition: cmtdef.h:798
XsensResultValue readData(const uint32_t maxLength, void *data, uint32_t *length)
Read data from the file and put it into the data buffer.
Definition: cmt1.cpp:1218
XsensResultValue writeData(const uint32_t length, const void *data)
Write data to the file.
Definition: cmt1.cpp:1319
XsensResultValue closeAndDelete(void)
Close the file and delete it.
Definition: cmt1.cpp:729
#define CMT_MAXMSGLEN
Definition: cmtdef.h:75
void clear(void)
Clear all data in the message.
Definition: cmtmessage.cpp:137
Callback function, called when a full message has been received from a port.
Definition: cmtdef.h:1074
XsensResultValue deleteData(const CmtFilePos start, const uint32_t length)
Delete the given data from the file.
Definition: cmt1.cpp:862
CmtCallbackFunction m_onMessageSent
The message sent function.
Definition: cmt2.h:58
uint32_t m_toEnd
The timestamp at which to end an operation.
Definition: cmt2.h:78
uint32_t getTimeOfDay(tm *date_, time_t *secs_)
A platform-independent clock.
Definition: xsens_time.cpp:29
#define CMT2_DEFAULT_TIMEOUT
Timeout in ms for level 2.
Definition: cmtdef.h:807
Cmt1s m_cmt1s
The CMT level 1 object that this class operates on.
Definition: cmt2.h:68
unsigned char uint8_t
Definition: rptypes.h:43
XsensResultValue open(const char *filename, const bool create, const bool readOnly)
Open a file.
Definition: cmt1.cpp:1113
Callback function, called when a full message has been sent by a port.
Definition: cmtdef.h:1075
int32_t m_onMessageReceivedInstance
Custom, user supplied parameter for the OnMessageReceived callback function, passed as the first argu...
Definition: cmt2.h:49
XsensResultValue setTimeout(const uint32_t ms=CMT1_DEFAULT_TIMEOUT)
Set the default timeout value to use in blocking operations.
Definition: cmt1.cpp:545
Tried to change a read-only value.
Definition: xsens_std.h:75
void * m_onMessageSentParam
Custom, user supplied parameter for the OnMessageSent callback function, passed as the last argument...
Definition: cmt2.h:62
int32_t m_onMessageSentInstance
Custom, user supplied parameter for the OnMessageSent callback function, passed as the first argument...
Definition: cmt2.h:60
XsensResultValue close(void)
Close the file.
Definition: cmt2.cpp:601
No serial port opened for reading/writing.
Definition: xsens_std.h:92
bool isChecksumOk(void) const
Compute the checksum and compare it with the stored checksum. Equal is ok.
Definition: cmtmessage.cpp:360
XsensResultValue
Xsens return values.
Definition: xsens_std.h:30
uint8_t getMessageId(void) const
Return the current value of the m_messageId field.
Definition: cmtmessage.h:190
XsensResultValue closeAndDelete(void)
Close the file and delete it.
Definition: cmt2.cpp:615
GLuint buffer
Definition: glew.h:1585
CmtFilePos getReadPosition(void)
Get the current read position.
Definition: cmt2.cpp:815
#define CMT2LOG(...)
Definition: cmt2.cpp:41
const uint8_t * getMessageStart(void) const
Return the start of the message buffer.
Definition: cmtmessage.h:195
XsensResultValue find(const void *needle, const uint32_t needleLength, CmtFilePos &pos)
Find a string of bytes in the file.
Definition: cmt1.cpp:938
XsensResultValue open(const char *portName, const uint32_t baudRate=CMT_DEFAULT_BAUD_RATE, uint32_t readBufSize=CMT_DEFAULT_READ_BUFFER_SIZE, uint32_t writeBufSize=CMT_DEFAULT_WRITE_BUFFER_SIZE)
Open a communcation channel to the given serial port name.
Definition: cmt1.cpp:322
XsensResultValue readData(const uint32_t maxLength, uint8_t *data, uint32_t *length=NULL)
Read data from the serial port and put it into the data buffer.
Definition: cmt1.cpp:478
#define CMT_MID_ERROR
Definition: cmtdef.h:473
Cmt2s()
Default constructor, initialize all members to their default values.
Definition: cmt2.cpp:111
#define CMT_LEN_MSGEXTHEADERCS
Definition: cmtdef.h:66
void getPortName(char *portname) const
Retrieve the port name that was last successfully opened.
Definition: cmt1.h:127
XsensResultValue open(const char *portName, const uint32_t baudRate=CMT_DEFAULT_BAUD_RATE)
Open a communication channel to the given serial port name.
Definition: cmt2.cpp:174
XsensResultValue waitForMessage(Message *rcv, const uint8_t msgId, uint32_t timeoutOverride, bool acceptErrorMessage)
Wait for a message to arrive.
Definition: cmt2.cpp:361
uint16_t m_readBufferCount
The number of valid bytes in the readBuffer.
Definition: cmt2.h:74
uint8_t m_readBuffer[CMT_DEFAULT_READ_BUFFER_SIZE]
Buffer for reading data until a valid message is read. Should be rarely used.
Definition: cmt2.h:72
XsensResultValue loadFromString(const uint8_t *source, const uint16_t size)
Read the entire message from the given source string.
Definition: cmtmessage.cpp:369
XsensResultValue close(void)
Close the file.
Definition: cmt1.cpp:705
#define CMT_PREAMBLE
Definition: cmtdef.h:58
void * m_onMessageReceivedParam
Custom, user supplied parameter for the OnMessageReceived callback function, passed as the last argum...
Definition: cmt2.h:51
#define CMT_EXTLENCODE
Definition: cmtdef.h:61
XsensResultValue getName(char *filename) const
Retrieve the filename that was last successfully opened.
Definition: cmt1.cpp:1006
int32_t findValidMessage(const uint8_t *buffer, const uint16_t bufferLength)
Find a valid message in the given buffer. If nothing is found, the function returns -1...
Definition: cmt2.cpp:45
GLuint res
Definition: glew.h:7143
CmtFilePos getFileSize(void)
Get the current file size.
Definition: cmt2.cpp:808
XsensResultValue setReadPosition(CmtFilePos pos)
Set the read position to the given position.
Definition: cmt2.cpp:822
GLenum target
Definition: glew.h:5023
__int32 int32_t
Definition: rptypes.h:48
struct xsens::MessageHeader::_mdl::_mextd m_extended
XsensResultValue close(void)
Close the serial communication port.
Definition: cmt2.cpp:129
Cmt1f * getCmt1f(void)
Get a reference to the embedded Cmt1f object.
Definition: cmt2.cpp:673
XsensResultValue setTimeout(const uint32_t ms=CMT2_DEFAULT_TIMEOUT)
Set the default timeout value to use in blocking operations.
Definition: cmt2.cpp:349
uint16_t getTotalMessageSize(void) const
Return the length of the message buffer.
Definition: cmtmessage.cpp:350
~Cmt2s()
Destructor, de-initialize, free memory allocated for buffers, etc.
Definition: cmt2.cpp:124
XsensResultValue writeMessage(Message *msg)
Send a message over the COM port.
Definition: cmt2.cpp:541
#define CMT_LEN_MSGHEADERCS
Definition: cmtdef.h:65
XsensResultValue m_lastResult
The last result of an operation.
Definition: cmt2.h:174
XsensResultValue create(const char *filename)
Create a new file with level 2 header.
Definition: cmt2.cpp:628
CmtFilePos getReadPos(void) const
Return the current read position.
Definition: cmt1.h:300
XsensResultValue getName(char *filename) const
Retrieve the filename that was last successfully opened.
Definition: cmt2.cpp:687
An invalid parameter is supplied.
Definition: xsens_std.h:53
const char * xsensResultText(const XsensResultValue result)
Definition: xsens_std.cpp:13
The low-level file communication class.
Definition: cmt1.h:204
XsensResultValue readMessage(Message *msg, const uint8_t msgId=0)
Read the next message from the file, when msgId is non-zero, the first matching message will be retur...
Definition: cmt2.cpp:730
CmtCallbackSelector
Definition: cmtdef.h:1068
Cmt2f()
Default constructor.
Definition: cmt2.cpp:586
A generic error occurred.
Definition: xsens_std.h:58
XsensResultValue create(const char *filename)
Open an empty file.
Definition: cmt1.cpp:781
XsensResultValue close(void)
Close the serial communication port.
Definition: cmt1.cpp:209
#define CMT_LEN_MSGHEADER
Definition: cmtdef.h:63
CmtFilePos getFileSize(void) const
Return the size of the file.
Definition: cmt1.h:284
GLfloat param
Definition: glew.h:1435
Cmt1f m_cmt1f
The Cmt1f object that is used for the low-level operations.
Definition: cmt2.h:172
struct xsens::MessageHeader::_mdl::_mextd::_mlen m_length
XsensResultValue writeMessage(const Message *msg)
Write a message to the end of the file.
Definition: cmt2.cpp:829
unsigned __int32 uint32_t
Definition: rptypes.h:49
uint32_t m_baudrate
The baudrate that was last set to be used by the port.
Definition: cmt2.h:66
uint32_t m_timeout
Timeout in ms for blocking operations.
Definition: cmt2.h:76
An I/O device is already opened with this object.
Definition: xsens_std.h:71
uint8_t m_data[CMT_MAXMSGLEN]
Definition: cmtdef.h:1086
bool m_readOnly
When set to true, the file is read-only and attempts to write to it will fail.
Definition: cmt2.h:176
uint8_t m_portNr
Definition: cmtdef.h:1087
Class for storing a single message.
Definition: cmtmessage.h:76
XsensResultValue getPortName(char *portname) const
Definition: cmt2.cpp:143
bool isOpen(void) const
Return whether the file is open or not.
Definition: cmt2.cpp:701
union xsens::MessageHeader::_mdl m_datlen
No file opened for reading/writing.
Definition: xsens_std.h:91
XsensResultValue(__cdecl * CmtCallbackFunction)(int32_t, CmtCallbackSelector, void *, void *)
Definition: cmtdef.h:1091
int32_t m_size
Definition: cmtdef.h:1085
XsensResultValue readMessage(Message *rcv)
Read a message from the COM port.
Definition: cmt2.cpp:200
XsensResultValue m_lastResult
The last result of an operation.
Definition: cmt2.h:70



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018