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