void OBD9141sim::handleRequests(){
//uint8_t message[5] = {0x68, 0x6A, 0xF1, mode, pid}; and checksum
uint8_t message[6] = {0};
uint8_t number_of_bytes = 0;
this->serial->setTimeout(OBD9141_INTERSYMBOL_WAIT*7 + 10 + 6*OBD9141_REQUEST_ANSWER_MS_PER_BYTE);
if (this->serial->available()){
number_of_bytes = this->serial->readBytes(message, 6);
if (number_of_bytes == 6){
OBD9141println("[SIM] Got request");
if (message[5] == OBD9141::checksum(&message, 5)){
OBD9141println("[SIM] Checksum matches.");
this->answerRequest(message[3], message[4]);
}
} else {
OBD9141println("[SIM] Failed reading stuff");
}
uint8_t zero_count = 0;
for (uint8_t i=0; i < 6 ; i++){
if (message[i] == 0){
zero_count++;
}
}
if (zero_count == 6){
if (not this->force_init_state){
this->uninitialize();
}
}
}
}
void OBD9141sim::handleInit(){
uint8_t v1 = 0x08;
uint32_t current_time = millis();
if ((current_time) > (this->last_change + OBD9141SIM_INIT_ATTEMPT_TIMEOUT)){
this->init_hold_value = 0;
this->init_state = 0;
}
if ((this->init_state == 0) and (this->init_hold_value == 21) and (((millis()) > (this->last_change + OBD9141SIM_WAIT_AFTER_5BAUD)))){
// send the 0x55 byte.
OBD9141print("[SIM] dropping to serial port and sending 0x55: ");
// OBD9141print("[SIM] Serial: ");
// OBD9141println((uint32_t) this->serial);
delay(10);
this->serial->begin(OBD9141_KLINE_BAUD);
// OBD9141print("[SIM] Below begin... ");
this->write(0x55);
this->init_state = 1;
this->last_change = current_time;
}
if ((this->init_state == 1) and (((current_time) > (this->last_change + OBD9141SIM_WAIT_BEFORE_V1)))){
this->write(v1);
this->init_state = 2;
this->last_change = current_time;
}
if ((this->init_state == 2) and (((current_time) > (this->last_change + OBD9141SIM_WAIT_BEFORE_V1)))){
this->write(v1);
this->init_state = 3;
this->last_change = current_time;
}
if ((this->init_state == 3) and (this->serial->available())){
//this->write(0x08);
uint8_t rec = this->serial->read();
uint8_t inv = ~rec;
if (v1 == inv){
this->init_state = 4;
this->last_change = current_time;
OBD9141println("[SIM] Got inverse, going to send 0xcc");
} else {
OBD9141println("[SIM] fail");
this->uninitialize();
}
}
if ((this->init_state == 4) and (((current_time) > (this->last_change + OBD9141SIM_WAIT_BEFORE_0xCC)))){
this->last_change = millis();
this->write(0xcc);
OBD9141print("[SIM] Init done.");
if (not this->force_init_state){
this->initialised = true;
}
}
}
bool OBD9141::request(void* request, uint8_t request_len, uint8_t ret_len){
uint8_t buf[request_len+1];
memcpy(buf, request, request_len); // copy request
buf[request_len] = this->checksum(&buf, request_len); // add the checksum
this->write(&buf, request_len+1);
// wait after the request, officially 30 ms, but we might as well wait
// for the data in the readBytes function.
// set proper timeout
this->serial->setTimeout(OBD9141_REQUEST_ANSWER_MS_PER_BYTE * ret_len + OBD9141_WAIT_FOR_REQUEST_ANSWER_TIMEOUT);
memset(this->buffer, 0, ret_len+1);
//OBD9141print("Trying to get x bytes: "); OBD9141println(ret_len+1);
if (this->serial->readBytes(this->buffer, ret_len+1)){
// for (uint8_t i=0; i< (ret_len+1); i++){
// OBD9141print(this->buffer[i]);OBD9141print(" ");
// };OBD9141println();
return (this->checksum(&(this->buffer[0]), ret_len) == this->buffer[ret_len]);// have data; return whether it is valid.
} else {
OBD9141println("Timeout on reading bytes.");
return false; // failed getting data.
}
}
void OBD9141sim::answerRequest(uint8_t mode, uint8_t pid){
// raw request: {0x68, 0x6A, 0xF1, 0x01, 0x0D}
// returns: 0x48 0x6B 0x11 0x41 0x0D 0x00 0x12
odb_answer_entry* answer = 0;
for (uint8_t i=0; i < OBD9141SIM_ANSWER_LIST; i++){
if ((this->answers[i].mode == mode) and (this->answers[i].pid == pid)){
answer = &(this->answers[i]);
break;
}
}
if (answer == 0){
// return not supported message?
return;
}
uint8_t message_length = 3+2+answer->len;
OBD9141print("[SIM] have an answer to request, mode: ");
OBD9141print(mode);OBD9141print(" , pid:");
OBD9141print(pid);OBD9141print(", length:");
OBD9141println(message_length+1);
uint8_t message[message_length+1];
message[0] = 0x48;
message[1] = 0x6B;
message[2] = 0x11;
message[3] = 0x40 + mode; // is this actually what happens!?
message[4] = pid;
message[5] = 0;
//memcpy(&(message[5]), &(answer->answer), answer->len);
// cannot memcpy, need to swap the answer
for (uint8_t i=0; i < (answer->len) ; i++){
message[answer->len-1 - i +5] = answer->data[i];
}
message[message_length] = OBD9141::checksum(&(message[0]), message_length);
// for (uint8_t i=0; i < (message_length+1); i++){
// OBD9141print("[SIM] message[");OBD9141print(i);OBD9141print("]:"); OBD9141println(message[i]);
// }
OBD9141print("[SIM] calcd checksum:"); OBD9141println(message[message_length]);
delay(OBD9141_AFTER_REQUEST_DELAY+1);
this->write(&(message[0]), message_length+1);
}
void OBD9141sim::processInterrupt () {
bool new_value = digitalRead(this->rx_pin);
if ((not this->initialised) and (this->init_state==0)){
this->last_change =millis();
this->init_hold_value <<= 1;
this->init_hold_value += new_value;
OBD9141print("[SIM] init_hold_value: "); OBD9141println(init_hold_value);
}
}
bool OBD9141::init(){
// this function performs the ISO9141 5-baud 'slow' init.
this->set_port(false); // disable the port.
this->kline(true);
delay(OBD9141_INIT_IDLE_BUS_BEFORE); // no traffic on bus for 3 seconds.
OBD9141println("Before magic 5 baud.");
// next, send the startup 5 baud init..
this->kline(false); delay(200); // start
this->kline(true); delay(400); // first two bits
this->kline(false); delay(400); // second pair
this->kline(true); delay(400); // third pair
this->kline(false); delay(400); // last pair
this->kline(true); delay(200); // stop bit
// this last 200 ms delay could also be put in the setTimeout below.
// But the spec says we have a stop bit.
// done, from now on it the bus can be treated ad a 10400 baud serial port.
OBD9141println("Before setting port.");
this->set_port(true);
OBD9141println("After setting port.");
uint8_t buffer[1];
this->serial->setTimeout(300+200);
// wait should be between 20 and 300 ms long
// read first value into buffer, should be 0x55
if (this->serial->readBytes(buffer, 1)){
OBD9141print("First read is: "); OBD9141println(buffer[0]);
if (buffer[0] != 0x55){
return false;
}
} else {
OBD9141println("Timeout on read 0x55.");
return false;
}
// we get here after we have passed receiving the first 0x55 from ecu.
this->serial->setTimeout(20); // w2 and w3 are pauses between 5 and 20 ms
uint8_t v1=0, v2=0; // sent by car: (either 0x08 or 0x94)
// read v1
if (!this->serial->readBytes(buffer, 1)){
OBD9141println("Timeout on read v1.");
return false;
} else {
v1 = buffer[0];
OBD9141print("read v1: "); OBD9141println(v1);
}
// read v2
if (!this->serial->readBytes(buffer, 1)){
OBD9141println("Timeout on read v2.");
return false;
} else {
v2 = buffer[0];
OBD9141print("read v2: "); OBD9141println(v2);
}
OBD9141print("v1: "); OBD9141println(v1);
OBD9141print("v2: "); OBD9141println(v2);
// these two should be identical according to the spec.
if (v1 != v2){
return false;
}
// we obtained w1 and w2, now invert and send it back.
// tester waits w4 between 25 and 50 ms:
delay(30);
this->write(~v2);
this->serial->setTimeout(50); // w5 is same as w4... max 50 ms
// finally, attempt to read 0xCC from the ECU, indicating succesful init.
if (!this->serial->readBytes(buffer, 1)){
OBD9141println("Timeout on 0xCC read.");
return false;
} else {
// OBD9141print("read 0xCC?: "); OBD9141println(buffer[0], HEX);
if ((buffer[0] == 0xCC)){ // done if this is inverse of 0x33
delay(OBD9141_INIT_POST_INIT_DELAY);
// this delay is not in the spec, but prevents requests immediately
// after the finishing of the init sequency.
return true; // yay! we are initialised.
} else {
return false;
}
}
}