| # 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) |
