AT Command et réponse

Bonjour,
sous arduino 2.3.3, j'ai connecté un ESP-01S sur un ATmega1284p et un avec un clavier connecté a un adaptateur PS2, j'envoi des commandes à l'ESP-01S, les réponses de ce module sont envoyé via UART1 de l'Atmega qui est connecté à un terminal.

le code ci-dessus fonctionne bien pour toutes les commandes quand ca renvoi OK.
Mais si il y a une erreur (réponse ERROR), il attends le timeout.

Comment je pourrais faire pour ne pas que ca attende le timeout quand OK n'est pas reçu du module ESP-01S.

waitFor(4000UL, "OK\r\n"); // Attend la reponse OK par exemple

code:
uint64_t waitFor(unsigned long time_out, const char *string)
{
    uint8_t so_far = 0;
    uint8_t received;
    uint16_t counter = 0;
    unsigned long previousMillis = 0;
    previousMillis = millis();
    
    do {
        if(mySerial.available())
		    {
            received = mySerial.read();
            sendChar(received);           
            counter++;
            if (received == string[so_far]) {
                so_far++;
            } else {
                so_far = 0;
            }
        }        
        
        if(millis() - previousMillis > time_out) { cputs("\r\nTIMEOUT\r\n"); break; }
    } while (string[so_far] != 0 );
    return counter;
}

Le timeout est quand même necessaire dans le cas ou il y a blocage.
Merci.

J'ai trouvé cette solution, qui permet de tester 5 code erreurs, ca fonctionne, mais je ne pas en mettre plus, car ca prends un peu plus temps, 1ms ou plus et je perd des caractères.

int _esp8266_waitResponse(unsigned long time_out)
{
    int index = 5;
    unsigned long previousMillis = 0;
    
    int so_far[index] = {0,0,0,0,0};
    int lengths[index] = {4,7,9,11,8};
    char* strings[index] = {"OK\r\n", "ERROR\r\n", "SEND OK\r\n", "SEND FAIL\r\n", "SET OK\r\n"};
    int responses[index] = {ESP8266_OK, ESP8266_ERROR, ESP8266_SEND_OK, ESP8266_SEND_FAIL, ESP8266_SET_OK};

    int received;
    int response;
    bool continue_loop = true;
    previousMillis = millis();

    while (continue_loop)
    {
        if(mySerial.available() > 0)
        {
            received = mySerial.read();            
            sendChar(received);       
            for (int i = 0; i < index; i++) {
                if (strings[i][so_far[i]] == received) {
                    so_far[i]++;
                    if (so_far[i] == lengths[i]) {
                        response = responses[i];
                        continue_loop = false;
                        break;
                    }
                } else {
                    so_far[i] = 0;
                }
            }
            
        }
        if(millis() - previousMillis > time_out) { cputs("\r\nTIMEOUT\r\n"); break; }
    }
    return response;
}