Mejoras de funcionamiento: robustez en parser getAB y timeout de comunicacion OBD2
This commit is contained in:
parent
98bbbf0109
commit
658cb520d7
2 changed files with 94 additions and 52 deletions
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in a new issue