OBD-II/GPS/G-force logger

5 files changed, 446 insertions, 357 deletions

diff --git a/obdlogger/MPU6050.cpp b/obdlogger/MPU6050.cpp
index 86901d9..3091236 100644
--- a/obdlogger/MPU6050.cpp
+++ b/obdlogger/MPU6050.cpp
@@ -192,4 +192,5 @@ int MPU6050_readout(accel_t_gyro_union* accel_t_gyro)
   SWAP (accel_t_gyro->reg.x_gyro_h, accel_t_gyro->reg.x_gyro_l);
   SWAP (accel_t_gyro->reg.y_gyro_h, accel_t_gyro->reg.y_gyro_l);
   SWAP (accel_t_gyro->reg.z_gyro_h, accel_t_gyro->reg.z_gyro_l);
+  return 0;
 }
diff --git a/obdlogger/OBD.cpp b/obdlogger/OBD.cpp
index 938523b..9d457bc 100644
--- a/obdlogger/OBD.cpp
+++ b/obdlogger/OBD.cpp
@@ -9,13 +9,14 @@
 #include <avr/pgmspace.h>
 #include "OBD.h"
 
-#define INIT_CMD_COUNT 7
+#define INIT_CMD_COUNT 4
 #define MAX_CMD_LEN 6
 
-const char PROGMEM s_initcmd[INIT_CMD_COUNT][MAX_CMD_LEN] = {"ATZ\r","ATE0\r","ATL1\r","ATI\r","0100\r","0120\r","0140\r"};
+const char PROGMEM s_initcmd[INIT_CMD_COUNT][MAX_CMD_LEN] = {"ATZ\r","ATE0\r","ATL1\r","ATI\r"};
 const char PROGMEM s_searching[] = "SEARCHING";
 const char PROGMEM s_cmd_fmt[] = "%02X%02X 1\r";
-const char PROGMEM s_cmd_sleep[MAX_CMD_LEN] = "atlp\r";
+const char PROGMEM s_cmd_sleep[] = "atlp\r";
+const char PROGMEM s_cmd_vin[] = "0902\r";
 const char PROGMEM s_response_begin[] = "41 ";
 
 unsigned int hex2uint16(const char *p)
@@ -63,53 +64,94 @@ void COBD::Query(unsigned char pid)
 {
 	char cmd[8];
 	sprintf_P(cmd, s_cmd_fmt, dataMode, pid);
-	WriteData(cmd);
+	write(cmd);
 }
 
 bool COBD::ReadSensor(byte pid, int& result, bool passive)
 {
-    if (passive) {
-        bool hasData;
-        unsigned long tick = millis();
-        while (!(hasData = DataAvailable()) && millis() - tick < OBD_TIMEOUT_SHORT);
-        if (!hasData) {
-            errors++;
-            return false;
-        }
-    } else {
-        Query(pid);
+    // send a query command
+    Query(pid);
+    // wait for reponse
+    bool hasData;
+    unsigned long tick = millis();
+    do {
+        DataIdleLoop();
+    } while (!(hasData = available()) && millis() - tick < OBD_TIMEOUT_SHORT);
+    if (!hasData) {
+        errors++;
+        return false;
     }
-    return GetResponse(pid, result);
+    // receive and parse the response
+    return GetResponseParsed(pid, result);
 }
 
-bool COBD::DataAvailable()
+bool COBD::available()
 {
 	return OBDUART.available();
 }
 
-char COBD::ReadData()
+char COBD::read()
 {
 	return OBDUART.read();
 }
 
-void COBD::WriteData(const char* s)
+void COBD::write(const char* s)
 {
 	OBDUART.write(s);
 }
 
-void COBD::WriteData(const char c)
+void COBD::write(const char c)
 {
 	OBDUART.write(c);
 }
 
-char* COBD::GetResponse(byte pid, char* buffer)
+int COBD::GetConvertedValue(byte pid, char* data)
+{
+    int result;
+	switch (pid) {
+	case PID_RPM:
+		result = GetLargeValue(data) >> 2;
+		break;
+	case PID_FUEL_PRESSURE:
+		result = GetSmallValue(data) * 3;
+		break;
+	case PID_COOLANT_TEMP:
+	case PID_INTAKE_TEMP:
+	case PID_AMBIENT_TEMP:
+		result = GetTemperatureValue(data);
+		break;
+	case PID_ABS_ENGINE_LOAD:
+		result = GetLargeValue(data) * 100 / 255;
+		break;
+	case PID_MAF_FLOW:
+		result = GetLargeValue(data) / 100;
+		break;
+	case PID_THROTTLE:
+	case PID_ENGINE_LOAD:
+	case PID_FUEL_LEVEL:
+		result = GetPercentageValue(data);
+		break;
+	case PID_TIMING_ADVANCE:
+		result = (GetSmallValue(data) - 128) >> 1;
+		break;
+	case PID_DISTANCE:
+	case PID_RUNTIME:
+		result = GetLargeValue(data);
+		break;
+	default:
+		result = GetSmallValue(data);
+	}
+	return result;
+}
+
+char* COBD::GetResponse(byte& pid, char* buffer)
 {
 	unsigned long startTime = millis();
 	byte i = 0;
 
 	for (;;) {
-		if (DataAvailable()) {
-			char c = ReadData();
+		if (available()) {
+			char c = read();
 			buffer[i] = c;
 			if (++i == OBD_RECV_BUF_SIZE - 1) {
                 // buffer overflow
@@ -139,7 +181,9 @@ char* COBD::GetResponse(byte pid, char* buffer)
 	char *p = buffer;
 	while ((p = strstr_P(p, s_response_begin))) {
         p += 3;
-        if (pid == 0 || hex2uint8(p) == pid) {
+        byte curpid = hex2uint8(p);
+        if (pid == 0) pid = curpid;
+        if (curpid == pid) {
             errors = 0;
             p += 2;
             if (*p == ' ')
@@ -149,84 +193,40 @@ char* COBD::GetResponse(byte pid, char* buffer)
 	return 0;
 }
 
-bool COBD::GetParsedData(byte pid, char* data, int& result)
-{
-	switch (pid) {
-	case PID_RPM:
-		result = GetLargeValue(data) >> 2;
-		break;
-	case PID_FUEL_PRESSURE:
-		result = GetSmallValue(data) * 3;
-		break;
-	case PID_COOLANT_TEMP:
-	case PID_INTAKE_TEMP:
-	case PID_AMBIENT_TEMP:
-		result = GetTemperatureValue(data);
-		break;
-	case PID_ABS_ENGINE_LOAD:
-		result = GetLargeValue(data) * 100 / 255;
-		break;
-	case PID_MAF_FLOW:
-		result = GetLargeValue(data) / 100;
-		break;
-	case PID_THROTTLE:
-	case PID_ENGINE_LOAD:
-	case PID_FUEL_LEVEL:
-		result = GetPercentageValue(data);
-		break;
-	case PID_SPEED:
-	case PID_BAROMETRIC:
-	case PID_INTAKE_PRESSURE:
-		result = GetSmallValue(data);
-		break;
-	case PID_TIMING_ADVANCE:
-		result = (GetSmallValue(data) - 128) >> 1;
-		break;
-	case PID_DISTANCE:
-	case PID_RUNTIME:
-		result = GetLargeValue(data);
-		break;
-	default:
-		return false;
-	}
-	return true;
-}
-
-bool COBD::GetResponse(byte pid, int& result)
+bool COBD::GetResponseParsed(byte& pid, int& result)
 {
     char buffer[OBD_RECV_BUF_SIZE];
     char* data = GetResponse(pid, buffer);
     if (!data) {
         // try recover next time
-        WriteData('\r');
+        write('\r');
 		return false;
     }
-    return GetParsedData(pid, data, result);
-}
-
-bool COBD::GetResponsePassive(byte& pid, int& result)
-{
-    char buffer[OBD_RECV_BUF_SIZE];
-    char* data = GetResponse(0, buffer);
-    if (!data) {
-        // try recover next time
-        return false;
-    }
-    pid = hex2uint8(data - 3);
-    return GetParsedData(pid, data, result);
+    result = GetConvertedValue(pid, data);
+    return true;
 }
 
 void COBD::Sleep(int seconds)
 {
     char cmd[MAX_CMD_LEN];
     strcpy_P(cmd, s_cmd_sleep);
-    WriteData(cmd);
+    write(cmd);
     if (seconds) {
         delay((unsigned long)seconds << 10);
-        WriteData('\r');
+        write('\r');
     }
 }
 
+bool COBD::IsValidPID(byte pid)
+{
+    if (pid >= 0x7f)
+        return false;
+    pid--;
+    byte i = pid >> 3;
+    byte b = 0x80 >> (pid & 0x7);
+    return pidmap[i] & b;
+}
+
 bool COBD::Init(bool passive)
 {
 	unsigned long currentMillis;
@@ -238,14 +238,14 @@ bool COBD::Init(bool passive)
         if (!passive) {
             char cmd[MAX_CMD_LEN];
             strcpy_P(cmd, s_initcmd[i]);
-            WriteData(cmd);
+            write(cmd);
         }
         n = 0;
         prompted = 0;
         currentMillis = millis();
         for (;;) {
-            if (DataAvailable()) {
-                char c = ReadData();
+            if (available()) {
+                char c = read();
                 if (c == '>') {
                     buffer[n] = 0;
                     prompted++;
@@ -261,10 +261,25 @@ bool COBD::Init(bool passive)
                     //WriteData("\r");
                     return false;
                 }
-                DataTimeout();
+                InitIdleLoop();
             }
         }
     }
+
+    // load pid map
+    memset(pidmap, 0, sizeof(pidmap));
+    for (byte i = 0; i < 4; i++) {
+        byte pid = i * 0x20;
+        Query(pid);
+        char* data = GetResponse(pid, buffer);
+        if (!data) break;
+        data--;
+        for (byte n = 0; n < 4; n++) {
+            if (data[n * 3] != ' ')
+                break;
+            pidmap[i * 4 + n] = hex2uint8(data + n * 3 + 1);
+        }
+    }
     errors = 0;
 	return true;
 }
diff --git a/obdlogger/OBD.h b/obdlogger/OBD.h
index e608fee..e97ab67 100644
--- a/obdlogger/OBD.h
+++ b/obdlogger/OBD.h
@@ -9,10 +9,10 @@
 #define OBD_TIMEOUT_LONG 7000 /* ms */
 #define OBD_TIMEOUT_INIT 3000 /* ms */
 #define OBD_SERIAL_BAUDRATE 38400
-#define OBD_RECV_BUF_SIZE 48
+#define OBD_RECV_BUF_SIZE 64
 
 #ifndef OBDUART
-#ifdef __AVR_ATmega32U4__ /* for Leonardo */
+#if defined(__AVR_ATmega32U4__) || defined(__AVR_ATmega2560__) || defined(__AVR_ATmega1280__)
 #define OBDUART Serial1
 #else
 #define OBDUART Serial
@@ -30,7 +30,7 @@
 #define PID_ABS_ENGINE_LOAD 0x43
 #define PID_AMBIENT_TEMP 0x46
 #define PID_FUEL_PRESSURE 0x0A
-#define PID_INTAKE_PRESSURE 0x0B
+#define PID_INTAKE_MAP 0x0B
 #define PID_BAROMETRIC 0x33
 #define PID_TIMING_ADVANCE 0x0E
 #define PID_FUEL_LEVEL 0x2F
@@ -43,25 +43,20 @@ unsigned char hex2uint8(const char *p);
 class COBD
 {
 public:
-    COBD()
-    {
-        dataMode = 1;
-        errors = 0;
-    }
+    COBD():dataMode(1),errors(0) {}
 	bool Init(bool passive = false);
 	bool ReadSensor(byte pid, int& result, bool passive = false);
+	bool IsValidPID(byte pid);
 	void Sleep(int seconds);
 	// Query and GetResponse for advanced usage only
 	void Query(byte pid);
-	virtual char* GetResponse(byte pid, char* buffer);
-	virtual bool GetResponse(byte pid, int& result);
-	virtual bool GetResponsePassive(byte& pid, int& result);
-	virtual bool DataAvailable();
+	char* GetResponse(byte& pid, char* buffer);
+	bool GetResponseParsed(byte& pid, int& result);
 	byte dataMode;
 	byte errors;
 	//char recvBuf[OBD_RECV_BUF_SIZE];
 protected:
-    static bool GetParsedData(byte pid, char* data, int& result);
+    static int GetConvertedValue(byte pid, char* data);
 	static int GetPercentageValue(char* data)
 	{
 		return (int)hex2uint8(data) * 100 / 255;
@@ -78,8 +73,11 @@ protected:
 	{
 		return (int)hex2uint8(data) - 40;
 	}
-	virtual char ReadData();
-	virtual void WriteData(const char* s);
-	virtual void WriteData(const char c);
-	virtual void DataTimeout() {}
+	virtual bool available();
+	virtual char read();
+	virtual void write(const char* s);
+	virtual void write(const char c);
+	virtual void InitIdleLoop() {}
+	virtual void DataIdleLoop() {}
+	byte pidmap[4 * 4];
 };
diff --git a/obdlogger/PCD8544.cpp b/obdlogger/PCD8544.cpp
index 9c85662..a54bfc8 100644
--- a/obdlogger/PCD8544.cpp
+++ b/obdlogger/PCD8544.cpp
@@ -25,12 +25,7 @@
 
 #include "PCD8544.h"
 
-#if ARDUINO < 100
-#include <WProgram.h>
-#else
 #include <Arduino.h>
-#endif
-
 #include <avr/pgmspace.h>
 
 
@@ -77,7 +72,7 @@ void PCD8544::begin(unsigned char width, unsigned char height, unsigned char mod
     digitalWrite(this->pin_reset, HIGH);
     digitalWrite(this->pin_sce, HIGH);
     digitalWrite(this->pin_reset, LOW);
-    delay(100);  
+    delay(100);
     digitalWrite(this->pin_reset, HIGH);
 
     // Set the LCD parameters...
@@ -171,11 +166,11 @@ void PCD8544::home()
 
 void PCD8544::setCursor(unsigned char column, unsigned char line)
 {
-    this->column = (column % this->width);
+    this->column = (column * 6 % this->width);
     this->line = (line % (this->height/9 + 1));
 
     this->send(PCD8544_CMD, 0x80 | this->column);
-    this->send(PCD8544_CMD, 0x40 | this->line); 
+    this->send(PCD8544_CMD, 0x40 | this->line);
 }
 
 
@@ -185,7 +180,7 @@ void PCD8544::createChar(unsigned char chr, const unsigned char *glyph)
     if (chr >= ' ') {
         return;
     }
-    
+
     this->custom[chr] = glyph;
 }
 
@@ -278,7 +273,7 @@ void PCD8544::drawColumn(unsigned char lines, unsigned char value)
 
     // Find the line where "value" resides...
     unsigned char mark = (lines*8 - 1 - value)/8;
-    
+
     // Clear the lines above the mark...
     for (unsigned char line = 0; line < mark; line++) {
         this->setCursor(scolumn, sline + line);
@@ -299,16 +294,16 @@ void PCD8544::drawColumn(unsigned char lines, unsigned char value)
         this->setCursor(scolumn, sline + line);
         this->send(PCD8544_DATA, 0xff);
     }
-  
+
     // Leave the cursor in a consistent position...
-    this->setCursor(scolumn + 1, sline); 
+    this->setCursor(scolumn + 1, sline);
 }
 
 
 void PCD8544::send(unsigned char type, unsigned char data)
 {
     digitalWrite(this->pin_dc, type);
-  
+
     digitalWrite(this->pin_sce, LOW);
     shiftOut(this->pin_sdin, this->pin_sclk, MSBFIRST, data);
     digitalWrite(this->pin_sce, HIGH);
diff --git a/obdlogger/obdlogger.ino b/obdlogger/obdlogger.ino
index 1f6bd2b..0366600 100644
--- a/obdlogger/obdlogger.ino
+++ b/obdlogger/obdlogger.ino
@@ -1,5 +1,5 @@
 /*************************************************************************
-* Arduino OBD-II Data Logger
+* Arduino GPS/OBD-II/G-Force Data Logger
 * Distributed under GPL v2.0
 * Copyright (c) 2013 Stanley Huang <stanleyhuangyc@gmail.com>
 * All rights reserved.
@@ -14,314 +14,408 @@
 #include "MPU6050.h"
 
 //#define SD_CS_PIN 4 // ethernet shield with SD
-//#define SD_CS_PIN 7 // microduino
-#define SD_CS_PIN 10 // SD breakout
+#define SD_CS_PIN 7 // microduino
+//#define SD_CS_PIN 10 // SD breakout
 
-#define GPS_BAUDRATE 4800 /* bps */
+#define GPS_BAUDRATE 38400 /* bps */
 
-// addition PIDs (non-OBD)
+#define STATE_SD_READY 0x1
+#define STATE_OBD_READY 0x2
+#define STATE_GPS_CONNECTED 0x4
+#define STATE_GPS_READY 0x8
+#define STATE_ACC_READY 0x10
+
+// additional PIDs (non-OBD)
 #define PID_GPS_DATETIME 0xF01
 #define PID_GPS_COORDINATE 0xF02
 #define PID_GPS_ALTITUDE 0xF03
 #define PID_GPS_SPEED 0xF04
 
-#define DATASET_INTERVAL 250 /* ms */
+#define DATASET_INTERVAL 100 /* ms */
+#define FILE_NAME_FORMAT "OBD%05d.CSV"
 
-// GPS logging can only be enabled when there is additional serial UART
-#if defined(__AVR_ATmega2560__) || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega644P__)
-#define ENABLE_GPS
+// 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
 
-// GPS
-#ifdef ENABLE_GPS
+#ifdef GPSUART
 TinyGPS gps;
-#endif
+#endif // GPSUART
 
 // SD card
 Sd2Card card;
 SdVolume volume;
 File sdfile;
 
-// LCD
+// enable one LCD
 LCD_OLED lcd; /* for I2C OLED module */
 //LCD_PCD8544 lcd; /* for LCD4884 shield or Nokia 5100 screen module */
 //LCD_1602 lcd; /* for LCD1602 shield */
 
-static uint32_t filesize = 0;
-static uint32_t datacount = 0;
-
-void ProcessGPSData(char c);
+static uint32_t fileSize = 0;
+static uint32_t lastFileSize = 0;
+static uint32_t lastDataTime;
+static int startDistance = 0;
+static uint16_t fileIndex = 0;
 
-class COBD2 : public COBD
+class CLogger : public COBD
 {
 public:
-    void DataTimeout()
+    void InitIdleLoop()
     {
-#ifdef ENABLE_GPS
+        // called while initializing
+#ifdef GPSUART
         // detect GPS signal
-        while (Serial1.available()) {
-            if (gps.encode(Serial1.read())) {
-                if (datacount == 0) {
-                    lcd.setCursor(9, 3);
-                    lcd.print("GPS:Yes");
-                }
+        if (GPSUART.available()) {
+            if (gps.encode(GPSUART.read())) {
+                state |= STATE_SD_READY;
             }
         }
+        if (state & STATE_ACC_READY) {
+            accel_t_gyro_union data;
+            MPU6050_readout(&data);
+            char buf[8];
+            sprintf(buf, "X:%4d", data.value.x_accel / 190);
+            lcd.setCursor(9, 1);
+            lcd.print(buf);
+            sprintf(buf, "Y:%4d", data.value.y_accel / 190);
+            lcd.setCursor(9, 2);
+            lcd.print(buf);
+            sprintf(buf, "Z:%4d", data.value.z_accel / 190);
+            lcd.setCursor(9, 3);
+            lcd.print(buf);
+            delay(50);
+        }
 #endif
     }
-};
-
-COBD2 obd;
-
-bool ShowCardInfo()
-{
-  if (card.init(SPI_HALF_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 = "N/A";
-        }
-
-        sprintf(buf, "SD Type: %s", type);
-        lcd.setCursor(0, 0);
-        lcd.print(buf);
-        if (!volume.init(card)) {
-            lcd.setCursor(0, 1);
-            lcd.print("No FAT!");
-            return false;
-        }
-
-        uint32_t volumesize = volume.blocksPerCluster();
-        volumesize >>= 1; // 512 bytes per block
-        volumesize *= volume.clusterCount();
-        volumesize >>= 10;
-
-        sprintf(buf, "SD Size: %dMB", (int)volumesize);
-        lcd.setCursor(0, 1);
-        lcd.print(buf);
-        return true;
-  } else {
+    void DataIdleLoop()
+    {
+        // called while waiting for OBD-II response
+#ifdef GPSUART
+        if (GPSUART.available())
+            ProcessGPS();
+#endif
+    }
+    void ShowStates()
+    {
         lcd.setCursor(0, 1);
-        lcd.print("No SD Card      ");
-        return false;
-  }
-}
+        lcd.print("OBD:");
+        lcd.print((state & STATE_OBD_READY) ? "Yes" : "No");
+        lcd.setCursor(0, 2);
+        lcd.print("GPS:");
+        if (state & STATE_GPS_READY)
+            lcd.print("Yes");
+        else if (state & STATE_GPS_CONNECTED)
+            lcd.print("--");
+        else
+            lcd.print("No");
+
+        lcd.setCursor(0, 3);
+        lcd.print("ACC:");
+        lcd.print((state & STATE_ACC_READY) ? "Yes" : "No");
+    }
+    void Setup()
+    {
+        state = 0;
 
-static uint32_t lastTime;
-static int startDistance = 0;
+        lcd.clear();
 
-static void CheckSD()
-{
-    uint16_t fileidx = 0;
-    char filename[13];
-    for (;;) {
-        if (!ShowCardInfo()) {
-            delay(1000);
-            continue;
-        }
+        // init SD card
+        if (CheckSD()) state |= STATE_SD_READY;
+        ShowStates();
 
-        delay(1000);
-        SD.begin(SD_CS_PIN);
+        if (MPU6050_init() == 0) state |= STATE_ACC_READY;
+        ShowStates();
 
-        // determine file name
-        for (uint16_t index = 1; index < 65535; index++) {
-            sprintf(filename, "OBD%05d.CSV", index);
-            if (!SD.exists(filename)) {
-                fileidx = index;
+#ifdef GPSUART
+        unsigned long t = millis();
+        do {
+            if (GPSUART.available()) {
+                state |= STATE_GPS_CONNECTED;
                 break;
             }
-        }
-        if (fileidx) break;
-        lcd.setCursor(0, 2);
-        lcd.print("SD error  ");
-    }
+        } while (millis() - t <= 1000);
+#endif
 
-    lcd.clearLine(2);
-    lcd.setCursor(0, 2);
-    lcd.print(filename);
+        do {
+            ShowStates();
+        } while (!Init());
 
-    filesize = 0;
-    sdfile = SD.open(filename, FILE_WRITE);
-    if (sdfile) {
-        return;
-    }
+        state |= STATE_OBD_READY;
+        ReadSensor(PID_DISTANCE, startDistance);
 
-    lcd.setCursor(0, 2);
-    lcd.print("File error");
-}
+        ShowStates();
+        delay(1000);
 
-void InitScreen()
-{
-    byte n = lcd.getLines() <= 2 ? 6 : 9;
-    lcd.clear();
-    lcd.backlight(true);
-    lcd.setCursor(n, 0);
-    lcd.print("kph");
-    lcd.setCursor(n, 1);
-    lcd.print("rpm");
-}
+        // open file for logging
+        if (!(state & STATE_SD_READY)) {
+            do {
+                delay(1000);
+            } while (!CheckSD());
+            state |= STATE_SD_READY;
+            ShowStates();
+        }
 
-void setup()
-{
-    lcd.begin();
-    lcd.clear();
-    lcd.backlight(true);
-    lcd.print("OBD/GPS Logger");
-    lcd.setCursor(0, 1);
-    lcd.print("Initializing...");
-    delay(2000);
+        fileSize = 0;
+        for (;;) {
+            char filename[13];
+            sprintf(filename, FILE_NAME_FORMAT, fileIndex);
+            sdfile = SD.open(filename, FILE_WRITE);
+            if (sdfile) break;
+            lcd.setCursor(0, 2);
+            lcd.print("SD Error");
+            delay(1000);
+            CheckSD();
+        }
 
-    // start serial communication at the adapter defined baudrate
-    OBDUART.begin(OBD_SERIAL_BAUDRATE);
+        InitScreen();
+        lastDataTime = millis();
+    }
+    void LogData(byte pid)
+    {
+        uint32_t start = millis();
 
-    // init SD card
-    pinMode(SS, OUTPUT);
-    CheckSD();
+        // send a query to OBD adapter for specified OBD-II pid
+        int value;
+        if (ReadSensor(pid, value)) {
+            ProcessOBD(pid, value);
+        }
 
-    // initiate OBD-II connection until success
-    lcd.clearLine(3);
-    lcd.setCursor(0, 3);
-    lcd.print("OBD:No");
+        // flush SD data every 1KB
+        if (fileSize - lastFileSize >= 1024) {
+            sdfile.flush();
+            // display logged data size
+            char buf[7];
+            sprintf(buf, "%4uKB", (int)(fileSize >> 10));
+            lcd.setCursor(10, 3);
+            lcd.print(buf);
+            lastFileSize = fileSize;
+        }
 
-#ifdef ENABLE_GPS
-    Serial1.begin(GPS_BAUDRATE);
-    unsigned long t = millis();
-    do {
-        if (Serial1.available()) {
-            lcd.setCursor(9, 3);
-            lcd.print("GPS:No");
-            break;
+        if (state & STATE_ACC_READY) {
+            ProcessAccelerometer();
         }
-    } while (millis() - t <= 1000);
+
+        // if OBD response is very fast, go on processing GPS data for a while
+        while (millis() - start < DATASET_INTERVAL) {
+#ifdef GPSUART
+            if (GPSUART.available())
+                ProcessGPS();
 #endif
+        }
+    }
+    void Reconnect()
+    {
+        sdfile.close();
+        lcd.clear();
+        lcd.print("Reconnecting");
+        state = 0;
+        digitalWrite(SD_CS_PIN, LOW);
+        for (int i = 0; !Init(); i++) {
+            if (i == 10) lcd.clear();
+        }
+        Setup();
+    }
+private:
+    bool CheckSD()
+    {
+        lcd.setCursor(0, 0);
+        if (card.init(SPI_HALF_SPEED, SD_CS_PIN)) {
+            const char* type;
+            char buf[20];
 
-    while (!obd.Init());
+            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";
+            }
 
-    obd.ReadSensor(PID_DISTANCE, startDistance);
+            lcd.print(type);
+            lcd.write(' ');
+            if (!volume.init(card)) {
+                lcd.print("No FAT!");
+                return false;
+            }
 
-    lcd.setCursor(0, 3);
-    lcd.setCursor(4, 3);
-    lcd.print("Yes");
-    delay(1000);
+            uint32_t volumesize = volume.blocksPerCluster();
+            volumesize >>= 1; // 512 bytes per block
+            volumesize *= volume.clusterCount();
+            volumesize >>= 10;
 
-    InitScreen();
-    lastTime = millis();
-}
+            sprintf(buf, "%dG", (int)(volumesize + 511) >> 10);
+            lcd.print(buf);
+        } else {
+            lcd.print("No SD Card      ");
+            return false;
+        }
 
-static char databuf[32];
-static int len = 0;
-static int value = 0;
+        if (!SD.begin(SD_CS_PIN)) {
+            lcd.setCursor(8, 0);
+            lcd.print("Bad SD");
+            return false;
+        }
 
-#ifdef ENABLE_GPS
-void ProcessGPSData(char c)
-{
-    if (!gps.encode(c))
-        return;
+        char filename[13];
+        // now determine log file name
+        for (fileIndex = 1; fileIndex; fileIndex++) {
+            sprintf(filename, FILE_NAME_FORMAT, fileIndex);
+            if (!SD.exists(filename)) {
+                break;
+            }
+        }
+        if (!fileIndex) {
+            lcd.setCursor(8, 8);
+            lcd.print("Bad File");
+            return false;
+        }
 
-    // parsed GPS data is ready
-    uint32_t curTime = millis();
-    unsigned long fix_age;
+        filename[8] = 0;
+        lcd.setCursor(11, 0);
+        lcd.print(filename + 3);
+        return true;
+    }
 
+    void InitScreen()
     {
+        lcd.clear();
+        lcd.backlight(true);
+        lcd.setCursor(lcd.getLines() <= 2 ? 4 : 7, 0);
+        lcd.print("kph");
+        lcd.setCursor(5, 1);
+        lcd.print("rpm");
+        lcd.setCursor(9, 1);
+        lcd.print("THR:  %");
+    }
+    void ProcessGPS()
+    {
+        char buf[32];
+        // process GPS data
+        char c = GPSUART.read();
+        if (!gps.encode(c)) return;
+
+        // parsed GPS data is ready
+        uint32_t dataTime = millis();
+        uint16_t elapsed = (uint16_t)(dataTime - lastDataTime);
+        unsigned long fix_age;
+        int len;
+
         unsigned long date, time;
         gps.get_datetime(&date, &time, &fix_age);
-        len = sprintf(databuf, "%d,F01,%ld %ld\n", (int)(curTime - lastTime), date, time);
-        sdfile.write((uint8_t*)databuf, len);
-    }
+        len = sprintf(buf, "%u,F01,%ld %ld\n", elapsed, date, time);
+        sdfile.write((uint8_t*)buf, len);
 
-    {
         long lat, lon;
         gps.get_position(&lat, &lon, &fix_age);
-        len = sprintf(databuf, "%d,F02,%ld %ld\n", (int)(curTime - lastTime), lat, lon);
-        sdfile.write((uint8_t*)databuf, len);
+        len = sprintf(buf, "%u,F02,%ld %ld\n", elapsed, lat, lon);
+        sdfile.write((uint8_t*)buf, len);
+
         // display LAT/LON if screen is big enough
         if (lcd.getLines() > 3) {
-            sprintf(databuf, "%3d.%ld", (int)(lat / 100000), lat % 100000);
+            if (((unsigned int)dataTime / 1000) & 1)
+                sprintf(buf, "LAT: %d.%ld  ", (int)(lat / 100000), lat % 100000);
+            else
+                sprintf(buf, "LON: %d.%ld  ", (int)(lon / 100000), lon % 100000);
             lcd.setCursor(0, 2);
-            lcd.print(databuf);
-            sprintf(databuf, "%3d.%ld", (int)(lon / 100000), lon % 100000);
-            lcd.setCursor(0, 3);
-            lcd.print(databuf);
+            lcd.print(buf);
         }
-    }
-    len = sprintf(databuf, "%d,F03,%ld %ld\n", (int)(curTime - lastTime), gps.speed() * 1852 / 100);
-    sdfile.write((uint8_t*)databuf, len);
 
-    len = sprintf(databuf, "%d,F04,%ld %ld\n", (int)(curTime - lastTime), gps.altitude());
-    sdfile.write((uint8_t*)databuf, len);
-    lastTime = curTime;
-}
-#endif
-
-void RetrieveData(byte pid)
-{
-    // issue a query for OBD
-    obd.Query(pid);
+        len = sprintf(buf, "%u,F03,%ld\n", elapsed, gps.speed() * 1852 / 100);
+        sdfile.write((uint8_t*)buf, len);
 
-    // flush data in the buffer
-    if (len > 0) {
-        sdfile.write((uint8_t*)databuf, len);
+        len = sprintf(buf, "%u,F04,%ld\n", elapsed, gps.altitude());
+        sdfile.write((uint8_t*)buf, len);
+        lastDataTime = dataTime;
+    }
 
-        char* buf = databuf; // data in buffer saved, free for other use
-        if (datacount % 100 == 99) {
-            sdfile.flush();
-            sprintf(buf, "%4uKB", (int)(filesize >> 10));
-            lcd.setCursor(10, 3);
-            lcd.print(buf);
-        }
+    void ProcessAccelerometer()
+    {
+        accel_t_gyro_union data;
+        MPU6050_readout(&data);
+        char buf[20];
+        sprintf(buf, "%d %d %d  ", data.value.x_accel / 190, data.value.y_accel / 190, data.value.z_accel / 190);
+        lcd.setCursor(0, 2);
+        lcd.print(buf);
+    }
 
+    void ProcessOBD(byte pid, int value)
+    {
+        char buf[32];
+        uint32_t dataTime = millis();
+        uint16_t elapsed = (uint16_t)(dataTime - lastDataTime);
+        byte len = sprintf(buf, "%u,%X,%d\n", elapsed, pid, value);
+        // log OBD data
+        sdfile.write((uint8_t*)buf, len);
+        fileSize += len;
+
+        // display OBD data
         switch (pid) {
         case PID_RPM:
             sprintf(buf, "%4u", (unsigned int)value % 10000);
-            lcd.setCursor(0, 0);
-            lcd.printLarge(buf);
+            lcd.setCursor(0, 1);
+            lcd.print(buf);
             break;
         case PID_SPEED:
             sprintf(buf, "%3u", (unsigned int)value % 1000);
-            lcd.setCursor(2, 1);
+            lcd.setCursor(0, 0);
             lcd.printLarge(buf);
             break;
+        case PID_THROTTLE:
+            sprintf(buf, "%2d", value % 100);
+            lcd.setCursor(13, 1);
+            lcd.print(buf);
+            break;
         case PID_DISTANCE:
             if (value >= startDistance) {
-                sprintf(buf, "%4dkm", value - startDistance);
-                lcd.setCursor(10, 2);
+                sprintf(buf, "%4ukm", ((uint16_t)value - startDistance) % 1000);
+                lcd.setCursor(10, 0);
                 lcd.print(buf);
             }
             break;
         }
+        lastDataTime = dataTime;
     }
+    byte state;
+};
 
-#ifdef ENABLE_GPS
-    while (Serial1.available()) {
-        ProcessGPSData(Serial1.read());
-    }
+CLogger logger;
+
+void setup()
+{
+    lcd.begin();
+    lcd.clear();
+    lcd.backlight(true);
+    lcd.print("OBD/GPS Logger");
+    lcd.setCursor(0, 1);
+    lcd.print("Initializing...");
+
+    Wire.begin();
+
+    // start serial communication at the adapter defined baudrate
+    OBDUART.begin(OBD_SERIAL_BAUDRATE);
+#ifdef GPSUART
+    GPSUART.begin(GPS_BAUDRATE);
 #endif
 
-    if (obd.GetResponse(pid, value)) {
-        uint32_t curTime = millis();
-        len = sprintf(databuf, "%d,%X,%d\n", (int)(curTime - lastTime), pid, value);
-        filesize += len;
-        datacount++;
-        lastTime = curTime;
-        return;
-    }
-    len = 0;
+    pinMode(SS, OUTPUT);
+    delay(1000);
+
+    logger.Setup();
 }
 
 void loop()
 {
-    static char count = 0;
+    static byte count = 0;
     unsigned long t = millis();
 
     switch (count++) {
@@ -329,37 +423,23 @@ void loop()
     case 64:
     case 128:
     case 192:
-        RetrieveData(PID_DISTANCE);
+        logger.LogData(PID_DISTANCE);
         break;
     case 4:
-        RetrieveData(PID_COOLANT_TEMP);
+        logger.LogData(PID_COOLANT_TEMP);
         break;
     case 20:
-        RetrieveData(PID_INTAKE_TEMP);
+        logger.LogData(PID_INTAKE_TEMP);
         break;
     }
 
-    RetrieveData(PID_RPM);
-    RetrieveData(PID_SPEED);
-    //RetrieveData(PID_THROTTLE);
-    //RetrieveData(PID_ABS_ENGINE_LOAD);
+    logger.LogData(PID_RPM);
+    logger.LogData(PID_SPEED);
+    logger.LogData(PID_THROTTLE);
+    //LogData(PID_ABS_ENGINE_LOAD);
 
-    if (obd.errors >= 5) {
-        sdfile.close();
-        lcd.clear();
-        lcd.print("Reconnecting...");
-        digitalWrite(SD_CS_PIN, LOW);
-        for (int i = 0; !obd.Init(); i++) {
-            if (i == 10) lcd.clear();
-        }
-        digitalWrite(SD_CS_PIN, HIGH);
-        CheckSD();
-        delay(1000);
-        InitScreen();
+    if (logger.errors >= 5) {
+        logger.Reconnect();
         count = 0;
-        return;
     }
-
-    t = millis() - t;
-    if (t < DATASET_INTERVAL) delay(DATASET_INTERVAL - t);
 }