From ceb4720c4a89735fb1ef128b66a7e582270043da Mon Sep 17 00:00:00 2001
From: Stanley Huang <stanleyhuangyc@gmail.com>
Date: Sun, 12 Feb 2017 23:01:21 +1100
Subject: Improved compatibility
---
libraries/OBD/OBD.cpp | 104 ++++++++++-----------
libraries/OBD/OBD.h | 16 ++--
.../OBD/examples/obd_i2c_test/obd_i2c_test.ino | 19 ++--
.../OBD/examples/obd_uart_test/obd_uart_test.ino | 82 +++++++++-------
4 files changed, 116 insertions(+), 105 deletions(-)
diff --git a/libraries/OBD/OBD.cpp b/libraries/OBD/OBD.cpp
index 5dc22a2..2dbede8 100644
--- a/libraries/OBD/OBD.cpp
+++ b/libraries/OBD/OBD.cpp
@@ -83,19 +83,11 @@ bool COBD::readPID(byte pid, int& result)
byte COBD::readPID(const byte pid[], byte count, int result[])
{
- // send a multiple query command
- char buffer[128];
- char *p = buffer;
- byte results = 0;
- for (byte n = 0; n < count; n++) {
- p += sprintf(p, "%02X%02X\r", dataMode, pid[n]);
- }
- write(buffer);
- // receive and parse the response
+ byte results = 0;
for (byte n = 0; n < count; n++) {
- byte curpid = pid[n];
- if (getResult(curpid, result[n]))
+ if (readPID(pid[n], result[n])) {
results++;
+ }
}
return results;
}
@@ -112,9 +104,7 @@ byte COBD::readDTC(uint16_t codes[], byte maxCodes)
char buffer[128];
sprintf_P(buffer, n == 0 ? PSTR("03\r") : PSTR("03%02X\r"), n);
write(buffer);
- Serial.println(buffer);
if (receive(buffer, sizeof(buffer)) > 0) {
- Serial.println(buffer);
if (!strstr(buffer, "NO DATA")) {
char *p = strstr(buffer, "43");
if (p) {
@@ -146,6 +136,10 @@ void COBD::clearDTC()
void COBD::write(const char* s)
{
+#ifdef DEBUG
+ DEBUG.print("<<<");
+ DEBUG.println(s);
+#endif
OBDUART.write(s);
}
@@ -325,39 +319,43 @@ bool COBD::isValidPID(byte pid)
return pidmap[i] & b;
}
-void COBD::begin()
+byte COBD::begin()
{
- OBDUART.begin(OBD_SERIAL_BAUDRATE);
-#ifdef DEBUG
- DEBUG.begin(115200);
-#endif
- recover();
+ long baudrates[] = {38400, 115200};
+ byte version = 0;
+ for (byte n = 0; n < sizeof(baudrates) / sizeof(baudrates[0]) && version == 0; n++) {
+ OBDUART.begin(baudrates[n]);
+ version = getVersion();
+ }
+ return version;
+}
- char buffer[32];
- version = 0;
+byte COBD::getVersion()
+{
+ byte version = 0;
for (byte n = 0; n < 3; n++) {
+ char buffer[32];
if (sendCommand("ATI\r", buffer, sizeof(buffer), 200)) {
- char *p = strstr(buffer, "OBDUART");
+ char *p = strchr(buffer, ' ');
if (p) {
- p += 9;
+ p += 2;
version = (*p - '0') * 10 + (*(p + 2) - '0');
break;
}
}
}
+ return version;
}
byte COBD::receive(char* buffer, byte bufsize, int timeout)
{
unsigned char n = 0;
unsigned long startTime = millis();
+ char c = 0;
for (;;) {
if (OBDUART.available()) {
- char c = OBDUART.read();
- if (n > 2 && c == '>') {
- // prompt char received
- break;
- } else if (!buffer) {
+ c = OBDUART.read();
+ if (!buffer) {
n++;
} else if (n < bufsize - 1) {
if (c == '.' && n > 2 && buffer[n - 1] == '.' && buffer[n - 2] == '.') {
@@ -365,10 +363,17 @@ byte COBD::receive(char* buffer, byte bufsize, int timeout)
n = 0;
timeout = OBD_TIMEOUT_LONG;
} else {
+ if (c == '\r' || c == '\n' || c == ' ') {
+ if (n == 0 || buffer[n - 1] == '\r' || buffer[n - 1] == '\n') continue;
+ }
buffer[n++] = c;
}
}
} else {
+ if (c == '>') {
+ // prompt char received
+ break;
+ }
if (millis() - startTime > timeout) {
// timeout
break;
@@ -376,19 +381,24 @@ byte COBD::receive(char* buffer, byte bufsize, int timeout)
dataIdleLoop();
}
}
- if (buffer) buffer[n] = 0;
+ if (buffer) {
+ buffer[n] = 0;
+ }
+#ifdef DEBUG
+ DEBUG.print(">>>");
+ DEBUG.println(buffer);
+#endif
return n;
}
void COBD::recover()
{
- char buf[16];
- sendCommand("AT\r", buf, sizeof(buf));
+ sendCommand("\r", 0, 0);
}
bool COBD::init(OBD_PROTOCOLS protocol)
{
- const char *initcmd[] = {"ATZ\r", "ATE0\r", "ATL1\r"};
+ const char *initcmd[] = {"ATZ\r", "ATE0\r", "ATH0\r"};
char buffer[64];
for (unsigned char i = 0; i < sizeof(initcmd) / sizeof(initcmd[0]); i++) {
@@ -448,6 +458,12 @@ bool COBD::setBaudRate(unsigned long baudrate)
return true;
}
+bool COBD::memsInit()
+{
+ char buf[16];
+ return sendCommand("ATTEMP\r", buf, sizeof(buf)) > 0 && !strchr(buf, '?');
+}
+
bool COBD::memsRead(int* acc, int* gyr = 0, int* mag = 0, int* temp = 0)
{
char buf[64];
@@ -508,23 +524,14 @@ void COBD::debugOutput(const char *s)
* OBD-II I2C Adapter
*************************************************************************/
-void COBDI2C::begin()
+byte COBDI2C::begin()
{
Wire.begin();
#ifdef DEBUG
DEBUG.begin(115200);
#endif
recover();
-
- char buffer[32];
- version = 0;
- if (sendCommand("ATI\r", buffer, sizeof(buffer), 200)) {
- char *p = strstr(buffer, "OBDUART");
- if (p) {
- p += 9;
- version = (*p - '0') * 10 + (*(p + 2) - '0');
- }
- }
+ return getVersion();
}
void COBDI2C::end()
@@ -592,17 +599,6 @@ byte COBDI2C::receive(char* buffer, byte bufsize, int timeout)
return 0;
}
-byte COBDI2C::readPID(const byte pid[], byte count, int result[])
-{
- byte results = 0;
- for (byte n = 0; n < count; n++) {
- if (readPID(pid[n], result[n])) {
- results++;
- }
- }
- return results;
-}
-
void COBDI2C::setQueryPID(byte pid, byte obdPid[])
{
byte n = 0;
diff --git a/libraries/OBD/OBD.h b/libraries/OBD/OBD.h
index 711ee8e..f106e41 100644
--- a/libraries/OBD/OBD.h
+++ b/libraries/OBD/OBD.h
@@ -13,7 +13,6 @@
#define OBD_TIMEOUT_SHORT 1000 /* ms */
#define OBD_TIMEOUT_LONG 5000 /* ms */
#define OBD_TIMEOUT_GPS 200 /* ms */
-#define OBD_SERIAL_BAUDRATE 38400
#ifndef OBDUART
#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168P__)
@@ -102,8 +101,8 @@ class COBD
{
public:
COBD():dataMode(1),errors(0),m_state(OBD_DISCONNECTED) {}
- // begin serial UART
- virtual void begin();
+ // begin serial UART, return the version number on success and 0 on failure
+ virtual byte begin();
// initialize OBD-II connection
virtual bool init(OBD_PROTOCOLS protocol = PROTO_AUTO);
// un-initialize OBD-II connection
@@ -129,7 +128,7 @@ public:
// get VIN as a string, buffer length should be >= OBD_RECV_BUF_SIZE
virtual bool getVIN(char* buffer, byte bufsize);
// initialize MEMS sensor
- virtual bool memsInit() { return version > 10; }
+ virtual bool memsInit();
// read out MEMS data (acc for accelerometer, gyr for gyroscope, temp in 0.1 celcius degree)
virtual bool memsRead(int* acc, int* gyr = 0, int* mag = 0, int* temp = 0);
// send query for specified PID
@@ -138,14 +137,14 @@ public:
virtual bool getResult(byte& pid, int& result);
// determine if the PID is supported
virtual bool isValidPID(byte pid);
+ // get adapter firmware version
+ virtual byte getVersion();
// set current PID mode
byte dataMode;
- // occurrence of errors
+ // number of subsequent errors
byte errors;
// bit map of supported PIDs
byte pidmap[4 * 4];
- // adapter version
- byte version;
protected:
virtual char* getResponse(byte& pid, char* buffer, byte bufsize);
virtual byte receive(char* buffer, byte bufsize, int timeout = OBD_TIMEOUT_SHORT);
@@ -200,10 +199,9 @@ typedef struct {
class COBDI2C : public COBD {
public:
- void begin();
+ byte begin();
void end();
bool readPID(byte pid, int& result);
- byte readPID(const byte pid[], byte count, int result[]);
void write(const char* s);
// API not applicable
bool setBaudRate(unsigned long baudrate) { return false; }
diff --git a/libraries/OBD/examples/obd_i2c_test/obd_i2c_test.ino b/libraries/OBD/examples/obd_i2c_test/obd_i2c_test.ino
index 38da62e..810aacf 100644
--- a/libraries/OBD/examples/obd_i2c_test/obd_i2c_test.ino
+++ b/libraries/OBD/examples/obd_i2c_test/obd_i2c_test.ino
@@ -12,10 +12,11 @@
#include <OBD.h>
COBDI2C obd;
+bool hasMEMS;
void testOut()
{
- static const char cmds[][6] = {"ATZ\r", "ATL1\r", "ATH0\r", "ATRV\r", "0100\r", "010C\r", "0902\r"};
+ static const char cmds[][6] = {"ATZ\r", "ATH0\r", "ATRV\r", "0100\r", "010C\r", "0902\r"};
char buf[128];
for (byte i = 0; i < sizeof(cmds) / sizeof(cmds[0]); i++) {
@@ -111,15 +112,12 @@ void setup() {
delay(500);
obd.begin();
+ hasMEMS = obd.memsInit();
Serial.print("MEMS:");
- if (obd.memsInit()) {
- Serial.println("OK");
- } else {
- Serial.println("NO");
- }
-
+ Serial.println(hasMEMS ? "Yes" : "No");
+
// send some commands for testing and show response for debugging purpose
- //testOut();
+ testOut();
// initialize OBD-II adapter
do {
@@ -151,6 +149,7 @@ void setup() {
void loop() {
readPIDs();
readBatteryVoltage();
- readMEMS();
+ if (hasMEMS) {
+ readMEMS();
+ }
}
-
diff --git a/libraries/OBD/examples/obd_uart_test/obd_uart_test.ino b/libraries/OBD/examples/obd_uart_test/obd_uart_test.ino
index 477ac61..1f58405 100644
--- a/libraries/OBD/examples/obd_uart_test/obd_uart_test.ino
+++ b/libraries/OBD/examples/obd_uart_test/obd_uart_test.ino
@@ -18,10 +18,11 @@ SoftwareSerial mySerial(A2, A3);
//#define mySerial Serial
COBD obd;
+bool hasMEMS;
void testOut()
{
- static const char cmds[][6] = {"ATZ\r", "ATL1\r", "ATH0\r", "ATRV\r", "0100\r", "010C\r", "0902\r"};
+ static const char cmds[][6] = {"ATZ\r", "ATI\r", "ATH0\r", "ATRV\r", "0100\r", "010C\r", "0902\r"};
char buf[128];
for (byte i = 0; i < sizeof(cmds) / sizeof(cmds[0]); i++) {
@@ -41,6 +42,7 @@ void testOut()
mySerial.write('\n');
p++;
}
+ mySerial.println();
} else {
mySerial.println("Timeout");
}
@@ -64,7 +66,7 @@ void readPIDSingle()
void readPIDMultiple()
{
- static const byte pids[] = {PID_SPEED, PID_ENGINE_LOAD, PID_THROTTLE, PID_COOLANT_TEMP, PID_INTAKE_TEMP};
+ static const byte pids[] = {PID_SPEED, PID_ENGINE_LOAD, PID_THROTTLE, PID_COOLANT_TEMP};
int values[sizeof(pids)];
if (obd.readPID(pids, sizeof(pids), values) == sizeof(pids)) {
mySerial.print('[');
@@ -92,29 +94,33 @@ void readBatteryVoltage()
void readMEMS()
{
- int x, y, z;
- mySerial.print('[');
- mySerial.print(millis());
- mySerial.print(']');
- if (obd.readAccel(x, y, z)) {
- mySerial.print("ACC:");
- mySerial.print(x);
- mySerial.print('/');
- mySerial.print(y);
- mySerial.print('/');
- mySerial.print(z);
- mySerial.print(' ');
- }
- if (obd.readGyro(x, y, z)) {
- mySerial.print("GYRO:");
- mySerial.print(x);
- mySerial.print('/');
- mySerial.print(y);
- mySerial.print('/');
- mySerial.print(z);
- mySerial.print(' ');
- }
- mySerial.println();
+ int acc[3];
+ int gyro[3];
+ int temp;
+
+ if (!obd.memsRead(acc, gyro, 0, &temp)) return;
+
+ Serial.print('[');
+ Serial.print(millis());
+ Serial.print(']');
+
+ Serial.print("ACC:");
+ Serial.print(acc[0]);
+ Serial.print('/');
+ Serial.print(acc[1]);
+ Serial.print('/');
+ Serial.print(acc[2]);
+
+ Serial.print(" GYRO:");
+ Serial.print(gyro[0]);
+ Serial.print('/');
+ Serial.print(gyro[1]);
+ Serial.print('/');
+ Serial.print(gyro[2]);
+
+ Serial.print(" TEMP:");
+ Serial.print((float)temp / 10, 1);
+ Serial.println("C");
}
void setup()
@@ -123,15 +129,27 @@ void setup()
while (!mySerial);
// this will begin serial
- obd.begin();
+ byte version = obd.begin();
- mySerial.print("Adapter version: ");
- mySerial.println(obd.version);
+ mySerial.print("Freematics OBD-II Adapter ");
+ if (version > 0) {
+ mySerial.print("Ver. ");
+ mySerial.print(version / 10);
+ mySerial.print('.');
+ mySerial.println(version % 10);
+ } else {
+ mySerial.println("not detected");
+ for (;;);
+ }
delay(1000);
// send some commands for testing and show response for debugging purpose
- //testOut();
-
+ testOut();
+
+ hasMEMS = obd.memsInit();
+ mySerial.print("MEMS:");
+ mySerial.println(hasMEMS ? "Yes" : "No");
+
// initialize OBD-II adapter
do {
mySerial.println("Init...");
@@ -156,7 +174,7 @@ void setup()
}
mySerial.println();
}
- delay(3000);
+ delay(5000);
}
@@ -165,7 +183,7 @@ void loop()
readPIDSingle();
readPIDMultiple();
readBatteryVoltage();
- if (obd.version > 10) {
+ if (hasMEMS) {
readMEMS();
}
}
--
cgit v1.2.3