diff options
Diffstat (limited to 'libraries/OBD/OBD.cpp')
| -rw-r--r-- | libraries/OBD/OBD.cpp | 217 |
1 files changed, 109 insertions, 108 deletions
diff --git a/libraries/OBD/OBD.cpp b/libraries/OBD/OBD.cpp index 9d457bc..55f31e6 100644 --- a/libraries/OBD/OBD.cpp +++ b/libraries/OBD/OBD.cpp @@ -45,15 +45,15 @@ unsigned char hex2uint8(const char *p) unsigned char c2 = *(p + 1); if (c1 >= 'A' && c1 <= 'F') c1 -= 7; - else if (c1 >='a' && c1 <= 'f') - c1 -= 39; + else if (c1 >='a' && c1 <= 'f') + c1 -= 39; else if (c1 < '0' || c1 > '9') return 0; if (c2 >= 'A' && c2 <= 'F') c2 -= 7; - else if (c2 >= 'a' && c2 <= 'f') - c2 -= 39; + else if (c2 >= 'a' && c2 <= 'f') + c2 -= 39; else if (c2 < '0' || c2 > '9') return 0; @@ -69,20 +69,20 @@ void COBD::Query(unsigned char pid) bool COBD::ReadSensor(byte pid, int& result, bool passive) { - // send a query command - Query(pid); - // wait for reponse - bool hasData; - unsigned long tick = millis(); - do { - DataIdleLoop(); - } while (!(hasData = available()) && millis() - tick < OBD_TIMEOUT_SHORT); - if (!hasData) { - errors++; - return false; - } - // receive and parse the response - return GetResponseParsed(pid, result); + // send a query command + Query(pid); + // wait for reponse + bool hasData; + unsigned long tick = millis(); + do { + DataIdleLoop(); + } while (!(hasData = available()) && millis() - tick < OBD_TIMEOUT_SHORT); + if (!hasData) { + errors++; + return false; + } + // receive and parse the response + return GetResponseParsed(pid, result); } bool COBD::available() @@ -107,7 +107,7 @@ void COBD::write(const char c) int COBD::GetConvertedValue(byte pid, char* data) { - int result; + int result; switch (pid) { case PID_RPM: result = GetLargeValue(data) >> 2; @@ -154,77 +154,78 @@ char* COBD::GetResponse(byte& pid, char* buffer) char c = read(); buffer[i] = c; if (++i == OBD_RECV_BUF_SIZE - 1) { - // buffer overflow - break; + // buffer overflow + break; } if (c == '>' && i > 6) { // prompt char reached break; } } else { - buffer[i] = 0; - unsigned int timeout; - if (dataMode != 1 || strstr_P(buffer, s_searching)) { - timeout = OBD_TIMEOUT_LONG; - } else { - timeout = OBD_TIMEOUT_SHORT; - } - if (millis() - startTime > timeout) { - // timeout - errors++; - break; - } + buffer[i] = 0; + unsigned int timeout; + if (dataMode != 1 || strstr_P(buffer, s_searching)) { + timeout = OBD_TIMEOUT_LONG; + } else { + timeout = OBD_TIMEOUT_SHORT; + } + if (millis() - startTime > timeout) { + // timeout + errors++; + break; + } + DataIdleLoop(); } } buffer[i] = 0; char *p = buffer; while ((p = strstr_P(p, s_response_begin))) { - p += 3; - byte curpid = hex2uint8(p); - if (pid == 0) pid = curpid; - if (curpid == pid) { - errors = 0; - p += 2; - if (*p == ' ') - return p + 1; - } + p += 3; + byte curpid = hex2uint8(p); + if (pid == 0) pid = curpid; + if (curpid == pid) { + errors = 0; + p += 2; + if (*p == ' ') + return p + 1; + } } return 0; } bool COBD::GetResponseParsed(byte& pid, int& result) { - char buffer[OBD_RECV_BUF_SIZE]; - char* data = GetResponse(pid, buffer); - if (!data) { - // try recover next time - write('\r'); + char buffer[OBD_RECV_BUF_SIZE]; + char* data = GetResponse(pid, buffer); + if (!data) { + // try recover next time + write('\r'); return false; - } - result = GetConvertedValue(pid, data); - return true; + } + result = GetConvertedValue(pid, data); + return true; } void COBD::Sleep(int seconds) { - char cmd[MAX_CMD_LEN]; - strcpy_P(cmd, s_cmd_sleep); - write(cmd); - if (seconds) { - delay((unsigned long)seconds << 10); - write('\r'); - } + char cmd[MAX_CMD_LEN]; + strcpy_P(cmd, s_cmd_sleep); + write(cmd); + if (seconds) { + delay((unsigned long)seconds << 10); + write('\r'); + } } bool COBD::IsValidPID(byte pid) { - if (pid >= 0x7f) - return false; - pid--; - byte i = pid >> 3; - byte b = 0x80 >> (pid & 0x7); - return pidmap[i] & b; + if (pid >= 0x7f) + return false; + pid--; + byte i = pid >> 3; + byte b = 0x80 >> (pid & 0x7); + return pidmap[i] & b; } bool COBD::Init(bool passive) @@ -235,51 +236,51 @@ bool COBD::Init(bool passive) char buffer[OBD_RECV_BUF_SIZE]; for (unsigned char i = 0; i < INIT_CMD_COUNT; i++) { - if (!passive) { - char cmd[MAX_CMD_LEN]; - strcpy_P(cmd, s_initcmd[i]); - write(cmd); - } - n = 0; - prompted = 0; - currentMillis = millis(); - for (;;) { - if (available()) { - char c = read(); - if (c == '>') { - buffer[n] = 0; - prompted++; - } else if (n < OBD_RECV_BUF_SIZE - 1) { - buffer[n++] = c; - } - } else if (prompted) { - break; - } else { - unsigned long elapsed = millis() - currentMillis; - if (elapsed > OBD_TIMEOUT_INIT) { - // init timeout - //WriteData("\r"); - return false; - } - InitIdleLoop(); - } - } - } + if (!passive) { + char cmd[MAX_CMD_LEN]; + strcpy_P(cmd, s_initcmd[i]); + write(cmd); + } + n = 0; + prompted = 0; + currentMillis = millis(); + for (;;) { + if (available()) { + char c = read(); + if (c == '>') { + buffer[n] = 0; + prompted++; + } else if (n < OBD_RECV_BUF_SIZE - 1) { + buffer[n++] = c; + } + } else if (prompted) { + break; + } else { + unsigned long elapsed = millis() - currentMillis; + if (elapsed > OBD_TIMEOUT_INIT) { + // init timeout + //WriteData("\r"); + return false; + } + InitIdleLoop(); + } + } + } - // load pid map - memset(pidmap, 0, sizeof(pidmap)); - for (byte i = 0; i < 4; i++) { - byte pid = i * 0x20; - Query(pid); - char* data = GetResponse(pid, buffer); - if (!data) break; - data--; - for (byte n = 0; n < 4; n++) { - if (data[n * 3] != ' ') - break; - pidmap[i * 4 + n] = hex2uint8(data + n * 3 + 1); - } - } - errors = 0; + // load pid map + memset(pidmap, 0, sizeof(pidmap)); + for (byte i = 0; i < 4; i++) { + byte pid = i * 0x20; + Query(pid); + char* data = GetResponse(pid, buffer); + if (!data) break; + data--; + for (byte n = 0; n < 4; n++) { + if (data[n * 3] != ' ') + break; + pidmap[i * 4 + n] = hex2uint8(data + n * 3 + 1); + } + } + errors = 0; return true; } |