diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 463487dd78..fffd140fc6 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -70,12 +70,12 @@ class Car: if self.CP.customStockLongAvailable and self.CP.pcmCruise and self.params.get_bool("CustomStockLong"): self.CP.pcmCruiseSpeed = False - services = self.sm.data.keys() | {'longitudinalPlan'} + services = self.sm.data.keys() | {'longitudinalPlanSP'} self.sm = messaging.SubMaster(list(services)) cslc_path = f'openpilot.selfdrive.controls.lib.sunnypilot.custom_stock_longitudinal_controller.car.{self.CP.carName}' CustomStockLongitudinalController = __import__(cslc_path + '.controller', fromlist=['CustomStockLongitudinalController']).CustomStockLongitudinalController - self.custom_stock_longitudinal_controller = CustomStockLongitudinalController(self, self.CI.CC, self.CP) + self.custom_stock_longitudinal_controller = CustomStockLongitudinalController(self, self.CI.CC, self.CI.CS, self.CP) openpilot_enabled_toggle = self.params.get_bool("OpenpilotEnabledToggle") diff --git a/selfdrive/controls/lib/sunnypilot/custom_stock_longitudinal_controller/car/hyundai/controller.py b/selfdrive/controls/lib/sunnypilot/custom_stock_longitudinal_controller/car/hyundai/controller.py index 0cf9edc1b2..7ed937042b 100644 --- a/selfdrive/controls/lib/sunnypilot/custom_stock_longitudinal_controller/car/hyundai/controller.py +++ b/selfdrive/controls/lib/sunnypilot/custom_stock_longitudinal_controller/car/hyundai/controller.py @@ -9,13 +9,13 @@ ButtonType = car.CarState.ButtonEvent.Type class CustomStockLongitudinalController(CustomStockLongitudinalControllerBase): - def __init__(self, car, car_controller, CP): - super().__init__(car, car_controller, CP) + def __init__(self, car, car_controller, car_state, CP): + super().__init__(car, car_controller, car_state, CP) self.accel_button = Buttons.RES_ACCEL self.decel_button = Buttons.SET_DECEL self.set_speed_buttons = (ButtonType.accelCruise, ButtonType.decelCruise) - def create_can_mock_button_messages(self, CS: car.CarState) -> list[SendCan]: + def create_can_mock_button_messages(self) -> list[SendCan]: can_sends = [] if self.cruise_button is not None: if self.CP.carFingerprint in LEGACY_SAFETY_MODE_CAR: @@ -25,11 +25,11 @@ class CustomStockLongitudinalController(CustomStockLongitudinalControllerBase): # send resume at a max freq of 10Hz if (self.car_controller.frame - self.car_controller.last_button_frame) * DT_CTRL > 0.1 * send_freq: # send 25 messages at a time to increases the likelihood of cruise buttons being accepted - can_sends.extend([hyundaican.create_clu11(self.car_controller.packer, self.car_controller.frame, CS.clu11, self.cruise_button, self.CP)] * 25) + can_sends.extend([hyundaican.create_clu11(self.car_controller.packer, self.car_controller.frame, self.car_state.clu11, self.cruise_button, self.CP)] * 25) if (self.car_controller.frame - self.car_controller.last_button_frame) * DT_CTRL >= 0.15 * send_freq: self.car_controller.last_button_frame = self.car_controller.frame elif self.car_controller.frame % 2 == 0: - can_sends.extend([hyundaican.create_clu11(self.car_controller.packer, (self.car_controller.frame // 2) + 1, CS.clu11, self.cruise_button, self.CP)] * 25) + can_sends.extend([hyundaican.create_clu11(self.car_controller.packer, (self.car_controller.frame // 2) + 1, self.car_state.clu11, self.cruise_button, self.CP)] * 25) return can_sends @@ -45,11 +45,11 @@ class CustomStockLongitudinalController(CustomStockLongitudinalControllerBase): return can_sends - def create_mock_button_messages(self, CS: car.CarState, CC: car.CarControl) -> list[SendCan]: + def create_mock_button_messages(self) -> list[SendCan]: can_sends = [] if self.CP.carFingerprint in CANFD_CAR: can_sends.extend(self.create_canfd_mock_button_messages()) else: - can_sends.extend(self.create_can_mock_button_messages(CS)) + can_sends.extend(self.create_can_mock_button_messages()) return can_sends diff --git a/selfdrive/controls/lib/sunnypilot/custom_stock_longitudinal_controller/controllers.py b/selfdrive/controls/lib/sunnypilot/custom_stock_longitudinal_controller/controllers.py index 40cfb79c78..2d85f51783 100644 --- a/selfdrive/controls/lib/sunnypilot/custom_stock_longitudinal_controller/controllers.py +++ b/selfdrive/controls/lib/sunnypilot/custom_stock_longitudinal_controller/controllers.py @@ -17,9 +17,10 @@ SendCan = tuple[int, bytes, int] class CustomStockLongitudinalControllerBase(ABC): - def __init__(self, car, car_controller, CP): + def __init__(self, car, car_controller, car_state, CP): self.car = car self.car_controller = car_controller + self.car_state = car_state self.CP = CP self.params = Params() @@ -64,7 +65,7 @@ class CustomStockLongitudinalControllerBase(ABC): return customStockLongitudinalControl def get_set_speed_buttons(self, CS: car.CarState) -> bool: - return any(be.type in self.set_speed_buttons for be in CS.out.buttonEvents) + return any(be.type in self.set_speed_buttons for be in CS.buttonEvents) def handle_button_state(self, CS: car.CarState) -> int | None: state = self.button_states.get(self.button_state) @@ -89,30 +90,30 @@ class CustomStockLongitudinalControllerBase(ABC): return self.steady_speed def get_cruise_button(self, CS: car.CarState) -> int: - self.target_speed = round(self.final_speed_kph * (CV.KPH_TO_MPH if not CS.params_list.is_metric else 1)) - self.v_cruise = round(CS.out.cruiseState.speed * (CV.MS_TO_MPH if not CS.params_list.is_metric else CV.MS_TO_KPH)) + self.target_speed = round(self.final_speed_kph * (CV.KPH_TO_MPH if not self.car_state.params_list.is_metric else 1)) + self.v_cruise = round(CS.cruiseState.speed * (CV.MS_TO_MPH if not self.car_state.params_list.is_metric else CV.MS_TO_KPH)) cruise_button = self.handle_button_state(CS) return cruise_button @abstractmethod - def create_mock_button_messages(self, CS: car.CarState, CC: car.CarControl) -> list[SendCan]: + def create_mock_button_messages(self) -> list[SendCan]: pass def update(self, CS: car.CarState, CC: car.CarControl) -> list[SendCan]: - self.is_active = CS.out.cruiseState.enabled and not CC.cruiseControl.cancel and not CC.cruiseControl.resume + self.is_active = CS.cruiseState.enabled and not CC.cruiseControl.cancel and not CC.cruiseControl.resume if self.car.sm.updated['longitudinalPlanSP']: self.v_tsc_state = self.car.sm['longitudinalPlanSP'].visionTurnControllerState self.slc_state = self.car.sm['longitudinalPlanSP'].speedLimitControlState self.m_tsc_state = self.car.sm['longitudinalPlanSP'].turnSpeedControlState self.v_tsc = self.car.sm['longitudinalPlanSP'].visionTurnSpeed - self.speed_limit_offseted = self.car.sm['longitudinalPlanSP'].speedLimitOffseted + self.speed_limit_offseted = self.car.sm['longitudinalPlanSP'].speedLimit + self.car.sm['longitudinalPlanSP'].speedLimitOffset self.m_tsc = self.car.sm['longitudinalPlanSP'].turnSpeed - self.v_cruise_min = get_set_point(CS.params_list.is_metric) + self.v_cruise_min = get_set_point(self.car_state.params_list.is_metric) self.final_speed_kph = self.get_v_target(CC) self.cruise_button = self.get_cruise_button(CS) - can_sends = self.create_mock_button_messages(CS, CC) + can_sends = self.create_mock_button_messages() return can_sends