Automatic detecting UART and I2C OBD adapter

2 files changed, 575 insertions, 552 deletions

diff --git a/megalogger/config.h b/megalogger/config.h
index 62c9c96..e4345fb 100644
--- a/megalogger/config.h
+++ b/megalogger/config.h
@@ -2,11 +2,8 @@
 #define CONFIG_H_INCLUDED
 
 /**************************************
-* Choose model of OBD-II Adapter
+* OBD-II Adapter options
 **************************************/
-// OBD_MODEL_I2C for I2C version
-// OBD_MODEL_UART for UART version
-#define OBD_MODEL OBD_MODEL_I2C
 #define OBD_PROTOCOL PROTO_AUTO
 
 /**************************************
@@ -48,7 +45,6 @@
 // 38400bps for G6010 5Hz GPS receiver
 // 115200bps for G7020 10Hz GPS receiver
 #define GPS_BAUDRATE 38400 /* bps */
-//#define GPS_OPEN_BAUDRATE 4800 /* bps */
 
 /**************************************
 * Accelerometer & Gyro
diff --git a/megalogger/megalogger.ino b/megalogger/megalogger.ino
index b536e62..34d7840 100644
--- a/megalogger/megalogger.ino
+++ b/megalogger/megalogger.ino
@@ -7,9 +7,7 @@
 *************************************************************************/
 
 #include <Arduino.h>
-#if OBD_MODEL == OBD_MODEL_I2C
 #include <Wire.h>
-#endif
 #include <OBD.h>
 #include <SPI.h>
 #include <SD.h>
@@ -46,6 +44,8 @@ TinyGPS gps;
 
 #endif
 
+void doIdleTasks();
+
 static uint8_t lastFileSize = 0;
 static uint32_t lastGPSDataTime = 0;
 static uint32_t lastACCDataTime = 0;
@@ -63,611 +63,528 @@ static byte pidTier3[] = {PID_COOLANT_TEMP, PID_INTAKE_TEMP, PID_AMBIENT_TEMP, P
 #define TIER_NUM2 sizeof(pidTier2)
 #define TIER_NUM3 sizeof(pidTier3)
 
-#if OBD_MODEL == OBD_MODEL_I2C
-class COBDLogger : public COBDI2C, public CDataLogger
-#else
-class COBDLogger : public COBD, public CDataLogger
-#endif
+byte state = 0;
+COBD* obd = 0;
+CDataLogger logger;
+
+class CMyOBD : public COBD
 {
-public:
-    COBDLogger():state(0) {}
-    void setup()
+    void dataIdleLoop()
     {
-        lastGPSDataTime = 0;
-
-        showStates();
-
-#if USE_MPU6050
-        if (MPU6050_init() == 0) state |= STATE_ACC_READY;
-        showStates();
-#endif
+        doIdleTasks();
+    }
+};
 
-#if USE_GPS
-        unsigned long t = millis();
-        do {
-            if (GPSUART.available() && GPSUART.read() == '\r') {
-                state |= STATE_GPS_CONNECTED;
-                break;
-            }
-        } while (millis() - t <= 2000);
-#endif
+class CMyOBDI2C : public COBDI2C
+{
+    void dataIdleLoop()
+    {
+        doIdleTasks();
+    }
+};
 
-        do {
-            showStates();
-        } while (!init(OBD_PROTOCOL));
+void setColorByValue(int value, int threshold1, int threshold2, int threshold3)
+{
+    if (value < 0) value = -value;
+    if (value < threshold1) {
+      lcd.setColor(RGB16_WHITE);
+    } else if (value < threshold2) {
+      byte n = (uint32_t)(threshold2 - value) * 255 / (threshold2 - threshold1);
+      lcd.setColor(255, 255, n);
+    } else if (value < threshold3) {
+      byte n = (uint32_t)(threshold3 - value) * 255 / (threshold3 - threshold2);
+      lcd.setColor(255, n, 0);
+    } else {
+      lcd.setColor(255, 0, 0);
+    }
+}
 
-        state |= STATE_OBD_READY;
+void showSensorData(byte pid, int value)
+{
+    char buf[8];
+    switch (pid) {
+    case PID_RPM:
+        lcd.setFontSize(FONT_SIZE_XLARGE);
+        lcd.setCursor(34, 6);
+        if (value >= 10000) break;
+        setColorByValue(value, 2500, 3500, 5000);
+        lcd.printInt(value, 4);
+        break;
+    case PID_SPEED:
+        lcd.setFontSize(FONT_SIZE_XLARGE);
+        lcd.setCursor(50, 2);
+        if (value > 350) break;
+        setColorByValue(value, 50, 80, 160);
+        lcd.printInt((unsigned int)value % 1000, 3);
+        break;
+    case PID_THROTTLE:
+        lcd.setFontSize(FONT_SIZE_SMALL);
+        lcd.setCursor(38, 10);
+        if (value >= 100) value = 99;
+        setColorByValue(value, 50, 75, 90);
+        lcd.printInt(value, 2);
+        break;
+    case PID_ENGINE_LOAD:
+        lcd.setFontSize(FONT_SIZE_SMALL);
+        lcd.setCursor(108, 10);
+        if (value >= 100) value = 99;
+        setColorByValue(value, 50, 75, 90);
+        lcd.printInt(value, 2);
+        break;
+    case PID_ENGINE_FUEL_RATE:
+        if (value < 1000) {
+            lcd.setFontSize(FONT_SIZE_SMALL);
+            lcd.setCursor(38, 12);
+            lcd.printInt(value);
+        }
+        break;
+    }
+    lcd.setColor(RGB16_WHITE);
+}
 
-        showStates();
+void initScreen()
+{
+    lcd.clear();
+    lcd.draw4bpp(frame0[0], 78, 58);
+    lcd.setXY(164, 0);
+    lcd.draw4bpp(frame0[0], 78, 58);
+    lcd.setXY(0, 124);
+    lcd.draw4bpp(frame0[0], 78, 58);
+    lcd.setXY(164, 124);
+    lcd.draw4bpp(frame0[0], 78, 58);
+
+    lcd.setColor(RGB16_CYAN);
+    lcd.setFontSize(FONT_SIZE_SMALL);
+    lcd.setCursor(110, 4);
+    lcd.print("km/h");
+    lcd.setCursor(110, 7);
+    lcd.print("rpm");
+    lcd.setCursor(8, 10);
+    lcd.print("THR:    %");
+    lcd.setCursor(78, 10);
+    lcd.print("LOAD:   %");
+    lcd.setCursor(8, 12);
+    lcd.print("FUEL:   L/h");
+    lcd.setCursor(78, 12);
+    lcd.print("BATT:     V");
+
+    //lcd.setFont(FONT_SIZE_MEDIUM);
+    lcd.setColor(RGB16_CYAN);
+    lcd.setCursor(185, 2);
+    lcd.print("ELAPSED:");
+    lcd.setCursor(185, 5);
+    lcd.print("DISTANCE:");
+    lcd.setCursor(185, 6);
+    lcd.print("(km)");
+    lcd.setCursor(185, 8);
+    lcd.print("AVG SPEED:");
+    lcd.setCursor(185, 9);
+    lcd.print("(km/h)");
+    lcd.setCursor(185, 11);
+    lcd.print("OBD TIME:");
+    lcd.setCursor(185, 12);
+    lcd.print("(ms)");
+
+    lcd.setCursor(20, 18);
+    lcd.print("LAT:");
+    lcd.setCursor(20, 21);
+    lcd.print("LON:");
+    lcd.setCursor(20, 24);
+    lcd.print("ALT:");
+    lcd.setCursor(20, 27);
+    lcd.print("SAT:");
+
+    lcd.setFontSize(FONT_SIZE_SMALL);
+    lcd.setCursor(204, 18);
+    lcd.print("ACCELEROMETER");
+    lcd.setCursor(214, 22);
+    lcd.print("GYROSCOPE");
+    lcd.setCursor(185, 27);
+    lcd.print("LOG SIZE:");
+    lcd.setCursor(185, 20);
+    lcd.setColor(RGB16_WHITE);
+    lcd.print("X:     Y:     Z:");
+    lcd.setCursor(185, 24);
+    lcd.print("X:     Y:     Z:");
+
+    //lcd.setColor(0xFFFF);
+    /*
+    lcd.setCursor(32, 4);
+    lcd.print("%");
+    lcd.setCursor(68, 5);
+    lcd.print("Intake Air");
+    lcd.setCursor(112, 4);
+    lcd.print("C");
+    */
+
+    state |= STATE_GUI_ON;
+}
 
-        //lcd.setFont(FONT_SIZE_MEDIUM);
-        //lcd.setCursor(0, 14);
-        //lcd.print("VIN: XXXXXXXX");
+bool connectOBD()
+{
+    lcd.setCursor(60, 8);
+    lcd.print("UART");
+    obd = new CMyOBD;
+    obd->begin();
+    if (obd->init(OBD_PROTOCOL))
+        return true;
+    obd->end();
+    delete obd;
+
+    lcd.setCursor(60, 8);
+    lcd.print("I2C ");
+    obd = (COBD*)new CMyOBDI2C;
+    obd->begin();
+    if (obd->init(OBD_PROTOCOL))
+        return true;
+    obd->end();
+    delete obd;
 
-        // open file for logging
-        if (!(state & STATE_SD_READY)) {
-            if (checkSD()) {
-                state |= STATE_SD_READY;
-                showStates();
-            }
-        }
+    obd = 0;
+    return false;
+}
 
+bool checkSD()
+{
 #if ENABLE_DATA_LOG
-        uint16_t index = openFile();
-        lcd.setCursor(0, 16);
-        lcd.print("File ID:");
-        lcd.printInt(index);
-#endif
+    Sd2Card card;
+    SdVolume volume;
+    state &= ~STATE_SD_READY;
+    pinMode(SS, OUTPUT);
+    lcd.setCursor(0, 4);
 
-        showECUCap();
-        delay(1000);
-
-        initScreen();
-
-        startTime = millis();
-        lastRefreshTime = millis();
-    }
-    void loop()
-    {
-        static byte index = 0;
-        static byte index2 = 0;
-        static byte index3 = 0;
-        uint32_t t = millis();
-
-        logOBDData(pidTier1[index++]);
-        t = millis() - t;
-
-        if (index == TIER_NUM1) {
-            index = 0;
-            if (index2 == TIER_NUM2) {
-                index2 = 0;
-                if (isValidPID(pidTier3[index3])) {
-                    logOBDData(pidTier3[index3]);
-                }
-                index3 = (index3 + 1) % TIER_NUM3;
-                if (index3 == 0) {
-                    int v = getVoltage();
-                    lcd.setFontSize(FONT_SIZE_SMALL);
-                    lcd.setCursor(108, 12);
-                    lcd.printInt(v / 10);
-                    lcd.write('.');
-                    lcd.printInt(v % 10);
-                    logData(PID_VOLTAGE, v);
-                }
-            } else {
-                if (isValidPID(pidTier2[index2])) {
-                    logOBDData(pidTier2[index2]);
-                }
-                index2++;
-            }
+    lcd.setFontSize(FONT_SIZE_MEDIUM);
+    if (card.init(SPI_FULL_SPEED, SD_CS_PIN)) {
+        const char* type;
+        switch(card.type()) {
+        case SD_CARD_TYPE_SD1:
+            type = "SD1";
+            break;
+        case SD_CARD_TYPE_SD2:
+            type = "SD2";
+            break;
+        case SD_CARD_TYPE_SDHC:
+            type = "SDHC";
+            break;
+        default:
+            type = "SDx";
         }
 
-        if (dataTime -  lastRefreshTime >= 1000) {
-            char buf[12];
-            unsigned int sec = (dataTime - startTime) / 1000;
-            sprintf(buf, "%02u:%02u", sec / 60, sec % 60);
-            lcd.setFontSize(FONT_SIZE_MEDIUM);
-            lcd.setCursor(250, 2);
-            lcd.print(buf);
-            lcd.setCursor(250, 11);
-            if (t < 10000) {
-              lcd.printInt((uint16_t)t);
-            }
-            lastRefreshTime = dataTime;
+        lcd.print(type);
+        lcd.write(' ');
+        if (!volume.init(card)) {
+            lcd.print("No FAT!");
+            return false;
         }
 
-        if (errors >= 3) {
-            reconnect();
-        }
+        uint32_t volumesize = volume.blocksPerCluster();
+        volumesize >>= 1; // 512 bytes per block
+        volumesize *= volume.clusterCount();
+        volumesize >>= 10;
 
-#if USE_GPS
-        if (millis() - lastGPSDataTime > GPS_DATA_TIMEOUT || gps.satellites() < 3) {
-            // GPS not ready
-            state &= ~STATE_GPS_READY;
-        } else {
-            // GPS ready
-            state |= STATE_GPS_READY;
-        }
-#endif
+        lcd.print((int)volumesize);
+        lcd.print("MB");
+    } else {
+        lcd.print("No SD Card");
+        return false;
     }
-    bool checkSD()
-    {
-#if ENABLE_DATA_LOG
-        Sd2Card card;
-        SdVolume volume;
-        state &= ~STATE_SD_READY;
-        pinMode(SS, OUTPUT);
-        lcd.setCursor(0, 4);
-
-        lcd.setFontSize(FONT_SIZE_MEDIUM);
-        if (card.init(SPI_FULL_SPEED, SD_CS_PIN)) {
-            const char* type;
-            char buf[20];
-
-            switch(card.type()) {
-            case SD_CARD_TYPE_SD1:
-                type = "SD1";
-                break;
-            case SD_CARD_TYPE_SD2:
-                type = "SD2";
-                break;
-            case SD_CARD_TYPE_SDHC:
-                type = "SDHC";
-                break;
-            default:
-                type = "SDx";
-            }
-
-            lcd.print(type);
-            lcd.write(' ');
-            if (!volume.init(card)) {
-                lcd.print("No FAT!");
-                return false;
-            }
-
-            uint32_t volumesize = volume.blocksPerCluster();
-            volumesize >>= 1; // 512 bytes per block
-            volumesize *= volume.clusterCount();
-            volumesize >>= 10;
-
-            sprintf(buf, "%dMB", (int)volumesize);
-            lcd.print(buf);
-        } else {
-            lcd.print("No SD Card");
-            return false;
-        }
 
-        lcd.setCursor(0, 6);
-        if (!SD.begin(SD_CS_PIN)) {
-            lcd.print("Bad SD");
-            return false;
-        }
-
-        state |= STATE_SD_READY;
-        return true;
-#else
+    lcd.setCursor(0, 6);
+    if (!SD.begin(SD_CS_PIN)) {
+        lcd.print("Bad SD");
         return false;
-#endif
     }
-private:
-    void dataIdleLoop()
-    {
-        // callback while waiting OBD data
-        if (state & STATE_GUI_ON) {
-            if (state & STATE_ACC_READY) {
-                processAccelerometer();
-            }
-#if USE_GPS
-            uint32_t t = millis();
-            while (GPSUART.available() && millis() - t < MAX_GPS_PROCESS_TIME) {
-                processGPS();
-            }
+
+    state |= STATE_SD_READY;
+    return true;
+#else
+    return false;
 #endif
+}
 
-            if (getState() != OBD_CONNECTED) {
-                // display while initializing
-                char buf[10];
-                unsigned int t = (millis() - startTime) / 1000;
-                sprintf(buf, "%02u:%02u", t / 60, t % 60);
-                lcd.setFontSize(FONT_SIZE_SMALL);
-                lcd.setCursor(0, 28);
-                lcd.print(buf);
-            }
-            return;
-        }
-    }
 #if USE_GPS
-    void processGPS()
-    {
-        // process GPS data
-        char c = GPSUART.read();
-        if (!gps.encode(c))
-            return;
+void processGPS()
+{
+    // process GPS data
+    char c = GPSUART.read();
+    if (!gps.encode(c))
+        return;
 
-        // parsed GPS data is ready
-        uint32_t time;
-        uint32_t date;
+    // parsed GPS data is ready
+    uint32_t time;
+    uint32_t date;
 
-        dataTime = millis();
+    logger.dataTime = millis();
 
-        gps.get_datetime(&date, &time, 0);
-        logData(PID_GPS_TIME, (int32_t)time);
+    gps.get_datetime(&date, &time, 0);
+    logger.logData(PID_GPS_TIME, (int32_t)time);
 
-        int kph = gps.speed() * 1852 / 100000;
-        logData(PID_GPS_SPEED, kph);
+    int kph = gps.speed() * 1852 / 100000;
+    logger.logData(PID_GPS_SPEED, kph);
 
-        // no need to log GPS data when vehicle has not been moving
-        // that's when previous speed is zero and current speed is also zero
-        byte sat = gps.satellites();
-        if (sat >= 3 && sat < 100) {
-            int32_t lat, lon;
-            gps.get_position(&lat, &lon, 0);
-            logData(PID_GPS_LATITUDE, lat);
-            logData(PID_GPS_LONGITUDE, lon);
-            logData(PID_GPS_ALTITUDE, (int)(gps.altitude() / 100));
+    // no need to log GPS data when vehicle has not been moving
+    // that's when previous speed is zero and current speed is also zero
+    byte sat = gps.satellites();
+    if (sat >= 3 && sat < 100) {
+        int32_t lat, lon;
+        gps.get_position(&lat, &lon, 0);
+        logger.logData(PID_GPS_LATITUDE, lat);
+        logger.logData(PID_GPS_LONGITUDE, lon);
+        logger.logData(PID_GPS_ALTITUDE, (int)(gps.altitude() / 100));
 
-            lcd.setFontSize(FONT_SIZE_MEDIUM);
-            char buf[16];
-            sprintf(buf, "%d.%ld", (int)(lat / 100000), abs(lat) % 100000);
-            lcd.setCursor(50, 18);
-            lcd.print(buf);
-            sprintf(buf, "%d.%ld", (int)(lon / 100000), abs(lon) % 100000);
-            lcd.setCursor(50, 21);
+        lcd.setFontSize(FONT_SIZE_MEDIUM);
+        char buf[16];
+        sprintf(buf, "%d.%ld", (int)(lat / 100000), abs(lat) % 100000);
+        lcd.setCursor(50, 18);
+        lcd.print(buf);
+        sprintf(buf, "%d.%ld", (int)(lon / 100000), abs(lon) % 100000);
+        lcd.setCursor(50, 21);
+        lcd.print(buf);
+        int32_t alt = gps.altitude();
+        if (alt < 1000000) {
+            sprintf(buf, "%dm ", (int)(alt / 100));
+            lcd.setCursor(50, 24);
             lcd.print(buf);
-            int32_t alt = gps.altitude();
-            if (alt < 1000000) {
-                sprintf(buf, "%dm ", (int)(alt / 100));
-                lcd.setCursor(50, 24);
-                lcd.print(buf);
-            }
-            lcd.setCursor(50, 27);
-            lcd.printInt(gps.satellites());
-
         }
-        lastGPSDataTime = dataTime;
+        lcd.setCursor(50, 27);
+        lcd.printInt(gps.satellites());
     }
+    lastGPSDataTime = logger.dataTime;
+}
 #endif
-    void processAccelerometer()
-    {
-#if USE_MPU6050
-        dataTime = millis();
-
-        if (dataTime - lastACCDataTime < ACC_DATA_INTERVAL) {
-            return;
-        }
-
-        char buf[8];
-        accel_t_gyro_union data;
-        MPU6050_readout(&data);
-
-        lcd.setFontSize(FONT_SIZE_SMALL);
 
-        data.value.x_accel /= ACC_DATA_RATIO;
-        data.value.y_accel /= ACC_DATA_RATIO;
-        data.value.z_accel /= ACC_DATA_RATIO;
-        data.value.x_gyro /= GYRO_DATA_RATIO;
-        data.value.y_gyro /= GYRO_DATA_RATIO;
-        data.value.z_gyro /= GYRO_DATA_RATIO;
-
-        sprintf(buf, "%3d", data.value.x_accel);
-        setColorByValue(data.value.x_accel, 50, 100, 200);
-        lcd.setCursor(197, 20);
-        lcd.print(buf);
-        sprintf(buf, "%3d", data.value.y_accel);
-        setColorByValue(data.value.y_accel, 50, 100, 200);
-        lcd.setCursor(239, 20);
-        lcd.print(buf);
-        sprintf(buf, "%3d", data.value.z_accel);
-        setColorByValue(data.value.z_accel, 50, 100, 200);
-        lcd.setCursor(281, 20);
-        lcd.print(buf);
+void processAccelerometer()
+{
+#if USE_MPU6050
+    logger.dataTime = millis();
 
-        lcd.setColor(RGB16_WHITE);
-        sprintf(buf, "%3d ", data.value.x_gyro);
-        lcd.setCursor(197, 24);
-        lcd.print(buf);
-        sprintf(buf, "%3d ", data.value.y_gyro);
-        lcd.setCursor(239, 24);
-        lcd.print(buf);
-        sprintf(buf, "%3d ", data.value.z_gyro);
-        lcd.setCursor(281, 24);
-        lcd.print(buf);
+    if (logger.dataTime - lastACCDataTime < ACC_DATA_INTERVAL) {
+        return;
+    }
 
-        // log x/y/z of accelerometer
-        logData(PID_ACC, data.value.x_accel, data.value.y_accel, data.value.z_accel);
-        // log x/y/z of gyro meter
-        logData(PID_GYRO, data.value.x_gyro, data.value.y_gyro, data.value.z_gyro);
+    char buf[8];
+    accel_t_gyro_union data;
+    MPU6050_readout(&data);
+
+    lcd.setFontSize(FONT_SIZE_SMALL);
+
+    data.value.x_accel /= ACC_DATA_RATIO;
+    data.value.y_accel /= ACC_DATA_RATIO;
+    data.value.z_accel /= ACC_DATA_RATIO;
+    data.value.x_gyro /= GYRO_DATA_RATIO;
+    data.value.y_gyro /= GYRO_DATA_RATIO;
+    data.value.z_gyro /= GYRO_DATA_RATIO;
+
+    sprintf(buf, "%3d", data.value.x_accel);
+    setColorByValue(data.value.x_accel, 50, 100, 200);
+    lcd.setCursor(197, 20);
+    lcd.print(buf);
+    sprintf(buf, "%3d", data.value.y_accel);
+    setColorByValue(data.value.y_accel, 50, 100, 200);
+    lcd.setCursor(239, 20);
+    lcd.print(buf);
+    sprintf(buf, "%3d", data.value.z_accel);
+    setColorByValue(data.value.z_accel, 50, 100, 200);
+    lcd.setCursor(281, 20);
+    lcd.print(buf);
 
-        lastACCDataTime = dataTime;
+    lcd.setColor(RGB16_WHITE);
+    sprintf(buf, "%3d ", data.value.x_gyro);
+    lcd.setCursor(197, 24);
+    lcd.print(buf);
+    sprintf(buf, "%3d ", data.value.y_gyro);
+    lcd.setCursor(239, 24);
+    lcd.print(buf);
+    sprintf(buf, "%3d ", data.value.z_gyro);
+    lcd.setCursor(281, 24);
+    lcd.print(buf);
+
+    // log x/y/z of accelerometer
+    logger.logData(PID_ACC, data.value.x_accel, data.value.y_accel, data.value.z_accel);
+    // log x/y/z of gyro meter
+    logger.logData(PID_GYRO, data.value.x_gyro, data.value.y_gyro, data.value.z_gyro);
+
+    lastACCDataTime = logger.dataTime;
 #endif
+}
+
+void logOBDData(byte pid)
+{
+    char buffer[OBD_RECV_BUF_SIZE];
+    uint32_t start = millis();
+    int value;
+
+    // send query for OBD-II PID
+    obd->sendQuery(pid);
+    pid = 0;
+    // read responded PID and data
+    if (!obd->getResult(pid, value)) {
+        return;
     }
-    void logOBDData(byte pid)
-    {
-        char buffer[OBD_RECV_BUF_SIZE];
-        uint32_t start = millis();
 
-        // read OBD-II data
-        int value;
-        sendQuery(pid);
-        pid = 0;
-        if (!getResult(pid, value)) {
-            return;
-        }
+    logger.dataTime = millis();
+    // display data
+    showSensorData(pid, value);
 
-        dataTime = millis();
-        // display data
-        showSensorData(pid, value);
-
-        // log data to SD card
-        logData(0x100 | pid, value);
-
-        if (pid == PID_SPEED) {
-            if (lastSpeedTime) {
-                // estimate distance travelled since last speed update
-                distance += (uint32_t)(value + lastSpeed) * (dataTime - lastSpeedTime) / 2 / 3600;
-                // display speed
-                lcd.setFontSize(FONT_SIZE_MEDIUM);
-                lcd.setCursor(250, 5);
-                lcd.printInt(distance / 1000);
-                lcd.write('.');
-                lcd.printInt(((uint16_t)distance % 1000) / 100);
-                // calculate and display average speed
-                int avgSpeed = (unsigned long)distance * 1000 / (millis() - startTime) * 36 / 10;
-                lcd.setCursor(250, 8);
-                lcd.printInt(avgSpeed);
-            }
-            lastSpeed = value;
-            lastSpeedTime = dataTime;
+    // log data to SD card
+    logger.logData(0x100 | pid, value);
+
+    if (pid == PID_SPEED) {
+        if (lastSpeedTime) {
+            // estimate distance travelled since last speed update
+            distance += (uint32_t)(value + lastSpeed) * (logger.dataTime - lastSpeedTime) / 2 / 3600;
+            // display speed
+            lcd.setFontSize(FONT_SIZE_MEDIUM);
+            lcd.setCursor(250, 5);
+            lcd.printInt(distance / 1000);
+            lcd.write('.');
+            lcd.printInt(((uint16_t)distance % 1000) / 100);
+            // calculate and display average speed
+            int avgSpeed = (unsigned long)distance * 1000 / (millis() - startTime) * 36 / 10;
+            lcd.setCursor(250, 8);
+            lcd.printInt(avgSpeed);
         }
+        lastSpeed = value;
+        lastSpeedTime = logger.dataTime;
+    }
 #if ENABLE_DATA_LOG
-        // flush SD data every 1KB
-        if ((dataSize >> 10) != lastFileSize) {
-            flushFile();
-            // display logged data size
-            lcd.setFontSize(FONT_SIZE_SMALL);
-            lcd.setCursor(242, 27);
-            lcd.print((unsigned int)(dataSize >> 10));
-            lcd.print("KB");
-            lastFileSize = dataSize >> 10;
-        }
+    // flush SD data every 1KB
+    if ((logger.dataSize >> 10) != lastFileSize) {
+        logger.flushFile();
+        // display logged data size
+        lcd.setFontSize(FONT_SIZE_SMALL);
+        lcd.setCursor(242, 27);
+        lcd.print((unsigned int)(logger.dataSize >> 10));
+        lcd.print("KB");
+        lastFileSize = logger.dataSize >> 10;
+    }
 #endif
 
-        // if OBD response is very fast, go on processing other data for a while
+    // if OBD response is very fast, go on processing other data for a while
 #ifdef OBD_MIN_INTERVAL
-        while (millis() - start < OBD_MIN_INTERVAL) {
-            dataIdleLoop();
-        }
-#endif
+    while (millis() - start < OBD_MIN_INTERVAL) {
+        doIdleTasks();
     }
-    void showECUCap()
-    {
-        byte pidlist[] = {PID_RPM, PID_SPEED, PID_THROTTLE, PID_ENGINE_LOAD, PID_MAF_FLOW, PID_INTAKE_MAP, PID_FUEL_LEVEL, PID_FUEL_PRESSURE, PID_COOLANT_TEMP, PID_INTAKE_TEMP, PID_AMBIENT_TEMP, PID_TIMING_ADVANCE, PID_BAROMETRIC};
-        const char* namelist[] = {"ENGINE RPM", "SPEED", "THROTTLE", "ENGINE LOAD", "MAF", "MAP", "FUEL LEVEL", "FUEL PRESSURE", "COOLANT TEMP", "INTAKE TEMP","AMBIENT TEMP", "IGNITION TIMING", "BAROMETER"};
+#endif
+}
 
-        lcd.setFontSize(FONT_SIZE_MEDIUM);
-        for (byte i = 0; i < sizeof(pidlist) / sizeof(pidlist[0]); i++) {
-            lcd.setCursor(160, i * 2 + 4);
-            lcd.print(namelist[i]);
-            lcd.write(' ');
-            bool valid = isValidPID(pidlist[i]);
-            lcd.setColor(valid ? RGB16_GREEN : RGB16_RED);
-            lcd.draw(valid ? tick : cross, 16, 16);
-            lcd.setColor(RGB16_WHITE);
+void showECUCap()
+{
+    byte pidlist[] = {PID_ENGINE_LOAD, PID_COOLANT_TEMP, PID_FUEL_PRESSURE, PID_INTAKE_MAP, PID_RPM, PID_SPEED, PID_TIMING_ADVANCE, PID_INTAKE_TEMP, PID_MAF_FLOW, PID_THROTTLE, PID_AUX_INPUT,
+        PID_EGR_ERROR, PID_COMMANDED_EVAPORATIVE_PURGE, PID_FUEL_LEVEL, PID_CONTROL_MODULE_VOLTAGE, PID_ABSOLUTE_ENGINE_LOAD, PID_AMBIENT_TEMP, PID_COMMANDED_THROTTLE_ACTUATOR, PID_ETHANOL_FUEL,
+        PID_FUEL_RAIL_PRESSURE, PID_HYBRID_BATTERY_PERCENTAGE, PID_ENGINE_OIL_TEMP, PID_FUEL_INJECTION_TIMING, PID_ENGINE_FUEL_RATE, PID_ENGINE_TORQUE_DEMANDED, PID_ENGINE_TORQUE_PERCENTAGE};
+
+    lcd.setFontSize(FONT_SIZE_MEDIUM);
+    for (byte i = 0; i < sizeof(pidlist) / sizeof(pidlist[0]); i += 2) {
+        lcd.setCursor(184, i + 4);
+        for (byte j = 0; j < 2; j++) {
+            lcd.print("  ");
+            lcd.print((int)pidlist[i + j] | 0x100, HEX);
+            bool valid = obd->isValidPID(pidlist[i + j]);
+            if (valid) {
+                lcd.setColor(RGB16_GREEN);
+                lcd.draw(tick, 16, 16);
+                lcd.setColor(RGB16_WHITE);
+            } else {
+                lcd.print("  ");
+            }
         }
     }
-    void reconnect()
-    {
+}
+
+void reconnect()
+{
+    // fade out backlight
+    for (int n = 254; n >= 0; n--) {
+        lcd.setBackLight(n);
+        delay(20);
+    }
 #if ENABLE_DATA_LOG
-        closeFile();
+    logger.closeFile();
 #endif
-        uninit();
-        lcd.clear();
-        lcd.setFontSize(FONT_SIZE_MEDIUM);
-        lcd.print("Reconnecting...");
-        state &= ~(STATE_OBD_READY | STATE_GUI_ON);
-        //digitalWrite(SD_CS_PIN, LOW);
-        for (uint16_t i = 0; ; i++) {
-            if (i == 3) {
-                // fade out backlight
-                for (int n = 254; n >= 0; n--) {
-                    lcd.setBackLight(n);
-                    delay(20);
-                }
-                lcd.clear();
-            }
-            if ((getState() != OBD_CONNECTED || errors > 1) && !init())
-                continue;
+    lcd.clear();
+    state &= ~(STATE_OBD_READY | STATE_GUI_ON);
+    //digitalWrite(SD_CS_PIN, LOW);
+    for (;;) {
+        if (!obd->init())
+            continue;
 
-            int value;
-            if (read(PID_RPM, value) && value > 0)
-                break;
+        int value;
+        if (obd->read(PID_RPM, value) && value > 0)
+            break;
 
-            Narcoleptic.delay(2000);
-        }
-        // re-initialize
-        state |= STATE_OBD_READY;
-        startTime = millis();
-        lastSpeedTime = 0;
-        distance = 0;
+        Narcoleptic.delay(2000);
+    }
+    // re-initialize
+    state |= STATE_OBD_READY;
+    startTime = millis();
+    lastSpeedTime = 0;
+    distance = 0;
 #if ENABLE_DATA_LOG
-        openFile();
+    logger.openFile();
 #endif
-        initScreen();
-        // fade in backlight
-        for (int n = 1; n <= 255; n++) {
-            lcd.setBackLight(n);
-            delay(10);
-        }
+    initScreen();
+    // fade in backlight
+    for (int n = 1; n <= 255; n++) {
+        lcd.setBackLight(n);
+        delay(10);
     }
-    byte state;
+}
 
-    // screen layout related stuff
-    void showStates()
-    {
-        lcd.setFontSize(FONT_SIZE_MEDIUM);
+// screen layout related stuff
+void showStates()
+{
+    lcd.setFontSize(FONT_SIZE_MEDIUM);
 
-        lcd.setCursor(0, 8);
-        lcd.print("OBD ");
-        lcd.setColor((state & STATE_OBD_READY) ? RGB16_GREEN : RGB16_RED);
-        lcd.draw((state & STATE_OBD_READY) ? tick : cross, 16, 16);
-        lcd.setColor(RGB16_WHITE);
-
-        lcd.setCursor(0, 10);
-        lcd.print("ACC ");
-        lcd.setColor((state & STATE_ACC_READY) ? RGB16_GREEN : RGB16_RED);
-        lcd.draw((state & STATE_ACC_READY) ? tick : cross, 16, 16);
-        lcd.setColor(RGB16_WHITE);
-
-        lcd.setCursor(0, 12);
-        lcd.print("GPS ");
-        if (state & STATE_GPS_READY) {
-            lcd.setColor(RGB16_GREEN);
-            lcd.draw(tick, 16, 16);
-        } else if (state & STATE_GPS_CONNECTED)
-            lcd.print("--");
-        else {
-            lcd.setColor(RGB16_RED);
-            lcd.draw(cross, 16, 16);
-        }
-        lcd.setColor(RGB16_WHITE);
+    lcd.setCursor(0, 8);
+    lcd.print("OBD ");
+    lcd.setColor((state & STATE_OBD_READY) ? RGB16_GREEN : RGB16_RED);
+    lcd.draw((state & STATE_OBD_READY) ? tick : cross, 16, 16);
+    lcd.setColor(RGB16_WHITE);
+
+    lcd.setCursor(0, 10);
+    lcd.print("ACC ");
+    lcd.setColor((state & STATE_ACC_READY) ? RGB16_GREEN : RGB16_RED);
+    lcd.draw((state & STATE_ACC_READY) ? tick : cross, 16, 16);
+    lcd.setColor(RGB16_WHITE);
+
+    lcd.setCursor(0, 12);
+    lcd.print("GPS ");
+    if (state & STATE_GPS_CONNECTED) {
+        lcd.setColor(RGB16_GREEN);
+        lcd.draw(tick, 16, 16);
+    } else {
+        lcd.setColor(RGB16_RED);
+        lcd.draw(cross, 16, 16);
     }
-    void setColorByValue(int value, int threshold1, int threshold2, int threshold3)
-    {
-        if (value < 0) value = -value;
-        if (value < threshold1) {
-          lcd.setColor(RGB16_WHITE);
-        } else if (value < threshold2) {
-          byte n = (uint32_t)(threshold2 - value) * 255 / (threshold2 - threshold1);
-          lcd.setColor(255, 255, n);
-        } else if (value < threshold3) {
-          byte n = (uint32_t)(threshold3 - value) * 255 / (threshold3 - threshold2);
-          lcd.setColor(255, n, 0);
-        } else {
-          lcd.setColor(255, 0, 0);
-        }
+    lcd.setColor(RGB16_WHITE);
+}
+
+void doIdleTasks()
+{
+    if (!(state & STATE_GUI_ON))
+        return;
+
+    if (state & STATE_ACC_READY) {
+        processAccelerometer();
     }
-    void showSensorData(byte pid, int value)
-    {
-        char buf[8];
-        switch (pid) {
-        case PID_RPM:
-            lcd.setFontSize(FONT_SIZE_XLARGE);
-            lcd.setCursor(34, 6);
-            if (value >= 10000) break;
-            setColorByValue(value, 2500, 3500, 5000);
-            lcd.printInt(value, 4);
-            break;
-        case PID_SPEED:
-            lcd.setFontSize(FONT_SIZE_XLARGE);
-            lcd.setCursor(50, 2);
-            if (value > 350) break;
-            setColorByValue(value, 50, 80, 160);
-            lcd.printInt((unsigned int)value % 1000, 3);
-            break;
-        case PID_THROTTLE:
-            lcd.setFontSize(FONT_SIZE_SMALL);
-            lcd.setCursor(38, 10);
-            if (value >= 100) value = 99;
-            setColorByValue(value, 50, 75, 90);
-            lcd.printInt(value, 2);
-            break;
-        case PID_ENGINE_LOAD:
-            lcd.setFontSize(FONT_SIZE_SMALL);
-            lcd.setCursor(108, 10);
-            if (value >= 100) value = 99;
-            setColorByValue(value, 50, 75, 90);
-            lcd.printInt(value, 2);
-            break;
-        case PID_ENGINE_FUEL_RATE:
-            if (value < 1000) {
-                lcd.setFontSize(FONT_SIZE_SMALL);
-                lcd.setCursor(38, 12);
-                lcd.printInt(value);
-            }
-            break;
-        }
-        lcd.setColor(RGB16_WHITE);
+#if USE_GPS
+    uint32_t t = millis();
+    while (GPSUART.available() && millis() - t < MAX_GPS_PROCESS_TIME) {
+        processGPS();
     }
-    void initScreen()
-    {
-        lcd.clear();
-        lcd.draw4bpp(frame0[0], 78, 58);
-        lcd.setXY(164, 0);
-        lcd.draw4bpp(frame0[0], 78, 58);
-        lcd.setXY(0, 124);
-        lcd.draw4bpp(frame0[0], 78, 58);
-        lcd.setXY(164, 124);
-        lcd.draw4bpp(frame0[0], 78, 58);
-
-        lcd.setColor(RGB16_CYAN);
-        lcd.setFontSize(FONT_SIZE_SMALL);
-        lcd.setCursor(110, 4);
-        lcd.print("km/h");
-        lcd.setCursor(110, 7);
-        lcd.print("rpm");
-        lcd.setCursor(8, 10);
-        lcd.print("THR:    %");
-        lcd.setCursor(78, 10);
-        lcd.print("LOAD:   %");
-        lcd.setCursor(8, 12);
-        lcd.print("FUEL:   L/h");
-        lcd.setCursor(78, 12);
-        lcd.print("BATT:     V");
-
-        //lcd.setFont(FONT_SIZE_MEDIUM);
-        lcd.setColor(RGB16_CYAN);
-        lcd.setCursor(185, 2);
-        lcd.print("ELAPSED:");
-        lcd.setCursor(185, 5);
-        lcd.print("DISTANCE:");
-        lcd.setCursor(185, 6);
-        lcd.print("(km)");
-        lcd.setCursor(185, 8);
-        lcd.print("AVG SPEED:");
-        lcd.setCursor(185, 9);
-        lcd.print("(km/h)");
-        lcd.setCursor(185, 11);
-        lcd.print("OBD TIME:");
-        lcd.setCursor(185, 12);
-        lcd.print("(ms)");
-
-        lcd.setCursor(20, 18);
-        lcd.print("LAT:");
-        lcd.setCursor(20, 21);
-        lcd.print("LON:");
-        lcd.setCursor(20, 24);
-        lcd.print("ALT:");
-        lcd.setCursor(20, 27);
-        lcd.print("SAT:");
+#endif
 
+    if (obd->getState() != OBD_CONNECTED) {
+        // display while initializing
+        char buf[10];
+        unsigned int t = (millis() - startTime) / 1000;
+        sprintf(buf, "%02u:%02u", t / 60, t % 60);
         lcd.setFontSize(FONT_SIZE_SMALL);
-        lcd.setCursor(204, 18);
-        lcd.print("ACCELEROMETER");
-        lcd.setCursor(214, 22);
-        lcd.print("GYROSCOPE");
-        lcd.setCursor(185, 27);
-        lcd.print("LOG SIZE:");
-        lcd.setCursor(185, 20);
-        lcd.setColor(RGB16_WHITE);
-        lcd.print("X:     Y:     Z:");
-        lcd.setCursor(185, 24);
-        lcd.print("X:     Y:     Z:");
-
-        //lcd.setColor(0xFFFF);
-        /*
-        lcd.setCursor(32, 4);
-        lcd.print("%");
-        lcd.setCursor(68, 5);
-        lcd.print("Intake Air");
-        lcd.setCursor(112, 4);
-        lcd.print("C");
-        */
-
-        state |= STATE_GUI_ON;
+        lcd.setCursor(0, 28);
+        lcd.print(buf);
     }
-};
-
-static COBDLogger logger;
+}
 
 void setup()
 {
@@ -677,9 +594,11 @@ void setup()
     lcd.print("MEGA LOGGER - OBD-II/GPS/G-FORCE");
     lcd.setColor(RGB16_WHITE);
 
+    Wire.begin();
+
     for (int n = 0; n <= 255; n++) {
         lcd.setBackLight(n);
-        delay(10);
+        delay(5);
     }
 
 #if USE_GPS
@@ -693,16 +612,124 @@ void setup()
     // switching to 10Hz mode, effective only for MTK3329
     //GPSUART.println(PMTK_SET_NMEA_OUTPUT_ALLDATA);
     //GPSUART.println(PMTK_SET_NMEA_UPDATE_10HZ);
+    lastGPSDataTime = 0;
 #endif
 
-    logger.begin();
     logger.initSender();
 
-    logger.checkSD();
-    logger.setup();
+    checkSD();
+
+#if USE_MPU6050
+    if (MPU6050_init() == 0) state |= STATE_ACC_READY;
+#endif
+    showStates();
+
+#if USE_GPS
+    unsigned long t = millis();
+    do {
+        if (GPSUART.available() && GPSUART.read() == '\r') {
+            state |= STATE_GPS_CONNECTED;
+            break;
+        }
+    } while (millis() - t <= 2000);
+    showStates();
+#endif
+
+    delay(1000);
+
+    while (!connectOBD());
+    state |= STATE_OBD_READY;
+
+    showStates();
+
+    //lcd.setFont(FONT_SIZE_MEDIUM);
+    //lcd.setCursor(0, 14);
+    //lcd.print("VIN: XXXXXXXX");
+
+    // open file for logging
+    if (!(state & STATE_SD_READY)) {
+        if (checkSD()) {
+            state |= STATE_SD_READY;
+            showStates();
+        }
+    }
+
+#if ENABLE_DATA_LOG
+    uint16_t index = logger.openFile();
+    lcd.setCursor(0, 16);
+    lcd.print("File ID:");
+    lcd.println(index);
+#endif
+
+    showECUCap();
+    delay(2000);
+
+    initScreen();
+
+    startTime = millis();
+    lastRefreshTime = millis();
 }
 
 void loop()
 {
-    logger.loop();
+    static byte index = 0;
+    static byte index2 = 0;
+    static byte index3 = 0;
+    uint32_t t = millis();
+
+    logOBDData(pidTier1[index++]);
+    t = millis() - t;
+
+    if (index == TIER_NUM1) {
+        index = 0;
+        if (index2 == TIER_NUM2) {
+            index2 = 0;
+            if (obd->isValidPID(pidTier3[index3])) {
+                logOBDData(pidTier3[index3]);
+            }
+            index3 = (index3 + 1) % TIER_NUM3;
+            if (index3 == 0) {
+                int v = obd->getVoltage();
+                lcd.setFontSize(FONT_SIZE_SMALL);
+                lcd.setCursor(108, 12);
+                lcd.printInt(v / 10);
+                lcd.write('.');
+                lcd.printInt(v % 10);
+                logger.logData(PID_VOLTAGE, v);
+            }
+        } else {
+            if (obd->isValidPID(pidTier2[index2])) {
+                logOBDData(pidTier2[index2]);
+            }
+            index2++;
+        }
+    }
+
+    if (logger.dataTime -  lastRefreshTime >= 1000) {
+        char buf[12];
+        unsigned int sec = (logger.dataTime - startTime) / 1000;
+        sprintf(buf, "%02u:%02u", sec / 60, sec % 60);
+        lcd.setFontSize(FONT_SIZE_MEDIUM);
+        lcd.setCursor(250, 2);
+        lcd.print(buf);
+        lcd.setCursor(250, 11);
+        if (t < 10000) {
+          lcd.printInt((uint16_t)t);
+        }
+        lastRefreshTime = logger.dataTime;
+    }
+
+    if (obd->errors >= 3) {
+        reconnect();
+    }
+
+#if USE_GPS
+    if (millis() - lastGPSDataTime > GPS_DATA_TIMEOUT || gps.satellites() < 3) {
+        // GPS not ready
+        state &= ~STATE_GPS_READY;
+    } else {
+        // GPS ready
+        state |= STATE_GPS_READY;
+    }
+#endif
 }