//////////////////////////////////////////////////////////////////////////////////////////
// Wait for data to arrive or a timeout to occur.
XsensResultValue Cmt1s::waitForData (const uint32_t maxLength,
uint8_t* data, uint32_t* length)
{
CMT1LOG("L1: waitForData, mto=%u, length=%p\n",m_timeout,length);
uint32_t timeout = m_timeout;
uint32_t ln;
if (length == NULL)
length = &ln;
uint32_t eTime = getTimeOfDay(NULL) + timeout;
uint32_t newLength = 0;
*length = 0;
while ((*length < maxLength) && (getTimeOfDay() <= eTime))
{
readData(maxLength - *length, data + *length, &newLength);
*length += newLength;
}
CMT1LOG("L1: waitForData result: read %u of %u bytes\n",length[0],maxLength);
if (length[0] < maxLength)
return (m_lastResult = XRV_TIMEOUT);
else
return (m_lastResult = XRV_OK);
}
//////////////////////////////////////////////////////////////////////////////////////////
// Read data from the serial port and put it into the data buffer.
XsensResultValue Cmt1s::readData(
const uint32_t maxLength, uint8_t* data, uint32_t* length)
{
CMT1LOG("L1: readData, maxlength=%u, length=%p\n", maxLength, length);
uint32_t ln;
if (length == nullptr) length = &ln;
if (!m_isOpen) return (m_lastResult = XRV_NOPORTOPEN);
#ifdef _WIN32
BOOL rres = ::ReadFile(m_handle, data, maxLength, (DWORD*)length, nullptr);
if (m_onBytesReceived != nullptr && *length > 0)
{
CmtBinaryData* bytes = (CmtBinaryData*)malloc(sizeof(CmtBinaryData));
bytes->m_size = *length;
bytes->m_portNr = m_port;
memcpy(bytes->m_data, data, *length);
#ifdef _LOG_CALLBACKS
CMTLOG(
"C1: onBytesReceived(%d,(%d,%d),%p)\n",
(int32_t)m_onBytesReceivedInstance, (int32_t)bytes->m_size,
(int32_t)bytes->m_portNr, m_onBytesReceivedParam);
#endif
m_onBytesReceived(
m_onBytesReceivedInstance, CMT_CALLBACK_ONBYTESRECEIVED, bytes,
m_onBytesReceivedParam);
}
if (!rres)
{
CMT1LOG("L1: readData, ReadFile returned error %u\n", ::GetLastError());
return (m_lastResult = XRV_ERROR);
}
#else
*length = read(m_handle, data, maxLength);
#endif
#ifdef _LOG_RX_TX
if (*length > 0)
{
if (rx_log == nullptr)
{
char fname[CMT_MAX_FILENAME_LENGTH];
sprintf(fname, "rx_%03d_%d.log", (int32_t)m_port, m_baudrate);
rx_log = fopen(fname, "wb");
}
fwrite(data, 1, *length, rx_log);
}
#endif
CMT1LOG(
(length[0] ? "L1: readData returned success, read %u of %u bytes, "
"first: %02x\n"
: "L1: readData returned success, read %u bytes\n"),
length[0], maxLength, data[0]);
return (m_lastResult = XRV_OK);
}
//////////////////////////////////////////////////////////////////////////////////////////
// Set the default timeout value to use in blocking operations.
XsensResultValue Cmt1s::setTimeout (const uint32_t ms)
{
CMT1LOG("L1: Setting timeout to %u ms\n",ms);
m_timeout = ms;
#ifdef _WIN32
// Set COM timeouts
COMMTIMEOUTS commTimeouts;
GetCommTimeouts(m_handle,&commTimeouts); // Fill CommTimeouts structure
// immediate return if data is available, wait 1ms otherwise
if (m_timeout > 0)
{
commTimeouts.ReadIntervalTimeout = 0;
commTimeouts.ReadTotalTimeoutConstant = m_timeout; // ms time
commTimeouts.ReadTotalTimeoutMultiplier = 0;
commTimeouts.WriteTotalTimeoutConstant = m_timeout;
commTimeouts.WriteTotalTimeoutMultiplier = 0;
}
else
{
// immediate return whether data is available or not
commTimeouts.ReadIntervalTimeout = MAXDWORD;
commTimeouts.ReadTotalTimeoutConstant = 0;
commTimeouts.ReadTotalTimeoutMultiplier = 0;
commTimeouts.WriteTotalTimeoutConstant = 0;
commTimeouts.WriteTotalTimeoutMultiplier = 0;
}
SetCommTimeouts(m_handle, &commTimeouts); // Set CommTimeouts structure
#else
// Timeout 0.1 sec for first byte, read minimum of 0 bytes
m_commState.c_cc[VMIN] = 0;
m_commState.c_cc[VTIME] = (m_timeout+99)/100; // ds time
// Set the new options for the port if it is open
if (m_isOpen)
tcsetattr(m_handle,TCSANOW, &m_commState);
#endif
return (m_lastResult = XRV_OK);
}
//////////////////////////////////////////////////////////////////////////////////////////
// Open a communication channel to the given serial port name.
XsensResultValue Cmt1s::open( const char *portName,
const uint32_t baudRate,
uint32_t readBufSize,
uint32_t writeBufSize)
{
MRPT_UNUSED_PARAM(readBufSize); MRPT_UNUSED_PARAM(writeBufSize);
m_endTime = 0;
CMT1LOG("L1: Open port %s at %d baud\n", portName, baudRate);
if (m_isOpen)
{
CMT1LOG("L1: Port already open\n");
return (m_lastResult = XRV_ALREADYOPEN);
}
m_baudrate = baudRate;
#ifdef _WIN32
char winPortName[32];
// Open port
sprintf(winPortName, "\\\\.\\%s", portName);
m_handle = CreateFileA(winPortName, GENERIC_READ | GENERIC_WRITE, 0, NULL,
OPEN_EXISTING, 0, NULL);
if (m_handle == INVALID_HANDLE_VALUE)
{
CMT1LOG("L1: Port cannot be opened\n");
return (m_lastResult = XRV_INPUTCANNOTBEOPENED);
}
// Once here, port is open
m_isOpen = true;
//Get the current state & then change it
GetCommState(m_handle, &m_commState); // Get current state
m_commState.BaudRate = baudRate; // Setup the baud rate
m_commState.Parity = NOPARITY; // Setup the Parity
m_commState.ByteSize = 8; // Setup the data bits
m_commState.StopBits = TWOSTOPBITS; // Setup the stop bits
m_commState.fDsrSensitivity = FALSE; // Setup the flow control
m_commState.fOutxCtsFlow = FALSE; // NoFlowControl:
m_commState.fOutxDsrFlow = FALSE;
m_commState.fOutX = FALSE;
m_commState.fInX = FALSE;
if (!SetCommState(m_handle, (LPDCB)&m_commState)) {// Set new state
// Bluetooth ports cannot always be opened with 2 stopbits
// Now try to open port with 1 stopbit.
m_commState.StopBits = ONESTOPBIT;
if (!SetCommState(m_handle, (LPDCB)&m_commState)) {
CloseHandle(m_handle);
m_handle = INVALID_HANDLE_VALUE;
m_isOpen = false;
return (m_lastResult = XRV_INPUTCANNOTBEOPENED);
}
}
m_port = atoi(&portName[3]);
sprintf(m_portname, "%s", portName);
setTimeout(m_timeout);
// Other initialization functions
EscapeCommFunction(m_handle, SETRTS); // Enable RTS (for Xbus Master use)
// Set DTR (Calibration sensors need DTR to startup, won't hurt otherwise
EscapeCommFunction(m_handle, SETDTR);
SetupComm(m_handle,readBufSize,writeBufSize); // Set queue size
// Remove any 'old' data in buffer
//PurgeComm(m_handle, PURGE_TXCLEAR | PURGE_RXCLEAR);
PurgeComm(m_handle, PURGE_TXABORT | PURGE_RXABORT | PURGE_TXCLEAR | PURGE_RXCLEAR);
#else // !_WIN32
// Open port
m_handle = ::open(portName, O_RDWR | O_NOCTTY);
// O_RDWR: Read+Write
// O_NOCTTY: Raw input, no "controlling terminal"
// O_NDELAY: Don't care about DCD signal
if (m_handle < 0) {
// Port not open
return m_lastResult = XRV_INPUTCANNOTBEOPENED;
}
// Once here, port is open
m_isOpen = true;
/* Start configuring of port for non-canonical transfer mode */
// Get current options for the port
tcgetattr(m_handle, &m_commState);
// Set baudrate.
cfsetispeed(&m_commState, baudRate);
cfsetospeed(&m_commState, baudRate);
// Enable the receiver and set local mode
m_commState.c_cflag |= (CLOCAL | CREAD);
// Set character size to data bits and set no parity Mask the characte size bits
m_commState.c_cflag &= ~(CSIZE|PARENB);
m_commState.c_cflag |= CS8; // Select 8 data bits
m_commState.c_cflag |= CSTOPB; // send 2 stop bits
// Disable hardware flow control
m_commState.c_cflag &= ~CRTSCTS;
m_commState.c_lflag &= ~(ECHO|ECHONL|ICANON|ISIG|IEXTEN);
// Disable software flow control
m_commState.c_iflag &= ~(IGNBRK|BRKINT|PARMRK|ISTRIP|INLCR|IGNCR|ICRNL|IXON);
// Set Raw output
m_commState.c_oflag &= ~OPOST;
// Timeout 0.001 sec for first byte, read minimum of 0 bytes
m_commState.c_cc[VMIN] = 0;
m_commState.c_cc[VTIME] = (m_timeout+99)/100; // 1
// Set the new options for the port
tcsetattr(m_handle,TCSANOW, &m_commState);
m_port = 0;
sprintf(m_portname, "%s", portName);
tcflush(m_handle, TCIOFLUSH);
// setting RTS and DTR; RTS for Xbus Master, DTR for calibration sensors
int cmbits;
if (ioctl(m_handle, TIOCMGET, &cmbits) < 0)
{
return (m_lastResult = XRV_ERROR);
}
cmbits |= TIOCM_RTS|TIOCM_DTR;
if (ioctl(m_handle, TIOCMSET, &cmbits) < 0)
{
return (m_lastResult = XRV_ERROR);
}
#endif // !_WIN32
CMT1LOG("L1: Port opened\n");
return (m_lastResult = XRV_OK);
}
