diff --git a/selfdrive/car/bmw/carstate.py b/selfdrive/car/bmw/carstate.py index 3b311eb61f8693..59e09fa3b6efed 100644 --- a/selfdrive/car/bmw/carstate.py +++ b/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: diff --git a/selfdrive/car/bmw/interface.py b/selfdrive/car/bmw/interface.py index a3c3f0db3c6361..b5ada2cbc1d3a3 100755 --- a/selfdrive/car/bmw/interface.py +++ b/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: