My code was also flawed, which I changed to:
(This is the working function I use now)
int flush_buffer_until(const char UNTIL[], const int UNTIL_LEN, bool stop_at_ok = false)
{
// TODO Add timeout to return -1
int u = 0; // Until message incremental counter
int8_t o = 0; // OK message incremental counter
char in; // Incoming byte
if (UNTIL == OK_TEXT)
{
// Override for when you only want to stop on OK
stop_at_ok = false;
}
// If UNTIL_LEN <= 0 we have an issue with passing arguments
while (UNTIL_LEN > 0)
{
while (hat_port.available() > 0)
{
in = hat_port.read(); // Incoming byte
if (in == UNTIL[u])
{
u++; // End of message is approaching
o = 0; // We can stop looking for OK if we were
}
else if ((in == OK_TEXT[o]) && stop_at_ok)
{
o++; // We're approaching the OK message
u = 0; // We can stop looking for UNTIL if we were
}
else
{
if (in == OK_TEXT[0] && stop_at_ok)
{
// We stopped the last sequence of OK, but we may have started a new sequence
o = 1;
continue;
}
else if (in == UNTIL[0])
{
// We stopped the last sequence of UNTIL, but we may have started a new sequence
// We don't continue here in case UNTIL is of length 1
u = 1;
}
else
{
// Neither OK, UNTIL or their starting letters were seen, so reset their counters
u = 0;
o = 0;
continue;
}
}
if (u >= UNTIL_LEN)
{
return 1;
}
else if (o >= OK_LEN)
{
return 2;
}
}
}
return -1;
}
This was also an issue, but is now fixed.