From 91335d13a15c13cbb1fad11b35e89d14ebd8e903 Mon Sep 17 00:00:00 2001 From: Stanley Huang Date: Mon, 24 Feb 2014 01:44:56 +0800 Subject: Update OBD library --- libraries/OBD/OBD.cpp | 397 +++++++++++++++++++++++++++++--------------------- libraries/OBD/OBD.h | 43 +++--- 2 files changed, 258 insertions(+), 182 deletions(-) diff --git a/libraries/OBD/OBD.cpp b/libraries/OBD/OBD.cpp index 95c1463..b02cea9 100644 --- a/libraries/OBD/OBD.cpp +++ b/libraries/OBD/OBD.cpp @@ -75,7 +75,7 @@ void COBD::sendQuery(unsigned char pid) write(cmd); } -bool COBD::read(byte pid, int& result, bool passive) +bool COBD::read(byte pid, int& result) { // send a query command sendQuery(pid); @@ -90,7 +90,7 @@ bool COBD::read(byte pid, int& result, bool passive) return false; } // receive and parse the response - return getResponseParsed(pid, result); + return getResult(pid, result); } bool COBD::available() @@ -133,21 +133,19 @@ int COBD::normalizeData(byte pid, char* data) case PID_ENGINE_OIL_TEMP: result = getTemperatureValue(data); break; + case PID_THROTTLE: + case PID_ENGINE_LOAD: + case PID_FUEL_LEVEL: case PID_ABSOLUTE_ENGINE_LOAD: case PID_ETHANOL_PERCENTAGE: case PID_HYBRID_BATTERY_PERCENTAGE: - result = getLargeValue(data) * 100 / 255; // % + result = getPercentageValue(data); break; case PID_MAF_FLOW: result = getLargeValue(data) / 100; break; - case PID_THROTTLE: - case PID_ENGINE_LOAD: - case PID_FUEL_LEVEL: - result = getPercentageValue(data); - break; case PID_TIMING_ADVANCE: - result = (getSmallValue(data) - 128) >> 1; + result = (int)(getSmallValue(data) / 2) - 64; break; case PID_DISTANCE: // km case PID_RUNTIME: // second @@ -158,8 +156,8 @@ int COBD::normalizeData(byte pid, char* data) case PID_CONTROL_MODULE_VOLTAGE: // V result = getLargeValue(data) / 1000; break; - case PID_ENGINE_FUEL_RATE: // L/min - result = getLargeValue(data) * 3; + case PID_ENGINE_FUEL_RATE: // L/h + result = getLargeValue(data) / 20; break; case PID_ENGINE_TORQUE_PERCENTAGE: // % result = (int)getSmallValue(data) - 125; @@ -172,31 +170,31 @@ int COBD::normalizeData(byte pid, char* data) char* COBD::getResponse(byte& pid, char* buffer) { - byte n = receive(buffer); - if (n > 6) { - char *p = buffer; - while ((p = strstr(p, "41 "))) { - p += 3; - byte curpid = hex2uint8(p); - if (pid == 0) pid = curpid; - if (curpid == pid) { - errors = 0; - p += 2; - if (*p == ' ') - return p + 1; - } - }; + receive(buffer); + char *p = buffer; + while ((p = strstr(p, "41 "))) { + p += 3; + byte curpid = hex2uint8(p); + if (pid == 0) pid = curpid; + if (curpid == pid) { + errors = 0; + p += 2; + if (*p == ' ') + return p + 1; + } else { + receive(buffer); + p = buffer; + } } return 0; } -bool COBD::getResponseParsed(byte& pid, int& result) +bool COBD::getResult(byte& pid, int& result) { char buffer[OBD_RECV_BUF_SIZE]; char* data = getResponse(pid, buffer); if (!data) { - // try recover next time - //write('\r'); + recover(); return false; } result = normalizeData(pid, data); @@ -229,65 +227,64 @@ void COBD::begin() OBDUART.begin(OBD_SERIAL_BAUDRATE); } -byte COBD::receive(char* buffer) +byte COBD::receive(char* buffer, int timeout) { - unsigned long startTime = millis(); + unsigned long startTime = millis(); unsigned char n = 0; - int timeout = OBD_TIMEOUT_SHORT; - bool prompted = false; - - buffer[0] = 0; - for (;;) { - if (available()) { - char c = read(); - if (n > 2 && c == '>') { - // prompt char received - prompted = true; - } else if (n < OBD_RECV_BUF_SIZE - 1) { - buffer[n++] = c; - buffer[n] = 0; - if (strstr(buffer, STR_SEARCHING)) { - strcpy(buffer, buffer + sizeof(STR_SEARCHING)); - n -= sizeof(STR_SEARCHING); - timeout = OBD_TIMEOUT_LONG; - } - } - } else if (prompted) { - break; - } else { - if (millis() - startTime > timeout) { - // timeout - return 0; - } - dataIdleLoop(); - } - } + bool prompted = false; + + buffer[0] = 0; + for (;;) { + if (available()) { + char c = read(); + if (n > 2 && c == '>') { + // prompt char received + prompted = true; + } else if (n < OBD_RECV_BUF_SIZE - 1) { + buffer[n++] = c; + buffer[n] = 0; + if (strstr(buffer, STR_SEARCHING)) { + strcpy(buffer, buffer + sizeof(STR_SEARCHING)); + n -= sizeof(STR_SEARCHING); + timeout = OBD_TIMEOUT_LONG; + } + } + } else if (prompted) { + break; + } else { + if (millis() - startTime > timeout) { + // timeout + return 0; + } + dataIdleLoop(); + } + } + return n; +} - return n; +void COBD::recover() +{ + write('\r'); + delay(50); + while (available()) read(); } -bool COBD::init(bool passive) +bool COBD::init() { - unsigned long currentMillis; char buffer[OBD_RECV_BUF_SIZE]; - m_state = OBD_CONNECTING; - - write('\r'); - delay(100); - while (available()) read(); + m_state = OBD_CONNECTING; + recover(); for (unsigned char i = 0; i < sizeof(s_initcmd) / sizeof(s_initcmd[0]); i++) { - if (!passive) { - char cmd[MAX_CMD_LEN]; - strcpy_P(cmd, s_initcmd[i]); + char cmd[MAX_CMD_LEN]; + strcpy_P(cmd, s_initcmd[i]); #ifdef DEBUG - debugOutput(cmd); + debugOutput(cmd); #endif - write(cmd); - } + write(cmd); if (receive(buffer) == 0) { - return false; + return false; } } while (available()) read(); @@ -295,20 +292,20 @@ bool COBD::init(bool passive) // load pid map memset(pidmap, 0, sizeof(pidmap)); for (byte i = 0; i < 4; i++) { - byte pid = i * 0x20; - sendQuery(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); - } + byte pid = i * 0x20; + sendQuery(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); + } } while (available()) read(); - m_state = OBD_CONNECTED; + m_state = OBD_CONNECTED; errors = 0; return true; } @@ -316,10 +313,10 @@ bool COBD::init(bool passive) #ifdef DEBUG void COBD::debugOutput(const char *s) { - DEBUG.print('['); - DEBUG.print(millis()); - DEBUG.print(']'); - DEBUG.print(s); + DEBUG.print('['); + DEBUG.print(millis()); + DEBUG.print(']'); + DEBUG.print(s); } #endif @@ -329,124 +326,196 @@ void COBD::debugOutput(const char *s) void COBDI2C::begin(byte addr) { - m_addr = addr; - Wire.begin(); + m_addr = addr; + Wire.begin(); + memset(obdPid, 0, sizeof(obdPid)); + memset(obdInfo, 0, sizeof(obdInfo)); } bool COBDI2C::init() { - m_state = OBD_CONNECTING; - sendCommand(CMD_QUERY_STATUS); - - char recvbuf[MAX_PAYLOAD_SIZE]; - for (byte n = 0; n < 3; n++) { - memset(recvbuf, 0, sizeof(recvbuf)); - receive(recvbuf); - if (!memcmp(recvbuf, "OBD ", 4)) - break; - } - if (recvbuf[4] == 'Y') { - memcpy(pidmap, recvbuf + 16, sizeof(pidmap)); - m_state = OBD_CONNECTED; - return true; - } else { - m_state = OBD_DISCONNECTED; - return false; - } + m_state = OBD_CONNECTING; + sendCommand(CMD_QUERY_STATUS); + + char recvbuf[MAX_PAYLOAD_SIZE]; + for (byte n = 0; n < 3; n++) { + memset(recvbuf, 0, sizeof(recvbuf)); + receive(recvbuf); + if (!memcmp(recvbuf, "OBD ", 4)) + break; + } + if (recvbuf[4] == 'Y') { + memcpy(pidmap, recvbuf + 16, sizeof(pidmap)); + m_state = OBD_CONNECTED; + return true; + } else { + m_state = OBD_DISCONNECTED; + return false; + } } -bool COBDI2C::read(byte pid, int& result, bool passive) +bool COBDI2C::read(byte pid, int& result) { - uint32_t t = millis(); - sendQuery(pid); - dataIdleLoop(); - return getResponseParsed(pid, result); + sendQuery(pid); + dataIdleLoop(); + return getResult(pid, result); } void COBDI2C::write(char* s) { - COMMAND_BLOCK cmdblock = {millis(), CMD_SEND_COMMAND}; - Wire.beginTransmission(m_addr); - Wire.write((byte*)&cmdblock, sizeof(cmdblock)); - Wire.write(s); - Wire.endTransmission(); + COMMAND_BLOCK cmdblock = {millis(), CMD_SEND_AT_COMMAND}; + Wire.beginTransmission(m_addr); + Wire.write((byte*)&cmdblock, sizeof(cmdblock)); + Wire.write(s); + Wire.endTransmission(); } bool COBDI2C::sendCommand(byte cmd, uint8_t data, byte* payload, byte payloadBytes) { - COMMAND_BLOCK cmdblock = {millis(), cmd, data}; - Wire.beginTransmission(m_addr); - bool success = Wire.write((byte*)&cmdblock, sizeof(COMMAND_BLOCK)) == sizeof(COMMAND_BLOCK); - if (payload) Wire.write(payload, payloadBytes); - Wire.endTransmission(); - return success; + COMMAND_BLOCK cmdblock = {millis(), cmd, data}; + Wire.beginTransmission(m_addr); + bool success = Wire.write((byte*)&cmdblock, sizeof(COMMAND_BLOCK)) == sizeof(COMMAND_BLOCK); + if (payload) Wire.write(payload, payloadBytes); + Wire.endTransmission(); + return success; } -byte COBDI2C::receive(char* buffer) +byte COBDI2C::receive(char* buffer, int timeout) { - uint32_t start = millis(); - byte offset = 0; - do { - Wire.requestFrom((byte)m_addr, (byte)MAX_PAYLOAD_SIZE, (byte)1); + uint32_t start = millis(); + byte offset = 0; + do { + Wire.requestFrom((byte)m_addr, (byte)MAX_PAYLOAD_SIZE, (byte)1); - bool hasEnd = false; - for (byte i = 0; i < MAX_PAYLOAD_SIZE; i++) { - if ((buffer[offset + i] = Wire.read()) == 0) - hasEnd = true; - } + bool hasEnd = false; + for (byte i = 0; i < MAX_PAYLOAD_SIZE; i++) { + if ((buffer[offset + i] = Wire.read()) == 0) + hasEnd = true; + } - if (buffer[0] == 0) { - // data not ready - dataIdleLoop(); - continue; - } + if (buffer[0] == 0) { + // data not ready + dataIdleLoop(); + continue; + } - offset += MAX_PAYLOAD_SIZE; - if (!hasEnd) { - continue; - } + offset += MAX_PAYLOAD_SIZE; + if (!hasEnd) { + continue; + } - return offset; - } while(millis() - start < OBD_TIMEOUT_LONG); - return 0; + return offset; + } while(millis() - start < OBD_TIMEOUT_LONG); + return 0; } bool COBDI2C::btInit(uint16_t baudrate) { - return sendCommand(CMD_UART_BEGIN, baudrate / 1200); + return sendCommand(CMD_UART_BEGIN, baudrate / 1200); } bool COBDI2C::btSend(byte* data, byte length) { - return sendCommand(CMD_UART_SEND, 0, data, length); + return sendCommand(CMD_UART_SEND, 0, data, length); } bool COBDI2C::btReceive(byte* buffer, byte bufsize) { - if (!sendCommand(CMD_UART_RECV, bufsize)) return false; - memset(buffer, 0, MAX_PAYLOAD_SIZE); - delay(10); - Wire.requestFrom((byte)m_addr, (byte)MAX_PAYLOAD_SIZE, (byte)1); - Wire.readBytes((char*)buffer, MAX_PAYLOAD_SIZE); - return true; + if (!sendCommand(CMD_UART_RECV, bufsize)) return false; + memset(buffer, 0, MAX_PAYLOAD_SIZE); + delay(10); + Wire.requestFrom((byte)m_addr, (byte)MAX_PAYLOAD_SIZE, (byte)1); + Wire.readBytes((char*)buffer, MAX_PAYLOAD_SIZE); + return true; } bool COBDI2C::gpsQuery(GPS_DATA* gpsdata) { - if (!sendCommand(CMD_GPS_QUERY, 0)) return false; - delay(1); - Wire.requestFrom((byte)m_addr, (byte)MAX_PAYLOAD_SIZE, (byte)1); - Wire.readBytes((char*)gpsdata, MAX_PAYLOAD_SIZE); - return true; + if (!sendCommand(CMD_GPS_QUERY, 0)) return false; + Wire.requestFrom((byte)m_addr, (byte)MAX_PAYLOAD_SIZE, (byte)1); + Wire.readBytes((char*)gpsdata, MAX_PAYLOAD_SIZE); + return true; +} + +void COBDI2C::gpsSetup(uint32_t baudrate, const char* cmds) +{ + sendCommand(CMD_GPS_SETUP, baudrate / 1200, (byte*)cmds, cmds ? strlen(cmds) : 0); +} + +void COBDI2C::setPID(byte pid) +{ + byte n = 0; + for (; n < MAX_PIDS && obdPid[n]; n++) { + if (obdPid[n] == pid) + return; + } + if (n == MAX_PIDS) { + memmove(obdPid, obdPid + 1, sizeof(obdPid[0]) * (MAX_PIDS - 1)); + n = MAX_PIDS - 1; + } + obdPid[n] = pid; +} +void COBDI2C::applyPIDs() +{ + sendCommand(CMD_APPLY_OBD_PIDS, 0, (byte*)obdPid, sizeof(obdPid)); + delay(200); } -void COBDI2C::gpsStart(uint32_t baudrate, const char* cmds) +void COBDI2C::loadData() { - sendCommand(CMD_GPS_START, baudrate / 1200, (byte*)cmds, strlen(cmds)); + sendCommand(CMD_LOAD_OBD_DATA); + dataIdleLoop(); + Wire.requestFrom((byte)m_addr, (byte)MAX_PAYLOAD_SIZE, (byte)0); + Wire.readBytes((char*)obdInfo, MAX_PAYLOAD_SIZE); } -void COBDI2C::gpsStop() +uint16_t COBDI2C::getData(byte pid, int& result) { - sendCommand(CMD_GPS_STOP); + byte n; + for (n = 0; n < MAX_PIDS && obdPid[n] != pid; n++); + if (n == MAX_PIDS) + return -1; + + PID_INFO* pi = obdInfo + n; + switch (pid) { + case PID_RPM: + result = pi->value >> 2; + break; + case PID_FUEL_PRESSURE: + result = (int)pi->value * 3; + break; + case PID_COOLANT_TEMP: + case PID_INTAKE_TEMP: + case PID_AMBIENT_TEMP: + case PID_ENGINE_OIL_TEMP: + result = (int)pi->value - 40; + break; + case PID_THROTTLE: + case PID_ENGINE_LOAD: + case PID_FUEL_LEVEL: + case PID_ABSOLUTE_ENGINE_LOAD: + case PID_ETHANOL_PERCENTAGE: + case PID_HYBRID_BATTERY_PERCENTAGE: + result = pi->value * 100 / 255; // % + break; + case PID_MAF_FLOW: + result = pi->value / 100; + break; + case PID_TIMING_ADVANCE: + result = (int)pi->value / 2 - 64; + break; + case PID_CONTROL_MODULE_VOLTAGE: // V + result = pi->value / 1000; + break; + case PID_ENGINE_FUEL_RATE: // L/h + result = pi->value / 20; + break; + case PID_ENGINE_TORQUE_PERCENTAGE: // % + result = (int)pi->value - 125; + break; + default: + result = pi->value; + } + return result; } diff --git a/libraries/OBD/OBD.h b/libraries/OBD/OBD.h index 2c0f670..ba44f20 100644 --- a/libraries/OBD/OBD.h +++ b/libraries/OBD/OBD.h @@ -57,11 +57,12 @@ class COBD public: COBD():dataMode(1),errors(0),m_state(OBD_DISCONNECTED) {} virtual void begin(); - virtual bool init(bool passive = false); - virtual bool read(byte pid, int& result, bool passive = false); + virtual bool init(); + virtual bool read(byte pid, int& result); virtual void sleep(int seconds); // Query and GetResponse for advanced usage only virtual void sendQuery(byte pid); + virtual bool getResult(byte& pid, int& result); bool isValidPID(byte pid); byte getState() { return m_state; } byte dataMode; @@ -70,13 +71,13 @@ public: byte vin[17]; protected: virtual char* getResponse(byte& pid, char* buffer); - virtual bool getResponseParsed(byte& pid, int& result); - virtual byte receive(char* buffer); + virtual byte receive(char* buffer, int timeout = OBD_TIMEOUT_SHORT); virtual bool available(); virtual char read(); virtual void write(char* s); virtual void write(char c); virtual void dataIdleLoop() {} + void recover(); void debugOutput(const char* s); int normalizeData(byte pid, char* data); byte m_state; @@ -102,21 +103,21 @@ private: #define I2C_ADDR 0x62 #define MAX_PAYLOAD_SIZE 32 +#define MAX_PIDS 8 #define CMD_QUERY_STATUS 0x10 -#define CMD_SEND_COMMAND 0x11 -#define CMD_QUERY_DATA 0x12 -#define CMD_UART_BEGIN 0x13 -#define CMD_UART_SEND 0x14 -#define CMD_UART_RECV 0x15 -#define CMD_GPS_START 0x20 -#define CMD_GPS_STOP 0x21 +#define CMD_SEND_AT_COMMAND 0x11 +#define CMD_APPLY_OBD_PIDS 0x12 +#define CMD_LOAD_OBD_DATA 0x13 +#define CMD_GPS_SETUP 0x20 #define CMD_GPS_QUERY 0x22 +#define CMD_UART_BEGIN 0x30 +#define CMD_UART_SEND 0x31 +#define CMD_UART_RECV 0x32 typedef struct { - uint32_t time; - uint16_t pid; - float value; + uint16_t age; + uint16_t value; } PID_INFO; typedef struct { @@ -142,18 +143,24 @@ class COBDI2C : public COBD { public: void begin(byte addr = I2C_ADDR); bool init(); - bool read(byte pid, int& result, bool passive = false); + bool read(byte pid, int& result); void write(char* s); + // Asynchronized access API + void setPID(byte pid); + void applyPIDs(); + void loadData(); + uint16_t getData(byte pid, int& result); // Bluetooth communication API bool btInit(uint16_t baudrate = 9600); bool btSend(byte* data, byte length); bool btReceive(byte* buffer, byte bufsize); // GPS API bool gpsQuery(GPS_DATA* gpsdata); - void gpsStart(uint32_t baudrate = 38400, const char* cmds = 0); - void gpsStop(); + void gpsSetup(uint32_t baudrate, const char* cmds = 0); private: bool sendCommand(byte cmd, uint8_t data = 0, byte* payload = 0, byte payloadBytes = 0); - byte receive(char* buffer); + byte receive(char* buffer, int timeout = OBD_TIMEOUT_SHORT); byte m_addr; + PID_INFO obdInfo[MAX_PIDS]; + byte obdPid[MAX_PIDS]; }; -- cgit v1.2.3