diff --git a/sketch/obd2_gauge/meter.h b/sketch/obd2_gauge/meter.h index f97aa66..a172533 100644 --- a/sketch/obd2_gauge/meter.h +++ b/sketch/obd2_gauge/meter.h @@ -634,40 +634,87 @@ void initScreen() { /*-------------------------------*/ // get A B from response return in global A B variable -void getAB(String elm_rsp) { // 41 05 2A 3C 01> - if (pidList[pidIndex] == "ATRV") { +void getAB(String elm_rsp, String currentPid) { // 41 05 2A 3C 01> + A = 0; + B = 0; + if (currentPid == "ATRV") { return; } - if (elm_rsp.indexOf("NO DATA") != -1) { // pid no data - A = 0; - B = 0; + if (elm_rsp.indexOf("NO DATA") != -1 || + elm_rsp.indexOf("UNABLE") != -1 || + elm_rsp.indexOf("ERROR") != -1 || + elm_rsp.indexOf("STOPPED") != -1) { return; } - // check if pid is 2 or 3 command length. - uint8_t charcnt = pidList[pidIndex].length() / 2; // 0105->2,221E1C->3 - String strs[8]; - uint8_t StringCount = 0; - while (elm_rsp.length() > 0) { // keep reading each char - int index = elm_rsp.indexOf(' '); // check space - if (index == -1) // No space found - { // only data without space is now in strs {41,05,aa,bb} - strs[StringCount++] = elm_rsp; // last byte - if (StringCount > charcnt + 1) { - A = strtol(strs[charcnt].c_str(), NULL, 16); // byte 3 save to A - B = strtol(strs[charcnt + 1].c_str(), NULL, 16); // byte 4 save to B - } else if (StringCount > charcnt) { - A = strtol(strs[charcnt].c_str(), NULL, 16); - B = 0; - } - break; - } else { // found space - strs[StringCount++] = - elm_rsp.substring(0, index); // copy strings from 0 to space to strs - elm_rsp = elm_rsp.substring(index + - 1); // copy the rest behind space to elm_rsp - if (StringCount >= 7) break; + + // Normalize: replace carriage returns, newlines, and prompt chars with spaces + elm_rsp.replace('\r', ' '); + elm_rsp.replace('\n', ' '); + elm_rsp.replace('>', ' '); + + // Extract expected PID hex byte (e.g. "0105" -> "05", "010C" -> "0C", "010D" -> "0D") + String pidByte = (currentPid.length() >= 4) ? currentPid.substring(2) : currentPid; + pidByte.toUpperCase(); + + // 1. Tokenized search: look for "41" followed by PID byte + String tokens[16]; + uint8_t tokenCount = 0; + int startIdx = 0; + while (startIdx < elm_rsp.length() && tokenCount < 16) { + while (startIdx < elm_rsp.length() && elm_rsp.charAt(startIdx) == ' ') { + startIdx++; } - } // while + if (startIdx >= elm_rsp.length()) break; + int endIdx = elm_rsp.indexOf(' ', startIdx); + if (endIdx == -1) endIdx = elm_rsp.length(); + tokens[tokenCount++] = elm_rsp.substring(startIdx, endIdx); + startIdx = endIdx + 1; + } + + for (uint8_t i = 0; i < tokenCount; i++) { + tokens[i].toUpperCase(); + } + + for (uint8_t i = 0; i + 1 < tokenCount; i++) { + if (tokens[i] == "41" && tokens[i + 1] == pidByte) { + if (i + 2 < tokenCount) { + A = strtol(tokens[i + 2].c_str(), NULL, 16); + } + if (i + 3 < tokenCount) { + B = strtol(tokens[i + 3].c_str(), NULL, 16); + } + return; + } + } + + // 2. Compact string fallback (in case adapter sends responses without spaces, e.g. "410D32") + String compact = ""; + for (int i = 0; i < elm_rsp.length(); i++) { + char c = elm_rsp.charAt(i); + if (c != ' ') compact += (char)toupper(c); + } + String targetPrefix = "41" + pidByte; + int fIdx = compact.indexOf(targetPrefix); + if (fIdx != -1) { + int dataIdx = fIdx + targetPrefix.length(); + if (dataIdx + 2 <= compact.length()) { + A = strtol(compact.substring(dataIdx, dataIdx + 2).c_str(), NULL, 16); + } + if (dataIdx + 4 <= compact.length()) { + B = strtol(compact.substring(dataIdx + 2, dataIdx + 4).c_str(), NULL, 16); + } + return; + } + + // 3. Positional fallback as in original author code + uint8_t charcnt = currentPid.length() / 2; + if (tokenCount > charcnt + 1) { + A = strtol(tokens[charcnt].c_str(), NULL, 16); + B = strtol(tokens[charcnt + 1].c_str(), NULL, 16); + } else if (tokenCount > charcnt) { + A = strtol(tokens[charcnt].c_str(), NULL, 16); + B = 0; + } } // getAB /*-------------------------------*/ void updateMeter(uint8_t pidNo, String response) { // update parameter on screen @@ -684,7 +731,7 @@ void updateMeter(uint8_t pidNo, String response) { // update parameter on screen bool digit = pidConfig[pidIdx][7].toInt(); int warn = warningValue[pidIdx].toInt(); - getAB(response); + getAB(response, pidList[pidNo]); float data = 0.0; switch (formula) { case 0: @@ -705,12 +752,12 @@ void updateMeter(uint8_t pidNo, String response) { // update parameter on screen case 5: data = (256 * A + B) / 16.0; if (data > 150 || data < 0) - data = old_data[pidIndex]; + data = old_data[pidNo]; break; case 6: data = (A * 256 + B) * 5 / 72 - 18; if (data > 150 || data < 0) - data = old_data[pidIndex]; + data = old_data[pidNo]; break; case 7: data = A; // km/h (PID 010D Vehicle Speed) diff --git a/sketch/obd2_gauge/obd2_gauge.ino b/sketch/obd2_gauge/obd2_gauge.ino index 23b80c8..c710bac 100644 --- a/sketch/obd2_gauge/obd2_gauge.ino +++ b/sketch/obd2_gauge/obd2_gauge.ino @@ -327,29 +327,24 @@ recent_client_addr : {0x00,0x00,0x00,0x00,0x00,0x00} array of bytes[6] bt_message = ""; for (uint8_t initIndex = 0; initIndex < elm327InitCount; initIndex++) { //send init ELM327 AT command BTSerial.print(elm327Init[initIndex] + "\r"); - uint32_t waitTimeout = (initIndex == elm327InitCount - 1) ? 5000 : 1500; - uint32_t startWait = millis(); - while (millis() - startWait < waitTimeout) { - if (BTSerial.available() > 0) { - char incomingChar = BTSerial.read(); - if (incomingChar == '>') { //check prompt - BTSerial.flush(); //clear buffer + if (initIndex == elm327InitCount - 1) delay(2000); //wait for 0100 Searching..protocal + else delay(100); //must add delay , unless will skip BTSerial.avaialble>0 + while (BTSerial.available() > 0) { + char incomingChar = BTSerial.read(); + if (incomingChar == '>') { //check prompt + BTSerial.flush(); //clear buffer #ifdef SERIAL_DEBUG - Serial.println(bt_message); + Serial.println(bt_message); #endif #ifdef TERMINAL - Terminal(bt_message, 0, 48, 320, 191); + Terminal(bt_message, 0, 48, 320, 191); #endif - if (initIndex == 1) BTSerial.print("\r"); //ATBRD23 response - bt_message = ""; - break; - } else { - bt_message.concat(incomingChar); //keep elm327 respond - } + if (initIndex == 1) BTSerial.print("\r"); //ATBRD23 response + bt_message = ""; } else { - delay(10); + bt_message.concat(incomingChar); //keep elm327 respond } - } //while waitTimeout + } //while } //for initScreen(); @@ -461,8 +456,8 @@ void loop() { //---------------------- //BLUETOOTH read static uint32_t lastPidSendTime = 0; - // Watchdog: If waiting for prompt for more than 1500ms, recover! - if (!prompt && (millis() - lastPidSendTime > 1500)) { + // Watchdog: If waiting for prompt for more than 6000ms (standard OBD2 maximum ECU timeout), recover! + if (!prompt && (millis() - lastPidSendTime > 6000)) { #ifdef SERIAL_DEBUG Serial.printf("PID timeout for %s, recovering...\n", pidList[pidIndex].c_str()); #endif @@ -480,7 +475,7 @@ void loop() { Serial.println(bt_message); #endif #ifdef TERMINAL //for degbugging - getAB(bt_message); + getAB(bt_message, pidList[pidIndex]); Terminal(bt_message + "->" + String(A) + "," + String(B), 0, 48, 320, 191); #endif prompt = true; //pid response ready to calculate & display