diff options
| -rw-r--r-- | libraries/OBD/OBD.cpp | 1015 |
1 files changed, 506 insertions, 509 deletions
diff --git a/libraries/OBD/OBD.cpp b/libraries/OBD/OBD.cpp index 2c9a2c7..328d91d 100644 --- a/libraries/OBD/OBD.cpp +++ b/libraries/OBD/OBD.cpp @@ -1,509 +1,506 @@ -/*************************************************************************
-* Arduino Library for OBD-II UART/I2C Adapter
-* Distributed under GPL v2.0
-* Visit http://freematics.com for more information
-* (C)2012-2014 Stanley Huang <stanleyhuangyc@gmail.com>
-*************************************************************************/
-
-#include <Arduino.h>
-#include "OBD.h"
-
-//#define DEBUG Serial
-//#define REDIRECT Serial
-
-uint16_t hex2uint16(const char *p)
-{
- char c = *p;
- uint16_t i = 0;
- for (char n = 0; c && n < 4; c = *(++p)) {
- if (c >= 'A' && c <= 'F') {
- c -= 7;
- } else if (c>='a' && c<='f') {
- c -= 39;
- } else if (c == ' ') {
- continue;
- } else if (c < '0' || c > '9') {
- break;
- }
- i = (i << 4) | (c & 0xF);
- n++;
- }
- return i;
-}
-
-byte hex2uint8(const char *p)
-{
- byte c1 = *p;
- byte c2 = *(p + 1);
- if (c1 >= 'A' && c1 <= 'F')
- c1 -= 7;
- 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 < '0' || c2 > '9')
- return 0;
-
- return c1 << 4 | (c2 & 0xf);
-}
-
-/*************************************************************************
-* OBD-II UART Adapter
-*************************************************************************/
-#include <Wire.h>
-
-void COBD::sendQuery(byte pid)
-{
- char cmd[8];
- sprintf(cmd, "%02X%02X\r", dataMode, pid);
-#ifdef DEBUG
- debugOutput(cmd);
-#endif
- write(cmd);
-}
-
-bool COBD::read(byte pid, int& result)
-{
- // send a query command
- sendQuery(pid);
- // receive and parse the response
- return getResult(pid, result);
-}
-
-bool COBD::available()
-{
- return OBDUART.available();
-}
-
-char COBD::read()
-{
- char c = OBDUART.read();
-#ifdef REDIRECT
- REDIRECT.write(c);
-#endif
- return c;
-}
-
-void COBD::write(const char* s)
-{
- OBDUART.write(s);
-}
-
-void COBD::write(char c)
-{
- OBDUART.write(c);
-}
-
-int COBD::normalizeData(byte pid, char* data)
-{
- int result;
- switch (pid) {
- case PID_RPM:
- result = getLargeValue(data) >> 2;
- break;
- case PID_FUEL_PRESSURE:
- result = getSmallValue(data) * 3;
- break;
- case PID_COOLANT_TEMP:
- case PID_INTAKE_TEMP:
- case PID_AMBIENT_TEMP:
- 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 = getPercentageValue(data);
- break;
- case PID_MAF_FLOW:
- result = getLargeValue(data) / 100;
- break;
- case PID_TIMING_ADVANCE:
- result = (int)(getSmallValue(data) / 2) - 64;
- break;
- case PID_DISTANCE: // km
- case PID_RUNTIME: // second
- case PID_FUEL_RAIL_PRESSURE: // kPa
- case PID_ENGINE_REF_TORQUE: // Nm
- result = getLargeValue(data);
- break;
- case PID_CONTROL_MODULE_VOLTAGE: // V
- result = getLargeValue(data) / 1000;
- break;
- case PID_ENGINE_FUEL_RATE: // L/h
- result = getLargeValue(data) / 20;
- break;
- case PID_ENGINE_TORQUE_PERCENTAGE: // %
- result = (int)getSmallValue(data) - 125;
- break;
- default:
- result = getSmallValue(data);
- }
- return result;
-}
-
-char* COBD::getResponse(byte& pid, char* buffer)
-{
- while (receive(buffer, OBD_TIMEOUT_SHORT) > 0) {
- 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;
- }
- }
- }
- return 0;
-}
-
-bool COBD::getResult(byte& pid, int& result)
-{
- char buffer[OBD_RECV_BUF_SIZE];
- char* data = getResponse(pid, buffer);
- if (!data) {
- recover();
- errors++;
- return false;
- }
- result = normalizeData(pid, data);
- return true;
-}
-
-void COBD::setProtocol(byte h)
-{
- if (h == -1) {
- write("ATSP00\r");
- } else {
- char cmd[8];
- sprintf(cmd, "ATSP%d\r", h);
- write(cmd);
- }
-}
-
-void COBD::sleep()
-{
- write("ATLP\r");
-}
-
-void COBD::wakeup()
-{
- write('\r');
-}
-
-bool COBD::isValidPID(byte pid)
-{
- if (pid >= 0x7f)
- return true;
- pid--;
- byte i = pid >> 3;
- byte b = 0x80 >> (pid & 0x7);
- return pidmap[i] & b;
-}
-
-void COBD::begin(unsigned long baudrate)
-{
- OBDUART.begin(OBD_SERIAL_BAUDRATE);
- if (baudrate) {
- OBDUART.print("ATBR1 ");
- OBDUART.print(baudrate);
- OBDUART.print('\r');
- OBDUART.end();
- delay(100);
- OBDUART.begin(baudrate);
- }
-}
-
-byte COBD::receive(char* buffer, int timeout)
-{
- unsigned char n = 0;
- for (unsigned long startTime = millis();;) {
- if (available()) {
- char c = read();
- if (n > 2 && c == '>') {
- // prompt char received
- break;
- } else if (!buffer) {
- n++;
- } else if (n < OBD_RECV_BUF_SIZE - 1) {
- if (c == '.' && n > 2 && buffer[n - 1] == '.' && buffer[n - 2] == '.') {
- n = 0;
- timeout = OBD_TIMEOUT_LONG;
- } else {
- buffer[n++] = c;
- }
- }
- } else {
- if (millis() - startTime > timeout) {
- // timeout
- return 0;
- }
- dataIdleLoop();
- }
- }
- if (buffer) buffer[n] = 0;
- return n;
-}
-
-void COBD::recover()
-{
- write('\r');
- delay(100);
- while (available()) read();
-}
-
-bool COBD::init(byte protocol)
-{
- const char *initcmd[] = {"ATZ\r","ATE0\r","ATL1\r"};
- char buffer[OBD_RECV_BUF_SIZE];
-
- m_state = OBD_CONNECTING;
- recover();
-
- for (unsigned char i = 0; i < sizeof(initcmd) / sizeof(initcmd[0]); i++) {
-#ifdef DEBUG
- debugOutput(initcmd[i]);
-#endif
- write(initcmd[i]);
- if (receive(buffer) == 0) {
- return false;
- }
- delay(50);
- }
- if (protocol) {
- write("ATSP");
- write('0' + protocol);
- write('\r');
- receive(buffer);
- delay(50);
- }
- while (available()) read();
-
- // 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);
- }
- delay(100);
- }
- while (available()) read();
-
- m_state = OBD_CONNECTED;
- errors = 0;
- return true;
-}
-
-void COBD::uninit()
-{
- m_state = OBD_DISCONNECTED;
-}
-
-#ifdef DEBUG
-void COBD::debugOutput(const char *s)
-{
- DEBUG.print('[');
- DEBUG.print(millis());
- DEBUG.print(']');
- DEBUG.print(s);
-}
-#endif
-
-/*************************************************************************
-* OBD-II I2C Adapter
-*************************************************************************/
-
-void COBDI2C::begin(byte addr)
-{
- m_addr = addr;
- Wire.begin();
- memset(obdPid, 0, sizeof(obdPid));
- memset(obdInfo, 0, sizeof(obdInfo));
-}
-
-bool COBDI2C::init(byte protocol)
-{
- 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)
-{
- sendQuery(pid);
- dataIdleLoop();
- return getResult(pid, result);
-}
-
-void COBDI2C::write(const char* s)
-{
- 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;
-}
-
-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);
-
- 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;
- }
-
- offset += MAX_PAYLOAD_SIZE;
- if (!hasEnd) {
- continue;
- }
-
- return offset;
- } while(millis() - start < OBD_TIMEOUT_LONG);
- return 0;
-}
-
-bool COBDI2C::gpsQuery(GPS_DATA* gpsdata)
-{
- 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::loadData()
-{
- sendCommand(CMD_LOAD_OBD_DATA);
- dataIdleLoop();
- Wire.requestFrom((byte)m_addr, (byte)MAX_PAYLOAD_SIZE, (byte)0);
- Wire.readBytes((char*)obdInfo, MAX_PAYLOAD_SIZE);
-}
-
-uint16_t COBDI2C::getData(byte pid, int& result)
-{
- 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;
-}
+/************************************************************************* +* Arduino Library for OBD-II UART/I2C Adapter +* Distributed under GPL v2.0 +* Visit http://freematics.com for more information +* (C)2012-2014 Stanley Huang <stanleyhuangyc@gmail.com> +*************************************************************************/ + +#include <Arduino.h> +#include "OBD.h" + +//#define DEBUG Serial +//#define REDIRECT Serial + +uint16_t hex2uint16(const char *p) +{ + char c = *p; + uint16_t i = 0; + for (char n = 0; c && n < 4; c = *(++p)) { + if (c >= 'A' && c <= 'F') { + c -= 7; + } else if (c>='a' && c<='f') { + c -= 39; + } else if (c == ' ') { + continue; + } else if (c < '0' || c > '9') { + break; + } + i = (i << 4) | (c & 0xF); + n++; + } + return i; +} + +byte hex2uint8(const char *p) +{ + byte c1 = *p; + byte c2 = *(p + 1); + if (c1 >= 'A' && c1 <= 'F') + c1 -= 7; + 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 < '0' || c2 > '9') + return 0; + + return c1 << 4 | (c2 & 0xf); +} + +/************************************************************************* +* OBD-II UART Adapter +*************************************************************************/ +#include <Wire.h> + +void COBD::sendQuery(byte pid) +{ + char cmd[8]; + sprintf(cmd, "%02X%02X\r", dataMode, pid); +#ifdef DEBUG + debugOutput(cmd); +#endif + write(cmd); +} + +bool COBD::read(byte pid, int& result) +{ + // send a query command + sendQuery(pid); + // receive and parse the response + return getResult(pid, result); +} + +bool COBD::available() +{ + return OBDUART.available(); +} + +char COBD::read() +{ + char c = OBDUART.read(); +#ifdef REDIRECT + REDIRECT.write(c); +#endif + return c; +} + +void COBD::write(const char* s) +{ + OBDUART.write(s); +} + +void COBD::write(char c) +{ + OBDUART.write(c); +} + +int COBD::normalizeData(byte pid, char* data) +{ + int result; + switch (pid) { + case PID_RPM: + result = getLargeValue(data) >> 2; + break; + case PID_FUEL_PRESSURE: + result = getSmallValue(data) * 3; + break; + case PID_COOLANT_TEMP: + case PID_INTAKE_TEMP: + case PID_AMBIENT_TEMP: + 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 = getPercentageValue(data); + break; + case PID_MAF_FLOW: + result = getLargeValue(data) / 100; + break; + case PID_TIMING_ADVANCE: + result = (int)(getSmallValue(data) / 2) - 64; + break; + case PID_DISTANCE: // km + case PID_RUNTIME: // second + case PID_FUEL_RAIL_PRESSURE: // kPa + case PID_ENGINE_REF_TORQUE: // Nm + result = getLargeValue(data); + break; + case PID_CONTROL_MODULE_VOLTAGE: // V + result = getLargeValue(data) / 1000; + break; + case PID_ENGINE_FUEL_RATE: // L/h + result = getLargeValue(data) / 20; + break; + case PID_ENGINE_TORQUE_PERCENTAGE: // % + result = (int)getSmallValue(data) - 125; + break; + default: + result = getSmallValue(data); + } + return result; +} + +char* COBD::getResponse(byte& pid, char* buffer) +{ + while (receive(buffer, OBD_TIMEOUT_SHORT) > 0) { + 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; + } + } + } + return 0; +} + +bool COBD::getResult(byte& pid, int& result) +{ + char buffer[OBD_RECV_BUF_SIZE]; + char* data = getResponse(pid, buffer); + if (!data) { + recover(); + errors++; + return false; + } + result = normalizeData(pid, data); + return true; +} + +void COBD::setProtocol(byte h) +{ + if (h == -1) { + write("ATSP00\r"); + } else { + char cmd[8]; + sprintf(cmd, "ATSP%d\r", h); + write(cmd); + } +} + +void COBD::sleep() +{ + write("ATLP\r"); +} + +void COBD::wakeup() +{ + write('\r'); +} + +bool COBD::isValidPID(byte pid) +{ + if (pid >= 0x7f) + return true; + pid--; + byte i = pid >> 3; + byte b = 0x80 >> (pid & 0x7); + return pidmap[i] & b; +} + +void COBD::begin(unsigned long baudrate) +{ + OBDUART.begin(OBD_SERIAL_BAUDRATE); + if (baudrate) { + OBDUART.print("ATBR1 "); + OBDUART.print(baudrate); + OBDUART.print('\r'); + OBDUART.end(); + delay(100); + OBDUART.begin(baudrate); + } +} + +byte COBD::receive(char* buffer, int timeout) +{ + unsigned char n = 0; + for (unsigned long startTime = millis();;) { + if (available()) { + char c = read(); + if (n > 2 && c == '>') { + // prompt char received + break; + } else if (!buffer) { + n++; + } else if (n < OBD_RECV_BUF_SIZE - 1) { + if (c == '.' && n > 2 && buffer[n - 1] == '.' && buffer[n - 2] == '.') { + n = 0; + timeout = OBD_TIMEOUT_LONG; + } else { + buffer[n++] = c; + } + } + } else { + if (millis() - startTime > timeout) { + // timeout + return 0; + } + dataIdleLoop(); + } + } + if (buffer) buffer[n] = 0; + return n; +} + +void COBD::recover() +{ + write('\r'); + delay(100); + while (available()) read(); +} + +bool COBD::init(byte protocol) +{ + const char *initcmd[] = {"ATZ\r","ATE0\r","ATL1\r"}; + char buffer[OBD_RECV_BUF_SIZE]; + + m_state = OBD_CONNECTING; + recover(); + + for (unsigned char i = 0; i < sizeof(initcmd) / sizeof(initcmd[0]); i++) { +#ifdef DEBUG + debugOutput(initcmd[i]); +#endif + write(initcmd[i]); + if (receive(buffer) == 0) { + return false; + } + delay(50); + } + if (protocol) { + write("ATSP"); + write('0' + protocol); + write('\r'); + receive(buffer); + delay(50); + } + while (available()) read(); + + // 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); + } + delay(100); + } + while (available()) read(); + + m_state = OBD_CONNECTED; + errors = 0; + return true; +} + +#ifdef DEBUG +void COBD::debugOutput(const char *s) +{ + DEBUG.print('['); + DEBUG.print(millis()); + DEBUG.print(']'); + DEBUG.print(s); +} +#endif + +/************************************************************************* +* OBD-II I2C Adapter +*************************************************************************/ + +void COBDI2C::begin(byte addr) +{ + m_addr = addr; + Wire.begin(); + memset(obdPid, 0, sizeof(obdPid)); + memset(obdInfo, 0, sizeof(obdInfo)); +} + +bool COBDI2C::init(byte protocol) +{ + 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)); + int value; + if (read(PID_RPM, value)) { + m_state = OBD_CONNECTED; + return true; + } + } + m_state = OBD_DISCONNECTED; + return false; +} + +bool COBDI2C::read(byte pid, int& result) +{ + sendQuery(pid); + dataIdleLoop(); + return getResult(pid, result); +} + +void COBDI2C::write(const char* s) +{ + 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; +} + +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); + + 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; + } + + offset += MAX_PAYLOAD_SIZE; + if (!hasEnd) { + continue; + } + + return offset; + } while(millis() - start < OBD_TIMEOUT_LONG); + return 0; +} + +bool COBDI2C::gpsQuery(GPS_DATA* gpsdata) +{ + 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::loadData() +{ + sendCommand(CMD_LOAD_OBD_DATA); + dataIdleLoop(); + Wire.requestFrom((byte)m_addr, (byte)MAX_PAYLOAD_SIZE, (byte)0); + Wire.readBytes((char*)obdInfo, MAX_PAYLOAD_SIZE); +} + +uint16_t COBDI2C::getData(byte pid, int& result) +{ + 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; +} |