Reference data logger sketch for Freematics OBD-II UART Adapter

3 files changed, 696 insertions, 0 deletions

diff --git a/datalogger/config.h b/datalogger/config.h
new file mode 100644
index 0000000..177f128
--- /dev/null
+++ b/datalogger/config.h
@@ -0,0 +1,59 @@
+#ifndef CONFIG_H_INCLUDED
+#define CONFIG_H_INCLUDED
+
+/**************************************
+* Data logging/streaming out
+**************************************/
+
+// enable(1)/disable(0) data logging (if SD card is present)
+#define ENABLE_DATA_LOG 1
+
+// enable(1)/disable(0) data streaming
+#define ENABLE_DATA_OUT 1
+
+#define ENABLE_DATA_CACHE 0
+#define MAX_CACHE_SIZE 256  /* bytes */
+
+// followings define the format of data streaming, enable one of them only
+// FORMAT_BIN is required by Freematics OBD iOS App
+//#define STREAM_FORMAT FORMAT_BIN
+// FORMAT_TEXT is for readable text output
+#define STREAM_FORMAT FORMAT_TEXT
+
+// serial baudrate for data out streaming
+#define STREAM_BAUDRATE 115200
+
+// maximum size per file, a new file will be created on reaching this size
+#define MAX_LOG_FILE_SIZE 1024 /* KB */
+
+/**************************************
+* Hardware setup
+**************************************/
+
+// max allowed time for connecting OBD-II (0 for forever)
+#define OBD_ATTEMPT_TIME 0 /* seconds */
+
+// OBD-II UART baudrate
+#define OBD_UART_BAUDRATE 115200L
+
+// SD pin
+#define SD_CS_PIN 10
+
+// enable(1)/disable(0) MEMS sensor
+#define USE_MPU6050 1
+#define ACC_DATA_RATIO 172
+#define GYRO_DATA_RATIO 256
+#define COMPASS_DATA_RATIO 8
+
+// enable(1)/disable(0) GPS module
+#define USE_GPS 1
+#define LOG_GPS_NMEA_DATA 0
+#define LOG_GPS_PARSED_DATA 1
+
+// GPS parameters
+#define GPS_SERIAL_BAUDRATE 115200L
+
+// motion detection
+#define START_MOTION_THRESHOLD 10000 /* for wakeup on movement */
+
+#endif // CONFIG_H_INCLUDED
diff --git a/datalogger/datalogger.h b/datalogger/datalogger.h
new file mode 100644
index 0000000..fb70353
--- /dev/null
+++ b/datalogger/datalogger.h
@@ -0,0 +1,212 @@
+/*************************************************************************
+* Telematics Data Logger Class
+* Distributed under BSD license
+* Written by Stanley Huang <support@freematics.com.au>
+* Visit http://freematics.com for more information
+*************************************************************************/
+
+// additional custom PID for data logger
+#define PID_DATA_SIZE 0x80
+
+#if ENABLE_DATA_LOG
+File sdfile;
+#endif
+
+class CDataLogger {
+public:
+    CDataLogger():m_lastDataTime(0),dataTime(0),dataSize(0)
+    {
+#if ENABLE_DATA_CACHE
+        cacheBytes = 0;
+#endif
+    }
+    void initSender()
+    {
+#if ENABLE_DATA_OUT
+        Serial.begin(STREAM_BAUDRATE);
+#endif
+    }
+    byte genTimestamp(char* buf, bool absolute)
+    {
+      byte n;
+      if (absolute || dataTime >= m_lastDataTime + 60000) {
+        // absolute timestamp
+        n = sprintf_P(buf, PSTR("#%lu,"), dataTime);
+      } else {
+        // relative timestamp
+        uint16_t elapsed = (unsigned int)(dataTime - m_lastDataTime);
+        n = sprintf_P(buf, PSTR("%u,"), elapsed);
+      }
+      return n;
+    }
+    void record(const char* buf, byte len)
+    {
+#if ENABLE_DATA_LOG
+        char tmp[12];
+        byte n = genTimestamp(tmp, dataSize == 0);
+        dataSize += sdfile.write((uint8_t*)tmp, n);
+        dataSize += sdfile.write(buf, len);
+        sdfile.write('\n');
+        dataSize++;
+#endif
+        m_lastDataTime = dataTime;
+    }
+    void dispatch(const char* buf, byte len)
+    {
+#if ENABLE_DATA_CACHE
+        // reserve some space for timestamp, ending white space and zero terminator
+        int l = cacheBytes + len + 12 - CACHE_SIZE;
+        if (l >= 0) {
+          // cache full
+#if CACHE_SHIFT
+          // discard the oldest data
+          for (l = CACHE_SIZE / 2; cache[l] && cache[l] != ' '; l++);
+          if (cache[l]) {
+            cacheBytes -= l;
+            memcpy(cache, cache + l + 1, cacheBytes);
+          } else {
+            cacheBytes = 0;  
+          }
+#else
+          return;        
+#endif
+        }
+        // add new data at the end
+        byte n = genTimestamp(cache + cacheBytes, cacheBytes == 0);
+        if (n == 0) {
+          // same timestamp 
+          cache[cacheBytes - 1] = ';';
+        } else {
+          cacheBytes += n;
+          cache[cacheBytes++] = ',';
+        }
+        if (cacheBytes + len < CACHE_SIZE - 1) {
+          memcpy(cache + cacheBytes, buf, len);
+          cacheBytes += len;
+          cache[cacheBytes++] = ' ';
+        }
+        cache[cacheBytes] = 0;
+#else
+        //char tmp[12];
+        //byte n = genTimestamp(tmp, dataTime >= m_lastDataTime + 100);
+        //Serial.write(tmp, n);
+#endif
+#if ENABLE_DATA_OUT
+        Serial.write((uint8_t*)buf, len);
+        Serial.println();
+#endif
+    }
+    void logData(const char* buf, byte len)
+    {
+        dispatch(buf, len);
+        record(buf, len);
+    }
+    void logData(uint16_t pid)
+    {
+        char buf[8];
+        byte len = translatePIDName(pid, buf);
+        dispatch(buf, len);
+        record(buf, len);
+    }
+    void logData(uint16_t pid, int16_t value)
+    {
+        char buf[16];
+        byte n = translatePIDName(pid, buf);
+        byte len = sprintf_P(buf + n, PSTR("%d"), value) + n;
+        dispatch(buf, len);
+        record(buf, len);
+    }
+    void logData(uint16_t pid, int32_t value)
+    {
+        char buf[20];
+        byte n = translatePIDName(pid, buf);
+        byte len = sprintf_P(buf + n, PSTR("%ld"), value) + n;
+        dispatch(buf, len);
+        record(buf, len);
+    }
+    void logData(uint16_t pid, uint32_t value)
+    {
+        char buf[20];
+        byte n = translatePIDName(pid, buf);
+        byte len = sprintf_P(buf + n, PSTR("%lu"), value) + n;
+        dispatch(buf, len);
+        record(buf, len);
+    }
+    void logData(uint16_t pid, int value1, int value2, int value3)
+    {
+        char buf[24];
+        byte n = translatePIDName(pid, buf);
+        n += sprintf_P(buf + n, PSTR("%d,%d,%d"), value1, value2, value3);
+        dispatch(buf, n);
+        record(buf, n);
+    }
+    void logCoordinate(uint16_t pid, int32_t value)
+    {
+        char buf[24];
+        byte len = translatePIDName(pid, buf);
+        len += sprintf_P(buf + len, PSTR("%d.%06lu"), (int)(value / 1000000), abs(value) % 1000000);
+        dispatch(buf, len);
+        record(buf, len);
+    }
+#if ENABLE_DATA_LOG
+    uint16_t openFile(uint32_t dateTime = 0)
+    {
+        uint16_t fileIndex;
+        char path[20] = "/DATA";
+
+        dataSize = 0;
+        if (SD.exists(path)) {
+            if (dateTime) {
+               // using date and time as file name 
+               sprintf(path + 5, "/%08lu.CSV", dateTime);
+               fileIndex = 1;
+            } else {
+              // use index number as file name
+              for (fileIndex = 1; fileIndex; fileIndex++) {
+                  sprintf(path + 5, "/DAT%05u.CSV", fileIndex);
+                  if (!SD.exists(path)) {
+                      break;
+                  }
+              }
+              if (fileIndex == 0)
+                  return 0;
+            }
+        } else {
+            SD.mkdir(path);
+            fileIndex = 1;
+            sprintf(path + 5, "/DAT%05u.CSV", 1);
+        }
+
+        sdfile = SD.open(path, FILE_WRITE);
+        if (!sdfile) {
+            return 0;
+        }
+        return fileIndex;
+    }
+    void closeFile()
+    {
+        sdfile.close();
+        dataSize = 0;
+    }
+    void flushFile()
+    {
+        sdfile.flush();
+    }
+#endif
+    uint32_t dataTime;
+    uint32_t dataSize;
+#if ENABLE_DATA_CACHE
+    void purgeCache()
+    {
+      cacheBytes = 0;
+    }
+    char cache[CACHE_SIZE];
+    int cacheBytes;
+#endif
+private:
+    byte translatePIDName(uint16_t pid, char* text)
+    {
+        return sprintf_P(text, PSTR("%X,"), pid);
+    }
+    uint32_t m_lastDataTime;
+};
diff --git a/datalogger/datalogger.ino b/datalogger/datalogger.ino
new file mode 100644
index 0000000..8bda37d
--- /dev/null
+++ b/datalogger/datalogger.ino
@@ -0,0 +1,425 @@
+/*************************************************************************
+* OBD-II/MEMS/GPS Data Logging Sketch for Freematics ONE
+* Distributed under BSD license
+* Visit http://freematics.com/products/freematics-one for more information
+* Developed by Stanley Huang <stanleyhuangyc@gmail.com>
+* 
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+* THE SOFTWARE.
+*************************************************************************/
+
+#include <Arduino.h>
+#include <Wire.h>
+#include <OBD.h>
+#include <SPI.h>
+#include <SD.h>
+#include <TinyGPS.h>
+#include "config.h"
+#include "datalogger.h"
+
+// states
+#define STATE_SD_READY 0x1
+#define STATE_OBD_READY 0x2
+#define STATE_GPS_FOUND 0x4
+#define STATE_GPS_READY 0x8
+#define STATE_MEMS_READY 0x10
+#define STATE_FILE_READY 0x20
+
+static uint8_t lastFileSize = 0;
+static uint16_t fileIndex = 0;
+
+uint16_t MMDD = 0;
+uint32_t UTC = 0;
+
+#if USE_MPU6050 || USE_MPU9250
+byte accCount = 0; // count of accelerometer readings
+long accSum[3] = {0}; // sum of accelerometer data
+int accCal[3] = {0}; // calibrated reference accelerometer data
+byte deviceTemp; // device temperature (celcius degree)
+#endif
+
+#if USE_GPS
+// GPS logging can only be enabled when there is additional hardware serial UART
+#define GPSUART Serial2
+TinyGPS gps;
+
+typedef struct {
+    uint32_t date;
+    uint32_t time;
+    int32_t lat;
+    int32_t lng;
+    int16_t alt;
+    uint8_t speed;
+    uint8_t sat;
+    int16_t heading;
+} GPS_DATA;
+
+bool getGPSData(GPS_DATA* gd)
+{
+    bool parsed = false;
+    while (!parsed && GPSUART.available()) {
+      char c = GPSUART.read();
+      parsed = gps.encode(c);
+    }
+    if (!parsed) return false;
+	gps.get_datetime((unsigned long*)&gd->date, (unsigned long*)&gd->time, 0);
+	gps.get_position((long*)&gd->lat, (long*)&gd->lng, 0);
+    gd->sat = gps.satellites();
+    gd->speed = gps.speed() * 1852 / 100000; /* km/h */
+    gd->alt = gps.altitude();
+    gd->heading = gps.course() / 100;
+    return true;
+}
+
+bool initGPS(uint32_t baudrate)
+{
+    if (baudrate)
+      GPSUART.begin(baudrate);  
+    else
+      GPSUART.end();
+    return true;
+}
+#endif
+
+class COBDLogger : public COBD, public CDataLogger
+{
+public:
+    COBDLogger():state(0) {}
+    void setup()
+    {
+        state = 0;
+
+        delay(500);
+        byte ver = begin();
+        Serial.print("Firmware Ver. ");
+        Serial.println(ver);
+
+#if USE_MPU6050 || USE_MPU9250
+        Serial.print("MEMS ");
+        if (memsInit()) {
+          state |= STATE_MEMS_READY;
+          Serial.println("OK");
+        } else {
+          Serial.println("NO");
+        }
+#endif
+
+#if ENABLE_DATA_LOG
+        Serial.print("SD ");
+        uint16_t volsize = initSD();
+        if (volsize) {
+          Serial.print(volsize);
+          Serial.println("MB");
+        } else {
+          Serial.println("NO");
+        }
+#endif
+
+        Serial.print("OBD ");
+        if (init()) {
+          Serial.println("OK");
+        } else {
+          Serial.println("NO");
+          reconnect();
+        }
+        state |= STATE_OBD_READY;
+
+#if 0
+        // retrieve VIN
+        char buffer[128];
+        if ((state & STATE_OBD_READY) && getVIN(buffer, sizeof(buffer))) {
+          Serial.print("VIN:");
+          Serial.println(buffer);
+        }
+#endif
+
+#if USE_GPS
+        Serial.print("GPS ");
+        if (initGPS(GPS_SERIAL_BAUDRATE)) {
+          state |= STATE_GPS_FOUND;
+          Serial.println("OK");
+        } else {
+          Serial.println("NO");
+        }
+#endif
+
+#if USE_MPU6050 || USE_MPU9250
+		// obtain reference MEMS data
+		Serial.print("Calibrating MEMS...");
+		for (uint32_t t = millis(); millis() - t < 1000; ) {
+			readMEMS();
+		}
+		calibrateMEMS();
+		Serial.print(accCal[0]);
+		Serial.print('/');
+		Serial.print(accCal[1]);
+		Serial.print('/');
+		Serial.println(accCal[2]);
+#endif
+
+		delay(500);
+    }
+#if USE_GPS
+    void logGPSData()
+    {
+#if LOG_GPS_NMEA_DATA
+        // issue the command to get NMEA data (one line per request)
+        char buf[255];
+        byte n = getGPSRawData(buf, sizeof(buf));
+        if (n) {
+            dataTime = millis();
+            // strip heading $GPS and ending \r\n
+            logData(buf + 4, n - 6);
+        }
+#endif
+#if LOG_GPS_PARSED_DATA
+        // issue the command to get parsed GPS data
+        GPS_DATA gd = {0};
+        
+        if (getGPSData(&gd)) {
+            dataTime = millis();
+            if (gd.time && gd.time != UTC) {
+              byte day = gd.date / 10000;
+              if (MMDD % 100 != day) {
+                logData(PID_GPS_DATE, gd.date);
+              }
+              logData(PID_GPS_TIME, gd.time);
+              logData(PID_GPS_LATITUDE, gd.lat);
+              logData(PID_GPS_LONGITUDE, gd.lng);
+              logData(PID_GPS_ALTITUDE, gd.alt);
+              logData(PID_GPS_SPEED, gd.speed);
+              logData(PID_GPS_SAT_COUNT, gd.sat);
+              // save current date in MMDD format
+              unsigned int DDMM = gd.date / 100;
+              UTC = gd.time;
+              MMDD = (DDMM % 100) * 100 + (DDMM / 100);
+              // set GPS ready flag
+              state |= STATE_GPS_READY;
+            }
+        }
+#endif
+    }
+#endif
+#if ENABLE_DATA_LOG
+    uint16_t initSD()
+    {
+        state &= ~STATE_SD_READY;
+        pinMode(SS, OUTPUT);
+
+        Sd2Card card;
+        uint32_t volumesize = 0;
+        if (card.init(SPI_HALF_SPEED, SD_CS_PIN)) {
+            SdVolume volume;
+            if (volume.init(card)) {
+              volumesize = volume.blocksPerCluster();
+              volumesize >>= 1; // 512 bytes per block
+              volumesize *= volume.clusterCount();
+              volumesize /= 1000;
+            }
+        }
+        if (SD.begin(SD_CS_PIN)) {
+          state |= STATE_SD_READY;
+          return volumesize; 
+        } else {
+          return 0;
+        }
+    }
+    void flushData()
+    {
+      // flush SD data every 1KB
+        byte dataSizeKB = dataSize >> 10;
+        if (dataSizeKB != lastFileSize) {
+            flushFile();
+            lastFileSize = dataSizeKB;
+#if MAX_LOG_FILE_SIZE
+            if (dataSize >= 1024L * MAX_LOG_FILE_SIZE) {
+              closeFile();
+              state &= ~STATE_FILE_READY;
+            }
+#endif
+        }
+    }
+#endif
+    void reconnect()
+    {
+        Serial.println("Reconnecting");
+        // try to re-connect to OBD
+        if (init()) return;
+#if ENABLE_DATA_LOG
+        closeFile();
+#endif
+        // turn off GPS power
+#if USE_GPS
+        initGPS(0);
+#endif
+        state &= ~(STATE_OBD_READY | STATE_GPS_READY);
+        Serial.println("Standby");
+        // put OBD chips into low power mode
+        enterLowPowerMode();
+        
+        // calibrate MEMS for several seconds
+        for (;;) {
+#if USE_MPU6050 || USE_MPU9250
+          accSum[0] = 0;
+          accSum[1] = 0;
+          accSum[2] = 0;
+          for (accCount = 0; accCount < 10; ) {
+            readMEMS();
+            delay(30);
+          }
+          // calculate relative movement
+          unsigned long motion = 0;
+          for (byte i = 0; i < 3; i++) {
+            long n = accSum[i] / accCount - accCal[i];
+            motion += n * n;
+          }
+          // check movement
+          if (motion > START_MOTION_THRESHOLD) {
+            Serial.println(motion);
+            // try OBD reading
+#endif
+            leaveLowPowerMode();
+            if (init()) {
+              // OBD is accessible
+              break;
+            }
+            enterLowPowerMode();
+#if USE_MPU6050 || USE_MPU9250
+            // calibrate MEMS again in case the device posture changed
+            calibrateMEMS();
+          }
+#endif
+        }
+#ifdef ARDUINO_ARCH_AVR
+        // reset device
+        void(* resetFunc) (void) = 0; //declare reset function at address 0
+        resetFunc();
+#else
+        setup();
+#endif
+    }
+#if USE_MPU6050 || USE_MPU9250
+    void calibrateMEMS()
+    {
+		// get accelerometer calibration reference data
+		if (accCount) {
+			accCal[0] = accSum[0] / accCount;
+			accCal[1] = accSum[1] / accCount;
+			accCal[2] = accSum[2] / accCount;
+		}
+	}
+    void readMEMS()
+    {
+        // load accelerometer and temperature data
+        int16_t acc[3] = {0};
+        int16_t temp; // device temperature (in 0.1 celcius degree)
+        memsRead(acc, 0, 0, &temp);
+        if (accCount >= 250) {
+          accSum[0] >>= 1;
+          accSum[1] >>= 1;
+          accSum[2] >>= 1;
+          accCount >>= 1;
+        }
+        accSum[0] += acc[0];
+        accSum[1] += acc[1];
+        accSum[2] += acc[2];
+        accCount++;
+        deviceTemp = temp / 10;
+    }
+ #endif
+    byte state;
+};
+
+static COBDLogger one;
+
+void setup()
+{
+	Serial.begin(115200);
+    one.initSender();
+    one.setup();
+}
+
+void loop()
+{
+#if ENABLE_DATA_LOG
+    if (!(one.state & STATE_FILE_READY) && (one.state & STATE_SD_READY)) {
+      if (one.state & STATE_GPS_FOUND) {
+        // GPS connected
+        Serial.print("File ");
+        if (one.state & STATE_GPS_READY) {
+          uint32_t dateTime = (uint32_t)MMDD * 10000 + UTC / 10000;
+          if (one.openFile(dateTime) != 0) {
+            Serial.println(dateTime);
+            MMDD = 0;
+            one.state |= STATE_FILE_READY;
+          } else {
+            Serial.println("File error");
+          }
+        }
+      } else {
+        // no GPS connected 
+        int index = one.openFile(0);
+        if (index != 0) {
+          one.state |= STATE_FILE_READY;
+          Serial.println(index);
+        } else {
+          Serial.println("File error");
+        }
+      }
+    }
+#endif
+    if (one.state & STATE_OBD_READY) {
+        byte pids[]= {0, PID_RPM, PID_SPEED, PID_THROTTLE, PID_ENGINE_LOAD};
+        byte pids2[] = {PID_COOLANT_TEMP, PID_INTAKE_TEMP, PID_DISTANCE};
+        int values[sizeof(pids)];
+        static byte index2 = 0;
+        pids[0] = pids2[index2 = (index2 + 1) % sizeof(pids2)];
+        // read multiple OBD-II PIDs
+        if (one.readPID(pids, sizeof(pids), values) == sizeof(pids)) {
+          one.dataTime = millis();
+          for (byte n = 0; n < sizeof(pids); n++) {
+            one.logData((uint16_t)pids[n] | 0x100, values[n]);
+          }
+        }
+        if (one.errors >= 10) {
+            one.reconnect();
+        }
+    } else {
+      if (!OBD_ATTEMPT_TIME || millis() < OBD_ATTEMPT_TIME * 1000) {
+        if (one.init()) {
+            one.state |= STATE_OBD_READY;
+        }
+      }
+    }
+    
+    // log battery voltage (from voltmeter), data in 0.01v
+    int v = one.getVoltage() * 100;
+    one.dataTime = millis();
+    one.logData(PID_BATTERY_VOLTAGE, v);
+
+#if USE_MPU6050 || USE_MPU9250
+    if ((one.state & STATE_MEMS_READY) && accCount) {
+       // log the loaded MEMS data
+      one.logData(PID_ACC, accSum[0] / accCount / ACC_DATA_RATIO, accSum[1] / accCount / ACC_DATA_RATIO, accSum[2] / accCount / ACC_DATA_RATIO);
+    }
+#endif
+#if USE_GPS
+    if (one.state & STATE_GPS_FOUND) {
+      one.logGPSData();
+    }
+#endif
+
+#if ENABLE_DATA_LOG
+    uint32_t logsize = sdfile.size();
+    if (logsize > 0) {
+      one.flushData();
+      Serial.print(sdfile.size());
+      Serial.println(" bytes");
+    }
+#endif
+}