# Copyright (C) 2017 The Android Open Source Project # # Licensed under the Apache License, Version 2.0 (the 'License'); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an 'AS IS' BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. # import datetime import time from xml.dom import minidom import diagnostic_sensors as s import vhal_consts_2_0 as c from diagnostic_builder import DiagnosticEventBuilder # interval of generating driving information SAMPLE_INTERVAL_SECONDS = 0.5 RPM_LOW = 1000 RPM_HIGH = 3000 REVERSE_DURATION_SECONDS = 10 PARK_DURATION_SECONDS = 10 # roughly 5 miles/hour REVERSE_SPEED_METERS_PER_SECOND = 2.3 UTC_TIME_FORMAT = "%Y-%m-%dT%H:%M:%S" # Diagnostics property constants. The value is based on the record from a test drive FUEL_SYSTEM_STATUS_VALUE = 2 AMBIENT_AIR_TEMPERATURE_VALUE = 21 ENGINE_COOLANT_TEMPERATURE_VALUE = 75 def speed2Gear(speed): """ Get the current gear based on speed of vehicle. The conversion may not be strictly real but are close enough to a normal vehicle. Assume the vehicle is moving forward. """ if speed < 4.4: # 0 - 10 mph return c.VEHICLEGEAR_GEAR_1 elif speed < 11.2: # 10 - 25 mph return c.VEHICLEGEAR_GEAR_2 elif speed < 20.1: # 25 - 45 mph return c.VEHICLEGEAR_GEAR_3 elif speed < 26.8: # 45 - 60 mph return c.VEHICLEGEAR_GEAR_4 else: # > 60 mph return c.VEHICLEGEAR_GEAR_5 class GpxFrame(object): """ A class representing a track point from GPX file """ def __init__(self, trkptDom): timeElements = trkptDom.getElementsByTagName('time') if timeElements: # time value in GPX is in UTC format: YYYY-MM-DDTHH:MM:SS, need to parse it self.datetime = datetime.datetime.strptime(timeElements[0].firstChild.nodeValue, UTC_TIME_FORMAT) speedElements = trkptDom.getElementsByTagName('speed') if speedElements: self.speedInMps = float(speedElements[0].firstChild.nodeValue) class DrivingInfoGenerator(object): """ A class that generates driving information like speed, odometer, rpm, diagnostics etc. It takes a GPX file which describes a real route that consists of a sequence of location data, and then derive driving information from those data. One assumption is that it is automatic transmission car, so that current gear does not necessarily match selected gear. """ def __init__(self, gpxFile, vhal): self.gpxDom = minidom.parse(gpxFile) # Speed of vehicle (meter / second) self.speedInMps = 0 # Fixed RPM with average value during driving self.rpm = RPM_LOW # Odometer (kilometer) self.odometerInKm = 0 # Gear selection self.selectedGear = c.VEHICLEGEAR_GEAR_PARK # Current gear self.currentGear = c.VEHICLEGEAR_GEAR_PARK # Timestamp while driving on route defined in GPX file self.datetime = 0 # Get Diagnostics live frame property configure vhal.getConfig(c.VEHICLEPROPERTY_OBD2_LIVE_FRAME) self.liveFrameConfig = vhal.rxMsg() def _generateFrame(self, listener): """ Handle newly generated vehicle property with listener """ listener.handle(c.VEHICLEPROPERTY_PERF_VEHICLE_SPEED, 0, self.speedInMps, "PERF_VEHICLE_SPEED") listener.handle(c.VEHICLEPROPERTY_ENGINE_RPM, 0, self.rpm, "ENGINE_RPM") listener.handle(c.VEHICLEPROPERTY_PERF_ODOMETER, 0, self.odometerInKm, "PERF_ODOMETER") listener.handle(c.VEHICLEPROPERTY_CURRENT_GEAR, 0, self.currentGear, "CURRENT_GEAR") listener.handle(c.VEHICLEPROPERTY_OBD2_LIVE_FRAME, 0, self._buildDiagnosticLiveFrame(), "DIAGNOSTIC_LIVE_FRAME") def _buildDiagnosticLiveFrame(self): """ Build a diagnostic live frame with a few sensor fields set """ builder = DiagnosticEventBuilder(self.liveFrameConfig) builder.setStringValue('') builder.addIntSensor(s.DIAGNOSTIC_SENSOR_INTEGER_FUEL_SYSTEM_STATUS, FUEL_SYSTEM_STATUS_VALUE) builder.addIntSensor(s.DIAGNOSTIC_SENSOR_INTEGER_AMBIENT_AIR_TEMPERATURE, AMBIENT_AIR_TEMPERATURE_VALUE) builder.addFloatSensor(s.DIAGNOSTIC_SENSOR_FLOAT_ENGINE_COOLANT_TEMPERATURE, ENGINE_COOLANT_TEMPERATURE_VALUE) builder.addFloatSensor(s.DIAGNOSTIC_SENSOR_FLOAT_ENGINE_RPM, self.rpm) builder.addFloatSensor(s.DIAGNOSTIC_SENSOR_FLOAT_VEHICLE_SPEED, self.speedInMps) return builder.build() def _generateFromGpxFrame(self, gpxFrame, listener): """ Generate a sequence of vehicle property frames from current track point to the next one. The frequency of frames are pre-defined. Some assumptions here: - Two track points are very close to each other (e.g. 1 second driving distance) - It is a straight line between two track point - Speed is changing linearly between two track point Given the info: timestamp1 : speed1 timestamp2 : speed2 Vehicle properties in each frame are derived like this: - Speed is calculated based on linear model - Odometer is calculated based on speed and time - RPM will be set to a low value if not accelerating, otherwise set to a high value - Current gear will be set according to speed """ duration = (gpxFrame.datetime - self.datetime).total_seconds() speedIncrement = (gpxFrame.speedInMps - self.speedInMps) / duration * SAMPLE_INTERVAL_SECONDS self.rpm = RPM_HIGH if speedIncrement > 0 else RPM_LOW timeElapsed = 0 while timeElapsed < duration: self._generateFrame(listener) if timeElapsed + SAMPLE_INTERVAL_SECONDS < duration: self.odometerInKm += (self.speedInMps + speedIncrement / 2.0) * SAMPLE_INTERVAL_SECONDS / 1000 self.speedInMps += speedIncrement time.sleep(SAMPLE_INTERVAL_SECONDS) else: timeLeft = duration - timeElapsed self.odometerInKm += (self.speedInMps + gpxFrame.speedInMps) / 2.0 * timeLeft / 1000 self.speedInMps = gpxFrame.speedInMps time.sleep(timeLeft) self.currentGear = speed2Gear(self.speedInMps) timeElapsed += SAMPLE_INTERVAL_SECONDS self.datetime = gpxFrame.datetime def _generateInReverseMode(self, duration, listener): print "Vehicle is reversing" listener.handle(c.VEHICLEPROPERTY_GEAR_SELECTION, 0, c.VEHICLEGEAR_GEAR_REVERSE, "GEAR_SELECTION") listener.handle(c.VEHICLEPROPERTY_CURRENT_GEAR, 0, c.VEHICLEGEAR_GEAR_REVERSE, "CURRENT_GEAR") self.rpm = RPM_LOW self.speedInMps = REVERSE_SPEED_METERS_PER_SECOND curTime = 0 while curTime < duration: self._generateFrame(listener) self.odometerInKm += self.speedInMps * SAMPLE_INTERVAL_SECONDS / 1000 curTime += SAMPLE_INTERVAL_SECONDS time.sleep(SAMPLE_INTERVAL_SECONDS) # After reverse is done, set speed to 0 self.speedInMps = .0 def _generateInParkMode(self, duration, listener): print "Vehicle is parked" listener.handle(c.VEHICLEPROPERTY_GEAR_SELECTION, 0, c.VEHICLEGEAR_GEAR_PARK, "GEAR_SELECTION") listener.handle(c.VEHICLEPROPERTY_CURRENT_GEAR, 0, c.VEHICLEGEAR_GEAR_PARK, "CURRENT_GEAR") # Assume in park mode, engine is still on self.rpm = RPM_LOW self.speedInMps = .0 curTime = 0 while curTime < duration: self._generateFrame(listener) curTime += SAMPLE_INTERVAL_SECONDS time.sleep(SAMPLE_INTERVAL_SECONDS) def generate(self, listener): # First, car is parked (probably in garage) self._generateInParkMode(PARK_DURATION_SECONDS, listener) # Second, car will reverse (out of garage) self._generateInReverseMode(REVERSE_DURATION_SECONDS, listener) trk = self.gpxDom.getElementsByTagName('trk')[0] trkseg = trk.getElementsByTagName('trkseg')[0] trkpts = trkseg.getElementsByTagName('trkpt') print "Vehicle start moving forward" listener.handle(c.VEHICLEPROPERTY_GEAR_SELECTION, 0, c.VEHICLEGEAR_GEAR_DRIVE, "GEAR_SELECTION") firstGpxFrame = GpxFrame(trkpts[0]) self.speedInMps = firstGpxFrame.speedInMps self.datetime = firstGpxFrame.datetime for i in xrange(1, len(trkpts)): self._generateFromGpxFrame(GpxFrame(trkpts[i]), listener)