diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index ff56632286..2763ca31cc 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -7,6 +7,8 @@ See the LICENSE.md file in the root directory for more details. from cereal import messaging, custom from opendbc.car import structs +from openpilot.common.constants import CV +from openpilot.selfdrive.car.cruise import V_CRUISE_MAX from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.smart_cruise_control import SmartCruiseControl from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import SpeedLimitAssist @@ -41,6 +43,9 @@ class LongitudinalPlannerSP: return self.dec.mode() def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]: + v_cruise_cluster_kph = min(sm['carState'].vCruiseCluster, V_CRUISE_MAX) + v_cruise_cluster = v_cruise_cluster_kph * CV.KPH_TO_MS + long_enabled = sm['carControl'].enabled long_override = sm['carControl'].cruiseControl.override @@ -53,7 +58,7 @@ class LongitudinalPlannerSP: self.resolver.update(v_ego, sm) # Speed Limit Assist - self.sla.update(long_enabled, long_override, v_ego, a_ego, sm['carState'].vCruiseCluster, + self.sla.update(long_enabled, long_override, v_ego, a_ego, v_cruise_cluster, self.resolver.speed_limit, self.resolver.speed_limit_offset, self.resolver.distance, self.events_sp) targets = { diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py b/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py index 5fce4a54fc..aff465aaa6 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py @@ -64,8 +64,8 @@ class SpeedLimitAssist: self.v_ego = 0. self.a_ego = 0. self.v_offset = 0. - self.v_cruise_setpoint = 0. - self.v_cruise_setpoint_prev = 0. + self.v_cruise_cluster = 0. + self.v_cruise_cluster_prev = 0. self.initial_max_set = False self._speed_limit = 0. self._speed_limit_offset = 0. @@ -95,8 +95,8 @@ class SpeedLimitAssist: return bool(self._speed_limit != self.speed_limit_prev) @property - def v_cruise_setpoint_changed(self) -> bool: - return bool(self.v_cruise_setpoint != self.v_cruise_setpoint_prev) + def v_cruise_cluster_changed(self) -> bool: + return bool(self.v_cruise_cluster != self.v_cruise_cluster_prev) def get_v_target_from_control(self) -> float: if self.is_enabled: @@ -118,18 +118,10 @@ class SpeedLimitAssist: self.enabled = self.params.get("SpeedLimitMode", return_default=True) == Mode.assist def initial_max_set_confirmed(self) -> bool: - return bool(abs(self.v_cruise_setpoint - REQUIRED_INITIAL_MAX_SET_SPEED) <= CRUISE_SPEED_TOLERANCE) + return bool(abs(self.v_cruise_cluster - REQUIRED_INITIAL_MAX_SET_SPEED) <= CRUISE_SPEED_TOLERANCE) - def detect_manual_cruise_change(self) -> bool: - # If cruise speed changed and it's not what SLA would set - if self.v_cruise_setpoint_changed: - expected_cruise = self.speed_limit_final - return bool(abs(self.v_cruise_setpoint - expected_cruise) > CRUISE_SPEED_TOLERANCE) - - return False - - def update_calculations(self, v_cruise_setpoint: float) -> None: - self.v_cruise_setpoint = v_cruise_setpoint if not np.isnan(v_cruise_setpoint) else 0.0 + def update_calculations(self, v_cruise_cluster: float) -> None: + self.v_cruise_cluster = v_cruise_cluster if not np.isnan(v_cruise_cluster) else 0.0 # Update current velocity offset (error) self.v_offset = self.speed_limit_final - self.v_ego @@ -163,14 +155,14 @@ class SpeedLimitAssist: else: # ACTIVE if self.state == SpeedLimitAssistState.active: - if self.detect_manual_cruise_change(): + if self.v_cruise_cluster_changed: self.state = SpeedLimitAssistState.inactive elif self._speed_limit > 0 and self.v_offset < LIMIT_SPEED_OFFSET_TH: self.state = SpeedLimitAssistState.adapting # ADAPTING elif self.state == SpeedLimitAssistState.adapting: - if self.detect_manual_cruise_change(): + if self.v_cruise_cluster_changed: self.state = SpeedLimitAssistState.inactive elif self.v_offset >= LIMIT_SPEED_OFFSET_TH: self.state = SpeedLimitAssistState.active @@ -235,7 +227,7 @@ class SpeedLimitAssist: elif self.speed_limit_changed: events_sp.add(EventNameSP.speedLimitChanged) - def update(self, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float, v_cruise_setpoint: float, + def update(self, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float, v_cruise_cluster: float, speed_limit: float, speed_limit_offset: float, distance: float, events_sp: EventsSP) -> None: self.long_enabled = long_enabled self.long_override = long_override @@ -247,13 +239,13 @@ class SpeedLimitAssist: self._distance = distance self.update_params() - self.update_calculations(v_cruise_setpoint) + self.update_calculations(v_cruise_cluster) self.is_enabled, self.is_active = self.update_state_machine() self.update_events(events_sp) # Update change tracking variables self.speed_limit_prev = self._speed_limit - self.v_cruise_setpoint_prev = self.v_cruise_setpoint + self.v_cruise_cluster_prev = self.v_cruise_cluster self.long_enabled_prev = self.long_enabled self.output_v_target = self.get_v_target_from_control() diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py b/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py index c4197f6505..deb846fec7 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py @@ -68,14 +68,12 @@ class TestSpeedLimitAssist: self.sla.speed_limit_prev = 0. self.sla.last_valid_speed_limit_offsetted = 0. self.sla._distance = 0. - self.sla.v_cruise_setpoint = 0. - self.sla.v_cruise_setpoint_prev = 0. self.events_sp.clear() - def initialize_active_state(self, v_cruise_setpoint): + def initialize_active_state(self, initialize_v_cruise): self.sla.state = SpeedLimitAssistState.active - self.sla.v_cruise_setpoint = v_cruise_setpoint - self.sla.v_cruise_setpoint_prev = v_cruise_setpoint + self.sla.v_cruise_cluster = initialize_v_cruise + self.sla.v_cruise_cluster_prev = initialize_v_cruise def test_initial_state(self): assert self.sla.state == SpeedLimitAssistState.disabled @@ -135,7 +133,7 @@ class TestSpeedLimitAssist: def test_adapting_to_active_transition(self): self.sla.state = SpeedLimitAssistState.adapting - self.sla.v_cruise_setpoint_prev = REQUIRED_INITIAL_MAX_SET_SPEED + self.sla.v_cruise_cluster_prev = REQUIRED_INITIAL_MAX_SET_SPEED self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, 0, self.events_sp) assert self.sla.state == SpeedLimitAssistState.active @@ -143,7 +141,7 @@ class TestSpeedLimitAssist: def test_manual_cruise_change_detection(self): self.sla.state = SpeedLimitAssistState.active expected_cruise = SPEED_LIMITS['highway'] - self.sla.v_cruise_setpoint_prev = expected_cruise + self.sla.v_cruise_cluster_prev = expected_cruise different_cruise = SPEED_LIMITS['highway'] + 5 self.sla.update(True, False, SPEED_LIMITS['city'], 0, different_cruise, SPEED_LIMITS['city'], 0, 0, self.events_sp) @@ -179,7 +177,7 @@ class TestSpeedLimitAssist: def test_distance_based_adapting(self): self.sla.state = SpeedLimitAssistState.adapting - self.sla.v_cruise_setpoint_prev = REQUIRED_INITIAL_MAX_SET_SPEED + self.sla.v_cruise_cluster_prev = REQUIRED_INITIAL_MAX_SET_SPEED distance = 100.0 current_speed = SPEED_LIMITS['highway']