diff options
| -rw-r--r-- | samples/obdtest/obdtest.ino | 254 |
1 files changed, 159 insertions, 95 deletions
diff --git a/samples/obdtest/obdtest.ino b/samples/obdtest/obdtest.ino index d91bff4..e2102cc 100644 --- a/samples/obdtest/obdtest.ino +++ b/samples/obdtest/obdtest.ino @@ -1,12 +1,25 @@ #include <Arduino.h> #include <Wire.h> #include "MultiLCD.h" +#include "TinyGPS.h" #include "OBD.h" +#include "MPU6050.h" #define INIT_CMD_COUNT 4 #define MAX_CMD_LEN 6 -#define HAVE_ACCEL 0 +// GPS logging can only be enabled when there is additional hardware serial UART +#if defined(__AVR_ATmega2560__) || defined(__AVR_ATmega1280__) +#define GPSUART Serial3 +#elif defined(__AVR_ATmega644P__) +#define GPSUART Serial1 +#endif + +#define GPS_BAUDRATE 4800 /* bps */ + +#ifdef GPSUART +TinyGPS gps; +#endif // GPSUART int MPU6050_read(int start, uint8_t *buffer, int size); @@ -23,11 +36,7 @@ char oldkey=-1; byte index = 0; uint16_t pid = 0x0111; -#if HAVE_ACCEL -#include "MPU6050.h" - -int stateMPU6050; -#endif +bool hasMPU6050 = false; //create object to control an LCD. LCD_OLED lcd; @@ -61,11 +70,11 @@ public: } else if (n < OBD_RECV_BUF_SIZE - 1) { buffer[n++] = c; - if (c == '\r' || c == '\n') - lcd.setCursor(0, -1); - else { + if (c == '\n') + lcd.changeLine(); + else if (c >= ' ') { lcd.write(c); - delay(1); + delay(5); } } } else if (prompted) { @@ -84,7 +93,7 @@ public: } delay(500); } - delay(4500); + delay(1500); char* data; memset(pidmap, 0, sizeof(pidmap)); @@ -111,15 +120,75 @@ public: sprintf(buffer, "%02X ", pidmap[i]); lcd.print(buffer); } - delay(3000); + delay(2000); errors = 0; return true; } + void SendQuery() + { + char buf[17]; + switch (index) { + case 0: + sprintf(buf, "[%04X]", pid); + break; + case 1: + sprintf(buf, "%03X[%01X]", pid >> 4, pid & 0xf); + break; + case 2: + sprintf(buf, "%02X[%01X]%01X", pid >> 8, (pid >> 4) & 0xf, pid & 0xf); + break; + case 3: + sprintf(buf, "%01X[%01X]%02X", pid >> 12, (pid >> 8) & 0xf, pid & 0xff); + break; + case 4: + sprintf(buf, "[%01X]%03X", pid >> 12, pid & 0xfff); + break; + } + + lcd.setCursor(0, 0); + lcd.print(buf); + + dataMode = (byte)(pid >> 8); + Query((byte)pid); + } + void Recover() + { + WriteData('\r'); + } }; COBDTester obd; +#ifdef GPSUART +void ShowGPSData() +{ + // parsed GPS data is ready + char buf[32]; + unsigned long fix_age; + + if (lcd.getLines() > 2) { + unsigned long date, time; + gps.get_datetime(&date, &time, &fix_age); + sprintf(buf, "TIME:%ld", time); + lcd.setCursor(0, 2); + lcd.print(buf); + } + + if (lcd.getLines() > 3) { + long lat, lon; + gps.get_position(&lat, &lon, &fix_age); + // display LAT/LON if screen is big enough + lcd.setCursor(0, 3); + if ((millis() / 1000) & 1) + sprintf(buf, "LAT:%3d.%5ld", (int)(lat / 100000), lat % 100000); + else + sprintf(buf, "LON:%3d.%5ld", (int)(lon / 100000), lon % 100000); + lcd.print(buf); + } +} +#endif + // Convert ADC value to key number char get_key(unsigned int input) { @@ -131,8 +200,7 @@ char get_key(unsigned int input) return -1; } -#if HAVE_ACCEL -void readMPU6050() +void testMPU6050() { int error; float dT; @@ -148,7 +216,7 @@ void readMPU6050() lcd.setCursor(0, 1); error = MPU6050_read (MPU6050_ACCEL_XOUT_H, (uint8_t *) &accel_t_gyro, sizeof(accel_t_gyro)); if (error != 0) { - lcd.print("ACC Error!"); + lcd.print("MPU6050 N/A"); return; } @@ -182,82 +250,51 @@ void readMPU6050() lcd.setCursor(0, 1); lcd.print(buf); } -#endif -void query() -{ - char buf[17]; - - switch (index) { - case 0: - sprintf(buf, "[%04X]", pid); - break; - case 1: - sprintf(buf, "%03X[%01X]", pid >> 4, pid & 0xf); - break; - case 2: - sprintf(buf, "%02X[%01X]%01X", pid >> 8, (pid >> 4) & 0xf, pid & 0xf); - break; - case 3: - sprintf(buf, "%01X[%01X]%02X", pid >> 12, (pid >> 8) & 0xf, pid & 0xff); - break; - case 4: - sprintf(buf, "[%01X]%03X", pid >> 12, pid & 0xfff); - break; - } - - lcd.setCursor(0, 0); - lcd.print(buf); - lcd.setCursor(0, 1); - - obd.dataMode = (byte)(pid >> 8); - obd.Query((byte)pid); - -#if HAVE_ACCEL - if (stateMPU6050 == 0) { - readMPU6050(); - } -#endif -} void setup() { Wire.begin(); lcd.begin(); - lcd.print("OBD TESTER v1.2"); + lcd.print("OBD TESTER 1.2"); OBDUART.begin(38400); - delay(1000); - -#if HAVE_ACCEL - lcd.clear(); - lcd.print("Init MPU6050..."); - lcd.setCursor(0, 1); - - stateMPU6050 = MPU6050_init(); - - char buf[16]; - if (stateMPU6050 != 0) { - sprintf(buf, "Error: %d", stateMPU6050); - lcd.print(buf); - } else { - unsigned long t = millis(); - do { - readMPU6050(); - delay(100); - } while (millis() - t <= 10000); - } - delay(1000); +#ifdef GPSUART + GPSUART.begin(GPS_BAUDRATE); #endif do { lcd.clear(); - lcd.print("Init OBD..."); + lcd.print("Init MPU6050..."); + lcd.setCursor(0, 1); + + hasMPU6050 = MPU6050_init() == 0; + if (hasMPU6050) { + unsigned long t = millis(); + do { + testMPU6050(); + delay(100); + } while (millis() - t <= 10000); + } + delay(1000); -#if HAVE_ACCEL - if (stateMPU6050 == 0) { - readMPU6050(); - } +#ifdef GPSUART + if (GPSUART.available()) { + lcd.clear(); + lcd.print("Init GPS..."); + do { + if (gps.encode(GPSUART.read())) { + // GPS data ready + ShowGPSData(); + } + } while (GPSUART.available()); + delay(1000); + } #endif + lcd.clear(); + lcd.print("Init OBD..."); + if (hasMPU6050) { + testMPU6050(); + } delay(500); } while(!obd.Init()); @@ -265,31 +302,31 @@ void setup() lcd.clear(); sprintf(buffer, "RPM:%c", obd.IsValidPID(PID_RPM) ? 'Y' : 'N'); lcd.print(buffer); - lcd.setCursor(8, 0); + lcd.setCursor(6, 0); sprintf(buffer, "SPD:%c", obd.IsValidPID(PID_SPEED) ? 'Y' : 'N'); lcd.print(buffer); lcd.setCursor(0, 1); sprintf(buffer, "THR:%c", obd.IsValidPID(PID_THROTTLE) ? 'Y' : 'N'); lcd.print(buffer); - lcd.setCursor(8, 1); + lcd.setCursor(6, 1); sprintf(buffer, "LOAD:%c", obd.IsValidPID(PID_ENGINE_LOAD) ? 'Y' : 'N'); lcd.print(buffer); lcd.setCursor(0, 2); sprintf(buffer, "MAF:%c", obd.IsValidPID(PID_MAF_FLOW) ? 'Y' : 'N'); lcd.print(buffer); - lcd.setCursor(8, 2); + lcd.setCursor(6, 2); sprintf(buffer, "MAP:%c", obd.IsValidPID(PID_INTAKE_MAP) ? 'Y' : 'N'); lcd.print(buffer); lcd.setCursor(0, 3); sprintf(buffer, "FUEL:%c", obd.IsValidPID(PID_FUEL_LEVEL) ? 'Y' : 'N'); lcd.print(buffer); - lcd.setCursor(8, 3); + lcd.setCursor(6, 3); sprintf(buffer, "FKPA:%c", obd.IsValidPID(PID_FUEL_PRESSURE) ? 'Y' : 'N'); lcd.print(buffer); - delay(5000); + delay(3000); lcd.clear(); //query(); } @@ -298,26 +335,53 @@ void loop() { int value; char buffer[OBD_RECV_BUF_SIZE]; - obd.Query((byte)pid); + // issue a query for specified OBD-II pid + + obd.SendQuery(); + + do { +#ifdef GPSUART + // while waiting for response, test GPS + while (GPSUART.available()) { + if (gps.encode(GPSUART.read())) { + // GPS data ready + ShowGPSData(); + } + } +#endif + } while (!obd.DataAvailable()); + + // check OBD response buffer[0] = 0; - while (!obd.DataAvailable()); char* data = obd.GetResponse((byte)pid, buffer); - lcd.setCursor(0, 1); - lcd.print(buffer); - lcd.setCursor(0, 0); + lcd.setCursor(6, 0); if (!data) { - // try recover next time - obd.WriteData('\r'); lcd.print("Data Error"); + // try recover next time + obd.Recover(); + } else if (!obd.GetParsedData((byte)pid, data, value)) { + lcd.print("Parse Error"); + lcd.setCursor(0, 1); + lcd.print(buffer); } else { - if (obd.GetParsedData((byte)pid, data, value)) { - sprintf(buffer, "[%04X]=%d ", pid, value); - } else { - sprintf(buffer, "Parse Error"); + if (!hasMPU6050) { + char *p = buffer; + while (*p && *p < ' ') p++; + for (char *q = p; *q; q++) { + if (*q < ' ') *q = ' '; + } + lcd.setCursor(0, 1); + lcd.print(p); } + sprintf(buffer, "=%d", value); + lcd.setCursor(6, 0); lcd.print(buffer); } - delay(50); + + // test MPU6050 + if (hasMPU6050) { + testMPU6050(); + } #if 0 adc_key_in = analogRead(0); // read the value from the sensor |