keunyoung | ca51507 | 2015-07-10 12:21:47 -0700 | [diff] [blame] | 1 | /* |
| 2 | * Copyright (C) 2015 The Android Open Source Project |
| 3 | * |
| 4 | * Licensed under the Apache License, Version 2.0 (the "License"); |
| 5 | * you may not use this file except in compliance with the License. |
| 6 | * You may obtain a copy of the License at |
| 7 | * |
| 8 | * http://www.apache.org/licenses/LICENSE-2.0 |
| 9 | * |
| 10 | * Unless required by applicable law or agreed to in writing, software |
| 11 | * distributed under the License is distributed on an "AS IS" BASIS, |
| 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 13 | * See the License for the specific language governing permissions and |
| 14 | * limitations under the License. |
| 15 | */ |
| 16 | |
| 17 | package com.android.car; |
| 18 | |
Keun-young Park | e54ac27 | 2016-02-16 19:02:18 -0800 | [diff] [blame] | 19 | import android.car.Car; |
| 20 | import android.car.hardware.CarSensorEvent; |
| 21 | import android.car.hardware.CarSensorManager; |
| 22 | import android.car.hardware.ICarSensorEventListener; |
keunyoung | ca51507 | 2015-07-10 12:21:47 -0700 | [diff] [blame] | 23 | import android.content.Context; |
| 24 | import android.os.SystemClock; |
keunyoung | ca51507 | 2015-07-10 12:21:47 -0700 | [diff] [blame] | 25 | import android.util.Log; |
| 26 | |
keunyoung | ca51507 | 2015-07-10 12:21:47 -0700 | [diff] [blame] | 27 | import java.io.PrintWriter; |
keunyoung | cc449f7 | 2015-08-12 10:46:27 -0700 | [diff] [blame] | 28 | import java.util.List; |
| 29 | |
keunyoung | ca51507 | 2015-07-10 12:21:47 -0700 | [diff] [blame] | 30 | |
| 31 | /** |
| 32 | * Logical sensor implementing driving state policy. This policy sets only two states: |
| 33 | * no restriction vs fully restrictive. To enter no restriction state, speed should be zero |
| 34 | * while either parking brake is applied or transmission gear is in P. |
| 35 | */ |
| 36 | public class DrivingStatePolicy extends CarSensorService.LogicalSensorHalBase { |
| 37 | |
| 38 | private final Context mContext; |
| 39 | private CarSensorService mSensorService; |
| 40 | private int mDringState = CarSensorEvent.DRIVE_STATUS_FULLY_RESTRICTED; |
| 41 | private SensorListener mSensorListener; |
| 42 | private boolean mIsReady = false; |
| 43 | private boolean mStarted = false; |
| 44 | |
| 45 | private static final int[] SUPPORTED_SENSORS = { CarSensorManager.SENSOR_TYPE_DRIVING_STATUS }; |
| 46 | |
| 47 | private final ICarSensorEventListener mICarSensorEventListener = |
| 48 | new ICarSensorEventListener.Stub() { |
| 49 | @Override |
keunyoung | cc449f7 | 2015-08-12 10:46:27 -0700 | [diff] [blame] | 50 | public void onSensorChanged(List<CarSensorEvent> events) { |
| 51 | for (CarSensorEvent event: events) { |
| 52 | handleSensorEvent(event); |
| 53 | } |
keunyoung | ca51507 | 2015-07-10 12:21:47 -0700 | [diff] [blame] | 54 | } |
| 55 | }; |
| 56 | |
| 57 | public DrivingStatePolicy(Context context) { |
| 58 | mContext = context; |
| 59 | } |
| 60 | |
| 61 | @Override |
| 62 | public void init() { |
| 63 | mIsReady = true; |
| 64 | } |
| 65 | |
| 66 | @Override |
| 67 | public synchronized void onSensorServiceReady() { |
| 68 | mSensorService = |
| 69 | (CarSensorService) ICarImpl.getInstance(mContext).getCarService(Car.SENSOR_SERVICE); |
| 70 | int sensorList[] = mSensorService.getSupportedSensors(); |
| 71 | boolean hasSpeed = subscribeIfSupportedLocked(sensorList, |
| 72 | CarSensorManager.SENSOR_TYPE_CAR_SPEED, CarSensorManager.SENSOR_RATE_FASTEST); |
| 73 | if (!hasSpeed) { |
| 74 | Log.w(CarLog.TAG_SENSOR, |
| 75 | "No speed sensor from car. Driving state will be always fully restrictive"); |
| 76 | } |
| 77 | boolean hasParkingBrake = subscribeIfSupportedLocked(sensorList, |
| 78 | CarSensorManager.SENSOR_TYPE_PARKING_BRAKE, CarSensorManager.SENSOR_RATE_FASTEST); |
| 79 | boolean hasGear = subscribeIfSupportedLocked(sensorList, CarSensorManager.SENSOR_TYPE_GEAR, |
| 80 | CarSensorManager.SENSOR_RATE_FASTEST); |
| 81 | if (!hasParkingBrake && !hasGear) { |
| 82 | Log.w(CarLog.TAG_SENSOR, |
| 83 | "No brake info from car. Driving state will be always fully restrictive"); |
| 84 | } |
| 85 | } |
| 86 | |
| 87 | @Override |
| 88 | public void release() { |
keunyoung | ca51507 | 2015-07-10 12:21:47 -0700 | [diff] [blame] | 89 | } |
| 90 | |
keunyoung | 1ab8e18 | 2015-09-24 09:25:22 -0700 | [diff] [blame] | 91 | public static CarSensorEvent getDefaultValue(int sensorType) { |
keunyoung | ca51507 | 2015-07-10 12:21:47 -0700 | [diff] [blame] | 92 | if (sensorType != CarSensorManager.SENSOR_TYPE_DRIVING_STATUS) { |
keunyoung | 1ab8e18 | 2015-09-24 09:25:22 -0700 | [diff] [blame] | 93 | Log.w(CarLog.TAG_SENSOR, "getDefaultValue to DrivingStatePolicy with sensorType:" + |
keunyoung | ca51507 | 2015-07-10 12:21:47 -0700 | [diff] [blame] | 94 | sensorType); |
| 95 | return null; |
| 96 | } |
Pavel Maltsev | dfccc5a | 2016-04-11 21:56:04 -0700 | [diff] [blame] | 97 | // There's a race condition and timestamp from vehicle HAL could be slightly less |
| 98 | // then current call to SystemClock.elapsedRealtimeNanos() will return. |
| 99 | // We want vehicle HAL value always override this default value so we set timestamp to 0. |
| 100 | return createEvent(CarSensorEvent.DRIVE_STATUS_FULLY_RESTRICTED, 0 /* timestamp */); |
keunyoung | ca51507 | 2015-07-10 12:21:47 -0700 | [diff] [blame] | 101 | } |
| 102 | |
| 103 | @Override |
| 104 | public synchronized void registerSensorListener(SensorListener listener) { |
| 105 | mSensorListener = listener; |
keunyoung | ca51507 | 2015-07-10 12:21:47 -0700 | [diff] [blame] | 106 | } |
| 107 | |
| 108 | @Override |
| 109 | public synchronized boolean isReady() { |
| 110 | return mIsReady; |
| 111 | } |
| 112 | |
| 113 | @Override |
| 114 | public int[] getSupportedSensors() { |
| 115 | return SUPPORTED_SENSORS; |
| 116 | } |
| 117 | |
| 118 | @Override |
| 119 | public synchronized boolean requestSensorStart(int sensorType, int rate) { |
| 120 | mStarted = true; |
keunyoung | fe30ba0 | 2015-09-17 17:56:35 -0700 | [diff] [blame] | 121 | dispatchCarSensorEvent(mSensorListener, createEvent(mDringState)); |
keunyoung | ca51507 | 2015-07-10 12:21:47 -0700 | [diff] [blame] | 122 | return true; |
| 123 | } |
| 124 | |
| 125 | @Override |
| 126 | public synchronized void requestSensorStop(int sensorType) { |
| 127 | mStarted = false; |
| 128 | } |
| 129 | |
| 130 | @Override |
| 131 | public void dump(PrintWriter writer) { |
keunyoung | ca51507 | 2015-07-10 12:21:47 -0700 | [diff] [blame] | 132 | } |
| 133 | |
| 134 | private boolean subscribeIfSupportedLocked(int sensorList[], int sensorType, int rate) { |
| 135 | if (!CarSensorManager.isSensorSupported(sensorList, sensorType)) { |
| 136 | Log.i(CarLog.TAG_SENSOR, "Sensor not supported:" + sensorType); |
| 137 | return false; |
| 138 | } |
| 139 | return mSensorService.registerOrUpdateSensorListener(sensorType, rate, |
Keun-young Park | e54ac27 | 2016-02-16 19:02:18 -0800 | [diff] [blame] | 140 | mICarSensorEventListener); |
keunyoung | ca51507 | 2015-07-10 12:21:47 -0700 | [diff] [blame] | 141 | } |
| 142 | |
| 143 | private synchronized void handleSensorEvent(CarSensorEvent event) { |
| 144 | switch (event.sensorType) { |
| 145 | case CarSensorManager.SENSOR_TYPE_PARKING_BRAKE: |
| 146 | case CarSensorManager.SENSOR_TYPE_GEAR: |
| 147 | case CarSensorManager.SENSOR_TYPE_CAR_SPEED: |
| 148 | int drivingState = recalcDrivingStateLocked(); |
| 149 | if (drivingState != mDringState && mSensorListener != null) { |
| 150 | mDringState = drivingState; |
keunyoung | fe30ba0 | 2015-09-17 17:56:35 -0700 | [diff] [blame] | 151 | dispatchCarSensorEvent(mSensorListener, createEvent(mDringState)); |
keunyoung | ca51507 | 2015-07-10 12:21:47 -0700 | [diff] [blame] | 152 | } |
| 153 | break; |
| 154 | default: |
| 155 | break; |
| 156 | } |
| 157 | } |
| 158 | |
| 159 | private int recalcDrivingStateLocked() { |
| 160 | int drivingState = CarSensorEvent.DRIVE_STATUS_FULLY_RESTRICTED; |
| 161 | CarSensorEvent lastParkingBrake = mSensorService.getLatestSensorEvent( |
| 162 | CarSensorManager.SENSOR_TYPE_PARKING_BRAKE); |
| 163 | CarSensorEvent lastGear = mSensorService.getLatestSensorEvent( |
| 164 | CarSensorManager.SENSOR_TYPE_GEAR); |
| 165 | CarSensorEvent lastSpeed = mSensorService.getLatestSensorEvent( |
| 166 | CarSensorManager.SENSOR_TYPE_CAR_SPEED); |
| 167 | if (lastSpeed != null && lastSpeed.floatValues[0] == 0f) { // stopped |
| 168 | if (lastParkingBrake == null && isParkingBrakeApplied(lastParkingBrake)) { |
| 169 | if (lastGear != null && isGearInParkingOrNeutral(lastGear)) { |
| 170 | drivingState = CarSensorEvent.DRIVE_STATUS_UNRESTRICTED; |
| 171 | } |
| 172 | } else { // parking break not applied or not available |
| 173 | if (lastGear != null && isGearInParking(lastGear)) { // gear in P |
| 174 | drivingState = CarSensorEvent.DRIVE_STATUS_UNRESTRICTED; |
| 175 | } |
| 176 | } |
| 177 | } // else moving, full restriction |
| 178 | return drivingState; |
| 179 | } |
| 180 | |
| 181 | private boolean isSpeedZero(CarSensorEvent event) { |
| 182 | return event.floatValues[0] == 0f; |
| 183 | } |
| 184 | |
| 185 | private boolean isParkingBrakeApplied(CarSensorEvent event) { |
keunyoung | cc449f7 | 2015-08-12 10:46:27 -0700 | [diff] [blame] | 186 | return event.intValues[0] == 1; |
keunyoung | ca51507 | 2015-07-10 12:21:47 -0700 | [diff] [blame] | 187 | } |
| 188 | |
| 189 | private boolean isGearInParkingOrNeutral(CarSensorEvent event) { |
keunyoung | cc449f7 | 2015-08-12 10:46:27 -0700 | [diff] [blame] | 190 | int gear = event.intValues[0]; |
| 191 | return (gear == CarSensorEvent.GEAR_NEUTRAL) || |
| 192 | (gear == CarSensorEvent.GEAR_PARK); |
keunyoung | ca51507 | 2015-07-10 12:21:47 -0700 | [diff] [blame] | 193 | } |
| 194 | |
| 195 | private boolean isGearInParking(CarSensorEvent event) { |
keunyoung | cc449f7 | 2015-08-12 10:46:27 -0700 | [diff] [blame] | 196 | int gear = event.intValues[0]; |
| 197 | return gear == CarSensorEvent.GEAR_PARK; |
keunyoung | ca51507 | 2015-07-10 12:21:47 -0700 | [diff] [blame] | 198 | } |
| 199 | |
keunyoung | 1ab8e18 | 2015-09-24 09:25:22 -0700 | [diff] [blame] | 200 | private static CarSensorEvent createEvent(int drivingState) { |
Pavel Maltsev | dfccc5a | 2016-04-11 21:56:04 -0700 | [diff] [blame] | 201 | return createEvent(drivingState, SystemClock.elapsedRealtimeNanos()); |
| 202 | } |
| 203 | |
| 204 | private static CarSensorEvent createEvent(int drivingState, long timestamp) { |
| 205 | CarSensorEvent event = new CarSensorEvent( |
| 206 | CarSensorManager.SENSOR_TYPE_DRIVING_STATUS, |
| 207 | timestamp, |
| 208 | 0 /* float values */, |
| 209 | 1 /* int values */); |
keunyoung | cc449f7 | 2015-08-12 10:46:27 -0700 | [diff] [blame] | 210 | event.intValues[0] = drivingState; |
keunyoung | ca51507 | 2015-07-10 12:21:47 -0700 | [diff] [blame] | 211 | return event; |
| 212 | } |
| 213 | } |