Revision 5:ded8fe5991a2, committed 2017-09-05

Comitter:

miaotwilio

Date:

Tue Sep 05 15:00:17 2017 +0000

Parent:

4:f4e8f0fca631

Child:

6:4a25f3b9caef

Commit message:

add TripDataReader

Changed in this revision

--- /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;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TripDataReader.hpp	Tue Sep 05 15:00:17 2017 +0000
@@ -0,0 +1,62 @@
+#ifndef TRIP_DATA_READER
+#define TRIP_DATA_READER
+
+#include "mbed.h"
+#include "mtsas.h"
+
+class TripDataReader {
+    static const int HARD_BRAKE_THRESHOLD = 10;
+
+public:
+    struct TripData {
+        int distance;
+        int fuel;
+        int runtime;
+
+        float averageSpeed;
+        float minimumThrottle;
+        float averageThrottle;
+        float maximumThrottle;
+        int hardBrakeCount;
+    };
+    
+public:
+    TripDataReader(MTSSerial& obd_, DigitalOut& ledOBD_);
+
+public:
+    int init();
+
+    void sample();
+
+    void resetAverageWindow();
+
+    TripData getTripData();
+
+private:
+    int readPin(int PID, int dataBytes, uint8_t *data);
+
+private:
+    struct TripDataHistory {
+        int initialDistance;
+        int calls;
+        int speed;
+        int minT;
+        int avgT;
+        int maxT;
+        int speedSamples;
+        int throttleSamples;
+        int speedHist[10];
+        int speedHistPtr;
+        unsigned int brakeEventCount[32];
+        int hardBrakeCount;
+        bool hardBrakeState;
+    };
+
+private:
+    MTSSerial& obd;
+    DigitalOut& ledOBD;
+    TripDataHistory tripDataHistory;
+};
+
+#endif
+    
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/config.hpp	Tue Sep 05 15:00:17 2017 +0000
@@ -0,0 +1,11 @@
+#ifndef DRAGONFLY_MQTT_CONFIG_HPP
+#define DRAGONFLY_MQTT_CONFIG_HPP
+
+#define MQTT_GATEWAY_HOST "mqtt-sync.us1.twilio.com"
+#define MQTT_GATEWAY_PORT 8883
+
+#define VEHICLE_ID "XXXXX"
+#define VEHICLE_KEY "KYxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx"
+#define VEHICLE_SECRET "xxxxxxxxxxxxxxxxxxxxxxx"
+
+#endif
\ No newline at end of file

--- a/main.cpp	Fri Aug 18 08:40:35 2017 +0000
+++ b/main.cpp	Tue Sep 05 15:00:17 2017 +0000
@@ -1,4 +1,5 @@
 #include "MTSCellularManager.hpp"
+#include "TripDataReader.hpp"
 #include "TlsMQTTClient.hpp"
 #include "Certificates.h"
 #include "config.hpp"
@@ -10,15 +11,27 @@
 // BC_NCE = 0 enables the battery charger
 // BC_NCE = 1 disables the battery charger
 DigitalOut bc_nce(PB_2);
+
 DigitalOut ledMQTTYield(D5);
+
 DigitalOut ledGPS(D8);
 
-static const int VEHICLE_DATA_POLLING_PERIOD_MS = 5000;
+MTSSerial obd(D1, D0);
+DigitalOut ledOBD(D6);
+
+static const int VEHICLE_DATA_SAMPLING_PERIOD_MS = 50;
+static Ticker vehicleDataSamplingTicker;
+static TripDataReader* pTripDataReader = NULL;
+static void vehicleDataSamplingCallback();
+
+static const int VEHICLE_DATA_REPORTING_PERIOD_MS = 3000;
 
 static bool exitCmd = false;
 
-static void sendVehicleData(MTSCellularManager::GPSStatus& gpsStatus);
-static void test2Handler(MQTT::MessageData& data);
+static void sendVehicleData(const MTSCellularManager::GPSStatus& gpsStatus,
+                            const TripDataReader::TripData& tripData);
+
+//static void test2Handler(MQTT::MessageData& data);
 
 int main() {
     // Disable the battery charger unless a battery is attached.
@@ -52,18 +65,37 @@
         logInfo("GPS Initialized");
     }
 
+    TripDataReader tripDataReader(obd, ledOBD);
+    pTripDataReader = &tripDataReader;
+    while (true) {
+        logInfo("Initializing OBD");
+        int r = tripDataReader.init();
+        logInfo("OBD Initialization result: %d", r);
+        if (0 == r) {
+            logInfo("Initializing OBD sampling ticker");
+            vehicleDataSamplingTicker.attach(
+                        &vehicleDataSamplingCallback,
+                        VEHICLE_DATA_SAMPLING_PERIOD_MS / 1000.);
+            logInfo("OBD sampling ticker initialized");
+            break;
+        }
+        wait_ms(100);
+    }
+
     logInfo("Initializing CyaSSL");
     CyaSSL_Init();
 
     while (!exitCmd) {
+        wait_ms(VEHICLE_DATA_REPORTING_PERIOD_MS);
+        TripDataReader::TripData tripData = tripDataReader.getTripData();
+        tripDataReader.resetAverageWindow();
         MTSCellularManager::GPSStatus gpsStatus = cellularManager.gpsPollStatus();
         if (gpsStatus.success) {
             ledGPS = 0;
-            sendVehicleData(gpsStatus);
+            sendVehicleData(gpsStatus, tripData);
         } else {
             ledGPS = 1;
         }
-        wait_ms(VEHICLE_DATA_POLLING_PERIOD_MS);
     }
 
     logInfo("Cleaning up CyaSSL");
@@ -77,7 +109,12 @@
     return 0;
 }
 
-static void sendVehicleData(MTSCellularManager::GPSStatus& gpsStatus) {
+static void vehicleDataSamplingCallback() {
+    pTripDataReader->sample();
+}
+
+static void sendVehicleData(const MTSCellularManager::GPSStatus& gpsStatus,
+                            const TripDataReader::TripData& tripData) {
     ledMQTTYield = 0;
 
     logInfo("Connecting MQTT Client");
@@ -94,19 +131,27 @@
         logInfo("MQTT connected");
 
         sprintf(buf, "{"
-            "\"driver_id\": 1,"
-            "\"runtime\": 10,"
-            "\"miles\": 10,"
+            "\"driver_id\": %s,"
+            "\"runtime\": %d,"
+            "\"miles\": %d,"
             "\"speed\": %f,"
-            "\"minT\": 40,"
-            "\"maxT\": 60,"
-            "\"avgT\": 50,"
-            "\"fuel\": 50,"
-            "\"brake\": 0,"
+            "\"minT\": %f,"
+            "\"maxT\": %f,"
+            "\"avgT\": %f,"
+            "\"fuel\": %d,"
+            "\"brake\": %d,"
             "\"lat\": %lf,"
             "\"lon\": %lf"
             "}",
-                gpsStatus.speedVal,
+                VEHICLE_ID,
+                tripData.runtime,
+                tripData.distance,
+                tripData.averageSpeed,
+                tripData.minimumThrottle,
+                tripData.maximumThrottle,
+                tripData.averageThrottle,
+                tripData.fuel,
+                tripData.hardBrakeCount,
                 gpsStatus.latitudeVal,
                 gpsStatus.longitudeVal);
         message.qos = MQTT::QOS1;