• Home
  • Line#
  • Scopes#
  • Navigate#
  • Raw
  • Download
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