From 8ce5b2fdea4f8321f3bdee8e43c4ac21b2826bbb Mon Sep 17 00:00:00 2001 From: Stanley Huang Date: Mon, 30 Jan 2017 23:42:15 +1100 Subject: Fixed manual setting protocol issue --- libraries/OBD/OBD.cpp | 17 ++-- libraries/OBD2UART/OBD2UART.cpp | 100 +++++++++++---------- libraries/OBD2UART/OBD2UART.h | 12 +-- .../examples/obd_uart_test/obd_uart_test.ino | 56 +++++++----- 4 files changed, 101 insertions(+), 84 deletions(-) (limited to 'libraries') diff --git a/libraries/OBD/OBD.cpp b/libraries/OBD/OBD.cpp index 88ed733..5dc22a2 100644 --- a/libraries/OBD/OBD.cpp +++ b/libraries/OBD/OBD.cpp @@ -388,16 +388,19 @@ void COBD::recover() bool COBD::init(OBD_PROTOCOLS protocol) { - const char PROGMEM *initcmd[] = {PSTR("ATZ\r"),PSTR("ATE0\r"),PSTR("ATL1\r"),PSTR("ATSP%02u\r")}; + const char *initcmd[] = {"ATZ\r", "ATE0\r", "ATL1\r"}; char buffer[64]; for (unsigned char i = 0; i < sizeof(initcmd) / sizeof(initcmd[0]); i++) { - sprintf_P(buffer, initcmd[i], protocol); -#ifdef DEBUG - debugOutput(buffer); -#endif - write(buffer); - if (receive(buffer, sizeof(buffer), OBD_TIMEOUT_LONG) == 0 || (i > 0 && !strstr(buffer, "OK"))) { + write(initcmd[i]); + if (receive(buffer, sizeof(buffer), OBD_TIMEOUT_LONG) == 0) { + m_state = OBD_DISCONNECTED; + return false; + } + } + if (protocol != PROTO_AUTO) { + sprintf_P(buffer, PSTR("ATSP%u\r"), protocol); + if (receive(buffer, sizeof(buffer), OBD_TIMEOUT_LONG) == 0 && !strstr(buffer, "OK")) { m_state = OBD_DISCONNECTED; return false; } diff --git a/libraries/OBD2UART/OBD2UART.cpp b/libraries/OBD2UART/OBD2UART.cpp index 7f6fb39..d9fbdec 100644 --- a/libraries/OBD2UART/OBD2UART.cpp +++ b/libraries/OBD2UART/OBD2UART.cpp @@ -333,11 +333,14 @@ void COBD::begin() char buffer[32]; version = 0; + for (byte n = 0; n < 3; n++) { if (sendCommand("ATI\r", buffer, sizeof(buffer), 200)) { char *p = strstr(buffer, "OBDUART"); if (p) { p += 9; version = (*p - '0') * 10 + (*(p + 2) - '0'); + break; + } } } } @@ -383,16 +386,19 @@ void COBD::recover() bool COBD::init(OBD_PROTOCOLS protocol) { - const char PROGMEM *initcmd[] = {PSTR("ATZ\r"),PSTR("ATE0\r"),PSTR("ATL1\r"),PSTR("ATSP%02u\r")}; + const char *initcmd[] = {"ATZ\r", "ATE0\r", "ATL1\r"}; char buffer[64]; for (unsigned char i = 0; i < sizeof(initcmd) / sizeof(initcmd[0]); i++) { - sprintf_P(buffer, initcmd[i], protocol); -#ifdef DEBUG - debugOutput(buffer); -#endif - write(buffer); - if (receive(buffer, sizeof(buffer), OBD_TIMEOUT_LONG) == 0 || (i > 0 && !strstr(buffer, "OK"))) { + write(initcmd[i]); + if (receive(buffer, sizeof(buffer), OBD_TIMEOUT_LONG) == 0) { + m_state = OBD_DISCONNECTED; + return false; + } + } + if (protocol != PROTO_AUTO) { + sprintf_P(buffer, PSTR("ATSP%u\r"), protocol); + if (receive(buffer, sizeof(buffer), OBD_TIMEOUT_LONG) == 0 && !strstr(buffer, "OK")) { m_state = OBD_DISCONNECTED; return false; } @@ -440,48 +446,50 @@ bool COBD::setBaudRate(unsigned long baudrate) return true; } -float COBD::getTemperature() +bool COBD::memsRead(int* acc, int* gyr = 0, int* mag = 0, int* temp = 0) { - char buf[32]; - if (sendCommand("ATTEMP\r", buf, sizeof(buf)) > 0) { - char* p = getResultValue(buf); - if (p) return (float)(atoi(p) + 12412) / 340; + char buf[64]; + bool success; + if (acc) { + success = false; + if (sendCommand("ATACL\r", buf, sizeof(buf)) > 0) do { + char* p = getResultValue(buf); + if (!p) break; + acc[0] = atoi(p++); + if (!(p = strchr(p, ','))) break; + acc[1] = atoi(++p); + if (!(p = strchr(p, ','))) break; + acc[2] = atoi(++p); + success = true; + } while (0); + if (!success) return false; } - else { - return -1000; + if (gyr) { + success = false; + if (sendCommand("ATGYRO\r", buf, sizeof(buf)) > 0) do { + char* p = getResultValue(buf); + if (!p) break; + gyr[0] = atoi(p++); + if (!(p = strchr(p, ','))) break; + gyr[1] = atoi(++p); + if (!(p = strchr(p, ','))) break; + gyr[2] = atoi(++p); + success = true; + } while (0); + if (!success) return false; } -} - -bool COBD::readAccel(int& x, int& y, int& z) -{ - char buf[32]; - if (sendCommand("ATACL\r", buf, sizeof(buf)) > 0) do { - char* p = getResultValue(buf); - if (!p) break; - x = atoi(p++); - if (!(p = strchr(p, ','))) break; - y = atoi(++p); - if (!(p = strchr(p, ','))) break; - z = atoi(++p); - return true; - } while (0); - return false; -} - -bool COBD::readGyro(int& x, int& y, int& z) -{ - char buf[32]; - if (sendCommand("ATGYRO\r", buf, sizeof(buf)) > 0) do { - char* p = getResultValue(buf); - if (!p) break; - x = atoi(p++); - if (!(p = strchr(p, ','))) break; - y = atoi(++p); - if (!(p = strchr(p, ','))) break; - z = atoi(++p); - return true; - } while (0); - return false; + if (temp) { + success = false; + if (sendCommand("ATTEMP\r", buf, sizeof(buf)) > 0) { + char* p = getResultValue(buf); + if (p) { + *temp = (atoi(p) + 12412) / 34; + success = true; + } + } + if (!success) return false; + } + return true; } #ifdef DEBUG diff --git a/libraries/OBD2UART/OBD2UART.h b/libraries/OBD2UART/OBD2UART.h index 149e635..2103cc5 100644 --- a/libraries/OBD2UART/OBD2UART.h +++ b/libraries/OBD2UART/OBD2UART.h @@ -125,20 +125,16 @@ public: virtual float getVoltage(); // get VIN as a string, buffer length should be >= OBD_RECV_BUF_SIZE virtual bool getVIN(char* buffer, byte bufsize); - // get device temperature (in celsius degree) - virtual float getTemperature(); - // get accelerometer data - virtual bool readAccel(int& x, int& y, int& z); - // get gyroscope data - virtual bool readGyro(int& x, int& y, int& z); + // initialize MEMS sensor + virtual bool memsInit() { return version > 10; } + // 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 virtual void sendQuery(byte pid); // retrive and parse the response of specifie PID virtual bool getResult(byte& pid, int& result); // determine if the PID is supported virtual bool isValidPID(byte pid); - // init GPS module - // parse GPS data // set current PID mode byte dataMode; // occurrence of errors diff --git a/libraries/OBD2UART/examples/obd_uart_test/obd_uart_test.ino b/libraries/OBD2UART/examples/obd_uart_test/obd_uart_test.ino index 2d35485..4307648 100644 --- a/libraries/OBD2UART/examples/obd_uart_test/obd_uart_test.ino +++ b/libraries/OBD2UART/examples/obd_uart_test/obd_uart_test.ino @@ -1,9 +1,9 @@ /************************************************************************* * Testing sketch for Freematics OBD-II UART Adapter -* Reads and prints several OBD-II PIDs value +* Reads and prints several OBD-II PIDs value and MEMS sensor data * Distributed under GPL v2.0 * Visit http://freematics.com for more information -* Written by Stanley Huang +* Written by Stanley Huang *************************************************************************/ #include @@ -63,7 +63,7 @@ void readPIDSingle() void readPIDMultiple() { - static const byte pids[] = {PID_SPEED, PID_ENGINE_LOAD, PID_THROTTLE, PID_COOLANT_TEMP, PID_INTAKE_TEMP}; + const byte pids[] = {PID_ENGINE_LOAD, PID_COOLANT_TEMP, PID_RPM, PID_SPEED, PID_TIMING_ADVANCE, PID_INTAKE_TEMP, PID_THROTTLE, PID_FUEL_LEVEL}; int values[sizeof(pids)]; if (obd.readPID(pids, sizeof(pids), values) == sizeof(pids)) { mySerial.print('['); @@ -91,29 +91,33 @@ void readBatteryVoltage() void readMEMS() { - int x, y, z; + int acc[3]; + int gyro[3]; + int temp; + + if (!obd.memsRead(acc, gyro, 0, &temp)) return; + 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(); + + mySerial.print("ACC:"); + mySerial.print(acc[0]); + mySerial.print('/'); + mySerial.print(acc[1]); + mySerial.print('/'); + mySerial.print(acc[2]); + + mySerial.print(" GYRO:"); + mySerial.print(gyro[0]); + mySerial.print('/'); + mySerial.print(gyro[1]); + mySerial.print('/'); + mySerial.print(gyro[2]); + + mySerial.print(" TEMP:"); + mySerial.print((float)temp / 10, 1); + mySerial.println("C"); } void setup() @@ -131,6 +135,12 @@ void setup() // send some commands for testing and show response for debugging purpose //testOut(); + Serial.print("MEMS:"); + if (obd.memsInit()) { + Serial.println("OK"); + } else { + Serial.println("NO"); + } // initialize OBD-II adapter do { mySerial.println("Init..."); -- cgit v1.2.3