Chao Yan | 6f37e15 | 2017-10-11 18:35:45 -0700 | [diff] [blame] | 1 | # Copyright (C) 2017 The Android Open Source Project |
| 2 | # |
| 3 | # Licensed under the Apache License, Version 2.0 (the 'License'); |
| 4 | # you may not use this file except in compliance with the License. |
| 5 | # You may obtain a copy of the License at |
| 6 | # |
| 7 | # http://www.apache.org/licenses/LICENSE-2.0 |
| 8 | # |
| 9 | # Unless required by applicable law or agreed to in writing, software |
| 10 | # distributed under the License is distributed on an 'AS IS' BASIS, |
| 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 12 | # See the License for the specific language governing permissions and |
| 13 | # limitations under the License. |
| 14 | # |
| 15 | |
| 16 | import datetime |
| 17 | import time |
| 18 | |
| 19 | from xml.dom import minidom |
| 20 | |
Chao Yan | a5e50f9 | 2017-11-21 11:01:13 -0800 | [diff] [blame] | 21 | import diagnostic_sensors as s |
Chao Yan | 6f37e15 | 2017-10-11 18:35:45 -0700 | [diff] [blame] | 22 | import vhal_consts_2_0 as c |
| 23 | |
Chao Yan | a5e50f9 | 2017-11-21 11:01:13 -0800 | [diff] [blame] | 24 | from diagnostic_builder import DiagnosticEventBuilder |
| 25 | |
Chao Yan | 6f37e15 | 2017-10-11 18:35:45 -0700 | [diff] [blame] | 26 | # interval of generating driving information |
| 27 | SAMPLE_INTERVAL_SECONDS = 0.5 |
| 28 | |
| 29 | RPM_LOW = 1000 |
| 30 | RPM_HIGH = 3000 |
| 31 | |
| 32 | REVERSE_DURATION_SECONDS = 10 |
| 33 | PARK_DURATION_SECONDS = 10 |
| 34 | |
| 35 | # roughly 5 miles/hour |
| 36 | REVERSE_SPEED_METERS_PER_SECOND = 2.3 |
| 37 | |
| 38 | UTC_TIME_FORMAT = "%Y-%m-%dT%H:%M:%S" |
| 39 | |
Chao Yan | a5e50f9 | 2017-11-21 11:01:13 -0800 | [diff] [blame] | 40 | # Diagnostics property constants. The value is based on the record from a test drive |
| 41 | FUEL_SYSTEM_STATUS_VALUE = 2 |
| 42 | AMBIENT_AIR_TEMPERATURE_VALUE = 21 |
| 43 | ENGINE_COOLANT_TEMPERATURE_VALUE = 75 |
| 44 | |
Chao Yan | 6f37e15 | 2017-10-11 18:35:45 -0700 | [diff] [blame] | 45 | |
| 46 | def speed2Gear(speed): |
| 47 | """ |
| 48 | Get the current gear based on speed of vehicle. The conversion may not be strictly real but |
| 49 | are close enough to a normal vehicle. Assume the vehicle is moving forward. |
| 50 | """ |
| 51 | if speed < 4.4: |
| 52 | # 0 - 10 mph |
| 53 | return c.VEHICLEGEAR_GEAR_1 |
| 54 | elif speed < 11.2: |
| 55 | # 10 - 25 mph |
| 56 | return c.VEHICLEGEAR_GEAR_2 |
| 57 | elif speed < 20.1: |
| 58 | # 25 - 45 mph |
| 59 | return c.VEHICLEGEAR_GEAR_3 |
| 60 | elif speed < 26.8: |
| 61 | # 45 - 60 mph |
| 62 | return c.VEHICLEGEAR_GEAR_4 |
| 63 | else: |
| 64 | # > 60 mph |
| 65 | return c.VEHICLEGEAR_GEAR_5 |
| 66 | |
| 67 | class GpxFrame(object): |
| 68 | """ |
| 69 | A class representing a track point from GPX file |
| 70 | """ |
| 71 | def __init__(self, trkptDom): |
| 72 | timeElements = trkptDom.getElementsByTagName('time') |
| 73 | if timeElements: |
| 74 | # time value in GPX is in UTC format: YYYY-MM-DDTHH:MM:SS, need to parse it |
| 75 | self.datetime = datetime.datetime.strptime(timeElements[0].firstChild.nodeValue, |
| 76 | UTC_TIME_FORMAT) |
| 77 | speedElements = trkptDom.getElementsByTagName('speed') |
| 78 | if speedElements: |
| 79 | self.speedInMps = float(speedElements[0].firstChild.nodeValue) |
| 80 | |
| 81 | class DrivingInfoGenerator(object): |
| 82 | """ |
Chao Yan | a5e50f9 | 2017-11-21 11:01:13 -0800 | [diff] [blame] | 83 | A class that generates driving information like speed, odometer, rpm, diagnostics etc. It |
| 84 | takes a GPX file which describes a real route that consists of a sequence of location data, |
| 85 | and then derive driving information from those data. |
Chao Yan | 6f37e15 | 2017-10-11 18:35:45 -0700 | [diff] [blame] | 86 | |
Chao Yan | a5e50f9 | 2017-11-21 11:01:13 -0800 | [diff] [blame] | 87 | One assumption is that it is automatic transmission car, so that current gear does not |
Chao Yan | 6f37e15 | 2017-10-11 18:35:45 -0700 | [diff] [blame] | 88 | necessarily match selected gear. |
| 89 | """ |
| 90 | |
Chao Yan | a5e50f9 | 2017-11-21 11:01:13 -0800 | [diff] [blame] | 91 | def __init__(self, gpxFile, vhal): |
Chao Yan | 6f37e15 | 2017-10-11 18:35:45 -0700 | [diff] [blame] | 92 | self.gpxDom = minidom.parse(gpxFile) |
| 93 | # Speed of vehicle (meter / second) |
| 94 | self.speedInMps = 0 |
| 95 | # Fixed RPM with average value during driving |
| 96 | self.rpm = RPM_LOW |
| 97 | # Odometer (kilometer) |
| 98 | self.odometerInKm = 0 |
| 99 | # Gear selection |
| 100 | self.selectedGear = c.VEHICLEGEAR_GEAR_PARK |
| 101 | # Current gear |
| 102 | self.currentGear = c.VEHICLEGEAR_GEAR_PARK |
| 103 | # Timestamp while driving on route defined in GPX file |
| 104 | self.datetime = 0 |
Chao Yan | a5e50f9 | 2017-11-21 11:01:13 -0800 | [diff] [blame] | 105 | # Get Diagnostics live frame property configure |
| 106 | vhal.getConfig(c.VEHICLEPROPERTY_OBD2_LIVE_FRAME) |
| 107 | self.liveFrameConfig = vhal.rxMsg() |
| 108 | |
Chao Yan | 6f37e15 | 2017-10-11 18:35:45 -0700 | [diff] [blame] | 109 | |
| 110 | def _generateFrame(self, listener): |
| 111 | """ |
| 112 | Handle newly generated vehicle property with listener |
| 113 | """ |
| 114 | listener.handle(c.VEHICLEPROPERTY_PERF_VEHICLE_SPEED, 0, self.speedInMps, "PERF_VEHICLE_SPEED") |
| 115 | listener.handle(c.VEHICLEPROPERTY_ENGINE_RPM, 0, self.rpm, "ENGINE_RPM") |
| 116 | listener.handle(c.VEHICLEPROPERTY_PERF_ODOMETER, 0, self.odometerInKm, "PERF_ODOMETER") |
| 117 | listener.handle(c.VEHICLEPROPERTY_CURRENT_GEAR, 0, self.currentGear, "CURRENT_GEAR") |
Chao Yan | a5e50f9 | 2017-11-21 11:01:13 -0800 | [diff] [blame] | 118 | listener.handle(c.VEHICLEPROPERTY_OBD2_LIVE_FRAME, 0, |
| 119 | self._buildDiagnosticLiveFrame(), "DIAGNOSTIC_LIVE_FRAME") |
| 120 | |
| 121 | def _buildDiagnosticLiveFrame(self): |
| 122 | """ |
| 123 | Build a diagnostic live frame with a few sensor fields set |
| 124 | """ |
| 125 | builder = DiagnosticEventBuilder(self.liveFrameConfig) |
| 126 | builder.setStringValue('') |
| 127 | builder.addIntSensor(s.DIAGNOSTIC_SENSOR_INTEGER_FUEL_SYSTEM_STATUS, |
| 128 | FUEL_SYSTEM_STATUS_VALUE) |
| 129 | builder.addIntSensor(s.DIAGNOSTIC_SENSOR_INTEGER_AMBIENT_AIR_TEMPERATURE, |
| 130 | AMBIENT_AIR_TEMPERATURE_VALUE) |
| 131 | builder.addFloatSensor(s.DIAGNOSTIC_SENSOR_FLOAT_ENGINE_COOLANT_TEMPERATURE, |
| 132 | ENGINE_COOLANT_TEMPERATURE_VALUE) |
| 133 | builder.addFloatSensor(s.DIAGNOSTIC_SENSOR_FLOAT_ENGINE_RPM, self.rpm) |
| 134 | builder.addFloatSensor(s.DIAGNOSTIC_SENSOR_FLOAT_VEHICLE_SPEED, self.speedInMps) |
| 135 | return builder.build() |
Chao Yan | 6f37e15 | 2017-10-11 18:35:45 -0700 | [diff] [blame] | 136 | |
| 137 | def _generateFromGpxFrame(self, gpxFrame, listener): |
| 138 | """ |
| 139 | Generate a sequence of vehicle property frames from current track point to the next one. |
| 140 | The frequency of frames are pre-defined. |
| 141 | |
| 142 | Some assumptions here: |
| 143 | - Two track points are very close to each other (e.g. 1 second driving distance) |
| 144 | - It is a straight line between two track point |
| 145 | - Speed is changing linearly between two track point |
| 146 | |
| 147 | Given the info: |
| 148 | timestamp1 : speed1 |
| 149 | timestamp2 : speed2 |
| 150 | |
| 151 | Vehicle properties in each frame are derived like this: |
| 152 | - Speed is calculated based on linear model |
| 153 | - Odometer is calculated based on speed and time |
| 154 | - RPM will be set to a low value if not accelerating, otherwise set to a high value |
| 155 | - Current gear will be set according to speed |
| 156 | """ |
| 157 | |
| 158 | duration = (gpxFrame.datetime - self.datetime).total_seconds() |
| 159 | speedIncrement = (gpxFrame.speedInMps - self.speedInMps) / duration * SAMPLE_INTERVAL_SECONDS |
| 160 | self.rpm = RPM_HIGH if speedIncrement > 0 else RPM_LOW |
| 161 | |
| 162 | timeElapsed = 0 |
| 163 | while timeElapsed < duration: |
| 164 | self._generateFrame(listener) |
| 165 | if timeElapsed + SAMPLE_INTERVAL_SECONDS < duration: |
| 166 | self.odometerInKm += (self.speedInMps + speedIncrement / 2.0) * SAMPLE_INTERVAL_SECONDS / 1000 |
| 167 | self.speedInMps += speedIncrement |
| 168 | time.sleep(SAMPLE_INTERVAL_SECONDS) |
| 169 | else: |
| 170 | timeLeft = duration - timeElapsed |
| 171 | self.odometerInKm += (self.speedInMps + gpxFrame.speedInMps) / 2.0 * timeLeft / 1000 |
| 172 | self.speedInMps = gpxFrame.speedInMps |
| 173 | time.sleep(timeLeft) |
| 174 | |
| 175 | self.currentGear = speed2Gear(self.speedInMps) |
| 176 | timeElapsed += SAMPLE_INTERVAL_SECONDS |
| 177 | |
| 178 | self.datetime = gpxFrame.datetime |
| 179 | |
| 180 | def _generateInReverseMode(self, duration, listener): |
| 181 | print "Vehicle is reversing" |
| 182 | listener.handle(c.VEHICLEPROPERTY_GEAR_SELECTION, 0, c.VEHICLEGEAR_GEAR_REVERSE, |
| 183 | "GEAR_SELECTION") |
| 184 | listener.handle(c.VEHICLEPROPERTY_CURRENT_GEAR, 0, c.VEHICLEGEAR_GEAR_REVERSE, |
| 185 | "CURRENT_GEAR") |
| 186 | self.rpm = RPM_LOW |
| 187 | self.speedInMps = REVERSE_SPEED_METERS_PER_SECOND |
| 188 | curTime = 0 |
| 189 | while curTime < duration: |
| 190 | self._generateFrame(listener) |
| 191 | self.odometerInKm += self.speedInMps * SAMPLE_INTERVAL_SECONDS / 1000 |
| 192 | curTime += SAMPLE_INTERVAL_SECONDS |
| 193 | time.sleep(SAMPLE_INTERVAL_SECONDS) |
| 194 | # After reverse is done, set speed to 0 |
| 195 | self.speedInMps = .0 |
| 196 | |
| 197 | def _generateInParkMode(self, duration, listener): |
| 198 | print "Vehicle is parked" |
| 199 | listener.handle(c.VEHICLEPROPERTY_GEAR_SELECTION, 0, c.VEHICLEGEAR_GEAR_PARK, |
| 200 | "GEAR_SELECTION") |
| 201 | listener.handle(c.VEHICLEPROPERTY_CURRENT_GEAR, 0, c.VEHICLEGEAR_GEAR_PARK, |
| 202 | "CURRENT_GEAR") |
| 203 | # Assume in park mode, engine is still on |
| 204 | self.rpm = RPM_LOW |
| 205 | self.speedInMps = .0 |
| 206 | curTime = 0 |
| 207 | while curTime < duration: |
| 208 | self._generateFrame(listener) |
| 209 | curTime += SAMPLE_INTERVAL_SECONDS |
| 210 | time.sleep(SAMPLE_INTERVAL_SECONDS) |
| 211 | |
| 212 | def generate(self, listener): |
| 213 | # First, car is parked (probably in garage) |
| 214 | self._generateInParkMode(PARK_DURATION_SECONDS, listener) |
| 215 | # Second, car will reverse (out of garage) |
| 216 | self._generateInReverseMode(REVERSE_DURATION_SECONDS, listener) |
| 217 | |
| 218 | trk = self.gpxDom.getElementsByTagName('trk')[0] |
| 219 | trkseg = trk.getElementsByTagName('trkseg')[0] |
| 220 | trkpts = trkseg.getElementsByTagName('trkpt') |
| 221 | |
| 222 | print "Vehicle start moving forward" |
| 223 | listener.handle(c.VEHICLEPROPERTY_GEAR_SELECTION, 0, c.VEHICLEGEAR_GEAR_DRIVE, |
| 224 | "GEAR_SELECTION") |
| 225 | |
| 226 | firstGpxFrame = GpxFrame(trkpts[0]) |
| 227 | self.speedInMps = firstGpxFrame.speedInMps |
| 228 | self.datetime = firstGpxFrame.datetime |
| 229 | |
| 230 | for i in xrange(1, len(trkpts)): |
| 231 | self._generateFromGpxFrame(GpxFrame(trkpts[i]), listener) |