diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 2a2f83678b..1962ffa1a3 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -73,6 +73,10 @@ class Car: services = self.sm.data.keys() | {'longitudinalPlan'} 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) + openpilot_enabled_toggle = self.params.get_bool("OpenpilotEnabledToggle") controller_available = self.CI.CC is not None and openpilot_enabled_toggle and not self.CP.dashcamOnly @@ -177,6 +181,8 @@ class Car: # send car controls over can now_nanos = self.can_log_mono_time if REPLAY else int(time.monotonic() * 1e9) self.last_actuators_output, can_sends = self.CI.apply(CC, now_nanos) + if not self.CP.pcmCruiseSpeed: + can_sends.extend(self.custom_stock_longitudinal_controller.update(CS, CC)) self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) self.CC_prev = CC 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 29654fd374..f2ab1c5f4c 100644 --- a/selfdrive/controls/lib/sunnypilot/custom_stock_longitudinal_controller/controllers.py +++ b/selfdrive/controls/lib/sunnypilot/custom_stock_longitudinal_controller/controllers.py @@ -151,7 +151,7 @@ class CustomStockLongitudinalControllerBase(ABC): def create_mock_button_messages(self, CS: car.CarState, CC: car.CarControl) -> list[SendCan]: pass - def update(self, CS: car.CarState) -> None: + def update(self, CS: car.CarState, CC: car.CarControl) -> list[SendCan]: if self.car.sm.updated['longitudinalPlanSP']: self.v_tsc_state = self.car.sm['longitudinalPlanSP'].visionTurnControllerState self.slc_state = self.car.sm['longitudinalPlanSP'].speedLimitControlState @@ -174,3 +174,7 @@ class CustomStockLongitudinalControllerBase(ABC): slc_active = not sl_inactive and not sl_temp_inactive self.slc_active_stock = slc_active + + can_sends = self.create_mock_button_messages(CS, CC) + + return can_sends