miao zhicheng / Mbed 2 deprecated DragonflyMQTT

Using MQTT on Dragonfly

Dependencies:   MQTT mbed mtsas

TripDataReader.cpp@8:f8a346582627, 2017-09-14 (annotated)

Committer:
miaotwilio
Date:
Thu Sep 14 08:14:18 2017 +0000
Revision:
8:f8a346582627
Parent:
6:4a25f3b9caef 
more LEDs

Who changed what in which revision?

UserRevisionLine numberNew contents of line
miaotwilio 5:ded8fe5991a2 1 #include "TripDataReader.hpp"
miaotwilio 5:ded8fe5991a2 2
miaotwilio 5:ded8fe5991a2 3 #include <string.h>
miaotwilio 5:ded8fe5991a2 4
miaotwilio 5:ded8fe5991a2 5
miaotwilio 5:ded8fe5991a2 6 TripDataReader::TripDataReader(MTSSerial& obd_, DigitalOut& ledOBD_):
miaotwilio 5:ded8fe5991a2 7 obd(obd_),
miaotwilio 5:ded8fe5991a2 8 ledOBD(ledOBD_) {
miaotwilio 5:ded8fe5991a2 9 memset(&tripDataHistory, 0, sizeof(tripDataHistory));
miaotwilio 5:ded8fe5991a2 10 }
miaotwilio 5:ded8fe5991a2 11
miaotwilio 5:ded8fe5991a2 12 int TripDataReader::init() {
miaotwilio 5:ded8fe5991a2 13 char out[64], in[64];
miaotwilio 5:ded8fe5991a2 14 int i;
miaotwilio 5:ded8fe5991a2 15 obd.format(8, Serial::None, 1);
miaotwilio 5:ded8fe5991a2 16 obd.baud(38400);
miaotwilio 5:ded8fe5991a2 17
miaotwilio 5:ded8fe5991a2 18 sprintf(out, "ATZ\r");
miaotwilio 5:ded8fe5991a2 19 logInfo("sending command %s", out);
miaotwilio 5:ded8fe5991a2 20 obd.write(out, strlen(out));
miaotwilio 5:ded8fe5991a2 21 wait(1);
miaotwilio 5:ded8fe5991a2 22 i = obd.readable();
miaotwilio 5:ded8fe5991a2 23 logInfo("OBD: %d bytes readable", i);
miaotwilio 5:ded8fe5991a2 24 obd.read(in, i);
miaotwilio 5:ded8fe5991a2 25 in[i]=0;
miaotwilio 5:ded8fe5991a2 26 logInfo("OBD: in=%s", in);
miaotwilio 5:ded8fe5991a2 27 if( strncmp("OBDUART v", in, 9) != 0) {
miaotwilio 5:ded8fe5991a2 28 logError("OBD: unexpected response to ATZ command: %s", in);
miaotwilio 5:ded8fe5991a2 29 return -1;
miaotwilio 5:ded8fe5991a2 30 }
miaotwilio 5:ded8fe5991a2 31
miaotwilio 5:ded8fe5991a2 32 sprintf(out, "ATSP0\r");
miaotwilio 5:ded8fe5991a2 33 logInfo("sending command %s", out);
miaotwilio 5:ded8fe5991a2 34 obd.write(out, strlen(out));
miaotwilio 5:ded8fe5991a2 35 wait(1);
miaotwilio 5:ded8fe5991a2 36 i = obd.readable();
miaotwilio 5:ded8fe5991a2 37 logInfo("OBD: %d bytes readable", i);
miaotwilio 5:ded8fe5991a2 38 obd.read(in, i);
miaotwilio 5:ded8fe5991a2 39 in[i]=0;
miaotwilio 5:ded8fe5991a2 40 logInfo("OBD: in=%s", in);
miaotwilio 5:ded8fe5991a2 41 if( strncmp("OK", in, 2) != 0) {
miaotwilio 5:ded8fe5991a2 42 logError("OBD: unexpected response to ATSP0 command: %s", in);
miaotwilio 5:ded8fe5991a2 43 return -1;
miaotwilio 5:ded8fe5991a2 44 }
miaotwilio 5:ded8fe5991a2 45
miaotwilio 5:ded8fe5991a2 46 ledOBD = 0;
miaotwilio 5:ded8fe5991a2 47 return 0;
miaotwilio 5:ded8fe5991a2 48 }
miaotwilio 5:ded8fe5991a2 49
miaotwilio 5:ded8fe5991a2 50 void TripDataReader::sample() {
miaotwilio 5:ded8fe5991a2 51 int speedDiff;
miaotwilio 5:ded8fe5991a2 52
miaotwilio 5:ded8fe5991a2 53 //logInfo("TripDataReader::sample: calls=%d", tripDataHistory.calls);
miaotwilio 5:ded8fe5991a2 54
miaotwilio 5:ded8fe5991a2 55 if( (tripDataHistory.calls++ & 0x1) == 0 ) {
miaotwilio 5:ded8fe5991a2 56 // on even samples we will process speed readings and request throttle readings
miaotwilio 5:ded8fe5991a2 57 char buf[64];
miaotwilio 5:ded8fe5991a2 58 uint8_t data[8];
miaotwilio 5:ded8fe5991a2 59 int i = obd.readable(); // how much is available to read
miaotwilio 5:ded8fe5991a2 60 obd.read(buf, i);
miaotwilio 5:ded8fe5991a2 61 buf[i] = 0; // null terminate the string
miaotwilio 5:ded8fe5991a2 62 if(i >= 8) {
miaotwilio 5:ded8fe5991a2 63 //logInfo("OBD: in=%s", buf);
miaotwilio 5:ded8fe5991a2 64 for(i = 0; i < 3; i++)
miaotwilio 5:ded8fe5991a2 65 sscanf(buf+i*3, "%02hhX", data + i);
miaotwilio 5:ded8fe5991a2 66 if(data[0] != 0x41 || data[1] != 0x0D) {
miaotwilio 5:ded8fe5991a2 67 logError("error reading PID 0x0D: %s", buf);
miaotwilio 5:ded8fe5991a2 68 } else {
miaotwilio 5:ded8fe5991a2 69 tripDataHistory.speed += data[2];
miaotwilio 5:ded8fe5991a2 70 tripDataHistory.speedSamples++;
miaotwilio 5:ded8fe5991a2 71 speedDiff = tripDataHistory.speedHist[tripDataHistory.speedHistPtr] - data[2];
miaotwilio 5:ded8fe5991a2 72 if(tripDataHistory.speedHist[tripDataHistory.speedHistPtr] - data[2] > HARD_BRAKE_THRESHOLD) {
miaotwilio 5:ded8fe5991a2 73 if(!tripDataHistory.hardBrakeState) {
miaotwilio 5:ded8fe5991a2 74 tripDataHistory.hardBrakeCount++;
miaotwilio 5:ded8fe5991a2 75 tripDataHistory.hardBrakeState = true;
miaotwilio 5:ded8fe5991a2 76 }
miaotwilio 5:ded8fe5991a2 77 } else {
miaotwilio 5:ded8fe5991a2 78 tripDataHistory.hardBrakeState = false;
miaotwilio 5:ded8fe5991a2 79 }
miaotwilio 5:ded8fe5991a2 80 tripDataHistory.speedHistPtr = (tripDataHistory.speedHistPtr + 1) % 10;
miaotwilio 5:ded8fe5991a2 81 tripDataHistory.speedHist[tripDataHistory.speedHistPtr] = data[2];
miaotwilio 5:ded8fe5991a2 82 // this is just for counting to see what braking looks like during a drive
miaotwilio 5:ded8fe5991a2 83 if(speedDiff > 0) {
miaotwilio 5:ded8fe5991a2 84 if(speedDiff > 31) speedDiff = 31;
miaotwilio 5:ded8fe5991a2 85 tripDataHistory.brakeEventCount[speedDiff]++;
miaotwilio 5:ded8fe5991a2 86 }
miaotwilio 5:ded8fe5991a2 87 }
miaotwilio 5:ded8fe5991a2 88 } else {
miaotwilio 5:ded8fe5991a2 89 //logError("not enough bytes reading PID 0x0D");
miaotwilio 5:ded8fe5991a2 90 }
miaotwilio 5:ded8fe5991a2 91 // now request the throttle to be processed next time
miaotwilio 5:ded8fe5991a2 92 obd.write("0111\r", 5);
miaotwilio 5:ded8fe5991a2 93 } else {
miaotwilio 5:ded8fe5991a2 94 // on odd samples we will process throttle readings and request speed readings
miaotwilio 5:ded8fe5991a2 95 char buf[64];
miaotwilio 5:ded8fe5991a2 96 uint8_t data[8];
miaotwilio 5:ded8fe5991a2 97 int i = obd.readable(); // how much is available to read
miaotwilio 5:ded8fe5991a2 98 obd.read(buf, i);
miaotwilio 5:ded8fe5991a2 99 buf[i] = 0; // null terminate the string
miaotwilio 5:ded8fe5991a2 100 if(i >= 8) {
miaotwilio 5:ded8fe5991a2 101 //logInfo("OBD: in=%s", buf);
miaotwilio 5:ded8fe5991a2 102 for(i = 0; i < 3; i++)
miaotwilio 5:ded8fe5991a2 103 sscanf(buf+i*3, "%02hhX", data + i);
miaotwilio 5:ded8fe5991a2 104 if(data[0] != 0x41 || data[1] != 0x11) {
miaotwilio 5:ded8fe5991a2 105 logError("error reading PID 0x11: %s",buf);
miaotwilio 5:ded8fe5991a2 106 } else {
miaotwilio 5:ded8fe5991a2 107 if(data[2] < tripDataHistory.minT) tripDataHistory.minT = data[2];
miaotwilio 5:ded8fe5991a2 108 if(data[2] > tripDataHistory.maxT) tripDataHistory.maxT = data[2];
miaotwilio 5:ded8fe5991a2 109 tripDataHistory.avgT += data[2];
miaotwilio 5:ded8fe5991a2 110 tripDataHistory.throttleSamples++;
miaotwilio 5:ded8fe5991a2 111 }
miaotwilio 5:ded8fe5991a2 112 } else {
miaotwilio 5:ded8fe5991a2 113 //logError("not enough bytes reading PID 0x11");
miaotwilio 5:ded8fe5991a2 114 }
miaotwilio 5:ded8fe5991a2 115 // now request the throttle to be processed next time
miaotwilio 5:ded8fe5991a2 116 obd.write("010D\r", 5);
miaotwilio 5:ded8fe5991a2 117 }
miaotwilio 5:ded8fe5991a2 118 }
miaotwilio 5:ded8fe5991a2 119
miaotwilio 5:ded8fe5991a2 120 void TripDataReader::resetAverageWindow() {
miaotwilio 5:ded8fe5991a2 121 tripDataHistory.speed = 0;
miaotwilio 5:ded8fe5991a2 122 tripDataHistory.speedSamples = 0;
miaotwilio 5:ded8fe5991a2 123 tripDataHistory.minT = 255;
miaotwilio 5:ded8fe5991a2 124 tripDataHistory.avgT = 0;
miaotwilio 5:ded8fe5991a2 125 tripDataHistory.maxT = 0;
miaotwilio 5:ded8fe5991a2 126 tripDataHistory.throttleSamples = 0;
miaotwilio 5:ded8fe5991a2 127 }
miaotwilio 5:ded8fe5991a2 128
miaotwilio 5:ded8fe5991a2 129 TripDataReader::TripData TripDataReader::getTripData() {
miaotwilio 5:ded8fe5991a2 130 uint8_t obdData[16];
miaotwilio 5:ded8fe5991a2 131 TripData d;
miaotwilio 5:ded8fe5991a2 132
miaotwilio 6:4a25f3b9caef 133 d.averageSpeed = tripDataHistory.speedSamples ? (float)tripDataHistory.speed*0.62137119f/(float)tripDataHistory.speedSamples : 0;
miaotwilio 5:ded8fe5991a2 134 d.minimumThrottle = (float)tripDataHistory.minT*0.39215686f;
miaotwilio 6:4a25f3b9caef 135 d.averageThrottle = tripDataHistory.throttleSamples ? (float)tripDataHistory.avgT*0.39215686f/(float)tripDataHistory.throttleSamples : 0;
miaotwilio 5:ded8fe5991a2 136 d.maximumThrottle = (float)tripDataHistory.maxT*0.39215686f;
miaotwilio 5:ded8fe5991a2 137 d.hardBrakeCount = tripDataHistory.hardBrakeCount;
miaotwilio 5:ded8fe5991a2 138
miaotwilio 5:ded8fe5991a2 139 {
miaotwilio 5:ded8fe5991a2 140 // Read distance since Malfunction Indicator Lamp reset as a way to get miles traveled
miaotwilio 5:ded8fe5991a2 141 int result = readPin(0x31, 2, obdData);
miaotwilio 5:ded8fe5991a2 142 if(result != 4 || obdData[0] != 0x41 || obdData[1] != 0x31) {
miaotwilio 5:ded8fe5991a2 143 logError("error reading PID 0x31");
miaotwilio 5:ded8fe5991a2 144 } else {
miaotwilio 5:ded8fe5991a2 145 if(tripDataHistory.initialDistance <= 0)
miaotwilio 5:ded8fe5991a2 146 tripDataHistory.initialDistance = obdData[2]*256 + obdData[3];
miaotwilio 5:ded8fe5991a2 147 d.distance = obdData[2]*256 + obdData[3] - tripDataHistory.initialDistance;
miaotwilio 5:ded8fe5991a2 148 logInfo("initialDistance=%d, distance=%d", tripDataHistory.initialDistance, d.distance);
miaotwilio 5:ded8fe5991a2 149 }
miaotwilio 5:ded8fe5991a2 150 }
miaotwilio 5:ded8fe5991a2 151
miaotwilio 5:ded8fe5991a2 152 {
miaotwilio 5:ded8fe5991a2 153 // Get the fuel take level sensor percentage
miaotwilio 5:ded8fe5991a2 154 int result = readPin(0x2F, 1, obdData);
miaotwilio 5:ded8fe5991a2 155 if(result != 3 || obdData[0] != 0x41 || obdData[1] != 0x2F) {
miaotwilio 5:ded8fe5991a2 156 logError("error reading PID 0x2F");
miaotwilio 5:ded8fe5991a2 157 } else {
miaotwilio 5:ded8fe5991a2 158 d.fuel = obdData[2];
miaotwilio 5:ded8fe5991a2 159 logInfo("fuel=%d", d.fuel);
miaotwilio 5:ded8fe5991a2 160 }
miaotwilio 5:ded8fe5991a2 161 }
miaotwilio 5:ded8fe5991a2 162 {
miaotwilio 5:ded8fe5991a2 163 // Get the engine on time since start
miaotwilio 5:ded8fe5991a2 164 int result = readPin(0x1F, 2, obdData);
miaotwilio 5:ded8fe5991a2 165 if(result != 4 || obdData[0] != 0x41 || obdData[1] != 0x1F) {
miaotwilio 5:ded8fe5991a2 166 logError("error reading PID 0x1F");
miaotwilio 5:ded8fe5991a2 167 } else {
miaotwilio 5:ded8fe5991a2 168 d.runtime = obdData[2]*256 + obdData[3];
miaotwilio 5:ded8fe5991a2 169 logInfo("runtime=%d", d.runtime);
miaotwilio 5:ded8fe5991a2 170 }
miaotwilio 5:ded8fe5991a2 171 }
miaotwilio 5:ded8fe5991a2 172
miaotwilio 5:ded8fe5991a2 173 return d;
miaotwilio 5:ded8fe5991a2 174 }
miaotwilio 5:ded8fe5991a2 175
miaotwilio 5:ded8fe5991a2 176 int TripDataReader::readPin(int PID, int dataBytes, uint8_t *data) {
miaotwilio 5:ded8fe5991a2 177 char out[64], in[64];
miaotwilio 5:ded8fe5991a2 178 int i = 0;
miaotwilio 5:ded8fe5991a2 179 sprintf(out, "01%02X\r", PID);
miaotwilio 5:ded8fe5991a2 180 //logInfo("sending PID request %s", out);
miaotwilio 5:ded8fe5991a2 181 obd.write(out, strlen(out));
miaotwilio 5:ded8fe5991a2 182 wait(.1);
miaotwilio 5:ded8fe5991a2 183 i = obd.readable();
miaotwilio 5:ded8fe5991a2 184 //logInfo("OBD: %d bytes readable", i);
miaotwilio 5:ded8fe5991a2 185 obd.read(in, i);
miaotwilio 5:ded8fe5991a2 186 in[i]=0;
miaotwilio 5:ded8fe5991a2 187 //logInfo("OBD: in=%s", in);
miaotwilio 5:ded8fe5991a2 188
miaotwilio 5:ded8fe5991a2 189 if(i < (dataBytes * 3 + 5) )
miaotwilio 5:ded8fe5991a2 190 return -1;
miaotwilio 5:ded8fe5991a2 191 for(i = 0; i < (dataBytes+2); i++) {
miaotwilio 5:ded8fe5991a2 192 sscanf(in+i*3, "%02hhX", data + i);
miaotwilio 5:ded8fe5991a2 193 }
miaotwilio 5:ded8fe5991a2 194 /*for(i = 0; i < (dataBytes+2); i++) {
miaotwilio 5:ded8fe5991a2 195 logInfo("byte[%d]=%d", i, data[i]);
miaotwilio 5:ded8fe5991a2 196 }*/
miaotwilio 5:ded8fe5991a2 197 return i;
miaotwilio 5:ded8fe5991a2 198 }