Integrated code for MPU6050

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...");