iCub-main
MTComm.cpp
Go to the documentation of this file.
1 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2 // MTComm.cpp: implementation of the CMTComm class.
3 //
4 // Version 1.2.0
5 // Public release
6 //
7 // v1.2.0
8 // 27-02-2006 - Renamed Xbus class to Motion Tracker C++ Communication class, short MTComm
9 // - Defines XBRV_* accordingly renamed to MTRV_*
10 // - Fixed device length not correct for bid 0 when using Xbus Master and setDeviceMode function
11 //
12 // v1.1.7
13 // 15-02-2006 - Added fixed point signed 12.20 dataformat support
14 // Added selective calibrated data output per sensor type support
15 // Added outputmode temperature support
16 // Fixed warning C4244: '=' : conversion from '' to '', possible loss of data
17 // v1.1.6
18 // 25-01-2006 - Added escape function for CLRDTR, CLRRTS, SETDTR, SETRTS, SETXOFF, SETXON, SETBREAK, CLRBREAK
19 //
20 // v1.1.5
21 // 14-11-2005 - Made swapEndian a static member function, Job Mulder
22 //
23 // v1.1.4
24 // 08-11-2005 - Changed practically all uses of m_timeOut into uses of the new m_clkEnd
25 // - Changed COM timeout in win32 to return immediately if data is available,
26 // but wait 1ms otherwise
27 //
28 // v1.1.3
29 // 18-10-2005 - Added MID_REQPRODUCTCODE, MID_REQ/SETTRANSMITDELAY
30 // - Added MTRV_TIMEOUTNODATA indicating timeout occurred due to no data read
31 //
32 // v1.1.2
33 // 16-09-2005 - Added eMTS version 0.1->1.0 changes (EMTS_FACTORYMODE)
34 // - Added factory output mode defines
35 //
36 // v1.1.1
37 // 01-09-2005 - Added defines for Extended output mode
38 // - Added reqSetting (byte array in + out & no param variant)
39 //
40 // v1.1
41 // 08-08-2005 - Added file read and write support
42 // - Added functions for data retrieval (getValue etc)
43 // for easy data retrieval of acc, gyr, mag etc
44 // - ReadMessageRaw:
45 // - added a temporary buffer for unprocessed bytes
46 // - check for invalid length messages
47 // - Changed BID_MT into 1 and added BID_MASTER (=0xFF)
48 // - Changed various ....SerialPort functions to .....Port
49 // - Changed mtSendMessage functions to mtWriteMessage
50 // - Added numerous defines
51 // - Deleted obsolete functions
52 // - Changed function getLastErrorCode into getLastDeviceError
53 // - Changed OpenPort function for compatiblity with Bluetooth ports
54 // - Added workaround for lockup of USB driver (close function)
55 // - Added workaround for clock() problem with read function of USB driver
56 //
57 // v1.0.2
58 // 29-06-2005 - Inserted initSerialPort with devicename input
59 // - Changed return value defines names from X_ to MTRV_
60 // - Removed unneeded includes for linux
61 //
62 // v1.0.1
63 // 22-06-2005 - Fixed ReqSetting functions (byte array & param variant)
64 // mtSendRawString had wrong length input
65 //
66 // v1.0.0
67 // 20-06-2005 - Initial release
68 //
69 // ----------------------------------------------------------------------------
70 // This file is an Xsens Code Examples.
71 //
72 // Copyright (C) Xsens Technologies B.V., 2005. All rights reserved.
73 //
74 // This source code is intended only as a example of the implementation
75 // of the Xsens MT Communication protocol.
76 // It was written for cross platform capabilities.
77 //
78 // THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY
79 // KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
80 // IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A
81 // PARTICULAR PURPOSE.
83 
84 #include "MTComm.h"
85 
86 #ifdef _DEBUG
87 #undef THIS_FILE
88 static char THIS_FILE[]=__FILE__;
89 #define new DEBUG_NEW
90 #endif
91 
93 // Construction/Destruction
95 //
97 {
98  m_portOpen = false;
99  m_fileOpen = false;
100  m_deviceError = 0; // No error
101  m_retVal = MTRV_OK;
103  m_nTempBufferLen = 0;
104  m_clkEnd = 0;
105  for (int i=0;i<MAXDEVICES+1;i++) {
108  m_storedDataLength[i] = 0;
109  }
110 }
111 
113 {
114  close();
115 }
116 
118 // clockms
119 //
120 // Calculates the processor time used by the calling process.
121 // For linux use gettimeofday instead of clock() function
122 // When using read from FTDI serial port in a loop the
123 // clock() function very often keeps returning 0.
124 //
125 // Output
126 // returns processor time in milliseconds
127 //
129 {
130  clock_t clk;
131 #ifdef WIN32
132  clk = clock(); // Get current processor time
133 #if (CLOCKS_PER_SEC != 1000)
134  clk /= (CLOCKS_PER_SEC / 1000);
135  // clk = (clk * 1000) / CLOCKS_PER_SEC;
136 #endif
137 #else
138  struct timeval tv;
139  struct timezone tz;
140  gettimeofday(&tv, &tz);
141  clk = tv.tv_sec * 1000 + (tv.tv_usec / 1000);
142 #endif
143  return clk;
144 }
145 
147 // openPort (serial port number as input parameter)
148 //
149 // Opens a 'live' connection to a MT or XM
150 //
151 // Input
152 // portNumber : number of serial port to open (1-99)
153 // baudrate : baudrate value (One of the PBR_* defines), default = PBR_115K2
154 // inqueueSize : size of input queue, default = 4096
155 // outqueueSize: size of output queue, default = 1024
156 //
157 // Output
158 // returns MTRV_OK if serial port is successfully opened, else MTRV_INPUTCANNOTBEOPENED
159 //
160 short CMTComm::openPort(const int portNumber, const unsigned long baudrate, const unsigned long inqueueSize, const unsigned long outqueueSize)
161 {
162  m_clkEnd = 0;
163 
164  if (m_fileOpen || m_portOpen) {
166  }
167 #ifdef WIN32
168  char pchFileName[10];
169 
170  sprintf(pchFileName,"\\\\.\\COM%d",portNumber); // Create file name
171 
172  m_handle = CreateFile(pchFileName, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL);
173  if (m_handle == INVALID_HANDLE_VALUE) {
175  }
176 
177  // Once here, port is open
178  m_portOpen = true;
179 
180  //Get the current state & then change it
181  DCB dcb;
182 
183  GetCommState(m_handle, &dcb); // Get current state
184 
185  dcb.BaudRate = baudrate; // Setup the baud rate
186  dcb.Parity = NOPARITY; // Setup the Parity
187  dcb.ByteSize = 8; // Setup the data bits
188  dcb.StopBits = TWOSTOPBITS; // Setup the stop bits
189  dcb.fDsrSensitivity = FALSE; // Setup the flow control
190  dcb.fOutxCtsFlow = FALSE; // NoFlowControl:
191  dcb.fOutxDsrFlow = FALSE;
192  dcb.fOutX = FALSE;
193  dcb.fInX = FALSE;
194  if (!SetCommState(m_handle, (LPDCB)&dcb)) {// Set new state
195  // Bluetooth ports cannot always be opened with 2 stopbits
196  // Now try to open port with 1 stopbit.
197  dcb.StopBits = ONESTOPBIT;
198  if (!SetCommState(m_handle, (LPDCB)&dcb)) {
199  CloseHandle(m_handle);
200  m_handle = INVALID_HANDLE_VALUE;
201  m_portOpen = false;
203  }
204  }
205 
206  // Set COM timeouts
207  COMMTIMEOUTS CommTimeouts;
208 
209  GetCommTimeouts(m_handle,&CommTimeouts); // Fill CommTimeouts structure
210 
211  // immediate return if data is available, wait 1ms otherwise
212  CommTimeouts.ReadTotalTimeoutConstant = 1;
213  CommTimeouts.ReadIntervalTimeout = MAXDWORD;
214  CommTimeouts.ReadTotalTimeoutMultiplier = MAXDWORD;
215 
216  // immediate return whether data is available or not
217  // CommTimeouts.ReadTotalTimeoutConstant = 0;
218  // CommTimeouts.ReadIntervalTimeout = MAXDWORD;
219  // CommTimeouts.ReadTotalTimeoutMultiplier = 0;
220 
221  SetCommTimeouts(m_handle, &CommTimeouts); // Set CommTimeouts structure
222 
223  // Other initialization functions
224  EscapeCommFunction(m_handle, SETRTS); // Enable RTS (for Xbus Master use)
225  SetupComm(m_handle,inqueueSize,outqueueSize); // Set queue size
226 
227  // Remove any 'old' data in buffer
228  PurgeComm(m_handle, PURGE_TXCLEAR | PURGE_RXCLEAR);
229 
230  return (m_retVal = MTRV_OK);
231 #else
232  char chPort[15];
233  struct termios options;
234 
235  /* Open port */
236  sprintf(chPort,"/dev/ttyS%d",(portNumber - 1));
237 
238  m_handle = open(chPort, O_RDWR | O_NOCTTY);
239 
240  // O_RDWR: Read+Write
241  // O_NOCTTY: Raw input, no "controlling terminal"
242  // O_NDELAY: Don't care about DCD signal
243 
244  if (m_handle < 0) {
245  // Port not open
247  }
248 
249  // Once here, port is open
250  m_portOpen = true;
251 
252  /* Start configuring of port for non-canonical transfer mode */
253  // Get current options for the port
254  tcgetattr(m_handle, &options);
255 
256  // Set baudrate.
257  cfsetispeed(&options, baudrate);
258  cfsetospeed(&options, baudrate);
259 
260  // Enable the receiver and set local mode
261  options.c_cflag |= (CLOCAL | CREAD);
262  // Set character size to data bits and set no parity Mask the characte size bits
263  options.c_cflag &= ~(CSIZE|PARENB);
264  options.c_cflag |= CS8; // Select 8 data bits
265  options.c_cflag |= CSTOPB; // send 2 stop bits
266  // Disable hardware flow control
267  options.c_cflag &= ~CRTSCTS;
268  options.c_lflag &= ~(ECHO|ECHONL|ICANON|ISIG|IEXTEN);
269  // Disable software flow control
270  options.c_iflag &= ~(IGNBRK|BRKINT|PARMRK|ISTRIP|INLCR|IGNCR|ICRNL|IXON);
271  // Set Raw output
272  options.c_oflag &= ~OPOST;
273  // Timeout 0.005 sec for first byte, read minimum of 0 bytes
274  options.c_cc[VMIN] = 0;
275  options.c_cc[VTIME] = 5;
276 
277  // Set the new options for the port
278  tcsetattr(m_handle,TCSANOW, &options);
279 
280  tcflush(m_handle, TCIOFLUSH);
281 
282  return (m_retVal = MTRV_OK);
283 #endif
284 }
285 
287 // openPort (string as input parameter)
288 //
289 // Opens a 'live' connection to a MT or XM
290 //
291 // Input
292 // portName : device name of serial port ("/dev/ttyUSB0" or "\\\\.\\COM1")
293 // baudrate : baudrate value (One of the PBR_* defines), default = PBR_115K2
294 // inqueueSize : size of input queue, default = 4096
295 // outqueueSize: size of output queue, default = 1024
296 //
297 // Output
298 // returns MTRV_OK if serial port is successfully opened, else MTRV_INPUTCANNOTBEOPENED
299 //
300 short CMTComm::openPort(const char *portName, const unsigned long baudrate, const unsigned long inqueueSize, const unsigned long outqueueSize)
301 {
302  m_clkEnd = 0;
303 
304  if (m_fileOpen || m_portOpen) {
306  }
307 #ifdef WIN32
308  m_handle = CreateFile(portName, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL);
309  if (m_handle == INVALID_HANDLE_VALUE) {
311  }
312 
313  // Once here, port is open
314  m_portOpen = true;
315 
316  //Get the current state & then change it
317  DCB dcb;
318 
319  GetCommState(m_handle, &dcb); // Get current state
320 
321  dcb.BaudRate = baudrate; // Setup the baud rate
322  dcb.Parity = NOPARITY; // Setup the Parity
323  dcb.ByteSize = 8; // Setup the data bits
324  dcb.StopBits = TWOSTOPBITS; // Setup the stop bits
325  dcb.fDsrSensitivity = FALSE; // Setup the flow control
326  dcb.fOutxCtsFlow = FALSE; // NoFlowControl:
327  dcb.fOutxDsrFlow = FALSE;
328  dcb.fOutX = FALSE;
329  dcb.fInX = FALSE;
330  if (!SetCommState(m_handle, (LPDCB)&dcb)) {// Set new state
331  // Bluetooth ports cannot always be opened with 2 stopbits
332  // Now try to open port with 1 stopbit.
333  dcb.StopBits = ONESTOPBIT;
334  if (!SetCommState(m_handle, (LPDCB)&dcb)) {
335  CloseHandle(m_handle);
336  m_handle = INVALID_HANDLE_VALUE;
337  m_portOpen = false;
339  }
340  }
341 
342  // Set COM timeouts
343  COMMTIMEOUTS CommTimeouts;
344 
345  GetCommTimeouts(m_handle,&CommTimeouts); // Fill CommTimeouts structure
346 
347  // immediate return if data is available, wait 1ms otherwise
348  CommTimeouts.ReadTotalTimeoutConstant = 1;
349  CommTimeouts.ReadIntervalTimeout = MAXDWORD;
350  CommTimeouts.ReadTotalTimeoutMultiplier = MAXDWORD;
351 
352  // immediate return whether data is available or not
353  // CommTimeouts.ReadTotalTimeoutConstant = 0;
354  // CommTimeouts.ReadIntervalTimeout = MAXDWORD;
355  // CommTimeouts.ReadTotalTimeoutMultiplier = 0;
356  SetCommTimeouts(m_handle, &CommTimeouts); // Set CommTimeouts structure
357 
358  // Other initialization functions
359  EscapeCommFunction(m_handle, SETRTS); // Enable RTS (for Xbus Master use)
360  SetupComm(m_handle,inqueueSize,outqueueSize); // Set queue size
361 
362  // Remove any 'old' data in buffer
363  PurgeComm(m_handle, PURGE_TXCLEAR | PURGE_RXCLEAR);
364 
365  return (m_retVal = MTRV_OK);
366 #else
367  struct termios options;
368 
369  /* Open port */
370 
371  m_handle = open(portName, O_RDWR | O_NOCTTY);
372 
373  // O_RDWR: Read+Write
374  // O_NOCTTY: Raw input, no "controlling terminal"
375  // O_NDELAY: Don't care about DCD signal
376 
377  if (m_handle < 0) {
378  // Port not open
380  }
381 
382  // Once here, port is open
383  m_portOpen = true;
384 
385  /* Start configuring of port for non-canonical transfer mode */
386  // Get current options for the port
387  tcgetattr(m_handle, &options);
388 
389  // Set baudrate.
390  cfsetispeed(&options, baudrate);
391  cfsetospeed(&options, baudrate);
392 
393  // Enable the receiver and set local mode
394  options.c_cflag |= (CLOCAL | CREAD);
395  // Set character size to data bits and set no parity Mask the characte size bits
396  options.c_cflag &= ~(CSIZE|PARENB);
397  options.c_cflag |= CS8; // Select 8 data bits
398  options.c_cflag |= CSTOPB; // send 2 stop bits
399  // Disable hardware flow control
400  options.c_cflag &= ~CRTSCTS;
401  options.c_lflag &= ~(ECHO|ECHONL|ICANON|ISIG|IEXTEN);
402  // Disable software flow control
403  options.c_iflag &= ~(IGNBRK|BRKINT|PARMRK|ISTRIP|INLCR|IGNCR|ICRNL|IXON);
404  // Set Raw output
405  options.c_oflag &= ~OPOST;
406  // Timeout 0.005 sec for first byte, read minimum of 0 bytes
407  options.c_cc[VMIN] = 0;
408  options.c_cc[VTIME] = 5;
409 
410  // Set the new options for the port
411  tcsetattr(m_handle,TCSANOW, &options);
412 
413  tcflush(m_handle, TCIOFLUSH);
414 
415  return (m_retVal = MTRV_OK);
416 #endif
417 }
418 
420 // openFile
421 //
422 // Open file for reading and writing
423 // Filepos is at start of file
424 //
425 // Input
426 // fileName : file name including path
427 // createAlways: empties the log file, if existing
428 //
429 // Output
430 // returns MTRV_OK if the file is opened
431 // returns MTRV_INPUTCANNOTBEOPENED if the file can not be opened
432 // returns MTRV_ANINPUTALREADYOPEN if a serial port / file is already opened
433 //
434 short CMTComm::openFile(const char *fileName, bool createAlways)
435 {
436  m_clkEnd = 0;
437 
438  if (m_portOpen || m_portOpen) {
440  }
441 #ifdef WIN32
442  DWORD disposition = OPEN_ALWAYS;
443  if (createAlways == true) {
444  disposition = CREATE_ALWAYS;
445  }
446  m_handle = CreateFile(fileName, GENERIC_READ | GENERIC_WRITE, FILE_SHARE_READ, NULL, disposition, 0, NULL);
447  if (m_handle == INVALID_HANDLE_VALUE) {
449  }
450 #else
451  int openMode = O_RDWR | O_CREAT;
452  if (createAlways == true) {
453  openMode |= O_TRUNC;
454  }
455  m_handle = open(fileName, openMode, S_IRWXU);
456  if (m_handle < 0) {
458  }
459 #endif
460  m_timeOut = 0; // No timeout when using file input
461  m_fileOpen = true;
462  return (m_retVal = MTRV_OK);
463 
464 }
465 
467 // isPortOpen
468 //
469 // Return if serial port is open or not
470 //
472 {
473  return m_portOpen;
474 }
475 
477 // isFileOpen
478 //
479 // Return if serial port is open or not
480 //
482 {
483  return m_fileOpen;
484 }
485 
487 // readData
488 //
489 // Reads bytes from serial port or file
490 //
491 // Input
492 // msgBuffer : pointer to buffer in which next string will be stored
493 // nBytesToRead : number of buffer bytes to read from serial port
494 // retval : return value, either MTRV_OK, MTRV_ENDOFFILE or MTRV_NOINPUTINITIALIZED
495 //
496 // Output
497 // number of bytes actually read
498 int CMTComm::readData(unsigned char* msgBuffer, const int nBytesToRead)
499 {
500  if (!m_fileOpen && !m_portOpen) {
502  return 0;
503  }
504 #ifdef WIN32
505  DWORD nBytesRead;
506  BOOL retval = ReadFile(m_handle, msgBuffer, nBytesToRead, &nBytesRead, NULL);
507  if (retval && nBytesRead == 0 && m_fileOpen) {
509  }
510  else
511  m_retVal = MTRV_OK;
512  return nBytesRead;
513 #else
514  int nBytesRead = read(m_handle, msgBuffer, nBytesToRead);
515  if(nBytesRead == 0 && m_fileOpen) {
516  nBytesRead = 0;
518  }
519  else
520  m_retVal = MTRV_OK;
521  return nBytesRead;
522 
523  // In Linux it is sometimes better to read per byte instead of a block of bytes at once
524  // Use this block if experiencing problems with the above code
525  /*
526  int j = 0; // Index in buffer for read data
527  int k = 0; // Timeout factor
528  int nRead = 0; // Bytes read from port, return by read function
529 
530  do {
531  nRead = read(m_handle, &msgBuffer[j], 1);
532  if (nRead == 1) { // Byte read
533  k = 0;
534  j++;
535  }
536  else {
537  k++;
538  }
539 
540  if (k == 3) { // Timeout, too long no data read from port
541  return nRead;
542  }
543  }
544  while (j < nBytesToRead);
545 
546  return j;
547  */
548 #endif
549 }
550 
552 // writeData
553 //
554 // Writes bytes to serial port (to do: file)
555 //
556 // Input
557 // msgBuffer : a pointer to a char buffer containing
558 // the characters to be written to serial port
559 // nBytesToWrite : number of buffer bytes to write to serial port
560 //
561 // Output
562 // number of bytes actually written
563 //
564 // The MTComm return value is != MTRV_OK if serial port is closed
565 int CMTComm::writeData(const unsigned char* msgBuffer, const int nBytesToWrite)
566 {
567  if (!m_fileOpen && !m_portOpen) {
569  return 0;
570  }
571 
572  m_retVal = MTRV_OK;
573 #ifdef WIN32
574  DWORD nBytesWritten;
575  WriteFile(m_handle, msgBuffer, nBytesToWrite, &nBytesWritten, NULL);
576  return nBytesWritten;
577 #else
578  return write(m_handle, msgBuffer, nBytesToWrite);
579 #endif
580 }
581 
583 // flush
584 //
585 // Remove any 'old' data in COM port buffer and flushes internal
586 // temporary buffer
587 //
589 {
590  if (m_portOpen) {
591 #ifdef WIN32
592  // Remove any 'old' data in buffer
593  PurgeComm(m_handle, PURGE_TXCLEAR | PURGE_RXCLEAR);
594 #else
595  tcflush(m_handle, TCIOFLUSH);
596 #endif
597  }
598  m_nTempBufferLen = 0;
599  m_retVal = MTRV_OK;
600 }
601 
603 // escape
604 //
605 // Directs a COM port to perform an extended function
606 //
607 // Input
608 // function : Windows define. Can be one of following:
609 // CLRDTR, CLRRTS, SETDTR, SETRTS, SETXOFF, SETXON, SETBREAK, CLRBREAK
610 void CMTComm::escape(unsigned long function)
611 {
612 #ifdef WIN32
613  EscapeCommFunction(m_handle, function);
614 #else
615 #endif
616 }
617 
619 // setPortQueueSize
620 //
621 // Set input and output buffer size of serial port
622 //
623 void CMTComm::setPortQueueSize(const unsigned long inqueueSize /* = 4096 */, const unsigned long outqueueSize /* = 1024 */)
624 {
625  if (m_portOpen) {
626 #ifdef WIN32
627  SetupComm(m_handle,inqueueSize,outqueueSize); // Set queue size
628 #else
629  // Not yet implemented
630 #endif
631  }
632  m_retVal = MTRV_OK;
633 }
634 
636 // setFilePos
637 //
638 // Sets the current position of the file pointer for file input
639 //
640 // Input
641 // relPos : 32-bit value specifying the relative move in bytes
642 // origin is specified in moveMethod
643 // moveMethod : FILEPOS_BEGIN, FILEPOS_CURRENT or FILEPOS_END
644 // Output
645 //
646 // returns MTRV_OK if file pointer is successfully set
647 //
648 short CMTComm::setFilePos(long relPos, unsigned long moveMethod)
649 {
650 #ifdef WIN32
651  if (m_fileOpen) {
652  if(SetFilePointer(m_handle, relPos, NULL, moveMethod) != INVALID_SET_FILE_POINTER){
653  return (m_retVal = MTRV_OK);
654  }
655  }
656 #else
657  if (m_fileOpen) {
658  if (lseek(m_handle, relPos, moveMethod) != -1){
659  return (m_retVal = MTRV_OK);
660  }
661  }
662 #endif
663  return (m_retVal = MTRV_NOTSUCCESSFUL);
664 }
665 
667 // getFileSize
668 //
669 // Retrieves the file size of the opened file
670 //
671 // Input
672 // Output
673 // fileSize : Number of bytes of the current file
674 //
675 // returns MTRV_OK if successful
676 //
677 short CMTComm::getFileSize(unsigned long &fileSize)
678 {
679 #ifdef WIN32
680  if (m_fileOpen) {
681  if ((fileSize = GetFileSize(m_handle, NULL)) != INVALID_FILE_SIZE) {
682  return (m_retVal = MTRV_OK);
683  }
684  }
685 #else
686  if(m_fileOpen){
687  struct stat buf;
688  if (fstat(m_handle, &buf) == 0) {
689  fileSize = buf.st_size;
690  return (m_retVal = MTRV_OK);
691  }
692  }
693 #endif
694  return (m_retVal = MTRV_NOTSUCCESSFUL);
695 }
696 
698 // close
699 //
700 // Closes handle of file or serial port
701 //
703 {
704  if (m_portOpen || m_fileOpen) {
705 #ifdef WIN32
706  if (m_portOpen) { // Because of USB-serial driver bug
707  flush();
708  }
709  CloseHandle(m_handle);
710 #else
711  ::close(m_handle);
712 #endif
713  }
714  m_fileOpen = m_portOpen = false;
715  m_timeOut = TO_DEFAULT; // Restore timeout value (file input)
716  m_clkEnd = 0;
717  m_nTempBufferLen = 0;
718  m_deviceError = 0; // No error
719  for(int i=0;i<MAXDEVICES+1;i++){
722  m_storedDataLength[i] = 0;
723  }
724  return (m_retVal = MTRV_OK);
725 }
726 
728 // readMessage
729 //
730 // Reads the next message from serial port buffer or file.
731 // To be read within current time out period
732 //
733 // Input
734 // Output
735 // mid : MessageID of message received
736 // data : array pointer to data bytes (no header)
737 // dataLen : number of data bytes read
738 // bid : BID or address of message read (optional)
739 //
740 // returns MTRV_OK if a message has been read else <>MTRV_OK
741 //
742 // Remarks
743 // allocate enough memory for message buffer
744 // use setTimeOut for different timeout value
745 short CMTComm::readMessage(unsigned char &mid, unsigned char data[], short &dataLen, unsigned char *bid)
746 {
747  unsigned char buffer[MAXMSGLEN];
748  short msgLen;
749 
750  if (!(m_fileOpen || m_portOpen)) {
752  }
753 
754  if (readMessageRaw(buffer, &msgLen) == MTRV_OK) {
755  // Message read
756  mid = buffer[IND_MID];
757  if (bid != NULL) {
758  *bid = buffer[IND_BID];
759  }
760  if (buffer[IND_LEN] != EXTLENCODE) {
761  dataLen = buffer[IND_LEN];
762  memcpy(data, &buffer[IND_DATA0], dataLen);
763  }
764  else{
765  dataLen = buffer[IND_LENEXTH]*256 + buffer[IND_LENEXTL];
766  memcpy(data, &buffer[IND_DATAEXT0], dataLen);
767  }
768  }
769  return m_retVal;
770 }
771 
773 // readDataMessage
774 //
775 // Read a MTData or XMData message from serial port (using TimeOut val)
776 //
777 // Input
778 // data : pointer to buffer in which the DATA field of MTData/XMData is stored
779 // dataLen : number of bytes in buffer (= number bytes in DATA field)
780 // Output
781 // returns MTRV_OK if MTData / XMData message has been read else <>MTRV_OK
782 //
783 // Remarks
784 // allocate enough memory for data buffer
785 // use setTimeOut for different timeout value
786 short CMTComm::readDataMessage(unsigned char data[], short &dataLen)
787 {
788  unsigned char buffer[MAXMSGLEN];
789  short buflen;
790 
791  if (!(m_fileOpen || m_portOpen)) {
793  }
794 
795  if(readMessageRaw(buffer,&buflen) == MTRV_OK){
796  if (buffer[IND_MID] == MID_MTDATA) { // MID_XMDATA is same
797  if (buffer[IND_LEN] != EXTLENCODE) {
798  dataLen = buffer[IND_LEN];
799  memcpy(data, &buffer[IND_DATA0], dataLen);
800  }
801  else{
802  dataLen = buffer[IND_LENEXTH]*256 + buffer[IND_LENEXTL];
803  memcpy(data, &buffer[IND_DATAEXT0], dataLen);
804  }
805  return (m_retVal = MTRV_OK);
806  }
807  else if (buffer[IND_MID] == MID_ERROR){
808  m_deviceError = buffer[IND_DATA0];
809  return (m_retVal = MTRV_RECVERRORMSG); // Error message received
810  }
811  }
812  return m_retVal;
813 }
814 
816 // readMessageRaw
817 //
818 // Read a message from serial port for a certain period
819 //
820 // Input
821 // msgBuffer : pointer to buffer in which next msg will be stored
822 // msgBufferLength: integer to number of bytes written in buffer (header + data + chksum)
823 // Output
824 // returns MTRV_OK if a message has been read else <>MTRV_OK
825 //
826 // Remarks
827 // allocate enough memory for message buffer
828 // use setTimeOut for different timeout value
829 short CMTComm::readMessageRaw(unsigned char *msgBuffer, short *msgBufferLength)
830 {
831  clock_t clkStart, clkEnd;
832  int state = 0;
833  int nBytesToRead = 1;
834  int nBytesRead = 0;
835  int nOffset = 0;
836  int nMsgDataLen = 0;
837  int nMsgLen;
838  unsigned char chCheckSum;
839  bool Synced = false;
840 
841  if (!(m_fileOpen || m_portOpen)) {
843  }
844 
845  // Copy previous read bytes if available
846  if (m_nTempBufferLen > 0) {
847  memcpy(msgBuffer, m_tempBuffer, m_nTempBufferLen);
848  nBytesRead = m_nTempBufferLen;
849  m_nTempBufferLen = 0;
850  }
851  clkStart = clockms(); // Get current processor time
852  clkEnd = m_clkEnd; // check if the end timer is already set
853  if (clkEnd == 0)
854  clkEnd = clkStart + m_timeOut;
855 
856  while(true) {
857  do {
858  // First check if we already have some bytes read
859  if (nBytesRead > 0 && nBytesToRead > 0) {
860  if (nBytesToRead > nBytesRead) {
861  nOffset += nBytesRead;
862  nBytesToRead -= nBytesRead;
863  nBytesRead = 0;
864  }
865  else{
866  nOffset += nBytesToRead;
867  nBytesRead -= nBytesToRead;
868  nBytesToRead = 0;
869  }
870  }
871 
872  // Check if serial port buffer must be read
873  if (nBytesToRead > 0) {
874  nBytesRead = readData(msgBuffer+nOffset, nBytesToRead);
875  if (m_retVal == MTRV_ENDOFFILE) {
876  return (m_retVal = MTRV_ENDOFFILE);
877  }
878  nOffset += nBytesRead;
879  nBytesToRead -= nBytesRead;
880  nBytesRead = 0;
881  }
882 
883  if(nBytesToRead == 0){
884  switch(state){
885  case 0: // Check preamble
886  if(msgBuffer[IND_PREAMBLE] == PREAMBLE){
887  state++;
888  nBytesToRead = 3;
889  }
890  else{
891  nOffset = 0;
892  nBytesToRead = 1;
893  }
894  break;
895  case 1: // Check ADDR, MID, LEN
896  if (msgBuffer[IND_LEN] != 0xFF) {
897  state = 3;
898  nBytesToRead = (nMsgDataLen = msgBuffer[IND_LEN]) + 1; // Read LEN + 1 (chksum)
899  }
900  else {
901  state = 2;
902  nBytesToRead = 2; // Read extended length
903  }
904  break;
905  case 2:
906  state = 3;
907  nBytesToRead = (nMsgDataLen = msgBuffer[IND_LENEXTH] * 256 + msgBuffer[IND_LENEXTL]) + 1; // Read LENEXT + CS
908  if (nMsgDataLen > MAXMSGLEN-LEN_MSGEXTHEADERCS) {
909  // Not synced - check for preamble in the bytes read
910  for (int i = 1; i < LEN_MSGEXTHEADER; i++) {
911  if (msgBuffer[i] == PREAMBLE) {
912  // Found a maybe preamble - 'fill buffer'
913  nBytesRead = LEN_MSGEXTHEADER - i;
914  memmove(msgBuffer, msgBuffer+i,nBytesRead);
915  break;
916  }
917  }
918  Synced = false;
919  nOffset = 0;
920  state = 0;
921  nBytesToRead = 1; // Start looking for preamble
922  }
923  break;
924  case 3: // Check msg
925  chCheckSum = 0;
926  nMsgLen = nMsgDataLen + 5 + (msgBuffer[IND_LEN] == 0xFF?2:0);
927 
928  for(int i = 1; i < nMsgLen; i++){
929  chCheckSum += msgBuffer[i];
930  }
931 
932  if(chCheckSum == 0){ // Checksum ok?
933  if (nBytesRead > 0) { // Store bytes if read too much
934  memcpy(m_tempBuffer, msgBuffer+nMsgLen, nBytesRead);
935  m_nTempBufferLen = nBytesRead;
936  }
937  *msgBufferLength = nMsgLen;
938  Synced = true;
939  return (m_retVal = MTRV_OK);
940  }
941  else{
942  if(!Synced){
943  // Not synced - maybe recheck for preamble in this incorrect message
944  for (int i = 1; i < nMsgLen; i++) {
945  if (msgBuffer[i] == PREAMBLE) {
946  // Found a maybe preamble - 'fill buffer'
947  nBytesRead = nMsgLen - i;
948  memmove(msgBuffer, msgBuffer+i,nBytesRead);
949  break;
950  }
951  }
952  }
953  Synced = false;
954  nOffset = 0;
955  state = 0;
956  nBytesToRead = 1; // Start looking for preamble
957  }
958  break;
959  }
960  }
961  } while((clkEnd >= clockms()) || m_timeOut == 0 || nBytesRead != 0);
962 
963  // Check if pending message has a valid message
964  if(state > 0){
965  int i;
966  // Search for preamble
967  for (i = 1; i < nOffset; i++) {
968  if (msgBuffer[i] == PREAMBLE) {
969  // Found a maybe preamble - 'fill buffer'
970  nBytesRead = nOffset-i-1; // no preamble
971  memmove(msgBuffer+1, msgBuffer+i+1,nBytesRead);
972  break;
973  }
974  }
975  if (i < nOffset) {
976  // Found preamble in message - recheck
977  nOffset = 1;
978  state = 1;
979  nBytesToRead = 3; // Start looking for preamble
980  continue;
981  }
982  }
983  break;
984  }
985 
986  return (m_retVal = MTRV_TIMEOUT);
987 }
988 
990 // writeMessage (optional: integer value)
991 //
992 // Sends a message and in case of an serial port interface it checks
993 // if the return message (ack, error or timeout). See return value
994 // In case an file is opened the functions returns directly after
995 // 'sending' the message
996 //
997 // Use this function for GotoConfig, Reset, ResetOrientation etc
998 //
999 // Input
1000 // mid : MessageID of message to send
1001 // dataValue : A integer value that will be included into the data message field
1002 // can be a 1,2 or 4 bytes values
1003 // dataValueLen : Size of dataValue in bytes
1004 // bid : BID or address to use in message to send (default = 0xFF)
1005 //
1006 // Return value
1007 // = MTRV_OK if an Ack message received / or data successfully written to file
1008 // = MTRV_RECVERRORMSG if an error message received
1009 // = MTRV_TIMEOUT if timeout occurred
1010 // = MTRV_NOINPUTINITIALIZED
1011 //
1012 short CMTComm::writeMessage(const unsigned char mid, const unsigned long dataValue,
1013  const unsigned char dataValueLen, const unsigned char bid)
1014 {
1015  unsigned char buffer[MAXMSGLEN];
1016  short msgLen;
1017 
1018  if (!(m_fileOpen || m_portOpen)) {
1019  return (m_retVal = MTRV_NOINPUTINITIALIZED);
1020  }
1021 
1022  buffer[IND_PREAMBLE] = PREAMBLE;
1023  buffer[IND_BID] = bid;
1024  buffer[IND_MID] = mid;
1025  buffer[IND_LEN] = dataValueLen;
1026  swapEndian((const unsigned char *)&dataValue,&buffer[IND_DATA0],dataValueLen);
1027  calcChecksum(buffer,LEN_MSGHEADER + dataValueLen);
1028 
1029  // Send message
1030  writeData(buffer, LEN_MSGHEADERCS + dataValueLen);
1031 
1032  // Return if file opened
1033  if (m_fileOpen) {
1034  return (m_retVal = MTRV_OK);
1035  }
1036 
1037  // Keep reading until an Ack or Error message is received (or timeout)
1038  clock_t clkStart, clkOld;
1039  bool msgRead = false;
1040 
1041  clkStart = clockms(); // Get current processor time
1042  clkOld = m_clkEnd;
1043  if (clkOld == 0)
1044  m_clkEnd = m_timeOut + clkStart;
1045 
1046  while (m_clkEnd >= clockms() || (m_timeOut == 0)) {
1047  if(readMessageRaw(buffer, &msgLen) == MTRV_OK){
1048  // Message received
1049  msgRead = true;
1050  if(buffer[IND_MID] == (mid+1)) {
1051  m_clkEnd = clkOld;
1052  return (m_retVal = MTRV_OK); // Acknowledge received
1053  }
1054  else if (buffer[IND_MID] == MID_ERROR){
1055  m_deviceError = buffer[IND_DATA0];
1056  m_clkEnd = clkOld;
1057  return (m_retVal = MTRV_RECVERRORMSG); // Error message received
1058  }
1059  }
1060  }
1061 
1062  m_clkEnd = clkOld;
1063  if (msgRead)
1064  return (m_retVal = MTRV_TIMEOUT);
1065  else
1066  return (m_retVal = MTRV_TIMEOUTNODATA);
1067 }
1068 
1070 // writeMessage (data array)
1071 //
1072 // Sends a message and in case of an serial port interface it checks
1073 // if the return message (ack, error or timeout). See return value
1074 // In case an file is opened the functions returns directly after
1075 // 'sending' the message
1076 //
1077 // Input
1078 // mid : MessageID of message to send
1079 // data : array pointer to data bytes
1080 // dataLen : number of bytes to include in message
1081 // bid : BID or address to use in message to send (default = 0xFF)
1082 //
1083 // Output
1084 // = MTRV_OK if an Ack message received
1085 // = MTRV_RECVERRORMSG if an error message received
1086 // = MTRV_TIMEOUT if timeout occurred
1087 // = MTRV_NOINPUTINITIALIZED
1088 //
1089 short CMTComm::writeMessage(const unsigned char mid, const unsigned char data[],
1090  const unsigned short &dataLen, const unsigned char bid)
1091 {
1092  unsigned char buffer[MAXMSGLEN];
1093  short msgLen;
1094  short headerLength;
1095 
1096  if (!(m_fileOpen || m_portOpen)) {
1097  return (m_retVal = MTRV_NOINPUTINITIALIZED);
1098  }
1099 
1100  // Build message to send
1101  buffer[IND_PREAMBLE] = PREAMBLE;
1102  buffer[IND_BID] = bid;
1103  buffer[IND_MID] = mid;
1104 
1105  if (dataLen < EXTLENCODE) {
1106  buffer[IND_LEN] = (unsigned char)dataLen;
1107  headerLength = LEN_MSGHEADER;
1108  }
1109  else {
1110  buffer[IND_LEN] = EXTLENCODE;
1111  buffer[IND_LENEXTH] = (unsigned char)(dataLen >> 8);
1112  buffer[IND_LENEXTL] = (unsigned char)(dataLen & 0x00FF);
1113  headerLength = LEN_MSGEXTHEADER;
1114  }
1115  memcpy(&buffer[headerLength], data, dataLen);
1116  calcChecksum(buffer, headerLength + dataLen);
1117 
1118  // Send message
1119  writeData(buffer, headerLength + dataLen + LEN_CHECKSUM);
1120 
1121  // Return if file opened
1122  if (m_fileOpen) {
1123  return (m_retVal = MTRV_OK);
1124  }
1125 
1126  // Keep reading until an Ack or Error message is received (or timeout)
1127  bool msgRead = false;
1128  clock_t clkStart, clkOld;
1129 
1130  clkStart = clockms(); // Get current processor time
1131  clkOld = m_clkEnd;
1132  if (clkOld == 0)
1133  m_clkEnd = m_timeOut + clkStart;
1134 
1135  while (m_clkEnd >= clockms() || (m_timeOut == 0)) {
1136  if(readMessageRaw(buffer, &msgLen) == MTRV_OK){
1137  // Message received
1138  msgRead = true;
1139  if(buffer[IND_MID] == (mid+1)) {
1140  m_clkEnd = clkOld;
1141  return (m_retVal = MTRV_OK); // Acknowledge received
1142  }
1143  else if (buffer[IND_MID] == MID_ERROR){
1144  m_deviceError = buffer[IND_DATA0];
1145  m_clkEnd = clkOld;
1146  return (m_retVal = MTRV_RECVERRORMSG); // Error message received
1147  }
1148  }
1149  }
1150 
1151  m_clkEnd = clkOld;
1152  if (msgRead)
1153  return (m_retVal = MTRV_TIMEOUT);
1154  else
1155  return (m_retVal = MTRV_TIMEOUTNODATA);
1156 }
1157 
1159 // waitForMessage
1160 //
1161 // Read messages from serial port or file using the current timeout period
1162 // until the received message is equal to a specific message identifier
1163 // By default the timeout period by file input is set to infinity (= until
1164 // end of file is reached)
1165 //
1166 // Input/Output
1167 // mid : message identifier of message that should be returned
1168 // data : pointer to buffer in which the data of the requested msg will be stored
1169 // dataLen : integer to number of data bytes
1170 // bid : optional, pointer which holds the bid of the returned message
1171 // Output
1172 // returns MTRV_OK if the message has been read else != MTRV_OK
1173 //
1174 // Remarks
1175 // allocate enough memory for data message buffer
1176 // use setTimeOut for different timeout value
1177 short CMTComm::waitForMessage(const unsigned char mid, unsigned char data[], short *dataLen, unsigned char *bid)
1178 {
1179  unsigned char buffer[MAXMSGLEN];
1180  short buflen;
1181 
1182  clock_t clkStart, clkOld;
1183 
1184  if (!(m_fileOpen || m_portOpen)) {
1185  return (m_retVal = MTRV_NOINPUTINITIALIZED);
1186  }
1187 
1188  clkStart = clockms(); // Get current processor time
1189  clkOld = m_clkEnd;
1190  if (clkOld == 0)
1191  m_clkEnd = m_timeOut + clkStart;
1192 
1193  while (m_clkEnd >= clockms() || (m_timeOut == 0)) {
1194  if (readMessageRaw(buffer, &buflen) == MTRV_OK) {
1195  if (buffer[IND_MID] == mid) {
1196  if (bid != NULL) {
1197  *bid = buffer[IND_BID];
1198  }
1199  if (data != NULL && dataLen != NULL) {
1200  if (buffer[IND_LEN] != EXTLENCODE) {
1201  *dataLen = buffer[IND_LEN];
1202  memcpy(data, &buffer[IND_DATA0], *dataLen);
1203  }
1204  else{
1205  *dataLen = buffer[IND_LENEXTH]*256 + buffer[IND_LENEXTL];
1206  memcpy(data, &buffer[IND_DATAEXT0], *dataLen);
1207  }
1208  }
1209  else if(dataLen != NULL)
1210  dataLen = 0;
1211  m_clkEnd = clkOld;
1212  return (m_retVal = MTRV_OK);
1213  }
1214  }
1215  else if (m_retVal == MTRV_ENDOFFILE) {
1216  m_clkEnd = clkOld;
1217  return (m_retVal = MTRV_ENDOFFILE);
1218  }
1219  }
1220 
1221  m_clkEnd = clkOld;
1222  return (m_retVal = MTRV_TIMEOUT);
1223 }
1224 
1226 // reqSetting (integer & no param variant)
1227 //
1228 // Request a integer setting from the device. This setting
1229 // can be an unsigned 1,2 or 4 bytes setting. Only valid
1230 // for serial port connections
1231 //
1232 // Input
1233 // mid : Message ID of message to send
1234 // bid : Bus ID of message to send (def 0xFF)
1235 //
1236 // Output
1237 // = MTRV_OK if an Ack message is received
1238 // = MTRV_RECVERRORMSG if an error message is received
1239 // = MTRV_TIMEOUT if timeout occurred
1240 //
1241 // value contains the integer value of the data field of the ack message
1242 //
1243 short CMTComm::reqSetting(const unsigned char mid, unsigned long &value, const unsigned char bid)
1244 {
1245  unsigned char buffer[MAXMSGLEN];
1246  short msgLen;
1247 
1248  if (m_fileOpen) {
1250  }
1251  if (!m_portOpen) {
1252  return (m_retVal = MTRV_NOINPUTINITIALIZED);
1253  }
1254  buffer[IND_PREAMBLE] = PREAMBLE;
1255  buffer[IND_BID] = bid;
1256  buffer[IND_MID] = mid;
1257  buffer[IND_LEN] = 0;
1258  calcChecksum(buffer,LEN_MSGHEADER + buffer[IND_LEN]);
1259 
1260  // Send message
1261  writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
1262 
1263  // Read next message or else timeout
1264  if(readMessageRaw(buffer, &msgLen) == MTRV_OK){
1265  // Message received
1266  if(buffer[IND_MID] == (mid+1)) {
1267  // Acknowledge received
1268  value = 0;
1269  swapEndian(&buffer[IND_DATA0],(unsigned char *)&value, buffer[IND_LEN]);
1270  return (m_retVal = MTRV_OK);
1271  }
1272  else if (buffer[IND_MID] == MID_ERROR){
1273  m_deviceError = buffer[IND_DATA0];
1274  return (m_retVal = MTRV_RECVERRORMSG); // Error message received
1275  }
1276  else{
1277  return (m_retVal = MTRV_UNEXPECTEDMSG);// Unexpected message
1278  }
1279  }
1280  return m_retVal;
1281 }
1282 
1284 // reqSetting (integer & param variant)
1285 //
1286 // Request a integer setting from the device. This setting
1287 // can be an unsigned 1,2 or 4 bytes setting. Only valid
1288 // for serial port connections.
1289 //
1290 // Input
1291 // mid : Message ID of message to send
1292 // param : For messages that need a parameter
1293 // bid : Bus ID of message to send (def 0xFF)
1294 //
1295 // Output
1296 // = MTRV_OK if an Ack message is received
1297 // = MTRV_RECVERRORMSG if an error message is received
1298 // = MTRV_TIMEOUT if timeout occurred
1299 //
1300 // value contains the integer value of the data field of the ack message
1301 //
1302 short CMTComm::reqSetting(const unsigned char mid, const unsigned char param, unsigned long &value,
1303  const unsigned char bid)
1304 {
1305  unsigned char buffer[MAXMSGLEN];
1306  short msgLen;
1307 
1308  if (m_fileOpen) {
1310  }
1311  if (!m_portOpen) {
1312  return (m_retVal = MTRV_NOINPUTINITIALIZED);
1313  }
1314  buffer[IND_PREAMBLE] = PREAMBLE;
1315  buffer[IND_BID] = bid;
1316  buffer[IND_MID] = mid;
1317  if (param != 0xFF) {
1318  buffer[IND_LEN] = 1;
1319  buffer[IND_DATA0] = param;
1320  }
1321  else{
1322  buffer[IND_LEN] = 0;
1323  }
1324  calcChecksum(buffer,LEN_MSGHEADER + buffer[IND_LEN]);
1325 
1326  // Send message
1327  writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
1328 
1329  // Read next message or else timeout
1330  if(readMessageRaw(buffer, &msgLen) == MTRV_OK){
1331  // Message received
1332  if(buffer[IND_MID] == (mid+1)) {
1333  // Acknowledge received
1334  value = 0;
1335  if(param == 0xFF){
1336  swapEndian(&buffer[IND_DATA0],(unsigned char *)&value, buffer[IND_LEN]);
1337  }
1338  else{
1339  swapEndian(&buffer[IND_DATA0]+1,(unsigned char *)&value, buffer[IND_LEN]-1);
1340  }
1341  return (m_retVal = MTRV_OK);
1342  }
1343  else if (buffer[IND_MID] == MID_ERROR){
1344  m_deviceError = buffer[IND_DATA0];
1345  return (m_retVal = MTRV_RECVERRORMSG); // Error message received
1346  }
1347  else{
1348  return (m_retVal = MTRV_UNEXPECTEDMSG);// Unexpected message
1349  }
1350  }
1351  return m_retVal;
1352 }
1353 
1355 // reqSetting (float & no param variant)
1356 //
1357 // Request a float setting from the device. Only valid
1358 // for serial port connections.
1359 //
1360 // Input
1361 // mid : Message ID of message to send
1362 // bid : Bus ID of message to send (def 0xFF)
1363 //
1364 // Output
1365 // = MTRV_OK if an Ack message is received
1366 // = MTRV_RECVERRORMSG if an error message is received
1367 // = MTRV_TIMEOUT if timeout occurred
1368 //
1369 // value contains the float value of the acknowledge data field
1370 //
1371 short CMTComm::reqSetting(const unsigned char mid, float &value, const unsigned char bid)
1372 {
1373  unsigned char buffer[MAXMSGLEN];
1374  short msgLen;
1375 
1376  if (m_fileOpen) {
1378  }
1379  if (!m_portOpen) {
1380  return (m_retVal = MTRV_NOINPUTINITIALIZED);
1381  }
1382  buffer[IND_PREAMBLE] = PREAMBLE;
1383  buffer[IND_BID] = bid;
1384  buffer[IND_MID] = mid;
1385  buffer[IND_LEN] = 0;
1386  calcChecksum(buffer,LEN_MSGHEADER + buffer[IND_LEN]);
1387 
1388  // Send message
1389  writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
1390 
1391  // Read next message or else timeout
1392  if(readMessageRaw(buffer, &msgLen) == MTRV_OK){
1393  // Message received
1394  if(buffer[IND_MID] == (mid+1)) {
1395  // Acknowledge received
1396  value = 0;
1397  swapEndian(&buffer[IND_DATA0],(unsigned char *)&value, buffer[IND_LEN]);
1398  return (m_retVal = MTRV_OK);
1399  }
1400  else if (buffer[IND_MID] == MID_ERROR){
1401  m_deviceError = buffer[IND_DATA0];
1402  return (m_retVal = MTRV_RECVERRORMSG); // Error message received
1403  }
1404  else{
1405  return (m_retVal = MTRV_UNEXPECTEDMSG);// Unexpected message
1406  }
1407  }
1408  return m_retVal;
1409 }
1410 
1412 // reqSetting (float & param variant)
1413 //
1414 // Request a float setting from the device. Only valid
1415 // for serial port connections.
1416 //
1417 // Input
1418 // mid : Message ID of message to send
1419 // param : For messages that need a parameter (optional)
1420 // bid : Bus ID of message to send (def 0xFF)
1421 //
1422 // Output
1423 // = MTRV_OK if an Ack message is received
1424 // = MTRV_RECVERRORMSG if an error message is received
1425 // = MTRV_TIMEOUT if timeout occurred
1426 //
1427 // value contains the float value of the acknowledge data field
1428 //
1429 short CMTComm::reqSetting(const unsigned char mid, const unsigned char param, float &value,
1430  const unsigned char bid)
1431 {
1432  unsigned char buffer[MAXMSGLEN];
1433  short msgLen;
1434 
1435  if (m_fileOpen) {
1437  }
1438  if (!m_portOpen) {
1439  return (m_retVal = MTRV_NOINPUTINITIALIZED);
1440  }
1441  buffer[IND_PREAMBLE] = PREAMBLE;
1442  buffer[IND_BID] = bid;
1443  buffer[IND_MID] = mid;
1444  if (param != 0xFF) {
1445  buffer[IND_LEN] = 1;
1446  buffer[IND_DATA0] = param;
1447  }
1448  else{
1449  buffer[IND_LEN] = 0;
1450  }
1451  calcChecksum(buffer,LEN_MSGHEADER + buffer[IND_LEN]);
1452 
1453  // Send message
1454  writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
1455 
1456  // Read next message or else timeout
1457  if(readMessageRaw(buffer, &msgLen) == MTRV_OK){
1458  // Message received
1459  if(buffer[IND_MID] == (mid+1)) {
1460  // Acknowledge received
1461  value = 0;
1462  if(param == 0xFF){
1463  swapEndian(&buffer[IND_DATA0],(unsigned char *)&value, buffer[IND_LEN]);
1464  }
1465  else{
1466  swapEndian(&buffer[IND_DATA0]+1,(unsigned char *)&value, buffer[IND_LEN]-1);
1467  }
1468  return (m_retVal = MTRV_OK);
1469  }
1470  else if (buffer[IND_MID] == MID_ERROR){
1471  m_deviceError = buffer[IND_DATA0];
1472  return (m_retVal = MTRV_RECVERRORMSG); // Error message received
1473  }
1474  else{
1475  return (m_retVal = MTRV_UNEXPECTEDMSG);// Unexpected message
1476  }
1477  }
1478  return m_retVal;
1479 }
1480 
1482 // reqSetting (byte array & no param variant)
1483 //
1484 // Send a message to the device and the data of acknowledge message
1485 // will be returned. Only valid for serial port connections
1486 //
1487 // Input
1488 // mid : Message ID of message to send
1489 // bid : Bus ID of message to send (def 0xFF)
1490 //
1491 // Output
1492 // = MTRV_OK if an Ack message is received
1493 // = MTRV_RECVERRORMSG if an error message is received
1494 // = MTRV_TIMEOUT if timeout occurred
1495 //
1496 // data[] contains the data of the acknowledge message
1497 // dataLen contains the number bytes returned
1498 //
1499 short CMTComm::reqSetting(const unsigned char mid,
1500  unsigned char data[], short &dataLen, const unsigned char bid)
1501 {
1502  unsigned char buffer[MAXMSGLEN];
1503  short msgLen;
1504 
1505  if (m_fileOpen) {
1507  }
1508  if (!m_portOpen) {
1509  return (m_retVal = MTRV_NOINPUTINITIALIZED);
1510  }
1511  buffer[IND_PREAMBLE] = PREAMBLE;
1512  buffer[IND_BID] = bid;
1513  buffer[IND_MID] = mid;
1514  buffer[IND_LEN] = 0;
1515  calcChecksum(buffer,LEN_MSGHEADER + buffer[IND_LEN]);
1516 
1517  // Send message
1518  writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
1519 
1520  dataLen = 0;
1521  // Read next message or else timeout
1522  if(readMessageRaw(buffer, &msgLen) == MTRV_OK){
1523  // Message received
1524  if(buffer[IND_MID] == (mid+1)) {
1525  // Acknowledge received
1526  if (buffer[IND_LEN] != EXTLENCODE) {
1527  dataLen = buffer[IND_LEN];
1528  memcpy(data, &buffer[IND_DATA0], dataLen);
1529  }
1530  else{
1531  dataLen = buffer[IND_LENEXTH]*256 + buffer[IND_LENEXTL];
1532  memcpy(data, &buffer[IND_DATAEXT0], dataLen);
1533  }
1534  return (m_retVal = MTRV_OK);
1535  }
1536  else if (buffer[IND_MID] == MID_ERROR){
1537  m_deviceError = buffer[IND_DATA0];
1538  return (m_retVal = MTRV_RECVERRORMSG); // Error message received
1539  }
1540  else{
1541  return (m_retVal = MTRV_UNEXPECTEDMSG);// Unexpected message
1542  }
1543  }
1544  return m_retVal;
1545 }
1546 
1548 // reqSetting (byte array in + out & no param variant)
1549 //
1550 // Send a message to the device and the data of acknowledge message
1551 // will be returned. Only valid for serial port connections
1552 //
1553 // Input
1554 // mid : Message ID of message to send
1555 // bid : Bus ID of message to send (def 0xFF)
1556 // dataIn : Data to be included in request
1557 // dataInLen : Number of bytes in dataIn
1558 //
1559 // Output
1560 // = MTRV_OK if an Ack message is received
1561 // = MTRV_RECVERRORMSG if an error message is received
1562 // = MTRV_TIMEOUT if timeout occurred
1563 //
1564 // dataOut[] contains the data of the acknowledge message
1565 // dataOutLen contains the number bytes returned
1566 //
1567 short CMTComm::reqSetting(const unsigned char mid,
1568  unsigned char dataIn[], short dataInLen,
1569  unsigned char dataOut[], short &dataOutLen, const unsigned char bid)
1570 {
1571  unsigned char buffer[MAXMSGLEN];
1572  short headerLength;
1573  short msgLen;
1574 
1575  if (m_fileOpen) {
1577  }
1578  if (!m_portOpen) {
1579  return (m_retVal = MTRV_NOINPUTINITIALIZED);
1580  }
1581  buffer[IND_PREAMBLE] = PREAMBLE;
1582  buffer[IND_BID] = bid;
1583  buffer[IND_MID] = mid;
1584  if (dataInLen < EXTLENCODE) {
1585  buffer[IND_LEN] = (unsigned char)dataInLen;
1586  headerLength = LEN_MSGHEADER;
1587  }
1588  else {
1589  buffer[IND_LEN] = EXTLENCODE;
1590  buffer[IND_LENEXTH] = (unsigned char)(dataInLen >> 8);
1591  buffer[IND_LENEXTL] = (unsigned char)(dataInLen & 0x00FF);
1592  headerLength = LEN_MSGEXTHEADER;
1593  }
1594  memcpy(&buffer[headerLength], dataIn, dataInLen);
1595  calcChecksum(buffer, headerLength + dataInLen);
1596 
1597  // Send message
1598  writeData(buffer, headerLength + dataInLen + LEN_CHECKSUM);
1599 
1600  dataOutLen = 0;
1601  // Read next message or else timeout
1602  if(readMessageRaw(buffer, &msgLen) == MTRV_OK){
1603  // Message received
1604  if(buffer[IND_MID] == (mid+1)) {
1605  // Acknowledge received
1606  if (buffer[IND_LEN] != EXTLENCODE) {
1607  dataOutLen = buffer[IND_LEN];
1608  memcpy(dataOut, &buffer[IND_DATA0], dataOutLen);
1609  }
1610  else{
1611  dataOutLen = buffer[IND_LENEXTH]*256 + buffer[IND_LENEXTL];
1612  memcpy(dataOut, &buffer[IND_DATAEXT0], dataOutLen);
1613  }
1614  return (m_retVal = MTRV_OK);
1615  }
1616  else if (buffer[IND_MID] == MID_ERROR){
1617  m_deviceError = buffer[IND_DATA0];
1618  return (m_retVal = MTRV_RECVERRORMSG); // Error message received
1619  }
1620  else{
1621  return (m_retVal = MTRV_UNEXPECTEDMSG);// Unexpected message
1622  }
1623  }
1624  return m_retVal;
1625 }
1626 
1627 
1629 // reqSetting (byte array & param variant)
1630 //
1631 // Send a message to the device and the data of acknowledge message
1632 // will be returned. Only valid for serial port connections
1633 //
1634 // Input
1635 // mid : Message ID of message to send
1636 // param : For messages that need a parameter (optional)
1637 // bid : Bus ID of message to send (def 0xFF)
1638 //
1639 // Output
1640 // = MTRV_OK if an Ack message is received
1641 // = MTRV_RECVERRORMSG if an error message is received
1642 // = MTRV_TIMEOUT if timeout occurred
1643 //
1644 // data[] contains the data of the acknowledge message (including param!!)
1645 // dataLen contains the number bytes returned
1646 //
1647 short CMTComm::reqSetting(const unsigned char mid, const unsigned char param,
1648  unsigned char data[], short &dataLen, const unsigned char bid)
1649 {
1650  unsigned char buffer[MAXMSGLEN];
1651  short msgLen;
1652 
1653  if (m_fileOpen) {
1655  }
1656  if (!m_portOpen) {
1657  return (m_retVal = MTRV_NOINPUTINITIALIZED);
1658  }
1659  buffer[IND_PREAMBLE] = PREAMBLE;
1660  buffer[IND_BID] = bid;
1661  buffer[IND_MID] = mid;
1662  if (param != 0xFF) {
1663  buffer[IND_LEN] = 1;
1664  buffer[IND_DATA0] = param;
1665  }
1666  else{
1667  buffer[IND_LEN] = 0;
1668  }
1669  calcChecksum(buffer,LEN_MSGHEADER + buffer[IND_LEN]);
1670 
1671  // Send message
1672  writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
1673 
1674  dataLen = 0;
1675  // Read next message or else timeout
1676  if(readMessageRaw(buffer, &msgLen) == MTRV_OK){
1677  // Message received
1678  if(buffer[IND_MID] == (mid+1)) {
1679  // Acknowledge received
1680  if (buffer[IND_LEN] != EXTLENCODE) {
1681  dataLen = buffer[IND_LEN];
1682  memcpy(data, &buffer[IND_DATA0], dataLen);
1683  }
1684  else{
1685  dataLen = buffer[IND_LENEXTH]*256 + buffer[IND_LENEXTL];
1686  memcpy(data, &buffer[IND_DATAEXT0], dataLen);
1687  }
1688  return (m_retVal = MTRV_OK);
1689  }
1690  else if (buffer[IND_MID] == MID_ERROR){
1691  m_deviceError = buffer[IND_DATA0];
1692  return (m_retVal = MTRV_RECVERRORMSG); // Error message received
1693  }
1694  else{
1695  return (m_retVal = MTRV_UNEXPECTEDMSG);// Unexpected message
1696  }
1697  }
1698  return m_retVal;
1699 }
1700 
1702 // setSetting (integer & no param variant)
1703 //
1704 // Sets a integer setting of the device. This setting
1705 // can be an unsigned 1,2 or 4 bytes setting. Only valid
1706 // for serial port connections.
1707 //
1708 // Input
1709 // mid : Message ID of message to send
1710 // bid : Bus ID of message to send (def 0xFF)
1711 // value : Contains the integer value to be used
1712 // valuelen : Length in bytes of the value
1713 //
1714 // Output
1715 // = MTRV_OK if an Ack message is received
1716 // = MTRV_RECVERRORMSG if an error message is received
1717 // = MTRV_TIMEOUT if timeout occurred
1718 //
1719 //
1720 short CMTComm::setSetting(const unsigned char mid,
1721  const unsigned long value, const unsigned short valuelen,
1722  const unsigned char bid)
1723 {
1724  unsigned char buffer[MAXMSGLEN];
1725  short msgLen;
1726 
1727  if (m_fileOpen) {
1729  }
1730  if (!m_portOpen) {
1731  return (m_retVal = MTRV_NOINPUTINITIALIZED);
1732  }
1733  msgLen = LEN_MSGHEADER;
1734  buffer[IND_PREAMBLE] = PREAMBLE;
1735  buffer[IND_BID] = bid;
1736  buffer[IND_MID] = mid;
1737  buffer[IND_LEN] = (unsigned char)valuelen;
1738  swapEndian((unsigned char *)&value, &buffer[msgLen], valuelen);
1739  calcChecksum(buffer,LEN_MSGHEADER + buffer[IND_LEN]);
1740 
1741  // Send message
1742  writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
1743 
1744  // Read next received message
1745  if(readMessageRaw(buffer, &msgLen) == MTRV_OK){
1746  // Message received
1747  if(buffer[IND_MID] == (mid+1)) {
1748  return (m_retVal = MTRV_OK); // Acknowledge received
1749  }
1750  else if (buffer[IND_MID] == MID_ERROR){
1751  m_deviceError = buffer[IND_DATA0];
1752  return (m_retVal = MTRV_RECVERRORMSG); // Error message received
1753  }
1754  }
1755  return m_retVal;
1756 }
1757 
1759 // setSetting (integer & param variant)
1760 //
1761 // Sets a integer setting of the device. This setting
1762 // can be an unsigned 1,2 or 4 bytes setting. Only valid
1763 // for serial port connections.
1764 //
1765 // Input
1766 // mid : Message ID of message to send
1767 // param : For messages that need a parameter (optional)
1768 // bid : Bus ID of message to send (def 0xFF)
1769 // value : Contains the integer value to be used
1770 // valuelen : Length in bytes of the value
1771 //
1772 // Output
1773 // = MTRV_OK if an Ack message is received
1774 // = MTRV_RECVERRORMSG if an error message is received
1775 // = MTRV_TIMEOUT if timeout occurred
1776 //
1777 //
1778 short CMTComm::setSetting(const unsigned char mid, const unsigned char param,
1779  const unsigned long value, const unsigned short valuelen,
1780  const unsigned char bid)
1781 {
1782  unsigned char buffer[MAXMSGLEN];
1783  short msgLen;
1784 
1785  if (m_fileOpen) {
1787  }
1788  if (!m_portOpen) {
1789  return (m_retVal = MTRV_NOINPUTINITIALIZED);
1790  }
1791  msgLen = LEN_MSGHEADER;
1792  buffer[IND_PREAMBLE] = PREAMBLE;
1793  buffer[IND_BID] = bid;
1794  buffer[IND_MID] = mid;
1795  if (param != 0xFF) {
1796  msgLen++;
1797  buffer[IND_LEN] = valuelen+1;
1798  buffer[IND_DATA0] = param;
1799  }
1800  else{
1801  buffer[IND_LEN] = (unsigned char)valuelen;
1802  }
1803  swapEndian((unsigned char *)&value, &buffer[msgLen], valuelen);
1804  calcChecksum(buffer,LEN_MSGHEADER + buffer[IND_LEN]);
1805 
1806  // Send message
1807  writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
1808 
1809  // Read next received message
1810  if(readMessageRaw(buffer, &msgLen) == MTRV_OK){
1811  // Message received
1812  if(buffer[IND_MID] == (mid+1)) {
1813  return (m_retVal = MTRV_OK); // Acknowledge received
1814  }
1815  else if (buffer[IND_MID] == MID_ERROR){
1816  m_deviceError = buffer[IND_DATA0];
1817  return (m_retVal = MTRV_RECVERRORMSG); // Error message received
1818  }
1819  }
1820  return m_retVal;
1821 }
1822 
1824 // setSetting (float & no param variant)
1825 //
1826 // Sets a float setting of the device. Only valid
1827 // for serial port connections.
1828 //
1829 // Input
1830 // mid : Message ID of message to send
1831 // bid : Bus ID of message to send (def 0xFF)
1832 // value : Contains the float value to be used
1833 //
1834 // Output
1835 // = MTRV_OK if an Ack message is received
1836 // = MTRV_RECVERRORMSG if an error message is received
1837 // = MTRV_TIMEOUT if timeout occurred
1838 //
1839 short CMTComm::setSetting(const unsigned char mid, const float value, const unsigned char bid)
1840 {
1841  unsigned char buffer[MAXMSGLEN];
1842  short msgLen;
1843 
1844  if (m_fileOpen) {
1846  }
1847  if (!m_portOpen) {
1848  return (m_retVal = MTRV_NOINPUTINITIALIZED);
1849  }
1850  msgLen = LEN_MSGHEADER;
1851  buffer[IND_PREAMBLE] = PREAMBLE;
1852  buffer[IND_BID] = bid;
1853  buffer[IND_MID] = mid;
1854  buffer[IND_LEN] = LEN_FLOAT;
1855  swapEndian((unsigned char *)&value, &buffer[msgLen], LEN_FLOAT);
1857 
1858  // Send message
1859  writeData(buffer, LEN_MSGHEADERCS + LEN_FLOAT);
1860 
1861  // Read next received message
1862  if(readMessageRaw(buffer, &msgLen) == MTRV_OK){
1863  // Message received
1864  if(buffer[IND_MID] == (mid+1)) {
1865  return (m_retVal = MTRV_OK); // Acknowledge received
1866  }
1867  else if (buffer[IND_MID] == MID_ERROR){
1868  m_deviceError = buffer[IND_DATA0];
1869  return (m_retVal = MTRV_RECVERRORMSG); // Error message received
1870  }
1871  }
1872  return m_retVal;
1873 }
1874 
1876 // setSetting (float & param variant)
1877 //
1878 // Sets a float setting of the device. Only valid
1879 // for serial port connections.
1880 //
1881 // Input
1882 // mid : Message ID of message to send
1883 // param : For messages that need a parameter (optional)
1884 // bid : Bus ID of message to send (def 0xFF)
1885 // value : Contains the float value to be used
1886 //
1887 // Output
1888 // = MTRV_OK if an Ack message is received
1889 // = MTRV_RECVERRORMSG if an error message is received
1890 // = MTRV_TIMEOUT if timeout occurred
1891 //
1892 //
1893 short CMTComm::setSetting(const unsigned char mid, const unsigned char param,
1894  const float value, const unsigned char bid)
1895 {
1896  unsigned char buffer[MAXMSGLEN];
1897  short msgLen;
1898 
1899  if (m_fileOpen) {
1901  }
1902  if (!m_portOpen) {
1903  return (m_retVal = MTRV_NOINPUTINITIALIZED);
1904  }
1905  msgLen = LEN_MSGHEADER;
1906  buffer[IND_PREAMBLE] = PREAMBLE;
1907  buffer[IND_BID] = bid;
1908  buffer[IND_MID] = mid;
1909  if (param != 0xFF) {
1910  msgLen++;
1911  buffer[IND_LEN] = LEN_FLOAT+1;
1912  buffer[IND_DATA0] = param;
1913  }
1914  else{
1915  buffer[IND_LEN] = LEN_FLOAT;
1916  }
1917  swapEndian((unsigned char *)&value, &buffer[msgLen], LEN_FLOAT);
1918  calcChecksum(buffer,LEN_MSGHEADER + buffer[IND_LEN]);
1919 
1920  // Send message
1921  writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
1922 
1923  // Read next received message
1924  if(readMessageRaw(buffer, &msgLen) == MTRV_OK){
1925  // Message received
1926  if(buffer[IND_MID] == (mid+1)) {
1927  return (m_retVal = MTRV_OK); // Acknowledge received
1928  }
1929  else if (buffer[IND_MID] == MID_ERROR){
1930  m_deviceError = buffer[IND_DATA0];
1931  return (m_retVal = MTRV_RECVERRORMSG); // Error message received
1932  }
1933  }
1934  return m_retVal;
1935 }
1936 
1938 // setSetting (float & param & store variant)
1939 //
1940 // Sets a float setting of the device and with the Store field.
1941 // Only valid for serial port connections
1942 //
1943 // Input
1944 // mid : Message ID of message to send
1945 // param : For messages that need a parameter (optional)
1946 // value : Contains the float value to be used
1947 // store ; Store in non-volatile memory (1) or not (0)
1948 // bid : Bus ID of message to send (def 0xFF)
1949 //
1950 // Output
1951 // = MTRV_OK if an Ack message is received
1952 // = MTRV_RECVERRORMSG if an error message is received
1953 // = MTRV_TIMEOUT if timeout occurred
1954 //
1955 //
1956 short CMTComm::setSetting(const unsigned char mid, const unsigned char param,
1957  const float value, const bool store, const unsigned char bid)
1958 {
1959  unsigned char buffer[MAXMSGLEN];
1960  short msgLen;
1961 
1962  if (m_fileOpen) {
1964  }
1965  if (!m_portOpen) {
1966  return (m_retVal = MTRV_NOINPUTINITIALIZED);
1967  }
1968  msgLen = LEN_MSGHEADER;
1969  buffer[IND_PREAMBLE] = PREAMBLE;
1970  buffer[IND_BID] = bid;
1971  buffer[IND_MID] = mid;
1972  if (param != 0xFF) {
1973  msgLen++;
1974  buffer[IND_LEN] = LEN_FLOAT+2;
1975  buffer[IND_DATA0] = param;
1976  }
1977  else{
1978  buffer[IND_LEN] = LEN_FLOAT+1;
1979  }
1980  swapEndian((unsigned char *)&value, &buffer[msgLen], LEN_FLOAT);
1981  buffer[msgLen+LEN_FLOAT] = store;
1982  calcChecksum(buffer,LEN_MSGHEADER + buffer[IND_LEN]);
1983 
1984  // Send message
1985  writeData(buffer, LEN_MSGHEADERCS + buffer[IND_LEN]);
1986 
1987  // Read next received message
1988  if(readMessageRaw(buffer, &msgLen) == MTRV_OK){
1989  // Message received
1990  if(buffer[IND_MID] == (mid+1)) {
1991  return (m_retVal = MTRV_OK); // Acknowledge received
1992  }
1993  else if (buffer[IND_MID] == MID_ERROR){
1994  m_deviceError = buffer[IND_DATA0];
1995  return (m_retVal = MTRV_RECVERRORMSG); // Error message received
1996  }
1997  }
1998  return m_retVal;
1999 }
2000 
2002 // getDeviceMode
2003 //
2004 // Requests the current output mode/setting of input (file or serialport)
2005 // the Outputmode, Outputsettings, DataLength & number of devices
2006 // are stored in member variables of the MTComm class. These values
2007 // are needed for the GetValue functions.
2008 // The function optionally returns the number of devices
2009 //
2010 // File: expects the Configuration message at the start of the file
2011 // which holds the OutputMode & OutputSettings. File position
2012 // is after the first message
2013 //
2014 // Input
2015 // Output
2016 // numDevices : [optional] number of devices connected to port or
2017 // found in configuration file
2018 //
2019 // returns MTRV_OK if the mode & settings are read
2020 //
2021 short CMTComm::getDeviceMode(unsigned short *numDevices)
2022 {
2023  unsigned char mid, data[MAXMSGLEN];
2024  short datalen;
2025 
2026  if (numDevices != NULL) {
2027  *numDevices = 0;
2028  }
2029 
2030  // In case serial port is used (live device / XM or MT)
2031  if (m_portOpen) {
2032  if (reqSetting(MID_INITBUS,data,datalen) != MTRV_OK) {
2033  return m_retVal;
2034  }
2035 
2036  // Retrieve outputmode + outputsettings
2037  for (int i = 0; i < datalen / LEN_DEVICEID; i++) {
2039  return m_retVal;
2040  }
2041 
2043  return m_retVal;
2044  }
2045 
2047  return m_retVal;
2048  }
2049  }
2050 
2051  if (numDevices != NULL) {
2052  *numDevices = datalen / LEN_DEVICEID;
2053  }
2054 
2055  unsigned char masterDID[4];
2056  short DIDlen;
2057 
2058  if(reqSetting(MID_REQDID,masterDID,DIDlen) != MTRV_OK) {
2059  return m_retVal;
2060  }
2061 
2062  if (memcmp(masterDID,data,LEN_DEVICEID) != 0) {
2063  // Using an XbusMaster
2067  }
2068  else{
2072  }
2073  return (m_retVal = MTRV_OK);
2074  }
2075  else if(m_fileOpen){
2076  // Configuration message should be the first message in the file
2077  setFilePos(0);
2078  if (readMessage(mid,data,datalen) == MTRV_OK) {
2079  if (mid == MID_CONFIGURATION) {
2080  unsigned short _numDevices = 0;
2081  swapEndian(data+CONF_NUMDEVICES, (unsigned char *)&_numDevices,CONF_NUMDEVICESLEN);
2082  for(unsigned int i = 0; i < _numDevices; i++){
2083  m_storedOutputMode[BID_MT+i] = 0;
2089  m_storedDataLength[BID_MT+i] = 0;
2092  }
2093  if (numDevices != NULL) {
2094  *numDevices = _numDevices;
2095  }
2096  if (memcmp(data+CONF_MASTERDID, data+CONF_DID, LEN_DEVICEID) != 0) {
2097  // Using an XbusMaster
2101  }
2102  else{
2106  }
2107  return (m_retVal = MTRV_OK);
2108  }
2109  }
2110  return (m_retVal = MTRV_NOTSUCCESSFUL);
2111  }
2112  return (m_retVal = MTRV_NOINPUTINITIALIZED);
2113 }
2114 
2116 // setDeviceMode
2117 //
2118 // Sets the current output mode/setting of input (not for file-based
2119 // inputs)
2120 //
2121 // Input
2122 // OutputMode : OutputMode to be set in device & stored in MTComm
2123 // class member variable
2124 // OutputSettings : OutputSettings to be set in device & stored in
2125 // MTComm class member variable
2126 // Output
2127 //
2128 // returns MTRV_OK if the mode & settings are read
2129 //
2130 short CMTComm::setDeviceMode(unsigned long OutputMode, unsigned long OutputSettings, const unsigned char bid)
2131 {
2132  // In case serial port is used (live XM / MT)
2133  if (m_portOpen) {
2134  // Set OutputMode
2135  if (setSetting(MID_SETOUTPUTMODE, OutputMode, LEN_OUTPUTMODE, bid) != MTRV_OK) {
2136  return m_retVal;
2137  }
2138  if (bid == BID_MASTER || (bid == BID_MT && m_storedOutputMode[0] != OUTPUTMODE_XM)) {
2139  m_storedOutputMode[0] = m_storedOutputMode[BID_MT] = OutputMode;
2140  }
2141  else{
2142  m_storedOutputMode[bid] = OutputMode;
2143  }
2144  // Set OutputSettings
2145  if (setSetting(MID_SETOUTPUTSETTINGS, OutputSettings, LEN_OUTPUTSETTINGS, bid) != MTRV_OK) {
2146  return m_retVal;
2147  }
2148  if (bid == BID_MASTER || (bid == BID_MT && m_storedOutputMode[0] != OUTPUTMODE_XM)) {
2149  m_storedOutputSettings[0] = m_storedOutputSettings[BID_MT] = OutputSettings;
2150  }
2151  else{
2152  m_storedOutputSettings[bid] = OutputSettings;
2153  }
2154  // Get DataLength from device
2155  if (OutputMode != OUTPUTMODE_XM) {
2156  unsigned long value;
2157  if (reqSetting(MID_REQDATALENGTH, value, bid) == MTRV_OK) {
2158  if (bid == BID_MASTER || bid == BID_MT && m_storedOutputMode[0] != OUTPUTMODE_XM) {
2160  }
2161  else{
2162  m_storedDataLength[bid] = value;
2163  }
2164  }
2165  }else{
2167  }
2168  return (m_retVal = MTRV_OK);
2169  }
2171 }
2172 
2174 // getMode
2175 //
2176 // Gets the output mode/setting used in MTComm class and the corresponding
2177 // datalength. These variables are set by the functions GetDeviceMode,
2178 // SetDeviceMode or SetMode
2179 //
2180 // Input
2181 // Output
2182 // OutputMode : OutputMode stored in MTComm class member variable
2183 // OutputSettings : OutputSettings stored in MTComm class member variable
2184 //
2185 // returns always MTRV_OK
2186 //
2187 short CMTComm::getMode(unsigned long &OutputMode, unsigned long &OutputSettings,
2188  unsigned short &dataLength, const unsigned char bid)
2189 {
2190  unsigned char nbid = (bid == BID_MASTER)?0:bid;
2191  OutputMode = m_storedOutputMode[nbid];
2192  OutputSettings = m_storedOutputSettings[nbid];
2193  dataLength = (unsigned short)m_storedDataLength[nbid];
2194  return (m_retVal = MTRV_OK);
2195 }
2196 
2198 // setMode
2199 //
2200 // Sets the output mode/setting used in MTComm class. Use the function
2201 // GetDeviceMode to retrieve the current values of file/device.
2202 // This function will also calculate the data length field
2203 //
2204 // Input
2205 // OutputMode : OutputMode to be stored in MTComm class member variable
2206 // OutputSettings : OutputSettings to be stored in MTComm class member variable
2207 // Output
2208 //
2209 // returns always MTRV_OK
2210 //
2211 short CMTComm::setMode(unsigned long OutputMode, unsigned long OutputSettings, const unsigned char bid)
2212 {
2213  unsigned char nbid = bid;
2214 
2215  if (nbid == BID_MASTER){
2216  nbid = 0;
2217  }
2218  m_storedOutputMode[nbid] = OutputMode;
2219  m_storedOutputSettings[nbid] = OutputSettings;
2220  if (OutputMode == INVALIDSETTINGVALUE || OutputSettings == INVALIDSETTINGVALUE) {
2221  m_storedDataLength[nbid] = 0;
2222  }
2223  else{
2224  unsigned short dataLength = 0;
2225  if (OutputMode & OUTPUTMODE_MT9) {
2227  }
2228  else if (OutputMode == OUTPUTMODE_XM){
2229  // XbusMaster concatenates sample counter
2230  dataLength = LEN_SAMPLECNT;
2231  }
2232  else{
2233  if (OutputMode & OUTPUTMODE_RAW) {
2234  dataLength = LEN_RAWDATA;
2235  }
2236  else{
2237  if (OutputMode & OUTPUTMODE_CALIB) {
2238  dataLength = LEN_CALIBDATA;
2239  }
2240  if (OutputMode & OUTPUTMODE_ORIENT) {
2241  switch(OutputSettings & OUTPUTSETTINGS_ORIENTMODE_MASK) {
2243  dataLength += LEN_ORIENT_QUATDATA;
2244  break;
2246  dataLength += LEN_ORIENT_EULERDATA;
2247  break;
2249  dataLength += LEN_ORIENT_MATRIXDATA;
2250  break;
2251  default:
2252  break;
2253  }
2254  }
2255  }
2256  switch(OutputSettings & OUTPUTSETTINGS_TIMESTAMP_MASK) {
2258  dataLength += LEN_SAMPLECNT;
2259  break;
2260  default:
2261  break;
2262  }
2263  }
2264  m_storedDataLength[nbid] = dataLength;
2265  }
2266  // If not XbusMaster store also in BID_MT
2267  if (bid == BID_MASTER && OutputMode != OUTPUTMODE_XM) {
2271  }
2272  return (m_retVal = MTRV_OK);
2273 }
2274 
2276 // getValue (unsigned short variant)
2277 //
2278 // Retrieves a unsigned short value from the data input parameter
2279 // This function is valid for the following value specifiers:
2280 // VALUE_RAW_TEMP
2281 // VALUE_SAMPLECNT
2282 //
2283 // Use getDeviceMode or setMode to initialize the Outputmode
2284 // and Outputsettings member variables used to retrieve the correct
2285 // value
2286 //
2287 // Input
2288 // valueSpec : Specifier of the value to be retrieved
2289 // data[] : Data field of a MTData / BusData message
2290 // bid : bus identifier of the device of which the
2291 // value should be returned (default = BID_MT)
2292 // Output
2293 // value : reference to unsigned short in which the retrieved
2294 // value will be returned
2295 //
2296 // Return value
2297 // MTRV_OK : value is successfully retrieved
2298 // != MTRV_OK : not successful
2299 //
2300 short CMTComm::getValue(const unsigned long valueSpec, unsigned short &value, const unsigned char data[],
2301  const unsigned char bid)
2302 {
2303  short offset = 0;
2304  unsigned char nbid = bid;
2305 
2306  if(nbid == BID_MASTER){
2307  nbid = 0;
2308  }
2309 
2310  // Check for invalid mode/settings
2313  }
2314 
2315  // Calculate offset for XM input
2316  if (m_storedOutputMode[0] == OUTPUTMODE_XM) {
2317  int i = 0;
2318  while (i < nbid) {
2319  offset += (short)m_storedDataLength[i++];
2320  }
2321  }
2322 
2323  // Check if data is unsigned short & available in data
2325  if (valueSpec == VALUE_RAW_TEMP) {
2327  offset += (m_storedOutputMode[nbid] == OUTPUTMODE_MT9 && \
2328  m_storedOutputSettings[nbid] == OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT)?LEN_SAMPLECNT:0;
2329  swapEndian(data + offset + valueSpec*LEN_UNSIGSHORT*3, (unsigned char *)&value, LEN_RAW_TEMP);
2330  m_retVal = MTRV_OK;
2331  }
2332  }
2333  else if (valueSpec == VALUE_SAMPLECNT) {
2335  if (!(m_storedOutputMode[nbid] == OUTPUTMODE_MT9)) {
2336  offset += (short)m_storedDataLength[nbid] - LEN_SAMPLECNT;
2337  }
2338  swapEndian(data + offset, (unsigned char *)&value, LEN_SAMPLECNT);
2339  m_retVal = MTRV_OK;
2340  }
2341  }
2342  return m_retVal;
2343 }
2344 
2346 // getValue (array of unsigned short variant)
2347 //
2348 // Retrieves an array of unsigned short values from the data input
2349 // parameter. This function is valid for the following value specifiers:
2350 // VALUE_RAW_ACC
2351 // VALUE_RAW_GYR
2352 // VALUE_RAW_MAG
2353 //
2354 // Use getDeviceMode or setMode to initialize the Outputmode
2355 // and Outputsettings member variables used to retrieve the correct
2356 // value
2357 //
2358 // Input
2359 // valueSpec : Specifier of the value to be retrieved
2360 // data[] : Data field of a MTData / BusData message
2361 // bid : bus identifier of the device of which the
2362 // value should be returned (default = BID_MT)
2363 // Output
2364 // value[] : pointer to array of unsigned shorts in which the
2365 // retrieved values will be returned
2366 //
2367 // Return value
2368 // MTRV_OK : value is successfully retrieved
2369 // != MTRV_OK : not successful
2370 //
2371 short CMTComm::getValue(const unsigned long valueSpec, unsigned short value[], const unsigned char data[],
2372  const unsigned char bid)
2373 {
2374  short offset = 0;
2375  unsigned char nbid = bid;
2376 
2377  if(nbid == BID_MASTER){
2378  nbid = 0;
2379  }
2380  // Check for invalid mode/settings
2383  }
2384 
2385  // Calculate offset for XM input
2386  if (m_storedOutputMode[0] == OUTPUTMODE_XM) {
2387  int i = 0;
2388  while (i < nbid) {
2389  offset += (short)m_storedDataLength[i++];
2390  }
2391  }
2392 
2393  // Check if data is unsigned short, available in data & retrieve data
2395  if (valueSpec >= VALUE_RAW_ACC && valueSpec <= VALUE_RAW_MAG) {
2397  offset += (short)(valueSpec*LEN_UNSIGSHORT*3);
2398  offset += (m_storedOutputMode[nbid] == OUTPUTMODE_MT9 && \
2399  m_storedOutputSettings[nbid] == OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT)?LEN_SAMPLECNT:0;
2400  for (int i = 0; i < 3; i++) {
2401  swapEndian(data+offset+i*LEN_UNSIGSHORT, (unsigned char *)value+i*LEN_UNSIGSHORT, LEN_UNSIGSHORT);
2402  }
2403  m_retVal = MTRV_OK;
2404  }
2405  }
2406  return m_retVal;
2407 }
2408 
2410 // getValue (array of floats variant)
2411 //
2412 // Retrieves an array of float values from the data input parameter.
2413 // This function is valid for the following value specifiers:
2414 // VALUE_TEMP
2415 // VALUE_CALIB_ACC
2416 // VALUE_CALIB_GYR
2417 // VALUE_CALIB_MAG
2418 // VALUE_ORIENT_QUAT
2419 // VALUE_ORIENT_EULER
2420 // VALUE_ORIENT_MATRIX
2421 //
2422 // Use getDeviceMode or setMode to initialize the Outputmode
2423 // and Outputsettings member variables used to retrieve the correct
2424 // value
2425 //
2426 // Input
2427 // valueSpec : Specifier of the value to be retrieved
2428 // data[] : Data field of a MTData / BusData message
2429 // bid : bus identifier of the device of which the
2430 // value should be returned (default = BID_MT)
2431 // Output
2432 // value[] : pointer to array of floats in which the
2433 // retrieved values will be returned
2434 //
2435 // Return value
2436 // MTRV_OK : value is successfully retrieved
2437 // != MTRV_OK : not successful
2438 //
2439 short CMTComm::getValue(const unsigned long valueSpec, float value[], const unsigned char data[],
2440  const unsigned char bid)
2441 {
2442  short offset = 0;
2443  int nElements = 0;
2444  unsigned char nbid = bid;
2445 
2446  if(nbid == BID_MASTER){
2447  nbid = 0;
2448  }
2449 
2450  // Check for invalid mode/settings
2453  }
2454 
2455  // Calculate offset for XM input
2456  if (m_storedOutputMode[0] == OUTPUTMODE_XM) {
2457  int i = 0;
2458  while (i < nbid) {
2459  offset += (short)m_storedDataLength[i++];
2460  }
2461  }
2462 
2463  // Check if data is float & available in data
2465  if (valueSpec == VALUE_TEMP) {
2466  if (m_storedOutputMode[nbid] & OUTPUTMODE_TEMP) {
2467  nElements = LEN_TEMPDATA / LEN_FLOAT;
2468  m_retVal = MTRV_OK;
2469  }
2470  }
2471  else if (valueSpec == VALUE_CALIB_ACC) {
2472  offset += ((m_storedOutputMode[nbid] & OUTPUTMODE_TEMP) != 0)?LEN_TEMPDATA:0;
2473  if (m_storedOutputMode[nbid] & OUTPUTMODE_CALIB &&
2475  nElements = LEN_CALIB_ACCDATA / LEN_FLOAT;
2476  m_retVal = MTRV_OK;
2477  }
2478  }
2479  else if (valueSpec == VALUE_CALIB_GYR) {
2480  offset += ((m_storedOutputMode[nbid] & OUTPUTMODE_TEMP) != 0)?LEN_TEMPDATA:0;
2481  if (m_storedOutputMode[nbid] & OUTPUTMODE_CALIB &&
2484  nElements = LEN_CALIB_GYRDATA / LEN_FLOAT;
2485  m_retVal = MTRV_OK;
2486  }
2487  }
2488  else if (valueSpec == VALUE_CALIB_MAG) {
2489  offset += ((m_storedOutputMode[nbid] & OUTPUTMODE_TEMP) != 0)?LEN_TEMPDATA:0;
2490  if (m_storedOutputMode[nbid] & OUTPUTMODE_CALIB &&
2494  nElements = LEN_CALIB_MAGDATA / LEN_FLOAT;
2495  m_retVal = MTRV_OK;
2496  }
2497  }
2498  else if (valueSpec >= VALUE_ORIENT_QUAT && valueSpec <= VALUE_ORIENT_MATRIX) {
2499  offset += ((m_storedOutputMode[nbid] & OUTPUTMODE_TEMP) != 0)?LEN_TEMPDATA:0;
2500  if ((m_storedOutputMode[nbid] & OUTPUTMODE_CALIB)) {
2504  }
2505  if (m_storedOutputMode[nbid] & OUTPUTMODE_ORIENT) {
2506  unsigned long orientmode = m_storedOutputSettings[nbid] & OUTPUTSETTINGS_ORIENTMODE_MASK;
2507  switch(valueSpec) {
2508  case VALUE_ORIENT_QUAT:
2509  if (orientmode == OUTPUTSETTINGS_ORIENTMODE_QUATERNION) {
2510  nElements = LEN_ORIENT_QUATDATA / LEN_FLOAT;
2511  m_retVal = MTRV_OK;
2512  }
2513  break;
2514  case VALUE_ORIENT_EULER:
2515  if (orientmode == OUTPUTSETTINGS_ORIENTMODE_EULER) {
2516  nElements = LEN_ORIENT_EULERDATA / LEN_FLOAT;
2517  m_retVal = MTRV_OK;
2518  }
2519  break;
2520  case VALUE_ORIENT_MATRIX:
2521  if (orientmode == OUTPUTSETTINGS_ORIENTMODE_MATRIX) {
2522  nElements = LEN_ORIENT_MATRIXDATA / LEN_FLOAT;
2523  m_retVal = MTRV_OK;
2524  }
2525  break;
2526  default:
2527  break;
2528  }
2529  }
2530  }
2531  if (m_retVal == MTRV_OK) {
2533  for (int i = 0; i < nElements; i++) {
2534  swapEndian(data+offset+i*LEN_FLOAT, (unsigned char *)value+i*LEN_FLOAT, LEN_FLOAT);
2535  }
2536  }
2537  else {
2538  int temp;
2539  for (int i = 0; i < nElements; i++) {
2540  swapEndian(data+offset+i*LEN_FLOAT, (unsigned char*)&temp, 4);
2541  value[i] = (float)temp/1048576;
2542  }
2543  }
2544  }
2545  return m_retVal;
2546 }
2547 
2549 // getLastDeviceError
2550 //
2551 // Returns the last reported device error of the latest received Error
2552 // message
2553 //
2554 // Output
2555 // Error code
2557 {
2558  return m_deviceError;
2559 }
2560 
2562 // getLastRetVal
2563 //
2564 // Returns the returned value of the last called function
2565 //
2566 // Output
2567 // Return value
2569 {
2570  return m_retVal;
2571 }
2572 
2574 // setTimeOut
2575 //
2576 // Sets the time out value in milliseconds used by the functions
2577 // Use 0 for infinite timeout
2578 //
2579 // Output
2580 // MTRV_OK is set, MTRV_INVALIDTIMEOUT if time value < 0
2581 short CMTComm::setTimeOut(short timeOutMs)
2582 {
2583  if (timeOutMs >= 0) {
2584  m_timeOut = timeOutMs;
2585  return (m_retVal = MTRV_OK);
2586  }
2587  else
2588  return (m_retVal = MTRV_INVALIDTIMEOUT);
2589 }
2590 
2592 // swapEndian
2593 //
2594 // Convert 2 or 4 bytes data from little to big endian or back
2595 //
2596 // Input
2597 // input : Pointer to data to be converted
2598 // output : Pointer where converted data is stored
2599 // length : Length of setting (0,2 & 4)
2600 //
2601 // Remarks:
2602 // Allocate enough bytes for output buffer
2603 void CMTComm::swapEndian(const unsigned char input[], unsigned char output[], const short length)
2604 {
2605  switch(length) {
2606  case 0:
2607  break;
2608  case 1:
2609  output[0] = input[0];
2610  break;
2611  case 2:
2612  output[0] = input[1];
2613  output[1] = input[0];
2614  break;
2615  case 4:
2616  output[0] = input[3];
2617  output[1] = input[2];
2618  output[2] = input[1];
2619  output[3] = input[0];
2620  break;
2621  default:
2622  for (short i=0, j=length-1 ; i < length ; i++, j--)
2623  output[j] = input[i];
2624  break;
2625  }
2626 }
2627 
2629 // calcChecksum
2630 //
2631 // Calculate and append checksum to msgBuffer
2632 //
2633 void CMTComm::calcChecksum(unsigned char *msgBuffer, const int msgBufferLength)
2634 {
2635  unsigned char checkSum = 0;
2636  int i;
2637 
2638  for(i = 1; i < msgBufferLength; i++)
2639  checkSum += msgBuffer[i];
2640 
2641  msgBuffer[msgBufferLength] = -checkSum; // Store chksum
2642 }
2643 
2645 // checkChecksum
2646 //
2647 // Checks if message checksum is valid
2648 //
2649 // Output
2650 // returns true checksum is OK
2651 bool CMTComm::checkChecksum(const unsigned char *msgBuffer, const int msgBufferLength)
2652 {
2653  unsigned char checkSum = 0;
2654  int i;
2655 
2656  for (i = 1; i < msgBufferLength; i++)
2657  checkSum += msgBuffer[i];
2658 
2659  if (checkSum == 0) {
2660  return true;
2661  }
2662  else {
2663  return false;
2664  }
2665 }
2666 
2667 
#define VALUE_RAW_TEMP
Definition: MTComm.h:468
#define MTRV_INVALIDVALUESPEC
Definition: MTComm.h:895
#define VALUE_RAW_MAG
Definition: MTComm.h:467
#define LEN_ORIENT_MATRIXDATA
Definition: MTComm.h:325
#define TO_DEFAULT
Definition: MTComm.h:835
#define MID_REQOUTPUTMODE
Definition: MTComm.h:212
#define MTRV_ENDOFFILE
Definition: MTComm.h:891
#define IND_LEN
Definition: MTComm.h:113
#define EXTLENCODE
Definition: MTComm.h:125
#define BID_MASTER
Definition: MTComm.h:123
#define VALUE_ORIENT_QUAT
Definition: MTComm.h:472
#define CONF_NUMDEVICESLEN
Definition: MTComm.h:682
#define CONF_NUMDEVICES
Definition: MTComm.h:663
#define MID_REQDID
Definition: MTComm.h:155
#define MTRV_TIMEOUTNODATA
Definition: MTComm.h:881
#define OUTPUTSETTINGS_CALIBMODE_MAG_MASK
Definition: MTComm.h:795
#define MID_SETOUTPUTSETTINGS
Definition: MTComm.h:221
#define CONF_MASTERDID
Definition: MTComm.h:655
#define INVALID_SET_FILE_POINTER
Definition: MTComm.h:106
#define OUTPUTSETTINGS_CALIBMODE_ACC_MASK
Definition: MTComm.h:793
#define CONF_DATALENGTHLEN
Definition: MTComm.h:685
#define MTRV_OK
Definition: MTComm.h:878
#define MID_REQDATALENGTH
Definition: MTComm.h:174
#define MID_REQOUTPUTSETTINGS
Definition: MTComm.h:218
#define LEN_CALIB_GYRDATA
Definition: MTComm.h:321
#define OUTPUTSETTINGS_ORIENTMODE_QUATERNION
Definition: MTComm.h:779
#define CONF_OUTPUTMODELEN
Definition: MTComm.h:686
#define CONF_OUTPUTMODE
Definition: MTComm.h:667
#define IND_LENEXTL
Definition: MTComm.h:116
#define MTRV_NOINPUTINITIALIZED
Definition: MTComm.h:892
#define MTRV_INVALIDFORFILEINPUT
Definition: MTComm.h:896
#define LEN_RAW_TEMP
Definition: MTComm.h:433
#define IND_BID
Definition: MTComm.h:111
#define OUTPUTMODE_CALIB
Definition: MTComm.h:772
#define INVALIDSETTINGVALUE
Definition: MTComm.h:478
#define LEN_MSGHEADER
Definition: MTComm.h:127
#define MID_CONFIGURATION
Definition: MTComm.h:178
#define LEN_MSGEXTHEADER
Definition: MTComm.h:128
#define MTRV_INVALIDTIMEOUT
Definition: MTComm.h:887
#define MID_SETOUTPUTMODE
Definition: MTComm.h:215
#define MID_INITBUS
Definition: MTComm.h:158
#define MAXMSGLEN
Definition: MTComm.h:139
#define OUTPUTMODE_ORIENT
Definition: MTComm.h:773
#define OUTPUTSETTINGS_ORIENTMODE_EULER
Definition: MTComm.h:780
#define VALUE_CALIB_GYR
Definition: MTComm.h:470
#define LEN_SAMPLECNT
Definition: MTComm.h:326
#define MTRV_INPUTCANNOTBEOPENED
Definition: MTComm.h:889
#define VALUE_CALIB_ACC
Definition: MTComm.h:469
#define OUTPUTSETTINGS_ORIENTMODE_MASK
Definition: MTComm.h:792
#define LEN_MSGEXTHEADERCS
Definition: MTComm.h:130
#define MAXDEVICES
Definition: MTComm.h:120
#define OUTPUTMODE_RAW
Definition: MTComm.h:770
#define LEN_CALIB_MAGX
Definition: MTComm.h:441
#define OUTPUTSETTINGS_CALIBMODE_GYR_MASK
Definition: MTComm.h:794
#define OUTPUTSETTINGS_ORIENTMODE_MATRIX
Definition: MTComm.h:781
#define LEN_RAWDATA
Definition: MTComm.h:318
#define PREAMBLE
Definition: MTComm.h:122
#define IND_MID
Definition: MTComm.h:112
#define LEN_TEMPDATA
Definition: MTComm.h:327
#define OUTPUTSETTINGS_TIMESTAMP_MASK
Definition: MTComm.h:791
#define MTRV_ANINPUTALREADYOPEN
Definition: MTComm.h:890
#define CONF_DID
Definition: MTComm.h:665
#define VALUE_ORIENT_EULER
Definition: MTComm.h:473
#define LEN_ORIENT_EULERDATA
Definition: MTComm.h:324
#define IND_PREAMBLE
Definition: MTComm.h:110
#define LEN_FLOAT
Definition: MTComm.h:134
#define CONF_DATALENGTH
Definition: MTComm.h:666
#define LEN_CHECKSUM
Definition: MTComm.h:131
#define IND_DATAEXT0
Definition: MTComm.h:117
#define OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT
Definition: MTComm.h:778
#define MTRV_UNEXPECTEDMSG
Definition: MTComm.h:888
#define VALUE_ORIENT_MATRIX
Definition: MTComm.h:474
#define LEN_MSGHEADERCS
Definition: MTComm.h:129
#define IND_DATA0
Definition: MTComm.h:114
#define LEN_ORIENT_QUATDATA
Definition: MTComm.h:323
#define LEN_CALIB_ACCDATA
Definition: MTComm.h:320
#define VALUE_RAW_ACC
Definition: MTComm.h:465
#define MTRV_NOTSUCCESSFUL
Definition: MTComm.h:879
#define LEN_CALIB_MAGDATA
Definition: MTComm.h:322
#define CONF_OUTPUTSETTINGSLEN
Definition: MTComm.h:687
#define MTRV_RECVERRORMSG
Definition: MTComm.h:884
#define LEN_OUTPUTSETTINGS
Definition: MTComm.h:220
#define VALUE_CALIB_MAG
Definition: MTComm.h:471
#define OUTPUTSETTINGS_XM
Definition: MTComm.h:776
#define LEN_OUTPUTMODE
Definition: MTComm.h:214
#define LEN_CALIB_ACCX
Definition: MTComm.h:435
#define OUTPUTMODE_TEMP
Definition: MTComm.h:771
#define MTRV_TIMEOUT
Definition: MTComm.h:880
#define VALUE_TEMP
Definition: MTComm.h:476
#define OUTPUTMODE_XM
Definition: MTComm.h:769
#define MTRV_NOVALIDMODESPECIFIED
Definition: MTComm.h:893
#define OUTPUTSETTINGS_DATAFORMAT_F1220
Definition: MTComm.h:790
#define MID_ERROR
Definition: MTComm.h:484
#define LEN_UNSIGSHORT
Definition: MTComm.h:132
#define LEN_CALIB_GYRX
Definition: MTComm.h:438
#define CONF_BLOCKLEN
Definition: MTComm.h:669
#define CONF_OUTPUTSETTINGS
Definition: MTComm.h:668
#define IND_LENEXTH
Definition: MTComm.h:115
#define BID_MT
Definition: MTComm.h:124
#define MID_MTDATA
Definition: MTComm.h:309
#define OUTPUTMODE_MT9
Definition: MTComm.h:768
#define LEN_DEVICEID
Definition: MTComm.h:157
#define LEN_CALIBDATA
Definition: MTComm.h:319
#define VALUE_SAMPLECNT
Definition: MTComm.h:475
@ data
short readMessage(unsigned char &mid, unsigned char data[], short &dataLen, unsigned char *bid=NULL)
Definition: MTComm.cpp:745
int readData(unsigned char *msgBuffer, const int nBytesToRead)
Definition: MTComm.cpp:498
int m_nTempBufferLen
Definition: MTComm.h:999
int m_handle
Definition: MTComm.h:983
short m_deviceError
Definition: MTComm.h:987
short setSetting(const unsigned char mid, const unsigned long value, const unsigned short valuelen, const unsigned char bid=BID_MASTER)
Definition: MTComm.cpp:1720
short setDeviceMode(unsigned long OutputMode, unsigned long OutputSettings, const unsigned char bid=BID_MASTER)
Definition: MTComm.cpp:2130
void escape(unsigned long function)
Definition: MTComm.cpp:610
short readDataMessage(unsigned char data[], short &dataLen)
Definition: MTComm.cpp:786
short openPort(const int portNumber, const unsigned long baudrate=PBR_115K2, const unsigned long inqueueSize=4096, const unsigned long outqueueSize=1024)
Definition: MTComm.cpp:160
unsigned long m_storedOutputMode[MAXDEVICES+1]
Definition: MTComm.h:993
unsigned char m_tempBuffer[MAXMSGLEN]
Definition: MTComm.h:998
clock_t m_clkEnd
Definition: MTComm.h:990
short getLastRetVal()
Definition: MTComm.cpp:2568
short openFile(const char *fileName, bool createAlways=false)
Definition: MTComm.cpp:434
short getFileSize(unsigned long &fileSize)
Definition: MTComm.cpp:677
clock_t clockms()
Definition: MTComm.cpp:128
short getLastDeviceError()
Definition: MTComm.cpp:2556
short m_retVal
Definition: MTComm.h:988
bool isFileOpen()
Definition: MTComm.cpp:481
short setMode(unsigned long OutputMode, unsigned long OutputSettings, const unsigned char bid=BID_MASTER)
Definition: MTComm.cpp:2211
short readMessageRaw(unsigned char *msgBuffer, short *msgBufferLength)
Definition: MTComm.cpp:829
bool m_fileOpen
Definition: MTComm.h:986
virtual ~CMTComm()
Definition: MTComm.cpp:112
bool isPortOpen()
Definition: MTComm.cpp:471
short waitForMessage(const unsigned char mid, unsigned char data[]=NULL, short *dataLen=NULL, unsigned char *bid=NULL)
Definition: MTComm.cpp:1177
short getValue(const unsigned long valueSpec, unsigned short &value, const unsigned char data[], const unsigned char bid=BID_MT)
Definition: MTComm.cpp:2300
void calcChecksum(unsigned char *msgBuffer, const int msgBufferLength)
Definition: MTComm.cpp:2633
short setTimeOut(short timeOutMs)
Definition: MTComm.cpp:2581
void setPortQueueSize(const unsigned long inqueueSize=4096, const unsigned long outqueueSize=1024)
Definition: MTComm.cpp:623
unsigned long m_storedDataLength[MAXDEVICES+1]
Definition: MTComm.h:995
bool m_portOpen
Definition: MTComm.h:985
short m_timeOut
Definition: MTComm.h:989
static void swapEndian(const unsigned char input[], unsigned char output[], const short length)
Definition: MTComm.cpp:2603
bool checkChecksum(const unsigned char *msgBuffer, const int msgBufferLength)
Definition: MTComm.cpp:2651
short getMode(unsigned long &OutputMode, unsigned long &OutputSettings, unsigned short &dataLength, const unsigned char bid=BID_MASTER)
Definition: MTComm.cpp:2187
short close()
Definition: MTComm.cpp:702
CMTComm()
Definition: MTComm.cpp:96
short setFilePos(long relPos, unsigned long moveMethod=FILEPOS_BEGIN)
Definition: MTComm.cpp:648
unsigned long m_storedOutputSettings[MAXDEVICES+1]
Definition: MTComm.h:994
void flush()
Definition: MTComm.cpp:588
short reqSetting(const unsigned char mid, unsigned long &value, const unsigned char bid=BID_MASTER)
Definition: MTComm.cpp:1243
int writeData(const unsigned char *msgBuffer, const int nBytesToWrite)
Definition: MTComm.cpp:565
short getDeviceMode(unsigned short *numDevices=NULL)
Definition: MTComm.cpp:2021
short writeMessage(const unsigned char mid, const unsigned long dataValue=0, const unsigned char dataValueLen=0, const unsigned char bid=BID_MASTER)
Definition: MTComm.cpp:1012
bool read(yarp::os::Searchable &cfgtotal, pc104Data &pc104data)
Definition: ethParser.cpp:92
bool write(const std::string filename, const FullRegulation &reg)
degrees offset
Definition: sine.m:4