Detect cruise control units kph/mph

lint ok

selfdrive/car/bmw/carstate.py

@@ -4,7 +4,6 @@
 from openpilot.selfdrive.car.conversions import Conversions as CV
 from openpilot.selfdrive.car.interfaces import CarStateBase
 from openpilot.selfdrive.car.bmw.values import DBC, CanBus, BmwFlags, CruiseSettings
-from openpilot.common.params import Params
 
 class CarState(CarStateBase):
   def __init__(self, CP):
@@ -16,7 +15,7 @@ def __init__(self, CP):
 
     self.cluster_min_speed = CruiseSettings.CLUSTER_OFFSET
 
-    self.is_metric = Params().get("IsMetric", encoding='utf8') == "1"   #todo set is_metric in _get_params somehow
+    self.is_metric = None
     self.cruise_stalk_speed = 0
     self.cruise_stalk_resume = False
     self.cruise_stalk_cancel = False
@@ -93,12 +92,6 @@ def update(self, cp_PT, cp_F, cp_aux):
     ret.cruiseState.available = not ret.espDisabled  #cruise not available when DSC fully off
     ret.cruiseState.nonAdaptive = False # bmw doesn't have a switch
 
-    # todo # *** determine is_metric based speed target vs actual speed ***
-    # if ret.cruiseState.enabled:
-    #   if abs(ret.cruiseState.speed / ret.vEgo - CV.MS_TO_KPH) < 0.3:
-    #     self.is_metric = True
-    #   elif abs(ret.cruiseState.speed / ret.vEgo - CV.MS_TO_MPH) < 0.3:
-    #     self.is_metric = False
 
     cruise_control_stal_msg = cp_PT.vl["CruiseControlStalk"]
     if self.CP.flags & BmwFlags.DYNAMIC_CRUISE_CONTROL:

selfdrive/car/bmw/interface.py

@@ -6,7 +6,6 @@
 from openpilot.selfdrive.car import get_safety_config
 from openpilot.selfdrive.car.bmw.values import CanBus, BmwFlags, CarControllerParams
 from openpilot.selfdrive.car.interfaces import CarInterfaceBase
-from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel
 
 ButtonType = car.CarState.ButtonEvent.Type
 EventName = car.CarEvent.EventName
@@ -36,7 +35,6 @@ def detect_stepper_override(steer_cmd, steer_act, v_ego, centering_coeff, steer_
 class CarInterface(CarInterfaceBase):
   def __init__(self, CP, CarController, CarState):
     super().__init__(CP, CarController, CarState)
-    self.VM = VehicleModel(CP)  # for the yawRate
 
     self.cp_F = self.CS.get_F_can_parser(CP)
     self.can_parsers.append(self.cp_F)
@@ -138,6 +136,23 @@ def _update(self, c):
     # ******************* do can recv *******************
     ret = self.CS.update(self.cp, self.cp_F, self.cp_aux)
 
+    # events
+    events = self.create_common_events(ret, pcm_enable=True)
+
+    # *** cruise control units detection ***
+    # when cruise is enabled the car sets cruiseState.speed = vEgo, so we can detect the ratio
+    # with resume this wouldn't work, but op will not engage on first resume anyway
+    if self.CS.is_metric is None and c.enabled and ret.vEgo > 0:
+      # note, when is_metric is None, cruiseState.speed is already scaled by CV.MPH_TO_MS by default
+      speed_ratio = ret.cruiseState.speed / ret.vEgo  # 1 if imperial, 1.6 if metric
+      if 0.8 < speed_ratio < 1.2:
+        self.CS.is_metric = False
+      elif 0.8 * CV.MPH_TO_KPH < speed_ratio < 1.2 * CV.MPH_TO_KPH:
+        self.CS.is_metric = True
+      else:
+        events.add(EventName.accFaulted)
+
+
     ret.buttonEvents = [
       *create_button_events(self.CS.cruise_stalk_speed > 0, self.CS.prev_cruise_stalk_speed > 0, {1: ButtonType.accelCruise}),
       *create_button_events(self.CS.cruise_stalk_speed < 0, self.CS.prev_cruise_stalk_speed < 0, {1: ButtonType.decelCruise}),
@@ -147,8 +162,6 @@ def _update(self, c):
         1: ButtonType.resumeCruise if not c.enabled else ButtonType.gapAdjustCruise}) # repurpose resume button to adjust driver personality when engaged
       ]
 
-    # events
-    events = self.create_common_events(ret, pcm_enable=True)
     if ret.vEgoCluster < self.CP.minEnableSpeed:
       events.add(EventName.belowEngageSpeed)
       if c.actuators.accel > 0.2: