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 16import datetime 17import time 18 19from xml.dom import minidom 20 21import diagnostic_sensors as s 22import vhal_consts_2_0 as c 23 24from diagnostic_builder import DiagnosticEventBuilder 25 26# interval of generating driving information 27SAMPLE_INTERVAL_SECONDS = 0.5 28 29RPM_LOW = 1000 30RPM_HIGH = 3000 31 32REVERSE_DURATION_SECONDS = 10 33PARK_DURATION_SECONDS = 10 34 35# roughly 5 miles/hour 36REVERSE_SPEED_METERS_PER_SECOND = 2.3 37 38UTC_TIME_FORMAT = "%Y-%m-%dT%H:%M:%S" 39 40# Diagnostics property constants. The value is based on the record from a test drive 41FUEL_SYSTEM_STATUS_VALUE = 2 42AMBIENT_AIR_TEMPERATURE_VALUE = 21 43ENGINE_COOLANT_TEMPERATURE_VALUE = 75 44 45 46def 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 67class 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 81class DrivingInfoGenerator(object): 82 """ 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. 86 87 One assumption is that it is automatic transmission car, so that current gear does not 88 necessarily match selected gear. 89 """ 90 91 def __init__(self, gpxFile, vhal): 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 105 # Get Diagnostics live frame property configure 106 vhal.getConfig(c.VEHICLEPROPERTY_OBD2_LIVE_FRAME) 107 self.liveFrameConfig = vhal.rxMsg() 108 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") 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() 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) 232