mtsComponentAddLatency::mtsComponentAddLatency(const std::string & componentName,
double periodInSeconds):
mtsTaskPeriodic(componentName, periodInSeconds, false /* real time */, 100 /* state table size */),
LatencyStateTable(10000, "DataWithLatency")
{
SetLatency(1.0); // default
}
mtsComponentAddLatency::mtsComponentAddLatency(const mtsTaskPeriodicConstructorArg &arg):
mtsTaskPeriodic(arg),
LatencyStateTable(10000, "DataWithLatency")
{
SetLatency(1.0); // default
}
int CKMotionIO::Connect()
{
char reason[256];
FT_STATUS ftStatus;
if (NonRespondingCount==2) return 1;
m_SaveChars[0]=0; // start anew
Mutex->Lock();
if (!RequestedDeviceAvail(reason))
{
ErrorMessageBox(reason);
Mutex->Unlock();
return 1;
}
#define TIME_TO_TRY_TO_OPEN 3000
// FT_ListDevices OK, number of devices connected is in numDevs
// usually during boot the board comes and goes, since it appeared
// to be there, try for a while to open it
DWORD t0=timeGetTime();
for (;;)
{
ftStatus = FT_OpenEx((void *)USB_Loc_ID,FT_OPEN_BY_LOCATION,&ftHandle);
if (ftStatus == FT_OK)
{
// FT_Open OK, use ftHandle to access device
if (SetLatency(2))
{
Mutex->Unlock();
return 1;
}
if (FlushInputBuffer())
{
FT_Close(ftHandle);
Mutex->Unlock();
return 1;
}
m_Connected=true; // All set
Mutex->Unlock();
return 0;
}
else
{
// FT_Open failed
if (timeGetTime()-t0 > TIME_TO_TRY_TO_OPEN)
{
ErrorMessageBox("Unable to open KMotion device");
Mutex->Unlock();
return 1;
}
Sleep(100); // delay a bit then loop back and try again
}
}
Mutex->Unlock();
return 0;
}
int CKMotionIO::Connect()
{
char reason[256];
int ftStatus;
if (NonRespondingCount==CONNECT_TRIES) return 1;
m_SaveChars[0]=0; // start anew
Mutex->Lock();
if (!RequestedDeviceAvail(reason))
{
ErrorMessageBox(reason);
Mutex->Unlock();
return 1;
}
#define TIME_TO_TRY_TO_OPEN 3000
// FT_ListDevices OK, number of devices connected is in numDevs
// usually during boot the board comes and goes, since it appeared
// to be there, try for a while to open it
DWORD t0=timeGetTime();
for (;;)
{
ftStatus = ftdi_usb_open_desc_index(ftdi, VENDOR, PRODUCT, NULL, NULL, USB_Loc_ID);
if (ftStatus < FT_OK)
{
log_info("ftdi_usb_open_desc_index failed: %d (%s)", ftStatus, ftdi_get_error_string(ftdi));
// FT_Open failed
if (timeGetTime()-t0 > TIME_TO_TRY_TO_OPEN)
{
ErrorMessageBox("Unable to open KMotion device");
Mutex->Unlock();
return 1;
}
Sleep(100); // delay a bit then loop back and try again
}
else
{
// FT_Open OK, use ftHandle to access device
if (SetLatency(2))
{
//Close handle
if(_ftdi_usb_close(ftdi) < FT_OK){
log_info("_ftdi_usb_close failed: %d (%s)", ftStatus, ftdi_get_error_string(ftdi));
}
Mutex->Unlock();
return 1;
}
if (FlushInputBuffer())
{
log_info("FAIL: FlushInputBuffer for device %d",USB_Loc_ID);
//This is the key to why it works on second attempt
//After usb_close has executed once the device works forever.
//The ftdi device is not closed when killing the KMotionServer
//Try to recover from this action when initialising this class.
if(_ftdi_usb_close(ftdi) < FT_OK){
log_info("_ftdi_usb_close failed: %d (%s)", ftStatus, ftdi_get_error_string(ftdi));
}
Mutex->Unlock();
return 1;
}
m_Connected=true; // All set
Mutex->Unlock();
return 0;
}
}
Mutex->Unlock();
return 0;
}
