Updated MEMS reading code

3 files changed, 28 insertions, 56 deletions

diff --git a/megaloggerHD/config.h b/megaloggerHD/config.h
index 553222c..46a0b88 100644
--- a/megaloggerHD/config.h
+++ b/megaloggerHD/config.h
@@ -4,8 +4,8 @@
 /**************************************
 * OBD-II Adapter options
 **************************************/
-//#define OBD_ADAPTER_I2C
-#define OBD_ADAPTER_UART
+#define OBD_ADAPTER_I2C
+//#define OBD_ADAPTER_UART
 #define OBD_PROTOCOL PROTO_AUTO
 
 /**************************************
@@ -48,11 +48,6 @@
 #define GYRO_DATA_RATIO 256
 
 /**************************************
-* Timeout/interval options
-**************************************/
-#define ACC_DATA_INTERVAL 200 /* ms */
-
-/**************************************
 * LCD module (uncomment only one)
 **************************************/
 LCD_R61581 lcd; /* 3.5" CTE35IPS/R61581 based LCD */
diff --git a/megaloggerHD/datalogger.h b/megaloggerHD/datalogger.h
index e1a6925..8d241d6 100644
--- a/megaloggerHD/datalogger.h
+++ b/megaloggerHD/datalogger.h
@@ -195,13 +195,13 @@ public:
 #endif
         record(buf, len);
     }
-    void logData(uint16_t pid, int value1, int value2, int value3)
+    void logData(uint16_t pid, int values[])
     {
         char buf[24];
         byte n = translatePIDName(pid, buf);
-        byte len = sprintf(buf + n, "%d,%d,%d", value1, value2, value3) + n;
+        byte len = sprintf(buf + n, "%d,%d,%d", values[0], values[1], values[2]) + n;
 #if STREAM_FORMAT == FORMAT_BIN
-        LOG_DATA_COMM ld = {dataTime, pid, 3, 0, {value1, value2, value3}};
+        LOG_DATA_COMM ld = {dataTime, pid, 3, 0, {values[0], values[1], values[2]}};
         ld.checksum = getChecksum((char*)&ld, 20);
         dispatch((const char*)&ld, 20);
 #else
diff --git a/megaloggerHD/megaloggerHD.ino b/megaloggerHD/megaloggerHD.ino
index efc5f71..9806885 100644
--- a/megaloggerHD/megaloggerHD.ino
+++ b/megaloggerHD/megaloggerHD.ino
@@ -13,10 +13,6 @@
 #include <MultiLCD.h>
 #include <TinyGPS.h>
 #include "config.h"
-#ifdef OBD_ADAPTER_I2C
-#include <I2Cdev.h>
-#include <MPU9150.h>
-#endif
 #if ENABLE_DATA_LOG
 #include <SD.h>
 #endif
@@ -46,16 +42,11 @@ static uint32_t startTime = 0;
 static uint16_t lastSpeed = 0;
 static uint32_t lastSpeedTime = 0;
 static uint32_t gpsDate = 0;
-static uint32_t lastMemsDataTime = 0;
 #if USE_GPS
 static uint32_t lastGPSDataTime = 0;
 static int gpsSpeed = -1;
 #endif
 
-#ifdef OBD_ADAPTER_I2C
-MPU6050 accelgyro;
-#endif
-
 byte state = 0;
 
 void processMEMS();
@@ -417,59 +408,48 @@ void processGPS()
 
 void processMEMS()
 {
-    int ax = 0, ay = 0, az = 0;
-    int gx = 0, gy = 0, gz = 0;
+    int acc[3];
+    int gyro[3];
 
-    if (logger.dataTime - lastMemsDataTime < ACC_DATA_INTERVAL) {
-        return;
-    }
-
-#ifdef OBD_ADAPTER_I2C
-    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
-#else
-    obd.readAccel(ax, ay, az);
-    obd.readGyro(gx, gy, gz);
-#endif
+    if (!obd.memsRead(acc, gyro)) return;
 
     logger.dataTime = millis();
 
-    ax /= ACC_DATA_RATIO;
-    ay /= ACC_DATA_RATIO;
-    az /= ACC_DATA_RATIO;
-    gx /= GYRO_DATA_RATIO;
-    gy /= GYRO_DATA_RATIO;
-    gz /= GYRO_DATA_RATIO;
+    acc[0] /= ACC_DATA_RATIO;
+    acc[1] /= ACC_DATA_RATIO;
+    acc[2] /= ACC_DATA_RATIO;
+    gyro[0] /= GYRO_DATA_RATIO;
+    gyro[1] /= GYRO_DATA_RATIO;
+    gyro[2] /= GYRO_DATA_RATIO;
 
     // display MEMS data
     lcd.setFontSize(FONT_SIZE_MEDIUM);
     lcd.setCursor(362, 5);
-    setColorByValue(ax, 50, 100, 200);
-    lcd.print(ax);
-    setColorByValue(ay, 50, 100, 200);
+    setColorByValue(acc[0], 50, 100, 200);
+    lcd.print(acc[0]);
+    setColorByValue(acc[1], 50, 100, 200);
     lcd.write('/');
-    lcd.print(ay);
-    setColorByValue(az, 50, 100, 200);
+    lcd.print(acc[1]);
+    setColorByValue(acc[2], 50, 100, 200);
     lcd.write('/');
-    lcd.print(az);
-    Serial.println(az);
+    lcd.print(acc[2]);
+    Serial.println(acc[2]);
     lcd.printSpace(8);
 
     // display gyro data
     lcd.setCursor(374, 10);
     lcd.setColor(RGB16_WHITE);
-    lcd.print(gx);
+    lcd.print(gyro[0]);
     lcd.write('/');
-    lcd.print(gy);
+    lcd.print(gyro[1]);
     lcd.write('/');
-    lcd.print(gz);
+    lcd.print(gyro[2]);
     lcd.printSpace(8);
 
     // log x/y/z of accelerometer
-    logger.logData(PID_ACC, ax, ay, az);
+    logger.logData(PID_ACC, acc);
     // log x/y/z of gyro meter
-    logger.logData(PID_GYRO, gx, gy, gz);
-
-    lastMemsDataTime = logger.dataTime;
+    logger.logData(PID_GYRO, gyro);
 }
 
 void logOBDData(byte pid, int value)
@@ -693,12 +673,9 @@ void setup()
 
 #ifdef OBD_ADAPTER_I2C
     Wire.begin();
-    accelgyro.initialize();
-    if (accelgyro.testConnection()) state |= STATE_MEMS_READY;
-#else
-    if (obd.version > 10)
-      state |= STATE_MEMS_READY;
 #endif
+    if (obd.memsInit())
+      state |= STATE_MEMS_READY;
 
     showStates();