From 5dcd27f1b355bc05ed9213228dada5a412d7149d Mon Sep 17 00:00:00 2001 From: Will Eccles Date: Tue, 8 Oct 2019 14:11:30 -0400 Subject: [PATCH] Added turbo PIDs and hex2uint32 --- libraries/OBD/OBD.cpp | 41 ++++++++++++++++++++++----------- libraries/OBD/OBD.h | 9 ++++++++ libraries/OBD2UART/OBD2UART.cpp | 30 +++++++++++++++++------- libraries/OBD2UART/OBD2UART.h | 8 +++++++ 4 files changed, 67 insertions(+), 21 deletions(-) diff --git a/libraries/OBD/OBD.cpp b/libraries/OBD/OBD.cpp index 92b9bdf..c8e0233 100644 --- a/libraries/OBD/OBD.cpp +++ b/libraries/OBD/OBD.cpp @@ -31,6 +31,12 @@ uint16_t hex2uint16(const char *p) return i; } +uint32_t hex2uint32(const char* p) { + uint32_t i = 0; + i = ((uint32_t)hex2uint16(p) << 16) | hex2uint16(p+4); + return i; +} + byte hex2uint8(const char *p) { byte c1 = *p; @@ -83,7 +89,7 @@ bool COBD::readPID(byte pid, int& result) byte COBD::readPID(const byte pid[], byte count, int result[]) { - byte results = 0; + byte results = 0; for (byte n = 0; n < count; n++) { if (readPID(pid[n], result[n])) { results++; @@ -96,9 +102,9 @@ byte COBD::readDTC(uint16_t codes[], byte maxCodes) { /* Response example: - 0: 43 04 01 08 01 09 + 0: 43 04 01 08 01 09 1: 01 11 01 15 00 00 00 - */ + */ byte codesRead = 0; for (byte n = 0; n < 6; n++) { char buffer[128]; @@ -113,7 +119,7 @@ byte COBD::readDTC(uint16_t codes[], byte maxCodes) if (*p == '\r') { p = strchr(p, ':'); if (!p) break; - p += 2; + p += 2; } uint16_t code = hex2uint16(p); if (code == 0) break; @@ -221,6 +227,15 @@ int COBD::normalizeData(byte pid, char* data) case PID_AIR_FUEL_EQUIV_RATIO: // 0~200 result = (long)getLargeValue(data) * 200 / 65536; break; + case PID_TURBO_A_TEMP: + case PID_TURBO_B_TEMP: + // these are both 7 byte values, skip the 3 most significant + result = hex2uint32(data+3); + break; + case PID_TURBO_RPM: + // this is a 5 byte value, skip the MSB + result = hex2uint32(data+1); + break; default: result = getSmallValue(data); } @@ -342,9 +357,9 @@ byte COBD::begin() byte version = 0; for (byte n = 0; n < sizeof(baudrates) / sizeof(baudrates[0]) && version == 0; n++) { OBDUART.begin(baudrates[n]); - version = getVersion(); + version = getVersion(); } - return version; + return version; } byte COBD::getVersion() @@ -528,7 +543,7 @@ bool COBD::memsRead(int16_t* acc, int16_t* gyr, int16_t* mag, int16_t* temp) } if (!success) return false; } - return true; + return true; } #ifdef DEBUG @@ -589,7 +604,7 @@ byte COBDI2C::receive(char* buffer, byte bufsize, int timeout) if (offset == 0 && c < 0xa) { // data not ready dataIdleLoop(); - continue; + continue; } if (buffer) buffer[offset++] = c; for (byte i = 1; i < MAX_PAYLOAD_SIZE && Wire.available(); i++) { @@ -714,14 +729,14 @@ bool COBDI2C::memsRead(int* acc, int* gyr, int* mag, int* temp) // 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; + *temp = ((int)(((uint16_t)accel_t_gyro.t_h << 8) | accel_t_gyro.t_l) + 512) / 34 + 350; } if (acc) { @@ -735,14 +750,14 @@ bool COBDI2C::memsRead(int* acc, int* gyr, int* mag, int* temp) 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; } diff --git a/libraries/OBD/OBD.h b/libraries/OBD/OBD.h index fa80537..a60d25e 100644 --- a/libraries/OBD/OBD.h +++ b/libraries/OBD/OBD.h @@ -5,6 +5,9 @@ * (C)2012-2016 Stanley Huang *************************************************************************/ +#ifndef OBD_H +#define OBD_H + #include #define OBD_MODEL_UART 0 @@ -74,6 +77,9 @@ #define PID_ENGINE_TORQUE_DEMANDED 0x61 #define PID_ENGINE_TORQUE_PERCENTAGE 0x62 #define PID_ENGINE_REF_TORQUE 0x63 +#define PID_TURBO_RPM 0x74 +#define PID_TURBO_A_TEMP 0x75 +#define PID_TURBO_B_TEMP 0x76 // non-OBD/custom PIDs (no mode number) #define PID_GPS_LATITUDE 0xA @@ -114,6 +120,7 @@ typedef enum { uint16_t hex2uint16(const char *p); uint8_t hex2uint8(const char *p); +uint32_t hex2uint32(const char* p); class COBD { @@ -241,3 +248,5 @@ class COBDI2C : public COBD { bool MPU6050_write_reg(int reg, uint8_t data); void MPU6050_store(int* pData, uint8_t data_l, uint8_t data_h); }; + +#endif // OBD_H diff --git a/libraries/OBD2UART/OBD2UART.cpp b/libraries/OBD2UART/OBD2UART.cpp index 89ca2b1..db56e71 100644 --- a/libraries/OBD2UART/OBD2UART.cpp +++ b/libraries/OBD2UART/OBD2UART.cpp @@ -29,6 +29,12 @@ uint16_t hex2uint16(const char *p) return i; } +uint32_t hex2uint32(const char* p) { + uint32_t i = 0; + i = ((uint32_t)hex2uint16(p) << 16) | hex2uint16(p+4); + return i; +} + byte hex2uint8(const char *p) { byte c1 = *p; @@ -74,7 +80,7 @@ bool COBD::readPID(byte pid, int& result) byte COBD::readPID(const byte pid[], byte count, int result[]) { - byte results = 0; + byte results = 0; for (byte n = 0; n < count; n++) { if (readPID(pid[n], result[n])) { results++; @@ -87,9 +93,9 @@ byte COBD::readDTC(uint16_t codes[], byte maxCodes) { /* Response example: - 0: 43 04 01 08 01 09 + 0: 43 04 01 08 01 09 1: 01 11 01 15 00 00 00 - */ + */ byte codesRead = 0; for (byte n = 0; n < 6; n++) { char buffer[128]; @@ -104,7 +110,7 @@ byte COBD::readDTC(uint16_t codes[], byte maxCodes) if (*p == '\r') { p = strchr(p, ':'); if (!p) break; - p += 2; + p += 2; } uint16_t code = hex2uint16(p); if (code == 0) break; @@ -212,6 +218,15 @@ int COBD::normalizeData(byte pid, char* data) case PID_AIR_FUEL_EQUIV_RATIO: // 0~200 result = (long)getLargeValue(data) * 200 / 65536; break; + case PID_TURBO_A_TEMP: + case PID_TURBO_B_TEMP: + // these are both 7 byte values, skip the 3 most significant + result = hex2uint32(data+3); + break; + case PID_TURBO_RPM: + // this is a 5 byte value, skip the MSB + result = hex2uint32(data+1); + break; default: result = getSmallValue(data); } @@ -339,9 +354,9 @@ byte COBD::begin() #endif version = getVersion(); if (version != 0) break; - OBDUART.end(); + OBDUART.end(); } - return version; + return version; } byte COBD::getVersion() @@ -610,7 +625,7 @@ bool COBD::memsRead(int16_t* acc, int16_t* gyr, int16_t* mag, int16_t* temp) } if (!success) return false; } - return true; + return true; } bool COBD::memsOrientation(float& yaw, float& pitch, float& roll) @@ -639,4 +654,3 @@ void COBD::debugOutput(const char *s) DEBUG.print(s); } #endif - diff --git a/libraries/OBD2UART/OBD2UART.h b/libraries/OBD2UART/OBD2UART.h index 38688fa..774283f 100644 --- a/libraries/OBD2UART/OBD2UART.h +++ b/libraries/OBD2UART/OBD2UART.h @@ -5,6 +5,9 @@ * (C)2012-2019 Stanley Huang *************************************************************************/ +#ifndef OBD2UART_H +#define OBD2UART_H + #include #define OBD_TIMEOUT_SHORT 1000 /* ms */ @@ -74,6 +77,9 @@ extern HardwareSerial Serial1; #define PID_ENGINE_TORQUE_DEMANDED 0x61 #define PID_ENGINE_TORQUE_PERCENTAGE 0x62 #define PID_ENGINE_REF_TORQUE 0x63 +#define PID_TURBO_RPM 0x74 +#define PID_TURBO_A_TEMP 0x75 +#define PID_TURBO_B_TEMP 0x76 // non-OBD/custom PIDs (no mode number) #define PID_ACC 0x20 @@ -103,6 +109,7 @@ typedef enum { uint16_t hex2uint16(const char *p); uint8_t hex2uint8(const char *p); +uint32_t hex2uint32(const char* p); class COBD { @@ -175,3 +182,4 @@ class COBD bool m_fusion = false; }; +#endif // OBD2UART_H