Using FTDI D2xx and Thorlabs APT communication protocol results in delays on Linux - c++

Question

I am trying to communicate with the Thorlabs TDC001 controllers (apt - dc servo controller) by using the FTDI D2xx driver on Linux. However, when I send writing commands, large delays occur (1-2 seconds) until the command is actually executed on TDC001.
In particular, this can be observed when the connected linear stage is moving and a new position command is sent. It takes 1-2 seconds until the stage actually changes its direction. Also, if I request DCSTATUSUPDATE (which gives position and velocity) and then read out the queue of FTDI, I do not get the right data. Only if I wait 1 second between requesting and reading, I get the (correct) data, but for the past. I added the C++ code for this case.
I need live-data and faster execution of writing commands for closed-loop control.
I'm not sure if the problem is on the side of Thorlabs or FTDI. Everything works, except for the large delays. There are other commands, e.g. MOVE_STOP, which respond immediately. Also, if I send a new position command right after finishing homing, it is executed immediately.
Whenever I ask for FT_GetStatus, there is nothing else in the Tx or Rx queue except the 20 bytes in Rx for DCSTATUSUPDATE.
The references for D2XX and APT communication protocol can be found here:
FTDI Programmer's Guide
Thorlabs APT Communication Protocol
The initialization function:
bool CommunicationFunctions::initializeKeyHandle(string serialnumber){
//INITIALIZATION//
/*
* This function initializes the TDC motor controller and finds its corresponding keyhandle.
*/
keyHandle = NULL;
// To open the device, the vendor and product ID must be set correctly
ftStatus = FT_SetVIDPID(0x403,0xfaf0);
}
//Open device:
const char* tmp = serialnumber.c_str();
int numAttempts=0;
while (keyHandle ==0){
ftStatus = FT_OpenEx(const_cast<char*>(tmp),FT_OPEN_BY_SERIAL_NUMBER, &keyHandle);
if (numAttempts++>20){
cerr << "Device Could Not Be Opened";
return false;
}
}
// Set baud rate to 115200
ftStatus = FT_SetBaudRate(keyHandle,115200);
// 8 data bits, 1 stop bit, no parity
ftStatus = FT_SetDataCharacteristics(keyHandle, FT_BITS_8, FT_STOP_BITS_1, FT_PARITY_NONE);
// Pre purge dwell 50ms.
usleep(50);
// Purge the device.
ftStatus = FT_Purge(keyHandle, FT_PURGE_RX | FT_PURGE_TX);
// Post purge dwell 50ms.
usleep(50);
// Reset device.
ftStatus = FT_ResetDevice(keyHandle);
// Set flow control to RTS/CTS.
ftStatus = FT_SetFlowControl(keyHandle, FT_FLOW_RTS_CTS, 0, 0);
// Set RTS.
ftStatus = FT_SetRts(keyHandle);
return true;
}
How I read out my data:
bool CommunicationFunctions::read_tdc(int32_t* position, uint16_t* velocity){
uint8_t *RxBuffer = new uint8_t[256]();
DWORD RxBytes;
DWORD BytesReceived = 0;
// Manually request status update:
uint8_t req_statusupdate[6] = {0x90,0x04,0x01,0x00,0x50,0x01};
ftStatus = FT_Write(keyHandle, req_statusupdate, (DWORD)6, &written);
if(ftStatus != FT_OK){
cerr << "Command could not be transmitted: Request status update" << endl;
return false;
}
// sleep(1); //**this sleep() would lead to right result, but I don't want this delay**
// Get number of bytes in queue of TDC001
FT_GetQueueStatus(keyHandle,&RxBytes);
// Check if there are bytes in queue before reading them, otherwise do
// not read anything in
if(RxBytes>0){
ftStatus=FT_Read(keyHandle,RxBuffer,RxBytes,&BytesReceived);
if(ftStatus != FT_OK){
cerr << "Read device failed!" << endl;
return false;
}
}
// Check if enough bytes are received, i.e. if signal is right
if(!(BytesReceived >= 6)){
cerr << "Error in bytes received" << endl;
return false;
}
// Look for correct message in RxBuffer and read out velocity and position
getPosVel(position,velocity,RxBuffer);
// Delete receive buffer
delete[] RxBuffer;
RxBuffer = NULL;
return true;
}
If I use read_tdc function after homing and during movement to absolute position, I just get "Homing completed" message in the first attempt. When I try read_tdc again, I get an old value (probably the one from before). I don't understand what happens here. Why does this old data even remain in the queue (latency is 10 ms). Can anybody help me to get faster responses and reactions?