diff --git a/sunnypilot/controls/lib/custom_stock_longitudinal_controller/controllers.py b/sunnypilot/controls/lib/custom_stock_longitudinal_controller/controllers.py index 304f406ff6..ed1d5e4b2b 100644 --- a/sunnypilot/controls/lib/custom_stock_longitudinal_controller/controllers.py +++ b/sunnypilot/controls/lib/custom_stock_longitudinal_controller/controllers.py @@ -5,7 +5,7 @@ from openpilot.common.conversions import Conversions as CV from openpilot.common.params import Params from openpilot.selfdrive.controls.lib.sunnypilot.speed_limit_controller import ACTIVE_STATES from openpilot.sunnypilot.controls.lib.custom_stock_longitudinal_controller.states import InactiveState, \ - AcceleratingState, DeceleratingState, HoldingState, ResettingState, LoadingState + LoadingState, AcceleratingState, DeceleratingState, HoldingState from openpilot.sunnypilot.controls.lib.custom_stock_longitudinal_controller.helpers import get_set_point, \ speed_hysteresis, update_manual_button_timers @@ -26,10 +26,10 @@ class CustomStockLongitudinalControllerBase(ABC): self.CP = CP self.params = Params() - self.final_speed_kph = 0 - self.target_speed = 0 - self.v_cruise = 0 + self.v_target = 0 + self.v_cruise_cluster = 0 self.v_cruise_min = 0 + self.cruise_button = None self.button_state = ButtonControlState.inactive self.v_tsc_state = VisionTurnControllerState.disabled @@ -41,8 +41,6 @@ class CustomStockLongitudinalControllerBase(ABC): self.is_ready = False self.is_ready_prev = False - self.cruise_button = None - self.speed = 0 self.speed_steady = 0 self.accel_button = None @@ -51,11 +49,10 @@ class CustomStockLongitudinalControllerBase(ABC): self.button_states = { ButtonControlState.inactive: InactiveState(self), + ButtonControlState.loading: LoadingState(self), ButtonControlState.accelerating: AcceleratingState(self), ButtonControlState.decelerating: DeceleratingState(self), ButtonControlState.holding: HoldingState(self), - ButtonControlState.resetting: ResettingState(self), - ButtonControlState.loading: LoadingState(self), } @abstractmethod @@ -66,9 +63,8 @@ class CustomStockLongitudinalControllerBase(ABC): customStockLongitudinalControl = custom.CarControlSP.CustomStockLongitudinalControl.new_message() customStockLongitudinalControl.state = self.button_state customStockLongitudinalControl.cruiseButton = 0 if self.cruise_button is None else int(self.cruise_button) - customStockLongitudinalControl.finalSpeedKph = float(self.final_speed_kph) - customStockLongitudinalControl.targetSpeed = float(self.target_speed) - customStockLongitudinalControl.vCruise = float(self.v_cruise) + customStockLongitudinalControl.vTarget = float(self.v_target) + customStockLongitudinalControl.vCruiseCluster = float(self.v_cruise_cluster) return customStockLongitudinalControl def update_msgs(self) -> None: @@ -85,19 +81,30 @@ class CustomStockLongitudinalControllerBase(ABC): self.m_tsc = self.car.sm['longitudinalPlanSP'].turnSpeed def update_calculations(self, CS: car.CarState, CC: car.CarControl) -> None: - self.v_cruise_min = get_set_point(self.car_state.params_list.is_metric) + is_metric = self.car_state.params_list.is_metric + v_cruise_kph = CC.vCruise + v_cruise = v_cruise_kph * CV.KPH_TO_MS + self.v_cruise_min = get_set_point(is_metric) + self.v_cruise_cluster = round(CS.cruiseState.speed * (CV.MS_TO_KPH if is_metric else CV.MS_TO_MPH)) - v_tsc_target = self.v_tsc * CV.MS_TO_KPH if self.v_tsc_state != VisionTurnControllerState.disabled else 255 - slc_target = self.speed_limit_offseted * CV.MS_TO_KPH if self.slc_state in ACTIVE_STATES else 255 - m_tsc_target = self.m_tsc * CV.MS_TO_KPH if self.m_tsc_state > TurnSpeedControlState.tempInactive else 255 - tsc_target = min(v_tsc_target, slc_target, m_tsc_target) - self.speed = speed_hysteresis(tsc_target, self.speed_steady, 1.5 * (1 if self.car_state.params_list.is_metric else CV.MPH_TO_KPH)) - self.speed_steady = self.speed + v_targets = {'cruise': CC.vCruise} - self.final_speed_kph = min(self.speed, CC.vCruise) + if self.v_tsc_state != VisionTurnControllerState.disabled: + v_targets['v_tsc'] = self.v_tsc - 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)) + if self.slc_state in ACTIVE_STATES: + v_targets['slc'] = self.speed_limit_offseted + + if self.m_tsc_state > TurnSpeedControlState.tempInactive: + v_targets['m_tsc'] = self.m_tsc + + source = min(v_targets, key=v_targets.get) + + v_target = speed_hysteresis(self, v_targets[source], self.speed_steady, 1.5 * (CV.KPH_TO_MS if is_metric else CV.MPH_TO_MS)) + v_target = min(v_target, v_cruise) + v_target = round(v_target * (CV.MS_TO_KPH if is_metric else CV.MS_TO_MPH)) + + self.v_target = v_target def update_state(self, CS: car.CarState, CC: car.CarControl) -> None: update_manual_button_timers(CS, self.cruise_buttons) @@ -112,14 +119,12 @@ class CustomStockLongitudinalControllerBase(ABC): self.is_ready_prev = self.is_ready def update(self, CS: car.CarState, CC: car.CarControl) -> list[SendCan]: - can_sends = [] - self.update_msgs() self.update_calculations(CS, CC) self.update_state(CS, CC) - can_sends.extend(self.create_mock_button_messages()) + can_sends = self.create_mock_button_messages() return can_sends diff --git a/sunnypilot/controls/lib/custom_stock_longitudinal_controller/helpers.py b/sunnypilot/controls/lib/custom_stock_longitudinal_controller/helpers.py index 246d5d1d96..84e4ced9ae 100644 --- a/sunnypilot/controls/lib/custom_stock_longitudinal_controller/helpers.py +++ b/sunnypilot/controls/lib/custom_stock_longitudinal_controller/helpers.py @@ -7,11 +7,14 @@ def get_set_point(is_metric: bool) -> float: return 30 if is_metric else 20 -def speed_hysteresis(speed: float, speed_steady: float, hyst: float) -> float: +def speed_hysteresis(controller, speed: float, speed_steady: float, hyst: float) -> float: if speed > speed_steady + hyst: speed_steady = speed - hyst elif speed < speed_steady - hyst: speed_steady = speed + hyst + + controller.speed_steady = speed + return speed_steady diff --git a/sunnypilot/controls/lib/custom_stock_longitudinal_controller/states.py b/sunnypilot/controls/lib/custom_stock_longitudinal_controller/states.py index 83efaffb3b..a25dd0c315 100644 --- a/sunnypilot/controls/lib/custom_stock_longitudinal_controller/states.py +++ b/sunnypilot/controls/lib/custom_stock_longitudinal_controller/states.py @@ -47,9 +47,9 @@ class InactiveState(ButtonStateBase): class LoadingState(ButtonStateBase): def handle(self) -> None: - if self.controller.target_speed > self.controller.v_cruise: + if self.controller.v_target > self.controller.v_cruise_cluster: self.controller.button_state = ButtonControlState.accelerating - elif self.controller.target_speed < self.controller.v_cruise and self.controller.v_cruise > self.controller.v_cruise_min: + elif self.controller.v_target < self.controller.v_cruise_cluster and self.controller.v_cruise_cluster > self.controller.v_cruise_min: self.controller.button_state = ButtonControlState.decelerating else: self.controller.button_state = ButtonControlState.holding @@ -59,7 +59,7 @@ class LoadingState(ButtonStateBase): class AcceleratingState(ButtonStateBase): def handle(self) -> int | None: self.button_count += 1 - if self.controller.target_speed <= self.controller.v_cruise or self.button_count > RESET_COUNT: + if self.controller.v_target <= self.controller.v_cruise_cluster or self.button_count > RESET_COUNT: self.button_count = 0 self.controller.button_state = ButtonControlState.holding return None @@ -69,7 +69,7 @@ class AcceleratingState(ButtonStateBase): class DeceleratingState(ButtonStateBase): def handle(self) -> int | None: self.button_count += 1 - if self.controller.target_speed >= self.controller.v_cruise or self.controller.v_cruise <= self.controller.v_cruise_min or self.button_count > RESET_COUNT: + if self.controller.v_target >= self.controller.v_cruise_cluster or self.controller.v_cruise_cluster <= self.controller.v_cruise_min or self.button_count > RESET_COUNT: self.button_count = 0 self.controller.button_state = ButtonControlState.holding return None @@ -81,11 +81,5 @@ class HoldingState(ButtonStateBase): self.button_count += 1 if self.button_count > HOLD_TIME: self.button_count = 0 - self.controller.button_state = ButtonControlState.resetting - return None - - -class ResettingState(ButtonStateBase): - def handle(self) -> None: - self.controller.button_state = ButtonControlState.loading + self.controller.button_state = ButtonControlState.loading return None