IMU to Arduino: Timing FTDI receiving function

My current setup: Arduino Uno with a Sparkfun USB host shield(DEV-09947) hooked up to a MTi-g Xsens IMU. I am using the USB Host 2.0 Library, and the FTDI library inside said library.

Able to receive and send messages fine. I have messed around with the delays, at 0 milliseconds delay after the Ftdi.RcvData function I get 65 out of 100 messages received correctly(using a checksum function). Using a 40 millisecond delay I get about 85 to 90 out of 100 correctly(this is what the example suggests to use). I am receiving 42 bytes in the buffer for each message received after configuration So that is about 3 milliseconds for the data to send at 115200 baud. I have decided to go with the 0 delay, since overall I will get more correct messages.

What I need to do, is time how quick I get the first correct message after I have entered the measurement mode. So I need to know how many milliseconds have passed since I entered the mode till I get valid message. I need to then do this test multiple times and get a general average. How would I go about doing this?

This is to determine if I need to dive into why I am getting so many incorrect messages.

On another note, the messages that I am receiving that are incorrect have a pattern to the incorrect portions of the messages they are sending which makes me think something more is happening then simply a bit getting switched or dropped. The incorrect messages have byte arrays inside them that are identical to what I sent the IMU earlier in the program. This is never suppose to occur, the IMU is not ever in any situation suppose to send these byte arrays, only acknowledgment byte arrays in response. Would the drivers be at fault? If so how can I fix it?

Here is code. A little messy since its more for testing random stuff not my final product.

bool arrayequal;
config mode

//uint8_t Message5[] = {0xFA,0xFF,0x10,0x00,0xF1}; // Message to make IMU go into config mode
int counter = 0;
uint8_t repeats = 0;
uint8_t test1[64];
uint8_t test2[64];
uint8_t test3[64];
uint8_t test4[64];
uint8_t test5[64];
int repeat = 0;
bool H = 0;
bool go = 0;
uint8_t data[42];
uint8_t test_1;
uint8_t test_2;
uint8_t test_3;
uint8_t test_4;
uint8_t test_5;

int counter1 = 0;

bool check;
int good = 0;
int bad = 0;
int z = 0;
uint8_t finalmessage[5];

void setup()
{
  for(int i = 0; i<sizeof(test1); i++){
    test1[i] = 0;
   }
   test1[0] = 0x00; test1[1] = 0x00; test1[2] = 0xFA; test1[3] = 0xFF; test1[4] = 0x31; test1[5] = 0x00; test1[6] = 0xD0;
  for(int i = 0; i<sizeof(test2); i++){
    test2[i] = 0;
   }
   test2[0] = 0x00; test2[1] = 0x00; test2[2] = 0xFA; test2[3] = 0x01; test2[4] = 0xD1; test2[5] = 0x00; test2[6] = 0x2E;
  for(int i = 0; i<sizeof(test3); i++){
    test3[i] = 0;
   }
   test3[0] = 0x00; test3[1] = 0x00; test3[2] = 0xFA; test3[3] = 0xFF; test3[4] = 0x11; test3[5] = 0x00; test3[6] = 0xF0;
  for(int i = 0; i<sizeof(test4); i++){
    test4[i] = 0;
   }
   test4[0] = 0x00; test4[1] = 0x00; test4[2] = 0xFA; test4[3] = 0xFF; test4[4] = 0x3E; test4[5] = 0x00; test4[6] = 0xC3;
  for(int i = 0; i<sizeof(test5); i++){
    test5[i] = 0;
   }
   test5[0] = 0x00; test5[1] = 0x00; test5[2] = 0xFA; test5[3] = 0xFF; test5[4] = 0x3E; test5[5] = 0x00; test5[6] = 0xC3;
  Serial.begin( 250000 );
#if !defined(__MIPSEL__)
  while (!Serial); // Wait for serial port to connect - used on Leonardo, Teensy and other boards with built-in USB CDC serial connection
#endif
  Serial.println("Start");

  if (Usb.Init() == -1)
      Serial.println("OSC did not start.");

  delay( 200 );
}

void loop()
{
    Usb.Task();

    if( Usb.getUsbTaskState() == USB_STATE_RUNNING )
    {
        uint8_t  rcode;
        
        //char strbuf[] = "The quick brown fox jumps over the lazy dog";
        //char strbuf[] = "This string contains 61 character to demonstrate FTDI buffers"; //add one symbol to it to see some garbage
       
       uint8_t buf[64];
       uint16_t rcvd = 64;
      
       if(counter  == 0){
//        for(int i = 0 ;i < 10; i++){
        uint8_t message2[] = {0xFA,0xFF,0x30,0x00,0xD1};

        rcode = Ftdi.SndData(sizeof(message2),(uint8_t*)message2);
        delay(40);

        
       }else if(counter == 1){
        for(int i = 0 ;i < 10; i++){
        
        uint8_t message3[] = {0xFA,0x01,0xD0,0x02,0x00,0x02,0x2B};
        rcode = Ftdi.SndData(sizeof(message3),(uint8_t*)message3);
        delay(40);
        }
       }else if(counter == 2){
         
        uint8_t message4[] = {0xFA,0xFF,0x10,0x00,0xF1};
        rcode = Ftdi.SndData(sizeof(message4),(uint8_t*)message4);
        delay(40);
       }else if(counter == 3){
        
        counter = 5;
       }else if(counter == 4){
        uint8_t message6[] = {0xFA,0xFF,0x30,0x00,0xD1};
        rcode = Ftdi.SndData(sizeof(message6),(uint8_t*)message6);
       }
       for(int i = 0; i<sizeof(buf); i++){
        buf[i] = 0;
       }
       for(int i = 0; i<sizeof(data); i++){
        data[i] = 0;
       }   
        rcode = Ftdi.RcvData(&rcvd,buf); //At current baud rate it should take 3 miliseconds for the messsages to be recieved.
//        delay(40);   
        if (rcode && rcode != hrNAK)
            ErrorMessage<uint8_t>(PSTR("Ret"), rcode);
        if( rcvd >2 ) { //more than zero bytes received
          if(counter != 5){
             for(uint16_t i=0; i < rcvd; i++ ) {
                Serial.print(buf[i+2],HEX); //printing on the screen
                Serial.print(" ");
               
             }
          }else{
            repeats = 0;
            repeats = Calibrationparsing1(buf, data);
            if(repeats == 1){
              repeat++;
            }
            
            for(uint16_t i=0; i < sizeof(data); i++ ) {
                  Serial.print(data[i],HEX); //printing on the screen
                  Serial.print(" ");
           }
            check = checksum(data,sizeof(data));
            if(check == true){
              counter1++;
              good++;
            }else{
              counter1++;
              bad++;
            }
            if(counter1 == 100){
              Serial.print("Good Messages: ");
              Serial.print(good);
              Serial.println();
              Serial.print("Bad Messages: ");
              Serial.print(bad);
              Serial.println();
              Serial.print("Repeats: ");
              Serial.print(repeat);
              Serial.println();
              delay(5000);
              repeat = 0;
              good = 0;
              bad = 0;
              counter1 = 0;
              
            }   
          }    
        }
             Serial.println();
             if(counter ==0 || counter ==1 || counter ==2 || counter == 3){
               FindMessage(sizeof(buf),buf,finalmessage);
  
               if(finalmessage[0] == test1[2] && finalmessage[1] == test1[3] && finalmessage[2] == test1[4] && finalmessage[3] == test1[5] && finalmessage[4] == test1[6]){
                counter = 1;
                Serial.print('1');
               }else if(finalmessage[0] == test2[2] && finalmessage[1] == test2[3] && finalmessage[2] == test2[4] && finalmessage[3] == test2[5] && finalmessage[4] == test2[6]){
                counter = 2;
                Serial.print('2');
               }else if(finalmessage[0] == test3[2] && finalmessage[1] == test3[3] && finalmessage[2] == test3[4] && finalmessage[3] == test3[5] && finalmessage[4] == test3[6]){
                counter = 3;
                Serial.print('3');
               }
             }
 
  }
}
uint8_t FindMessage(int Size, uint8_t *message,uint8_t *finalmessage){
  bool startcounting = false;
  bool stopcounting = true;
  int count = 0;
  uint8_t buf[Size];
  bool preamblespotted = false;
  int location = 0;
  for(int i = 0; i<Size; i++){
    buf[i] = message[i+2];
    if(buf[i-2] == 0xFA && (buf[i-1] == 0xFF || buf[i-1] == 0x01) && (buf[i] != 0x3E && buf[i] != 0x32 && buf[i] != 0xD0)){
      preamblespotted  = true;
      location = i-2;
      startcounting = true;
      stopcounting = false;
    }
    if(preamblespotted == true ){
      stopcounting  = true;
      finalmessage[0] = buf[location];
      finalmessage[1] = buf[location+1];
      finalmessage[2] = buf[location+2];
      finalmessage[3] = buf[location+3];
      finalmessage[4] = buf[location+4];  
    }else{
      finalmessage[0] = 0x00;
      finalmessage[1] = 0x00;
      finalmessage[2] = 0x00;
      finalmessage[3] = 0x00;
      finalmessage[4] = 0x00; 
    }
  }
}
bool checksum(uint8_t *buf,int Size){
  unsigned long value = 0;
  bool checksumcomplete;
  byte checksum;
  for(int i = 0; i<Size; i++){
    value = value  + buf[i];
  }
  checksum = (value&0xFF);
  if(checksum == 0x00){
    checksumcomplete = true;
  }else{
    checksumcomplete = false;
  }
    return checksumcomplete;
}


uint8_t parsdata_for_test(uint8_t *Array,int startingpoint, uint8_t *tobefilled){ //when using often make sure to keep track of starting point.
  for(int i = startingpoint, j = 0; i <startingpoint+42 && j<42; i++,j++){
    tobefilled[j] = Array[i];
  }
}


uint8_t Calibrationparsing1(uint8_t *buf, uint8_t *data){
  uint8_t test_4;
  uint8_t test_3;
  uint8_t test_2;
  uint8_t test_1;
  bool H = false;
  bool go = false;
  int z = 0;
  int c = 0;
  uint8_t repeat = 0;
   
while(H == false){//checks whether preamble has been seen yet             
  test_4 = buf[z+3];
  test_3 = buf[z+2];
  test_2 = buf[z+1];
  test_1 = buf[z];
  if(test_1 == 0xFA && test_2 == 0xFF && test_3 == 0x32 && test_4 ==0x26){
    H = true;
    go = true;
  }else if(z >64){
    H = true;
    go = false;
  }else{
    H = false;
    go = false;
  }
z++;
}
for(int i = z+4; i<z+30;i++){
  test_4 = buf[i+3];
  test_3 = buf[i+2];
  test_2 = buf[i+1];
  test_1 = buf[i];
  if(test_1 == 0xFA && test_2 == 0xFF && test_3 == 0x32 && test_4 ==0x26){//Used if the message FA FF 32 26 repeats itself, was observed occuring.
    z = i+1;
    repeats = 1;
    break;
  }
}
if(go == true){
  parsdata_for_test(buf,z,data);  
 
}else{
  Serial.print('1');
}
return repeats;
}