Main MRPT website > C++ reference for MRPT 1.9.9
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 = nullptr;
48  uint16_t pre = 0;
49  uint16_t target;
50  bool extended;
52  int32_t res;
53  Message* msg;
54 
55  // find the preamble
56  while (pre < bufferLength && (buffer[pre] != CMT_PREAMBLE)) ++pre;
57 
58  if (pre >= bufferLength) return -1;
59 
60  // check if the message is valid
61  length = bufferLength - pre;
62  if (length < CMT_LEN_MSGHEADERCS) return -1;
63 
64  // parse header
65 
66  hdr = (MessageHeader*)(buffer + pre);
67  if (hdr->m_length == CMT_EXTLENCODE)
68  {
69  extended = true;
70  if (length < CMT_LEN_MSGEXTHEADERCS) return -1;
71  }
72  else
73  extended = false;
74 
75  // check the reported size
76  target = extended
77  ? ((uint16_t)hdr->m_datlen.m_extended.m_length.m_high * 256 +
81  if (target <= length)
82  {
83  // header seems to be ok, check checksum
84  // check the checksum
85 
86  msg = new Message(buffer + pre, (uint16_t)target, (uint16_t)target);
87  if (msg->isChecksumOk())
88  {
89  delete msg;
90  return (int32_t)pre;
91  }
92  delete msg;
93  }
94 
95  // try next message
96  res = findValidMessage(buffer + pre + 1, length - 1);
97  if (res == -1) return -1;
98  return res + pre + 1;
99 }
100 
101 //////////////////////////////////////////////////////////////////////////////////////////
102 ///////////////////////////////////////// Cmt2s
103 ////////////////////////////////////////////
104 //////////////////////////////////////////////////////////////////////////////////////////
105 
106 //////////////////////////////////////////////////////////////////////////////////////////
107 // Default constructor, initializes all members to their default values.
108 Cmt2s::Cmt2s() : m_onMessageReceived(nullptr), m_onMessageSent(nullptr)
109 {
111  m_readBufferCount = 0;
114  m_toEnd = 0;
115 }
116 
117 //////////////////////////////////////////////////////////////////////////////////////////
118 // Destructor, de-initializes, frees memory allocated for buffers, etc.
121 {
122  if (m_cmt1s.getPortNr())
123  {
124  CMT2LOG("L2: Closing port %d\n", (int32_t)m_cmt1s.getPortNr());
125  m_toEnd = 0;
126  return m_lastResult = m_cmt1s.close();
127  }
128  else
129  return m_lastResult = XRV_NOPORTOPEN;
130 }
131 
132 //////////////////////////////////////////////////////////////////////////////////////////
133 // Retrieve the port that the object is connected to.
135 {
136  m_cmt1s.getPortName(portname);
137  // we know there should be at least "/dev/x"
138  if (strlen(portname) < 6) return m_lastResult = XRV_ERROR;
139  return m_lastResult = XRV_OK;
140 }
141 
142 //////////////////////////////////////////////////////////////////////////////////////////
143 // Retrieve the port that the object is connected to.
145 {
146  port = m_cmt1s.getPortNr();
147  if (port == 0) return m_lastResult = XRV_ERROR;
148  return m_lastResult = XRV_OK;
149 }
150 
151 //////////////////////////////////////////////////////////////////////////////////////////
152 // Retrieve the port that the object is connected to.
154 {
155  port = m_cmt1s.getPortNr();
156  if (port == 0) return m_lastResult = XRV_ERROR;
157  return m_lastResult = XRV_OK;
158 }
159 
160 //////////////////////////////////////////////////////////////////////////////////////////
161 // Open a communication channel to the given serial port name.
162 XsensResultValue Cmt2s::open(const char* portName, const uint32_t baudRate)
163 {
164  CMT2LOG("L2: Opening port %s @baud %d\n", portName, baudRate);
165  m_baudrate = baudRate;
166  m_lastResult = m_cmt1s.open(portName, baudRate);
167  m_toEnd = 0;
168  CMT2LOG(
169  "L2: Port open result %d: %s\n", (int32_t)m_lastResult,
171  return m_lastResult;
172 }
173 
174 #ifdef _WIN32
175 //////////////////////////////////////////////////////////////////////////////////////////
176 // Open a communication channel to the given COM port number.
177 XsensResultValue Cmt2s::open(const uint32_t portNumber, const uint32_t baudRate)
178 {
179  CMT2LOG("L2: Opening port %d @baud %d\n", (int32_t)portNumber, baudRate);
180  m_baudrate = baudRate;
181  m_lastResult = m_cmt1s.open(portNumber, baudRate);
182  m_toEnd = 0;
183  CMT2LOG(
184  "L2: Port open result %d: %s\n", (int32_t)m_lastResult,
186  return m_lastResult;
187 }
188 #endif
189 
190 //////////////////////////////////////////////////////////////////////////////////////////
191 // Read a message from the COM port.
193 {
195  uint16_t pre = 0;
196  uint32_t length = 0;
197  uint32_t target = 0;
198  uint16_t i;
199  bool extended = false;
200 
201  CMT2LOG("L2: readMessage started, bufferCount=%u\n", m_readBufferCount);
202 
203  if (m_readBufferCount == 0)
204  m_readBuffer[0] = (uint8_t)~CMT_PREAMBLE; // create a value that is
205  // definitely NOT a preamble
206 
210  &length);
212 
213  while (m_readBufferCount > 0)
214  {
215  while (m_readBufferCount > 0)
216  {
217  // find preamble
218  while ((pre < m_readBufferCount) &&
219  (m_readBuffer[pre] != CMT_PREAMBLE))
220  ++pre;
221 
222  if (pre == m_readBufferCount)
223  {
224  CMT2LOG("L2: readMessage no preamble found in buffer\n");
225  m_readBufferCount = 0;
226  return m_lastResult = XRV_TIMEOUT;
227  }
228 
229  CMT2LOG(
230  "L2: readMessage preamble found at position %u\n",
231  (uint32_t)pre);
232  // shift buffer to start
233  if (pre)
234  {
235  m_readBufferCount -= pre;
236  for (i = 0; i < m_readBufferCount; ++i)
237  m_readBuffer[i] = m_readBuffer[i + pre];
238  }
239 
241  {
242  CMT2LOG("L2: readMessage not enough header data read\n");
243  return m_lastResult = XRV_TIMEOUT;
244  }
245 
246  // read header
247  if (hdr->m_length == CMT_EXTLENCODE)
248  {
249  extended = true;
251  {
252  CMT2LOG(
253  "L2: readMessage not enough extended header data "
254  "read\n");
255  return m_lastResult = XRV_TIMEOUT;
256  }
257  }
258  else
259  extended = false;
260 
261  // check the reported size
262  target =
263  (extended
265  256 +
267  : (uint16_t)(hdr->m_length));
268  CMT2LOG(
269  "L2: readMessage bytes in buffer=%u, extended=%u, target=%u\n",
270  (uint32_t)m_readBufferCount, (uint32_t)extended,
271  (uint32_t)target);
272  if ((uint32_t)target > (uint32_t)CMT_MAXDATALEN)
273  {
274  // skip current preamble
275  pre = 1;
276  CMT2LOG(
277  "L2: readMessage invalid message length %u\n",
278  (uint32_t)target);
279  continue;
280  }
281  break; // everything seems to be ok, get out of inner while() loop
282  }
283 
284  // header seems to be ok, read until end and check checksum
285  if (extended)
286  target += CMT_LEN_MSGEXTHEADERCS;
287  else
288  target += CMT_LEN_MSGHEADERCS;
289 
290  // read the entire message
291  if (m_readBufferCount < target)
292  {
293  CMT2LOG(
294  "L2: readMessage readBufferCount %u < target %u\n",
296  return m_lastResult = XRV_TIMEOUT;
297  }
298 
299  // check the checksum
300  if (rcv->loadFromString(m_readBuffer, (uint16_t)target) == XRV_OK)
301  {
302  CMT2LOG("L2: readMessage OK\n");
303  if (m_onMessageReceived != nullptr)
304  {
305  CmtBinaryData* bytes =
306  (CmtBinaryData*)malloc(sizeof(CmtBinaryData));
307  bytes->m_size = target;
308  bytes->m_portNr = m_cmt1s.getPortNr();
309  // bytes->m_type = CMT_CALLBACK_ONMESSAGERECEIVED;
310  memcpy(bytes->m_data, m_readBuffer, target);
311 #ifdef _LOG_CALLBACKS
312  CMTLOG(
313  "C2: m_onMessageReceived(%d,(%d,%d),%p)\n",
315  (int32_t)bytes->m_size, (int32_t)bytes->m_portNr,
317 #endif
320  bytes, m_onMessageReceivedParam);
321  }
322 
323  m_readBufferCount -= (uint16_t)target;
324  if (m_readBufferCount)
325  for (i = 0; i < m_readBufferCount; ++i)
326  m_readBuffer[i] = m_readBuffer[i + target];
327 
328  return m_lastResult = XRV_OK;
329  }
330  CMT2LOG(
331  "L2: readMessage invalid checksum %02x %02x %02x %02x %02x\n",
332  rcv->getMessageStart()[0], rcv->getMessageStart()[1],
333  rcv->getMessageStart()[2], rcv->getMessageStart()[3],
334  rcv->getMessageStart()[4]);
335  // skip current preamble
336  pre = 1;
337  }
338 
339  CMT2LOG("L2: readMessage timed out\n");
340  // a timeout occurred
341  return m_lastResult = XRV_TIMEOUT;
342 }
343 
344 //////////////////////////////////////////////////////////////////////////////////////////
345 // Set the callback function for when a message has been received or sent
348  void* param)
349 {
350  switch (tp)
351  {
353  m_onMessageReceived = func;
354  m_onMessageReceivedInstance = instance;
356  return m_lastResult = XRV_OK;
358  m_onMessageSent = func;
359  m_onMessageSentInstance = instance;
361  return m_lastResult = XRV_OK;
362  default:
363  break;
364  }
366 }
367 
368 //////////////////////////////////////////////////////////////////////////////////////////
369 // Set the default timeout value to use in blocking operations.
371 {
372  CMT2LOG("L2: Setting timeout to %u ms\n", ms);
373  if ((m_lastResult = m_cmt1s.setTimeout(ms / 2)) != XRV_OK)
374  return m_lastResult; // this can't actually happen
375  m_timeout = ms;
376  m_toEnd = 0;
377  return (m_lastResult = XRV_OK);
378 }
379 
380 //////////////////////////////////////////////////////////////////////////////////////////
381 // Wait for a message to arrive.
383  Message* rcv, const uint8_t msgId, uint32_t timeoutOverride,
384  bool acceptErrorMessage)
385 {
387  uint16_t pre = 0;
388  uint32_t length = 0;
389  uint32_t target;
390  uint16_t i;
391  bool extended;
392  bool readsome = (m_readBufferCount > 0);
393  uint32_t toRestore = m_toEnd;
394 
395  CMT2LOG(
396  "L2: waitForMessage x%02x, TO=%u, TOend=%u, TOO=%u\n", (uint32_t)msgId,
397  m_timeout, m_toEnd, timeoutOverride);
398 
399  // The end-time may be misinterpreted around midnight, where it may be
400  // considered
401  // expired even if this is not the case. However, this is extremely rare.
402  if (m_toEnd == 0)
403  {
404  if (timeoutOverride != 0)
405  m_toEnd = (getTimeOfDay() + (uint32_t)timeoutOverride) %
407  else
408  m_toEnd =
410  if (m_toEnd == 0) m_toEnd = 1;
411  }
412 
413  if (m_readBufferCount == 0)
414  m_readBuffer[0] = (uint8_t)~CMT_PREAMBLE; // create a value that is
415  // definitely NOT a preamble
416 
417  do
418  {
419  // find preamble
420  while (m_readBuffer[pre] != CMT_PREAMBLE)
421  {
422  if ((++pre) >= m_readBufferCount)
423  {
424  m_readBufferCount = 0;
425  pre = 0;
426  if (m_toEnd >= getTimeOfDay())
427  {
431  if (m_readBufferCount > 0) readsome = true;
432  }
433  }
434  if (m_toEnd < getTimeOfDay()) break;
435  }
436  // shift buffer to start
437  if (pre)
438  {
439  m_readBufferCount -= pre;
440  for (i = 0; i < m_readBufferCount; ++i)
441  {
442  m_readBuffer[i] = m_readBuffer[i + pre];
443  }
444  }
445 
446  pre = 1; // make sure we skip the first item in the next iteration
447  // read header
449  (m_toEnd >= getTimeOfDay()))
450  {
455  }
457  (m_toEnd < getTimeOfDay()))
458  {
459  CMT2LOG(
460  "L2: waitForMessage timeout occurred trying to read header\n");
461  break;
462  }
463 
464  if (hdr->m_length == CMT_EXTLENCODE)
465  {
466  extended = true;
468  (m_toEnd >= getTimeOfDay()))
469  {
474  }
476  (m_toEnd < getTimeOfDay()))
477  {
478  CMT2LOG(
479  "L2: waitForMessage timeout occurred trying to read "
480  "extended header\n");
481  break;
482  }
483  }
484  else
485  extended = false;
486 
487  // check the reported size
488  if (extended &&
489  (((uint16_t)hdr->m_datlen.m_extended.m_length.m_high * 256 +
492  continue;
493 
494  // header seems to be ok, read until end and check checksum
495  if (extended)
496  target = ((uint16_t)hdr->m_datlen.m_extended.m_length.m_high * 256 +
499  else
500  target = hdr->m_length + CMT_LEN_MSGHEADERCS;
501 
502  // read the entire message
503  while ((m_readBufferCount < target) && (m_toEnd >= getTimeOfDay()))
504  {
507  &length);
509  }
510  if ((m_readBufferCount < target) && (m_toEnd < getTimeOfDay()))
511  {
512  CMT2LOG("L2: waitForMessage timeout occurred\n");
513  break;
514  }
515 
516  // check the checksum
517  // msg=new Message(m_readBuffer,(uint16_t) target, (uint16_t) target);
518  if (rcv->loadFromString(m_readBuffer, (uint16_t)target) == XRV_OK)
519  {
520  CMT2LOG(
521  "L2: waitForMessage received msg Id x%02x while expecting "
522  "x%02x, msg size=%u\n",
523  (uint32_t)rcv->getMessageId(), (uint32_t)msgId, target);
524  if (m_onMessageReceived != nullptr)
525  {
526  CmtBinaryData* bytes =
527  (CmtBinaryData*)malloc(sizeof(CmtBinaryData));
528  bytes->m_size = target;
529  bytes->m_portNr = m_cmt1s.getPortNr();
530  // bytes->m_type = CMT_CALLBACK_ONMESSAGERECEIVED;
531  memcpy(bytes->m_data, m_readBuffer, target);
532 #ifdef _LOG_CALLBACKS
533  CMTLOG(
534  "C2: m_onMessageReceived(%d,(%d,%d),%p)\n",
536  (int32_t)bytes->m_size, (int32_t)bytes->m_portNr,
538 #endif
541  bytes, m_onMessageReceivedParam);
542  }
543 
544  m_readBufferCount -= (uint16_t)target;
545  if (m_readBufferCount)
546  {
547  for (i = 0; i < m_readBufferCount; ++i)
548  {
549  m_readBuffer[i] = m_readBuffer[i + target];
550  }
551  }
552 
553  if ((msgId == 0) || (msgId == rcv->getMessageId()) ||
554  (acceptErrorMessage && rcv->getMessageId() == CMT_MID_ERROR))
555  {
556  m_toEnd = toRestore;
557  return m_lastResult = XRV_OK;
558  }
559  }
560  else
561  {
562  rcv->clear();
563  CMT2LOG("L2: waitForMessage load from string failed\n");
564  }
565  } while (m_toEnd >= getTimeOfDay());
566 
567  // a timeout occurred
568  if (readsome)
569  {
570  // check if the current data contains a valid message
572 
573  if (pos != -1)
574  {
575  CMT2LOG("L2: waitForMessage found message in message\n");
576  // shift data to start of buffer
577  pre = (uint16_t)pos;
578  m_readBufferCount -= pre;
579  for (i = 0; i < m_readBufferCount; ++i)
580  {
581  m_readBuffer[i] = m_readBuffer[i + pre];
582  }
584  rcv, msgId, 0, acceptErrorMessage); // parse the message
585  m_toEnd = toRestore;
586  return m_lastResult; // set by waitForMessage
587  }
588 
590  }
591  else
593  m_toEnd = toRestore;
594  return m_lastResult;
595 }
596 
597 //////////////////////////////////////////////////////////////////////////////////////////
598 // Send a message over the COM port.
600 {
601  CMT2LOG(
602  "L2: writeMessage %2x %2x %2x %2x %2x\n",
603  (int32_t)msg->getMessageStart()[0], (int32_t)msg->getMessageStart()[1],
604  (int32_t)msg->getMessageStart()[2], (int32_t)msg->getMessageStart()[3],
605  (int32_t)msg->getMessageStart()[4]);
606  uint32_t written = 0;
608  msg->getTotalMessageSize(), msg->getMessageStart(), &written);
609 
610  if (m_lastResult != XRV_OK)
611  {
612  CMT2LOG(
613  "L2: writeMessage returns %d: %s\n", (int32_t)m_lastResult,
615  return m_lastResult;
616  }
617 
618  if (written != msg->getTotalMessageSize())
619  {
620  CMT2LOG(
621  "L2: writeMessage wrote %u of %u bytes, returns %d: %s\n", written,
624  return (m_lastResult = XRV_ERROR);
625  }
626 
627  if (m_onMessageSent != nullptr)
628  {
629  CmtBinaryData* bytes = (CmtBinaryData*)malloc(sizeof(CmtBinaryData));
630  bytes->m_size = msg->getTotalMessageSize();
631  bytes->m_portNr = m_cmt1s.getPortNr();
632  memcpy(
633  bytes->m_data, msg->getMessageStart(), msg->getTotalMessageSize());
634 #ifdef _LOG_CALLBACKS
635  CMTLOG(
636  "C2: m_onMessageSent(%d,(%d,%d),%p)\n",
639 #endif
643  }
644 
645  CMT2LOG("L2: writeMessage successful\n");
646  return (m_lastResult = XRV_OK);
647 }
648 
649 //////////////////////////////////////////////////////////////////////////////////////////
650 ///////////////////////////////////////// Cmt2f
651 ////////////////////////////////////////////
652 //////////////////////////////////////////////////////////////////////////////////////////
653 
654 //////////////////////////////////////////////////////////////////////////////////////////
655 // Default constructor
657 {
659  m_readOnly = true;
660 }
661 
662 //////////////////////////////////////////////////////////////////////////////////////////
663 // Destructor.
665 //////////////////////////////////////////////////////////////////////////////////////////
666 // Close the file.
668 {
669  if (!m_cmt1f.isOpen()) return m_lastResult = XRV_NOFILEOPEN;
670 
671  // save any unsaved data
672  // close the file
673  m_cmt1f.close();
674  m_readOnly = true;
675  return m_lastResult = XRV_OK;
676 }
677 
678 //////////////////////////////////////////////////////////////////////////////////////////
679 // Close the file and delete it.
681 {
682  if (!m_cmt1f.isOpen()) return m_lastResult = XRV_NOFILEOPEN;
683 
684  // close the file
686  m_readOnly = true;
687  return m_lastResult = XRV_OK;
688 }
689 
690 //////////////////////////////////////////////////////////////////////////////////////////
691 // Create a new file with level 2 header
692 XsensResultValue Cmt2f::create(const char* filename)
693 {
694  if (m_cmt1f.isOpen()) return m_lastResult = XRV_ALREADYOPEN;
695 
696  // create file
697  m_lastResult = m_cmt1f.create(filename);
698  if (m_lastResult != XRV_OK) return m_lastResult;
699 
700  m_readOnly = false;
701 
702  // check if we can actually write to the file
703  m_lastResult = m_cmt1f.writeData(5, "Xsens");
705  if (m_lastResult != XRV_OK) m_cmt1f.close();
706  return m_lastResult;
707 }
708 
709 //////////////////////////////////////////////////////////////////////////////////////////
710 // Create a new file with level 2 header
711 XsensResultValue Cmt2f::create(const wchar_t* filename)
712 {
713  if (m_cmt1f.isOpen()) return m_lastResult = XRV_ALREADYOPEN;
714 
715  // create file
716  m_lastResult = m_cmt1f.create(filename);
717  if (m_lastResult != XRV_OK) return m_lastResult;
718 
719  m_readOnly = false;
720 
721  // check if we can actually write to the file
722  m_lastResult = m_cmt1f.writeData(5, "Xsens");
724  if (m_lastResult != XRV_OK) m_cmt1f.close();
725  return m_lastResult;
726 }
727 
728 //////////////////////////////////////////////////////////////////////////////////////////
729 Cmt1f* Cmt2f::getCmt1f(void) { return &m_cmt1f; }
730 //////////////////////////////////////////////////////////////////////////////////////////
731 // Return the error code of the last operation.
733 //////////////////////////////////////////////////////////////////////////////////////////
734 // Retrieve the filename that was last successfully opened.
735 XsensResultValue Cmt2f::getName(char* filename) const
736 {
737  return m_lastResult = m_cmt1f.getName(filename);
738 }
739 
740 //////////////////////////////////////////////////////////////////////////////////////////
741 // Retrieve the filename that was last successfully opened.
742 XsensResultValue Cmt2f::getName(wchar_t* filename) const
743 {
744  return m_lastResult = m_cmt1f.getName(filename);
745 }
746 
747 //////////////////////////////////////////////////////////////////////////////////////////
748 // Return whether the file is open or not.
749 bool Cmt2f::isOpen(void) const { return m_cmt1f.isOpen(); }
750 //////////////////////////////////////////////////////////////////////////////////////////
751 // Open a file and read the header
752 XsensResultValue Cmt2f::open(const char* filename, const bool readOnly)
753 {
754  if (m_cmt1f.isOpen()) return m_lastResult = XRV_ALREADYOPEN;
755  m_lastResult = m_cmt1f.open(filename, !readOnly, readOnly);
756  m_readOnly = readOnly;
757  return m_lastResult;
758 }
759 
760 //////////////////////////////////////////////////////////////////////////////////////////
761 // Open a file and read the header
762 XsensResultValue Cmt2f::open(const wchar_t* filename, const bool readOnly)
763 {
764  if (m_cmt1f.isOpen()) return m_lastResult = XRV_ALREADYOPEN;
765  m_lastResult = m_cmt1f.open(filename, !readOnly, readOnly);
766  m_readOnly = readOnly;
767  return m_lastResult;
768 }
769 
770 //////////////////////////////////////////////////////////////////////////////////////////
771 // Read the next message from the file
773 {
774  CmtFilePos pos;
775  uint8_t needle = CMT_PREAMBLE;
777  uint32_t length, bcount;
779  bool extended;
780  uint16_t target;
781 
782  while (m_lastResult == XRV_OK)
783  {
784  bcount = 0;
785 
786  // find a message preamble
787  m_lastResult = m_cmt1f.find(&needle, 1, pos);
788 
789  if (m_lastResult != XRV_OK) return m_lastResult;
790 
791  // read header
793  bcount += length;
794  if (m_lastResult != XRV_OK) return m_lastResult;
795 
796  if (hdr->m_length == CMT_EXTLENCODE)
797  {
798  extended = true;
800  CMT_LEN_MSGEXTHEADERCS - bcount, &buffer[bcount], &length);
801  bcount += length;
802  if (m_lastResult != XRV_OK)
803  {
804  m_cmt1f.setReadPos(pos + 1);
805  continue;
806  }
807  }
808  else
809  extended = false;
810 
811  // check the reported size
812  if (extended &&
813  (((uint16_t)hdr->m_datlen.m_extended.m_length.m_high * 256 +
816  {
817  m_cmt1f.setReadPos(pos + 1);
818  continue;
819  }
820 
821  // header seems to be ok, read until end and check checksum
822  if (extended)
823  target = ((uint16_t)hdr->m_datlen.m_extended.m_length.m_high * 256 +
826  else
827  target = hdr->m_length + CMT_LEN_MSGHEADERCS;
828 
829  // read the entire message
830  m_lastResult =
831  m_cmt1f.readData(target - bcount, &buffer[bcount], &length);
832  bcount += length;
833  if (m_lastResult != XRV_OK)
834  {
835  m_cmt1f.setReadPos(pos + 1);
836  continue;
837  }
838 
839  // check the checksum
840  // msg=new Message(m_readBuffer,(uint16_t) target, (uint16_t) target);
841  if (msg->loadFromString(buffer, (uint16_t)target) == XRV_OK)
842  {
843  if ((msgId == 0) || (msgId == msg->getMessageId()))
844  return m_lastResult = XRV_OK;
845  pos += target - 1;
846  }
847  msg->clear();
848  m_cmt1f.setReadPos(pos + 1);
849  }
850  return m_lastResult;
851 }
852 
853 //////////////////////////////////////////////////////////////////////////////////////////
854 // Get the current file size
856 //////////////////////////////////////////////////////////////////////////////////////////
857 // Get the current read position of the file
859 //////////////////////////////////////////////////////////////////////////////////////////
860 // Set the read position to the given position
862 {
863  return m_lastResult = m_cmt1f.setReadPos(pos);
864 }
865 
866 //////////////////////////////////////////////////////////////////////////////////////////
867 // Write a message to the end of the file
869 {
870  if (m_readOnly) return m_lastResult = XRV_READONLY;
872  msg->getTotalMessageSize(), msg->getMessageStart());
873 }
874 
875 } // end of xsens namespace
Operation was performed successfully.
Definition: xsens_std.h:34
A message header.
Definition: cmtmessage.h:36
__int64 CmtFilePos
Definition: cmtf.h:25
unsigned __int16 uint16_t
Definition: rptypes.h:44
XsensResultValue appendData(const uint32_t length, const void *data)
Write data to the end of the file.
Definition: cmt1.cpp:694
#define CMT_MAXDATALEN
Definition: cmtdef.h:75
void getPortName(char *portname) const
Retrieve the port name that was last successfully opened.
Definition: cmt1.h:145
XsensResultValue setReadPos(const CmtFilePos pos)
Set the new absolute read position.
Definition: cmt1.cpp:1272
XsensResultValue open(const char *filename, const bool readOnly=false)
Open a file and read the header.
Definition: cmt2.cpp:752
XsensResultValue writeData(const uint32_t length, const uint8_t *data, uint32_t *written)
Write the data to the serial port.
Definition: cmt1.cpp:633
XsensResultValue getName(char *filename) const
Retrieve the filename that was last successfully opened.
Definition: cmt1.cpp:1010
#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:53
GLuint buffer
Definition: glext.h:3917
XsensResultValue getPortNr(uint8_t &port) const
Retrieve the port that the object is connected to.
Definition: cmt2.cpp:144
~Cmt2f()
Destructor.
Definition: cmt2.cpp:664
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:346
bool isOpen(void) const
Return whether the file is open or not.
Definition: cmt1.h:363
#define CMTLOG(...)
Definition: cmt1.h:41
#define CMT_DEFAULT_BAUD_RATE
The default baud rate of the Cmt1s serial communication.
Definition: cmtdef.h:798
CmtFilePos getReadPos(void) const
Return the current read position.
Definition: cmt1.h:350
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:1220
XsensResultValue writeData(const uint32_t length, const void *data)
Write data to the file.
Definition: cmt1.cpp:1312
XsensResultValue closeAndDelete(void)
Close the file and delete it.
Definition: cmt1.cpp:737
#define CMT_MAXMSGLEN
Definition: cmtdef.h:77
void clear(void)
Clear all data in the message.
Definition: cmtmessage.cpp:147
Callback function, called when a full message has been received from a port.
Definition: cmtdef.h:1140
XsensResultValue deleteData(const CmtFilePos start, const uint32_t length)
Delete the given data from the file. The function erases the given data from the file at the given wr...
Definition: cmt1.cpp:867
CmtCallbackFunction m_onMessageSent
The message sent function.
Definition: cmt2.h:67
XsensResultValue getName(char *filename) const
Retrieve the filename that was last successfully opened.
Definition: cmt2.cpp:735
uint32_t m_toEnd
The timestamp at which to end an operation.
Definition: cmt2.h:90
XsensResultValue readData(const uint32_t maxLength, uint8_t *data, uint32_t *length=nullptr)
Read data from the serial port and put it into the data buffer.
Definition: cmt1.cpp:485
#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:79
XsensResultValue getPortName(char *portname) const
Definition: cmt2.cpp:134
unsigned char uint8_t
Definition: rptypes.h:41
XsensResultValue open(const char *filename, const bool create, const bool readOnly)
Open a file.
Definition: cmt1.cpp:1116
Callback function, called when a full message has been sent by a port.
Definition: cmtdef.h:1142
int32_t m_onMessageReceivedInstance
Custom, user supplied parameter for the OnMessageReceived callback function, passed as the first argu...
Definition: cmt2.h:56
XsensResultValue setTimeout(const uint32_t ms=CMT1_DEFAULT_TIMEOUT)
Set the default timeout value to use in blocking operations.
Definition: cmt1.cpp:561
void * m_onMessageSentParam
Custom, user supplied parameter for the OnMessageSent callback function, passed as the last argument...
Definition: cmt2.h:73
int32_t m_onMessageSentInstance
Custom, user supplied parameter for the OnMessageSent callback function, passed as the first argument...
Definition: cmt2.h:70
XsensResultValue close(void)
Close the file.
Definition: cmt2.cpp:667
XsensResultValue
Xsens return values.
Definition: xsens_std.h:31
XsensResultValue closeAndDelete(void)
Close the file and delete it.
Definition: cmt2.cpp:680
XsensResultValue(__cdecl * CmtCallbackFunction)(int32_t, CmtCallbackSelector, void *, void *)
Definition: cmtdef.h:1160
CmtFilePos getReadPosition(void)
Get the current read position.
Definition: cmt2.cpp:858
#define CMT2LOG(...)
Definition: cmt2.cpp:41
XsensResultValue find(const void *needle, const uint32_t needleLength, CmtFilePos &pos)
Find a string of bytes in the file.
Definition: cmt1.cpp:941
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
#define CMT_MID_ERROR
Definition: cmtdef.h:474
Cmt2s()
Default constructor, initialize all members to their default values.
Definition: cmt2.cpp:108
#define CMT_LEN_MSGEXTHEADERCS
Definition: cmtdef.h:68
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:162
XsensResultValue waitForMessage(Message *rcv, const uint8_t msgId, uint32_t timeoutOverride, bool acceptErrorMessage)
Wait for a message to arrive.
Definition: cmt2.cpp:382
uint16_t m_readBufferCount
The number of valid bytes in the readBuffer.
Definition: cmt2.h:86
uint8_t m_readBuffer[CMT_DEFAULT_READ_BUFFER_SIZE]
Buffer for reading data until a valid message is read.
Definition: cmt2.h:84
XsensResultValue loadFromString(const uint8_t *source, const uint16_t size)
Read the entire message from the given source string.
Definition: cmtmessage.cpp:398
XsensResultValue close(void)
Close the file.
Definition: cmt1.cpp:713
#define CMT_PREAMBLE
Definition: cmtdef.h:60
void * m_onMessageReceivedParam
Custom, user supplied parameter for the OnMessageReceived callback function, passed as the last argum...
Definition: cmt2.h:59
#define CMT_EXTLENCODE
Definition: cmtdef.h:63
int32_t findValidMessage(const uint8_t *buffer, const uint16_t bufferLength)
Find a valid message in the given buffer.
Definition: cmt2.cpp:45
CmtFilePos getFileSize(void)
Get the current file size.
Definition: cmt2.cpp:855
XsensResultValue setReadPosition(CmtFilePos pos)
Set the read position to the given position.
Definition: cmt2.cpp:861
__int32 int32_t
Definition: rptypes.h:46
struct xsens::MessageHeader::_mdl::_mextd m_extended
uint8_t getMessageId(void) const
Return the current value of the m_messageId field.
Definition: cmtmessage.h:236
XsensResultValue close(void)
Close the serial communication port.
Definition: cmt2.cpp:120
Cmt1f * getCmt1f(void)
Get a reference to the embedded Cmt1f object.
Definition: cmt2.cpp:729
const uint8_t * getMessageStart(void) const
Return the start of the message buffer.
Definition: cmtmessage.h:241
XsensResultValue getLastResult(void) const
Return the error code of the last operation.
Definition: cmt2.cpp:732
uint8_t getPortNr(void) const
Retrieve the port number that was last successfully opened.
Definition: cmt1.h:143
XsensResultValue setTimeout(const uint32_t ms=CMT2_DEFAULT_TIMEOUT)
Set the default timeout value to use in blocking operations.
Definition: cmt2.cpp:370
~Cmt2s()
Destructor, de-initialize, free memory allocated for buffers, etc.
Definition: cmt2.cpp:119
CmtFilePos getFileSize(void) const
Return the size of the file.
Definition: cmt1.h:329
XsensResultValue writeMessage(Message *msg)
Send a message over the COM port.
Definition: cmt2.cpp:599
#define CMT_LEN_MSGHEADERCS
Definition: cmtdef.h:67
XsensResultValue m_lastResult
The last result of an operation.
Definition: cmt2.h:203
XsensResultValue create(const char *filename)
Create a new file with level 2 header.
Definition: cmt2.cpp:692
GLuint GLsizei GLsizei * length
Definition: glext.h:4064
const char * xsensResultText(const XsensResultValue result)
Definition: xsens_std.cpp:13
The low-level file communication class.
Definition: cmt1.h:240
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:772
bool isOpen(void) const
Return whether the file is open or not.
Definition: cmt2.cpp:749
CmtCallbackSelector
Definition: cmtdef.h:1120
The namespace of all Xsens software since 2006.
Definition: cmt1.cpp:95
Cmt2f()
Default constructor.
Definition: cmt2.cpp:656
XsensResultValue create(const char *filename)
Open an empty file.
Definition: cmt1.cpp:789
XsensResultValue close(void)
Close the serial communication port.
Definition: cmt1.cpp:205
#define CMT_LEN_MSGHEADER
Definition: cmtdef.h:65
Cmt1f m_cmt1f
The Cmt1f object that is used for the low-level operations.
Definition: cmt2.h:201
GLuint res
Definition: glext.h:7268
struct xsens::MessageHeader::_mdl::_mextd::_mlen m_length
uint16_t getTotalMessageSize(void) const
Return the length of the message buffer.
Definition: cmtmessage.cpp:378
XsensResultValue writeMessage(const Message *msg)
Write a message to the end of the file.
Definition: cmt2.cpp:868
unsigned __int32 uint32_t
Definition: rptypes.h:47
bool isChecksumOk(void) const
Compute the checksum and compare it with the stored checksum.
Definition: cmtmessage.cpp:390
GLfloat param
Definition: glext.h:3831
uint32_t m_baudrate
The baudrate that was last set to be used by the port.
Definition: cmt2.h:77
uint32_t getTimeOfDay(tm *date_, time_t *secs_)
A platform-independent clock.
Definition: xsens_time.cpp:34
uint32_t m_timeout
Timeout in ms for blocking operations.
Definition: cmt2.h:88
uint8_t m_data[CMT_MAXMSGLEN]
Definition: cmtdef.h:1155
bool m_readOnly
When set to true, the file is read-only and attempts to write to it will fail.
Definition: cmt2.h:207
uint8_t m_portNr
Definition: cmtdef.h:1156
Class for storing a single message.
Definition: cmtmessage.h:83
union xsens::MessageHeader::_mdl m_datlen
void memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) noexcept
An OS and compiler independent version of "memcpy".
Definition: os.cpp:355
int32_t m_size
Definition: cmtdef.h:1154
XsensResultValue readMessage(Message *rcv)
Read a message from the COM port.
Definition: cmt2.cpp:192
XsensResultValue m_lastResult
The last result of an operation.
Definition: cmt2.h:81



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019