Update OBD library

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];

 };