add G-force display and logging

1 files changed, 202 insertions, 144 deletions

diff --git a/obdlogger/obdlogger.ino b/obdlogger/obdlogger.ino
index 0366600..6f26c49 100644
--- a/obdlogger/obdlogger.ino
+++ b/obdlogger/obdlogger.ino
@@ -30,8 +30,11 @@
 #define PID_GPS_COORDINATE 0xF02
 #define PID_GPS_ALTITUDE 0xF03
 #define PID_GPS_SPEED 0xF04
+#define PID_ACC 0xF10
+#define PID_GYRO 0xF11
 
-#define DATASET_INTERVAL 100 /* ms */
+#define OBD_MIN_INTERVAL 100 /* ms */
+#define GPS_DATA_TIMEOUT 3000 /* ms */
 #define FILE_NAME_FORMAT "OBD%05d.CSV"
 
 // GPS logging can only be enabled when there is additional hardware serial UART
@@ -58,12 +61,14 @@ LCD_OLED lcd; /* for I2C OLED module */
 static uint32_t fileSize = 0;
 static uint32_t lastFileSize = 0;
 static uint32_t lastDataTime;
+static uint32_t lastGPSDataTime = 0;
 static int startDistance = 0;
 static uint16_t fileIndex = 0;
 
 class CLogger : public COBD
 {
 public:
+    CLogger():state(0) {}
     void InitIdleLoop()
     {
         // called while initializing
@@ -72,6 +77,7 @@ public:
         if (GPSUART.available()) {
             if (gps.encode(GPSUART.read())) {
                 state |= STATE_SD_READY;
+                lastGPSDataTime = millis();
             }
         }
         if (state & STATE_ACC_READY) {
@@ -87,7 +93,7 @@ public:
             sprintf(buf, "Z:%4d", data.value.z_accel / 190);
             lcd.setCursor(9, 3);
             lcd.print(buf);
-            delay(50);
+            delay(100);
         }
 #endif
     }
@@ -99,32 +105,8 @@ public:
             ProcessGPS();
 #endif
     }
-    void ShowStates()
-    {
-        lcd.setCursor(0, 1);
-        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;
-
-        lcd.clear();
-
-        // init SD card
-        if (CheckSD()) state |= STATE_SD_READY;
         ShowStates();
 
         if (MPU6050_init() == 0) state |= STATE_ACC_READY;
@@ -145,11 +127,12 @@ public:
         } while (!Init());
 
         state |= STATE_OBD_READY;
-        ReadSensor(PID_DISTANCE, startDistance);
 
         ShowStates();
         delay(1000);
 
+        ReadSensor(PID_DISTANCE, startDistance);
+
         // open file for logging
         if (!(state & STATE_SD_READY)) {
             do {
@@ -160,69 +143,63 @@ public:
         }
 
         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();
+        char filename[13];
+        sprintf(filename, FILE_NAME_FORMAT, fileIndex);
+        sdfile = SD.open(filename, FILE_WRITE);
+        if (!sdfile) {
+            lcd.setCursor(0, 0);
+            lcd.print("File Error");
+            delay(3000);
         }
 
         InitScreen();
         lastDataTime = millis();
     }
-    void LogData(byte pid)
+    void Loop()
     {
-        uint32_t start = millis();
+        static byte count = 0;
 
-        // send a query to OBD adapter for specified OBD-II pid
-        int value;
-        if (ReadSensor(pid, value)) {
-            ProcessOBD(pid, value);
-        }
+        LogData(PID_RPM);
 
-        // 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;
+        if (millis() - lastGPSDataTime > GPS_DATA_TIMEOUT) {
+            // GPS not ready
+            state &= ~STATE_GPS_READY;
+            LogData(PID_SPEED);
+        } else {
+            // GPS ready
+            state |= STATE_GPS_READY;
         }
 
+        LogData(PID_THROTTLE);
+
         if (state & STATE_ACC_READY) {
             ProcessAccelerometer();
         }
 
-        // 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
+        switch (count++) {
+        case 0:
+        case 64:
+        case 128:
+        case 192:
+            LogData(PID_DISTANCE);
+            break;
+        case 4:
+            LogData(PID_COOLANT_TEMP);
+            break;
+        case 20:
+            LogData(PID_INTAKE_TEMP);
+            break;
         }
-    }
-    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();
+
+        if (errors >= 5) {
+            Reconnect();
+            count = 0;
         }
-        Setup();
     }
-private:
     bool CheckSD()
     {
         lcd.setCursor(0, 0);
+        state &= ~STATE_SD_READY;
         if (card.init(SPI_HALF_SPEED, SD_CS_PIN)) {
             const char* type;
             char buf[20];
@@ -253,7 +230,7 @@ private:
             volumesize *= volume.clusterCount();
             volumesize >>= 10;
 
-            sprintf(buf, "%dG", (int)(volumesize + 511) >> 10);
+            sprintf(buf, "%dGB", (int)(volumesize + 511) >> 10);
             lcd.print(buf);
         } else {
             lcd.print("No SD Card      ");
@@ -280,36 +257,30 @@ private:
             return false;
         }
 
-        filename[8] = 0;
-        lcd.setCursor(11, 0);
-        lcd.print(filename + 3);
+        filename[2] = '[';
+        filename[8] = ']';
+        filename[9] = 0;
+        lcd.setCursor(9, 0);
+        lcd.print(filename + 2);
+        state |= STATE_SD_READY;
         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:  %");
-    }
+private:
     void ProcessGPS()
     {
         char buf[32];
         // process GPS data
         char c = GPSUART.read();
-        if (!gps.encode(c)) return;
+        if (!gps.encode(c))
+            return;
 
         // parsed GPS data is ready
         uint32_t dataTime = millis();
+        lastGPSDataTime = dataTime;
+
         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(buf, "%u,F01,%ld %ld\n", elapsed, date, time);
@@ -321,48 +292,123 @@ private:
         sdfile.write((uint8_t*)buf, len);
 
         // display LAT/LON if screen is big enough
-        if (lcd.getLines() > 3) {
-            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(buf);
-        }
+        if (((unsigned int)dataTime / 1000) & 1)
+            sprintf(buf, "%d.%ld  ", (int)(lat / 100000), lat % 100000);
+        else
+            sprintf(buf, "%d.%ld  ", (int)(lon / 100000), lon % 100000);
+        lcd.setCursor(0, 3);
+        lcd.print(buf);
 
-        len = sprintf(buf, "%u,F03,%ld\n", elapsed, gps.speed() * 1852 / 100);
+        unsigned int speed = (unsigned int)(gps.speed() * 1852 / 100 / 1000);
+        ShowSensorData(PID_SPEED, speed);
+        len = sprintf(buf, "%u,F03,%ld\n", elapsed, speed);
         sdfile.write((uint8_t*)buf, len);
 
         len = sprintf(buf, "%u,F04,%ld\n", elapsed, gps.altitude());
         sdfile.write((uint8_t*)buf, len);
         lastDataTime = dataTime;
     }
-
     void ProcessAccelerometer()
     {
         accel_t_gyro_union data;
         MPU6050_readout(&data);
+        uint32_t dataTime = millis();
+
+        ShowGForce(data.value.y_accel);
+
+        int len;
+        uint16_t elapsed = (uint16_t)(dataTime - lastDataTime);
         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);
+        // log x/y/z of accelerometer
+        len = sprintf(buf, "%u,F10,%d %d %d", data.value.x_accel, data.value.y_accel, data.value.z_accel);
+        sdfile.write((uint8_t*)buf, len);
+        // log x/y/z of gyro meter
+        len = sprintf(buf, "%u,F11,%d %d %d", data.value.x_gyro, data.value.y_gyro, data.value.z_gyro);
+        sdfile.write((uint8_t*)buf, len);
     }
+    void LogData(byte pid)
+    {
+        uint32_t start = millis();
+
+        // send a query to OBD adapter for specified OBD-II pid
+        int value;
+        if (ReadSensor(pid, value)) {
+            // save data timestamp at first time
+            uint32_t dataTime = millis();
+
+            // display data on screen
+            ShowSensorData(pid, value);
+
+            // log data to SD card
+            char buf[32];
+            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;
+            lastDataTime = dataTime;
+        }
+
+        // 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;
+        }
 
-    void ProcessOBD(byte pid, int value)
+        // if OBD response is very fast, go on processing GPS data for a while
+        while (millis() - start < OBD_MIN_INTERVAL) {
+#ifdef GPSUART
+            if (GPSUART.available())
+                ProcessGPS();
+#endif
+        }
+    }
+    void Reconnect()
     {
-        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;
+        sdfile.close();
+        lcd.clear();
+        lcd.print("Reconnecting...");
+        state &= ~(STATE_OBD_READY | STATE_ACC_READY);
+        //digitalWrite(SD_CS_PIN, LOW);
+        for (int i = 0; !Init(); i++) {
+            if (i == 10) lcd.clear();
+        }
+        fileIndex++;
+        Setup();
+    }
+    byte state;
 
-        // display OBD data
+    // screen layout related stuff
+    void ShowStates()
+    {
+        lcd.setCursor(0, 1);
+        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");
+    }
+    virtual void ShowSensorData(byte pid, int value)
+    {
+        char buf[8];
         switch (pid) {
         case PID_RPM:
             sprintf(buf, "%4u", (unsigned int)value % 10000);
-            lcd.setCursor(0, 1);
+            lcd.setCursor(4, 2);
             lcd.print(buf);
             break;
         case PID_SPEED:
@@ -372,23 +418,58 @@ private:
             break;
         case PID_THROTTLE:
             sprintf(buf, "%2d", value % 100);
-            lcd.setCursor(13, 1);
+            lcd.setCursor(13, 2);
             lcd.print(buf);
             break;
         case PID_DISTANCE:
-            if (value >= startDistance) {
-                sprintf(buf, "%4ukm", ((uint16_t)value - startDistance) % 1000);
+            if ((unsigned int)value >= startDistance) {
+                sprintf(buf, "%4ukm", ((unsigned int)value - startDistance) % 1000);
                 lcd.setCursor(10, 0);
                 lcd.print(buf);
             }
             break;
         }
-        lastDataTime = dataTime;
     }
-    byte state;
+    virtual void ShowGForce(int g)
+    {
+        byte n;
+        /* 0~2g -> 0~8 */
+        g /= 190 * 25;
+        lcd.setCursor(0, 1);
+        if (g == 0) {
+            lcd.clearLine(1);
+        } else if (g < 0 && g >= -8) {
+            for (n = 0; n < 8 + g; n++) {
+                lcd.write(' ');
+            }
+            for (; n < 8; n++) {
+                lcd.write('<');
+            }
+            lcd.print("        ");
+        } else if (g > 0 && g < 8) {
+            lcd.print("        ");
+            for (n = 0; n < g; n++) {
+                lcd.write('>');
+            }
+            for (; n < 8; n++) {
+                lcd.write(' ');
+            }
+        }
+    }
+    virtual void InitScreen()
+    {
+        lcd.clear();
+        lcd.backlight(true);
+        lcd.setCursor(lcd.getLines() <= 2 ? 4 : 7, 0);
+        lcd.print("kph");
+        lcd.setCursor(0, 2);
+        lcd.print("RPM:");
+        lcd.setCursor(9, 2);
+        lcd.print("THR:  %");
+    }
 };
 
-CLogger logger;
+static CLogger logger;
 
 void setup()
 {
@@ -408,38 +489,15 @@ void setup()
 #endif
 
     pinMode(SS, OUTPUT);
+
     delay(1000);
 
+    lcd.clear();
+    logger.CheckSD();
     logger.Setup();
 }
 
 void loop()
 {
-    static byte count = 0;
-    unsigned long t = millis();
-
-    switch (count++) {
-    case 0:
-    case 64:
-    case 128:
-    case 192:
-        logger.LogData(PID_DISTANCE);
-        break;
-    case 4:
-        logger.LogData(PID_COOLANT_TEMP);
-        break;
-    case 20:
-        logger.LogData(PID_INTAKE_TEMP);
-        break;
-    }
-
-    logger.LogData(PID_RPM);
-    logger.LogData(PID_SPEED);
-    logger.LogData(PID_THROTTLE);
-    //LogData(PID_ABS_ENGINE_LOAD);
-
-    if (logger.errors >= 5) {
-        logger.Reconnect();
-        count = 0;
-    }
+    logger.Loop();
 }