From 8ce5b2fdea4f8321f3bdee8e43c4ac21b2826bbb Mon Sep 17 00:00:00 2001
From: Stanley Huang <stanleyhuangyc@gmail.com>
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 <stanleyhuangyc@gmail.com>
+* Written by Stanley Huang <support@freematics.com.au>
*************************************************************************/
#include <SoftwareSerial.h>
@@ -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