Diff: TripDataReader.cpp

Revision:

5:ded8fe5991a2

Child:

6:4a25f3b9caef

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TripDataReader.cpp	Tue Sep 05 15:00:17 2017 +0000
@@ -0,0 +1,198 @@
+#include "TripDataReader.hpp"
+
+#include <string.h>
+
+
+TripDataReader::TripDataReader(MTSSerial& obd_, DigitalOut& ledOBD_):
+    obd(obd_),
+    ledOBD(ledOBD_) {
+    memset(&tripDataHistory, 0, sizeof(tripDataHistory));
+}
+
+int TripDataReader::init() {
+    char out[64], in[64];
+    int i;
+    obd.format(8, Serial::None, 1);
+    obd.baud(38400);
+
+    sprintf(out, "ATZ\r");
+    logInfo("sending command %s", out);
+    obd.write(out, strlen(out));
+    wait(1);
+    i = obd.readable();
+    logInfo("OBD: %d bytes readable", i);
+    obd.read(in, i);
+    in[i]=0;
+    logInfo("OBD: in=%s", in);
+    if( strncmp("OBDUART v", in, 9) != 0) {
+        logError("OBD: unexpected response to ATZ command: %s", in);
+        return -1;
+    }
+
+    sprintf(out, "ATSP0\r");
+    logInfo("sending command %s", out);
+    obd.write(out, strlen(out));
+    wait(1);
+    i = obd.readable();
+    logInfo("OBD: %d bytes readable", i);
+    obd.read(in, i);
+    in[i]=0;
+    logInfo("OBD: in=%s", in);
+    if( strncmp("OK", in, 2) != 0) {
+        logError("OBD: unexpected response to ATSP0 command: %s", in);
+        return -1;
+    }
+
+    ledOBD = 0;
+    return 0;
+}
+
+void TripDataReader::sample() {
+    int speedDiff;
+
+    //logInfo("TripDataReader::sample: calls=%d", tripDataHistory.calls);
+
+    if( (tripDataHistory.calls++ & 0x1) == 0 ) {
+        // on even samples we will process speed readings and request throttle readings
+        char buf[64];
+        uint8_t data[8];
+        int i = obd.readable(); // how much is available to read
+        obd.read(buf, i);
+        buf[i] = 0;  // null terminate the string
+        if(i >= 8) {
+            //logInfo("OBD: in=%s", buf);
+            for(i = 0; i < 3; i++)
+                sscanf(buf+i*3, "%02hhX", data + i);
+            if(data[0] != 0x41 || data[1] != 0x0D) {
+                logError("error reading PID 0x0D: %s", buf);
+            } else {
+                tripDataHistory.speed += data[2];
+                tripDataHistory.speedSamples++;
+                speedDiff = tripDataHistory.speedHist[tripDataHistory.speedHistPtr] - data[2];
+                if(tripDataHistory.speedHist[tripDataHistory.speedHistPtr] - data[2] > HARD_BRAKE_THRESHOLD) {
+                    if(!tripDataHistory.hardBrakeState) {
+                        tripDataHistory.hardBrakeCount++;
+                        tripDataHistory.hardBrakeState = true;
+                    }
+                } else {
+                    tripDataHistory.hardBrakeState = false;
+                }
+                tripDataHistory.speedHistPtr = (tripDataHistory.speedHistPtr + 1) % 10;
+                tripDataHistory.speedHist[tripDataHistory.speedHistPtr] = data[2];
+                // this is just for counting to see what braking looks like during a drive
+                if(speedDiff > 0) {
+                    if(speedDiff > 31) speedDiff = 31;
+                    tripDataHistory.brakeEventCount[speedDiff]++;
+                }
+            }
+        } else {
+            //logError("not enough bytes reading PID 0x0D");
+        }
+        // now request the throttle to be processed next time
+        obd.write("0111\r", 5);
+    } else {
+        // on odd samples we will process throttle readings and request speed readings
+        char buf[64];
+        uint8_t data[8];
+        int i = obd.readable(); // how much is available to read
+        obd.read(buf, i);
+        buf[i] = 0;  // null terminate the string
+        if(i >= 8) {
+            //logInfo("OBD: in=%s", buf);
+            for(i = 0; i < 3; i++)
+                sscanf(buf+i*3, "%02hhX", data + i);
+            if(data[0] != 0x41 || data[1] != 0x11) {
+                logError("error reading PID 0x11: %s",buf);
+            } else {
+                if(data[2] < tripDataHistory.minT) tripDataHistory.minT = data[2];
+                if(data[2] > tripDataHistory.maxT) tripDataHistory.maxT = data[2];
+                tripDataHistory.avgT += data[2];
+                tripDataHistory.throttleSamples++;
+            }
+        } else {
+            //logError("not enough bytes reading PID 0x11");
+        }
+        // now request the throttle to be processed next time
+        obd.write("010D\r", 5);
+    }
+}
+
+void TripDataReader::resetAverageWindow() {
+    tripDataHistory.speed = 0;
+    tripDataHistory.speedSamples = 0;
+    tripDataHistory.minT = 255;
+    tripDataHistory.avgT = 0;
+    tripDataHistory.maxT = 0;
+    tripDataHistory.throttleSamples = 0;
+}
+
+TripDataReader::TripData TripDataReader::getTripData() {
+    uint8_t obdData[16];
+    TripData d;
+
+    d.averageSpeed = (float)tripDataHistory.speed*0.62137119f/(float)tripDataHistory.speedSamples;
+    d.minimumThrottle = (float)tripDataHistory.minT*0.39215686f;
+    d.averageThrottle = (float)tripDataHistory.avgT*0.39215686f/(float)tripDataHistory.throttleSamples;
+    d.maximumThrottle = (float)tripDataHistory.maxT*0.39215686f;
+    d.hardBrakeCount = tripDataHistory.hardBrakeCount;
+
+    {
+        // Read distance since Malfunction Indicator Lamp reset as a way to get miles traveled
+        int result = readPin(0x31, 2, obdData);
+        if(result != 4 || obdData[0] != 0x41 || obdData[1] != 0x31) {
+            logError("error reading PID 0x31");
+        } else {
+            if(tripDataHistory.initialDistance <= 0)
+                tripDataHistory.initialDistance =  obdData[2]*256 + obdData[3];
+            d.distance = obdData[2]*256 + obdData[3] - tripDataHistory.initialDistance;
+            logInfo("initialDistance=%d, distance=%d", tripDataHistory.initialDistance, d.distance);
+        }
+    }
+
+    {
+        // Get the fuel take level sensor percentage
+        int result = readPin(0x2F, 1, obdData);
+        if(result != 3 || obdData[0] != 0x41 || obdData[1] != 0x2F) {
+            logError("error reading PID 0x2F");
+        } else {
+            d.fuel = obdData[2];
+            logInfo("fuel=%d", d.fuel);
+        }
+    }
+    {
+        // Get the engine on time since start
+        int result = readPin(0x1F, 2, obdData);
+        if(result != 4 || obdData[0] != 0x41 || obdData[1] != 0x1F) {
+            logError("error reading PID 0x1F");
+        } else {
+            d.runtime = obdData[2]*256 + obdData[3];
+            logInfo("runtime=%d", d.runtime);
+        }
+    }
+
+    return d;
+}
+
+int TripDataReader::readPin(int PID, int dataBytes, uint8_t *data) {
+    char out[64], in[64];
+    int i = 0;
+    sprintf(out, "01%02X\r", PID);
+    //logInfo("sending PID request %s", out);
+    obd.write(out, strlen(out));
+    wait(.1);
+    i = obd.readable();
+    //logInfo("OBD: %d bytes readable", i);
+    obd.read(in, i);
+    in[i]=0;
+    //logInfo("OBD: in=%s", in);
+
+    if(i < (dataBytes * 3 + 5) )
+        return -1;
+    for(i = 0; i < (dataBytes+2); i++) {
+        sscanf(in+i*3, "%02hhX", data + i);
+    }
+    /*for(i = 0; i < (dataBytes+2); i++) {
+        logInfo("byte[%d]=%d", i, data[i]);
+    }*/
+    return i;
+}