void UsbInterfacePrivate::threadFunc()
{
HANDLE handles[1+m_oCount];
handles[0] = m_quitEvent;
handles[m_oCount] = m_waitEvents[m_oCount-1];
//= { m_quitEvent, m_waitEvents[0], m_waitEvents[1] };
// start first read operation
for (m_readIdx = 0 ; m_readIdx < (m_oCount-1); ++m_readIdx)
{
handles[m_readIdx+1] = m_waitEvents[m_readIdx];
//m_readIdx = 0;
m_overlapped[m_readIdx] = OVERLAPPED();
::ResetEvent(m_waitEvents[m_readIdx]); //lint !e534
m_overlapped[m_readIdx].hEvent = m_waitEvents[m_readIdx];
m_winUsb.ReadPipe(m_usbHandle[1],
m_bulkInPipe,
m_fixedBuffer[m_readIdx],
(ULONG)m_fixedBufferSize,
0,
&m_overlapped[m_readIdx]); //lint !e534
}
int fastCount = 0;
//m_readIdx = 1;
bool policyFast = false;
bool run = true;
while (run)
{
// start follow-up read operation
m_overlapped[m_readIdx] = OVERLAPPED();
::ResetEvent(m_waitEvents[m_readIdx]); //lint !e534
m_overlapped[m_readIdx].hEvent = m_waitEvents[m_readIdx];
m_winUsb.ReadPipe(m_usbHandle[1],
m_bulkInPipe,
m_fixedBuffer[m_readIdx],
(ULONG)m_fixedBufferSize,
0,
&m_overlapped[m_readIdx]); //lint !e534
m_readIdx = (m_readIdx + 1) % m_oCount;
int64_t tBegin = XsTime_timeStampNow(0);
DWORD waitResult = ::WaitForMultipleObjects(1+m_oCount, handles, FALSE, INFINITE);
#if 0 // not sure if this causes problems, but it should help in catching up
int64_t tEnd = XsTime_timeStampNow(0);
switch (tEnd - tBegin)
{
case 0:
if (++fastCount > m_fastPolicyThreshold && !policyFast)
{
policyFast = true;
// set fast policy
UCHAR enable = TRUE;
m_winUsb.SetPipePolicy(m_usbHandle[1], m_bulkInPipe, IGNORE_SHORT_PACKETS, sizeof(UCHAR), &enable); //lint !e534
}
break;
case 1:
if (fastCount)
--fastCount;
if (policyFast && fastCount <= m_fastPolicyThreshold)
{
// reset policy
policyFast = false;
UCHAR enable = FALSE;
m_winUsb.SetPipePolicy(m_usbHandle[1], m_bulkInPipe, IGNORE_SHORT_PACKETS, sizeof(UCHAR), &enable); //lint !e534
}
break;
default:
fastCount = 0;
if (policyFast)
{
// reset policy
policyFast = false;
UCHAR enable = FALSE;
m_winUsb.SetPipePolicy(m_usbHandle[1], m_bulkInPipe, IGNORE_SHORT_PACKETS, sizeof(UCHAR), &enable); //lint !e534
}
break;
}
#endif
// handle data
switch (waitResult)
{
case WAIT_TIMEOUT:
case WAIT_FAILED:
case WAIT_OBJECT_0:
run = false;
break;
default:
if (waitResult >= WAIT_ABANDONED_0)
{
JLDEBUG(gJournal, "WFMO abandoned: " << (waitResult - WAIT_OBJECT_0));
break;
}
#ifndef XSENS_RELEASE
JLDEBUG(gJournal, "WFMO trigger: " << (waitResult - WAIT_OBJECT_0));
#endif
{
// put data into buffer
int idx = m_readIdx;
DWORD dataRead = 0;
if (!m_winUsb.GetOverlappedResult(m_usbHandle[0], &m_overlapped[idx], &dataRead, FALSE))
{
// error
DWORD err = ::GetLastError();
switch (err)
{
case ERROR_SEM_TIMEOUT:
case ERROR_IO_INCOMPLETE:
//JLDEBUG(gJournal, "m_winUsb.GetOverlappedResult resulted in acceptable windows error " << err);
break;
default:
JLALERT(gJournal, "m_winUsb.GetOverlappedResult resulted in windows error " << err);
run = false;
break;
}
//assert (err == ERROR_IO_INCOMPLETE);
}
else
{
// append unread data to var buffer
JLTRACE(gJournal, "m_winUsb.GetOverlappedResult resulted in " << dataRead << " bytes being read");
XsByteArray ref(&m_fixedBuffer[idx][0], dataRead, XSDF_None);
::EnterCriticalSection(&m_mutex);
m_varBuffer.append(ref);
::LeaveCriticalSection(&m_mutex);
}
} break;
}
}
}
/*! \relates XsTimeStamp
\brief Returns the current time in ms since the epoch (Jan 1st 1970)
\see XsTime_timeStampNow
\param dest The object to write the time to, may be 0 in which case only the return value is generated.
\returns The current time in ms since the epoch (Jan 1st 1970)
*/
int64_t XsTimeStamp_now(XsTimeStamp* dest)
{
return (int64_t) XsTime_timeStampNow(dest);
}
/*! \brief Stabilize the clock
\details Repeatedly call XsTime_timeStampNow for 16-32 ms to stabilize the clock
*/
void XsTime_initializeTime()
{
long long start = XsTime_timeStampNow(0);
while (XsTime_timeStampNow(0) - start < 32) {}
}
