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

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?