Here is my version of OBDuino from their code:
//This is a character buffer that will store the data from the serial port
char rxData[20];
char rxIndex=0;
//Variables to hold the speed and RPM data.
int vehicleSpeed=0;
int vehicleRPM=0;
void setup(){
Serial1.begin(9600);
Serial.begin(9600); //This is for the serial debug
//Wait for a little while before sending the reset command to the OBD-II-UART
delay(1500);
//Reset the OBD-II-UART
Serial1.print("ATWS\r");
//Wait for a bit before starting to send commands after the reset.
delay(1000);
// turn echo off
Serial1.print("ATE0\r");
// send 01 00 until we are connected
do
{
Serial1.print("0100\r");
delay(1000);
getResponse(); //This is only called once since the schoecho has been turned off using ATE0\r
Serial.print("0100 Response: ");
Serial.print(rxData[0]);Serial.print(rxData[1]);Serial.print(rxData[3]);Serial.print(rxData[4]);Serial.print(rxData[6]);Serial.print(rxData[7]); Serial.print(rxData[9]);Serial.print(rxData[10]);
Serial.println("");
}
while(ELM_CHECK_RESPONSE("0100", rxData)!=0);
Serial1.flush();
Serial1.print("ATDPN\r"); //Shows the protocol
getResponse();
Serial.print("Protocol: ");Serial.print(rxData[0]);Serial.print(rxData[1]);Serial.println("");
// str[0] should be 'A' for automatic
// set header to talk directly to ECU#1
if(rxData[1]=='1') // PWM
Serial1.print("ATSHE410F1\r");
else if(rxData[1]=='2') // VPW
Serial1.print("ATSHA810F1\r");
else if(rxData[1]=='3') // ISO 9141
Serial1.print("ATSH6810F1\r");
else if(rxData[1]=='6') // CAN 11 bits
Serial1.print("ATSH7E0\r");
else if(rxData[1]=='7') // CAN 29 bits
Serial1.print("ATSHDA10F1\r");
//Delete any data that may be in the serial port before we begin.
Serial1.flush();
}
void loop(){
//Delete any data that may be in the serial port before we begin.
Serial1.flush();
Serial1.print("010D\r"); //Query the OBD-II-UART for the Vehicle Speed
//Get the response from the OBD-II-UART board. We get two responses
//because the OBD-II-UART echoes the command that is sent.
//We want the data in the second response.
getResponse();
//Convert the string data to an integer
vehicleSpeed = strtol(&rxData[6],0,16);
//Print the speed data to debug serial window
Serial.print("SPEED: ");
Serial.print(vehicleSpeed);
Serial.print(" km/h");
Serial.println(" ");
delay(20);
//Delete any data that may be left over in the serial port.
Serial1.flush();
//Query the OBD-II-UART for the Vehicle rpm
Serial1.println("010C\r");
//Get the response from the OBD-II-UART board
getResponse();
//Convert the string data to an integer
//NOTE: RPM data is two bytes long, and delivered in 1/4 RPM from the OBD-II-UART
vehicleRPM = ((strtol(&rxData[6],0,16)*256)+strtol(&rxData[9],0,16))/4;
//Print the rpm data to debug serial window
Serial.print("RPM: ");
Serial.print(vehicleRPM);
Serial.println(" ");
//Give the OBD bus a rest
delay(50);
}
byte ELM_CHECK_RESPONSE(const char *cmd, char *str)
{
// cmd is something like "010D"
// str should be "41 0D blabla"
if(cmd[0]+4 != rxData[0]
|| cmd[1]!= rxData[1]
|| cmd[2]!= rxData[3]
|| cmd[3]!= rxData[4])
return 1;
return 0; // no error
}
void getResponse(void){
char inChar=0;
//Keep reading characters until we get a carriage return
while(inChar != '\r'){
//If a character comes in on the serial port, we need to act on it.
if(Serial1.available() > 0){
//Start by checking if we've received the end of message character ('\r').
if(Serial1.peek() == '\r'){
//Clear the Serial buffer
inChar=Serial1.read();
//Put the end of string character on our data string
rxData[rxIndex]='\0';
//Reset the buffer index so that the next character goes back at the beginning of the string.
rxIndex=0;
}
//If we didn't get the end of message character, just add the new character to the string.
else{
//Get the new character from the Serial port.
inChar = Serial1.read();
//Add the new character to the string, and increment the index variable.
rxData[rxIndex++]=inChar;
}
}
}
}