Fix driving state for manual transmission.

Listen to parking brake status in addition to the gear selection.
Revisit the initialization portion of the driving state and ux
restrictions when car service is created to handle corner case
situations when sensor data is not available from the VHAL.

Bug: 69859926
Test: runtest -x CarDrivingRestrictions.java
Change-Id: I4d11e0e7275c4cca840235619d83462742ba3f40

2 files changed, 182 insertions, 24 deletions

diff --git a/service/src/com/android/car/CarDrivingStateService.java b/service/src/com/android/car/CarDrivingStateService.java
index 7f0eeea126..bc0fbc12e6 100644
--- a/service/src/com/android/car/CarDrivingStateService.java
+++ b/service/src/com/android/car/CarDrivingStateService.java
@@ -17,6 +17,7 @@
 package com.android.car;
 
 import android.annotation.Nullable;
+import android.car.VehicleAreaType;
 import android.car.drivingstate.CarDrivingStateEvent;
 import android.car.drivingstate.CarDrivingStateEvent.CarDrivingState;
 import android.car.drivingstate.ICarDrivingState;
@@ -53,10 +54,11 @@ public class CarDrivingStateService extends ICarDrivingState.Stub implements Car
     // List of clients listening to driving state events.
     private final List<DrivingStateClient> mDrivingStateClients = new ArrayList<>();
     // Array of properties that the service needs to listen to from CarPropertyService for deriving
-    // the driving state. ToDo (ramperry@) - fine tune this list - b/69859926
+    // the driving state.
     private static final int[] REQUIRED_PROPERTIES = {
             VehicleProperty.PERF_VEHICLE_SPEED,
-            VehicleProperty.GEAR_SELECTION};
+            VehicleProperty.GEAR_SELECTION,
+            VehicleProperty.PARKING_BRAKE_ON};
     private CarDrivingStateEvent mCurrentDrivingState;
     // For dumpsys logging
     private final LinkedList<Utils.TransitionLog> mTransitionLogs = new LinkedList<>();
@@ -64,6 +66,9 @@ public class CarDrivingStateService extends ICarDrivingState.Stub implements Car
     private long mLastGearTimestamp = NOT_RECEIVED;
     private float mLastSpeed;
     private long mLastSpeedTimestamp = NOT_RECEIVED;
+    private boolean mLastParkingBrakeState;
+    private long mLastParkingBrakeTimestamp = NOT_RECEIVED;
+    private List<Integer> mSupportedGears;
 
     public CarDrivingStateService(Context context, CarPropertyService propertyService) {
         mContext = context;
@@ -72,12 +77,15 @@ public class CarDrivingStateService extends ICarDrivingState.Stub implements Car
     }
 
     @Override
-    public void init() {
+    public synchronized void init() {
         if (!checkPropertySupport()) {
             Log.e(TAG, "init failure.  Driving state will always be fully restrictive");
             return;
         }
         subscribeToProperties();
+        mCurrentDrivingState = createDrivingStateEvent(inferDrivingStateLocked());
+        addTransitionLog(TAG + " Boot", CarDrivingStateEvent.DRIVING_STATE_UNKNOWN,
+                mCurrentDrivingState.eventValue, mCurrentDrivingState.timeStamp);
     }
 
     @Override
@@ -269,11 +277,17 @@ public class CarDrivingStateService extends ICarDrivingState.Stub implements Car
 
     @Override
     public void dump(PrintWriter writer) {
-        writer.println("Driving state chane log:");
+        writer.println("Driving state change log:");
         for (Utils.TransitionLog tLog : mTransitionLogs) {
             writer.println(tLog);
         }
         writer.println("Current Driving State: " + mCurrentDrivingState.eventValue);
+        if (mSupportedGears != null) {
+            writer.println("Supported gears:");
+            for (Integer gear : mSupportedGears) {
+                writer.print("Gear:" + gear);
+            }
+        }
     }
 
     /**
@@ -315,6 +329,9 @@ public class CarDrivingStateService extends ICarDrivingState.Stub implements Car
                         }
                         break;
                     case VehicleProperty.GEAR_SELECTION:
+                        if (mSupportedGears == null) {
+                            mSupportedGears = getSupportedGears();
+                        }
                         int curGear = (Integer) value.getValue();
                         if (DBG) {
                             Log.d(TAG, "Gear: " + curGear + "@" + curTimestamp);
@@ -326,6 +343,19 @@ public class CarDrivingStateService extends ICarDrivingState.Stub implements Car
                             Log.d(TAG, "Ignoring Gear with older timestamp:" + curTimestamp);
                         }
                         break;
+                    case VehicleProperty.PARKING_BRAKE_ON:
+                        boolean curParkingBrake = (boolean) value.getValue();
+                        if (DBG) {
+                            Log.d(TAG, "Parking Brake: " + curParkingBrake + "@" + curTimestamp);
+                        }
+                        if (curTimestamp > mLastParkingBrakeTimestamp) {
+                            mLastParkingBrakeTimestamp = curTimestamp;
+                            mLastParkingBrakeState = curParkingBrake;
+                        } else if (DBG) {
+                            Log.d(TAG, "Ignoring Parking Brake status with an older timestamp:"
+                                    + curTimestamp);
+                        }
+                        break;
                     default:
                         Log.e(TAG, "Received property event for unhandled propId=" + propId);
                         break;
@@ -357,6 +387,16 @@ public class CarDrivingStateService extends ICarDrivingState.Stub implements Car
         }
     }
 
+    private List<Integer> getSupportedGears() {
+        List<CarPropertyConfig> properyList = mPropertyService.getPropertyList();
+        for (CarPropertyConfig p : properyList) {
+            if (p.getPropertyId() == VehicleProperty.GEAR_SELECTION) {
+                return p.getConfigArray();
+            }
+        }
+        return null;
+    }
+
     private void addTransitionLog(String name, int from, int to, long timestamp) {
         if (mTransitionLogs.size() >= MAX_TRANSITION_LOG_SIZE) {
             mTransitionLogs.remove();
@@ -369,31 +409,35 @@ public class CarDrivingStateService extends ICarDrivingState.Stub implements Car
     /**
      * Infers the current driving state of the car from the other Car Sensor properties like
      * Current Gear, Speed etc.
-     * ToDo (ramperry@) - Fine tune this - b/69859926
      *
      * @return Current driving state
      */
     @CarDrivingState
     private int inferDrivingStateLocked() {
+        updateVehiclePropertiesIfNeeded();
+        if (DBG) {
+            Log.d(TAG, "Last known Gear:" + mLastGear + " Last known speed:" + mLastSpeed);
+        }
+
         /*
-            Simple logic to start off deriving driving state:
+            Logic to start off deriving driving state:
             1. If gear == parked, then Driving State is parked.
             2. If gear != parked,
-                2a. if speed == 0, then driving state is idling
-                2b. if speed != 0, then driving state is moving
-                2c. if speed unavailable, then driving state is unknown
-            This logic needs to be tested and iterated on.  Tracked in b/69859926
+                    2a. if parking brake is applied, then Driving state is parked.
+                    2b. if parking brake is not applied or unknown/unavailable, then driving state
+                    is still unknown.
+            3. If driving state is unknown at the end of step 2,
+                3a. if speed == 0, then driving state is idling
+                3b. if speed != 0, then driving state is moving
+                3c. if speed unavailable, then driving state is unknown
          */
-        if (DBG) {
-            Log.d(TAG, "Last known Gear:" + mLastGear + " Last known speed:" + mLastSpeed);
-        }
-        if (mLastGearTimestamp == NOT_RECEIVED) {
-            return CarDrivingStateEvent.DRIVING_STATE_UNKNOWN;
-        } else if (mLastGear == VehicleGear.GEAR_PARK) {
+
+        if (isVehicleKnownToBeParked()) {
             return CarDrivingStateEvent.DRIVING_STATE_PARKED;
         }
 
-        if (mLastSpeedTimestamp == NOT_RECEIVED) {
+        // We don't know if the vehicle is parked, let's look at the speed.
+        if (mLastSpeedTimestamp == NOT_RECEIVED || mLastSpeed < 0) {
             return CarDrivingStateEvent.DRIVING_STATE_UNKNOWN;
         } else if (mLastSpeed == 0f) {
             return CarDrivingStateEvent.DRIVING_STATE_IDLING;
@@ -402,6 +446,89 @@ public class CarDrivingStateService extends ICarDrivingState.Stub implements Car
         }
     }
 
+    /**
+     * Find if we have signals to know if the vehicle is parked
+     *
+     * @return true if we have enough information to say the vehicle is parked.
+     * false, if the vehicle is either not parked or if we don't have any information.
+     */
+    private boolean isVehicleKnownToBeParked() {
+        // If we know the gear is in park, return true
+        if (mLastGearTimestamp != NOT_RECEIVED && mLastGear == VehicleGear.GEAR_PARK) {
+            return true;
+        } else if (mLastParkingBrakeTimestamp != NOT_RECEIVED) {
+            // if gear is not in park or unknown, look for status of parking brake if transmission
+            // type is manual.
+            if (isCarManualTransmissionType()) {
+                return mLastParkingBrakeState;
+            }
+        }
+        // if neither information is available, return false to indicate we can't determine
+        // if the vehicle is parked.
+        return false;
+    }
+
+    /**
+     * If Supported gears information is available and GEAR_PARK is not one of the supported gears,
+     * transmission type is considered to be Manual.  Automatic transmission is assumed otherwise.
+     */
+    private boolean isCarManualTransmissionType() {
+        if (mSupportedGears != null
+                && !mSupportedGears.isEmpty()
+                && !mSupportedGears.contains(VehicleGear.GEAR_PARK)) {
+            return true;
+        }
+        return false;
+    }
+
+    /**
+     * Try querying the gear selection and parking brake if we haven't received the event yet.
+     * This could happen if the gear change occurred before car service booted up like in the
+     * case of a HU restart in the middle of a drive.  Since gear and parking brake are
+     * on-change only properties, we could be in this situation where we will have to query
+     * VHAL.
+     */
+    private void updateVehiclePropertiesIfNeeded() {
+        if (mLastGearTimestamp == NOT_RECEIVED) {
+            CarPropertyValue propertyValue = mPropertyService.getProperty(
+                    VehicleProperty.GEAR_SELECTION,
+                    VehicleAreaType.VEHICLE_AREA_TYPE_GLOBAL);
+            if (propertyValue != null) {
+                mLastGear = (Integer) propertyValue.getValue();
+                mLastGearTimestamp = propertyValue.getTimestamp();
+                if (DBG) {
+                    Log.d(TAG, "updateVehiclePropertiesIfNeeded: gear:" + mLastGear);
+                }
+            }
+        }
+
+        if (mLastParkingBrakeTimestamp == NOT_RECEIVED) {
+            CarPropertyValue propertyValue = mPropertyService.getProperty(
+                    VehicleProperty.PARKING_BRAKE_ON,
+                    VehicleAreaType.VEHICLE_AREA_TYPE_GLOBAL);
+            if (propertyValue != null) {
+                mLastParkingBrakeState = (boolean) propertyValue.getValue();
+                mLastParkingBrakeTimestamp = propertyValue.getTimestamp();
+                if (DBG) {
+                    Log.d(TAG, "updateVehiclePropertiesIfNeeded: brake:" + mLastParkingBrakeState);
+                }
+            }
+        }
+
+        if (mLastSpeedTimestamp == NOT_RECEIVED) {
+            CarPropertyValue propertyValue = mPropertyService.getProperty(
+                    VehicleProperty.PERF_VEHICLE_SPEED,
+                    VehicleAreaType.VEHICLE_AREA_TYPE_GLOBAL);
+            if (propertyValue != null) {
+                mLastSpeed = (float) propertyValue.getValue();
+                mLastSpeedTimestamp = propertyValue.getTimestamp();
+                if (DBG) {
+                    Log.d(TAG, "updateVehiclePropertiesIfNeeded: speed:" + mLastSpeed);
+                }
+            }
+        }
+    }
+
     private static CarDrivingStateEvent createDrivingStateEvent(int eventValue) {
         return new CarDrivingStateEvent(eventValue, SystemClock.elapsedRealtimeNanos());
     }
diff --git a/service/src/com/android/car/CarUxRestrictionsManagerService.java b/service/src/com/android/car/CarUxRestrictionsManagerService.java
index 449a48564e..23c4d32a52 100644
--- a/service/src/com/android/car/CarUxRestrictionsManagerService.java
+++ b/service/src/com/android/car/CarUxRestrictionsManagerService.java
@@ -50,6 +50,7 @@ public class CarUxRestrictionsManagerService extends ICarUxRestrictionsManager.S
     private static final boolean DBG = false;
     private static final int MAX_TRANSITION_LOG_SIZE = 20;
     private static final int PROPERTY_UPDATE_RATE = 5; // Update rate in Hz
+    private static final float SPEED_NOT_AVAILABLE = -1.0F;
     private final Context mContext;
     private final CarDrivingStateService mDrivingStateService;
     private final CarPropertyService mCarPropertyService;
@@ -77,7 +78,7 @@ public class CarUxRestrictionsManagerService extends ICarUxRestrictionsManager.S
     }
 
     @Override
-    public void init() {
+    public synchronized void init() {
         try {
             if (!mHelper.loadUxRestrictionsFromXml()) {
                 Log.e(TAG, "Error reading Ux Restrictions Mapping. Falling back to defaults");
@@ -93,6 +94,38 @@ public class CarUxRestrictionsManagerService extends ICarUxRestrictionsManager.S
         // subscribe to property service for speed
         mCarPropertyService.registerListener(VehicleProperty.PERF_VEHICLE_SPEED,
                 PROPERTY_UPDATE_RATE, mICarPropertyEventListener);
+        initializeUxRestrictions();
+    }
+
+    // Update current restrictions by getting the current driving state and speed.
+    private void initializeUxRestrictions() {
+        CarDrivingStateEvent currentDrivingStateEvent =
+                mDrivingStateService.getCurrentDrivingState();
+        // if we don't have enough information from the CarPropertyService to compute the UX
+        // restrictions, then leave the UX restrictions unchanged from what it was initialized to
+        // in the constructor.
+        if (currentDrivingStateEvent == null || currentDrivingStateEvent.eventValue
+                == CarDrivingStateEvent.DRIVING_STATE_UNKNOWN) {
+            return;
+        }
+        int currentDrivingState = currentDrivingStateEvent.eventValue;
+        Float currentSpeed = getCurrentSpeed();
+        if (currentSpeed == SPEED_NOT_AVAILABLE) {
+            return;
+        }
+        // At this point the underlying CarPropertyService has provided us enough information to
+        // compute the UX restrictions that could be potentially different from the initial UX
+        // restrictions.
+        handleDispatchUxRestrictions(currentDrivingState, currentSpeed);
+    }
+
+    private Float getCurrentSpeed() {
+        CarPropertyValue value = mCarPropertyService.getProperty(VehicleProperty.PERF_VEHICLE_SPEED,
+                0);
+        if (value != null) {
+            return (Float) value.getValue();
+        }
+        return SPEED_NOT_AVAILABLE;
     }
 
     @Override
@@ -156,7 +189,6 @@ public class CarUxRestrictionsManagerService extends ICarUxRestrictionsManager.S
         return null;
     }
 
-
     /**
      * Unregister the given UX Restrictions listener
      *
@@ -282,10 +314,10 @@ public class CarUxRestrictionsManagerService extends ICarUxRestrictionsManager.S
             return;
         }
         int drivingState = event.eventValue;
-        CarPropertyValue value = mCarPropertyService.getProperty(VehicleProperty.PERF_VEHICLE_SPEED,
-                0);
-        if (value != null) {
-            mCurrentMovingSpeed = (Float) value.getValue();
+        Float speed = getCurrentSpeed();
+
+        if (speed != SPEED_NOT_AVAILABLE) {
+            mCurrentMovingSpeed = speed;
         } else if (drivingState == CarDrivingStateEvent.DRIVING_STATE_PARKED
                 || drivingState == CarDrivingStateEvent.DRIVING_STATE_UNKNOWN) {
             // If speed is unavailable, but the driving state is parked or unknown, it can still be
@@ -352,7 +384,6 @@ public class CarUxRestrictionsManagerService extends ICarUxRestrictionsManager.S
             uxRestrictions = getDefaultRestrictions(currentDrivingState);
         } else {
             uxRestrictions = mHelper.getUxRestrictions(currentDrivingState, speed);
-
         }
 
         if (DBG) {