88 static char THIS_FILE[]=__FILE__;
133 #if (CLOCKS_PER_SEC != 1000)
134 clk /= (CLOCKS_PER_SEC / 1000);
140 gettimeofday(&tv, &tz);
141 clk = tv.tv_sec * 1000 + (tv.tv_usec / 1000);
160 short CMTComm::openPort(
const int portNumber,
const unsigned long baudrate,
const unsigned long inqueueSize,
const unsigned long outqueueSize)
168 char pchFileName[10];
170 sprintf(pchFileName,
"\\\\.\\COM%d",portNumber);
172 m_handle = CreateFile(pchFileName, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL);
173 if (
m_handle == INVALID_HANDLE_VALUE) {
185 dcb.BaudRate = baudrate;
186 dcb.Parity = NOPARITY;
188 dcb.StopBits = TWOSTOPBITS;
189 dcb.fDsrSensitivity = FALSE;
190 dcb.fOutxCtsFlow = FALSE;
191 dcb.fOutxDsrFlow = FALSE;
194 if (!SetCommState(
m_handle, (LPDCB)&dcb)) {
197 dcb.StopBits = ONESTOPBIT;
198 if (!SetCommState(
m_handle, (LPDCB)&dcb)) {
207 COMMTIMEOUTS CommTimeouts;
209 GetCommTimeouts(
m_handle,&CommTimeouts);
212 CommTimeouts.ReadTotalTimeoutConstant = 1;
213 CommTimeouts.ReadIntervalTimeout = MAXDWORD;
214 CommTimeouts.ReadTotalTimeoutMultiplier = MAXDWORD;
221 SetCommTimeouts(
m_handle, &CommTimeouts);
224 EscapeCommFunction(
m_handle, SETRTS);
225 SetupComm(
m_handle,inqueueSize,outqueueSize);
228 PurgeComm(
m_handle, PURGE_TXCLEAR | PURGE_RXCLEAR);
233 struct termios options;
236 sprintf(chPort,
"/dev/ttyS%d",(portNumber - 1));
238 m_handle = open(chPort, O_RDWR | O_NOCTTY);
257 cfsetispeed(&options, baudrate);
258 cfsetospeed(&options, baudrate);
261 options.c_cflag |= (CLOCAL | CREAD);
263 options.c_cflag &= ~(CSIZE|PARENB);
264 options.c_cflag |= CS8;
265 options.c_cflag |= CSTOPB;
267 options.c_cflag &= ~CRTSCTS;
268 options.c_lflag &= ~(ECHO|ECHONL|ICANON|ISIG|IEXTEN);
270 options.c_iflag &= ~(IGNBRK|BRKINT|PARMRK|ISTRIP|INLCR|IGNCR|ICRNL|IXON);
272 options.c_oflag &= ~OPOST;
274 options.c_cc[VMIN] = 0;
275 options.c_cc[VTIME] = 5;
278 tcsetattr(
m_handle,TCSANOW, &options);
300 short CMTComm::openPort(
const char *portName,
const unsigned long baudrate,
const unsigned long inqueueSize,
const unsigned long outqueueSize)
308 m_handle = CreateFile(portName, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL);
309 if (
m_handle == INVALID_HANDLE_VALUE) {
321 dcb.BaudRate = baudrate;
322 dcb.Parity = NOPARITY;
324 dcb.StopBits = TWOSTOPBITS;
325 dcb.fDsrSensitivity = FALSE;
326 dcb.fOutxCtsFlow = FALSE;
327 dcb.fOutxDsrFlow = FALSE;
330 if (!SetCommState(
m_handle, (LPDCB)&dcb)) {
333 dcb.StopBits = ONESTOPBIT;
334 if (!SetCommState(
m_handle, (LPDCB)&dcb)) {
343 COMMTIMEOUTS CommTimeouts;
345 GetCommTimeouts(
m_handle,&CommTimeouts);
348 CommTimeouts.ReadTotalTimeoutConstant = 1;
349 CommTimeouts.ReadIntervalTimeout = MAXDWORD;
350 CommTimeouts.ReadTotalTimeoutMultiplier = MAXDWORD;
356 SetCommTimeouts(
m_handle, &CommTimeouts);
359 EscapeCommFunction(
m_handle, SETRTS);
360 SetupComm(
m_handle,inqueueSize,outqueueSize);
363 PurgeComm(
m_handle, PURGE_TXCLEAR | PURGE_RXCLEAR);
367 struct termios options;
371 m_handle = open(portName, O_RDWR | O_NOCTTY);
390 cfsetispeed(&options, baudrate);
391 cfsetospeed(&options, baudrate);
394 options.c_cflag |= (CLOCAL | CREAD);
396 options.c_cflag &= ~(CSIZE|PARENB);
397 options.c_cflag |= CS8;
398 options.c_cflag |= CSTOPB;
400 options.c_cflag &= ~CRTSCTS;
401 options.c_lflag &= ~(ECHO|ECHONL|ICANON|ISIG|IEXTEN);
403 options.c_iflag &= ~(IGNBRK|BRKINT|PARMRK|ISTRIP|INLCR|IGNCR|ICRNL|IXON);
405 options.c_oflag &= ~OPOST;
407 options.c_cc[VMIN] = 0;
408 options.c_cc[VTIME] = 5;
411 tcsetattr(
m_handle,TCSANOW, &options);
442 DWORD disposition = OPEN_ALWAYS;
443 if (createAlways ==
true) {
444 disposition = CREATE_ALWAYS;
446 m_handle = CreateFile(fileName, GENERIC_READ | GENERIC_WRITE, FILE_SHARE_READ, NULL, disposition, 0, NULL);
447 if (
m_handle == INVALID_HANDLE_VALUE) {
451 int openMode = O_RDWR | O_CREAT;
452 if (createAlways ==
true) {
455 m_handle = open(fileName, openMode, S_IRWXU);
506 BOOL retval = ReadFile(
m_handle, msgBuffer, nBytesToRead, &nBytesRead, NULL);
507 if (retval && nBytesRead == 0 &&
m_fileOpen) {
514 int nBytesRead =
read(
m_handle, msgBuffer, nBytesToRead);
575 WriteFile(
m_handle, msgBuffer, nBytesToWrite, &nBytesWritten, NULL);
576 return nBytesWritten;
593 PurgeComm(
m_handle, PURGE_TXCLEAR | PURGE_RXCLEAR);
613 EscapeCommFunction(
m_handle,
function);
627 SetupComm(
m_handle,inqueueSize,outqueueSize);
658 if (lseek(
m_handle, relPos, moveMethod) != -1){
681 if ((fileSize = GetFileSize(
m_handle, NULL)) != INVALID_FILE_SIZE) {
689 fileSize = buf.st_size;
831 clock_t clkStart, clkEnd;
833 int nBytesToRead = 1;
838 unsigned char chCheckSum;
859 if (nBytesRead > 0 && nBytesToRead > 0) {
860 if (nBytesToRead > nBytesRead) {
861 nOffset += nBytesRead;
862 nBytesToRead -= nBytesRead;
866 nOffset += nBytesToRead;
867 nBytesRead -= nBytesToRead;
873 if (nBytesToRead > 0) {
874 nBytesRead =
readData(msgBuffer+nOffset, nBytesToRead);
878 nOffset += nBytesRead;
879 nBytesToRead -= nBytesRead;
883 if(nBytesToRead == 0){
896 if (msgBuffer[
IND_LEN] != 0xFF) {
898 nBytesToRead = (nMsgDataLen = msgBuffer[
IND_LEN]) + 1;
914 memmove(msgBuffer, msgBuffer+i,nBytesRead);
926 nMsgLen = nMsgDataLen + 5 + (msgBuffer[
IND_LEN] == 0xFF?2:0);
928 for(
int i = 1; i < nMsgLen; i++){
929 chCheckSum += msgBuffer[i];
933 if (nBytesRead > 0) {
937 *msgBufferLength = nMsgLen;
944 for (
int i = 1; i < nMsgLen; i++) {
947 nBytesRead = nMsgLen - i;
948 memmove(msgBuffer, msgBuffer+i,nBytesRead);
967 for (i = 1; i < nOffset; i++) {
970 nBytesRead = nOffset-i-1;
971 memmove(msgBuffer+1, msgBuffer+i+1,nBytesRead);
1013 const unsigned char dataValueLen,
const unsigned char bid)
1025 buffer[
IND_LEN] = dataValueLen;
1038 clock_t clkStart, clkOld;
1039 bool msgRead =
false;
1050 if(buffer[
IND_MID] == (mid+1)) {
1090 const unsigned short &dataLen,
const unsigned char bid)
1106 buffer[
IND_LEN] = (
unsigned char)dataLen;
1111 buffer[
IND_LENEXTH] = (
unsigned char)(dataLen >> 8);
1112 buffer[
IND_LENEXTL] = (
unsigned char)(dataLen & 0x00FF);
1115 memcpy(&buffer[headerLength],
data, dataLen);
1127 bool msgRead =
false;
1128 clock_t clkStart, clkOld;
1139 if(buffer[
IND_MID] == (mid+1)) {
1182 clock_t clkStart, clkOld;
1199 if (
data != NULL && dataLen != NULL) {
1209 else if(dataLen != NULL)
1266 if(buffer[
IND_MID] == (mid+1)) {
1303 const unsigned char bid)
1317 if (param != 0xFF) {
1332 if(buffer[
IND_MID] == (mid+1)) {
1394 if(buffer[
IND_MID] == (mid+1)) {
1430 const unsigned char bid)
1444 if (param != 0xFF) {
1459 if(buffer[
IND_MID] == (mid+1)) {
1500 unsigned char data[],
short &dataLen,
const unsigned char bid)
1524 if(buffer[
IND_MID] == (mid+1)) {
1568 unsigned char dataIn[],
short dataInLen,
1569 unsigned char dataOut[],
short &dataOutLen,
const unsigned char bid)
1585 buffer[
IND_LEN] = (
unsigned char)dataInLen;
1590 buffer[
IND_LENEXTH] = (
unsigned char)(dataInLen >> 8);
1591 buffer[
IND_LENEXTL] = (
unsigned char)(dataInLen & 0x00FF);
1594 memcpy(&buffer[headerLength], dataIn, dataInLen);
1604 if(buffer[
IND_MID] == (mid+1)) {
1608 memcpy(dataOut, &buffer[
IND_DATA0], dataOutLen);
1648 unsigned char data[],
short &dataLen,
const unsigned char bid)
1662 if (param != 0xFF) {
1678 if(buffer[
IND_MID] == (mid+1)) {
1721 const unsigned long value,
const unsigned short valuelen,
1722 const unsigned char bid)
1737 buffer[
IND_LEN] = (
unsigned char)valuelen;
1738 swapEndian((
unsigned char *)&value, &buffer[msgLen], valuelen);
1747 if(buffer[
IND_MID] == (mid+1)) {
1779 const unsigned long value,
const unsigned short valuelen,
1780 const unsigned char bid)
1795 if (param != 0xFF) {
1801 buffer[
IND_LEN] = (
unsigned char)valuelen;
1803 swapEndian((
unsigned char *)&value, &buffer[msgLen], valuelen);
1812 if(buffer[
IND_MID] == (mid+1)) {
1864 if(buffer[
IND_MID] == (mid+1)) {
1894 const float value,
const unsigned char bid)
1909 if (param != 0xFF) {
1926 if(buffer[
IND_MID] == (mid+1)) {
1957 const float value,
const bool store,
const unsigned char bid)
1972 if (param != 0xFF) {
1990 if(buffer[
IND_MID] == (mid+1)) {
2026 if (numDevices != NULL) {
2051 if (numDevices != NULL) {
2055 unsigned char masterDID[4];
2080 unsigned short _numDevices = 0;
2082 for(
unsigned int i = 0; i < _numDevices; i++){
2093 if (numDevices != NULL) {
2094 *numDevices = _numDevices;
2156 unsigned long value;
2188 unsigned short &dataLength,
const unsigned char bid)
2190 unsigned char nbid = (bid ==
BID_MASTER)?0:bid;
2211 short CMTComm::setMode(
unsigned long OutputMode,
unsigned long OutputSettings,
const unsigned char bid)
2213 unsigned char nbid = bid;
2224 unsigned short dataLength = 0;
2301 const unsigned char bid)
2304 unsigned char nbid = bid;
2372 const unsigned char bid)
2375 unsigned char nbid = bid;
2400 for (
int i = 0; i < 3; i++) {
2440 const unsigned char bid)
2444 unsigned char nbid = bid;
2533 for (
int i = 0; i < nElements; i++) {
2539 for (
int i = 0; i < nElements; i++) {
2541 value[i] = (float)temp/1048576;
2583 if (timeOutMs >= 0) {
2609 output[0] = input[0];
2612 output[0] = input[1];
2613 output[1] = input[0];
2616 output[0] = input[3];
2617 output[1] = input[2];
2618 output[2] = input[1];
2619 output[3] = input[0];
2622 for (
short i=0, j=length-1 ; i < length ; i++, j--)
2623 output[j] = input[i];
2635 unsigned char checkSum = 0;
2638 for(i = 1; i < msgBufferLength; i++)
2639 checkSum += msgBuffer[i];
2641 msgBuffer[msgBufferLength] = -checkSum;
2653 unsigned char checkSum = 0;
2656 for (i = 1; i < msgBufferLength; i++)
2657 checkSum += msgBuffer[i];
2659 if (checkSum == 0) {
#define MTRV_INVALIDVALUESPEC
#define LEN_ORIENT_MATRIXDATA
#define MID_REQOUTPUTMODE
#define VALUE_ORIENT_QUAT
#define CONF_NUMDEVICESLEN
#define MTRV_TIMEOUTNODATA
#define OUTPUTSETTINGS_CALIBMODE_MAG_MASK
#define MID_SETOUTPUTSETTINGS
#define INVALID_SET_FILE_POINTER
#define OUTPUTSETTINGS_CALIBMODE_ACC_MASK
#define CONF_DATALENGTHLEN
#define MID_REQDATALENGTH
#define MID_REQOUTPUTSETTINGS
#define LEN_CALIB_GYRDATA
#define OUTPUTSETTINGS_ORIENTMODE_QUATERNION
#define CONF_OUTPUTMODELEN
#define MTRV_NOINPUTINITIALIZED
#define MTRV_INVALIDFORFILEINPUT
#define INVALIDSETTINGVALUE
#define MID_CONFIGURATION
#define MTRV_INVALIDTIMEOUT
#define MID_SETOUTPUTMODE
#define OUTPUTMODE_ORIENT
#define OUTPUTSETTINGS_ORIENTMODE_EULER
#define MTRV_INPUTCANNOTBEOPENED
#define OUTPUTSETTINGS_ORIENTMODE_MASK
#define LEN_MSGEXTHEADERCS
#define OUTPUTSETTINGS_CALIBMODE_GYR_MASK
#define OUTPUTSETTINGS_ORIENTMODE_MATRIX
#define OUTPUTSETTINGS_TIMESTAMP_MASK
#define MTRV_ANINPUTALREADYOPEN
#define VALUE_ORIENT_EULER
#define LEN_ORIENT_EULERDATA
#define OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT
#define MTRV_UNEXPECTEDMSG
#define VALUE_ORIENT_MATRIX
#define LEN_ORIENT_QUATDATA
#define LEN_CALIB_ACCDATA
#define MTRV_NOTSUCCESSFUL
#define LEN_CALIB_MAGDATA
#define CONF_OUTPUTSETTINGSLEN
#define MTRV_RECVERRORMSG
#define LEN_OUTPUTSETTINGS
#define OUTPUTSETTINGS_XM
#define MTRV_NOVALIDMODESPECIFIED
#define OUTPUTSETTINGS_DATAFORMAT_F1220
#define CONF_OUTPUTSETTINGS
short readMessage(unsigned char &mid, unsigned char data[], short &dataLen, unsigned char *bid=NULL)
int readData(unsigned char *msgBuffer, const int nBytesToRead)
short setSetting(const unsigned char mid, const unsigned long value, const unsigned short valuelen, const unsigned char bid=BID_MASTER)
short setDeviceMode(unsigned long OutputMode, unsigned long OutputSettings, const unsigned char bid=BID_MASTER)
void escape(unsigned long function)
short readDataMessage(unsigned char data[], short &dataLen)
short openPort(const int portNumber, const unsigned long baudrate=PBR_115K2, const unsigned long inqueueSize=4096, const unsigned long outqueueSize=1024)
unsigned long m_storedOutputMode[MAXDEVICES+1]
unsigned char m_tempBuffer[MAXMSGLEN]
short openFile(const char *fileName, bool createAlways=false)
short getFileSize(unsigned long &fileSize)
short getLastDeviceError()
short setMode(unsigned long OutputMode, unsigned long OutputSettings, const unsigned char bid=BID_MASTER)
short readMessageRaw(unsigned char *msgBuffer, short *msgBufferLength)
short waitForMessage(const unsigned char mid, unsigned char data[]=NULL, short *dataLen=NULL, unsigned char *bid=NULL)
short getValue(const unsigned long valueSpec, unsigned short &value, const unsigned char data[], const unsigned char bid=BID_MT)
void calcChecksum(unsigned char *msgBuffer, const int msgBufferLength)
short setTimeOut(short timeOutMs)
void setPortQueueSize(const unsigned long inqueueSize=4096, const unsigned long outqueueSize=1024)
unsigned long m_storedDataLength[MAXDEVICES+1]
static void swapEndian(const unsigned char input[], unsigned char output[], const short length)
bool checkChecksum(const unsigned char *msgBuffer, const int msgBufferLength)
short getMode(unsigned long &OutputMode, unsigned long &OutputSettings, unsigned short &dataLength, const unsigned char bid=BID_MASTER)
short setFilePos(long relPos, unsigned long moveMethod=FILEPOS_BEGIN)
unsigned long m_storedOutputSettings[MAXDEVICES+1]
short reqSetting(const unsigned char mid, unsigned long &value, const unsigned char bid=BID_MASTER)
int writeData(const unsigned char *msgBuffer, const int nBytesToWrite)
short getDeviceMode(unsigned short *numDevices=NULL)
short writeMessage(const unsigned char mid, const unsigned long dataValue=0, const unsigned char dataValueLen=0, const unsigned char bid=BID_MASTER)
bool read(yarp::os::Searchable &cfgtotal, pc104Data &pc104data)
bool write(const std::string filename, const FullRegulation ®)