miao zhicheng
/
DragonflyMQTT
Using MQTT on Dragonfly
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?
User | Revision | Line number | New 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 | } |