File content as of revision 5:ded8fe5991a2:

#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;
}