summaryrefslogtreecommitdiff
path: root/libraries/OBD
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/OBD')
-rw-r--r--libraries/OBD/OBD.cpp249
-rw-r--r--libraries/OBD/OBD.h23
-rw-r--r--libraries/OBD/examples/obd_i2c_test/obd_i2c_test.ino44
3 files changed, 239 insertions, 77 deletions
diff --git a/libraries/OBD/OBD.cpp b/libraries/OBD/OBD.cpp
index 0219f26..88ed733 100644
--- a/libraries/OBD/OBD.cpp
+++ b/libraries/OBD/OBD.cpp
@@ -100,7 +100,7 @@ byte COBD::readPID(const byte pid[], byte count, int result[])
return results;
}
-byte COBD::readDTC(uint16_t codes[], byte count)
+byte COBD::readDTC(uint16_t codes[], byte maxCodes)
{
/*
Response example:
@@ -110,7 +110,7 @@ byte COBD::readDTC(uint16_t codes[], byte count)
byte codesRead = 0;
for (byte n = 0; n < 6; n++) {
char buffer[128];
- sprintf(buffer, "03%02X\r", n);
+ sprintf_P(buffer, n == 0 ? PSTR("03\r") : PSTR("03%02X\r"), n);
write(buffer);
Serial.println(buffer);
if (receive(buffer, sizeof(buffer)) > 0) {
@@ -118,7 +118,7 @@ byte COBD::readDTC(uint16_t codes[], byte count)
if (!strstr(buffer, "NO DATA")) {
char *p = strstr(buffer, "43");
if (p) {
- while (codesRead < count && *p) {
+ while (codesRead < maxCodes && *p) {
p += 6;
if (*p == '\r') {
p = strchr(p, ':');
@@ -335,11 +335,14 @@ void COBD::begin()
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');
+ 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;
+ }
}
}
}
@@ -442,49 +445,50 @@ bool COBD::setBaudRate(unsigned long baudrate)
return true;
}
-
-float COBD::getTemperature()
-{
- char buf[32];
- if (sendCommand("ATTEMP\r", buf, sizeof(buf)) > 0) {
- char* p = getResultValue(buf);
- if (p) return (float)(atoi(p) + 12412) / 340;
+bool COBD::memsRead(int* acc, int* gyr = 0, int* mag = 0, int* temp = 0)
+{
+ 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
@@ -623,3 +627,150 @@ void COBDI2C::loadQueryData(PID_INFO obdInfo[])
Wire.requestFrom((byte)I2C_ADDR, (byte)MAX_PAYLOAD_SIZE, (byte)0);
Wire.readBytes((char*)obdInfo, sizeof(obdInfo[0]) * MAX_PIDS);
}
+
+#define MPU6050_I2C_ADDRESS 0x68
+#define MPU6050_ACCEL_XOUT_H 0x3B // R
+#define MPU6050_ACCEL_XOUT_L 0x3C // R
+#define MPU6050_ACCEL_YOUT_H 0x3D // R
+#define MPU6050_ACCEL_YOUT_L 0x3E // R
+#define MPU6050_ACCEL_ZOUT_H 0x3F // R
+#define MPU6050_ACCEL_ZOUT_L 0x40 // R
+#define MPU6050_TEMP_OUT_H 0x41 // R
+#define MPU6050_TEMP_OUT_L 0x42 // R
+#define MPU6050_GYRO_XOUT_H 0x43 // R
+#define MPU6050_GYRO_XOUT_L 0x44 // R
+#define MPU6050_GYRO_YOUT_H 0x45 // R
+#define MPU6050_GYRO_YOUT_L 0x46 // R
+#define MPU6050_GYRO_ZOUT_H 0x47 // R
+#define MPU6050_GYRO_ZOUT_L 0x48 // R
+#define MPU6050_PWR_MGMT_1 0x6B // R/W
+#define MPU6050_PWR_MGMT_2 0x6C // R/W
+#define MPU6050_WHO_AM_I 0x75 // R
+
+typedef struct
+{
+ uint8_t x_accel_h;
+ uint8_t x_accel_l;
+ uint8_t y_accel_h;
+ uint8_t y_accel_l;
+ uint8_t z_accel_h;
+ uint8_t z_accel_l;
+ uint8_t t_h;
+ uint8_t t_l;
+ uint8_t x_gyro_h;
+ uint8_t x_gyro_l;
+ uint8_t y_gyro_h;
+ uint8_t y_gyro_l;
+ uint8_t z_gyro_h;
+ uint8_t z_gyro_l;
+} MPU6050_READOUT_DATA;
+
+bool COBDI2C::memsInit()
+{
+ // default at power-up:
+ // Gyro at 250 degrees second
+ // Acceleration at 2g
+ // Clock source at internal 8MHz
+ // The device is in sleep mode.
+ //
+ uint8_t c;
+ bool success;
+ success = MPU6050_read (MPU6050_WHO_AM_I, &c, 1);
+ if (!success) return false;
+
+ // According to the datasheet, the 'sleep' bit
+ // should read a '1'. But I read a '0'.
+ // That bit has to be cleared, since the sensor
+ // is in sleep mode at power-up. Even if the
+ // bit reads '0'.
+ success = MPU6050_read (MPU6050_PWR_MGMT_2, &c, 1);
+ if (!success) return false;
+
+ // Clear the 'sleep' bit to start the sensor.
+ MPU6050_write_reg (MPU6050_PWR_MGMT_1, 0);
+ return true;
+}
+
+bool COBDI2C::memsRead(int* acc, int* gyr, int* mag, int* temp)
+{
+ bool success;
+
+ // Read the raw values.
+ // Read 14 bytes at once,
+ // containing acceleration, temperature and gyro.
+ // With the default settings of the MPU-6050,
+ // there is no filter enabled, and the values
+ // are not very stable.
+
+ MPU6050_READOUT_DATA accel_t_gyro;
+ success = MPU6050_read (MPU6050_ACCEL_XOUT_H, (uint8_t *)&accel_t_gyro, sizeof(MPU6050_READOUT_DATA));
+ if (!success) return false;
+
+ if (temp) {
+ // 340 per degrees Celsius, -512 at 35 degrees.
+ *temp = ((int)(((uint16_t)accel_t_gyro.t_h << 8) | accel_t_gyro.t_l) + 512) / 34 + 350;
+ }
+
+ if (acc) {
+ MPU6050_store(acc, accel_t_gyro.x_accel_l, accel_t_gyro.x_accel_h);
+ MPU6050_store(acc + 1, accel_t_gyro.y_accel_l, accel_t_gyro.y_accel_h);
+ MPU6050_store(acc + 2, accel_t_gyro.z_accel_l, accel_t_gyro.z_accel_h);
+ }
+
+ if (gyr) {
+ MPU6050_store(gyr, accel_t_gyro.x_gyro_l, accel_t_gyro.x_gyro_h);
+ MPU6050_store(gyr + 1, accel_t_gyro.y_gyro_l, accel_t_gyro.y_gyro_h);
+ MPU6050_store(gyr + 2, accel_t_gyro.z_gyro_l, accel_t_gyro.z_gyro_h);
+ }
+
+ if (mag) {
+ // no magnetometer
+ mag[0] = 0;
+ mag[1] = 0;
+ mag[2] = 0;
+ }
+
+ return true;
+}
+
+void COBDI2C::MPU6050_store(int* pData, uint8_t data_l, uint8_t data_h)
+{
+ uint8_t* ptr = (uint8_t*)pData;
+ *ptr = data_l;
+ *(ptr + 1) = data_h;
+}
+
+bool COBDI2C::MPU6050_read(int start, uint8_t* buffer, int size)
+{
+ int i, n;
+
+ Wire.beginTransmission(MPU6050_I2C_ADDRESS);
+ Wire.write(start);
+ Wire.endTransmission(false); // hold the I2C-bus
+
+ // Third parameter is true: relase I2C-bus after data is read.
+ Wire.requestFrom(MPU6050_I2C_ADDRESS, size, true);
+ while(Wire.available() && i<size)
+ {
+ buffer[i++]=Wire.read();
+ }
+ return i == size;
+}
+
+
+bool COBDI2C::MPU6050_write(int start, const uint8_t* pData, int size)
+{
+ int n;
+
+ Wire.beginTransmission(MPU6050_I2C_ADDRESS);
+ Wire.write(start); // write the start address
+ n = Wire.write(pData, size); // write data bytes
+ if (n != size) return false;
+ Wire.endTransmission(true); // release the I2C-bus
+ return true;
+}
+
+bool COBDI2C::MPU6050_write_reg(int reg, uint8_t data)
+{
+ return MPU6050_write(reg, &data, 1);
+}
diff --git a/libraries/OBD/OBD.h b/libraries/OBD/OBD.h
index 2e8404f..711ee8e 100644
--- a/libraries/OBD/OBD.h
+++ b/libraries/OBD/OBD.h
@@ -121,27 +121,23 @@ public:
// send AT command and receive response
virtual byte sendCommand(const char* cmd, char* buf, byte bufsize, int timeout = OBD_TIMEOUT_LONG);
// read diagnostic trouble codes (return number of DTCs read)
- virtual byte readDTC(uint16_t codes[], byte count = 1);
+ virtual byte readDTC(uint16_t codes[], byte maxCodes = 1);
// clear diagnostic trouble code
virtual void clearDTC();
// get battery voltage (works without ECU)
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
- 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
@@ -215,7 +211,16 @@ public:
void setQueryPID(byte pid, byte obdPid[]);
void applyQueryPIDs(byte obdPid[]);
void loadQueryData(PID_INFO obdInfo[]);
+ // initialize MEMS sensor
+ bool memsInit();
+ // read out MEMS sensor data (acc for accelerometer, gyr for gyroscope, temp in 0.1 celcius degree)
+ bool memsRead(int* acc, int* gyr = 0, int* mag = 0, int* temp = 0);
protected:
byte receive(char* buffer, byte bufsize, int timeout = OBD_TIMEOUT_SHORT);
bool sendCommandBlock(byte cmd, uint8_t data = 0, byte* payload = 0, byte payloadBytes = 0);
+private:
+ bool MPU6050_read(int start, uint8_t* buffer, int size);
+ bool MPU6050_write(int start, const uint8_t* pData, int size);
+ bool MPU6050_write_reg(int reg, uint8_t data);
+ void MPU6050_store(int* pData, uint8_t data_l, uint8_t data_h);
};
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 e00f0e7..38da62e 100644
--- a/libraries/OBD/examples/obd_i2c_test/obd_i2c_test.ino
+++ b/libraries/OBD/examples/obd_i2c_test/obd_i2c_test.ino
@@ -1,20 +1,17 @@
/*************************************************************************
-* Sample sketch for Freematics OBD-II I2C Adapter
+* Testing sketch for Freematics OBD-II I2C Adapter
* Reads and prints several OBD-II PIDs value and MEMS sensor data
* Distributed under GPL v2.0
* Visit http://freematics.com for more information
-* (C)2012-2015 Stanley Huang <stanleyhuangyc@gmail.com>
+* Written by Stanley Huang <support@freematics.com.au>
*************************************************************************/
#include <Arduino.h>
#include <Wire.h>
#include <SPI.h>
#include <OBD.h>
-#include <I2Cdev.h>
-#include <MPU9150.h>
COBDI2C obd;
-MPU6050 accelgyro;
void testOut()
{
@@ -48,30 +45,33 @@ void testOut()
void readMEMS()
{
- int16_t ax, ay, az;
- int16_t gx, gy, gz;
+ int acc[3];
+ int gyro[3];
int temp;
- accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
- temp = accelgyro.getTemperature();
+ if (!obd.memsRead(acc, gyro, 0, &temp)) return;
Serial.print('[');
Serial.print(millis());
Serial.print(']');
- Serial.print("ACC=");
- Serial.print(ax);
+ Serial.print("ACC:");
+ Serial.print(acc[0]);
Serial.print('/');
- Serial.print(ay);
+ Serial.print(acc[1]);
Serial.print('/');
- Serial.print(az);
+ Serial.print(acc[2]);
- Serial.print(" GYRO=");
- Serial.print(gx);
+ Serial.print(" GYRO:");
+ Serial.print(gyro[0]);
Serial.print('/');
- Serial.print(gy);
+ Serial.print(gyro[1]);
Serial.print('/');
- Serial.println(gz);
+ Serial.print(gyro[2]);
+
+ Serial.print(" TEMP:");
+ Serial.print((float)temp / 10, 1);
+ Serial.println("C");
}
void readPIDs()
@@ -110,11 +110,17 @@ void setup() {
Serial.begin(115200);
delay(500);
obd.begin();
- accelgyro.initialize();
+ Serial.print("MEMS:");
+ if (obd.memsInit()) {
+ Serial.println("OK");
+ } else {
+ Serial.println("NO");
+ }
+
// send some commands for testing and show response for debugging purpose
//testOut();
-
+
// initialize OBD-II adapter
do {
Serial.println("Init...");
generated by cgit v1.2.3 (git 2.39.1) at 2024-11-01 08:22:06 +0000