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
17package com.android.car;
18
19import android.car.Car;
20import android.car.hardware.CarSensorEvent;
21import android.car.hardware.CarSensorManager;
22import android.car.hardware.ICarSensorEventListener;
23import android.content.Context;
24import android.os.SystemClock;
25import android.util.Log;
26
27import com.android.car.hal.SensorHalServiceBase.SensorListener;
28
29import java.io.PrintWriter;
30import java.util.List;
31
32
33/**
34 * Logical sensor implementing driving state policy. This policy sets only two states:
35 * no restriction vs fully restrictive. To enter no restriction state, speed should be zero
36 * while either parking brake is applied or transmission gear is in P.
37 */
38public class DrivingStatePolicy extends CarSensorService.LogicalSensorHalBase {
39
40    private final Context mContext;
41    private CarSensorService mSensorService;
42    private int mDringState = CarSensorEvent.DRIVE_STATUS_FULLY_RESTRICTED;
43    private SensorListener mSensorListener;
44    private boolean mIsReady = false;
45    private boolean mStarted = false;
46
47    private static final int[] SUPPORTED_SENSORS = { CarSensorManager.SENSOR_TYPE_DRIVING_STATUS };
48
49    private final ICarSensorEventListener mICarSensorEventListener =
50            new ICarSensorEventListener.Stub() {
51        @Override
52        public void onSensorChanged(List<CarSensorEvent> events) {
53            for (CarSensorEvent event: events) {
54                handleSensorEvent(event);
55            }
56        }
57    };
58
59    public DrivingStatePolicy(Context context) {
60        mContext = context;
61    }
62
63    @Override
64    public void init() {
65        mIsReady = true;
66    }
67
68    @Override
69    public synchronized void onSensorServiceReady() {
70        mSensorService =
71                (CarSensorService) ICarImpl.getInstance(mContext).getCarService(Car.SENSOR_SERVICE);
72        int sensorList[] = mSensorService.getSupportedSensors();
73        boolean hasSpeed = subscribeIfSupportedLocked(sensorList,
74                CarSensorManager.SENSOR_TYPE_CAR_SPEED, CarSensorManager.SENSOR_RATE_FASTEST);
75        if (!hasSpeed) {
76            Log.w(CarLog.TAG_SENSOR,
77                    "No speed sensor from car. Driving state will be always fully restrictive");
78        }
79        boolean hasParkingBrake = subscribeIfSupportedLocked(sensorList,
80                CarSensorManager.SENSOR_TYPE_PARKING_BRAKE, CarSensorManager.SENSOR_RATE_FASTEST);
81        boolean hasGear = subscribeIfSupportedLocked(sensorList, CarSensorManager.SENSOR_TYPE_GEAR,
82                CarSensorManager.SENSOR_RATE_FASTEST);
83        if (!hasParkingBrake && !hasGear) {
84            Log.w(CarLog.TAG_SENSOR,
85                    "No brake info from car. Driving state will be always fully restrictive");
86        }
87    }
88
89    @Override
90    public void release() {
91        // TODO Auto-generated method stub
92    }
93
94    public static CarSensorEvent getDefaultValue(int sensorType) {
95        if (sensorType != CarSensorManager.SENSOR_TYPE_DRIVING_STATUS) {
96            Log.w(CarLog.TAG_SENSOR, "getDefaultValue to DrivingStatePolicy with sensorType:" +
97                    sensorType);
98            return null;
99        }
100        return createEvent(CarSensorEvent.DRIVE_STATUS_FULLY_RESTRICTED);
101    }
102
103    @Override
104    public synchronized void registerSensorListener(SensorListener listener) {
105        mSensorListener = listener;
106        if (mIsReady) {
107            mSensorListener.onSensorHalReady(this);
108        }
109    }
110
111    @Override
112    public synchronized boolean isReady() {
113        return mIsReady;
114    }
115
116    @Override
117    public int[] getSupportedSensors() {
118        return SUPPORTED_SENSORS;
119    }
120
121    @Override
122    public synchronized boolean requestSensorStart(int sensorType, int rate) {
123        mStarted = true;
124        dispatchCarSensorEvent(mSensorListener, createEvent(mDringState));
125        return true;
126    }
127
128    @Override
129    public synchronized void requestSensorStop(int sensorType) {
130        mStarted = false;
131    }
132
133    @Override
134    public void dump(PrintWriter writer) {
135        // TODO Auto-generated method stub
136    }
137
138    private boolean subscribeIfSupportedLocked(int sensorList[], int sensorType, int rate) {
139        if (!CarSensorManager.isSensorSupported(sensorList, sensorType)) {
140            Log.i(CarLog.TAG_SENSOR, "Sensor not supported:" + sensorType);
141            return false;
142        }
143        return mSensorService.registerOrUpdateSensorListener(sensorType, rate,
144                mICarSensorEventListener);
145    }
146
147    private synchronized void handleSensorEvent(CarSensorEvent event) {
148        switch (event.sensorType) {
149            case CarSensorManager.SENSOR_TYPE_PARKING_BRAKE:
150            case CarSensorManager.SENSOR_TYPE_GEAR:
151            case CarSensorManager.SENSOR_TYPE_CAR_SPEED:
152                int drivingState = recalcDrivingStateLocked();
153                if (drivingState != mDringState && mSensorListener != null) {
154                    mDringState = drivingState;
155                    dispatchCarSensorEvent(mSensorListener, createEvent(mDringState));
156                }
157                break;
158            default:
159                break;
160        }
161    }
162
163    private int recalcDrivingStateLocked() {
164        int drivingState = CarSensorEvent.DRIVE_STATUS_FULLY_RESTRICTED;
165        CarSensorEvent lastParkingBrake = mSensorService.getLatestSensorEvent(
166                CarSensorManager.SENSOR_TYPE_PARKING_BRAKE);
167        CarSensorEvent lastGear = mSensorService.getLatestSensorEvent(
168                CarSensorManager.SENSOR_TYPE_GEAR);
169        CarSensorEvent lastSpeed = mSensorService.getLatestSensorEvent(
170                CarSensorManager.SENSOR_TYPE_CAR_SPEED);
171        if (lastSpeed != null && lastSpeed.floatValues[0] == 0f) { // stopped
172            if (lastParkingBrake == null && isParkingBrakeApplied(lastParkingBrake)) {
173                if (lastGear != null && isGearInParkingOrNeutral(lastGear)) {
174                    drivingState = CarSensorEvent.DRIVE_STATUS_UNRESTRICTED;
175                }
176            } else { // parking break not applied or not available
177                if (lastGear != null && isGearInParking(lastGear)) { // gear in P
178                    drivingState = CarSensorEvent.DRIVE_STATUS_UNRESTRICTED;
179                }
180            }
181        } // else moving, full restriction
182        return drivingState;
183    }
184
185    private boolean isSpeedZero(CarSensorEvent event) {
186        return event.floatValues[0] == 0f;
187    }
188
189    private boolean isParkingBrakeApplied(CarSensorEvent event) {
190        return event.intValues[0] == 1;
191    }
192
193    private boolean isGearInParkingOrNeutral(CarSensorEvent event) {
194        int gear = event.intValues[0];
195        return (gear == CarSensorEvent.GEAR_NEUTRAL) ||
196                (gear == CarSensorEvent.GEAR_PARK);
197    }
198
199    private boolean isGearInParking(CarSensorEvent event) {
200        int gear = event.intValues[0];
201        return gear == CarSensorEvent.GEAR_PARK;
202    }
203
204    private static CarSensorEvent createEvent(int drivingState) {
205        CarSensorEvent event = new CarSensorEvent(CarSensorManager.SENSOR_TYPE_DRIVING_STATUS,
206                SystemClock.elapsedRealtimeNanos(), 0, 1);
207        event.intValues[0] = drivingState;
208        return event;
209    }
210}
211