int LIN::read(byte PID, byte* data, int length, int checksumtype) {
byte CRC, send_pid;
// +3 for Break, Sync and CRC after the Data
byte tmp[length+3];
uint8_t i = 0;
send_pid = ((PID&0x3F) | addrParity(PID)<<6);
_stream->write(send_pid);
_stream->flush();
//Serial.println("-----------------------");
/*
unsigned long actuall = micros();
while ( i < (length+4) ) {
if ( _stream->available() ) {
tmp[i] = _stream->read();
Serial.println(tmp[i], HEX);
i++;
}
if ( (actuall+response_maximalspace) > micros() ) {
break;
}
}
*/
elapsedMicros waiting;
[COLOR="#FF0000"] _stream->read(); /* Dummy read added by Geek493 */[/COLOR]
//Serial.println(response_maximalspace);
//Serial.println(waiting);
while ( i < (length+4) ) {
if ( _stream->available() ) {
tmp[i] = _stream->read();
//Serial.println(tmp[i], HEX);
i++;
}
if ( response_maximalspace < waiting ) {
//Serial.println(waiting);
break;
}
}
//Serial.println(waiting);
for (int i=3; i<length+3; i++)
data[i-3] = tmp[i];
if (checksumtype == 1)
CRC = dataChecksum(data, length, 0);
else
CRC = dataChecksum(data, length, send_pid);
//Serial.println(CRC,HEX);
//Serial.println(tmp[length+3],HEX);
if (CRC == tmp[length+3])
return CRC;
else
return -1;
}