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) {} }