mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-07-24 07:42:05 +08:00
Code Issues Packages Projects Releases Wiki Activity

fixes

Browse Source
This commit is contained in:
Jason Wen
2024-09-14 17:30:29 -04:00
parent 931e79e801
commit c63c7d8bfd
3 changed files with 19 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/car/card.py
+2 -2
View File
@@ -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")
selfdrive/controls/lib/sunnypilot/custom_stock_longitudinal_controller/car/hyundai/controller.py
+7 -7
View File
@@ -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
selfdrive/controls/lib/sunnypilot/custom_stock_longitudinal_controller/controllers.py
+10 -9
View File
@@ -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